[发明专利]一种基于航向平滑技术的自主水下机器人路径跟踪方法

权利要求书

1.一种基于航向平滑技术的自主水下机器人路径跟踪方法，其特征在于：所述方法具体过程为：

步骤一、测量初始时刻AUV的状态测量值，设置AUV的期望路径p(σ)；

步骤二、测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向角与期望的航向角ψ(σ(t))的差值；

步骤三、对步骤二得到的当前的航向角与期望的航向角ψ(σ(t))的差值进行优化，得到输入给控制器的参考航向；

将输入给控制器的参考航向和步骤二得到AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离结合，得到优化后的路径跟踪误差e_p(t)；

步骤四、使优化后的路径跟踪误差e_p(t)收敛，得到AUV的控制输入，AUV的控制输入包括控制AUV的力矩和力；

步骤五、判断AUV是否走完跟踪路径，若走完跟踪路径，得到AUV的控制输入；若没有走完跟踪路径，重新执行步骤二到步骤五，直至AUV走完跟踪路径；

所述步骤二中测量当前AUV的状态测量值，根据当前AUV的状态测量值和AUV的期望路径p(σ)得到AUV的路径跟踪误差；具体过程为：

设置AUV水平面运动数学模型：

其中η＝[x′ y ψ]^T为大地坐标系下的坐标和姿态角，x′、y为大地坐标系下AUV的位置，ψ为大地坐标系下AUV艏向，上角标T表示求转置，为大地坐标系与随体坐标系之间的关系，R(ψ)为水平面坐标变换矩阵；M＝M_RB+M_A为惯性矩阵；M_RB为刚体惯性阵，M_A为附加质量阵，v＝[u′ v′ r]^T为随体坐标系下的速度向量，包括运动的速率和角速度；u′为随体坐标系下AUV的纵向速率，v′为随体坐标系下AUV的横向速率，r为随体坐标系下AUV的转艏速率，为随体坐标系下的加速度，C(v)＝C_RB(v)+C_A(v)为科氏力和向心力的矩阵；C_RB(v)为科氏力的矩阵；C_A(v)为向心力的矩阵；D(v)为流体阻尼力和力矩对AUV产生的作用；τ_thr为随体坐标系下作用于机器人上的推力和力矩，即机器人的控制输入；τ_env为环境对AUV作用的干扰力；

将上述AUV水平面运动数学模型简化为如下形式：

其中x为AUV的状态量，u为AUV的控制输入，f为足够连续可微的非线性函数；

设置一条规则曲线Ρ

其中p(σ)为期望路径，σ为路径参数；为路径参数的上界；

曲线Ρ定义在映射p:上；

根据公式(3)，定义路径跟踪误差为：

e_p(t)＝x(t)-p(σ(t)) (4)

其中e_p为路径跟踪误差，x(t)为AUV的状态量，p(σ(t))为期望路径；

考虑当前AUV的位姿状态[x,y,ψ],以及路径p:x＝α₁(σ),y＝α₂(σ)，寻找曲线上离AUV当前状态最近的点为路径参考点，即归纳为如下形式：

其中α₁(σ)、α₂(σ)为参数方程；

通过求解上述优化问题式(5)，得到当前时刻的期望点的参数值σ(t)，即当前时刻的期望点的位置(x(σ(t)),y(σ(t)))；

期望航向角通过下式计算得到：

其中atan2是求方位角的函数，定义域范围为ψ∈(-π,π]；为参数方程；

路径跟踪误差e_p(t)包含AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离，当前的航向角与期望的航向角ψ(σ(t))的差值；

所述步骤三中对步骤二得到的当前的航向角与期望的航向角ψ(σ(t))的差值进行优化，得到输入给控制器的参考航向；具体过程为：

将单位圆分为四个象限，第一象限角度为(0，π/2)，第二象限角度为(0，-π/2)，第三象限角度为(-π/2，-π)，第四象限角度为(π/2，π)；1表示第一象限，2表示第二象限，3表示第三象限，4表示第四象限；

1)

若S1＝1，即对应前一个时刻期望角度在第一象限；

11)当S2＝1,2,4时，Δψ_R＝ψ_n-ψ_p；

其中，S1为单位圆上atan2函数在前一个时刻的位置，即一个表示前一个时刻所在象限的状态机；S2为单位圆上atan2函数在当前时刻的状态，ψ_n为单位圆上atan2函数在当前时刻的角度值；ψ_p为单位圆上atan2函数在前一个时刻的角度值；Δψ_R为航向角每个时刻绝对变化量；ψ_R为所有之前的时刻累积的角度信息，即参考模型中的航向角；

12)当S2＝3时，

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

其中abs()为绝对值函数；

2)

若S1＝2，即对应前一个时刻期望角度在第二象限；

21)当S2＝1,2,3时，

Δψ_R＝ψ_n-ψ_p；

22)当S2＝4时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

3)

若S1＝3，即对应前一个时刻期望角度在第三象限；

31)当S2＝1时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

32)当S2＝2,3时，

Δψ_R＝ψ_n-ψ_p

33)当S2＝4时，

Δψ_R＝-(π+ψ_p)-(π-ψ_n)＝ψ_n-ψ_p-2π

4)

若S1＝4，即对应前一个时刻期望角度在第四象限；

41)当S2＝1，4时，

Δψ_R＝ψ_n-ψ_p

42)当S2＝2时：

若(abs(ψ_n)+ψ_p)≤π

则Δψ_R＝ψ_n-ψ_p

若(abs(ψ_n)+ψ_p)＞π

则

43)当S2＝3时，

Δψ_R＝(π-ψ_p)+(π+ψ_n)＝ψ_n-ψ_p+2π

再利用式(6)得到AUV的参考模型中的航向角ψ_R；

将航向角ψ_R反映射回(-π,π]区间中，获得输入给控制器的参考航向；

将输入给控制器的参考航向和步骤二得到AUV当前的位置与当前时刻的期望点的位置(x(σ(t)),y(σ(t)))的距离结合，得到优化后的路径跟踪误差e_p(t)。