CN105955268B - 一种考虑局部避碰的uuv动目标滑模跟踪控制方法 - Google Patents

Abstract

本发明提供的是一种考虑局部避碰的UUV动目标滑模跟踪控制方法。实时探测UUV、运动目标以及障碍物位置信息；获取运动目标k时刻状态估计；建立UUV和运动目标以及障碍物的相对运动模型；基于目标跟踪半径和障碍物规避安全半径的大小，根据UUV与运动目标及障碍物的相对位置，在跟踪策略和避碰策略之间自主切换；根据指令速度和航向与UUV航速及转艏角速度测量反馈，得到跟踪控制偏差，基于水平面非奇异终端滑模控制器解算得到k时刻UUV推进器推力和方向舵转艏力矩；循环执行上述步骤，实现运动目标下一刻的跟踪控制。本发明将复杂环境中的局部规划策略与UUV动力学模型相结合，在UUV航行安全性的前提下保证对运动目标的跟踪精度。

Description

一种考虑局部避碰的UUV动目标滑模跟踪控制方法

技术领域

本发明涉及的是一种水下无人航行器的目标跟踪方法，具体地说是一种水下无人航行器的动目标跟踪方法。

背景技术

随着UUV使命任务和应用领域的增加，对UUV目标跟踪的能力要求也有大大提高，要求其不仅能识别静止的目标(例如援救定位尽快到达失事点、矿产开采、管道定位检修)，还可以跟踪机动目标，通过预测目标状态变化做出适应性调整以免丢失跟踪对象。

目前，动目标跟踪问题研究主要集中在航空航天等领域，主要指的是基于卡尔曼、粒子滤波等目标状态估计；比例导引法、尾追法、固定提前角法、平行接近法等动目标导引方法；人工势场、模糊避碰法等自主避障方法。针对UUV水下环境的研究还不够成熟，对于动态环境中执行既定目标轨迹跟踪过程中可能出现的碍航物考虑较少，即使有实时的运动规划，也很少结合UUV动力学模型及操纵性能通过运动控制加以实现。因此，将上述因素引入UUV目标跟踪控制中，真实反映其动态跟踪避障能力，对于安全可靠执行各种作业任务具有重要意义。

发明内容

本发明的目的在于提供一种能够实现UUV对运动目标跟踪、同时规避其航路上障碍物的导引策略，稳定快速、鲁棒性强的考虑局部避碰的UUV动目标滑模跟踪控制方法。

本发明的目的是这样实现的：

步骤一：UUV通过导航姿态传感器、位置测量***、前视声呐，实时探测UUV、运动目标以及障碍物位置信息；

步骤二：建立运动目标和障碍物运动学模型，基于不敏卡尔曼滤波获取运动目标k时刻状态估计；

步骤三：建立UUV和运动目标以及障碍物的相对运动模型，用于计算k时刻跟踪距离和视线角；

步骤四：基于目标跟踪半径D₁和障碍物规避安全半径D₂的大小，根据UUV与运动目标及障碍物的相对位置，在跟踪策略和避碰策略之间自主切换，改变UUV控制视线，规划UUVk时刻指令速度和航向；

步骤五：根据步骤四中得到的指令速度和航向与UUV航速及转艏角速度测量反馈，得到跟踪控制偏差，基于水平面非奇异终端滑模控制器解算得到k时刻UUV推进器推力和方向舵转艏力矩；

步骤六：循环执行步骤一至步骤五，实现运动目标下一刻的跟踪控制，直到任务结束。

本发明还可以包括：

1、跟踪策略和避碰策略之间自主切换的导引方式具体表示为：在全局跟踪过程中，如果出现ρ_oiv,k≤D₂+(R_io+R_v)，就激活避碰导引律式将视线切换到障碍物规避方面，一旦确认UUV运动到安全区域就执行目标跟踪导引律式立即恢复跟踪过程，

