CN112947447B - 一种基于同步规划-控制策略的水面无人舰艇自主导航方法 - Google Patents

Abstract

一种基于同步规划‑控制策略的水面无人舰艇自主导航方法，属于水面无人舰艇控制领域，基于一种同步规划‑控制的思想，使用一种基于栅格地图的改进人工势场方法进行避障路径规划，使用模型预测控制方法进行对于参考轨迹的跟踪，两类方法交替执行以实现自主导航控制。人工势场类的方法可以实现复杂水域环境下局部路径的快速、可靠生成，在每个采样时刻的具体执行过程中只执行有限个迭代步，从而仅生成未来较短时间内的参考路径，进一步缩短了路径规划环节的效率。由于水面无人舰艇是一类欠驱动系统，其运动规律服从复杂的动力学方程，模型预测控制方法可以在满足动力学方程的同时，实现对各类状态、控制变量约束条件的满足并给出最优的控制输入。

Description

一种基于同步规划-控制策略的水面无人舰艇自主导航方法

技术领域

本发明属于水面无人舰艇控制领域，涉及一种基于同步规划-控制策略的水面无人舰艇自主导航方法。

背景技术

由于在提高交通安全性与效率方面潜在的优势，水面无人舰艇得到了越来越广泛的关注，这也紧密牵引与其相关的导航、制导与控制技术迅猛发展。由于水面无人舰艇不配备舰载的操纵员，其必须通过岸上操纵员遥控操控或者自主导航实现安全的运动控制。相比于遥控操纵的方式，自主导航的运动控制模式具有更高的自动化等级，是未来水面无人舰艇的必然的发展趋势，这也相应地对环境感知、路径规划、舰艇控制等关键技术提出了更高的要求。

发明内容

为实现水面无人舰艇高质量的自主导航，本发明提出了一种基于同步规划-控制的水面无人舰艇自主导航方法。该方法基于一种同步规划-控制的思想，使用一种基于栅格地图的改进人工势场方法进行避障路径规划，使用模型预测控制方法进行对于参考轨迹的跟踪，两类方法交替执行以实现自主导航控制。人工势场类的方法可以实现复杂水域环境下局部路径的快速、可靠生成，在每个采样时刻的具体执行过程中只执行有限个迭代步，从而仅生成未来较短时间内的参考路径，进一步缩短了路径规划环节的效率。由于水面无人舰艇是一类欠驱动系统，其运动规律服从复杂的动力学方程，模型预测控制方法可以在满足动力学方程的同时，实现对各类状态、控制变量约束条件的满足并给出最优的控制输入。

为了达到上述目的，本发明采用的技术方案为：

一种基于同步规划-控制的水面无人舰艇自主导航方法，根据感知到的障碍物更新栅格地图。在每个采样步，根据采集到的水面无人舰艇状态，首先使用一种改进的人工势场方法生成未来有限时间内的参考路径，然后使用模型预测控制方法对该参考路径进行轨迹跟踪，将计算得到的控制输入施加到水面无人舰艇实现对其的驱动。重复上述步骤，直到水面无人舰艇抵达目标点附近。本发明的计算流程图如图1所示，包括以下步骤：

步骤1：根据水面无人舰艇实际机械结构设定状态变量、控制变量、约束条件

选定控制器设计过程中的水面无人舰艇状态变量(除包括横坐标、纵坐标、艏向、速度、艏向角速度等必要变量，还应为相关约束条件的方便施加，视具体情况进行扩充)，记作状态变量x；同时，选定控制器设计过程中对应的控制变量(如推进器推力、推进器角度等)，记作控制变量u。记水面无人舰艇服从的动力学方程为其中f为对应的常微分方程组。在实际自主导航过程中，为实现控制变量的平滑性，通常将机械结构的实际控制量(如推进器推力等)纳入状态变量x(状态空间大小记作N_x)，并将其对应的一阶导数(如推进其推力的变化率)记作控制器中的控制变量(控制变量个数记作N_u)。

在确定控制器的状态变量与控制变量后，根据水面无人舰艇的实际机械结构，确定与之相关的约束条件(如允许的艏向角速度、速度、加速度、推进力、推进力变化率范围等)，统一记作h(x,u,t)≤0的不等式约束形式。

确定水面无人舰艇的障碍感知范围Ω。

