[发明专利]一种无人艇路径跟踪导引策略和扰动补偿方法

权利要求书

1.一种无人艇路径跟踪导引策略和扰动补偿方法，其特征在于，具体包括以下步骤：

步骤1、建立位置误差系统；

将USV在北东坐标系下位置误差转换到SF坐标架下的位置误差，具体公式为：

式中(x_F,y_F)是期望全局路径上的路径点坐标；

步骤2、从运动学角度出发，设计基于流干扰观测器补偿和障碍李雅普诺夫函数即BLF的改进型ILOS导引算法；

步骤2.1、BLF对无人艇位置误差的限定：

考虑到USV在海上航行时受到的海流等环境干扰，将USV进行全局路径跟踪时产生的横向位置误差和纵向位置误差进行误差限制，避免USV因横向位置误差过大而撞上海上的静态碍航物，采取的手段是基于BLF的位置误差约束；采取对称型BLF来对ILOS导引算法中的位置误差系统(x_e,y_e)进行限制，具体公式为：

0≤||x_e||＜k_b,0≤||y_e||＜k_b

后面遇到的位置误差系统(x_e,y_e)均为受限制的，其中k_b为一正常数；

步骤2.2、流干扰观测器对海流的观测补偿：

考虑到李雅普诺夫能量函数：

对上式求时间的导数，有下面等式成立：

式中分别是对应变量的观测器设计；

为稳定设计下面的观测器：

联立上式有如下等式成立：

式中为海流干扰观测器，为进一步稳定设计下面的海流干扰观测器：

得到：

综上，可得到海流干扰观测器的设计为：

步骤2.3、设计BILOS导引算法的三大导引律：

通过BILOS闭环导引，整个导引算法分为三个部分，一是通过BLF技术对USV当前位置与全局期望路径上位置的误差进行限制，防止USV在航行过程中撞上静态碍航物，二是通过流干扰观测器和漂角观测器对BILOS导引的积分动作进行补偿，设计USV的期望艏向ψ_d，可传递给控制层设计控制器；三是设计虚拟控制律，也就是期望路径参数的更新速率，建立期望路径点与USV航行状态的动态协作关系；通过导引设计将路径跟踪转化为艏向跟踪和速度控制，简化控制器设计，具体表达式为：

步骤3、设计基于干扰补偿器补偿的自适应反步滑模路径跟踪控制器；

步骤3.1、干扰补偿器对海上风浪等复杂环境的观测和补偿：

海上风浪等复杂环境的数学模型如下：

式中，C_X和C_Y为风力系数，C_N为风力矩系数，ρ_a(kg/m³)为空气密度，A_T(m²)和A_L(m²)为正投影面积和侧投影面积，L_v(m)为无人艇总长。V_r的单位为节；

式中χ是浪向，ρ是海水密度，L是船长。波频S_i(t)由下面的公式给出：

式中A_i是波的幅度，λ_i为波长、w_ei、φ_i是时变的频率与相位；

干扰补偿器对海上不确定风浪等干扰的观测；

干扰补偿器的原理是将外部环境扰动引起的实际系统输出与标称模型输出之间的差异看作为作用在系统上的等效扰动，是基于标称模型的，然后在控制器的设计中将其作为补偿信号，以前馈的形式抵消环境干扰对系统造成的影响，实现环境干扰的完全抑制；

针对具体的USV动力学数学模型，设计如下不确定环境干扰下的扰动观测器，并将扰动误差以指数形式收敛于很小的原点领域内：

式中，是分别观测横向风浪等干扰力、纵向风浪等干扰力和艏向风浪等干扰力矩的观测值，V＝[u,v,r]^T是无人艇的速度向量，M是系统矩阵，C(V)是刚体科里奥利力向心力矩阵，D(V)是阻尼矩阵，K₀是观测系数矩阵；

步骤3.2、自适应反步滑模纵向推力控制律设计：

步骤3.3、自适应反步滑模艏向力矩控制律设计：

2.根据权利要求1所述一种无人艇路径跟踪导引策略和扰动补偿方法，其特征在于，所述BILOS导引算法是从ILOS导引算法改进过来的，基于LOS导引和SF标架的经典ILOS导引算法是解决USV在海流等环境干扰下路径跟踪控制的有效手段；为使USV安全避开全局静态碍航物高精度跟踪海上规划好的全局路径，结合BLF技术、漂角观测器补偿技术和流干扰观测器补偿技术设计出BILOS导引算法，指导USV完成海上全局路径跟踪；在给出BILOS导引算法之前，先给出ILOS矢量的定义，ILOS矢量指USV当前位置与其在计划航线上投影点的切线下一点的连线；通过引导USV的合速度沿着ILOS矢量的方向运动，就能够指导USV期望的全局路径，具体的BILOS导引算法给出的导引律包括以下步骤：

步骤2.3.1、根据步骤2.2中给出的USV位置误差系统对时间的导数，结合BLF有等式：

0≤||x_e||＜k_b,0≤||y_e||＜k_b

为方便研究，假设USV受环境影响产生的漂角β_r满足下面的条件：

则有如下的等式成立：

进一步化简为：

式中是BILOS导引算法设计的期望艏向角误差；

在数学上有下面的不等式成立：

|sin(ψ_d-ψ_F)|≤1,|cos(ψ_d-ψ_F)|≤1

所以可得到：

在之后的滑模控制器设计中，假设USV的艏向角能够跟踪上BILOS导引算法的导引律ψ_d，可改写成：

结合BLF，考虑到如下的能量函数：

式中为自适应漂角观测器的设计值；

对式求时间的导数，有如下的等式：

为稳定纵向位置误差x_e，设计如下的BILOS的导引律一：

式中为海流干扰观测器的观测值，k_x为正常数；

步骤2.3.2、为稳定横向位置误差y_e，并指导USV的和速度方向沿着ILOS矢量l_los的方向运动，设计如下的BILOS的导引律二：

式中y_int是用来补偿海流等环境干扰所产生的积分动作，用漂角观测器来代替，即是对自适应漂角观测器的设计，对积分动作设计为：

式中为海流干扰观测器的观测值，k_int为积分增益，满足下面的等式，其中k_i,l为正常值参数；

需要指出的是，通过对改进型ILOS导引算法积分项的设计，当USV距离计划全局路径较远时，积分增益k_int≈0，BILOS导引律二中的积分项y_int就不再起作用；当USV距离计划全局路径较近时，积分增益k_int≈1，BILOS导引律二中的积分项y_int就会起到观测和补偿环境干扰引起的漂角的作用，快速地减小横向位置误差y_e，同时不超过位置误差所规定的范围；

得到：

式中