其中：ρ_oiv,k为k时刻UUV与第i个障碍物之间的距离，UUV简化为半径R_v，R_io为第i个障碍物的半径，u_vRef,k为k时刻UUV导引速度、r_vRef,k为k时刻UUV导引转艏角速度、为最大航速、为最小航速、ψ_v,k为k时刻UUV的航向角、为UUV最大转艏角速度、n₀为跟踪速度增益、n₁为跟踪转艏控制增益、n₂为避碰速度增益、n₃为避碰转艏控制增益、ρ_vg,k为k时刻UUV与目标之间的距离、φ_vg,k为k时刻UUV与目标的视线角即视线矢量与惯性坐标系Eξ轴之间的夹角、φ_oiv,k为k时刻UUV与第i个障碍物之间的视线角、D₁为目标跟踪半径、D₂为障碍物规避安全半径、为饱和函数且取视线控制和饱和角速度的最小值、Φ(α)将转角转换到区间[-π,π]。

2、步骤五中所述UUV推进器推力和方向舵转艏力矩具体表示为：

其中：k时刻UUV推进器推力τ_u,k、k时刻方向舵转艏力矩τ_r,k、k时刻UUV实际航速u_v,k、k时刻UUV导引速度u_vRef,k、k时刻UUV实际转艏角速度r_v,k、k时刻UUV导引转艏角速度r_vRef,k、k时刻UUV横向运动速度v_v,k；非奇异终端航速控制滑模面s₁和转艏控制滑模面s₂，滑模面可调参数β₁＞0、β₂＞0，p₁、q₁、p₂、q₂为正奇数且航速跟踪控制偏差u_e,k＝u_vRef,k-u_v,k、艏向角速度控制偏差r_e,k＝r_ref,k-r_v,k； d₁₁＝X_u+X_|u|u|u|，d₂₂＝Y_v+Y_|v|v|v|，d₃₃＝Z_w+Z_|w|w|w|为动力学模型参数；“^”表示***模型参数的估计值，且i＝1,2,3,5,6，表示模型参数的摄动量，c₁sat(s₁/φ₁)、c₂sat(s₂/φ₂)为滑模控制器的不连续切换项，c₁、φ₁、c₂、φ₂为改变抖振及摄动能力的可调参数。

本发明导引策略的控制目标为：考虑UUV饱和约束条件下，二维动态环境中的目标跟踪和局部避障问题。①对于位置跟踪，如果则存在D₁＞0，使得对于有②对于障碍物避碰问题，如果ρ_oiv(t₀)＞D₂，则存在D₂＞0，使对于有ρ_oiv(t)＞(R_io+R_v)成立。

其中，q＝[x y ψ]^T表示惯性坐标系中位置和航向角，U＝[u r]^T表示航速和转艏角速度组成的运动学控制向量，用q_v,k＝[x_v,k y_v,k ψ_v,k]^T、U_v,k＝[u_v,k r_v,k]^T表示k时刻的UUV状态，q_g,k＝[x_g,k y_g,k ψ_g,k]^T、U_g,k＝[u_g,k r_g,k]^T表示k时刻的目标状态，q_io,k＝[x_io,k y_io,kψ_io,k]^T、U_io,k＝[u_io,k r_io,k]^T表示k时刻第i个障碍物状态。将UUV简化为半径R_v，中心位置为(x_v,k,y_v,k)的圆_，类似得到半径为R_g的目标和半径为R_io的障碍物的简化表示形式。ρ_vg,k为k时刻UUV与目标之间的距离，φ_vg,k为k时刻UUV与目标的视线角(视线矢量与惯性坐标系Eξ轴之间的夹角)；ρ_oiv,k为k时刻UUV与第i个障碍物之间的距离，φ_oiv,k为k时刻UUV与第i个障碍物之间的视线角；D₁为目标跟踪半径，D₂为障碍物规避安全半径。

对于UUV速度和航向上的饱和约束，设最大航速为最大转向变化率为另外为保证UUV潜伏在水下，其速度不能减速到零(会浮出水面)，设最小航速为则有：制定的导引策略具体如下：

(1)目标跟踪导引律为：