步骤2：设定算法参数

步骤2-1：设定模型预测控制算法参数

设定模型预测控制算法中相关参数，包括预测窗口长度T，采样步长δT，跟踪误差权矩阵控制变量权矩阵/>参数选取满足M＝T/δT为一正整数。

步骤2-2：设定人工势场方法参数

设定改进人工势场方法中相关参数，包括时间步长ΔT，引力系数K_att，斥力系数K_rep，目标点引力场线性-二次阈值ρ_g，障碍物斥力作用半径ρ_o(满足ρ_o≤Ω)，斥力分力退化系数n。参数选取满足变量m＝T/ΔT为一正整数，其代表在使用改进人工势场方法中，每次生成未来较短时间内m个时间点上的路径信息。

步骤2-3：定义必备符号

定义变量ρ(A,B)代表平面内A、B两点间的欧氏距离，定义变量代表由点B指向点A的单位向量。

步骤3：根据自主导航任务的需求，设定任务参数

根据自主导航的任务需求，设定任务参数，包括：任务开始执行的时间T₀、期望到达的目标点的坐标V(x_g,y_g)、期望的巡航速度v_const、水面无人舰艇最大角速度等效达到误差ε等。

初始化采样时刻指标z＝0。

步骤4：测量水面无人舰艇当前时刻的位姿信息及其它必要状态

在第z个采样时刻，即t＝T₀+(z-1)δT时刻，测量测量水面无人舰艇的位姿信息，包括空间坐标艏向/>速度/>等。同时获取其它反映水面无人舰艇状态的必要变量，记当前时刻水面无人舰艇的状态为x_z。

若则认为水面无人舰艇已经抵达期望达到的目标点，自主导航任务结束。

步骤5：建立代表周围环境的0-1栅格地图

根据感知到的障碍物建立具有代表周围环境的0-1栅格地图，其中1表该栅格内存在障碍需要进行规避(记作障碍栅格)，0代表该栅格无障碍可以通行(记作无障碍)。栅格的尺寸应当根据导航所需的精度、水面无人舰艇尺寸、水面无人舰艇运动速度等因素综合确定。

步骤6：使用一种改进人工势场方法生成未来较短时间内的参考路径，具体如下：

步骤6-1：初始化路径规划中的指标k并定义临时变量

初始化k＝0。定义临时变量

步骤6-2：若环境中存在动态障碍，更新障碍物位置信息

若环境中存在动态障碍物，则结合障碍物已知的运动规律或对其运动规律进行预测，从而更新t＝T₀+(z-1)δT+kΔT时刻的障碍物信息。

步骤6-3：根据目标点、障碍物的位置，确定改进人工势场方法中当前迭代步的合力的理想方向

首先计算目标点产生的引力场U_att和对应的引力F_att如下

然后，计算由障碍物产生的斥力场和对应的斥力。对于栅格地图中的第j个障碍栅格，记其中心位置为则由该栅格产生的斥力场U_rep,j和对应的斥力F_rep,j如下

记环境中一共有S个障碍栅格，则水面无人舰艇受到的合力为记/>的方向为/>则/>即为当前迭代步中合力的理想方向。

步骤6-4：使用角速度约束修正当前迭代步合力方向

根据时间步长ΔT、水面无人舰艇最大角速度计算相邻两个迭代步中水面无人舰艇艏向的最大允许改变量/>定义如下两个变量

Δθ^[k]＝θ^[k]-θ^[k+1] (7)

则在下一迭代步的艏向增量应由如下的饱和函数确定，即要求

根据式(9)计算的艏向增量和步骤3中设定的水面无人舰艇运动速度，可以计算(k+1)时刻水面无人舰艇的位置与艏向如下

步骤6-5：改进人工势场方法终止条件判断

更新迭代指标k＝k+1。若满足k＝m，使用与/>构造spline插值函数x(t)、y(t)与θ(t)，从而代表水面无人舰艇的横坐标、纵坐标与艏向在时间区间 [T₀+(z-1)δT,T₀+(z-1)δT+T]内的变化历程；若k＜m，返回步骤6-2。

步骤7：使用模型预测控制算法对步骤6中的参考路径进行跟踪，具体如下：

步骤7-1：建立模型预测控制模型

建立面向轨迹跟踪的模型预测控制模型如下

