[發(fā)明專利]水下機(jī)器人位姿控制方法

在審

• 申請(qǐng)?zhí)枺?/td>: 201410422945.2
• 申請(qǐng)日: 2014-08-25
• 公開（公告）號(hào): CN104155991A
• 公開（公告）日: 2014-11-19
• 發(fā)明（設(shè)計(jì)）人: 陳巍;湯忠強(qiáng);羅浩玨;陳絲雨
• 申請(qǐng)（專利權(quán)）人: 南京工程學(xué)院
• 主分類號(hào): G05D1/08
• 分類號(hào): G05D1/08
• 代理公司: 南京正聯(lián)知識(shí)產(chǎn)權(quán)代理有限公司 32243
• 代理人: 沈志海

技術(shù)領(lǐng)域

本發(fā)明涉及一種水下機(jī)器人位姿控制方法。

背景技術(shù)

近年來，水下機(jī)器人的研究逐漸成為航海研究中的新的前沿和熱點(diǎn)的問題。水下機(jī)器人在深海中的應(yīng)用前景也隨著海底探礦，海底電纜維護(hù)的發(fā)展而更為寬廣，同時(shí)對(duì)水下機(jī)器人機(jī)動(dòng)性和操縱性的要求也越來越高，在水下機(jī)器人執(zhí)行任務(wù)的過程中，不但要求其在環(huán)境擾動(dòng)下能夠按照預(yù)定的軌跡運(yùn)動(dòng)，而且在許多情況下需要利用水下機(jī)器人對(duì)目標(biāo)物進(jìn)行更細(xì)致的觀察和操作，這就需要水下機(jī)器人相對(duì)于目標(biāo)物的位置保持不變，即要求水下機(jī)器人具有能夠抵抗環(huán)境擾動(dòng)的較高的姿態(tài)穩(wěn)定性，這樣才能順利的完成細(xì)致、復(fù)雜的工作。

但是目前，許多水下機(jī)器人在水下姿態(tài)自動(dòng)調(diào)整方面沒有投入太大的精力，機(jī)器人定點(diǎn)懸浮，抵抗擾流的能力較弱。機(jī)器人在水下完成的任務(wù)有限。

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

本發(fā)明的目的是提供一種水下機(jī)器人位姿控制方法，能夠?qū)崿F(xiàn)機(jī)器人在水中的姿態(tài)平穩(wěn)，解決現(xiàn)有技術(shù)中存在的機(jī)器人定點(diǎn)懸浮，抵抗擾流的能力較弱，機(jī)器人在水下完成的任務(wù)有限的問題。

水下機(jī)器人，又稱無人潛水器，是一種作業(yè)于水下的極限作業(yè)機(jī)器人；

位姿：位姿分為位置和姿態(tài)，是描述機(jī)器人在空間中的位置和其自身的姿態(tài)；

歐拉角：確定定點(diǎn)轉(zhuǎn)動(dòng)剛體位置的3個(gè)一組獨(dú)立角參量，分別有偏航角ψ，俯仰角θ，翻滾角Φ。

本發(fā)明的技術(shù)解決方案是：

一種水下機(jī)器人位姿控制方法，

采用平衡結(jié)構(gòu)設(shè)計(jì)，對(duì)對(duì)稱的框架結(jié)構(gòu)進(jìn)行建模，

傳感器測(cè)得機(jī)器人姿態(tài)的俯仰角、翻滾角、偏航角；

與期望值做差值得到誤差，通過PID算法對(duì)誤差進(jìn)行調(diào)節(jié)，對(duì)電機(jī)進(jìn)行控制，使姿態(tài)保持平衡。

進(jìn)一步地，平衡結(jié)構(gòu)為重心向下、對(duì)稱的自穩(wěn)定框架結(jié)構(gòu)。

進(jìn)一步地，平衡結(jié)構(gòu)具體為：采用方形框架結(jié)構(gòu)，八個(gè)電機(jī)對(duì)稱的安裝在框架上，形成對(duì)稱的結(jié)構(gòu)，八個(gè)電機(jī)分成兩個(gè)部分，

