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

权利要求书

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(η)τ是等价控制量；

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

具体为：水下机器人的在大地坐标系中的位置为(x_i,y_i)，下标i＝L,F分别指的是领航者水下机器人和跟随者水下机器人，Ψ_L和Ψ_F分别是领航者和跟随者在大地坐标系下的偏航角，则水下机器人的平移方程为：

其中，υ_ix和υ_iy，下标i＝L,F，分别是领航者和跟随者在大地坐标系下的沿x和y轴上的速度，ω_L是领航者的偏航角速度；

领航者到跟随者的相对位置运动学方程为：

λ_x＝-(x_L-x_F)cos(Ψ_L)-(y_L-y_F)sin(Ψ_L) (7)

λ_y＝(x_L-x_F)sin(Ψ_L)-(y_L-y_F)cos(Ψ_L) (8)

其中，λ_x和λ_y分别是跟随者到领航者的相对位置在领航者机体坐标系x和y轴上的投影，对运动学方程(7)-(8)求导，并将方程(4)-(6)带入，得：

其中，ω_F是跟随者的偏航角速度；

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

具体为：令和e_Ψ＝Ψ_F-Ψ_L，其中，和是相对位置λ_x和λ_y的目标值，然后将e_x、e_y和e_Ψ的定义带入公式(9)-(11)，得到误差公式为：

将误差公式(12)-(14)写为向量形式为：

其中，e_λ＝[e_x e_y e_Ψ]^T是误差向量，u＝[υ_Fx υ_Fy ω_F]^T为系统的控制量，d为外界扰动，控制量无关模型信息为控制器系数为：

其中，k是正定可调参数；

建立如下的非线性扰动观测器：

其中，是外界扰动的估计，ξ是中间辅助变量，p(e_λ)的表达式为：

函数kl(e_λ)设为p(e_λ)对e_λ的偏导数：

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

具体为：设计轨迹跟踪控制器为：

其中，常数矩阵k_P为，k_P＝diag{a,a,a},a＞0；

将控制器(17)带入公式(9)-(11)，得出相对位置λ_x和λ_y的更新值，然后将相对位置λ_x和λ_y带入以下公式：

x_Fd(t)＝x_L(t)+λ_x(t)cos(Ψ_L(t))-λ_y(t)sin(Ψ_L(t))

y_Fd(t)＝y_L(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)，即实现水下机器人的位置跟踪。