其中性能指标J中第一项e^TPe反映了跟踪误差，第二项u^TRu反映了对控制量相关的评价，跟踪误差e＝(x-x,y-y,θ-θ)^T。f(x,u,t)与h(x,u,t)分别为步骤1中设定的无人水面舰艇动力学方程与约束条件。

步骤7-2：求解模型预测控制模型获得最优控制输入

使用合适的数值算法求解步骤7-1中建立的最优控制模型，为提高求解效率，使用步骤6 中获得的参考路径对问题中横坐标、纵坐标、艏向等变量进行初始化。记计算得到的控制历程为u^*(t)。

步骤8：将步骤7计算得到的控制输入施加于无人水面舰艇

在时间区间[T₀+(z-1)δT,T₀+zδT]内将u^*(T₀+(z-1)δT)施加于水面无人舰艇。

更新迭代指标z＝z+1，返回步骤4。

本发明的有益效果为：结合一种改进的人工势场方法与模型预控制方法，发展了一种面向水面无人舰艇自主导航的同步规划-控制方法。改进人工势场方法用于参考轨迹规划，保证了复杂水域环境下避障路径的高效、可靠生成，由于考虑了角速度约束，使得生成的参考轨迹具有良好的光滑性；模型预测控制方法用于对参考轨迹的轨迹跟踪，可以严格满足对于状态变量已经控制变量的约束条件，且能抵抗外界风浪扰动带来的影响。改进的人工势场方法与模型预测控制方法交替实施，不断地有前者生成参考轨迹并由后者进行跟踪，可以实现存在动态障碍、风浪扰动等复杂因素下的水面无人舰艇自主导航，具有良好的实时性与鲁棒性。

附图说明

图1为本发明的计算流程图。

图2为本发明实施例中水面无人舰艇的构型。

图3为本发明实施例中水域环境。

图4为本发明实施例中水域环境对应的人工势场。

图5为本发明实施例中计算得到的水面无人舰艇路径。

图6为本发明实施例中计算得到的水面无人舰艇横坐标时间历程。

图7为本发明实施例中计算得到的水面无人舰艇纵坐标时间历程。

图8为本发明实施例中计算得到的水面无人舰艇艏向时间历程。

图9为本发明实施例中计算得到的水面无人舰艇速度时间历程。

图10为本发明实施例中计算得到的水面无人舰艇角速度时间历程。

图11为本发明实施例中计算得到的水面无人舰艇推进力1时间历程。

图12为本发明实施例中计算得到的水面无人舰艇推进力2时间历程。

图13为本发明实施例中计算得到的水面无人舰艇推进力1变化率时间历程。

图14为本发明实施例中计算得到的水面无人舰艇推进力2变化率时间历程。

具体实施方式

以下结合具体实施例对本发明做进一步说明。

考虑如图2所示的水面无人舰艇在图3所示的200m×400m的水域中的自主导航问题，环境中静态障碍对应的人工势场如图4所示。起始点的位置与艏向分别设置为(35m,0m)与 90deg。

一种基于同步规划-控制策略的水面无人舰艇自主导航方法，包括以下步骤：

步骤1：根据水面无人舰艇实际机械结构设定状态变量、控制变量、约束条件

选定控制器设计过程中的水面无人舰艇状态变量(除包括横坐标、纵坐标、艏向、速度、艏向角速度等必要变量，还包括推进力1与推进力2)，记作状态变量x＝(x,y,θ,v,ω,F₁,F₂)^T，状态空间大小为N_x＝7；同时，选定控制器设计过程中对应的控制变量(推进力1的时间变化率 u₁与推进力2的时间变化率u₂)，记作控制变量u＝(u₁,u₂)^T，控制变量个数为N_u＝2。

记水面无人舰艇服从的动力学方程为具体表达为

其中，m_c＝2kg与I_c＝0.25kg·m²分别为水面无人舰艇的质量与转动惯量，r＝0.05m为推进器与水面无人舰艇对称轴之间的距离。

确定系统的相关约束条件，具体包含如下约束：

其中，[F_min,F_max]为许用的推进力范围，u_max为推进力变化率幅值的上限，v_max为速度上限，ω_max为首相角速度幅值的上限。将上述约束统一记作h(x,u,t)≤0的不等式约束，形式如下：

