[發明專利]水下機器人位姿控制方法

在審

• 申請號: 201410422945.2
• 申請日: 2014-08-25
• 公開（公告）號: CN104155991A
• 公開（公告）日: 2014-11-19
• 發明（設計）人: 陳巍;湯忠強;羅浩玨;陳絲雨
• 申請（專利權）人: 南京工程學院
• 主分類號: G05D1/08
• 分類號: G05D1/08
• 代理公司: 南京正聯知識產權代理有限公司 32243
• 代理人: 沈志海

1.一種水下機器人位姿控制方法，其特征在于：

采用平衡結構設計，對對稱的框架結構進行建模，

傳感器測得機器人姿態的俯仰角、翻滾角、偏航角；

與期望值做差值得到誤差，通過PID算法對誤差進行調節，對電機進行控制，使姿態保持平衡。

2.如權利要求1所述的水下機器人位姿控制方法，其特征在于：平衡結構為重心向下、對稱的自穩定框架結構。

3.如權利要求2所述的水下機器人位姿控制方法，其特征在于，平衡結構具體為：采用方形框架結構，八個電機對稱的安裝在框架上，形成對稱的結構，八個電機分成兩個部分，

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

4.如權利要求3所述的水下機器人位姿控制方法，其特征在于：傳感器系統包括MPU6050三軸陀螺儀、加速度計、AK8975電子羅盤、HSTL-18液位變送器；MPU6050三軸陀螺儀、加速度計測得機器人在x,y,z軸上旋轉的角度以及加速度，經過轉換成四元數；AK8975電子羅盤測得機器人方向的磁偏角；HSTL-18液位變送器測量出機器人所在水中的深度。

5.如權利要求4所述的水下機器人位姿控制方法，其特征在于：

仿照四旋翼飛行器對水下機器人進行數學建模；傳感器MPU6050三軸陀螺儀與加速度計，AK8975電子羅盤，HSTL-18液位變送器分別測出機器人姿態的四元數，偏航角和深度；利用PID算法對誤差調節，輸出量控制電機，使機器人的位姿保持平穩。

6.如權利要求1-5任一項所述的水下機器人位姿控制方法，其特征在于，實現水下機器人姿態的控制：

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

然后通過PID算法控制四個豎直方向的電機，將俯仰角和翻滾角調整為零。

7.如權利要求6所述的水下機器人位姿控制方法，其特征在于，利用如下公式(1)將四元數轉換成歐拉角得到偏航角(ψ)，俯仰角(θ)，翻滾角(Φ)，

8.如權利要求7所述的水下機器人位姿控制方法，其特征在于，當機器人偏離任務水深時，液位變送器所測的深度與原來的深度相比較得到誤差，然后利用PID算法，使機器人保持在任務水深。

9.如權利要求8所述的水下機器人位姿控制方法，其特征在于，當機器人偏離理想直線時，電子羅盤的數值發生變化，這就是方向的誤差，利用PID算法，對誤差進行糾正，確保機器人運動保持理想直線狀態。