CN113190025B - 一种适用于变结构水下机器人的运动控制方法 - Google Patents

Abstract

本发明涉及水下机器人的运动控制技术领域，尤其涉及变结构水下机器人运动控制方法，实现变结构水下机器人在机械臂扰动条件下的高精度航迹控制。本发明从速度滤波角度分析机械手扰动，采用基于相邻时刻测量相关性的速度滤波技术，统一机械手扰动和变结构水下机器人本身扰动的观测模型，降低扰动分析对动力学模型的依赖性，再根据速度反馈测量和目标航迹误差权重，考虑变结构水下机器人航行能力约束，实时计算变结构水下机器人的目标航向和目标速度等航迹控制量，实现变结构水下机器人在机械手扰动条件下的高精度航迹控制。

Description

一种适用于变结构水下机器人的运动控制方法

技术领域

本发明涉及水下机器人的运动控制技术领域，尤其涉及变结构水下机器人运动控制方法，实现变结构水下机器人在机械臂扰动条件下的高精度航迹控制。

背景技术

在海洋工程、海洋石油工业、海洋考古等领域，水下机器人得到广泛应用。水下机器人主要包括遥控水下机器人和自主水下机器人，遥控水下机器人依托脐带电缆作为供电链路和通信操控链路，同时遥控水下机器人配置机械手作为水下作业工具，承担水下固定目标抓取、海洋设备维修、石油管线检测等水下作业任务，遥控水下机器人的优点是作业能力强，缺点是受到脐带电缆的约束，作业半径有限；自主水下机器人本身携带能源，不依托脐带电缆供电和通信，具有搭载水文、声学、光学等多种载荷的能力，承担水下目标搜寻、海底地形地貌测绘、海洋水文调查等探测任务，自主水下机器人的优点是不受脐带电缆的约束，作业半径较大，缺点是自身携带能源，对本体流体线型要求较高，无法搭载破坏流体线型的机械手，作业能力较弱。与传统水下机器人不同，变结构水下机器人同时具有自主水下机器人、遥控水下机器人的优点，变结构水下机器人是搭载具有一定作业能力的机械手的新型水下机器人，它的技术特征是自身携带能源、无脐带电缆约束、低阻力流线外形、具有变形能力。当处于航行区时，变结构水下机器人回收机械手，变形为自主水下机器人形态进行航行；当到达作业区时，变结构水下机器人释放机械手，变形为遥控水下机器人形态进行作业。，而变结构水下机器人运动控制是变结构水下机器人的关键技术问题，其中变结构水下机器人控制技术的难点是如何解决变结构水下机器人在机械手扰动条件下的高精度航迹运动控制问题。传统机械手的扰动分析方法是从机械手传感器数据或机械手动力学模型角度来估计机械手对变结构水下机器人的运动扰动，它的缺点是严重依赖机械手的传感器和机械手的动力学模型，制约了变结构水下机器人控制方法的应用范围和鲁棒性。

发明内容

变结构水下机器人从航行区域到达作业模式后，需要将放置在机器人内部的机械手摆动到机器人艏部。与传统水下机器人采用静止作业模式不同，变结构水下机器人采用边航行边作业的模式来提高作业效率，面临的最大技术难题是如何克服机械手摆动对变结构水下机器人的运动控制扰动。传统机械手的扰动分析方法是基于机械手传感器数据或机械手动力学模型来估计机械手对变结构水下机器人的运动扰动，本发明创新地从速度滤波角度分析机械手扰动，采用基于相邻时刻测量相关性的速度滤波技术，统一机械手扰动和变结构水下机器人本身扰动的观测模型，降低扰动分析对动力学模型的依赖性，再根据速度反馈测量和目标航迹误差权重，考虑变结构水下机器人的航行能力约束，实时计算变结构水下机器人的目标航向和目标速度等航迹控制量，实现了变结构水下机器人在机械手扰动条件下的高精度航迹控制。

本发明为实现上述目的所采用的技术方案是：

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

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

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

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

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

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

所述步骤1)具体为：

根据变结构水下机器人当前时刻t_k的加速度和姿态角，得到变结构水下机器人在时刻t_k在目标航迹坐标系的预测速度V_k-和预测精度P_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)具体为：

其中，ε_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表示单位矩阵。

所述步骤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表示变结构水下机器人规划航迹的目标位置。

本发明具有以下有益效果及优点：

1.相对于传统水下机器人控制方法，本发明不依赖机械手的动力学模型，具有更好的算法鲁棒性，实现了变结构水下机器人在机械手扰动条件下的高精度航迹控制，提升了变结构水下机器人的水下作业能力。

2.应用范围广。本发明不但可以应用于变结构水下机器人平台水下作业工况，还可以用于遥控水下机器人的水下目标抓取、石油管线清理等海洋工程应用场景。

