[发明专利]一种基于领航者-跟踪者水下机器人位置跟踪方法

说明书

本发明公开了一种基于领航者‑跟踪者水下机器人位置跟踪方法，通过对水下机器人的动量及动量矩分析，联合转换矩阵，建立运动学方程；基于领航者‑跟随者策略，建立轨迹跟踪模型；基于领航者和跟随者之间的系统模型信息，建立轨迹跟踪的扰动观测器，对观测领航者和跟随者的未建模动态及外部扰动实时补偿抑制，提高了轨迹跟踪系统的鲁棒性；充分利用领航者和跟随者的模型信息，构建包含扰动观测值的轨迹跟踪控制器，充分考虑了模型信息，使得控制更加精确，便于工程实现；以轨迹跟踪控制器的输出为目标值，基于反正切三角函数，充分利用了反正切函数的有界性，构建全局位置控制控制器，避免水下机器人发生旋转。

技术领域

本发明属于水下机器人控制领域，涉及水下机器人控制方法，具体涉及一种基于领航者-跟踪者水下机器人位置跟踪方法。

背景技术

随着人类对海洋资源的开发越来越深入，水下机器人作为人类探索未知海洋最主要的方式受到国内外的广泛关注和发展；水下机器人作为一个集导航制导、人工智能、图像识别处理、数据通信为一体的复杂系统。复杂的非线性动力学特性和自身模型的不确定性使得水下机器人的平移、旋转控制稳定性难以保证；此外，受下潜深度、水压分布和水文扰流的影响，水下机器人对外界环境十分敏感。

在基于领航者-跟随者的水下机器人的轨迹跟踪问题中，一个特别重要的问题就是跟随者如何从相对于领航者的任意地方跟上领航者并保持一个相对的位置。跟随者只能获得其领航者相对于自身的相对位置信息。由于领航者和跟随者位置的获得存在一定的测量偏差和外界干扰，使得传统控制算法的鲁棒性大大下降。

发明内容

本发明的目的在于提供一种基于领航者-跟踪者水下机器人位置跟踪方法，以克服现有水下机器人在轨迹跟踪控制方面的鲁棒性弱的不足，本发明通过对考虑标称模型的未建模动态，设计了一种扰动观测器实时估计补偿未建模动态和外界扰动。针对水下机器人的位置子系统，考虑了工程实际中，机器人的姿态角上限带有约束，设计了一种基于反正切三角函数的位置控制器，充分利用了反正切三角函数的奇偶性和有界性的特点。

为达到上述目的，本发明采用如下技术方案：

一种基于领航者-跟踪者水下机器人位置跟踪方法，包括以下步骤：

步骤一：通过对水下机器人的动量及动量矩分析，联合转换矩阵，构建水下机器人动力学模型；

步骤二：基于领航者-跟随者策略，建立水下机器人轨迹跟踪动力学模型；

步骤三：基于领航者和跟随者之间的系统模型信息，建立轨迹跟踪的扰动观测器；

步骤四：利用领航者和跟随者的模型信息，构建包含扰动观测值的轨迹跟踪控制器；

步骤五：以轨迹跟踪控制器的输出为目标值，基于反正切三角函数，构建全局位置子系统控制器，对水下机器人位置进行跟踪。

进一步地，步骤一具体为：在大地坐标系下，x、y和z是机器人的位置坐标，φ、θ和是机器人的姿态角度，位置量的向量形式记为在机体坐标系下，μ、υ和ω为机器人的沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，速度量的向量形式记为ν＝[μ υ ω p q r]^T，方向满足右手螺旋定理；位置量求导的为沿大地坐标轴的速度向量，通过机体坐标系到大地坐标系的转换矩阵J(η)，能够获得ν到的转换关系：

对作用于水下机器人的动量与动量矩分析，建立如下的运动学方程：

其中，M是该模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力矩阵系数，τ表示控制量；

系统(1)和系统(2)联立得到：