基于动态势场的多无人船同步无碰撞路径规划与轨迹跟踪控制方法

摘要: 本发明涉及无人船自动驾驶的,尤其涉及一种基于动态势场的多无人船同步无碰撞路径规划与轨迹跟踪控制方法。背景技术:1、无人船在海洋资源开发、环境监测、水文测量、安全...
基于动态势场的多无人船同步无碰撞路径规划与轨迹跟踪控制方法

本发明涉及无人船自动驾驶的,尤其涉及一种基于动态势场的多无人船同步无碰撞路径规划与轨迹跟踪控制方法。

背景技术:

1、无人船在海洋资源开发、环境监测、水文测量、安全巡检和智能航运等领域具有广泛应用。相较于单艘无人船,多无人船协同系统能够扩大作业范围、提高任务执行效率,并增强复杂环境下的任务适应能力。多无人船在共享水域内协同航行时,既需要考虑目标点牵引和固定障碍物避让,还需要处理船舶之间的动态交互关系。路径冲突、避碰不及时以及协同决策不一致等问题均可能降低航行安全性和任务完成效率。

2、人工势场法具有结构简单、计算量小和便于实时实现等特点,常用于无人船局部路径规划。传统人工势场法通常通过目标点吸引势场和障碍物排斥势场共同作用,引导运动体向目标点移动并避开障碍物。然而,在多无人船协同场景中,仅考虑固定障碍物难以充分描述其他无人船对当前无人船产生的动态避碰影响,且串行式规划容易出现先规划船只影响后规划船只的顺序偏置。

3、轨迹跟踪控制是无人船自主航行的重要环节,其作用是使无人船沿规划出的参考轨迹稳定运动。比例微分控制器具有结构简单、参数易整定、实时性好等特点,适合在仿真验证和工程实现中作为轨迹跟踪控制器使用。但对于多无人船路径规划与轨迹跟踪一体化问题,仍需要一种能够同时考虑动态势场避碰、控制输入约束、同步迭代更新和轨迹跟踪稳定性的技术方案。

4、因此,亟需提出一种面向多无人船共享水域协同航行的无碰路径规划与轨迹跟踪控制方法,以在固定障碍物和动态船舶交互共同存在的情况下,生成平滑、可行、满足避碰要求的参考轨迹,并通过轨迹跟踪控制保证无人船能够稳定再现规划轨迹。

5、申请号为202510306151.8的发明专利公开了一种基于动态势场结合rrt算法的无人船路径规划方法,包括:确定待规划路径的起点与目标点;在地图空间中,以起点作为初始节点;通过成本函数为当前步节点选取对应的采样点;在已有节点中搜索出距离所述采样点最近的节点;以最近节点为起点,基于人工势场法结合威胁指数生成下一步节点;为每一步节点选取对应的采样点,直至其下一步节点与目标节点的距离在预设阈值内,从目标点回溯至起点生成初始路径;利用三次样条差值法平滑初始路径,输出完成规划的路径。通过降低采样成本以及评估障碍物对无人水面艇的潜在风险,提高下一步节点生成效率,并实现有效避障,进而提高路径规划的效率和质量。但是,现有的局部路径规划通常只考虑固定障碍物,这使得难以充分描述和处理其他无人船对现有无人船的动态碰撞避免效应。

技术实现思路

1、针对多无人船协同航行中路径冲突、避碰不及时、串行规划顺序偏置以及规划轨迹难以稳定跟踪的技术问题,本发明提出一种基于动态势场的多无人船同步无碰撞路径规划与轨迹跟踪控制方法,本发明实现了一种针对多无人船只、面向共享水域的同步迭代路径规划和轨迹跟踪控制方法,能够生成平滑、运动学上可行的参考轨迹,满足复杂场景中安全避免碰撞的要求,并通过控制器确保多艘无人船能够稳定且无碰撞地跟踪轨迹。

2、为了达到上述目的,本发明的技术方案是这样实现的:基于动态势场的多无人船同步无碰撞路径规划与轨迹跟踪控制方法,其步骤包括:

3、s1:建立惯性坐标系下无人船位姿状态和机体坐标系下无人船速度状态,构建三自由度运动学模型并进行离散化,获得无人船初始状态;

4、s2:针对无人船构建含终点吸引势场、固定障碍物排斥势场及动态障碍物排斥势场的总势场;

5、s3:设置控制输入幅值约束和输入增量约束,生成当前可行输入区间;在当前可行输入区间内生成候选控制输入集合并结合总势场计算综合代价;根据综合代价选择最优控制输入,将最优控制输入代入离散化的三自由度运动学模型预测下一采样时刻状态;

6、s4:采用多无人船同步更新机制,在多艘无人船均完成当前时刻规划后,同步将状态更新至下一采样时刻,得到规划轨迹和控制输入序列;

7、s5:设计带运动学前馈补偿的比例微分轨迹跟踪控制器,将规划轨迹作为输入,输出满足执行约束的纵向速度、横向速度和转向速度指令,驱动无人船跟踪参考轨迹。