其中，n₀为正常数速度增益，为保证跟踪的平滑性，使UUV在落后目标较远时以与距离成正比的速度跟踪；n₁为转艏控制增益，Φ(α)将转角限制在区间[-π,π)；为饱和函数，取视线控制和饱和角速度的最小值。

(2)避碰导引律为：

其中，0＜n₂≤1转速控制增益，n₃设计为正常数的转艏控制增益。

(3)视线切换策略：在全局跟踪过程中，如若出现ρ_oiv,k≤D₂+(R_io+R_v)的时刻，就激活避碰导引律，将视线切换到障碍物规避方面，一旦确认UUV运动到安全区域就执行目标跟踪导引律，立马恢复跟踪过程。

本发明设计了如下的非奇异终端滑模速度控制器和转艏控制器：

UUV推进器推力控制律为：

UUV方向舵转艏力矩控制律为：

其中，d₁₁＝X_u+X_|u|u|u|，d₂₂＝Y_v+Y_|v|v|v|，d₃₃＝Z_w+Z_|w|w|w|为动力学模型参数；“^”表示***模型参数的估计值，且 i＝1,2,3,5,6，表示模型参数的摄动量，c₁sat(s₁/φ₁)、c₂sat(s₂/φ₂)为滑模控制器的不连续切换项，通过调整参数c₁，φ₁，c₂，φ₂可增强控制器鲁棒性并改善滑模抖振现象。

UUV的航速跟踪误差为u_e,k＝u_vRef,k-u_v,k，转艏角速度跟踪误差为r_e,k＝r_ref,k-r_v,k，设计的非奇异Terminal滑模面分别为：

其中，β₁＞0，β₂＞0，p₁，q₁，p₂，q₂为正奇数且

本发明提供了一种能够实现UUV对运动目标跟踪同时规避其航路上障碍物的导引策略，并且结合UUV动力学模型和运动控制能力给出具体实现方法。本发明的有益效果在于：

(1)以UUV与目标及障碍物之间的距离作为动态规划准则，简化了动态环境目标跟踪和局部避障问题的分析，考虑了UUV的运动控制约束制定的简单有效切换策略，实现了UUV安全性和跟踪精度要求下的实时平稳运动规划。

(2)设计了非奇异终端滑模控制器，能够消除控制过程中可能出现的***奇异点，有限时间内实现对指令的稳定控制，并且十分适用于水下非线性，存在环境干扰和模型参数摄动的复杂工况。实现了考虑UUV动力学模型的有效跟踪过程，稳定快速、鲁棒性强。

附图说明

图1为本发明的总体框图；

图2为平面中UUV目标跟踪和被动避障双目标控制问题示意图；

图3为UUV目标跟踪导引示意图；

图4为UUV障碍物避碰导引示意图；

图5为UUV跟踪运动目标仿真案例轨迹；

图6a至图6d为UUV跟踪运动目标仿真案例的控制参数图；

图7为UUV跟踪运动目标同时规避动态障碍物仿真案例轨迹；

图8a至图8d为UUV跟踪运动目标同时规避动态障碍物仿真案例的控制参数图；

图9是本发明的流程图。

具体实施方式

下面结合附图对本发明做进一步说明：

UUV在执行回收对接等任务时，需要尾随跟踪特定目标，对于水下复杂环境，在跟踪目标运动航路上不可避免会出现随机障碍物(船只、浮游生物、水下垃圾等)，针对此情况本发明给出一种简单有效的UUV动态环境跟踪控制方法。

图1为本发明的总体工作框图，主要分为UUV模型及探测***、目标状态估计、动态规划模块和非奇异终端滑模控制器四个部分，目标和障碍物位置由UUV声呐探测***获取，UUV实时状态信息由其组合导航***测定，结合图9描述本发明的具体步骤。

步骤一：UUV通过导航姿态传感器、位置测量***、前视声呐等，实时探测UUV、运动目标以及障碍物位置信息(本发明的仿真图由直接给出目标及障碍物运动轨迹得到)；

步骤二：根据目标和障碍物运动规律，建立其运动学模型，基于轨迹点位置由不敏卡尔曼(UKF)滤波算法获取其k时刻状态估计；

