一种基于领航者-跟踪者水下机器人位置跟踪方法技术

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

A position tracking method for underwater vehicle based on navigator and seeker

The invention discloses a position tracking method of underwater robot based on the leader of the navigator. Through the analysis of the momentum and momentum moment of the underwater robot, the joint transformation matrix is used to establish the kinematic equation. The trajectory tracking model is established based on the leader of the navigator and the system model is based on the navigator and the follower. In order to improve the robustness of the trajectory tracking system, the disturbance observer is established to compensate the unmodeled dynamics and external disturbances of the navigator and the follower in real time. The type of information makes the control more accurate and convenient for the project realization; the target value of the trajectory tracking controller is taken as the target value, and the inverse tangent trigonometric function is based on the boundedness of the inverse tangent function, and the global position control controller is constructed to avoid the rotation of the underwater robot.

【技术实现步骤摘要】
一种基于领航者-跟踪者水下机器人位置跟踪方法
本专利技术属于水下机器人控制领域，涉及水下机器人控制方法，具体涉及一种基于领航者-跟踪者水下机器人位置跟踪方法。
技术介绍
随着人类对海洋资源的开发越来越深入，水下机器人作为人类探索未知海洋最主要的方式受到国内外的广泛关注和发展；水下机器人作为一个集导航制导、人工智能、图像识别处理、数据通信为一体的复杂系统。复杂的非线性动力学特性和自身模型的不确定性使得水下机器人的平移、旋转控制稳定性难以保证；此外，受下潜深度、水压分布和水文扰流的影响，水下机器人对外界环境十分敏感。在基于领航者-跟随者的水下机器人的轨迹跟踪问题中，一个特别重要的问题就是跟随者如何从相对于领航者的任意地方跟上领航者并保持一个相对的位置。跟随者只能获得其领航者相对于自身的相对位置信息。由于领航者和跟随者位置的获得存在一定的测量偏差和外界干扰，使得传统控制算法的鲁棒性大大下降。
技术实现思路
本专利技术的目的在于提供一种基于领航者-跟踪者水下机器人位置跟踪方法，以克服现有水下机器人在轨迹跟踪控制方面的鲁棒性弱的不足，本专利技术通过对考虑标称模型的未建模动态，设计了一种扰动观测器实时估计补偿未建模动态和外界扰动。针对水下机器人的位置子系统，考虑了工程实际中，机器人的姿态角上限带有约束，设计了一种基于反正切三角函数的位置控制器，充分利用了反正切三角函数的奇偶性和有界性的特点。为达到上述目的，本专利技术采用如下技术方案：一种基于领航者-跟踪者水下机器人位置跟踪方法，包括以下步骤：步骤一：通过对水下机器人的动量及动量矩分析，联合转换矩阵，构建水下机器人动力学模型；步骤二：基于领航者-跟随者策略，建立水下机器人轨迹跟踪动力学模型；步骤三：基于领航者和跟随者之间的系统模型信息，建立轨迹跟踪的扰动观测器；步骤四：利用领航者和跟随者的模型信息，构建包含扰动观测值的轨迹跟踪控制器；步骤五：以轨迹跟踪控制器的输出为目标值，基于反正切三角函数，构建全局位置子系统控制器，对水下机器人位置进行跟踪。进一步地，步骤一具体为：在大地坐标系下，x、y和z是机器人的位置坐标，φ、θ和是机器人的姿态角度，位置量的向量形式记为在机体坐标系下，μ、υ和ω为机器人的沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，速度量的向量形式记为ν＝[μυωpqr]T，方向满足右手螺旋定理；位置量求导的为沿大地坐标轴的速度向量，通过机体坐标系到大地坐标系的转换矩阵J(η)，能够获得ν到的转换关系：对作用于水下机器人的动量与动量矩分析，建立如下的运动学方程：其中，M是该模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力矩阵系数，τ表示控制量；系统(1)和系统(2)联立得到：其中，Mη(η)＝J-T(η)MJ-1(η)是广义惯性矩阵，Cη(ν,η)＝J-T(η)[C(ν)-MJ-1(η)]J-1(η)是广义科氏力矩阵，gη(η)＝J-T(η)g(η)是等价负浮力系数，，Dη(ν,η)＝J-T(η)D(ν)J-1(η)为等价阻尼，τC＝J-T(η)τ是等价控制量。进一步地，步骤二具体为：水下机器人的在大地坐标系中的位置为(xi,yi)，下标i＝L,F分别指的是领航者水下机器人和跟随者水下机器人，ΨL和ΨF分别是领航者和跟随者在大地坐标系下的偏航角，则水下机器人的平移方程为：其中，υix和υiy，下标i＝L,F，分别是领航者和跟随者在大地坐标系下的沿x和y轴上的速度，ωL是领航者的偏航角速度；领航者到跟随者的相对位置运动学方程为：λx＝-(xL-xF)cos(ΨL)-(yL-yF)sin(ΨL)(7)λy＝(xL-xF)sin(ΨL)-(yL-yF)cos(ΨL)(8)其中，λx和λy分别是跟随者到领航者的相对位置在领航者机体坐标系x和y轴上的投影，对运动学方程(7)-(8)求导，并将方程(4)-(6)带入，得：其中，ωF是跟随者的偏航角速度。进一步地，步骤三具体为：令和eΨ＝ΨF-ΨL，其中，和是相对位置λx和λy的目标值，然后将ex、ey和eΨ的定义带入系统(9)-(11)，得到误差系统为：将误差系统(12)-(14)写为向量形式为：其中，eλ＝[exeyeΨ]T是误差向量，u＝[υFxυFyωF]T为系统的控制量，d为外界扰动，控制量无关模型信息为控制器系数为：其中，k是正定可调参数；建立如下的非线性扰动观测器：其中，是外界扰动的估计，ξ是中间辅助变量，p(eλ)的表达式为：函数kl(eλ)设为p(eλ)对eλ的偏导数：进一步地，步骤四具体为：设计轨迹跟踪控制器为：其中，常数矩阵kP为，kP＝diag{a,a,a},a＞0；将控制器(17)带入系统(9)-(11)，得出相对位置λx和λy的更新值，然后将相对位置λx和λy带入以下公式：xFd(t)＝xL(t)+λx(t)cos(ΨL(t))-λy(t)sin(ΨL(t))yFd(t)＝yL(t)+λx(t)sin(ΨL(t))+λy(t)cos(ΨL(t))即得到跟随者的目标位置，此目标位置作为位置子系统的输入。进一步地，步骤五具体为：在水下机器人系统(3)中，定义位置与速度偏差ηe(t)＝η(t)-ηd(t)，ηd(t)是目标位置，ηe(t)是位置偏差量。和分别是速度偏差量，速度和目标速度，设计位置控制器：其中，α，β，m和n是正的可调增益，是跟踪信号的二阶导数；将位置控制器(18)带入模型(3)，即实现水下机器人的位置跟踪。与现有技术相比，本专利技术具有以下有益的技术效果：本专利技术利用扰动观测器对观测领航者和跟随者的未建模动态及外部扰动实时补偿抑制，提高了轨迹跟踪系统的鲁棒性；轨迹跟踪控制器充分考虑了模型信息，使得控制更加精确，便于工程实现；位置子系统的设计基于反正切的三角函数，充分利用了反正切函数的有界性，避免了水下机器人发生旋转。附图说明图1为本专利技术的流程图。具体实施方式下面对本专利技术作进一步详细描述：本专利技术设计了一个扰动观测器，利用水下机器人的模型信息，实时获得扰动观测值并补偿。设计带有扰动观测值的跟踪控制器，确保跟随者可以跟上领航者。工程实践中，不论模型是如何获得的，实际物理模型都与名义的数学模型存在一定的未建模偏差，并且系统中也存在环境扰动。扰动观测器将实际输出与数学模型输出的偏差看作是系统的扰动量，并计算这种扰动量实时补偿抵消掉。其次，考虑全局领航者的位置可以快速收敛于目标虚拟领航者位置的控制问题。为了防止发生翻转，实际工作中水下机器人的姿态工作范围一般是有界的。利用反正切三角函数的有界性设计位置控制子系统，使得领航者和跟随者可以平稳收敛到各自的目标位置。本专利技术将水下机器人的控制分为跟随者对领航者的轨迹跟踪和领航者、跟随者的位置控制。跟随者的轨迹跟踪控制器的输出作为自身位置控制器的输入目标值。一种基于领航者-跟踪者水下机器人位置跟踪方法，通过以下步骤实现：第一步：构建水下机器人动力学模型:在大地坐标系下，x、y和z是机器人的位置坐标，φ、θ和是机器人的姿态角度，位置量的向量形式记为在机体坐标系下，μ、υ和ω为机器人的沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，速度量的向量形式记为ν＝[μυωpqr]T，方向满足右手螺旋定理。位置量求导的为本文档来自技高网...

