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

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

【技术实现步骤摘要】
一种适用于变结构水下机器人的运动控制方法
本专利技术涉及水下机器人的运动控制
，尤其涉及变结构水下机器人运动控制方法，实现变结构水下机器人在机械臂扰动条件下的高精度航迹控制。
技术介绍
在海洋工程、海洋石油工业、海洋考古等领域，水下机器人得到广泛应用。水下机器人主要包括遥控水下机器人和自主水下机器人，遥控水下机器人依托脐带电缆作为供电链路和通信操控链路，同时遥控水下机器人配置机械手作为水下作业工具，承担水下固定目标抓取、海洋设备维修、石油管线检测等水下作业任务，遥控水下机器人的优点是作业能力强，缺点是受到脐带电缆的约束，作业半径有限；自主水下机器人本身携带能源，不依托脐带电缆供电和通信，具有搭载水文、声学、光学等多种载荷的能力，承担水下目标搜寻、海底地形地貌测绘、海洋水文调查等探测任务，自主水下机器人的优点是不受脐带电缆的约束，作业半径较大，缺点是自身携带能源，对本体流体线型要求较高，无法搭载破坏流体线型的机械手，作业能力较弱。与传统水下机器人不同，变结构水下机器人同时具有自主水下机器人、遥控水下机器人的优点，变结构水下机器人是搭载具有一定作业能力的机械手的新型水下机器人，它的技术特征是自身携带能源、无脐带电缆约束、低阻力流线外形、具有变形能力。当处于航行区时，变结构水下机器人回收机械手，变形为自主水下机器人形态进行航行；当到达作业区时，变结构水下机器人释放机械手，变形为遥控水下机器人形态进行作业。，而变结构水下机器人运动控制是变结构水下机器人的关键技术问题，其中变结构水下机器人控制技术的难点是如何解决变结构水下机器人在机械手扰动条件下的高精度航迹运动控制问题。传统机械手的扰动分析方法是从机械手传感器数据或机械手动力学模型角度来估计机械手对变结构水下机器人的运动扰动，它的缺点是严重依赖机械手的传感器和机械手的动力学模型，制约了变结构水下机器人控制方法的应用范围和鲁棒性。
技术实现思路
变结构水下机器人从航行区域到达作业模式后，需要将放置在机器人内部的机械手摆动到机器人艏部。与传统水下机器人采用静止作业模式不同，变结构水下机器人采用边航行边作业的模式来提高作业效率，面临的最大技术难题是如何克服机械手摆动对变结构水下机器人的运动控制扰动。传统机械手的扰动分析方法是基于机械手传感器数据或机械手动力学模型来估计机械手对变结构水下机器人的运动扰动，本专利技术创新地从速度滤波角度分析机械手扰动，采用基于相邻时刻测量相关性的速度滤波技术，统一机械手扰动和变结构水下机器人本身扰动的观测模型，降低扰动分析对动力学模型的依赖性，再根据速度反馈测量和目标航迹误差权重，考虑变结构水下机器人的航行能力约束，实时计算变结构水下机器人的目标航向和目标速度等航迹控制量，实现了变结构水下机器人在机械手扰动条件下的高精度航迹控制。本专利技术为实现上述目的所采用的技术方案是：一种适用于变结构水下机器人的运动控制方法，包括以下步骤：1)通过不同传感器分别采集变结构水下机器人的运动数据，得到变结构水下机器人在目标航迹坐标系的预测速度；2)根据变结构水下机器人的预测速度，分别计算变结构水下机器人在相邻时刻的速度测量新息误差、量测矩阵协方差以及量测雅各比矩阵；3)根据速度测量新息误差和量测矩阵协方差，通过离散卡尔曼滤波器得到变结构水下机器人在目标航迹坐标系的估计速度；4)根据变结构水下机器人的估计速度，分别计算变结构水下机器人的目标航向角、目标速度、目标推力、目标转艏力矩；5)根据变结构水下机器人的目标推力和目标转艏力矩，分配变结构水下机器人各个推进器的目标推力和目标转速，实现变结构水下机器人的运动控制。所述步骤1)具体为：根据变结构水下机器人当前时刻tk的加速度和姿态角，得到变结构水下机器人在时刻tk在目标航迹坐标系的预测速度Vk-和预测精度Pk-：其中，和分别表示时刻tk-1目标航迹坐标系下的估计速度和估计精度，Q(k-1,k)表示变结构水下机器人在目标航迹坐标系从时刻tk-1到时刻tk加速度积分的累积过程驱动噪声方差，G(t)表示加速度运动过程噪声在变结构水下机器人坐标系的雅各比矩阵，θ(t),ψ(t)分别表示变结构水下机器人任意时刻t的纵倾角、航向角，ax(t)和ay(t)分别表示变结构水下机器人任意时刻t的前向加速度、右向加速度，A(t)表示变结构水下机器人任意时刻t的前向加速度、右向加速度在大地系投影的加速度向量，表示大地坐标系速度到目标航迹系速度的旋转函数矩阵，ψtr表示目标航迹角，和分别表示变结构水下机器人加速度计的前向加速度方差、右向加速度方差，表示变结构水下机器人光纤罗经的姿态角测量方差。所述步骤2)具体为：其中，εk、Rk′、Hk分别表示时刻tk目标航迹坐标系下的速度量测的新息误差、量测矩阵协方差以及量测雅各比矩阵，ux,k和uy,k分别表示时刻tk变结构水下机器人的前向速度值和右向速度值，uk表示时刻tk变结构水下机器人的速度列向量，ak表示由变结构水下机器人时刻tk的前向加速度和右向加速度构成的列向量，B表示相邻时刻多普勒计程仪测量速度的相关协方差矩阵，表示任意时刻t将载体坐标系速度或加速度换算到目标航迹角系速度或加速度的旋转函数矩阵，I表示单位矩阵。所述步骤4)具体为：在变结构水下机器人最大航速约束条件下，以变结构水下机器人当前时刻的估计速度、当前航向状态作为输入量，采用基于横向航迹误差和纵向航迹误差的复合平滑权重策略，分别计算目标航向角ψobj、目标速度Vobj、目标推力、目标转艏力矩Mz：其中，Tx表示目标前向推力，Ty表示目标右向推力，eψ表示时刻tk的航向角误差向量，KM表示转艏力矩控制参数，ev表示时刻tk的速度误差向量，Vi+表示时刻ti的估计速度，KT表示时刻tk的推力控制参数矩阵，Xuu表示变结构水下机器人的前向速度阻力常数，Yvv表示变结构水下机器人的右向速度阻力常数，Vconst表示变结构水下机器人的最大速度上界，Kv表示时刻tk的速度控制参数矩阵，ex表示时刻tk的航迹误差向量，ψ(t)表示时刻t的航向角，Xk表示时刻tk变结构水下机器人的位置，Xobj表示变结构水下机器人规划航迹的目标位置。本专利技术具有以下有益效果及优点：1.相对于传统水下机器人控制方法，本专利技术不依赖机械手的动力学模型，具有更好的算法鲁棒性，实现了变结构水下机器人在机械手扰动条件下的高精度航迹控制，提升了变结构水下机器人的水下作业能力。2.应用范围广。本专利技术不但可以应用于变结构水下机器人平台水下作业工况，还可以用于遥控水下机器人的水下目标抓取、石油管线清理等海洋工程应用场景。3.经济成本较低。本专利技术不依赖机械手的传感器数据，简化了机械手的传感器配置，降低了机械手的设计成本。附图说明图1是本专利技术的变结构水下机器人结构图；图2是本专利技术的变结构水下机器人控制方法的流程图。具体实施方式下面结合附图及实施例对本专利技术做进一步的详细说明。整个本文档来自技高网...

