[发明专利]欠驱动自主水下航行器的对三维路径进行理解及跟踪控制方法

权利要求书

1.一种欠驱动自主水下航行器的对三维路径进行理解及跟踪控制方法，其特征是：

步骤一：将全局路径规划所得的三维路径理解为空间直线段序列；

步骤二：将空间直线段描述为自主水下航行器的目标直线段，进行惯性坐标系下的投影，在水平面形成二维直线，在垂直方向形成深度和高度坐标序列；

步骤三：欠驱动控制形式的自主水下航行器，对单个的水平面目标直线段进行跟踪控制，控制器采用分层的结构，位于上层的制导控制器将位置偏差转化为参考艏向角，位于下层的状态控制器将艏向角偏差转化为操纵面的转动执行角度；

步骤四：进行步骤二所述目标直线段的更替，并依次运行步骤三完成每个目标直线段的跟踪，最终完成空间直线段序列的跟踪，实现三维路径跟踪；

对全局路径的理解方式具体为：

整体上将给定的三维路径转化为空间直线段序列，依据全局路径的表示形式采用不同的理解方法，所述不同的理解方法包括：

(1)如果全局路径以离散点序列p＝{p₀,p₁,p₂,…,p_M}表示，则依据三次B样条理论进行插值计算，首先求解离散点所定义的样条曲线的解析表达式，依据公式：

其次按照一定的精细程度要求，选择变量t的值，给出均匀的点序列p′＝{p′₀,p′₁,p′₂,…,p′_Q}，Q＝M·s，s是与t有关的变量表示分辨率，

有了均匀的点序列，则直线段序列的表达式为：

l＝{l_0-1,l_1-2,l_2-3,…,l_i-i+1,…,l_Q-1-Q},i＝0,1,2,…,Q-1，

单个目标直线段的表达式：

(2)如果全局路径具有解析表达式：l＝L(t),t∈(a,b)，那么均匀取t的值，得到点序列，后续步骤同(1)；

对单个的水平面目标直线段进行跟踪控制的跟踪控制器具体为：

制导控制器以位置偏差为控制输入，以智能PID算法为控制率，输出参考艏向角；状态控制器以参考艏向角偏差为控制输入，以自适应S面算法为控制率，输出操纵面的转动执行角度；

制导控制器通过几何计算的方式计算载体与目标直线段之间的横向偏差P_e，以此作为智能PID制导控制器的输入，其中三维空间的直线航线S在水平面E-ξη的投影为ST₀-T₁|ζ＝0，一维平面上由点T₀(ξ₀,η₀)和T₁(ξ₁,η₁)所确定的方向线段，T₀≠T₁，自主水下航行器的位置P_s(ξ_s,η_s)，航向为ψ_s，包含以下步骤：

(1)计算横向距离，即偏差绝对值，公式为：

(2)计算正负，规定自主水下航行器位于方向线段T₀T₁的左侧时P_e＞0：

sgn(P_e)＝sgn[(ξ₁-ξ₀)·(η₁-η_s)-(η₁-η₀)·(η₁-η_s)]，

(3)计算横向偏差；

P_e＝sgn(P_e)·|P_e|；

PID算法采用差分形式：

n为时钟节拍，M_I为积分开始的时钟节拍，M_D为近几个计算周期偏差平均变化率计算的开始节拍；k_p，k_i，k_d分别为比例积分微分项系数，依据P_e和的变化通过模糊自适应的方式进行调整：

状态控制器采用自适应参数调整的S面控制方法，表达式如下：

e、为控制输入分别表示艏向角偏差和偏差变化率，f为控制输出、物理意义为归一化的转艏力矩，k_e、k_v为控制参数，依据模糊自适应的方式进行调整

依据自主水下航行器与目标直线段的相对位置关系进行目标的更替，具体为：

计算向量与向量的夹角，如果模超过，则认为应该进行目标的更替。