一種基于誤差四元數(shù)反饋的rov姿態(tài)控制方法
【技術(shù)領(lǐng)域】
[0001]本發(fā)明涉及一種ROV姿態(tài)控制方法，具體地說是一種基于誤差四元數(shù)反饋的ROV姿 態(tài)控制方法。
【背景技術(shù)】
[0002] 無人遙控潛水器(Remote Operated Vegicle)可用于作業(yè)、觀測等多種用途，根據(jù) 具體的用途不同，其技術(shù)實現(xiàn)方案也不盡相同。傳統(tǒng)的ROV其運動多為前后、左右、上下的直 線運動以及Z軸的旋轉(zhuǎn)運動。但在一些較復雜的作業(yè)環(huán)境下，如貼附船底、壁面、管道等具有 連續(xù)性、較大附著面的作業(yè)，則需要ROV能夠完成X、Y、Z三個方向的360°無死角旋轉(zhuǎn)運動并 實現(xiàn)姿態(tài)穩(wěn)定。相應ROV控制也不能使用常用的控制歐拉角的方法來進行姿態(tài)控制，因為歐 拉角法存在萬向節(jié)鎖的缺點，并且大多數(shù)的姿態(tài)傳感器在輸出歐拉角時，在± 180°或± 90° 時會出現(xiàn)不連續(xù)性。
[0003] 常用的旋轉(zhuǎn)描述方法有歐拉角、四元數(shù)、旋轉(zhuǎn)矩陣。其中歐拉角容易理解，形象直 觀，只需要三個值，但是三個值不同的旋轉(zhuǎn)順序會導致不同的結(jié)果，必須按照一個固定的順 序來旋轉(zhuǎn)，并且會造成萬向節(jié)鎖現(xiàn)象；四元數(shù)方法可以避免萬向節(jié)鎖現(xiàn)象，只需要一個四維 的四元數(shù)就可以執(zhí)行繞任意過原點的向量的旋轉(zhuǎn)，并可提供平滑差值，但是其理解相對困 難，不直觀，比歐拉角相對復雜;旋轉(zhuǎn)矩陣法旋轉(zhuǎn)軸可以是任意向量，但是矩陣法需要16個 元素，不僅理解困難，并且會大大增加計算量。
[0004] 中國專利201410422945.2公開的一種水下機器人位姿控制方法，主要是通過姿態(tài) 四元數(shù)獲得歐拉角將期望值的誤差輸入PID控制器進行計算，仍無法實現(xiàn)360°無死角旋轉(zhuǎn) 運動控制。
[0005] 因此設(shè)計一種能夠避免奇異點對姿態(tài)調(diào)整與控制的影響，實現(xiàn)全角度穩(wěn)定旋轉(zhuǎn)的 ROV姿態(tài)控制方法是非常必要的。

【發(fā)明內(nèi)容】