3.经济成本较低。本发明不依赖机械手的传感器数据，简化了机械手的传感器配置，降低了机械手的设计成本。

附图说明

图1是本发明的变结构水下机器人结构图；

图2是本发明的变结构水下机器人控制方法的流程图。

具体实施方式

下面结合附图及实施例对本发明做进一步的详细说明。

整个系统组成包括变结构水下机器人、变结构水下机器人搭载的多普勒计程仪、光纤罗经、加速度计、机械手等设备。其中变结构水下机器人是搭载多普勒计程仪、光纤罗经、加速度计、机械手的运载具，多普勒计程仪、光纤罗经、加速度计是变结构水下机器人的内部导航传感器，多普勒计程仪的作用是测量变结构水下机器人相对水底在载体坐标系的运动速度；光纤罗经的作用是测量变结构水下机器人的航向角、纵倾角、横滚角等姿态角；加速度计的作用是测量变结构水下机器人相对载体坐标系的加速度；机械手的作用是承担水下目标抓取等水下作业任务。

系统组成如图1所示。

如图2所示，整个系统按照如下流程工作：

定义和/>表示变结构水下机器人加速度计的前向加速度方差、右向加速度方差，它是加速度计的设备参数，是已知量，通常情况下二者值是相等的；/>和/>表示变结构水下机器人多普勒计程仪的前向速度测量随机误差的方差、右向速度测量误差的方差，它是多普勒计程仪的设备参数，是已知量，通常情况下二者值是相等的；/>和/>表示变结构水下机器人在相邻时刻多普勒计程仪的前向速度误差的相关协方差、右向速度误差的相关协方差，它们是描述多普勒计程仪数据质量受水声环境、变结构水下机器人设备电环境、变结构水下机器人执行机构噪声等因素干扰程度的物理量，它们是由变结构机器人设计师通过实航数据回归分析获得，是已知量，通常情况下二者值是相等的，定义B表示相邻时刻多普勒计程仪测量速度的相关协方差矩阵，即/>它是已知量。/>表示变结构水下机器人光纤罗经的姿态角测量方差，它是光纤罗经的设备参数，是已知量；定义t表示时间变量，t_k表示第k个时刻，k表示时间索引，且t_k-1≤t_k。

为了方便描述矩阵和向量的运算，定义T表示矩阵或向量的转置算子，||·||表示向量的2范数算子，定义表示根据目标航迹角换算大地坐标系速度到目标航迹系速度的旋转函数矩阵，定义dlg(·)表示对角矩阵算子，定义atan(·)表示计算向量的相位角函数，定义tanh(·)表示正切双曲计算函数。

第一步，预测变结构水下机器人在目标航迹坐标系的速度

根据变结构水下机器人当前时刻t_k的加速度和姿态角预测变结构水下机器人在时刻t_k在目标航迹坐标系的速度和精度，定义V_k ^-和P_k ^-表示变结构水下机器人时刻t_k在目标航迹坐标系的预测速度和预测精度，它们的计算方法如下：

其中V_k ^-和P_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的前向加速度a_x(t)、右向加速度a_y(t)在大地系投影的加速度向量，它是中间变量；/>表示根据目标航迹角ψ_tr换算大地坐标系速度到目标航迹系速度的旋转函数矩阵，是中间变量；ψ_tr表示目标航迹角，是已知量。

第二步，计算速度量测的新息误差、量测矩阵协方差、量测雅各比矩阵

在实际使用中，变结构水下机器人多普勒计程仪在相邻时刻的速度测量噪声存在某种程度的数据关联，即多普勒计程仪的速度测量噪声是自相关有色噪声，有色速度测量噪声是制约速度估计精度改善的关键因素。针对相邻时刻多普勒速度测量的有色噪声问题，采用相邻时刻速度测量非等比例差值法构建速度测量差分观测模型，计算速度量测的新息误差、量测矩阵协方差、量测雅各比矩阵，它们的计算方法如下：