确定水面无人舰艇的障碍感知范围Ω＝20m。

步骤2：设定算法参数

步骤2-1：设定模型预测控制算法参数

设定模型预测控制算法中相关参数，包括预测窗口长度T＝1s，采样步长δT＝0.1s，跟踪误差权矩阵Q＝diag(10⁵,10⁵,10⁴)，控制变量权矩阵R＝diag(1,1)。参数选取满足M＝T/δT＝10为一正整数。

步骤2-2：设定人工势场方法参数

设定改进人工势场方法中相关参数，包括时间步长ΔT＝0.2s，引力系数K_att＝15，斥力系数K_rep＝5，目标点引力场线性-二次阈值ρ_g＝100m，障碍物斥力作用半径ρ_o＝Ω＝20m(满足ρ_o≤Ω)，斥力分力退化系数n＝2。参数选取满足变量m＝T/ΔT＝5为一正整数，其代表在使用改进人工势场方法中，每次生成未来较短时间内m个时间点上的路径信息。

步骤2-3：定义必备符号

定义变量ρ(A,B)代表平面内A、B两点间的欧氏距离，定义变量代表由点B指向点A的单位向量。

步骤3：根据自主导航任务的需求，设定任务参数

根据自主导航的任务需求，设定任务参数，包括：任务开始执行的时间T₀＝0s、期望到达的目标点的坐标V(x_g,y_g)＝(110m,400m)、期望的巡航速度v_const＝1m/s、水面无人舰艇最大角速度等效达到误差ε＝v_const·T＝1m等。

初始化采样时刻指标z＝0。

步骤4：测量水面无人舰艇当前时刻的位姿信息及其它必要状态

在第z个采样时刻，即t＝T₀+(z-1)δT时刻，测量测量水面无人舰艇的位姿信息，包括空间坐标艏向/>速度/>等。同时获取其它反映水面无人舰艇状态的必要变量，记当前时刻水面无人舰艇的状态为x_z。

若则认为水面无人舰艇已经抵达期望达到的目标点，自主导航任务结束。

步骤5：建立代表周围环境的0-1栅格地图

根据感知到的障碍物建立具有代表周围环境的0-1栅格地图，其中1表该栅格内存在障碍需要进行规避(记作障碍栅格)，0代表该栅格无障碍可以通行(记作无障碍)。栅格的尺寸应当根据导航所需的精度、水面无人舰艇尺寸、水面无人舰艇运动速度等因素综合确定为 2m×2m。

步骤6：使用一种改进人工势场方法生成未来较短时间内的参考路径

步骤6-1：初始化路径规划中的指标k并定义临时变量

初始化k＝0。定义临时变量

步骤6-2：若环境中存在动态障碍，更新障碍物位置信息

若环境中存在动态障碍物，则结合障碍物已知的运动规律或对其运动规律进行预测，从而更新t＝T₀+(z-1)δT+kΔT时刻的障碍物信息。

步骤6-3：根据目标点、障碍物的位置，确定改进人工势场方法中当前迭代步的合力的理想方向

首先计算目标点产生的引力场U_att和对应的引力F_att如下

然后，计算由障碍物产生的斥力场和对应的斥力。对于栅格地图中的第j个障碍栅格，记其中心位置为则由该栅格产生的斥力场U_rep,j和对应的斥力F_rep,j如下

记环境中一共有S个障碍栅格，则水面无人舰艇受到的合力为记/>的方向为/>则/>即为当前迭代步中合力的理想方向。

步骤6-4：使用角速度约束修正当前迭代步合力方向

根据时间步长ΔT、水面无人舰艇最大角速度计算相邻两个迭代步中水面无人舰艇艏向的最大允许改变量/>定义如下两个变量

Δθ^[k]＝θ^[k]-θ^[k+1] (18)

则在下一迭代步的艏向增量应由如下的饱和函数确定，即要求

根据式(20)计算的艏向增量和步骤3中设定的水面无人舰艇运动速度，可以计算(k+1)时刻水面无人舰艇的位置与艏向如下

步骤6-5：改进人工势场方法终止条件判断

更新迭代指标k＝k+1。若满足k＝5，使用与/>构造spline插值函数x(t)、y(t)与θ(t)，从而代表水面无人舰艇的横坐标、纵坐标与艏向在时间区间 [T₀+(z-1)δT,T₀+(z-1)δT+T]内的变化历程；若k＜5，返回步骤6-2。

