一种基于领航者-跟踪者水下机器人位置跟踪方法与流程

技术特征：

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

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

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

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

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

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

2.根据权利要求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(η)τ是等价控制量。

3.根据权利要求2所述的一种基于领航者-跟踪者水下机器人位置跟踪方法，其特征在于，步骤二具体为：水下机器人的在大地坐标系中的位置为(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是跟随者的偏航角速度。

4.根据权利要求3所述的一种基于领航者-跟踪者水下机器人位置跟踪方法，其特征在于，步骤三具体为：令和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λ的偏导数：

5.根据权利要求4所述的一种基于领航者-跟踪者水下机器人位置跟踪方法，其特征在于，步骤四具体为：设计轨迹跟踪控制器为：

其中，常数矩阵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))

即得到跟随者的目标位置，此目标位置作为位置子系统的输入。

6.根据权利要求5所述的一种基于领航者-跟踪者水下机器人位置跟踪方法，其特征在于，步骤五具体为：在水下机器人系统(3)中，定义位置与速度偏差ηe(t)＝η(t)-ηd(t)，ηd(t)是目标位置，ηe(t)是位置偏差量，和分别是速度偏差量，速度和目标速度，设计位置控制器：

其中，α，β，m和n是正的可调增益，是跟踪信号的二阶导数；

将位置控制器(18)带入模型(3)，即实现水下机器人的位置跟踪。