其中ε_k、R_k′和H_k是待求解变量；ε_k表示时刻t_k目标航迹坐标系下的速度量测的新息误差，R_k′表示时刻t_k目标航迹坐标系下速度测量的量测矩阵协方差，H_k表示时刻t_k目标航迹坐标系下的速度量测的量测雅各比矩阵。定义u_x,k表示时刻t_k多普勒计程仪测量的前向速度值，u_y,k表示时刻t_k多普勒计程仪测量的右向速度值，u_k表示时刻t_k多普勒计程仪测量的速度列向量，它们是多普勒计程仪的测量输出值，是已知量；同理，u_x,k+1表示时刻t_k+1多普勒计程仪测量的前向速度值，u_y,k+1表示时刻t_k+1多普勒计程仪测量的右向速度值，u_k+1表示时刻t_k+1多普勒计程仪测量的速度列向量，它们是多普勒计程仪的测量输出值，是已知量；定义a_k表示由变结构水下机器人时刻t_k的前向加速度a_x,k、右向加速度a_y,k构成的列向量，它们是变结构水下机器人搭载的加速度计的测量值，是已知量；V_k ^-表示变结构水下机器人时刻t_k在目标航迹坐标系的预测速度，它是上一个步骤的计算结果，是已知量；B表示相邻时刻多普勒计程仪测量速度的相关协方差矩阵，它是已知量；Q(k-1,k)表示变结构水下机器人在目标航迹坐标系从时刻t_k-1到时刻t_k加速度积分的累积过程驱动噪声方差，它是上一个步骤的计算结果，是已知量；表示任意时刻t将载体坐标系速度或加速度换算到目标航迹角ψ_tr系速度或加速度的旋转函数矩阵，它是一个函数计算方法矩阵；/>表示时刻t_k对应的旋转函数矩阵，即令变量t取值为时刻t_k，它是已知量；I表示单位矩阵，它是常量。

第三步，修正速度预测误差，计算变结构水下机器人在目标航迹坐标系的速度

其中V_k ⁺和P_k ⁺是待求解变量，V_k ⁺和P_k ⁺表示时刻t_k目标航迹坐标系下的速度估计和精度估计；S_k是新息的协方差，它是中间变量；R_k′表示时刻t_k目标航迹坐标系下速度测量的量测矩阵协方差，它是上一个步骤的计算结果，是已知量；ε_k表示时刻t_k目标航迹坐标系下的速度量测的新息误差，它是上一个步骤的计算结果，是已知量。

第四步，计算目标航向角、目标速度、目标推力、目标转艏力矩

在变结构水下机器人最大航速约束条件下，以变结构水下机器人当前时刻速度、当前航向状态等作为输入量，采用基于横向航迹误差和纵向航迹误差的复合平滑权重策略，计算目标航向角、目标速度、目标推力、目标转艏力矩等控制量，实现变结构水下机器人高精度沿规划路径到达目标点。目标航向角、目标速度、目标推力、目标转艏力矩的计算方法如下：

其中，M_z表示目标转艏力矩，ψ_obj表示目标航向角，V_obj表示目标速度，T_x表示目标前向推力，T_y表示目标右向推力，它们是待求解变量；V_obj表示目标速度，它是2维列向量，第1个元素表示前向速度，第2个元素表示右向目标速度，它是待求解变量；e_ψ表示时刻t_k的航向角误差向量，它是3维列向量，它是中间变量；K_M表示转艏力矩控制参数，它是3维行向量，该参数用户可以自行设定，它是已知常量；e_v表示时刻t_k的速度误差向量，它是3维列向量，它是中间变量；V_i ⁺表示时刻t_i的速度，它是已知量；K_T表示时刻t_k的推力控制参数矩阵，它是2行6列的矩阵，该参数用户可以自行设定，是已知量；X_uu表示变结构水下机器人的前向速度阻力常数，它是已知量；Y_vv表示变结构水下机器人的右向速度阻力常数，它是已知量；V_const表示变结构水下机器人的最大速度上界，它是已知量；表示时刻t_k变结构水下机器人从载体坐标系到航迹坐标系的旋转矩阵，它是已知量；K_v表示时刻t_k的速度控制参数矩阵，它是2行12列矩阵，它是已知量；/>表示根据目标航迹角ψ_tr换算大地坐标系速度到目标航迹系速度的旋转函数矩阵，是中间变量；ψ_tr表示目标航迹角，是已知量；e_x表示时刻t_k的航迹误差向量，它是3维列向量，它是中间变量；ψ(t)表示时刻t的航向角，ψ(t_k-1)表示时刻t_k-1的航向角，ψ(t_k)表示时刻t_k的航向角，它们是光纤罗经的测量输出，是已知量；X_k表示时刻t_k变结构水下机器人的位置，它是二维列向量，第1个表示北向位置，第2个元素表示东向位置，它是已知量；X_k-1表示时刻t_k-1变结构水下机器人的位置，它是已知量；X_obj表示变结构水下机器人规划航迹的目标位置，它是已知量；V_k ⁺是表示时刻t_k目标航迹坐标系下的速度估计，它是上一步的计算结果，是已知量。

第五步，根据目标推力和目标转艏力矩，计算各个推进器的推力分配和目标转速。

根据水下机器人通用标准矢量推力分配方法，将变结构水下机器人的目标推力和目标转艏力矩分解为各个推进器的目标推力和目标转速，实现水下机器人的运动控制。

本发明是周期循环迭代调用步骤一到步骤五，本周期的输入是上一个周期的计算结果，本周期的输出是下一个周期的输入，往复迭代，实现变结构水下机器人实时在线的航行运动控制。

Claims (3)

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表示变结构水下机器人规划航迹的目标位置。