设目标动态模型为X_k＝FX_k-1+GW_k，状态向量F为***转移矩阵，G为***噪声变换矩阵，***状态噪声W_k＝[w_x,k,w_y,k]^T为零均值高斯噪声，协方差为Q_k。观测模型为Z_k＝h(X_k)+V_k，其中h为观测函数，V_k为服从高斯分布的测量噪声序列，协方差为R_k，且存在

cov[W_k]＝Q_k,cov[V_k]＝R_k

UKF算法的简化流程为：

1.给定***初值(初始状态均值估计值初始状态方差估计值P₀)

2.计算Sigma采样点χ_i及其权值λ_i，以最小的样本集来近似***状态的分布函数

其中n为状态向量的维度，κ为尺度参数，本设计中取2。

3.预测/时间更新，对Sigma采样点进行非线性变换和加权处理，得到状态向量均值、状态向量方差和输出向量的一步预测值P_k|k-1、

使用变换后的状态变量进行滤波估计,以减小估计误差，同时由于该算法采用了非线性的动态方程或量测方程,避免了线性化误差。

4.校正/测量更新

其中，Y_k为k时刻观测值，P_k分别为k时刻输出向量和状态向量方差，为其两者协方差，M_k为UKF校正增益，即为k时刻状态的滤波估计值。

步骤三：根据步骤二中滤波后的状态信息，基于UUV和运动目标以及障碍物的相对运动模型，计算k时刻的跟踪距离和视线角，如图2所示的平面中UUV目标跟踪和被动避障双目标控制问题；

在制定导引策略时，认为UUV横向速度是个微小的耦合量，对运动轨迹影响较小，设计时将运动学模型简化为如式(5)的形式，并且假定目标和障碍物满足相同的运动规律：

其中q＝[x y ψ]^T表示惯性坐标系中位置和航向角，U＝[u r]^T表示航速和转艏角速度组成的运动学控制向量。k时刻，对于UUV以向量q_v,k＝[x_v,k y_v,k ψ_v,k]^T、U_v,k＝[u_v,kr_v,k]^T来表示，目标以q_g,k＝[x_g,k y_g,k ψ_g,k]^T、U_g,k＝[u_g,k r_g,k]^T来表示，第i个障碍物以q_io,k＝[x_io,k y_io,k ψ_io,k]^T、U_io,k＝[u_io,k r_io,k]^T来表示；在示意图中UUV简化为半径R_v，中心位置为(x_v,k,y_v,k)的圆，类似得到半径为R_g的目标和半径为R_io的障碍物的简化表示形式。

带入UUV和目标的相对运动模型：

其中，ρ_vg,k为k时刻UUV与目标之间的距离，φ_vg,k为k时刻UUV与目标的视线角(视线矢量与惯性坐标系Eξ轴之间的夹角)。

式(6)取微分形式，联立式(5)推导得到UUV与目标的跟踪误差形式：

同理，UUV和第i个障碍物的相对运动模型为：

其中，ρ_oiv,k为k时刻UUV与第i个障碍物之间的距离，φ_oiv,k为k时刻UUV与第i个障碍物之间的视线角。

步骤四：基于步骤三中计算得到的k时刻UUV与目标及障碍物的相对位置，结合目标跟踪半径D₁和障碍物规避安全半径D₂的要求，制定全局跟踪和局部避碰自主切换策略，改变UUV控制视线，规划其k时刻的指令速度u_vRef,k和指令航向r_vRef,k；

(1)目标跟踪导引律：设计在最大转艏角速度的限制下，控制UUV航向尽快向跟踪视线方向转动，并且当UUV在目标接近圆内时以目标速度保持监视，当目标机动逃到跟踪圈以外时UUV采用最大航速追踪，如图3所示。尽量选用恒定线速度的方式有利于减少控制量，以转艏控制来增加航速控制的灵活性，具体可表示为：