【技术保护点】
1.一种基于领航者‑跟踪者水下机器人位置跟踪方法，其特征在于，包括以下步骤：步骤一：通过对水下机器人的动量及动量矩分析，联合转换矩阵，构建水下机器人动力学模型；步骤二：基于领航者‑跟随者策略，建立水下机器人轨迹跟踪动力学模型；步骤三：基于领航者和跟随者之间的系统模型信息，建立轨迹跟踪的扰动观测器；步骤四：利用领航者和跟随者的模型信息，构建包含扰动观测值的轨迹跟踪控制器；步骤五：以轨迹跟踪控制器的输出为目标值，基于反正切三角函数，构建全局位置子系统控制器，对水下机器人位置进行跟踪。

【技术特征摘要】
1.一种基于领航者-跟踪者水下机器人位置跟踪方法，其特征在于，包括以下步骤：步骤一：通过对水下机器人的动量及动量矩分析，联合转换矩阵，构建水下机器人动力学模型；步骤二：基于领航者-跟随者策略，建立水下机器人轨迹跟踪动力学模型；步骤三：基于领航者和跟随者之间的系统模型信息，建立轨迹跟踪的扰动观测器；步骤四：利用领航者和跟随者的模型信息，构建包含扰动观测值的轨迹跟踪控制器；步骤五：以轨迹跟踪控制器的输出为目标值，基于反正切三角函数，构建全局位置子系统控制器，对水下机器人位置进行跟踪。2.根据权利要求1所述的一种基于领航者-跟踪者水下机器人位置跟踪方法，其特征在于，步骤一具体为：在大地坐标系下，x、y和z是机器人的位置坐标，φ、θ和是机器人的姿态角度，位置量的向量形式记为在机体坐标系下，μ、υ和ω为机器人的沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，速度量的向量形式记为ν＝[μυωpqr]T，方向满足右手螺旋定理；位置量求导的为沿大地坐标轴的速度向量，通过机体坐标系到大地坐标系的转换矩阵J(η)，能够获得ν到的转换关系：对作用于水下机器人的动量与动量矩分析，建立如下的运动学方程：其中，M是该模型的惯性矩阵，C(ν)是科氏力矩阵，D(ν)代表系统阻尼矩阵，g(η)是负浮力矩阵系数，τ表示控制量；系统(1)和系统(2)联立得到：其中，Mη(η)＝J-T(η)MJ-1(η)是广义惯性矩阵，Cη(ν,η)＝J-T(η)[C(ν)-MJ-1(η)]J-1(η)是广义科氏力矩阵，gη(η)＝J-T(η)g(η)是等价负浮力系数，，Dη(ν,η)＝J-T(η)D(ν)J-1(η)为等价阻尼，τC＝J-T(η)τ是等价控制量。3.根据权利要求2所述的一种基于领航者-跟踪者水下机器人位置跟踪方法，其特征在于，步骤二具体为：水下机器人的在大地坐标系中的位置为(xi,yi)，下标i＝L,F分别指的是领航者水下机器人和跟随者水下机器人，ΨL和ΨF分别是领航者和跟随者在大地坐标系下的偏航角，则水下机器人的平移方程为：其中，υix和υiy，下标i＝L,F，分别是领航者和跟随者在大地坐标系下的沿x和y轴上的速度，ωL是领航者的偏航...

【专利技术属性】
技术研发人员：袁源，成磊，袁建平，
申请(专利权)人：西北工业大学，
类型：发明
国别省市：陕西,61