水下機(jī)器人位姿控制方法
【專利摘要】本發(fā)明提供一種水下機(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ī)器人位姿控制方法，可以使機(jī)器人在水下具有能抵抗環(huán)境擾動(dòng)的姿態(tài)穩(wěn)定性，確保機(jī)器人順利的完成細(xì)致，復(fù)雜的工作。
【專利說明】水下機(jī)器人位姿控制方法

【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種水下機(jī)器人位姿控制方法。

【背景技術(shù)】
[0002] 近年來，水下機(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ù)雜 的工作。
[0003] 但是目前，許多水下機(jī)器人在水下姿態(tài)自動(dòng)調(diào)整方面沒有投入太大的精力，機(jī)器 人定點(diǎn)懸浮，抵抗擾流的能力較弱。機(jī)器人在水下完成的任務(wù)有限。


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

[0004] 本發(fā)明的目的是提供一種水下機(jī)器人位姿控制方法，能夠?qū)崿F(xiàn)機(jī)器人在水中的姿 態(tài)平穩(wěn)，解決現(xiàn)有技術(shù)中存在的機(jī)器人定點(diǎn)懸浮，抵抗擾流的能力較弱，機(jī)器人在水下完成 的任務(wù)有限的問題。
[0005] 水下機(jī)器人，又稱無人潛水器，是一種作業(yè)于水下的極限作業(yè)機(jī)器人；
[0006] 位姿：位姿分為位置和姿態(tài)，是描述機(jī)器人在空間中的位置和其自身的姿態(tài)；
[0007] 歐拉角：確定定點(diǎn)轉(zhuǎn)動(dòng)剛體位置的3個(gè)一組獨(dú)立角參量，分別有偏航角Ψ，俯仰角 Θ，翻滾角Φ。
[0008] 本發(fā)明的技術(shù)解決方案是：
[0009] 一種水下機(jī)器人位姿控制方法，
[0010] 采用平衡結(jié)構(gòu)設(shè)計(jì)，對(duì)對(duì)稱的框架結(jié)構(gòu)進(jìn)行建模，
[0011] 傳感器測(cè)得機(jī)器人姿態(tài)的俯仰角、翻滾角、偏航角；
[0012] 與期望值做差值得到誤差，通過PID算法對(duì)誤差進(jìn)行調(diào)節(jié)，對(duì)電機(jī)進(jìn)行控制，使姿 態(tài)保持平衡。
[0013] 進(jìn)一步地，平衡結(jié)構(gòu)為重心向下、對(duì)稱的自穩(wěn)定框架結(jié)構(gòu)。
[0014] 進(jìn)一步地，平衡結(jié)構(gòu)具體為：采用方形框架結(jié)構(gòu)，八個(gè)電機(jī)對(duì)稱的安裝在框架上， 形成對(duì)稱的結(jié)構(gòu)，八個(gè)電機(jī)分成兩個(gè)部分，
[0015] 其中，四個(gè)電機(jī)分別堅(jiān)直的安裝在框架的四個(gè)直立的桿子上，通過正反轉(zhuǎn)控制機(jī) 器人的上升和下降；另外四個(gè)電機(jī)安裝在框架的底部，控制機(jī)器人的前進(jìn)后退。
[0016] 進(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ī)器人所在水中的深度。
[0017] 進(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)。
[0018] 進(jìn)一步地，實(shí)現(xiàn)水下機(jī)器人姿態(tài)的控制：
[0019] 首先進(jìn)行數(shù)學(xué)建模，采用歐拉角與四元數(shù)兩種工具相結(jié)合的方法描述機(jī)器人在水 下的姿態(tài)，先用傳感器測(cè)定機(jī)器人姿態(tài)的四元數(shù)（《，X，y，Z)，再將四元數(shù)轉(zhuǎn)換成歐拉角得到 偏航角（Ψ)，俯仰角（Θ)，翻滾角（Φ);堅(jiān)直放置的電機(jī)控制俯仰角、翻滾角，當(dāng)機(jī)器人的 姿態(tài)不是水平時(shí)，俯仰角和翻滾角就不是零，輸入的俯仰角和翻滾角就是誤差；
[0020] 然后通過PID算法控制四個(gè)堅(jiān)直方向的電機(jī)，將俯仰角和翻滾角調(diào)整為零。
[0021] 進(jìn)一步地，利用如下公式（1)將四元數(shù)轉(zhuǎn)換成歐拉角得到偏航角（Ψ)，俯仰角 (Θ)，翻滾角（Φ)，

