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

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

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

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