步骤7：使用模型预测控制算法对步骤6中的参考路径进行跟踪

步骤7-1：建立模型预测控制模型

建立面向轨迹跟踪的模型预测控制模型如下

其中性能指标J中第一项e^TPe反映了跟踪误差，第二项u^TRu反映了对控制量相关的评价，跟踪误差e＝(x-x,y-y,θ-θ)^T。f(x,u,t)与h(x,u,t)分别为步骤1中设定的无人水面舰艇动力学方程与约束条件。

步骤7-2：求解模型预测控制模型获得最优控制输入

使用保辛伪谱数值算法求解步骤7-1中建立的最优控制模型，为提高求解效率，使用步骤6中获得的参考路径对问题中横坐标、纵坐标、艏向等变量进行初始化。记计算得到的控制历程为u^*(t)。

步骤8：将步骤7计算得到的控制输入施加于无人水面舰艇

在时间区间[T₀+(z-1)δT,T₀+zδT]内将u^*(T₀+(z-1)δT)施加于水面无人舰艇。

更新迭代指标z＝z+1，返回步骤4。

根据上述步骤，水面无人舰艇在迭代指标z＝4599即t＝459.9s时刻抵达目标点附近，其航行轨迹如图5所示。在图6～图10中分别绘制了对应的横坐标、纵坐标、艏向、速度、角速度的时间历程；在图11～图12中绘制了推进力的时间历程；在图13～图14中绘制了推进力变化率的时间历程。从图5中发现，本方法产生的导航轨迹安全且平滑；从图6～图14中可以发现，对状态变量与控制变量施加的约束条件严格满足。

本发明通过一种改进人工势场方法与模型预测控制方法的集成，采用同步规划-控制思想，发展了一种基于同步规划-控制策略的水面无人舰艇自主导航方法。改进人工势场方法的引入，使得路径规划模块得以在每个采样时刻高效的生成未来有限时间内的高质量参考路径，其满足角速度约束且变化光滑；模型预测控制方法的引入，使得状态变量与控制变量相关的约束得以严格满足，且能抵抗外部的风浪扰动。该方法具有较好的计算效率与鲁棒性，可以实现高质量的水面无人舰艇自主导航。

以上所述实施例仅表达本发明的实施方式，但并不能因此而理解为对本发明专利的范围的限制，应当指出，对于本领域的技术人员来说，在不脱离本发明构思的前提下，还可以做出若干变形和改进，这些均属于本发明的保护范围。

Claims (1)

1.一种基于同步规划-控制策略的水面无人舰艇自主导航方法，其特征在于，包括如下步骤：

步骤1：根据水面无人舰艇实际机械结构设定状态变量、控制变量、约束条件

选定控制器设计过程中的水面无人舰艇状态变量，记作状态变量x；同时，选定控制器设计过程中对应的控制变量，记作控制变量u；记水面无人舰艇服从的动力学方程为其中f为对应的常微分方程组；在实际自主导航过程中，将机械结构的实际控制量纳入状态变量x，其中状态空间大小记作N_x，并将其对应的一阶导数记作控制器中的控制变量，其中控制变量个数记作N_u；

在确定控制器的状态变量与控制变量后，根据水面无人舰艇的实际机械结构，确定与之相关的约束条件，统一记作h(x,u,t)≤0的不等式约束形式；

确定水面无人舰艇的障碍感知范围Ω；

步骤2：设定算法参数

步骤2-1：设定模型预测控制算法参数

设定模型预测控制算法中相关参数，包括预测窗口长度T，采样步长δT，跟踪误差权矩阵控制变量权矩阵/>参数选取满足M＝T/δT为一正整数；

步骤2-2：设定人工势场方法参数

设定改进人工势场方法中相关参数，包括时间步长ΔT，引力系数K_att，斥力系数K_rep，目标点引力场线性-二次阈值ρ_g，障碍物斥力作用半径ρ_o，其中满足ρ_o≤Ω，斥力分力退化系数n；参数选取满足变量m＝T/ΔT为一正整数，其代表在使用改进人工势场方法中，每次生成未来较短时间内m个时间点上的路径信息；

步骤2-3：定义必备符号

