[发明专利]一种适用于变结构水下机器人的运动控制方法

权利要求书

1.一种适用于变结构水下机器人的运动控制方法，其特征在于，包括以下步骤：

1)通过不同传感器分别采集变结构水下机器人的运动数据，得到变结构水下机器人在目标航迹坐标系的预测速度；

2)根据变结构水下机器人的预测速度，分别计算变结构水下机器人在相邻时刻的速度测量新息误差、量测矩阵协方差以及量测雅各比矩阵；

3)根据速度测量新息误差和量测矩阵协方差，通过离散卡尔曼滤波器得到变结构水下机器人在目标航迹坐标系的估计速度；

4)根据变结构水下机器人的估计速度，分别计算变结构水下机器人的目标航向角、目标速度、目标推力、目标转艏力矩；

5)根据变结构水下机器人的目标推力和目标转艏力矩，分配变结构水下机器人各个推进器的目标推力和目标转速，实现变结构水下机器人的运动控制；

所述步骤1)具体为：

根据变结构水下机器人当前时刻t_k的加速度和姿态角，得到变结构水下机器人在时刻t_k在目标航迹坐标系的预测速度和预测精度

其中，和分别表示时刻t_k-1目标航迹坐标系下的估计速度和估计精度，Q(k-1,k)表示变结构水下机器人在目标航迹坐标系从时刻t_k-1到时刻t_k加速度积分的累积过程驱动噪声方差，G(t)表示加速度运动过程噪声在变结构水下机器人坐标系的雅各比矩阵，θ(t),ψ(t)分别表示变结构水下机器人任意时刻t的纵倾角、航向角，a_x(t)和a_y(t)分别表示变结构水下机器人任意时刻t的前向加速度、右向加速度，A(t)表示变结构水下机器人任意时刻t的前向加速度、右向加速度在大地系投影的加速度向量，表示大地坐标系速度到目标航迹系速度的旋转函数矩阵，ψ_tr表示目标航迹角，和分别表示变结构水下机器人加速度计的前向加速度方差、右向加速度方差，表示变结构水下机器人光纤罗经的姿态角测量方差。

2.根据权利要求1所述的一种适用于变结构水下机器人的运动控制方法，其特征在于，所述步骤2)具体为：

其中，ε_k、R′_k、H_k分别表示时刻t_k目标航迹坐标系下的速度量测的新息误差、量测矩阵协方差以及量测雅各比矩阵，u_x,k和u_y,k分别表示时刻t_k变结构水下机器人的前向速度值和右向速度值，u_k表示时刻t_k变结构水下机器人的速度列向量，a_k表示由变结构水下机器人时刻t_k的前向加速度和右向加速度构成的列向量，B表示相邻时刻多普勒计程仪测量速度的相关协方差矩阵，表示任意时刻t将载体坐标系速度或加速度换算到目标航迹角系速度或加速度的旋转函数矩阵，I表示单位矩阵。

3.根据权利要求1所述的一种适用于变结构水下机器人的运动控制方法，其特征在于，所述步骤4)具体为：

在变结构水下机器人最大航速约束条件下，以变结构水下机器人当前时刻的估计速度、当前航向状态作为输入量，采用基于横向航迹误差和纵向航迹误差的复合平滑权重策略，分别计算目标航向角ψ_obj、目标速度V_obj、目标推力、目标转艏力矩M_z：

其中，T_x表示目标前向推力，T_y表示目标右向推力，e_ψ表示时刻t_k的航向角误差向量，K_M表示转艏力矩控制参数，e_v表示时刻t_k的速度误差向量，V_i⁺表示时刻t_i的估计速度，K_T表示时刻t_k的推力控制参数矩阵，X_uu表示变结构水下机器人的前向速度阻力常数，Y_vv表示变结构水下机器人的右向速度阻力常数，V_const表示变结构水下机器人的最大速度上界，K_v表示时刻t_k的速度控制参数矩阵，e_x表示时刻t_k的航迹误差向量，ψ(t)表示时刻t的航向角，X_k表示时刻t_k变结构水下机器人的位置，X_obj表示变结构水下机器人规划航迹的目标位置。