其中，UUV有最大航速最大转向变化率的饱和约束，并为保证UUV潜伏在水下，设最小航速为即n₀为正常数速度增益，为保证跟踪的平滑性，使UUV在落后目标较远时以与距离成正比的速度跟踪；为饱和函数，取视线控制和饱和角速度的最小值，函数表示为：

n₁为转艏控制增益，Φ(α)将转角限制在区间[-π,π)，定义为：

(2)避碰导引律：当目标进入半径为D₂的障碍物危险圆以内时，设计在最大转艏角速度的限制下，控制UUV以最大线速度尽快转向远离障碍物的视线方向，如图4所示的避障导引策略，函数表示为：

其中，0＜n₂≤1转速控制增益，n₃设计为正常数的转艏控制增益。

(3)视线切换策略：基于以上两种情况的导引，可以证明存在(n₀,n₁,D₁)使得UUV对目标的跟踪误差一致收敛于D₁，存在(n₂,n₃,D₂)使得在初始时刻的条件下，***能够保证成功避开障碍物。得到动态环境中目标跟踪同时被动避障的策略是：在全局跟踪过程中，如若出现ρ_oiv,k≤D₂+(R_io+R_v)的时刻，就激活避碰导引律，将视线切换到障碍物规避，一旦确认UUV运动到安全区域就立马恢复跟踪过程。

步骤五：制定跟踪策略后还需要结合动力机构加以控制，因此根据步骤四中的导引指令与UUV航速及转艏角速度测量反馈，得到跟踪控制偏差，设计对于参数摄动及环境干扰具有很强鲁棒性的非奇异终端滑模控制器，解算得到k时刻UUV推进器推力τ_u,k和方向舵转艏力矩τ_r,k；

对于水平面UUV动力学模型具有如下简化形式：

将步骤四中得到的导引律u_vRef,k和r_vRef,k作为k时刻运动控制期望指令，与UUV实际航速航向相比较，得到跟踪控制偏差u_e,k＝u_vRef,k-u_v,k，r_e,k＝r_ref,k-r_v,k，基于非奇异终端滑模原理分别设计速度和转艏控制滑模面：

其中，可调参数β₁＞0，β₂＞0，p₁，q₁，p₂，q₂为正奇数且

推导得到水平面UUV推力和转艏力矩控制律：

其中，d₁₁＝X_u+X_|u|u|u|，d₂₂＝Y_v+Y_|v|v|v|，d₃₃＝Z_w+Z_|w|w|w|为动力学模型参数；“^”表示***模型参数的估计值，且 i＝1,2,3,5,6，表示模型参数的摄动量，c₁sat(s₁/φ₁)、c₂sat(s₂/φ₂)为滑模控制器的不连续切换项。

构造Lyapunov函数均正定，分别求导可以证明

负定，由此可知UUV跟踪控制偏差能在有限时间向零状态稳定，即实际航速和实际转艏角速度能在有限时间内跟踪导引指令。

步骤六：k+1时刻，跳转至步骤一，执行步骤一～步骤五，得到实时跟踪控制τ_u,k+1和τ_r,k+1；随着采样进行，循环上述过程直到收到任务结束指令。

给出本发明的两个实例仿真，轨迹及运动控制参数图分别见附图。图5、图6a至图6d为机动目标跟踪过程，整个过程相对平稳；图7、图8a至图8d展示的是跟踪环境未知，出现运动障碍物在航线上干扰的情况，对突变的运动指令仍然能快速稳定控制。

设随机运动障碍物轨迹方程为

UUV半径R_v＝3m，运动障碍物半径R_o＝5m，目标接近圆半径D₁＝10m，障碍物风险区半径D₂＝25m，数据采集间隔T_o＝0.5s。可以看出滑模控制器精度极高，对控制指令响应快误差小，UUV实际航迹基本能够与规划轨迹重合。在图7案例中，本仿真方法一方面能够保证对动目标的跟踪精度，另一方面ρ_ov(t)≤D₂+(R_o+R_v)＝33m时激活避障控制直至ρ_ov(t)＞D₂+(R_o+R_v)，短时避障期间ρ_ov(min)＝26.8024m＞R_o+R_v＝8m始终保证了UUV的航行安全性。