[0006] 本發(fā)明提供的一種基于誤差四元數(shù)反饋的ROV姿態(tài)控制方法，能夠控制ROV完成X、 Y、Z三個方向的360°無死角旋轉(zhuǎn)運動，且收斂快、穩(wěn)定性好。
[0007] 為了解決上述的技術(shù)問題，本發(fā)明采用下述技術(shù)方案：
[0008] 建立慣性坐標系和ROV的運動坐標系；
[0009] ROV對稱結(jié)構(gòu)設(shè)計，并對其三自由度旋轉(zhuǎn)姿態(tài)進行建模；
[0010]姿態(tài)傳感器對ROV進行檢測輸出姿態(tài)四元數(shù)，得到ROV的當前四元數(shù)與目標四元數(shù) 對比，通過運算求出誤差四元數(shù)；
[0011]將誤差四元數(shù)輸入PID控制器，計算得出ROV運動坐標系中三個軸上所需的力矩， 處理器根據(jù)所需力矩控制推進器電機的電壓，產(chǎn)生相應的推力，完成ROV的姿態(tài)調(diào)整。
[0012]進一步地，ROV對稱結(jié)構(gòu)設(shè)計為:在ROV的運動坐標系中的Χ、Υ、Ζ旋轉(zhuǎn)軸上推進器繞 每個軸成對配置，控制ROV繞各軸旋轉(zhuǎn)。
[0013] 進一步地，當前四元數(shù)的逆與目標四元數(shù)進行乘法運算求出誤差四元數(shù)。
[0014] 進一步地，在軟件設(shè)計中，主程序進行數(shù)據(jù)采集、接受指令和傳輸數(shù)據(jù)，中斷程序 進行誤差四元數(shù)計算、PID計算和電機控制信號計算。
[0015] 采用本發(fā)明的有益效果是，該ROV的姿態(tài)控制方法，可以使ROV完成360°無死角旋 轉(zhuǎn)運動并實現(xiàn)姿態(tài)穩(wěn)定，相比基于四元數(shù)或者旋轉(zhuǎn)矩陣的控制方法，該控制方法易于理解、 設(shè)計簡單，操作實施方便，相比基于歐拉角的控制方法不會出現(xiàn)萬向節(jié)鎖現(xiàn)象，從而導致控 制失靈，能夠確保完成如貼附船底、壁面、管道等具有連續(xù)性、較大附著面的作業(yè)，且該方法 收斂快、穩(wěn)定性好，在實際調(diào)試、使用過程中更可以靈活調(diào)整、優(yōu)化升級，以更好的滿足使用 需要。
【附圖說明】
[0016] 圖1是本發(fā)明所述一種基于誤差四元數(shù)反饋的ROV姿態(tài)控制方法的主循環(huán)流程圖；
[0017] 圖2是本發(fā)明所述一種基于誤差四元數(shù)反饋的ROV姿態(tài)控制方法的中斷流程圖；
[0018] 圖3是本發(fā)明仿真模型總體框圖；
[0019] 圖4是本發(fā)明仿真X軸階躍響應曲線圖；
[0020] 圖5是本發(fā)明仿真y軸階躍響應曲線圖；
[0021]圖6是本發(fā)明仿真z軸階躍響應曲線圖；
[0022]圖7是本發(fā)明仿真X軸正弦信號輸入響應曲線圖；
[0023] 圖8是本發(fā)明仿真X軸階躍干擾響應曲線圖；
[0024] 圖9是本發(fā)明仿真y軸階躍干擾響應曲線圖；
[0025] 圖10是本發(fā)明仿真z軸階躍干擾響應曲線圖；
[0026]圖11是本發(fā)明仿真X軸正弦干擾響應曲線圖；
【具體實施方式】
[0027]下面結(jié)合附圖和【具體實施方式】對本發(fā)明作進一步詳細說明。
[0028] 在圖1和圖2中，建立慣性坐標系和ROV的運動坐標系；
[0029] 慣性坐標系為坐標原點E為地球上某點;Εξ軸位于水平面上，ROV前進方向為正;En 軸位于Εξ平面內(nèi)按右手法則將Εξ順時針轉(zhuǎn)90° ;Εζ軸位于Εζξ平面，指向地心。運動坐標系為 坐標原點G取在ROV的中心G處，Gx軸平行于ROV基線指向前進方向；橫向軸Gy平行于基面指 向ROV的右側(cè);而Gz軸則指向ROV底部。
[0030] ROV對稱結(jié)構(gòu)設(shè)計，并對其三自由度旋轉(zhuǎn)姿態(tài)進行建模；
[0031] 根據(jù)剛體力學理論，當水下機器人重心和動系原點不重合時，其六自由度運動一 般方程為：

