[发明专利]一种水下机器人三维路径可视化跟踪方法

权利要求书

1.一种水下机器人三维路径可视化跟踪方法，其特征是：包括以下步骤：

步骤1：建立大地坐标系、载体坐标系和曲线坐标系，根据坐标系建立水下机器人的六自由度模型；

步骤2：根据建立的水下机器人的六自由度模型，建立路径跟踪误差模型，确定航向角偏差和潜浮角偏差；

步骤3：采用反步滑模控制方法，对建立的水下机器人的六自由度模型进行三维路径跟踪；

步骤4：采用深度强化学习方法对三维路径跟踪进行训练，完成水下机器人三维路径可视化跟踪。

2.根据权利要求1所述的一种水下机器人三维路径可视化跟踪方法，其特征是：所述步骤1具体为：

步骤1.1：建立大地坐标系，所述大地坐标系为海平面上的某点，大地坐标系中ξ轴正向与水下机器人AUV的主航向相同，ζ轴指向地心，ξ轴、η轴和ζ轴构成右手坐标系；

建立载体坐标系，载体坐标系的原点即为水下机器人AUV的质心，x_B轴与水下机器人AUV艏向固连，y_B轴与AUV右舷固连，x_B轴、y_B轴和z_B轴构成右手坐标系；

建立曲线坐标系，曲线坐标系原点为期望路径上的点P，x_SF沿期望路径切线方向，y_SF轴沿法线方向，x_SF轴、y_SF轴和z_SF轴构成右手坐标系；

步骤1.2：根据大地坐标系、载体坐标系和曲线坐标系，建立水下机器人的六自由度模型，所述六自由度模型包括动力学方程和运动学方程，通过下式表示动力学方程：

通过下式表示运动学方程：

其中，m为水下机器人的质量，I_y为绕y轴的转动惯量，I_z为绕z轴的转动惯量，u，v，w分别为纵向速度、横向速度和垂向速度，q，r为纵倾角速度和摇艏角速度，θ，ψ为纵倾角和艏向角，X_(·)，Y_(·)，Z_(·)，M_(·)，N_(·)均为水动力系数，z_g，z_b为重心和浮心位置，X为纵向推力，M和N为-推进器和舵联合作用产生的绕y轴和z轴的扭矩，ψ_B为水下机器人的航向角，θ_B为水下机器人的潜浮角，α为冲角，β为漂角；v_t为水下机器人的合速度。

3.根据权利要求1所述的一种水下机器人三维路径可视化跟踪方法，其特征是：所述步骤2具体为：

步骤2.1：根据建立的水下机器人的六自由度模型，在跟踪路径上定义一个虚拟的水下机器人AUV，通过下式表示虚拟的水下机器人AUV运动学方程：

其中，ψ_p和θ_p为虚拟目标的姿态角，V_p为虚拟机器人的合速度；

步骤2.2：将惯性坐标系下的真实水下机器人AUV和虚拟的水下机器人AUV位置误差转换到曲线坐标系中，通过下式表示转换过程：

对转换后的坐标系进行微分，得到误差动力学方程，通过下式表示误差动力学方程：

步骤2.3：忽略由三维空间中的非线性引起的误差，确定航向角偏差和潜浮角偏差，通过下式表示航向角偏差和潜浮角偏差：

其中，为航向角偏差，为潜浮角偏差。