[发明专利]基于二阶滤波器的水下机器人三维路径跟踪控制方法

权利要求书

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。