一种水下巡检机器人的轨迹跟踪控制方法技术

本发明专利技术公开了一种水下巡检机器人的轨迹跟踪控制方法。包括如下步骤：（1）、预设水下巡检机器人轨迹跟踪的参考轨迹，参考轨迹用导航路径规划算法得到从起始点到目标点的最优路径，将其作为机器人轨迹跟踪的预设参考轨迹；（2）、结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器；（3）、对水下巡检机器人作受力分析，建立动力学模型，并设计轨迹跟踪的动力学控制器；（4）、将运动学控制器的控制值作为动力学控制器的输入，得到满足轨迹跟踪所需的推力及转矩，实现水下巡检机器人平滑稳定的轨迹跟踪控制。本发明专利技术通过设计动态的目标函数，不仅解决了速度跳变问题，还减小了实际轨迹与参考轨迹之间的滞后，在目标函数中加入的最短跟踪步长优化函数项，减小了实际轨迹的路径长度，进而降低了轨迹跟踪能耗；此外还考虑实际水下环境对轨迹跟踪的影响，最终实现了平滑稳定的轨迹跟踪控制。

A trajectory tracking control method for underwater inspection robot

【技术实现步骤摘要】
一种水下巡检机器人的轨迹跟踪控制方法
本专利技术属于机器人轨迹跟踪控制
，具体涉及一种水下巡检机器人的轨迹跟踪控制方法。
技术介绍
自主水下机器人是海洋世界探索、海底资源开发及完成其他各项水下作业的重要工具。水下巡检机器人作为机器人的一个重要分支，在海底资源勘测、水下目标探测、水下管道检查、大坝裂缝检测等任务中发挥着重要的作用。三维水下环境中的轨迹跟踪控制问题是水下巡检机器人研究领域的热点及难点问题。水流等干扰因素的存在，对机器人的精确建模增加了难度，同时高度非线性和交叉耦合的系统动力学和不可预测的复杂水下环境，给控制器的设计带来相当大的困扰和不确定性。近年来，国内外研究机构在该课题上投入了大量精力，取得的科研成果也为其实际应用提供了重要的理论依据，进一步促进了水下机器人的发展与进步。但是，传统轨迹跟踪控制算法已不能满足当今水下机器人应用拓展的需要，例如反演控制、滑模控制和神经网络控制等，依然存在速度跳变、模型参数不确定以及控制律复杂度高等问题，并且没有考虑真实轨迹与期望轨迹间的滞后和轨迹跟踪能耗等问题。因此，实现水下机器人平滑稳定的轨迹本文档来自技高网...

【技术保护点】
1.一种水下巡检机器人的轨迹跟踪控制方法，其特征在于：包括如下步骤：/n(1)、预设水下巡检机器人轨迹跟踪的参考轨迹，参考轨迹用导航路径规划算法得到从起始点到目标点的最优路径，将其作为机器人轨迹跟踪的预设参考轨迹；/n(2)、结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器；/n(3)、对水下巡检机器人作受力分析，建立动力学模型，设计轨迹跟踪的动力学控制器；/n(4)、将运动学控制器的控制值作为动力学控制器的输入，得到满足轨迹跟踪所需的推力及转矩，实现水下巡检机器人平滑稳定的轨迹跟踪控制。/n

【技术特征摘要】
1.一种水下巡检机器人的轨迹跟踪控制方法，其特征在于：包括如下步骤：
(1)、预设水下巡检机器人轨迹跟踪的参考轨迹，参考轨迹用导航路径规划算法得到从起始点到目标点的最优路径，将其作为机器人轨迹跟踪的预设参考轨迹；
(2)、结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器；
(3)、对水下巡检机器人作受力分析，建立动力学模型，设计轨迹跟踪的动力学控制器；
(4)、将运动学控制器的控制值作为动力学控制器的输入，得到满足轨迹跟踪所需的推力及转矩，实现水下巡检机器人平滑稳定的轨迹跟踪控制。


2.根据权利要求1所述的一种水下巡检机器人的轨迹跟踪控制方法，其特征在于：所述步骤(2)中结合参考轨迹与运动学模型设计轨迹跟踪的运动学控制器步骤如下：
(2a)、结合参考轨迹与运动学方程建立轨迹跟踪的离散状态空间模型，具体算法如下：
将水下巡检机器人轨迹跟踪控制系统看作是带有输入量u＝[uvwqr]T和状态量ε＝[xyzθψ]T的控制系统，其运动学方程表示为对于给定的参考轨迹，轨迹上每个点都满足运动学方程，其形式表示为其中εd＝[xdydzdθdψd]T表示水下巡检机器人的期望位姿，ud＝[udvdwdqdrd]T表示水下巡检机器人的期望速度；将式的右端项在点(εd,ud)处进行泰勒级数展开，忽略高阶项只保留一次项，得到：



式中Jf(ε)和Jf(u)分别表示f相对于ε和u的雅可比矩阵；
然后将式(1)减去一般形式的运动学方程，并将其近似离散化得到离散的状态空间模型为：






式中k表示离散时域，I表示单位矩阵，T采样时间间隔，并且A(t)＝Jf(ε)和B(t)＝Jf(u)；
假定则离散状态空间模型进一步表示为：



式中和其中m＝n，m和n分别表示输入维度与状态维度；
假定系统的预测时域和控制时域分别为Np和Nc，系统在未来时刻的预测输出表示如下：
Y(k)＝ψkξ(k|k)+ΘkΔU(k)(5)
式中且



在预测时域内的系统状态量和输出量通过系统的当前状态量ξ(k|k)和控制时域内的控制增量ΔU(k)计算得到；
(2b)、设定合适的优化目标函数并对其求解得到系统的控制增量，具体算法如下：
设计目标函数：



式中Q和R分别表示系统状态量和控制量的权重矩阵，S表示J3项的权重矩阵，Δx(k+i)＝u(k)·T、Δy(k+i)＝v(k)·T和Δz(k+i)＝w(k)·T，T表示采样时间间隔；设定函数项||ε(k+i|k)-εd(k+i|k)||2和||Δx(k+i)||2+||Δy(k+i)||2+||Δz(k+i)||2的权重矩阵Q与S都是固定的单位矩阵；而函数项||Δu(k+i|k)||2的权重矩阵R随轨迹跟踪误差在线调整；
跟随轨迹跟踪误差在线调整的权重矩阵R表示如为：
R＝λI(7)
式中I表示m×m的单位矩阵，λ是跟随当前轨迹跟踪误差而在线动态调整的系数，表示为：






式中emin和emax分别表示参考跟踪精度要求而确定的两个轨迹跟踪误差边界，e表示当前轨...

【专利技术属性】
技术研发人员：范新南，吴中坚，倪建军，史朋飞，汪杰，
申请(专利权)人：河海大学常州校区，
类型：发明
国别省市：江苏;32