[發明專利]水下機器人及其水中高可視度情況下姿態與運動控制方法

有效

• 申請號: 202110769162.1
• 申請日: 2021-10-08
• 公開（公告）號: CN113602462B
• 公開（公告）日: 2022-09-23
• 發明（設計）人: 李宏勝;盧欣;卞舒豪;陳瑋;江銘煜;楊雪;劉燁;張家輝
• 申請（專利權）人: 南京工程學院
• 主分類號: B63G8/14
• 分類號: B63G8/14;B63G8/00;B63C11/52;G06F30/15;G06F30/17;G06F30/20;G06F17/16
• 代理公司: 南京蘇高專利商標事務所(普通合伙) 32204
• 代理人: 柏尚春

1.一種水下機器人在水中高可視度情況下的姿態與運動控制方法，水下機器人包括AUV機器人本體和控制模塊，所述AUV機器人本體包括機械固定結構(6)，所述機械固定結構(6)為鋁型材搭建的矩形框架；所述機械固定結構(6)頂部固定連接有PVC浮力材料(1)；四個相同的水下推進器(2)通過推進器固定件(3)固定于材機械固定結構(6)頂部四個角上；另外四個相同的水下推進器(2)通過推進器固定件(3)分別固定于四個支撐板(5)上，每個支撐板(5)的兩端分別固定于機械固定結構(6)的兩條底邊上；AUV控制艙體(4)固定于材機械固定結構(6)上，AUV控制艙體(4)內設有陀螺儀、深度計、加速度計和電池，機械固定結構(6)的底部安裝配重材料(7)；所述控制模塊包括設于地面的單目測距攝像頭、無線手柄、水陸信息交互功能的微型可移動地面站；所述機械固定結構(6)采用歐式鋁型材搭建而成，并通過M5×3cm尺寸螺絲和歐標螺母實現鋁型材之間的固定；所述支撐板(5)通過M5×5cm尺寸螺絲和歐標螺母安裝機械固定結構(6)的兩條底邊上；其特征在于，包括如下步驟：

步驟一、設置控制模塊，確保與AUV水下機器人本體各項連接正確；

步驟二、根據剛體在水中運動的動力學和運動學模型推算AUV水下無纜機器人在機體坐標系下的動力學和運動學模型，并根據機體的機械結構和水中工作狀態對其進行簡化；

步驟三、根據AUV水下無纜機器人的推進器布局進行力學分析，采用分離控制的方法分別對機體的垂直方向和水平方向進行分析和設計；

步驟四、通過機體內置的陀螺儀、加速度計獲取機體的實時角速度和加速度，其中加速度包括機體三軸加速度a_x、a_y、a_z，角速度包括翻滾角速度俯仰角速度和偏航角速度并根據機體加速度和角速度分別求解機體相對姿態，將二者經過卡爾曼融合算法后的結果作為機體實際姿態；

步驟五、通過機體內置的深度計和可移動地面站連接的單目攝像頭獲取機體的實時位置信息，通過將單目攝像頭的像素坐標系下的像素坐標與其對應的機體坐標系下的位置坐標進行數據擬合，并通過解算出的函數關系求解出AUV機體水面折射倒影位置，根據機體深度信息以及水的折射原理計算出機體實際水平位置，從而進行AUV機體的水平位置測距；

步驟六、通過可移動地面站所連接的無線手柄控制機體的運動，當接收到可移動地面站發送的停止信號時，機體以當前的姿態與位置信息作為水下機器人的期望姿態穩定參數包括翻滾角Φ、俯仰角θ、偏航角Ψ，和位置穩定參數包括水平位置X和Y、深度Z；

步驟七、水下機器人根據可移動地面站發送停止信號時機體的姿態穩定參數和位置穩定參數作為期望值，與通過慣性測量元件采集到的機體的慣性數據解算出的姿態角信息作比較，并根據計算出的偏差值分別引入姿態角度角速度雙閉環PID算法和位置環單閉環PID算法進行調節，從而對機體的姿態與位置狀態進行控制；

步驟八、根據步驟二建立的AUV水下無纜機器人的動力學和運動學模型以及步驟三建立的AUV水下無纜機器人的力學模型對姿態角度角速度雙閉環PID算法和位置環單閉環PID輸出項依次進行運動學動力學處理和力學處理，根據運動學動力學模型解算出控制機體姿態和運動的合力或合力矩，再根據力學模型解算出對應的水下推進器轉速，從而通過對水下機器人推進器進行油門控制實現AUV水下無纜機器人的姿態和運動控制；

步驟九、判斷機體當前的姿態和位置信息是否滿足機體的期望值，即機體的姿態穩定參數和位置穩定參數，如不滿足則執行步驟七，如滿足則執行步驟十；

步驟十、AUV水下無纜機器人達到機體期望的姿態穩定參數和位置穩定參數，機體繼續接收可移動地面站控制信號，如果接收到運動信號后機體按照控制要求進行運動，并在下次獲取到停止信號時重新執行步驟七，如未接收到，則控制推進器保持機體當前姿態和位置不發生變化。