本发明属于水下机器人控制领域，涉及水下机器人控制方法，具体涉及一种基于领航者-跟踪者水下机器人位置跟踪方法。
背景技术：
随着人类对海洋资源的开发越来越深入，水下机器人作为人类探索未知海洋最主要的方式受到国内外的广泛关注和发展；水下机器人作为一个集导航制导、人工智能、图像识别处理、数据通信为一体的复杂系统。复杂的非线性动力学特性和自身模型的不确定性使得水下机器人的平移、旋转控制稳定性难以保证；此外，受下潜深度、水压分布和水文扰流的影响，水下机器人对外界环境十分敏感。
在基于领航者-跟随者的水下机器人的轨迹跟踪问题中，一个特别重要的问题就是跟随者如何从相对于领航者的任意地方跟上领航者并保持一个相对的位置。跟随者只能获得其领航者相对于自身的相对位置信息。由于领航者和跟随者位置的获得存在一定的测量偏差和外界干扰，使得传统控制算法的鲁棒性大大下降。
技术实现要素：
本发明的目的在于提供一种基于领航者-跟踪者水下机器人位置跟踪方法，以克服现有水下机器人在轨迹跟踪控制方面的鲁棒性弱的不足，本发明通过对考虑标称模型的未建模动态，设计了一种扰动观测器实时估计补偿未建模动态和外界扰动。针对水下机器人的位置子系统，考虑了工程实际中，机器人的姿态角上限带有约束，设计了一种基于反正切三角函数的位置控制器，充分利用了反正切三角函数的奇偶性和有界性的特点。
为达到上述目的，本发明采用如下技术方案：
一种基于领航者-跟踪者水下机器人位置跟踪方法，包括以下步骤：
步骤一：通过对水下机器人的动量及动量矩分析，联合转换矩阵，构建水下机器人动力学模型；
步骤二：基于领航者-跟随者策略，建立水下机器人轨迹跟踪动力学模型；
步骤三：基于领航者和跟随者之间的系统模型信息，建立轨迹跟踪的扰动观测器；
步骤四：利用领航者和跟随者的模型信息，构建包含扰动观测值的轨迹跟踪控制器；
步骤五：以轨迹跟踪控制器的输出为目标值，基于反正切三角函数，构建全局位置子系统控制器，对水下机器人位置进行跟踪。
进一步地，步骤一具体为：在大地坐标系下，x、y和z是机器人的位置坐标，φ、θ和是机器人的姿态角度，位置量的向量形式记为在机体坐标系下，μ、υ和ω为机器人的沿机体坐标轴的线速度，p、q和r是机器人绕机体坐标轴的角速度，速度量的向量形式记为ν＝[μ υ ω p q r]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λ＝[ex ey eΨ]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是机器人绕机体坐标轴的角速度，速度量的向量形式记为ν＝[μ υ ω p q r]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λ＝[ex ey eΨ]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是机器人绕机体坐标轴的角速度，速度量的向量形式记为ν＝[μ υ ω p q r]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(η)，Cη(ν,η)＝J-T(η)[C(ν)-MJ-1(η)]J-1(η)，Dη(ν,η)＝J-T(η)D(ν)J-1(η)和τC＝J-T(η)τ。
相关的参数定义如下：
rB＝[xB,yB,zB]T＝[0,0,0]T，rG＝[xG,yG,zG]T＝[0,0,0.05]T，m＝125，
C(ν)＝CA(ν)+CRB(ν)，
D(ν)＝diag{48|μ|,48|υ|,48|ω|,110|p|,110|q|,110|r|}，
其中，xB，yB和zB是机器人在机体坐标系下的浮心坐标，xG，yG和zG表示机器人质心坐标，m表示机器人质量，I0表示转动惯量矩阵，ν1＝[μ υ ω]T和ν2＝[p q r]T是速度ν的平移速度分量和角速度分量，CA(ν)和CRB(ν)分别是所排流体的运动所具有的柯氏矩阵和柯氏矩阵。
第二步：构建水下机器人轨迹跟踪动力学模型:
水下机器人的在大地坐标系中的位置为(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)带入，得：
第三步：设计轨迹跟踪扰动观测器:
令和eΨ＝ΨF-ΨL，其中，和是相对位置λx和λy的目标值。然后将ex、ey和eΨ的定义带入系统(9)-(11)，得到误差系统为：
将误差系统(12)-(14)写为向量形式为：
其中，eλ＝[ex ey eΨ]T是误差向量，u＝[υFx υFy ωF]T为系统的控制量，d为外界扰动，控制量无关模型信息为控制器系数为：
建立如下的非线性扰动观测器：
其中，k是一个正定标量，p(eλ)的表达式为：
函数kl(eλ)设为p(eλ)对eλ的偏导数：
可调参数k设置为：
k＝0.1
第四步：设计水下机器人轨迹跟踪控制器:
设计轨迹跟踪控制器为：
其中，常数矩阵kP为，kP＝diag{a,a,a},a＞0。
可调参数a的取值如下：
a＝1
将控制器带入系统(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)，设计位置控制子系统：
其中是跟踪信号的二阶导数。α、β、m和n是控制器增益，都是正常数。
可调参数α、β、m和n的取值如下：
α＝diag{2,2,4,3,3,4}
β＝diag{0.6,0.6,0.5,0.4,0.4,0.4}
m＝diag{6,6,3,5,5,5}
n＝diag{30,30,55,47,47,50}。