主权项 |
1.一种基于二阶滤波器的水下机器人三维路径跟踪控制方法,其特征在于包含以下步骤:步骤1.建立固定坐标系、机器人载体坐标系和Serret-Frenet坐标系,获取期望路径,水下机器人开始路径跟踪,完成两个二阶滤波器的初始化;步骤2.通过水下机器人搭载的定位声纳传感器、姿态传感器,采集水下机器人当前位置、姿态角、角速度和速度数据信息,并结合期望路径的方向与速度,根据视线角导引思想计算得到水下机器人理想的姿态控制量ψ<sub>co</sub>、θ<sub>co</sub>,和理想速度控制量u<sub>co</sub>;步骤3.将步骤2中得到的理想控制量ψ<sub>co</sub>、θ<sub>co</sub>、u<sub>co</sub>输入至基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器,得到水下机器人的姿态与速度控制量ψ<sub>c</sub>、θ<sub>c</sub>、u<sub>c</sub>,及其导数<img file="FDA0000411496380000011.GIF" wi="280" he="83" />结合机器人运动变量ψ、θ、u,得到滤波姿态与速度跟踪误差量<img file="FDA0000411496380000012.GIF" wi="161" he="71" /><img file="FDA0000411496380000013.GIF" wi="43" he="59" />与理想角速度控制量r<sub>co</sub>、q<sub>co</sub>;步骤4.将步骤3中得到的水下机器人理想角速度控制量r<sub>co</sub>、q<sub>co</sub>输入至另一个基于水下机器人三维路径跟踪运动学误差模型所建立的二阶滤波器,得到水下机器人的角速度控制量r<sub>c</sub>、q<sub>c</sub>,及其导数<img file="FDA0000411496380000014.GIF" wi="168" he="71" />结合机器人角运动变量r、q,得到滤波角速度跟踪误差量<img file="FDA0000411496380000015.GIF" wi="162" he="72" />步骤5.利用步骤3中得到的滤波姿态与速度跟踪误差量<img file="FDA0000411496380000016.GIF" wi="247" he="71" />以及步骤4中得到的滤波角速度跟踪误差量<img file="FDA0000411496380000017.GIF" wi="162" he="72" />解算得到水下机器人推进器推力F<sub>u</sub>,与水平舵角δ<sub>s</sub>、垂直舵角δ<sub>r</sub>,分别作用于机器人推进器及舵机,实现三维路径跟踪控制;步骤6.利用步骤3中得到的水下机器人姿态与速度控制量ψ<sub>c</sub>、θ<sub>c</sub>、u<sub>c</sub>,滤波姿态与速度跟踪误差量<img file="FDA0000411496380000018.GIF" wi="220" he="72" />与理想角速度控制量r<sub>co</sub>、q<sub>co</sub>,结合步骤4中得到的水下机器人的角速度控制量r<sub>c</sub>、q<sub>c</sub>,以及滤波角速度跟踪误差量<img file="FDA0000411496380000019.GIF" wi="167" he="72" />构造滤波误差补偿回路;步骤7.计算当前水下机器人位置η<sup>n</sup>=(x,y,z)与标定的转向点WP<sub>k</sub>=(x<sub>k</sub>,y<sub>k</sub>,z<sub>k</sub>)之间的距离<img file="FDA00004114963800000110.GIF" wi="777" he="108" />若小于设定的切换半径R,则表示完成当前指定路径的跟踪任务,否则继续步骤2。 |