[0038]其中：
[0039] m-水下機器人質(zhì)量；
[0040] xg、yg、Zg-水下機器人的重心坐標；
[00411 Ix、Iy、IZ-水下機器人的質(zhì)量m對Ox、Oy、Oz、軸的轉(zhuǎn)動慣量；
[0042] u、v、w、p、q、r_六自由度的（角)速度；
[0043] 眾、旮、旮、'令、六自由度的（角）加速度；
[0044] 父、丫、2、1(^-六自由度的力(矩）。
[0045] 水下機器人的任意一個自由度之間的運動都和其他自由度運動有關(guān)，也就是說在 六個自由度之間存在交叉耦合，這是水下機器人的難點之一。根據(jù)ROV的外形與實際工作情 況，對ROV的三自由度旋轉(zhuǎn)姿態(tài)進行建模仿真分析。
[0046]在ROV的運動坐標系中的X、Y、Z旋轉(zhuǎn)軸上推進器繞每個軸成對配置，控制ROV繞各 軸旋轉(zhuǎn)。當ROV在低速狀態(tài)下作業(yè)時，因為ROV物理模型的設(shè)計已經(jīng)使ROV具有近似的三個對 稱面，所以可以忽略其耦合作用，略去高階項，只取到二階。此時模型可簡化為：
[0048]姿態(tài)傳感器對ROV進行檢測輸出姿態(tài)四元數(shù)，得到ROV的當前四元數(shù)與目標四元數(shù) 對比，通過運算求出誤差四元數(shù)；
[0049]用單位四元數(shù)來描述ROV的姿態(tài)，其表示的是ROV載體坐標系與慣性參考坐標系的 旋轉(zhuǎn)關(guān)系。用誤差四元數(shù)來描述ROV當前姿態(tài)與目標姿態(tài)的旋轉(zhuǎn)關(guān)系。這里聲明相關(guān)的四元 數(shù)符號：
[0050] 表1四元數(shù)參數(shù)聲明
[0052] Δ q = q_1 X Q
[0053] 這里的乘號為四元數(shù)乘法，(T1為q的逆，等于q的共輒，即：
[0054] q-1=[qQ-q1 -q2 -q3]T
[0055] 可求得：
[0057] 將誤差四元數(shù)輸入PID控制器，計算得出ROV運動坐標系中三個軸上所需的力矩， 處理器根據(jù)所需力矩控制推進器電機的電壓，產(chǎn)生相應的推力，完成ROV的姿態(tài)調(diào)整。
[0058] 在軟件設(shè)計中，主程序進行數(shù)據(jù)采集、接受指令和傳輸數(shù)據(jù)，中斷程序進行誤差四 元數(shù)計算、PID計算和電機控制信號計算，中斷為定時中斷，頻率為50Hz。
[0059]為驗證控制方法與ROV設(shè)計的可行性，使用Simulink對姿態(tài)控制系統(tǒng)進行仿真分 析。
[0060] 在圖3中，控制系統(tǒng)首先讀取傳感器的數(shù)據(jù)，采集到當前四元數(shù)信息，然后與目標 四元數(shù)對比，求得誤差四元數(shù)。根據(jù)誤差四元數(shù)求得當前姿態(tài)與目標姿態(tài)之間的旋轉(zhuǎn)關(guān)系， 輸入PID控制器，對三個軸上的力矩進行計算，控制六個電機的推力，以控制ROV的姿態(tài)。
[0061] 這樣ROV的角速度發(fā)生變化，被傳感器采集到，傳感器便輸出新的四元數(shù)姿態(tài)信 息，系統(tǒng)進入下一個循環(huán)。
[0062]在圖4一圖7中，系統(tǒng)以階躍信號做為輸入，觀察其響應，來判斷系統(tǒng)的性能是否滿 足要求，PID參數(shù)是否是最佳參數(shù)。每個軸的階躍響應曲線，可以看出其響應比較迅速，收斂 速度非常快。當X軸的輸入為一正弦信號，可以看出其輸出響應曲線有一定的延時，但是穩(wěn) 定性較好。
[0063]在圖8-圖11中，一個系統(tǒng)的穩(wěn)定行不能只檢測輸入信號不同時是否穩(wěn)定，還要檢 測在有干擾的情況下是否具有很好的魯棒性。這里同樣采用兩組實驗，一組為三個軸的干 擾全部輸入階躍信號，一組為X軸輸入正弦干擾信號。
[0064]階躍干擾信號幅值為10Nm，正弦干擾信號幅值為10Nm，相對于推進器的推力來說 是比較大的干擾力矩，階躍干擾信號的響應曲線顯示系統(tǒng)回復很快，在短時間內(nèi)便能回復 目標姿態(tài)。正弦信號干擾時系統(tǒng)會出現(xiàn)同頻率的搖動，其X軸姿態(tài)角會有一個同頻率的幅值 為0.2的正弦變化，其變化范圍較小，系統(tǒng)穩(wěn)定。
[0065]須陳明，以上所述乃是本發(fā)明的較佳實施例，若以本發(fā)明的構(gòu)想所作的改變，其產(chǎn) 生的功能作用仍未超出說明書與圖示所涵蓋的精神時，均應在本發(fā)明的范圍內(nèi)。
【主權(quán)項】
1. 一種基于誤差四元數(shù)反饋的ROV姿態(tài)控制方法，其特征是： 建立慣性坐標系和R0V的運動坐標系； R0V對稱結(jié)構(gòu)設(shè)計，并對其三自由度旋轉(zhuǎn)姿態(tài)進行建模； 姿態(tài)傳感器對R0V進行檢測輸出姿態(tài)四元數(shù)，得到R0V的當前四元數(shù)與目標四元數(shù)對 比，通過運算求出誤差四元數(shù)； 將誤差四元數(shù)輸入PID控制器，計算得出R0V運動坐標系中三個軸上所需的力矩，處理 器根據(jù)所需力矩控制推進器電機的電壓，產(chǎn)生相應的推力，完成R0V的姿態(tài)調(diào)整。2. 根據(jù)權(quán)利要求1所述的一種基于誤差四元數(shù)反饋的R0V姿態(tài)控制方法，其特征是:R0V 對稱結(jié)構(gòu)設(shè)計為:在R0V的運動坐標系中的X、Y、Z旋轉(zhuǎn)軸上推進器繞每個軸成對配置，控制 R0V繞各軸旋轉(zhuǎn)。3. 根據(jù)權(quán)利要求1所述的一種基于誤差四元數(shù)反饋的R0V姿態(tài)控制方法，其特征是：當 前四元數(shù)的逆與目標四元數(shù)進行乘法運算求出誤差四元數(shù)。4. 根據(jù)權(quán)利要求1所述的一種基于誤差四元數(shù)反饋的R0V姿態(tài)控制方法，其特征是:在 軟件設(shè)計中，主程序進行數(shù)據(jù)采集、接受指令和傳輸數(shù)據(jù)，中斷程序進行誤差四元數(shù)計算、 PID計算和電機控制彳目號計算。
【專利摘要】本發(fā)明公開一種基于誤差四元數(shù)反饋的ROV姿態(tài)控制方法，建立慣性坐標系和ROV的運動坐標系；并對其三自由度旋轉(zhuǎn)姿態(tài)進行建模；姿態(tài)傳感器對ROV進行檢測輸出姿態(tài)四元數(shù)，ROV的當前四元數(shù)與目標四元數(shù)對比，求出誤差四元數(shù)；輸入PID控制器，計算得出ROV運動坐標系中三個軸上所需的力矩，處理器根據(jù)所需力矩控制推進器電機的電壓，產(chǎn)生相應的推力，完成ROV的姿態(tài)調(diào)整，使ROV完成360°無死角旋轉(zhuǎn)運動并實現(xiàn)姿態(tài)穩(wěn)定，實現(xiàn)較復雜工作環(huán)境下作業(yè)。
【IPC分類】B63C11/52, B25J9/04
【公開號】CN105619394
【申請?zhí)枴緾N201610108631
【發(fā)明人】溫泉, 宋曉暖, 曹雪峰, 徐興洲, 孫偉志, 熊海霞
【申請人】青島海山海洋裝備有限公司
【公開日】2016年6月1日
【申請日】2016年2月29日