基于速度障碍与模糊参数的无人艇规划方法

：本发明属于无人艇，特别是涉及基于速度障碍与模糊参数的无人艇规划方法。

背景技术

0、
背景技术：

1、随着水面无人艇技术的快速发展，其应用领域日益扩大，涵盖海洋勘测、资源开发、边境巡逻等多个方面。因此，水面无人艇的运动规划研究变得尤为关键，这一研究领域的主要目标是提高无人艇的自主性、效率和安全性。无人艇的运动规划通常可分为路径规划与轨迹规划两个部分。路径规划负责在全局环境中搜索并确定从起始点到目标点的最佳路径，考虑全局环境信息如地图、障碍物等，并通常采用路径规划算法在已知的地图中搜索最优路径。而轨迹规划则负责在给定路径下，生成符合无人艇动力学的避碰轨迹，以确保无人艇在水域中高效、安全的行驶。

2、在无人艇的避障技术领域，已有研究者提出了多种解决方案。例如，改进的a算法适用于简单环境下的全局路径规划，但并未完全解决避障问题。基于稀疏点约束的迭代势场算法虽提高了避障效率，但未充分考虑无人艇的运动特性。改进的rrt算法考虑了无人艇的运动特性，但其随机性导致在多障碍环境下避障效果不佳。基于模型预测控制的避障算法虽具有较高的避障成功率，但在构建代价函数时未进行权重参数的实时调整，从而在多障碍物环境下存在非最优避障路线问题。双向rrt算法虽降低了搜索树的随机性，但主要集中在运动学层面，未深入考虑动力学特性。深度学习技术虽在路径规划方面显示出潜力，但结合无人艇运动特性构建神经网络较为困难，且模型训练需耗费大量时间和数据。

3、鉴于上述研究，本发明旨在解决多障碍环境下无人艇的避障成功率低和避障路线非最优的问题。本发明首先从无人艇的运动学与动力学角度出发，改进传统动态窗口法中的速度约束，转化为对无人艇的动力约束，并采用速度障碍法对每一时刻的动力系统输入量施加限制，以严格保证无人艇行进时能避开所有障碍物。接着，设计模糊参数选择器，借助专家经验构建模糊规则，对算法中评价函数的权重进行实时动态调整，使系统输入量更符合人为操纵习惯，优化避障路线。

技术实现思路

0、
技术实现要素：

1、本发明的目的是针对现有技术的缺陷，提供基于速度障碍与模糊参数的无人艇规划方法，以解决上述背景技术提出的问题。

2、为实现上述目的，本发明提供如下技术方案：基于速度障碍与模糊参数的无人艇规划方法，具体步骤如下：

3、s1：建立无人艇运动模型与轨迹规划问题模型；

4、s2：应用一种系统约束窗口法(sc-dwa)，将动态窗口法中的速度约束转化为系统输入约束，适应欠驱动无人艇的运动特性；对无人艇动力系统输入施加限制，包括最大值限制和变化量的最大变化量限制；使用速度障碍法对无人艇纵荡与横荡速度进行限制，确保避障；

5、s3：实施轨迹评优步骤，包括对系统输入空间进行采样，生成多条预测轨迹；设计评价函数，对每条预测轨迹进行评分，考虑方向角度、障碍物距离、纵荡线速度因素；

6、s4：选取最优轨迹，操作如下：对评价函数结果进行归一化处理，为每个评价指标配上权重参数；基于总分选择评分最高的轨迹作为最优轨迹；

7、s5：融合模糊参数选择器(fsc-dwa)以调整评价函数权重，包括：对输入量进行模糊化，采用mamdani模糊规则进行模糊推理；对输出模糊值进行解模糊，实时调整权重参数。

8、作为本发明的一种技术优选方案，步骤s1中所述无人艇运动模型，对于欠驱动无人艇在水域中行驶，其运动学与动力学数学模型可以描述为：

9、

10、

11、

