[发明专利]一种基于动力学约束的全局-局部混合无人船路径规划方法

权利要求书

1.一种基于动力学约束的全局-局部混合无人船路径规划方法，其特征在于，包括如下步骤：

S1、构建无人船运动学模型，并对构建的无人船运动学模型进行动力学分析；所述步骤S1具体为：

S11、构建无人船运动学模型，所述无人船运动学模型具体如下：

其中，(x,y,ψ)表示无人船在地球坐标系的位置和航向，(u,v,r)分别表示在船体坐标系下无人船的前向速度、横向速度和航向角速度；

S12、针对欠驱动无人船模型，由于前向和偏航动力的限制，横向速度v是有界的，因此在动力学约束的情况下，需约束前向速度和航向角速度，且这两个速度受限于：

其中，★∈{u,r}是受限边界；表示前向加速度，表示航向角加速度；

S13、设定安全范围，以无人船中心为圆心设定一个在整个避障过程中不能与障碍物接触的安全范围D_sA，保证无人船的安全行驶，安全范围D_sA被定义为：

其中，p_s＝[x_s,y_s]^T，p_s表示无人船当前位置坐标，x_s，y_s分别表示无人船当前位置横坐标和纵坐标，p＝[x₁,y₁]^T，d_s为安全范围半径，p(t)表示无人船安全范围内点的坐标，x₁，y₁分别表示无人船安全范围内点的横坐标和纵坐标；

S2、考虑无人船安全范围，采用存储节点的方式，实现无人船全局路径的规划；

S3、针对动态的不可预见的环境，构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构，实现无人船局部路径的规划；所述步骤S3具体为：

S31、构建基于FDM模糊决策模块和FDW精准动态窗口模块的分层结构，具体如下：

其中，p_obs＝[x₀,y₀]^T表示障碍物的位置，d_so＝η·u₁是由障碍物的速度决定的安全扩展域长度，η＞0表示安全扩展系数，在这里传感器的测量范围是0＜L_FDW＜L_FDM＜∞；L_FDW表示FDW模块的作用范围；

S32、所述FDM模糊决策模块为了躲避距离较远的障碍物，通过考虑路径点和障碍物的相对方向，建立模糊规则表；

S33、在所述FDW精准动态窗口模块中：

可行速度限制必须能确保无人船紧急停车实现紧急避碰，因此可行性速度集合定义如下：

其中，l(p,p_o)是在规划的弧形路径上无人船与最近障碍物的距离；

通过可达速度集V_r的约束形成速度量动态窗口，且该动态窗口里的速度可以在下一个时间间隔内精准到达，基于当前无人船的速度(u_c,r_c)，可达速度集V_r定义如下：

其中，T0是时间间隔，u_c和r_c分别表示无人船当前的前向速度和航向角速度；

基于无人船动力学固有特性影响，可执行性集V_p定义如下：

结合V_a、V_r和V_p，最终得出复合约束集V_c：

V_c＝V_a∩V_r∩V_p

S34、在复合约束集V_c的约束条件下，采用如下评价函数得出最优速度：

G_c(u,r)＝αheading(u,r)+βdist(u,r)+γvelocity(u,r)

其中，heading(u,r)表示使无人船朝向目标点航行的评价因素，其中表示目标点的方向，表示无人船的航向；dist(u,r)表示在相应预测轨迹上无人船与最近障碍物的距离，如果距离太大或者在轨迹附近没有障碍物，则此评价因素被赋值为定量常数；velocity(u,r)表示无人船沿着相应轨迹的航行速度；α＞0、β＞0、γ＞0是定义的权重系数；

S4、融合上述无人船全局路径规划方案和无人船局部路径规划方案，形成全局局部混合避障策略。