一种无人艇及无人艇编队在线航迹规划方法及系统与流程

1.本发明涉及无人艇航迹规划技术领域，特别是涉及一种无人艇及无人艇编队在线航迹规划方法及系统。


背景技术：

2.现有大部分无人艇航迹规划解决方案多采用离线任务航迹规划模式，在结构化或海图已知环境下以寻关键任务点的方式操纵无人艇，此种解决方案被称为半自主化方案，离线规划的任务航迹不能处理海图信息的不确定性和海上出现的突发海事情况，因此，离线规划的航迹难以应用于无人艇自主航行任务。
3.现有的半自主无人艇系统采用智能搜索方法，如离线规划任务航迹，然后控制无人艇以直线航迹依次到达各个关键任务点，这个过程中，需要保证环境信息是静态的，且环境对无人艇运动干扰较小，然而，现实的海洋环境是动态的、非结构化的，具有大量的未知和不确定因素，例如，海况时刻变化、海洋中漂浮的碍航物位置随海浪流动、岛屿信息不精确、存在预先难以发现的小目标碍航物、预先规划的航迹上可能出现行为和意图未知的动态船舶、无人艇运动控制模型和参数本身存在误差，使得无人艇实际执行的航迹与预规划的航迹偏差较大，此时，传统智能化搜索方法很难根据实时环境信息调整或重规划航迹，并且，传统智能方法的计算复杂度高，搜索过程中存在两点边值问题(boundary value problem)，通常无人艇艇载计算机的处理能力有限，计算资源有限，因此，传统智能化方案难以快速响应外部环境的变化并在线规划航迹。
4.传统的基于行为的反应式的航迹行为推理器通常是短视的，且需要微调的参数较多，会造成规划器陷入局部最优，而基于案例的模糊逻辑推理机构通常需要大量的规则推理出一条避障行为，而当下流行的强化学习系统在在线规划过程中经常不够稳定，基于数据驱动的深度学习模型通常对数据集要求较高，然而，通常情况下可用的数据集有限，且负例样本比较稀缺。
5.如何实现无人艇及无人艇编队的在线航迹规划，成为一个亟待解决的技术问题。


技术实现要素：

