[發明專利]基于二階濾波器的水下機器人三維路徑跟蹤控制方法

有效

• 申請號: 201310553699.X
• 申請日: 2013-11-11
• 公開（公告）號: CN103576693A
• 公開（公告）日: 2014-02-12
• 發明（設計）人: 王宏健;陳子印;邊信黔;李娟;嚴浙平;陳興華
• 申請（專利權）人: 哈爾濱工程大學
• 主分類號: G05D1/10
• 分類號: G05D1/10
• 代理公司: 暫無信息
• 代理人: 暫無信息

1.一種基于二階濾波器的水下機器人三維路徑跟蹤控制方法，其特征在于包含以下步驟：

步驟1.建立固定坐標系、機器人載體坐標系和Serret-Frenet坐標系，獲取期望路徑，水下機器人開始路徑跟蹤，完成兩個二階濾波器的初始化；

步驟2.通過水下機器人搭載的定位聲納傳感器、姿態傳感器，采集水下機器人當前位置、姿態角、角速度和速度數據信息，并結合期望路徑的方向與速度，根據視線角導引思想計算得到水下機器人理想的姿態控制量ψ_co、θ_co，和理想速度控制量u_co；

步驟3.將步驟2中得到的理想控制量ψ_co、θ_co、u_co輸入至基于水下機器人三維路徑跟蹤運動學誤差模型所建立的二階濾波器，得到水下機器人的姿態與速度控制量ψ_c、θ_c、u_c，及其導數結合機器人運動變量ψ、θ、u，得到濾波姿態與速度跟蹤誤差量與理想角速度控制量r_co、q_co；

步驟4.將步驟3中得到的水下機器人理想角速度控制量r_co、q_co輸入至另一個基于水下機器人三維路徑跟蹤運動學誤差模型所建立的二階濾波器，得到水下機器人的角速度控制量r_c、q_c，及其導數結合機器人角運動變量r、q，得到濾波角速度跟蹤誤差量

步驟5.利用步驟3中得到的濾波姿態與速度跟蹤誤差量以及步驟4中得到的濾波角速度跟蹤誤差量解算得到水下機器人推進器推力F_u，與水平舵角δ_s、垂直舵角δ_r，分別作用于機器人推進器及舵機，實現三維路徑跟蹤控制；

步驟6.利用步驟3中得到的水下機器人姿態與速度控制量ψ_c、θ_c、u_c，濾波姿態與速度跟蹤誤差量與理想角速度控制量r_co、q_co，結合步驟4中得到的水下機器人的角速度控制量r_c、q_c，以及濾波角速度跟蹤誤差量構造濾波誤差補償回路；

步驟7.計算當前水下機器人位置ηⁿ=(x,y,z)與標定的轉向點WP_k=(x_k,y_k,z_k)之間的距離若小于設定的切換半徑R，則表示完成當前指定路徑的跟蹤任務，否則繼續步驟2。