8、所述三自由度运动学模型为:

9、;

10、其中,、、、分别为第i艘无人船在时间t的航向角、纵向速度、横向速度和转向速度,、、分别为第i艘无人船在时间t的纵向位置导数、横向位置导数和航向角导数;

11、采用欧拉一阶离散法对三自由度运动学模型进行离散化:

12、;

13、其中,、、分别为第i艘无人船在采样时刻k的纵向位置、横向位置和航向角,、、分别为第i艘无人船在采样时刻k的纵向速度、横向速度和转向速度,、、分别为第i艘无人船在下一采样时刻的纵向位置、横向位置和航向角,为采样周期。

14、所述总势场为:

15、;

16、其中,为无人船当前位置,为终点吸引势场,为固定障碍物排斥势场,为动态障碍物排斥势场。

17、所述设置控制输入幅值约束和输入增量约束,生成当前可行输入区间的方法为:

18、对第艘无人船的控制输入设置幅值约束:

19、,,;

20、其中,、分别为无人船纵向速度幅值的最小值和最大值约束,、分别为无人船横向速度幅值的最小值和最大值约束,、分别为无人船转向速度幅值的最小值和最大值约束;

21、设置输入增量约束:

22、;

23、其中,、、分别为第i艘无人船在上一采样时刻的纵向速度、横向速度和转向速度,为纵向速度输入增量的最大值,为横向速度输入增量的最大值,为转向速度输入增量的最大值;

24、第艘无人船在采样时刻的可行输入区间为:

25、。

26、所述候选控制输入为:

27、;

28、其中,表示第组候选控制输入纵向速度,表示第组候选控制输入横向速度,表示第组候选控制输入转向速度,为候选控制输入集合中的索引;

29、所述综合代价为:

30、;

31、其中,为势场代价,为距离代价,为航向代价,为控制输入变化量代价,为倒退惩罚项,为碰撞惩罚项。

32、所述根据综合代价选择最优控制输入,将最优控制输入代入离散化的三自由度运动学模型预测下一采样时刻状态的方法为:

33、在所有候选控制输入集合中,选取综合代价最小的一组作为采样时刻k的最优控制输入:

34、;

35、其中,为最优纵向速度,为最优横向速度,为最优转向速度,表示当目标函数最小化时的极小值的自变量;

36、所述下一采样时刻状态为:

37、;

38、其中,、、分别为第艘无人船在下一采样时刻的最优纵向位置、最优横向位置和最优偏航角。

39、所述采用多无人船同步更新机制,在多艘无人船均完成当前时刻规划后,同步将状态更新至下一采样时刻的方法为:

40、每艘无人船基于采样时刻的状态独立完成当前时刻规划,得到各自的下一采样时刻状态和最优控制输入;

41、在每艘无人船均完成当前时刻规划后,再统一更新状态:;

42、将最优控制输入作为下一采样时刻输入增量约束的参考值:

43、。

44、所述得到规划轨迹和控制输入序列的方法为:

45、每次状态更新后,判断第艘无人船当前位置与终点之间的距离是否小于给定阈值:

46、;

47、当所有无人船均满足终点判定条件时,规划过程结束;

48、记录每艘无人船的规划轨迹:;

49、其中,为第艘无人船从起始点航行至终点所经历的总离散迭代步数,;

50、记录每艘无人船的控制输入序列:

51、。

52、将惯性坐标系下的位置误差转换到机体坐标系的方法为:

53、;

54、其中,、分别为第i艘无人船在采样时刻k的纵向位置误差和横向位置误差,、分别为第i艘无人船在采样时刻k的期望纵向位置和期望横向位置;

55、航向误差为:;其中,为第i艘无人船在采样时刻k的航向误差,为第艘船在采样时刻的期望航向角。

56、所述比例微分轨迹跟踪控制器的控制律计算方法为:

57、;

58、其中,、为运动学前馈补偿项,、分别为第i艘无人船在采样时刻k的期望纵向速度和期望横向速度;

59、三通道比例微分控制律为:

60、;

61、其中,、、为比例增益,、、为微分增益,为第i艘无人船在采样时刻k的期望转向速度。

62、本发明的有益效果:

63、1、本发明将终点吸引势场、固定障碍物排斥势场和其他无人船动态障碍物势场统一到一个动态总势场中,使每艘无人船能够在规划过程中同时考虑目标牵引、静态避障和船间动态避碰。

64、2、本发明采用多无人船同步更新机制,各无人船在同一采样时刻均基于相同的全局状态进行独立规划,待全部规划完成后再统一更新,能够避免串行规划顺序对规划结果造成的影响。

65、3、本发明在候选输入选择过程中同时考虑势场值、终点距离、航向误差、输入变化量、倒退惩罚和船间安全距离惩罚,使规划轨迹具有较好的避碰安全性、平滑性和运动可行性。