6.本发明的目的是提供一种无人艇及无人艇编队在线航迹规划方法及系统，以实现无人艇及无人艇编队的在线航迹规划。
7.为实现上述目的，本发明提供了如下方案：
8.本发明提供一种无人艇在线航迹规划方法，包括如下步骤：
9.在无人艇的感知域内选取子目标点；
10.在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点；
11.采用人工势场法调整所述初始采样点，获得调整后的采样点；
12.判断调整后的采样点与障碍物的距离是否大于安全距离，获得第一判断结果；
13.若所述第一判断结果表示是，则采用rrt*规划器将航迹向调整后的采样点扩展预
设距离；
14.若所述第一判断结果表示否，则采用rrt*规划器将航迹向初始采样点扩展预设距离；
15.返回步骤“在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点”，直到航迹规划时间用完。
16.可选的，所述采用人工势场法调整所述初始采样点，获得调整后的采样点，具体包括：
17.基于人工势场法构建合势场；
18.根据合势场计算无人艇受到的合力；
19.利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点；其中，第0次调整后的采样点为初始采样点；
20.判断i的数值是否小于调整次数阈值，获得第二判断结果；
21.若所述第二判断结果表示是，则令i的数值增加1，返回步骤“利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点”；
22.若所述第二判断结果表示否，则输出第i次调整后的采样点。
23.可选的，所述基于人工势场法构建合势场，具体包括：
24.判断无人艇与障碍物的距离是否大于避障半径，获得第三判断结果；
25.若所述第三判断结果表示是，则采用人工势场法构建子目标点产生的引力势场作为所述合势场；
26.若所述第三判断结果表示否，则采用人工势场法构建子目标点产生的引力势场和障碍物产生的斥力势场的合势场。
27.可选的，所述利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点，具体包括：
28.根据所述合力，利用公式q
rs,i
＝q
rs,i-1
λ(f
total
/|f
total
|)，对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点；
29.其中，q
rs,i-1
和q
rs,i
分别表示第i-1次调整后和第i次调整后的采样点；f
total
表示合力，λ表示调整率。
30.一种无人艇在线航迹规划系统，包括：
31.子目标点选取模块，用于在无人艇的感知域内选取子目标点；
32.初始采样点选取模块，用于在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点；
33.初始采样点调整模块，用于采用人工势场法调整所述初始采样点，获得调整后的采样点；
34.第一判断模块，用于判断调整后的采样点与障碍物的距离是否大于安全距离，获得第一判断结果；
35.第一航迹扩展模块，用于若所述第一判断结果表示是，则采用rrt*规划器将航迹向调整后的采样点扩展预设距离；
36.第二航迹扩展模块，用于若所述第一判断结果表示否，则采用rrt*规划器将航迹向初始采样点扩展预设距离；
37.返回模块，用于返回步骤“在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点”，直到航迹规划时间用完。
38.可选的，所述初始采样点调整模块，具体包括：
39.合势场构建子模块，用于基于人工势场法构建合势场；
40.合力计算子模块，用于根据合势场计算无人艇受到的合力；
41.采样点调整子模块，用于利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点；其中，第0次调整后的采样点为初始采样点；
42.第二判断子模块，用于判断i的数值是否小于调整次数阈值，获得第二判断结果；
43.返回子模块，用于若所述第二判断结果表示是，则令i的数值增加1，返回步骤“利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点”；
44.调整后的采样点输出子模块，用于若所述第二判断结果表示否，则输出第i次调整后的采样点。
45.可选的，所述合势场构建子模块，具体包括：
46.第三判断子模块，用于判断无人艇与障碍物的距离是否大于避障半径，获得第三判断结果；
47.第一合势场构建单元，用于若所述第三判断结果表示是，则采用人工势场法构建子目标点产生的引力势场作为所述合势场；
48.第二合势场构建单元，用于若所述第三判断结果表示否，则采用人工势场法构建子目标点产生的引力势场和障碍物产生的斥力势场的合势场。
49.可选的，所述采样点调整子模块，具体包括：
50.采样点调整单元，用于根据所述合力，利用公式q
rs,i
＝q
rs,i-1
λ(f
total
/|f
total
|)，对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点；
51.其中，q
rs,i-1
和q
rs,i
分别表示第i-1次调整后和第i次调整后的采样点；f
total
表示合力，λ表示调整率。
52.一种无人艇编队在线航迹规划方法，包括如下步骤：
53.根据无人艇编队中每个成员艇的位置和感知域，确定虚拟领航者的位置和无人艇编队的感知域；
54.根据虚拟领航者的位置和无人艇编队的感知域，采用无人艇在线航迹规划方法规划所述虚拟领航者的局部航迹；
55.无人艇编队中的每个成员艇分别根据所述无人艇与虚拟领航者的相对位置和所述局部航迹，确定每个所述无人艇在下一时刻的期望位置；
56.采用rrt*算法和所述人工势场法确定扩展每个所述无人艇的航迹至每个所述无人艇的期望位置。
57.一种无人艇编队在线航迹规划系统，包括设置在每个成员艇上的导航模块、运动控制模块、实时感知模块、紧急制动模块和数据存储模块，设置在艇载工控机上的在线航迹规划模块和网络报文调度模块，所述艇载工控机设置在无人艇编队中的任意一个成员艇上；
58.每个成员艇的导航模块、运动控制模块、实时感知模块、紧急制动模块和数据存储模块分别通过所述网络报文调度模块与所述在线航迹规划模块连接；
59.所述在线航迹规划模块用于根据无人艇编队中每个成员艇的位置和感知域，确定虚拟领航者的位置和无人艇编队的感知域；根据虚拟领航者的位置和无人艇编队的感知域，采用无人艇在线航迹规划方法规划所述虚拟领航者的局部航迹。
60.根据本发明提供的具体实施例，本发明公开了以下技术效果：
61.本发明公开了一种无人艇及无人艇编队在线航迹规划方法及系统，无人艇在线航迹规划方法包括如下步骤：在无人艇的感知域内选取子目标点；在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点；采用人工势场法调整所述初始采样点，获得调整后的采样点；判断调整后的采样点与障碍物的距离是否大于安全距离，获得第一判断结果；若所述第一判断结果表示是，则采用rrt*规划器将航迹向调整后的采样点扩展预设距离；若所述第一判断结果表示否，则采用rrt*规划器将航迹向初始采样点扩展预设距离；返回步骤“在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点”，直到航迹规划时间用完。本发明基于rrt*算法和人工势场法实现无人艇航迹的在线快速规划。
附图说明
62.为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
63.图1为本发明提供的一种无人艇在线航迹规划方法的流程图；
64.图2为本发明提供的一种无人艇编队在线航迹规划方法的流程图；
65.图3为本发明提供的避障方法的原理图；
66.图4为本发明提供的一种无人艇编队在线航迹规划系统的结构示意图。
具体实施方式
67.下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
68.本发明的目的是提供一种无人艇及无人艇编队在线航迹规划方法及系统，以实现无人艇及无人艇编队的在线航迹规划。
69.为使本发明的上述目的、特征和优点能够更加明显易懂，下面结合附图和具体实施方式对本发明作进一步详细的说明。
70.本发明提出的系统解决方案是将避障控制问题拆分为两阶段的航迹-速度规划的在线航迹规划模块，在线路径规划模块着重解决最优的可行航迹查询问题，将航迹跟踪问题交给交给底层的运动控制模块，模块中采用的两种规划方案是人工势场法(artificial potential field，apf)和随机化的最优化的快速扩展随机树方法(optimal rapidly-exploring random tree star，rrt*)。
71.基于人工势场法的航迹规划器，在指向目标的引力势场和躲避障碍物的斥力势场
的合势场梯度下降的方向快速查询航迹，但是，该方案建立在对已知环境模型的数学建模基础上，由于无人艇运动模型和环境信息不精确，使得障碍物产生的斥力势场间不可比，因此算法存在局部最优、相似障碍物间存在震荡等问题，且该方案没有考虑无人艇的运动情况，难以考虑运动约束，例如，无人艇最小转弯半径约束和速度/加速度性能限制条件，且难以考虑实际航迹执行代价，例如，实际航迹过程中，无人艇的急转向航迹难以跟踪且耗能较大，且海流对无人艇能耗影响很大。因此，该方案直接应用于实际海洋环境下时不够充分。
72.为了解决人工势场方案可能存在的不足，本专利采用rrt*作为基础航迹规划器，将环境势场作为在线规划的启发引导条件，共同构建在线航迹规划器。
73.基本rrt*方案的基本运算流程是，首先采用均匀分布的概率密度函数随机选择无碰撞采样点，然后，试着向采样点扩展，在扩展之前，根据运动模型在无人艇的状态空间中选择合适的策略，从航迹树上采样点的最近邻节点向采样点扩展，在此过程中考虑扩展的可行性，即，扩展是否是无碰撞的以及扩展是否满足约束条件，静态障碍物的碰撞检测主要是指扩展出的航迹段是否与障碍物相交。传统rrt*方法会通过重连航迹边进行航迹树的局部优化，以保证航迹树根节点到达各树节点的航迹边组合在当前情况下是最短的，重连过程是，当扩展成功，生成新的航迹树节点qnew后，在qnew的邻域内选取qnew的近邻航迹树节点，判断近邻节点将qnew作为父节点重连后，航迹树根节点到达近邻节点的航迹边长度是否小于原来从根节点到达近邻节点的航迹边长度，若条件成立，则将近邻节点重连到qnew上，并删除其与原来父节点之间的边。
74.rrt*采用增量扩展方式，在路径的每一步扩展过程中考虑无人艇的运动模型和约束条件，由于在线规划出的路径为无人艇智能控制提供关键目标点，属于上层决策过程，规划出的路径还需要进行多步处理，以形成最终的可执行轨迹，实时性是在线路径规划方案必须满足的条件，通常情况下，模型预测控制方法要求在线路径规划器以不低于1mhz的频率提供运动控制目标点。因此，在规划过程中不需要求解复杂的运动和约束微分方程，采用简化的三自由度无人艇运动模型，在扩展过程中，判断规划空间中的备选路径点是否存在约束违背、冲突等现象。
75.航迹能耗同样是航迹规划器的重要性能指标，在海洋环境下，无人艇的能耗与海流密切相关，能耗较优的航迹应该包含较多的顺流部分，降低无人艇抵抗海流的能耗，通过在航迹点之间的代价距离的定义中考虑海流因素，采用rrt*重连过程优化航迹，以在线规划能耗较低的航迹。
76.另外，对于无人艇编队而言，编队保持过程中成员之间的协同约束条件也非常复杂，多艇编队成员航迹规划过程应该是动态的，每个成员的位置都会成为其他成员航迹规划的约束条件，如果独立规划每个成员的航迹，那么，规划器的计算复杂程度通常难以接受。为此，本发明拟采用一种虚拟领航者的思路，在线规划编队中心点的参考航迹，以参考航迹点和各无人艇成员的方位信息为协同变量信息，分布式地协同规划成员航迹。
77.本发明提出一种无人艇及无人艇编队在线航迹规划方法及系统，为无人艇编队成员在线快速规划局部航迹，综合海事规则、海流信息，在航迹长度和能耗之间做出折中，规划综合性能较优的航迹。
78.实施例1
79.实施例1提供一种无人艇在线规划方法及系统。
80.如图1所示，一种无人艇在线航迹规划方法，包括如下步骤：
81.步骤101，在无人艇的感知域内选取子目标点。
82.子目标选取的原则是：若当前感知域与全局航迹有交集，在当前无人艇的感知域范围，距离无人艇的局部航迹终点最远的全局航迹上的关键任务点，若当前感知域与全局航迹无交点，则在感知域边界上选择距离全局航迹最近的点作为局部在线规划的子目标点。
83.步骤102，在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点。
84.步骤103，采用人工势场法调整所述初始采样点，获得调整后的采样点。
85.步骤103具体包括：基于人工势场法构建合势场；根据合势场计算无人艇受到的合力；利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点；其中，第0次调整后的采样点为初始采样点；判断i的数值是否小于调整次数阈值，获得第二判断结果；若所述第二判断结果表示是，则令i的数值增加1，返回步骤“利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点”；若所述第二判断结果表示否，则输出第i次调整后的采样点。
86.其中，所述基于人工势场法构建合势场，具体包括：判断无人艇与障碍物的距离是否大于避障半径，获得第三判断结果；若所述第三判断结果表示是，则采用人工势场法构建子目标点产生的引力势场作为所述合势场；若所述第三判断结果表示否，则采用人工势场法构建子目标点产生的引力势场和障碍物产生的斥力势场的合势场。
87.所述利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点，具体包括：根据所述合力，利用公式q
rs,i
＝q
rs,i-1
λ(f
total
/|f
total
|)，对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点；其中，q
rs,i-1
和q
rs,i
分别表示第i-1次调整后和第i次调整后的采样点；f
total
表示合力，λ表示调整率。
88.步骤104，判断调整后的采样点与障碍物的距离是否大于安全距离，获得第一判断结果。
89.步骤105，若所述第一判断结果表示是，则采用rrt*规划器将航迹向调整后的采样点扩展预设距离。
90.步骤106，若所述第一判断结果表示否，则采用rrt*规划器将航迹向初始采样点扩展预设距离。
91.返回步骤“在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点”，直到航迹规划时间用完。
92.一种无人艇在线航迹规划系统，包括：子目标点选取模块，用于在无人艇的感知域内选取子目标点；初始采样点选取模块，用于在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点；初始采样点调整模块，用于采用人工势场法调整所述初始采样点，获得调整后的采样点；第一判断模块，用于判断调整后的采样点与障碍物的距离是否大于安全距离，获得第一判断结果；第一航迹扩展模块，用于若所述第一判断结果表示是，则采用rrt*规划器将航迹向调整后的采样点扩展预设距离；第二航迹扩展模块，用于若所述第一判断结果表示否，则采用rrt*规划器将航迹向初始采样点扩展预设距离；返回模块，用于返回步骤“在无人艇的感知域内采用均匀分布的随机函数选取无碰撞的初始采样点”，
直到航迹规划时间用完。
93.其中，所述初始采样点调整模块，具体包括：合势场构建子模块，用于基于人工势场法构建合势场；合力计算子模块，用于根据合势场计算无人艇受到的合力；采样点调整子模块，用于利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点；其中，第0次调整后的采样点为初始采样点；第二判断子模块，用于判断i的数值是否小于调整次数阈值，获得第二判断结果；返回子模块，用于若所述第二判断结果表示是，则令i的数值增加1，返回步骤“利用所述合力对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点”；调整后的采样点输出子模块，用于若所述第二判断结果表示否，则输出第i次调整后的采样点。
94.所述合势场构建子模块，具体包括：第三判断子模块，用于判断无人艇与障碍物的距离是否大于避障半径，获得第三判断结果；第一合势场构建单元，用于若所述第三判断结果表示是，则采用人工势场法构建子目标点产生的引力势场作为所述合势场；第二合势场构建单元，用于若所述第三判断结果表示否，则采用人工势场法构建子目标点产生的引力势场和障碍物产生的斥力势场的合势场。
95.所述采样点调整子模块，具体包括：采样点调整单元，用于根据所述合力，利用公式q
rs,i
＝q
rs,i-1
λ(f
total
/|f
total
|)，对第i-1次调整后的采样点进行调整，获得第i次调整后的采样点；其中，q
rs,i-1
和q
rs,i
分别表示第i-1次调整后和第i次调整后的采样点；f
total
表示合力，λ表示调整率。
96.实施例2
97.本发明实施例2提供一种无人艇编队在线航迹规划方法，如图2所示，包括如下步骤：
98.根据无人艇编队中每个成员艇的位置和感知域，确定虚拟领航者的位置和无人艇编队的感知域。
99.若无人艇编队未到达目标，主艇(无人艇编队中计算性能强的无人艇，负责局部航迹规划，无人艇编队中的成员为无人艇编队中的所有无人艇)，首先判断各无人艇编队成员的运行状态，以确定是否需要启动局部航迹规划循环，无人艇运行状态根据运动控制模块反馈的航迹点跟踪的稳定性和误差指标数据、紧急制动命令、网络报文调度状态、数据存储模块运行状态综合仲裁，各成员通过外部网络将状态数据发送给主艇。若无人艇编队运行良好，则主艇开启一轮局部航迹规划循环，否则，主艇发送紧急制动命令。局部航迹规划开始前，根据各成员传送来的北斗定位系统、惯导系统和陀螺仪获取无人艇的导航数据，对编队进行定位，确定编队中心点(虚拟领航者所在位置)的坐标、航向、姿态。
100.采用实施例1所述的无人艇在线航迹规划方法规划所述虚拟领航者的局部航迹。
101.步骤1，选取局部规划的子目标点。
102.根据各成员的避碰雷达和视觉系统反馈的实时感知数据确定当前无人艇编队可感知的环境范围，定义该范围为无人艇编队的感知域，局部子目标选取的原则是：若当前感知域与全局航迹有交点，在当前无人艇编队的感知域范围，距离当前虚拟领航者的局部航迹终点最远的全局航迹上的关键任务点，若当前感知域与全局航迹无交点，则在感知域边界上选择距离全局航迹最近的点作为局部在线规划的子目标点。
103.局部规划的时长是系统的超参数，根据局部规划器的效率、感知域半径以及无人
艇的控制需求制定，保证在给定时间内大部分局部规划都能搜索到联通局部目标点的可行子航迹，且能够满足无人艇对环境变化的及时响应。
104.步骤2，结合rrt*方法和人工势场法在当前感知域内对航迹进行一步扩展：
105.首先，根据导航模块反馈数据获取无人艇坐标、航向以及航行姿态。然后，在感知域内采用均匀分布的随机函数随机采样，根据避碰雷达和视觉系统反馈的无人艇与障碍物/动态船舶距离信息判断：若采样点距离障碍物大于无人艇与障碍物间的最小安全距离，则根基当前启发式采样次数阈值k，将采样点沿着合势场方向调整k步，其中，当无人艇距离障碍物大于避障半径时，采用人工势场法建立的合势场只考虑子目标产生的引力势场，引导规划器快速搜索到达子目标的航迹，当无人艇距离障碍物小于避障半径时，合势场考虑引力势场与斥力势场，若采样点与障碍物距离小于无人艇与障碍物间的最小安全距离，则调整失败，rrt*规划器引导航迹树向原始无碰的随机采样点扩展，否则，若调整k次后的采样点仍然与障碍物距离大于最小安全半径，则引导航迹树向调整后的采样点扩展。
106.若航迹扩展失败次数超过特定阈值，则向紧急制动模块报告局部规划失败的警报信息。
107.自适应地调整启发式调整阈值k，若航迹点调整失败，则k自减，系统认为当前规划范围距离障碍物较近，不适合使用势场调整，减少使用势场调整的次数；若调整成功，说明区域内势场能有效地引导航迹规划过程，此时，将调整次数阈值重新设为k。
108.采样点调整公式为：q
rs,i
＝q
rs,i-1
λ(f
total
/|f
total
|)，其中，q
rs,i
为调整i次后的采样点，q
rs,i-1
为第i次调整前的采样点，λ为调整率，f
total
为势场法计算的合力。
109.为了更有效利用雷达传感数据，提高在线航迹规划效率，提出图3所示的局部避碰势场建立方案。无人艇经常通过雷达等发射性传感器感知无人艇与障碍物表面的位置关系，进而实现避碰，因此，利用发射性雷达传感器数据特点，定义一种发射性传感器条件下的斥力场的计算方案，如图3所示，前置雷达发射一组水平射线，其中，r0，r1，r2，r
n-2
，r
n-1
，rn与障碍物表面接触，假设连续射线对应一个单独的障碍物，图示根据返回的射线数据，判断出当前无人艇面对两个独立的障碍物，根据射线方位及其返回的无人艇与障碍物距离数据，计算出对应的斥力，分别从两个障碍物对应的斥力中选择n个斥力代表障碍物对无人艇产生的斥力，合成后得到障碍物对无人艇的斥力。
110.这种方式可以直接从雷达返回的射线束中计算障碍物对无人艇的斥力，不需要额外的变换，因此使用更加方便，并且，计算过程中考虑无人艇与障碍物表面的对应关系，此种计算方式可以应用于不规则障碍物的躲避势场计算问题。
111.在规划过程中考虑无人艇的运动模型和约束，避免无人艇由于运动超限引起侧翻，采用差动推进式无人艇运动模型，为了提高规划效率，在航迹规划过程中只考虑三自由度的无人艇运动模型，不同坐标系下的速度转换公式如下：
[0112][0113]
其中，为世界坐标系下无人艇在x，y轴的速度以及无人艇的角速度，u，v，r为无人艇在艇坐标系下的线速度和角速度，usv是典型的欠驱动系统，其控制系统的输
入只是左右发动机产生的力和力矩，考虑左右发动机在纵荡方向推动，此时，输入力矩简化为t
port
和t
stdb
分别为左右舷的推力，b为两侧发动机间距，差动力矩产生艏摇。
[0114]
假设船体对称，将艇动力学模型简化为：
[0115][0116]mjj
是惯性矩阵对角线元素，du，dv，dr，d
ui
，d
vi
，d
ri
是纵荡，横荡，艏摇的水阻尼，与船体形状相关。系统的有效的输入力矩是纵荡力矩τu和艏摇力矩τr。附加质量参数反映了水压力和力矩，与加速度成比例，通过参数辨识和回归过程，估计的附加质量如下：
[0117][0118]
l是有效艇长，w是艇宽，d是平均入水深度，ρ是水密度。
[0119]
根据文献.，可以在仿真中令水阻尼系数为：
[0120][0121]
在每次扩展航迹树时考虑上述运动模型，在无人艇的位姿空间内找到接近采样点的可行航迹段。
[0122]
航迹最小转弯半径为r
min
＝4(u2 v2)/ω
max
，其中，ω
max
是无人艇最大角加速度，有了最小转弯半径约束后，无人艇在某一时刻的航迹可扩展区域为扇型区域。无人艇的非完整性约束考虑sinθ
·
dx-cosθ
·
dy＝0，其中，dx和dy是世界坐标系下无人艇在x轴和y轴方向上的增量。
[0123]
在障碍物周围定义环形斥力场如下：
[0124][0125][0126]
其中，ro为避碰半径，若目标艇与无人艇距离小于ro，则执行避碰动作，k(θ
io
)是新
增加的弹簧系数，用于提高避碰路径的光滑性。由此计算的斥力为：
[0127][0128][0129][0130][0131]
其中，n
io
为由障碍物指向无人艇的单位方向向量，n
io
方向的斥力分量将无人艇推离障碍物，用于静态避碰，而n
io
⊥
方向的斥力分量垂直于n
io
的方向，其作用是为无人艇提供转向力矩，使无人艇产生转向避碰运动。
[0132]
静态障碍物和动态艇数据通过雷达传感器以及视觉系统获得，根据障碍物数据为n
io
⊥
添加方向权值，为无人艇提供左右转向的斥力矩，以根据国际海事规则产生不同方向的转向运动。目标艇避碰势场定义规则如下：根据海事规则可以得到如下规律：无人艇应该向着增大|θs|的方位转向以躲避目标艇。由此可以定义n
io
⊥
的符号，以在势场定义中考虑海事规则。|θs|＝0或π是上述规则规则的奇异值，此时，向顺时针方向微调vo。
[0133]
步骤3，若航迹树扩展成功，则启动局部航迹优化模块：
[0134]
在新生成的航迹树节点的邻域范围进行航迹树的局部重连以优化邻域各节点的代价，用到达邻域节点的新的更优航迹取代原始航迹，在航迹树节点间代价函数定义时，考虑转弯角度、海流与航迹段夹角、欧氏距离等，以更加实际地考虑航迹改进的问题。将当前规划数据发送至数据存储模块。
[0135]
若局部航迹规划时间未用完，则返回步骤2“结合rrt*方法和人工势场法在当前感知域内对航迹进行一步扩展”。
[0136]
步骤4，当局部规划时间用完，则在感知域内查询接近子目标点的航迹段，并采用dubins曲线对其进行平滑，然后，将其作为编队成员多航迹规划的参考航迹。
[0137]
无人艇编队中的每个成员艇分别根据所述无人艇与虚拟领航者的相对位置和所述局部航迹，确定每个所述无人艇在下一时刻的期望位置。
[0138]
采用rrt*算法和所述人工势场法确定扩展每个所述无人艇的航迹至每个所述无人艇的期望位置。
[0139]
主艇将参考航迹信息通过网络发送给编队成员，各成员根据无人艇编队的协同数据(包括各无人艇的位置、航向、姿态)的融合，完成对自己定位并确定自己在编队中的相对位置，基于虚拟领航者框架，根据下一时刻虚拟领航者的位置和姿态，根据编队队形等确定各成员下一时刻的期待位置，并将其作为成员局部航迹规划的目标，规划编队成员的航迹，
为了避免规划过程陷入死锁，为成员航迹规划过程制定优先级，局部航迹规划系统将规划结果通过网络发送给运动控制模块，航迹执行过程中，通过避碰雷达反馈信息判断各成员间的碰撞态势，若成员间距离较近，则成员向主艇发送碰撞警报。
[0140]
若编队未抵达目标点，则重启局部在线规划过程。
[0141]
实施例3
[0142]
本发明实施例3一种无人艇编队在线航迹规划系统，如图4所示，其中，在线航迹规划模块是主要的模块，而其它模块对在线航迹规划模块起到辅助数据提供作用，在线航迹规划块是无人艇自主航行过程中主要运行的模块，该嵌入式程序作为独立的线程运行于艇载工控机，该艇载工控机设置在主艇上，具体包括：
[0143]
设置在无人艇编队中的每个成员艇上的导航模块、运动控制模块、实时感知模块、紧急制动模块和数据存储模块，设置在成员艇中的主艇上的在线航迹规划模块和网络报文调度模块。其中，导航模块主要负责确定无人艇的坐标、航向和姿态，这些数据通过北斗定位系统、惯导系统和陀螺仪获取；运动控制模块负责跟踪规划出的运动目标点，根据特定的运动控制算法，跟踪实时规划的航迹点，驱动无人艇驱动器，并将航迹点跟踪的稳定性和误差指标数据反馈回网络报文调度模块；实时感知模块负责感知无人艇附近的碍航物和动态船舶信息，数据主要来源于机器视觉系统和避碰雷达；紧急制动模块用于处理应急避障；数据存储模块负责管理无人艇的航行数据；网络报文调度程序以独立线程运行于艇载工控机中，负责传递各子模块信息。采用tcp/ip协议的socket编程方法对各模块进行网络互连，采用轮询的方式获取各个子模块的信息，各模块上的数字代表信息轮询的顺序，系统通过网络报文调度模块轮询各模块的信息，每秒钟轮询一次，模块回复的时限是0.2秒，若超时，则询问下一模块，若超时次数超过3次，则系统报错，同时启动应急模块。在线航迹规划模块每次规划一段局部航迹，可拆分成多个运动目标，每次以报文的形式上传一组运动目标点。
[0144]
各子模块报文通过不同的报文头相互区分，报文长度1024字节，不同类型的报文的标准格式预先定义，报文内容以结构体形式存储。
[0145]
重点考虑在线航迹规划模块，其负责规划无人艇编队的参考航迹，该模块以独立线程运行于艇载工控机上。
[0146]
在线航迹规划模块需要的输入信息是：无人艇的导航数据、控制状态、实时感知的环境数据、系统状态，其中，导航数据给出无人艇的实际坐标、方位等信息，在线规划模块通过导航数据对自身进行定位，而通过控制状态明确无人艇当前控制情况，实时提供的航迹点的跟踪情况，实时环境感知数据为无人艇提供当前环境的碍航物分布情况，系统状态信息明确当前无人艇操控处于手动模式、自动模式还是紧急制动模式，通过上述信息，无人艇在线规划航迹段，在线航迹规划模块的输出是局部航迹段，进一步解算为多个关键控制点和运动指令，通过网络报文调度模块发送给运动控制模块。
[0147]
该调度过程循环执行，直到无人艇到达任务点。
[0148]
局部环境势场定义、运动模型和约束条件、已知环境模型和全局任务数据(包括全局航迹)通过下载的方式，在任务开始前传入艇载工控机，作为在线航迹规划的已知条件。
[0149]
本发明针对无人艇在非结构化海洋环境下的自主航行问题，提出一种在线航迹规划系统实现方案，考虑了在线航迹规划模块与无人艇自主航行的相关模块之间的数据流和控制流。
[0150]
为了提高在线航迹规划效率和航迹的可行性，采用人工势场引导随机航迹规划器的执行过程，为了更真实地反映无人艇躲避障碍物或动态艇的过程，在势场中考虑了无人艇的速度、国际海事规则等；提出了不规则障碍物情况下斥力场的计算方法，基于雷达射线计算斥力；随机化航迹规划器根据势场通过自适应地调整采样点，提高航迹搜索速度，综合考虑了环境因素和海流干扰因素，规划出的航迹能耗更小。
[0151]
为了同时规划编队成员的多条航迹，根据编队中心参考航迹、编队队形和协同约束条件，依次在线计算出无人艇编队成员的可行航迹。
[0152]
本发明通过随机化航迹规划器根据势场通过自适应地调整采样点，提高航迹搜索速度；基于发射性雷达数据计算斥力，而无需复杂的坐标变换过程；根据编队中心参考航迹、编队队形和协同约束条件，依次分布式地在线计算出无人艇编队成员的可行航迹，简化了复杂的协同航迹计算过程。
[0153]
为了更真实地反映无人艇躲避障碍物或动态艇的过程，在势场中考虑了无人艇的速度等，势场更加精确，随机规划器综合考虑了环境因素和海流干扰因素，规划出的航迹能耗更小。
[0154]
本说明书中各个实施例采用递进的方式描述，每个实施例重点说明的都是与其他实施例的不同之处，各个实施例之间相同相似部分互相参见即可。
[0155]
本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处。综上所述，本说明书内容不应理解为对本发明的限制。