12、式中，下标k与k+1分别表示当前时刻与下一时刻；η＝[x,y,ψ]t为位姿向量，包括无人艇在惯性坐标系下的坐标(x,y)和艏摇角ψ；ν＝[u,v,r]t为速度向量，包括无人艇在附体坐标系下的纵/横荡速度(u,v)和艏摇角速率r；τ＝[τu,0,τr]t为系统输入向量；m＝diag(m11,m22,m33)和d(ν)＝diag(d11,d22,d33)分别为惯性质量矩阵和系统阻尼矩阵；c(ν)为科里奥利向心矩阵；r(*)为关于姿态向量“*”的旋转矩阵函数。

13、作为本发明的一种技术优选方案，步骤s1中所述轨迹规划问题具体表现为：动态窗口法中所采用的速度约束公式未能充分适应欠驱动无人艇的运动特性，导致在水域环境中的避障决策受到限制，以下是该速度约束公式：

14、

15、式中，um是最大纵荡线速度，rm是最大艏摇角速度，dist(u,r)是与预测轨迹相交的障碍物与无人艇之间的最近距离。

16、作为本发明的一种技术优选方案，步骤s2中所述系统约束窗口法，该算法进一步包括以下步骤：

17、s2-1:将动态窗口法中的速度约束，转化为系统输入约束，由于无人艇自身动力系统存在极限，系统输入需受以下约束：

18、约束一：当前时刻的系统输入，不能超过系统额定的最大输入：

19、τs1∈{(τu_k,τr_k)|τu_min≤τu_k≤τu_max,τr_min≤τr_k≤τr_max} (3)

20、约束二：系统输入变化量不能超过系统额定的最大变化量：

21、τs2∈{(τu_k,τr_k)|τu_k-1-δτu_m≤τu_k≤τu_k-1+δτu_m,

22、τr_k-1-δτr_m≤τr_k≤τr_k-1+δτr_m} (4)

23、式中，δτu_m为τu的最大变化量，δτr_m为τr的最大变化量；

24、约束三：下一时刻的可达避障速度，对于当前时刻的系统输入也存在约束：

25、

26、式中，τk＝(τu_k,0,τr_k)t为当前时刻系统输入向量，c为碰撞锥，νk+1为下一时刻的可达避障速度；

27、s2-2:经过步骤s2-1约束后，假设当前时刻所有满足约束的系统输入共同组成系统输入空间τs，可将其描述为：

28、τs∈τs1∩τs2∩τs3 (6)

29、s2-3:假设存在障碍物o做了膨胀处理，过无人艇的几何中心p做o膨胀边界的两条切线l1与l2，由l1、l2、和点p组成的锐角l1pl2称为碰撞锥c，v'表示纵荡速度u，横荡速度v，以及障碍物速度vo的合速度，那么，要使无人艇避开障碍物，合速度v'需满足以下关系式：

30、

31、作为本发明的一种技术优选方案，步骤s3中所述实施轨迹评优步骤，具体评优由以下步骤组成：

32、s3-1:对于系统输入空间τs，以0.5为采样间隔，分别对τs中的τu和τr进行采样，得到若干组采样输入(τui,τri)

33、(τui,τri)∈{(τui,τri)|τui∈τs,τui∈τs,i∈n*} (8)

34、其中，n*为正整数集合，i表示第i组采样；

35、s3-2：采样后，对于每一组采样结果，在2s的模拟时间内都可以模拟出一条预测轨迹；

36、s3-3：使用以下评价函数，计算每一条轨迹的优劣，具体计算公式如下：

37、评价函数一：评价预测轨迹末端点的行驶方向和终点方向之间的角度大小，角度越小，评价函数得分越高：

38、head(τu,τr)＝180-λ (9)

39、式中，λ为预测轨迹末端点的行驶方向和终点方向之间的角度；

40、评价函数二：评价预测轨迹末端点与最近障碍物的距离大小，距离越大，评价函数得分越高：

41、

42、式中，(x,y)为预测轨迹末端点坐标，(xo,yo)为最近障碍物的膨胀椭圆圆心坐标，d为膨胀边界到膨胀椭圆圆心的距离；