【權(quán)利要求】
1. 一種水下機(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)保 持平衡。
2. 如權(quán)利要求1所述的水下機(jī)器人位姿控制方法，其特征在于：平衡結(jié)構(gòu)為重心向下、 對(duì)稱的自穩(wěn)定框架結(jié)構(gòu)。
3. 如權(quán)利要求2所述的水下機(jī)器人位姿控制方法，其特征在于，平衡結(jié)構(gòu)具體為：采用 方形框架結(jié)構(gòu)，八個(gè)電機(jī)對(duì)稱的安裝在框架上，形成對(duì)稱的結(jié)構(gòu)，八個(gè)電機(jī)分成兩個(gè)部分， 其中，四個(gè)電機(jī)分別堅(jiān)直的安裝在框架的四個(gè)直立的桿子上，通過正反轉(zhuǎn)控制機(jī)器人 的上升和下降；另外四個(gè)電機(jī)安裝在框架的底部，控制機(jī)器人的前進(jìn)后退。
4. 如權(quán)利要求3所述的水下機(jī)器人位姿控制方法，其特征在于：傳感器系統(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ī)器人所在水中的深度。
5. 如權(quán)利要求4所述的水下機(jī)器人位姿控制方法，其特征在于： 仿照四旋翼飛行器對(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)。
6. 如權(quán)利要求1-5任一項(xiàng)所述的水下機(jī)器人位姿控制方法，其特征在于，實(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)換成歐拉角得到偏航 角（Ψ)，俯仰角（Θ)，翻滾角（Φ);堅(jiān)直放置的電機(jī)控制俯仰角、翻滾角，當(dāng)機(jī)器人的姿態(tài) 不是水平時(shí)，俯仰角和翻滾角就不是零，輸入的俯仰角和翻滾角就是誤差； 然后通過PID算法控制四個(gè)堅(jiān)直方向的電機(jī)，將俯仰角和翻滾角調(diào)整為零。
7. 如權(quán)利要求6所述的水下機(jī)器人位姿控制方法，其特征在于，利用如下公式（1)將四 元數(shù)轉(zhuǎn)換成歐拉角得到偏航角（Ψ)，俯仰角（Θ)，翻滾角（Φ)，
(1)〇
8. 如權(quán)利要求7所述的水下機(jī)器人位姿控制方法，其特征在于，當(dāng)機(jī)器人偏離任務(wù)水 深時(shí)，液位變送器所測(cè)的深度與原來的深度相比較得到誤差，然后利用PID算法，使機(jī)器人 保持在任務(wù)水深。
9. 如權(quán)利要求8所述的水下機(jī)器人位姿控制方法，其特征在于，當(dāng)機(jī)器人偏離理想直 線時(shí)，電子羅盤的數(shù)值發(fā)生變化，這就是方向的誤差，利用PID算法，對(duì)誤差進(jìn)行糾正，確保 機(jī)器人運(yùn)動(dòng)保持理想直線狀態(tài)。
【文檔編號(hào)】G05D1/08GK104155991SQ201410422945
【公開日】2014年11月19日 申請(qǐng)日期:2014年8月25日 優(yōu)先權(quán)日:2014年8月25日
【發(fā)明者】陳巍, 湯忠強(qiáng), 羅浩玨, 陳絲雨 申請(qǐng)人:南京工程學(xué)院