Claims (3)

1.一种考虑局部避碰的UUV动目标滑模跟踪控制方法，其特征是：

步骤一：UUV通过导航姿态传感器、位置测量***、前视声呐，实时探测UUV、运动目标以及障碍物位置信息；

步骤二：建立运动目标和障碍物运动学模型，基于不敏卡尔曼滤波获取运动目标k时刻状态估计；

步骤三：建立UUV和运动目标以及障碍物的相对运动模型，用于计算k时刻跟踪距离和视线角；

步骤四：基于目标跟踪半径D₁和障碍物规避安全半径D₂的大小，根据UUV与运动目标及障碍物的相对位置，在跟踪策略和避碰策略之间自主切换，改变UUV控制视线，规划UUVk时刻指令速度和航向；

步骤五：根据步骤四中得到的指令速度和航向与UUV航速及转艏角速度测量反馈，得到跟踪控制偏差，基于水平面非奇异终端滑模控制器解算得到k时刻UUV推进器推力和方向舵转艏力矩；

步骤六：循环执行步骤一至步骤五，实现运动目标下一刻的跟踪控制，直到任务结束。

2.根据权利要求1所述的一种考虑局部避碰的UUV动目标滑模跟踪控制方法，其特征是跟踪策略和避碰策略之间自主切换的导引方式具体表示为：在全局跟踪过程中，如果出现ρ_oiv,k≤D₂+(R_io+R_v)，就激活避碰导引律式将视线切换到障碍物规避方面，一旦确认UUV运动到安全区域就执行目标跟踪导引律式立即恢复跟踪过程，

其中：ρ_oiv,k为k时刻UUV与第i个障碍物之间的距离，R_io为第i个障碍物的半径，UUV简化为半径R_v，u_vRef,k为k时刻UUV导引速度、r_vRef,k为k时刻UUV导引转艏角速度、为最大航速、为最小航速、ψ_v,k为k时刻UUV的航向角、为UUV最大转艏角速度、n₀为跟踪速度增益、n₁为跟踪转艏控制增益、n₂为避碰速度增益、n₃为避碰转艏控制增益、ρ_vg,k为k时刻UUV与目标之间的距离、φ_vg,k为k时刻UUV与目标的视线角即视线矢量与惯性坐标系Eξ轴之间的夹角、φ_oiv,k为k时刻UUV与第i个障碍物之间的视线角、D₁为目标跟踪半径、D₂为障碍物规避安全半径、为饱和函数且取视线控制和饱和角速度的最小值、Φ(α)将转角转换到区间[-π,π]。

3.根据权利要求1或2所述的一种考虑局部避碰的UUV动目标滑模跟踪控制方法，其特征是步骤五中所述UUV推进器推力和方向舵转艏力矩具体表示为：

其中：k时刻UUV推进器推力τ_u,k、k时刻方向舵转艏力矩τ_r,k、k时刻UUV实际航速u_v,k、k时刻UUV导引速度u_vRef,k、k时刻UUV实际转艏角速度r_v,k、k时刻UUV导引转艏角速度r_vRef,k、k时刻UUV横向运动速度v_v,k；非奇异终端航速控制滑模面s₁和转艏控制滑模面s₂，滑模面可调参数β₁＞0、β₂＞0，p₁、q₁、p₂、q₂为正奇数且航速跟踪控制偏差u_e,k＝u_vRef,k-u_v,k、艏向角速度控制偏差r_e,k＝r_ref,k-r_v,k； d₁₁＝X_u+X_|u|u|u|，d₂₂＝Y_v+Y_|v|v|v|，d₃₃＝Z_w+Z_|w|w|w|为动力学模型参数；“^”表示***模型参数的估计值，且i＝1,2,3,5,6，表示模型参数的摄动量，c₁sat(s₁/φ₁)、c₂sat(s₂/φ₂)为滑模控制器的不连续切换项，c₁、φ₁、c₂、φ₂为改变抖振及摄动能力的可调参数。