43、评价函数三：评价预测轨迹末端点的纵荡线速度大小，线速度越大，评价函数得分越高：

44、vel＝abs(u) (11)

45、式中，u为预测轨迹末端点的纵荡线速度，abs(u)为u的绝对值。

46、作为本发明的一种技术优选方案，所述s3-1中对系统输入空间进行采样，具体采样由以下步骤组成：

47、s3-1-1：利用计算得到的速度约束结果taus1和taus2，分别对应于算法中的式(3)和式(4)，以及另一组速度约束taus3，对应于公式(5)；

48、s3-1-2：确定采样空间space，通过计算taus1和taus2的交集得到，其中：space的前两个元素分别为taus1和taus2中对应元素的最大值和最小值，确定无人艇推力输入τui的采样范围；space的后两个元素同样基于taus1和taus2中对应元素的最大值和最小值，确定无人艇转向输入τri的采样范围；

49、s3-1-3：通过对space进行两重循环采样，采样间隔为0.5，以生成多个推力和转向输入组合(τui,τri)；

50、s3-1-4：对每个采样点(τui,τri)进行检验，判断其是否同时满足taus3中的约束条件；

51、s3-1-5：将满足条件的采样点(τui,τri)添加到结果集res中，用于后续轨迹规划或其他相关计算。

52、作为本发明的一种技术优选方案，步骤s4中所述选取最优轨迹，将评价函数的结果分别做归一化处理，并为每个评价函数配上相应的权重参数，方可计算每一条预测轨迹的总分，计算公式为：

53、

54、式中，α，β，γ均为权重参数，下标i表示第i条轨迹，max(*)表示取*中的最大值。

55、作为本发明的一种技术优选方案，步骤s5中所述融合模糊参数选择器进行评价函数权重的调整，其具体步骤为：

56、s5-1：使用分档模糊集法制定模糊集，并选取三角形隶属度函:将模糊参数选择器的输入量进行模糊化，三角形隶属函数的关系式为：

57、

58、式中，a和c为隶属度函数底端横坐标，b为隶属度函数顶端横坐标；

59、s5-2：选取mamdani模糊规则进行模糊推理，得到输出模糊值，其计算公式为：

60、

61、式中，ri为第i条模糊规则，i∈n*，模糊规则中的语言变量ai,n和bi,m分别为模糊参数选择器的输入变量xi和输出变量yi定义在第i条模糊规则中的模糊集；

62、s5-3：最后，采用面积平均法对输出模糊值进行解模糊，计算公式为：

63、

64、式中，n为模糊运算结果中包含的模糊集个数，yi*为第i个模糊集与坐标轴围成面积的平分线所对应的值，μimax为第i个集合的隶属度，y*为解模糊所得的精确值。

65、与相关的现有技术相比，本技术提案相比现有技术，有以下主要技术优点：本发明的有益效果是：

66、提高避障成功率：通过引入系统约束窗口法(sc-dwa)，本发明从动力学角度出发，对无人艇的系统输入进行约束。特别是对横漂速度的严格限制，确保了无人艇在行进时始终能够有效避开障碍物，显著提高了避障的成功率。

67、优化避障轨迹：本发明进一步提出了基于模糊参数选择器的系统约束窗口法(fsc-dwa)。通过结合专家经验和实时调整权重参数，fsc-dwa算法使规划的输入量更符合人的操纵习惯，优化了避障轨迹，使其更加合理。

68、轨迹性能提升：经仿真验证，本发明生成的轨迹在曲折性和轨迹长度方面明显优于传统动态窗口法。这证实了本发明在改善轨迹规划方面的有效性，提供了更为平滑和短距离的避障路线。

69、工程适用性增强：本发明为后续研究提供了新的方向，即将本算法与全局路径规划算法相结合。这一结合有望进一步提高无人艇运动规划的工程适用性，为无人艇在复杂水域环境中的自主运行提供更为强大的支持。