【技术保护点】
1.一种适用于变结构水下机器人的运动控制方法，其特征在于，包括以下步骤：/n1)通过不同传感器分别采集变结构水下机器人的运动数据，得到变结构水下机器人在目标航迹坐标系的预测速度；/n2)根据变结构水下机器人的预测速度，分别计算变结构水下机器人在相邻时刻的速度测量新息误差、量测矩阵协方差以及量测雅各比矩阵；/n3)根据速度测量新息误差和量测矩阵协方差，通过离散卡尔曼滤波器得到变结构水下机器人在目标航迹坐标系的估计速度；/n4)根据变结构水下机器人的估计速度，分别计算变结构水下机器人的目标航向角、目标速度、目标推力、目标转艏力矩；/n5)根据变结构水下机器人的目标推力和目标转艏力矩，分配变结构水下机器人各个推进器的目标推力和目标转速，实现变结构水下机器人的运动控制。/n

【技术特征摘要】
1.一种适用于变结构水下机器人的运动控制方法，其特征在于，包括以下步骤：
1)通过不同传感器分别采集变结构水下机器人的运动数据，得到变结构水下机器人在目标航迹坐标系的预测速度；
2)根据变结构水下机器人的预测速度，分别计算变结构水下机器人在相邻时刻的速度测量新息误差、量测矩阵协方差以及量测雅各比矩阵；
3)根据速度测量新息误差和量测矩阵协方差，通过离散卡尔曼滤波器得到变结构水下机器人在目标航迹坐标系的估计速度；
4)根据变结构水下机器人的估计速度，分别计算变结构水下机器人的目标航向角、目标速度、目标推力、目标转艏力矩；
5)根据变结构水下机器人的目标推力和目标转艏力矩，分配变结构水下机器人各个推进器的目标推力和目标转速，实现变结构水下机器人的运动控制。


2.根据权利要求1所述的一种适用于变结构水下机器人的运动控制方法，其特征在于，所述步骤1)具体为：
根据变结构水下机器人当前时刻tk的加速度和姿态角，得到变结构水下机器人在时刻tk在目标航迹坐标系的预测速度和预测精度



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

【专利技术属性】
技术研发人员：王轶群，田启岩，李硕，张奇峰，赵宏宇，翟新宝，董旭洋，周雪山，白金刚，
申请(专利权)人：中国科学院沈阳自动化研究所，
类型：发明
国别省市：辽宁;21