定义变量ρ(A,B)代表平面内A、B两点间的欧氏距离，定义变量代表由点B指向点A的单位向量；

步骤3：根据自主导航任务的需求，设定任务参数

根据自主导航的任务需求，设定任务参数，包括：任务开始执行的时间T₀、期望到达的目标点的坐标V(x_g,y_g)、期望的巡航速度v_const、水面无人舰艇最大角速度等效达到误差ε；

初始化采样时刻指标z＝0；

步骤4：测量水面无人舰艇当前时刻的位姿信息及其它必要状态

在第z个采样时刻，即t＝T₀+(z-1)δT时刻，测量水面无人舰艇的位姿信息，包括空间坐标艏向/>速度/>同时获取其它反映水面无人舰艇状态的必要变量，记当前时刻水面无人舰艇的状态为x_z；

若则认为水面无人舰艇已经抵达期望达到的目标点，自主导航任务结束；

步骤5：建立代表周围环境的0-1栅格地图

根据感知到的障碍物建立具有代表周围环境的0-1栅格地图，其中1表该栅格内存在障碍需要进行规避，记作障碍栅格，0代表该栅格无障碍可以通行，记作无障碍；所述栅格的尺寸根据导航所需的精度、水面无人舰艇尺寸、水面无人舰艇运动速度因素综合确定；

步骤6：使用改进的人工势场方法生成未来较短时间内的参考路径；

步骤6-1：初始化路径规划中的指标k并定义临时变量

初始化k＝0；定义临时变量

步骤6-2：若环境中存在动态障碍，更新障碍物位置信息

若环境中存在动态障碍物，则结合障碍物已知的运动规律或对其运动规律进行预测，从而更新t＝T₀+(z-1)δT+kΔT时刻的障碍物信息；

步骤6-3：根据目标点、障碍物的位置，确定改进人工势场方法中当前迭代步的合力的理想方向

首先，计算目标点产生的引力场U_att和对应的引力F_att，公式如下：

然后，计算由障碍物产生的斥力场和对应的斥力；对于栅格地图中的第j个障碍栅格，记其中心位置为则由该栅格产生的斥力场U_rep,j和对应的斥力F_rep,j如下：

记环境中一共有S个障碍栅格，则水面无人舰艇受到的合力为记/>的方向为/>则/>即为当前迭代步中合力的理想方向；

步骤6-4：使用角速度约束修正当前迭代步合力方向

根据时间步长ΔT、水面无人舰艇最大角速度计算相邻两个迭代步中水面无人舰艇艏向的最大允许改变量/>定义如下两个变量：

Δθ^[k]＝θ^[k]-θ^[k+1] (7)

则在下一迭代步的艏向增量由如下的饱和函数确定，即要求

根据式计算的艏向增量和步骤3中设定的水面无人舰艇运动速度，计算(k+1)时刻水面无人舰艇的位置与艏向，如下：

步骤6-5：改进人工势场方法终止条件判断

更新迭代指标k＝k+1；若满足k＝m，使用与/>构造spline插值函数x(t)、y(t)与θ(t)，从而代表水面无人舰艇的横坐标、纵坐标与艏向在时间区间[T₀+(z-1)δT,T₀+(z-1)δT+T]内的变化历程；若k＜m，返回步骤6-2

步骤7：使用模型预测控制算法对步骤6中的参考路径进行跟踪；

步骤7-1：建立面向轨迹跟踪的模型预测控制模型，如下：

其中，性能指标J中第一项e^TPe反映了跟踪误差，第二项u^TRu反映对控制量相关的评价，跟踪误差e＝(x-x(t),y-y(t),θ-θ(t))^T；f(x,u,t)与h(x,u,t)分别为步骤1中设定的无人水面舰艇动力学方程与约束条件；

步骤7-2：求解模型预测控制模型获得最优控制输入

采用合适的数值算法求解步骤7-1中建立的最优控制模型，使用步骤6中获得的参考路径对问题中横坐标、纵坐标、艏向变量进行初始化；记计算得到的控制历程为u^*(t)

步骤8：将步骤7计算得到的控制输入施加于无人水面舰艇；

在时间区间[T₀+(z-1)δT,T₀+zδT]内将u^*(T₀+(z-1)δT)施加于水面无人舰艇；

更新迭代指标z＝z+1，返回步骤4。