其中，四個(gè)電機(jī)分別豎直的安裝在框架的四個(gè)直立的桿子上，通過正反轉(zhuǎn)控制機(jī)器人的上升和下降；另外四個(gè)電機(jī)安裝在框架的底部，控制機(jī)器人的前進(jìn)后退。

進(jìn)一步地，傳感器系統(tǒng)包括MPU6050三軸陀螺儀、加速度計(jì)、AK8975電子羅盤、HSTL-18液位變送器；MPU6050三軸陀螺儀、加速度計(jì)測(cè)得機(jī)器人在x,y,z軸上旋轉(zhuǎn)的角度以及加速度，經(jīng)過轉(zhuǎn)換成四元數(shù)；AK8975電子羅盤測(cè)得機(jī)器人方向的磁偏角；HSTL-18液位變送器測(cè)量出機(jī)器人所在水中的深度。

進(jìn)一步地，仿照四旋翼飛行器對(duì)水下機(jī)器人進(jìn)行數(shù)學(xué)建模；傳感器MPU6050三軸陀螺儀與加速度計(jì)，AK8975電子羅盤，HSTL-18液位變送器分別測(cè)出機(jī)器人姿態(tài)的四元數(shù)，偏航角和深度；利用PID算法對(duì)誤差調(diào)節(jié)，輸出量控制電機(jī)，使機(jī)器人的位姿保持平穩(wěn)。

進(jìn)一步地，實(shí)現(xiàn)水下機(jī)器人姿態(tài)的控制：

首先進(jìn)行數(shù)學(xué)建模，采用歐拉角與四元數(shù)兩種工具相結(jié)合的方法描述機(jī)器人在水下的姿態(tài)，先用傳感器測(cè)定機(jī)器人姿態(tài)的四元數(shù)(w,x,y,z)，再將四元數(shù)轉(zhuǎn)換成歐拉角得到偏航角(ψ)，俯仰角(θ)，翻滾角(Φ)；豎直放置的電機(jī)控制俯仰角、翻滾角，當(dāng)機(jī)器人的姿態(tài)不是水平時(shí)，俯仰角和翻滾角就不是零，輸入的俯仰角和翻滾角就是誤差；

然后通過PID算法控制四個(gè)豎直方向的電機(jī)，將俯仰角和翻滾角調(diào)整為零。

進(jìn)一步地，利用如下公式(1)將四元數(shù)轉(zhuǎn)換成歐拉角得到偏航角(ψ)，俯仰角(θ)，翻滾角(Φ)，

進(jìn)一步地，當(dāng)機(jī)器人偏離任務(wù)水深時(shí)，液位變送器所測(cè)的深度與原來的深度相比較得到誤差，然后利用PID算法，使機(jī)器人保持在任務(wù)水深。

進(jìn)一步地，當(dāng)機(jī)器人偏離理想直線時(shí)，電子羅盤的數(shù)值發(fā)生變化，這就是方向的誤差，利用PID算法，對(duì)誤差進(jìn)行糾正，確保機(jī)器人運(yùn)動(dòng)保持理想直線狀態(tài)。

本發(fā)明的有益效果是：該種水下機(jī)器人位姿控制方法，可以使機(jī)器人在水下具有能抵抗環(huán)境擾動(dòng)的姿態(tài)穩(wěn)定性，確保機(jī)器人順利的完成細(xì)致，復(fù)雜的工作。能夠?qū)崿F(xiàn)操作員根據(jù)任務(wù)要求設(shè)定機(jī)器人的深度，靈活的讓機(jī)器人停留在各種深度完成任務(wù)，操作更加人性化。并可以克服電機(jī)本征參數(shù)不完全相同，減少操作員的負(fù)擔(dān)，還能夠比較精確的到達(dá)任務(wù)區(qū)。

附圖說明

圖1是本發(fā)明實(shí)施例中水下機(jī)器人的結(jié)構(gòu)示意圖；

圖2是機(jī)器人動(dòng)作原理圖；

圖2的每個(gè)方向視圖中，上面一排由左至右分別為1號(hào)電機(jī)、2號(hào)電機(jī)，下面一排由左至右分別為3號(hào)電機(jī)、4號(hào)電機(jī)。

具體實(shí)施方式