【技术实现步骤摘要】
本专利技术涉及无人船领域，尤其涉及一种带避障功能的无人船测绘航线控制方法及系统。
技术介绍
1、无人船进行水上水下断面测量测绘相比有人船，效率高、安全性高及成本低的优点，通常无人船会搭载测量测绘设备、高精度定位rtk及毫米波雷达等。
2、无人船进行水上水下断面测量的时候，航线规划是无人船水上水下地形测绘的首要步骤。合理的航线规划不仅可以提高测绘效率，还能确保测绘数据的准确性和完整性。
3、在执行航线任务的时候有两个重要的功能需要满足，一是无人船的沿航线航行的精确控制，二是出现碰撞安全问题的及时干预处理。现有的无人船控制在该方面能力较弱，有的无人船控制航线时在较大风浪下会有航线入轨慢、控制偏移等情况，有的无人船没有安全防碰撞功能。如何通过科学规划，精准控制无人船；如何让无人船可以避开复杂地形和潜在危险区域，按照预定路径准确航行并实时记录测绘数据是重要的研究内容。
技术实现思路
1、本专利技术的主要目的在于解决现有技术中人船的沿航线航行的精确控制的技术问题。一种带避障功能的无人船测绘航线控制方法，包括以下步骤：
2、在gis地图上绘制航线，所述航线是由多个点的连线形成；
3、根据规划的航线，计算出所述航线的外接矩形区域，将外接矩形区域划分为固定大小的网格；
4、检测并获得障碍物在网格中的位置，将障碍物所在的网格点标注为障碍物；
5、根据障碍物在网格中的位置，采用a星算法绕开障碍物到后续航线上。
6、作为
7、获取障碍物到船的侧向距离x1，前向距离y1,姿态角度θ1；
8、根据以下公式计算东西偏移距离xx，南北偏移距离yy；
9、xx＝len*cos(θ)
10、yy＝len*sin(θ)
11、其中：
12、
13、根据船的经纬度计算出障碍物的经纬度；
14、将障碍物的经纬度对应到网格上，把网格点标注为障碍物。
15、作为优先的技术方案，根据障碍物在网格中的位置，采用a星算法绕开障碍物到后续航线上，包括：
16、每个网格单元定义状态：0：可通行。
17、1：存在障碍物，不可通行。
18、输入参数：
19、起点s(xs,ys)：无人船当前位置；
20、终点g(xg,yg)：后续航线的最近目标点；
21、邻接节点定义：
22、采用8邻域方式，每个节点最多有8个邻居。
23、节点间移动的代价：
24、水平或垂直移动：代价为1；
25、对角线移动：代价为根号2；
26、初始化：
27、创建两个列表：
28、开放列表：存储待处理的节点，初始包含起点s；
29、关闭列表：存储已处理过的节点，初始为空；
30、定义起点的代价值：
31、g(s)＝0：起点到起点的实际代价为0；
32、h(s)＝euclideandistance(s,g)：起点到终点的启发式估计值；
33、f(s)＝g(s)+h(s)：总代价值；
34、搜索过程：
35、重复以下步骤直到找到路径或开放列表为空：
36、选择当前节点：
37、从开放列表中选取总代价值f＝g+h最小的节点，设为当前节点c(xc,yc)；
38、将c从开放列表移入关闭列表；
39、终止条件：
40、如果当前节点为终点节点g，结束搜索，返回路径；
41、生成邻居节点：
42、找到当前节点c的所有可通行邻居节点，即未在关闭列表，且未被标记为障碍物；
43、对每个邻居节点n(xn,yn)：
44、计算实际代价g(n)＝g(c)+cost(c,n)；
45、计算启发式估计值h(n)＝euclideandistance(n,g)；
46、总代价值f(n)＝g(n)+h(n)；
47、更新开放列表：
48、如果邻居节点n不在开放列表中：
49、将n加入开放列表，记录其父节点为c。
50、如果n已在开放列表中且新的g(n)值更小：
51、更新n的父节点为c，并更新其g值和f值；
52、路径重建：
53、当搜索到终点g时，从终点回溯其父节点，直至起点，生成路径，从而绕开障碍物到后续航线上。
54、作为优先的技术方案，所述方法还包括航线控制步骤，包括：
55、将航线拆分为多个线段，每段均包括控制起点和控制终点；
56、实时计算期望角度α和船只当前角度测量角度β，通过pid控制器，进行输出实际运行角度，实际运行角度angle的计算公式为：
57、angle＝u(t)+β
58、其中：u(t)＝kp*e(t)+ki*∫e(t)dt+kd*d(e)/d(t)
59、其中：t为时间；u(t)为控制器的输出角度误差；kp、ki、kd分别为比例、积分、微分系数；e(t)为系统误差：e(t)＝α-β；
60、设定的巡航速度，计算出x方向控制速度vx以及y方向控制速度vy后，发送到无人船引擎控制单元，计算公式为：
61、vx＝v*cos(angle)
62、vy＝v*sin(angle)
63、引擎控制单元根据控制参数控制马达运行。
64、作为优先的技术方案，所述方法还包括惯性影响控制，包括：
65、根据船只距离起点/终点的距离d，动态调整速度：
66、d<rstart：匀加速至巡航速度；
67、d>rend：匀减速以确保精确到达目标点；
68、加速阶段：v＝vstart+a·t,0<t<tacc
69、减速阶段：v＝vend-a·t,0<t<tdec
70、其中a为加速度，tacc和tdec为加速和减速时间；
71、在拐弯点提前进入减速状态，结合内切路径调整算法实现平滑转弯，避免偏航，船的速度valive的计算公式为：
72、
73、其中，sdist为船到起点或结束点的距离，radius为加速、减速作业半径；vstand为标准设定速度。
74、本专利技术第二方面提供了一种带避障功能的无人船测绘航线控制系统，包括：
75、航线绘制单元，在gis地图上绘制航线，所述航线是由多个点的连线形成；
76、网格绘制单元，根据规划的航线，计算出所述航线的外接矩形区域，将外接矩形区域划分为固定大小的网格；
【技术保护点】
1.一种带避障功能的无人船测绘航线控制方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种带避障功能的无人船测绘航线控制方法，其特征在于，所述检测并获得障碍物在网格中的位置，将障碍物所在的网格点标注为障碍物，包括：
3.根据权利要求1所述的一种带避障功能的无人船测绘航线控制方法，其特征在于，所述根据障碍物在网格中的位置，采用A星算法绕开障碍物到后续航线上，包括：
4.根据权利要求1所述的一种带避障功能的无人船测绘航线控制方法，其特征在于，所述方法还包括航线控制步骤，包括：
5.根据权利要求1所述的一种带避障功能的无人船测绘航线控制方法，其特征在于，所述方法还包括惯性影响控制，包括：
6.一种带避障功能的无人船测绘航线控制系统，其特征在于，所述系统包括：
7.一种电子设备，所述电子设备包括存储器和至少一个处理器，所述存储器中存储有指令；
8.一种计算机可读存储介质，所述计算机可读存储介质上存储有指令，其特征在于，所述指令被处理器执行时实现如权利要求1-5中任一项所述带避障功能的无人船测绘航
...【技术特征摘要】
1.一种带避障功能的无人船测绘航线控制方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种带避障功能的无人船测绘航线控制方法，其特征在于，所述检测并获得障碍物在网格中的位置，将障碍物所在的网格点标注为障碍物，包括：
3.根据权利要求1所述的一种带避障功能的无人船测绘航线控制方法，其特征在于，所述根据障碍物在网格中的位置，采用a星算法绕开障碍物到后续航线上，包括：
4.根据权利要求1所述的一种带避障功能的无人船测绘航线控制方法，其特征在于，所述方法还包括航线控...
【专利技术属性】
技术研发人员：刘黎明，王得举，郑吕，
申请(专利权)人：上海普适导航科技股份有限公司，
类型：发明
国别省市：
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。