CN112797987B - 一种无人船避障的航行方法、装置、计算机设备及存储介质 - Google Patents

一种无人船避障的航行方法、装置、计算机设备及存储介质 Download PDF

Info

Publication number
CN112797987B
CN112797987B CN202110304729.8A CN202110304729A CN112797987B CN 112797987 B CN112797987 B CN 112797987B CN 202110304729 A CN202110304729 A CN 202110304729A CN 112797987 B CN112797987 B CN 112797987B
Authority
CN
China
Prior art keywords
unmanned ship
obstacle
point
radar
path
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110304729.8A
Other languages
English (en)
Other versions
CN112797987A (zh
Inventor
程宇威
朱健楠
庞长松
池雨豪
虞梦苓
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shaanxi Orca Electronic Intelligent Technology Co ltd
Original Assignee
Shaanxi Orca Electronic Intelligent Technology Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shaanxi Orca Electronic Intelligent Technology Co ltd filed Critical Shaanxi Orca Electronic Intelligent Technology Co ltd
Priority to CN202110304729.8A priority Critical patent/CN112797987B/zh
Publication of CN112797987A publication Critical patent/CN112797987A/zh
Application granted granted Critical
Publication of CN112797987B publication Critical patent/CN112797987B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/20—Instruments for performing navigational calculations
    • G01C21/203—Specially adapted for sailing ships
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/0206—Control of position or course in two dimensions specially adapted to water vehicles

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Navigation (AREA)
  • Traffic Control Systems (AREA)

Abstract

本发明公开了一种无人船避障的航行方法、装置、计算机设备及存储介质，无人船避障的航行方法包括以下步骤：步骤一、获取无人船航行路线
Figure 782955DEST_PATH_IMAGE001
；步骤二、对所述航行路线
Figure 168937DEST_PATH_IMAGE001
进行预处理，得到预处理路线
Figure 604467DEST_PATH_IMAGE002
；步骤三、控制无人船沿所述预处理路线
Figure 202938DEST_PATH_IMAGE003
进行自主航行，并采集无人船当前位置的位置坐标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地图
Figure 486152DEST_PATH_IMAGE004
;步骤四、根据所述实时地图
Figure 26724DEST_PATH_IMAGE004
，实时计算无人船未来路线上的障碍分布
Figure 762599DEST_PATH_IMAGE005
；步骤五、根据所述障碍分布
Figure 227078DEST_PATH_IMAGE005
，判断无人船未来路线是否穿过障碍；步骤六、若无人船未来路线穿过障碍，则计算绕行障碍的目标点
Figure 364798DEST_PATH_IMAGE006
；通过本方法可绕开障碍继续沿原路线航行，精确避障并降低计算量。

Description

一种无人船避障的航行方法、装置、计算机设备及存储介质
技术领域
本发明涉及无人船自主航行技术领域，更具体地说是指一种无人船避障的航行方法、装置、计算机设备及存储介质。
背景技术
近年来，随着无人驾驶技术的迅猛发展，无人驾驶市场不断增长，以无人船为例，各类无人船逐渐被推广至水质检测、水面维护、桥梁检测、水体测绘、海事搜救等多种作业场景下，辅助人们工作。感知是无人驾驶技术的基础,在复杂的水面环境下,无人船需要具备灵敏的环境感知能力。由于现有的无人船环境感知能力有限，通常需要人工操作辅助其作业任务，无法实现全天候完全无人自主作业，难以大范围推广与应用。作为自主执行任务中必不可少的一部分，良好的自主避障能力有助于提升无人船的自主性，更好地执行任务。
现有的启发式的路径规划算法，一般需要采集并处理尽可能多的环境数据，进行大量数据计算，由于计算量较大，因此，对计算硬件性能要求较高，导致价格高昂。同时，现有避障方法主要解决的是点到点的避障功能，但是现实生活中无人驾驶系统往往需要按照规定路径行驶。因此，我们要求无人驾驶装置除了能够精确避障的同时降低计算量，还要具备能够按照规定路径行走的能力。因此，低计算量的高精度实时避障方法具有十分重要的现实意义。
发明内容
为了解决上述现有技术的缺陷，本发明的目的是提供一种无人船避障的航行方法、装置、计算机设备及存储介质。
为达到上述目的，本发明的技术方案是：一种无人船避障的航行方法，包括以下步骤：
步骤一、获取无人船航行路线 A；
步骤二、对所述航行路线 A 进行预处理，得到预处理路线 A＇；
步骤三、控制无人船沿所述预处理路线A＇进行自主航行，并采集无人船当前位置 的位置坐标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地图
Figure 240159DEST_PATH_IMAGE001
;
步骤四、根据所述实时地图
Figure 146935DEST_PATH_IMAGE001
，实时计算无人船未来路线上的障碍分布
Figure 301973DEST_PATH_IMAGE002
；
步骤五、根据所述障碍分布
Figure 227073DEST_PATH_IMAGE002
，判断无人船未来路线是否穿过障碍；
步骤六、若无人船未来路线穿过障碍，则计算绕行障碍的目标点
Figure 74943DEST_PATH_IMAGE003
；
步骤七、根据无人船实时位置
Figure 469015DEST_PATH_IMAGE004
、所述实时地图
Figure 162165DEST_PATH_IMAGE001
和所述目标点
Figure 958082DEST_PATH_IMAGE005
，计算障碍 绕行方向 dire；
步骤八、控制无人船沿所述障碍绕行方向 dire 绕行，并实时计算当前路径距离 无人船的最近点
Figure 694963DEST_PATH_IMAGE006
，根据所述最近点
Figure 576331DEST_PATH_IMAGE006
计算障碍分布
Figure 73172DEST_PATH_IMAGE007
，更新避障的目标点
Figure 989175DEST_PATH_IMAGE008
， 将障碍分布
Figure 162537DEST_PATH_IMAGE009
替换为障碍分布
Figure 796780DEST_PATH_IMAGE002
，并重复执行步骤五至步骤八，直到无人船到达目标点为 止。
其进一步技术方案为：所述对所述航行路线A进行预处理，得到预处理路线 A＇的步骤，包括：
设定航行路线 A 两点间距小于
Figure 831732DEST_PATH_IMAGE010
，从航行路线A的第一个点开始遍历，对相邻两 个路径点间距
Figure 602242DEST_PATH_IMAGE011
大于
Figure 697237DEST_PATH_IMAGE012
时,插入间距为
Figure 536886DEST_PATH_IMAGE013
的点队列；对相邻两个路径点间距
Figure 375529DEST_PATH_IMAGE014
小于
Figure 266125DEST_PATH_IMAGE012
时，在中点插入一个点，完成对航行路线 A 的预处理，得到预处理路线 A＇。
其进一步技术方案为：所述控制无人船沿所述预处理路线 A＇进行自主航行的步骤，包括：
控制无人船前往第一个路径点 r1，到达第一个路径点后更新无人船当前路径点索引index，随后无人船根据控制系统指令向路径点索引为index+1 的位置运动，到达路径点索引为index+1 的位置后，更新路径点索引并根据控制系统指令继续向路径点索引为index+2的位置运动，重复此过程直至无人船到达所述预处理路线 A＇终点，此时自主航行结束。
其进一步技术方案为：所述采集无人船当前位置的位置坐标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地图 M 的步骤，包括：
以无人船初始位置为原点，正北方向为
Figure 532021DEST_PATH_IMAGE015
轴正方向，正东方向为
Figure 875278DEST_PATH_IMAGE016
轴正方向，建立 世界坐标系，形成环境栅格地图B；
基于无人船当前位置采集环境雷达点云 P，并以无人船船艏朝向为
Figure 501300DEST_PATH_IMAGE017
轴正方向，无 人船船艏右侧方向为 x 轴正方向，建立平面直角坐标系，作为雷达坐标系；
对时间上相邻的5包雷达点云
Figure 246402DEST_PATH_IMAGE018
根据相应时刻的坐标进行方向角配准，并将5包 雷达点云合并为1包，形成雷达点云 P＇，其中
Figure 683200DEST_PATH_IMAGE019
代表第
Figure 513752DEST_PATH_IMAGE019
包雷达点云;
对合并的雷达点云 P＇进行滤波，得到滤波后的雷达点云 P＇＇；
计算雷达坐标系与世界坐标系间的旋转矩阵 Rot 和平移矩阵 Trans，根据所述 旋转矩阵 Rot 和所述平移矩阵 Trans 计算滤波后的雷达点云 P＇＇在世界坐标系中的位 置，并在环境栅格地图 B 标记，使环境栅格地图 B 中包含当前相应雷达的障碍，得到实时 地图
Figure 694198DEST_PATH_IMAGE020
。
其进一步技术方案为：所述对时间上相邻的5包雷达点云
Figure 559386DEST_PATH_IMAGE018
根据相应时刻的坐标 进行方向角配准，并将5包雷达点云合并为1包，形成雷达点云 P＇，其中
Figure 416352DEST_PATH_IMAGE019
代表第
Figure 203043DEST_PATH_IMAGE019
包雷达 点云的步骤，包括：
以最后一包雷达点云时刻的坐标为对准点，方向角为对准方向，分别计算前4包雷 达点云相对最后一包雷达点云的平移向量
Figure 452758DEST_PATH_IMAGE021
和偏转角度
Figure 906873DEST_PATH_IMAGE022
，其中
Figure 951053DEST_PATH_IMAGE023
正方向为逆时针；
利用偏转角度
Figure 474307DEST_PATH_IMAGE023
计算每一包雷达点云投影到最后一包雷达点云所在坐标系的旋 转矩阵
Figure 262134DEST_PATH_IMAGE024
，平移向量
Figure 570756DEST_PATH_IMAGE025
，
Figure 785837DEST_PATH_IMAGE026
；
根据所述旋转矩阵
Figure 812698DEST_PATH_IMAGE027
和所述平移向量
Figure 122326DEST_PATH_IMAGE025
计算每一包雷达点云相应最后一包 雷达点云投影后的雷达点云
Figure 816612DEST_PATH_IMAGE028
，然后合并形成雷达点云
Figure 937015DEST_PATH_IMAGE029
，其中，计算投影雷达点云
Figure 451173DEST_PATH_IMAGE030
，雷达点云
Figure 315224DEST_PATH_IMAGE031
。
其进一步技术方案为：所述对合并的雷达点云
Figure 847705DEST_PATH_IMAGE032
进行滤波，得到滤波后的雷达点云
Figure 404589DEST_PATH_IMAGE033
的步骤，包括：
对雷达点云
Figure 140463DEST_PATH_IMAGE032
使用直通滤波滤除近处由于无人船本身运动带来水花产生的水杂 波，得到雷达点云
Figure 73784DEST_PATH_IMAGE034
，
Figure 477084DEST_PATH_IMAGE035
其中,
Figure 188557DEST_PATH_IMAGE036
代表雷达点云在雷达坐标系下的
Figure 411728DEST_PATH_IMAGE037
轴，
Figure 148740DEST_PATH_IMAGE017
轴的距离，
Figure 140966DEST_PATH_IMAGE038
代表雷达点云与雷 达坐标系下原点之间的连线与
Figure 39652DEST_PATH_IMAGE037
轴组成的夹角，
Figure 999387DEST_PATH_IMAGE039
分别为相应物理量的阈值；
设置雷达点云信噪比阈值
Figure 274510DEST_PATH_IMAGE040
，在雷达点云
Figure 121244DEST_PATH_IMAGE041
中滤除低信噪比的雷达点云，得到雷 达点云
Figure 456410DEST_PATH_IMAGE042
，
Figure 654173DEST_PATH_IMAGE043
；
使用
Figure 467408DEST_PATH_IMAGE044
聚类的方法滤除雷达点云
Figure 417916DEST_PATH_IMAGE045
中的离群点，得到雷达点云簇
Figure 658404DEST_PATH_IMAGE046
，
Figure 343463DEST_PATH_IMAGE047
；
对雷达点云簇
Figure 960390DEST_PATH_IMAGE048
中的每个雷达点云簇进行跟踪，将无法跟踪的雷达点云簇使用卡 尔曼滤波的方法滤除，得到雷达点云簇
Figure 781715DEST_PATH_IMAGE049
，其中
Figure 707952DEST_PATH_IMAGE050
，并将雷达点云簇
Figure 614728DEST_PATH_IMAGE051
中的 所有雷达点云合并，得到最终滤波后的雷达点云
Figure 35345DEST_PATH_IMAGE052
。
其进一步技术方案为：所述根据所述实时地图 M，实时计算无人船未来路线上的障碍分布 D 的步骤，包括：
无人船以路径索引
Figure 711177DEST_PATH_IMAGE053
查找当前无人船航行路径的路径点
Figure 559047DEST_PATH_IMAGE054
, 将路径点
Figure 202387DEST_PATH_IMAGE055
作 为无人船当前位于路径上的点；
计算点
Figure 895536DEST_PATH_IMAGE054
沿路径前进方向一定距离内的所有点，形成局部点队列
Figure 957033DEST_PATH_IMAGE056
；
计算局部点队列
Figure 444646DEST_PATH_IMAGE056
中每个路径点
Figure 326015DEST_PATH_IMAGE057
在实时地图M中的坐标 C；
以每个路径点
Figure 337702DEST_PATH_IMAGE058
为实时地图 M 中的局部中心点，裁取相应地图，形成地图队列
Figure 253705DEST_PATH_IMAGE059
；
记第i个路径点的地图队列
Figure 177799DEST_PATH_IMAGE060
中障碍栅格的数量 ni，设置障碍所占栅格阈值为 λ,当 ni >λ 时，路径点 i 判定为障碍点并记为 G，否则记为 F，局部点队列 R' 中的所有 障碍点信息构成障碍分布 D。
其进一步技术方案为：所述根据所述障碍分布 D，判断无人船未来路线是否穿过障碍的步骤，包括：
对路径点障碍分布
Figure 546464DEST_PATH_IMAGE061
进行滑动，得到滑动窗口
Figure 581416DEST_PATH_IMAGE062
；
当所述 G 的数量大于
Figure 617505DEST_PATH_IMAGE063
时，则判定此时路径上存在障碍，并记录窗口第一个点的 索引
Figure 961767DEST_PATH_IMAGE064
。
其进一步技术方案为：所述若无人船未来路线穿过障碍，则计算绕行障碍的目标 点
Figure 817728DEST_PATH_IMAGE065
的步骤，包括：
从索引
Figure 656371DEST_PATH_IMAGE066
处继续滑动路径点障碍分布
Figure 546966DEST_PATH_IMAGE061
，得到滑动窗口
Figure 812863DEST_PATH_IMAGE067
；
当所述 F 的数量大于
Figure 405387DEST_PATH_IMAGE068
时，记录窗口所在的最后一个路径点，将该路径点作为绕 行障碍的目标点
Figure 782142DEST_PATH_IMAGE069
。
其进一步技术方案为：所述根据无人船实时位置
Figure 527244DEST_PATH_IMAGE070
、所述实时地图
Figure 964041DEST_PATH_IMAGE020
和所述目标 点
Figure 794594DEST_PATH_IMAGE071
，计算障碍绕行方向 dire 的步骤，包括：
计算无人船实时位置在实时地图M中的坐标
Figure 224307DEST_PATH_IMAGE072
, 以此坐标 Coord作为中心点， 截取相应栅格地图
Figure 823916DEST_PATH_IMAGE073
；
从所述栅格地图
Figure 431615DEST_PATH_IMAGE073
中筛选出所有的障碍点
Figure 749464DEST_PATH_IMAGE074
，对障碍点进行相关滤波处理，得到
Figure 733600DEST_PATH_IMAGE075
；
以正东方向为0度方向，顺时针旋转，均分为n个扇区；
将障碍点
Figure 171404DEST_PATH_IMAGE076
投影到所有的扇区中，分别计算每个扇区对于无人船航行的危险程度
Figure 215583DEST_PATH_IMAGE077
，计算
Figure 20728DEST_PATH_IMAGE077
的方式：
Figure 542976DEST_PATH_IMAGE078
其中
Figure 117177DEST_PATH_IMAGE079
为每个扇区内障碍点到无人船的最小距离，
Figure 66678DEST_PATH_IMAGE080
为无人船的探测距离；
依据不同扇区的 T 值，选取绕行危险最低，速度最快的扇区
Figure 342808DEST_PATH_IMAGE081
作为障碍绕行扇区；
将所述障碍绕行扇区的中心方向角度作为障碍绕行方向
Figure 58960DEST_PATH_IMAGE082
。
其进一步技术方案为：所述依据不同扇区的 T 值，选取绕行危险最低，速度最快 的扇区
Figure 753246DEST_PATH_IMAGE083
作为障碍绕行扇区的步骤，包括：
计算目标位置所在的目标扇区；
设定威胁阈值
Figure 670387DEST_PATH_IMAGE084
，筛选威胁
Figure 387807DEST_PATH_IMAGE085
的扇区，视为安全扇区，
Figure 783016DEST_PATH_IMAGE086
的扇区视为危险扇区
Figure 784339DEST_PATH_IMAGE087
滤除所述危险扇区以及危险扇区相邻的安全扇区，得到保留扇区；
在所述保留扇区中选取距离所述目标扇区最近的扇区 c 作为障碍绕行扇区。
其进一步技术方案为：所述控制无人船沿所述障碍绕行方向dire绕行，并实时计 算当前路径距离无人船的最近点
Figure 606802DEST_PATH_IMAGE088
，根据所述最近点
Figure 873835DEST_PATH_IMAGE088
计算障碍分布
Figure 275998DEST_PATH_IMAGE089
，更新避障 的目标点
Figure 944876DEST_PATH_IMAGE090
，将障碍分布
Figure 859612DEST_PATH_IMAGE091
替换为障碍分布
Figure 613941DEST_PATH_IMAGE092
的步骤，包括：
以当前路径点索引
Figure 350953DEST_PATH_IMAGE093
为标志，取距离路径点路径反方向一定距离的路径点
Figure 874338DEST_PATH_IMAGE094
到路径正方向一定距离的路径点
Figure 304182DEST_PATH_IMAGE095
之间的所有点形成局部点队列
Figure 280229DEST_PATH_IMAGE096
，此处
Figure 24194DEST_PATH_IMAGE097
代表距离第
Figure 402085DEST_PATH_IMAGE053
个路径点
Figure 206093DEST_PATH_IMAGE098
米的最近的路径点，其中路径方向为正方向；
计算
Figure 489062DEST_PATH_IMAGE096
中的每个点到无人船实时位置的距离
Figure 302297DEST_PATH_IMAGE099
，
Figure 534695DEST_PATH_IMAGE100
为局部点队列 中第
Figure 775184DEST_PATH_IMAGE101
个点到无人船实时位置的距离；
根据
Figure 460243DEST_PATH_IMAGE102
计算距离无人船实时位置最近点的索引
Figure 60857DEST_PATH_IMAGE103
，将索引
Figure 147762DEST_PATH_IMAGE104
作为当前点的索引；
以索引
Figure 559152DEST_PATH_IMAGE104
代表的路径点计算当前路径的障碍分布
Figure 731507DEST_PATH_IMAGE091
，将障碍分布D' 替换为障 碍分布 D。
本发明还提出一种无人船自主避障的航行装置，包括：
获取单元，用于获取无人船航行路线 A；
预处理单元，用于对所述航行路线 A 进行预处理，得到预处理路线 A＇；
采集单元，用于控制无人船沿所述预处理路线A＇进行自主航行，并采集无人船当 前位置的位置坐标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地 图
Figure 620966DEST_PATH_IMAGE020
;
第一计算单元，用于根据所述实时地图
Figure 811644DEST_PATH_IMAGE020
，实时计算无人船未来路线上的障碍分布
Figure 393936DEST_PATH_IMAGE105
；
判断单元，用于根据所述障碍分布
Figure 53587DEST_PATH_IMAGE105
，判断无人船未来路线是否穿过障碍；
第二计算单元，用于若无人船未来路线穿过障碍，则计算绕行障碍的目标点
Figure 12316DEST_PATH_IMAGE106
；
第三计算单元，用于根据无人船实时位置
Figure 542654DEST_PATH_IMAGE107
、所述实时地图
Figure 545114DEST_PATH_IMAGE020
和所述目标点
Figure 692062DEST_PATH_IMAGE071
，计算障碍绕行方向 dire；
更新单元，用于控制无人船沿所述障碍绕行方向 dire 绕行，并实时计算当前路 径距离无人船的最近点
Figure 188902DEST_PATH_IMAGE088
，根据所述最近点
Figure 104906DEST_PATH_IMAGE088
计算障碍分布
Figure 763420DEST_PATH_IMAGE089
，更新避障的目标点
Figure 381352DEST_PATH_IMAGE090
，将障碍分布
Figure 681883DEST_PATH_IMAGE091
替换为障碍分布
Figure 452393DEST_PATH_IMAGE105
，并重复执行判断单元、第二计算单元、第三计算单 元和更新单元，直到无人船到达目标点为止。
本发明还提出一种计算机设备，所述计算机设备包括存储器及处理器，所述存储器上存储有计算机程序，所述处理器执行所述计算机程序时实现如上所述的无人船避障的航行方法。
本发明还提出一种存储介质，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时可实现如上所述的无人船避障的航行方法。
本发明与现有技术相比的有益效果是：本发明一种无人船避障的航行方法通过实 时计算无人船未来路线上的障碍分布
Figure 547388DEST_PATH_IMAGE108
，并根据障碍分布
Figure 403349DEST_PATH_IMAGE108
，判断无人船未来路线是否穿过 障碍，若穿过障碍，则更新绕行障碍的目标点，直到无人船到达目标点为止。通过本方法当 无人船航行路线上出现障碍时，无人船可自主规划路线，绕开障碍继续沿原路线航行，实现 精确避障的同时降低计算量，降低对计算硬件性能要求，降低成本，具备能够按照规定路径 行走的能力。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明技术手段，可依照说明书的内容予以实施，并且为了让本发明的上述说明和其它目的、特征及优点能够更明显易懂，特举较佳实施例，详细说明如下。
附图说明
图1为本发明实施例提供的无人船避障的航行方法的应用场景示意图；
图2为本发明实施例提供的一种无人船避障的航行方法的流程示意图；
图3为本发明实施例提供的一种无人船避障的航行方法的子流程示意图；
图4为本发明实施例提供的一种无人船避障的航行方法的子流程示意图；
图5为本发明实施例提供的一种无人船避障的航行装置的示意性框图；
图6为本发明实施例提供的一种无人船避障的航行装置的预处理单元的示意性框图；
图7为本发明实施例提供的一种无人船避障的航行装置的采集单元的示意性框图；
图8为本发明另一实施例提供的一种无人船避障的航行装置的采集单元的示意性框图；
图9为本发明实施例提供的一种无人船避障的航行装置的雷达点云模块的示意性框图；
图10为本发明实施例提供的一种无人船避障的航行装置的雷达点云滤波模块的示意性框图；
图11为本发明实施例提供的一种无人船避障的航行装置的第一计算单元的示意性框图；
图12为本发明实施例提供的一种无人船避障的航行装置的判断单元的示意性框图；
图13为本发明实施例提供的一种无人船避障的航行装置的第二计算单元的示意性框图；
图14为本发明实施例提供的一种无人船避障的航行装置的第三计算单元的示意性框图；
图15为本发明实施例提供的一种无人船避障的航行装置的选取模块的示意性框图；
图16为本发明实施例提供的一种无人船避障的航行装置的更新单元的示意性框图；
图17为本发明实施例提供的计算机设备的示意性框图。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，下面结合附图和具体实施方式对本发明作进一步详细说明。
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和 “包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/ 或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
请参阅图1和图2，图1为本发明实施例提供的无人船避障的航行方法的应用场景示意图。图2为本发明实施例提供的无人船避障的航行方法的示意性流程图。该无人船避障的航行方法应用于服务器中，该服务器与终端和无人船进行数据交互，由终端输入无人船的航行路线并传输至服务器，服务器获取无人船的航行路线并进行计算和处理，最终输出执行信息，可将该执行信息传输至无人船做出对应的响应。
图2为本发明实施例提供的一种无人船避障的航行方法的流程示意图；如图2所示，该方法包括以下步骤S10至S80。
S10、获取无人船航行路线 A。
S20、对航行路线 A 进行预处理，得到预处理路线 A＇。
S30、控制无人船沿预处理路线A＇进行自主航行，并采集无人船当前位置的位置坐 标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地图
Figure 491259DEST_PATH_IMAGE020
。
S40、根据实时地图
Figure 116276DEST_PATH_IMAGE020
，实时计算无人船未来路线上的障碍分布
Figure 319855DEST_PATH_IMAGE105
。
S50、根据障碍分布
Figure 646800DEST_PATH_IMAGE105
，判断无人船未来路线是否穿过障碍。
S60、若无人船未来路线穿过障碍，则计算绕行障碍的目标点
Figure 757976DEST_PATH_IMAGE106
。
S70、根据无人船实时位置
Figure 237498DEST_PATH_IMAGE107
、实时地图
Figure 189143DEST_PATH_IMAGE020
和目标点
Figure 754116DEST_PATH_IMAGE071
，计算障碍绕行方向 dire。
S80、控制无人船沿障碍绕行方向 dire 绕行，并实时计算当前路径距离无人船的 最近点
Figure 200141DEST_PATH_IMAGE088
，根据最近点
Figure 799750DEST_PATH_IMAGE088
计算障碍分布
Figure 407449DEST_PATH_IMAGE089
，更新避障的目标点
Figure 725298DEST_PATH_IMAGE090
，将障碍分布
Figure 427543DEST_PATH_IMAGE091
替换为障碍分布
Figure 147237DEST_PATH_IMAGE105
，并重复执行步骤S50至步骤S80，直到无人船到达目标点为止。
在本实施例中，实时计算无人船未来路线上的障碍分布
Figure 925838DEST_PATH_IMAGE108
，并根据障碍分布
Figure 730983DEST_PATH_IMAGE109
，判 断无人船未来路线是否穿过障碍，若穿过障碍，则更新绕行障碍的目标点，直到无人船到达 目标点为止。通过本方法当无人船航行路线上出现障碍时，无人船可自主规划路线，绕开障 碍继续沿原路线航行，实现精确避障的同时降低计算量，降低对计算硬件性能要求，降低成 本，具备能够按照规定路径行走的能力。
在一实施例中，步骤S20包括以下步骤；
设定航行路线 A 两点间距小于
Figure 502498DEST_PATH_IMAGE110
，从航行路线A的第一个点开始遍历，对相邻两 个路径点间距
Figure 76699DEST_PATH_IMAGE111
大于
Figure 291780DEST_PATH_IMAGE112
时,插入间距为
Figure 53062DEST_PATH_IMAGE113
的点队列；对相邻两个路径点间距
Figure 644581DEST_PATH_IMAGE111
小于
Figure 807709DEST_PATH_IMAGE112
时，在中点插入一个点，完成对航行路线 A 的预处理，得到预处理路线 A＇。
在本实施例中，通过插值法对航行路线 A 进行预处理，使后面自主航行更加精准。
在一实施例中，步骤S30包括以下步骤：
控制无人船前往第一个路径点 r1，到达第一个路径点后更新无人船当前路径点索引index，随后无人船根据控制系统指令向路径点索引为index+1 的位置运动，到达路径点索引为index+1 的位置后，更新路径点索引并根据控制系统指令继续向路径点索引为index+2的位置运动，重复此过程直至无人船到达预处理路线 A＇终点，此时自主航行结束。
在本实施例中，通过更新无人船当前路径点索引并沿更新索引的路径点所在位置运动，实现控制无人船沿预处理路线 A＇进行自主航行。
在本实施例中，如图3所示，步骤S30还包括步骤S32至S36：
S32、以无人船初始位置为原点，正北方向为
Figure 442958DEST_PATH_IMAGE017
轴正方向，正东方向为
Figure 957116DEST_PATH_IMAGE114
轴正方向， 建立世界坐标系，形成环境栅格地图B。
S33、基于无人船当前位置采集环境雷达点云 P，并以无人船船艏朝向为
Figure 86746DEST_PATH_IMAGE017
轴正方 向，无人船船艏右侧方向为 x 轴正方向，建立平面直角坐标系，作为雷达坐标系。
S34、对时间上相邻的5包雷达点云
Figure 104381DEST_PATH_IMAGE018
根据相应时刻的坐标进行方向角配准，并将 5包雷达点云合并为1包，形成雷达点云 P＇，其中
Figure 926844DEST_PATH_IMAGE019
代表第
Figure 911986DEST_PATH_IMAGE019
包雷达点云。
S35、对合并的雷达点云 P＇进行滤波，得到滤波后的雷达点云 P＇＇。
S36、计算雷达坐标系与世界坐标系间的旋转矩阵 Rot 和平移矩阵 Trans，根据 旋转矩阵 Rot 和平移矩阵 Trans 计算滤波后的雷达点云 P＇＇在世界坐标系中的位置，并 在环境栅格地图 B 标记，使环境栅格地图 B 中包含当前相应雷达的障碍，得到实时地图
Figure 845307DEST_PATH_IMAGE020
。
在本实施例中，通过采集无人船当前位置的位置坐标，进行方向角配准，以及基于 无人船当前位置采集环境雷达点云，构造实时地图
Figure 983027DEST_PATH_IMAGE115
，方便后续计算无人船未来路线上路径 点的障碍分布。
在一实施例中，步骤S34包括以下步骤：
以最后一包雷达点云时刻的坐标为对准点，方向角为对准方向，分别计算前4包雷 达点云相对最后一包雷达点云的平移向量
Figure 710812DEST_PATH_IMAGE021
和偏转角度
Figure 199562DEST_PATH_IMAGE022
，其中
Figure 405415DEST_PATH_IMAGE023
正方向为逆时针。
利用偏转角度
Figure 646910DEST_PATH_IMAGE023
计算每一包雷达点云投影到最后一包雷达点云所在坐标系的旋 转矩阵
Figure 201388DEST_PATH_IMAGE024
，平移向量
Figure 177434DEST_PATH_IMAGE025
，
Figure 718137DEST_PATH_IMAGE026
。
根据旋转矩阵
Figure 33712DEST_PATH_IMAGE027
和平移向量
Figure 368878DEST_PATH_IMAGE025
计算每一包雷达点云相应最后一包雷达点云 投影后的雷达点云
Figure 97800DEST_PATH_IMAGE028
，然后合并形成雷达点云
Figure 629144DEST_PATH_IMAGE029
，其中，计算投影雷达点云
Figure 127121DEST_PATH_IMAGE030
，雷达点云
Figure 305293DEST_PATH_IMAGE031
。
在本实施例中，通过计算旋转矩阵
Figure 521511DEST_PATH_IMAGE027
、平移向量
Figure 404016DEST_PATH_IMAGE025
、投影并合并即可得到雷 达点云
Figure 943451DEST_PATH_IMAGE029
，计算量小，可以降低计算量。
在一实施例中，步骤S35包括以下步骤：
对雷达点云
Figure 885999DEST_PATH_IMAGE032
使用直通滤波滤除近处由于无人船本身运动带来水花产生的水杂 波，得到雷达点云
Figure 527196DEST_PATH_IMAGE034
，
Figure 947813DEST_PATH_IMAGE035
其中,
Figure 420382DEST_PATH_IMAGE036
代表雷达点云在雷达坐标系下的
Figure 189624DEST_PATH_IMAGE037
轴，
Figure 114855DEST_PATH_IMAGE017
轴的距离，
Figure 542425DEST_PATH_IMAGE038
代表雷达点云与雷 达坐标系下原点之间的连线与
Figure 869501DEST_PATH_IMAGE037
轴组成的夹角，
Figure 153852DEST_PATH_IMAGE039
分别为相应物理量的阈值。
设置雷达点云信噪比阈值
Figure 753330DEST_PATH_IMAGE040
，在雷达点云
Figure 515749DEST_PATH_IMAGE041
中滤除低信噪比的雷达点云，得到雷 达点云
Figure 369436DEST_PATH_IMAGE042
，
Figure 824688DEST_PATH_IMAGE043
。
使用
Figure 724511DEST_PATH_IMAGE044
聚类的方法滤除雷达点云
Figure 743151DEST_PATH_IMAGE045
中的离群点，得到雷达点云簇
Figure 779240DEST_PATH_IMAGE046
，
Figure 405394DEST_PATH_IMAGE047
。
对雷达点云簇
Figure 464617DEST_PATH_IMAGE048
中的每个雷达点云簇进行跟踪，将无法跟踪的雷达点云簇使用卡 尔曼滤波的方法滤除，得到雷达点云簇
Figure 568839DEST_PATH_IMAGE049
，其中
Figure 725014DEST_PATH_IMAGE050
，并将雷达点云簇
Figure 974598DEST_PATH_IMAGE051
中的 所有雷达点云合并，得到最终滤波后的雷达点云
Figure 583434DEST_PATH_IMAGE052
。
在本实施例中，通过一系列滤波，以降低杂波的干扰，提高自主航行的精度。
在一实施例中，步骤S40包括以下步骤：
无人船以路径索引
Figure 694610DEST_PATH_IMAGE053
查找当前无人船航行路径的路径点
Figure 705291DEST_PATH_IMAGE054
, 将路径点
Figure 673247DEST_PATH_IMAGE055
作 为无人船当前位于路径上的点。
计算点
Figure 690750DEST_PATH_IMAGE054
沿路径前进方向一定距离内的所有点，形成局部点队列
Figure 136775DEST_PATH_IMAGE056
。
在本实施例中，计算点
Figure 267542DEST_PATH_IMAGE055
沿路径前进方向10m内的所有点。
在另一实施例中，可以根据实际情况对前进距离进行设置比如9m、11m或12m等。
计算局部点队列
Figure 609662DEST_PATH_IMAGE056
中每个路径点
Figure 193090DEST_PATH_IMAGE057
在实时地图M中的坐标 C。
以每个路径点
Figure 629756DEST_PATH_IMAGE058
为实时地图 M 中的局部中心点，裁取相应地图，形成地图队列
Figure 615030DEST_PATH_IMAGE059
。
记第i个路径点的地图队列
Figure 924789DEST_PATH_IMAGE060
中障碍栅格的数量 ni，设置障碍所占栅格阈值为 λ,当 ni >λ 时，路径点 i 判定为障碍点并记为 G，否则记为 F，局部点队列 R' 中的所有 障碍点信息构成障碍分布 D。
在本实施例中，通过计算局部点队列
Figure 933196DEST_PATH_IMAGE056
中每个路径点
Figure 173553DEST_PATH_IMAGE057
在实时地图M中的坐标 C， 在地图队列
Figure 13333DEST_PATH_IMAGE060
中计算对应每个路径点的障碍分布 D，计算量低，降低成本。
在一实施例中，步骤S50包括以下步骤：
对路径点障碍分布
Figure 166097DEST_PATH_IMAGE061
进行滑动，得到滑动窗口
Figure 458538DEST_PATH_IMAGE062
。
当 G 的数量大于
Figure 768166DEST_PATH_IMAGE063
时，则判定此时路径上存在障碍，并记录窗口第一个点的索引
Figure 462452DEST_PATH_IMAGE064
。
在本实施例中，索引
Figure 114013DEST_PATH_IMAGE116
所记录的路径点为障碍点，表示路线穿过障碍。
在一实施例中，步骤S60包括以下步骤：
从索引
Figure 97013DEST_PATH_IMAGE064
处继续滑动路径点障碍分布
Figure 492222DEST_PATH_IMAGE092
，得到滑动窗口
Figure 227966DEST_PATH_IMAGE067
。
当 F 的数量大于
Figure 316007DEST_PATH_IMAGE068
时，记录窗口所在的最后一个路径点，将该路径点作为绕行障 碍的目标点
Figure 583041DEST_PATH_IMAGE069
。
在本实施例中，通过设置避障障碍所占栅格阈值
Figure 454045DEST_PATH_IMAGE117
，判断此时路径上是否存在障 碍，简单高效。
在一实施例中，步骤S70包括以下步骤：
计算无人船实时位置在实时地图M中的坐标
Figure 122924DEST_PATH_IMAGE072
, 以此坐标 Coord作为中心点， 截取相应栅格地图
Figure 381867DEST_PATH_IMAGE073
。
从栅格地图
Figure 57567DEST_PATH_IMAGE073
中筛选出所有的障碍点
Figure 60158DEST_PATH_IMAGE074
，对障碍点进行相关滤波处理，得到
Figure 521227DEST_PATH_IMAGE075
。
以正东方向为0度方向，顺时针旋转，均分为n个扇区。
在本实施例中，采用每5度形成一个扇区，共形成72个扇区。
在另一实施例中，可以根据实际情况进行均分，比如采用每4度或每6度等来均分。
将障碍点
Figure 685492DEST_PATH_IMAGE076
投影到所有的扇区中，分别计算每个扇区对于无人船航行的危险程度
Figure 927117DEST_PATH_IMAGE077
，计算
Figure 654771DEST_PATH_IMAGE077
的方式：
Figure 32663DEST_PATH_IMAGE078
其中
Figure 633408DEST_PATH_IMAGE079
为每个扇区内障碍点到无人船的最小距离，
Figure 34434DEST_PATH_IMAGE080
为无人船的探测距离。
依据不同扇区的 T 值，选取绕行危险最低，速度最快的扇区
Figure 378827DEST_PATH_IMAGE081
作为障碍绕行扇区。
将障碍绕行扇区的中心方向角度作为障碍绕行方向
Figure 63755DEST_PATH_IMAGE082
。
在本实施例中，通过计算不同扇区的 T 值，以选取绕行危险最低，速度最快的扇 区
Figure 569823DEST_PATH_IMAGE081
作为障碍绕行扇区，从而将障碍绕行扇区的中心方向角度作为障碍绕行方向
Figure 786041DEST_PATH_IMAGE082
，简单 高效。
在一实施例中，如图4所示，步骤S75包括步骤S751至S754：
S751、计算目标位置所在的目标扇区。
S752、设定威胁阈值
Figure 606229DEST_PATH_IMAGE084
，筛选威胁
Figure 958713DEST_PATH_IMAGE085
的扇区，视为安全扇区，
Figure 822633DEST_PATH_IMAGE086
的扇区视为危 险扇区
Figure 260567DEST_PATH_IMAGE118
S753、滤除危险扇区以及危险扇区相邻的安全扇区，得到保留扇区。
S754、在保留扇区中选取距离目标扇区最近的扇区 c 作为障碍绕行扇区。
在本实施例中，通过设定威胁阈值
Figure 946764DEST_PATH_IMAGE119
，并滤除危险扇区以及危险扇区相邻的安全扇 区，得到保留扇区，实现更近一步降低危险扇区的干扰，精度高。
在一实施例中，步骤S80包括以下步骤：
以当前路径点索引
Figure 91437DEST_PATH_IMAGE093
为标志，取距离路径点路径反方向一定距离的路径点
Figure 204887DEST_PATH_IMAGE094
到路径正方向一定距离的路径点
Figure 130117DEST_PATH_IMAGE095
之间的所有点形成局部点队列
Figure 541376DEST_PATH_IMAGE096
，此处
Figure 868452DEST_PATH_IMAGE097
代表距离第
Figure 90486DEST_PATH_IMAGE053
个路径点
Figure 237434DEST_PATH_IMAGE098
米的最近的路径点，其中路径方向为正方向。
在本实施例中，以当前路径点索引
Figure 265433DEST_PATH_IMAGE093
为标志，取路径点沿路径反方向2m内，路径 正方向10m内的所有路径点
Figure 633966DEST_PATH_IMAGE120
到路径点
Figure 89218DEST_PATH_IMAGE121
之间的所有点形成局部点队列
Figure 926724DEST_PATH_IMAGE096
。
在另一实施例中，可根据实际情况进行反方向距离和正方向距离的设置，比如取反方向3m或4m，正方向11m或12m等多种设置。
计算
Figure 492835DEST_PATH_IMAGE096
中的每个点到无人船实时位置的距离
Figure 528924DEST_PATH_IMAGE099
，
Figure 342028DEST_PATH_IMAGE100
为局部点队列 中第
Figure 463568DEST_PATH_IMAGE101
个点到无人船实时位置的距离。
根据
Figure 567790DEST_PATH_IMAGE102
计算距离无人船实时位置最近点的索引
Figure 723965DEST_PATH_IMAGE103
，将索引
Figure 521019DEST_PATH_IMAGE104
作为当前点的索引。
以索引
Figure 864276DEST_PATH_IMAGE104
代表的路径点计算当前路径的障碍分布
Figure 506610DEST_PATH_IMAGE091
，将障碍分布D' 替换为障 碍分布 D。
在本实施例中，实时计算当前路径距离无人船的最近点
Figure 251712DEST_PATH_IMAGE122
，根据最近点
Figure 688510DEST_PATH_IMAGE123
计算障碍分布
Figure 526188DEST_PATH_IMAGE124
，更新避障的目标点
Figure 972213DEST_PATH_IMAGE125
，将障碍分布
Figure 571822DEST_PATH_IMAGE126
替换为障碍分布
Figure 445100DEST_PATH_IMAGE108
，实现无人船自 主规划路线，绕开障碍继续沿原路线航行，精确避障的同时降低计算量。
图5是本发明实施例提供的一种无人船避障的航行装置的示意性框图。如图5所示，对应于以上无人船避障的航行方法，本发明还提供一种无人船避障的航行装置。该无人船避障的航行装置包括用于执行上述无人船避障的航行方法的单元，该装置可以被配置于台式电脑、平板电脑或手提电脑等终端中。具体地，请参阅图5，该无人船避障的航行装置包括：
获取单元10，用于获取无人船航行路线 A。
预处理单元20，用于对航行路线 A 进行预处理，得到预处理路线 A＇。
采集单元30，用于控制无人船沿预处理路线A＇进行自主航行，并采集无人船当前 位置的位置坐标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地图
Figure 231790DEST_PATH_IMAGE020
。
第一计算单元40，用于根据实时地图
Figure 465194DEST_PATH_IMAGE020
，实时计算无人船未来路线上的障碍分布
Figure 184889DEST_PATH_IMAGE105
。
判断单元50，用于根据障碍分布
Figure 963489DEST_PATH_IMAGE105
，判断无人船未来路线是否穿过障碍。
第二计算单元60，用于若无人船未来路线穿过障碍，则计算绕行障碍的目标点
Figure 503054DEST_PATH_IMAGE106
。
第三计算单元70，用于根据无人船实时位置
Figure 25303DEST_PATH_IMAGE107
、实时地图
Figure 848771DEST_PATH_IMAGE020
和目标点
Figure 798272DEST_PATH_IMAGE071
，计算 障碍绕行方向 dire。
更新单元80，用于控制无人船沿障碍绕行方向 dire 绕行，并实时计算当前路径 距离无人船的最近点
Figure 90714DEST_PATH_IMAGE088
，根据最近点
Figure 151073DEST_PATH_IMAGE088
计算障碍分布
Figure 579781DEST_PATH_IMAGE089
，更新避障的目标点
Figure 215030DEST_PATH_IMAGE090
， 将障碍分布
Figure 463609DEST_PATH_IMAGE091
替换为障碍分布
Figure 593239DEST_PATH_IMAGE105
，并重复执行判断单元、第二计算单元、第三计算单元和更 新单元，直到无人船到达目标点为止。
在本实施例中，实时计算无人船未来路线上的障碍分布
Figure 876453DEST_PATH_IMAGE108
，并根据障碍分布
Figure 433336DEST_PATH_IMAGE109
，判 断无人船未来路线是否穿过障碍，若穿过障碍，则更新绕行障碍的目标点，直到无人船到达 目标点为止。通过本方法当无人船航行路线上出现障碍时，无人船可自主规划路线，绕开障 碍继续沿原路线航行，实现精确避障的同时降低计算量，降低对计算硬件性能要求，降低成 本，具备能够按照规定路径行走的能力。
在一实施例中，如图6所示，预处理单元20包括：
预处理模块21，用于设定航行路线 A 两点间距小于
Figure 418479DEST_PATH_IMAGE110
，从航行路线A的第一个点 开始遍历，对相邻两个路径点间距
Figure 86220DEST_PATH_IMAGE127
大于
Figure 223941DEST_PATH_IMAGE112
时,插入间距为
Figure 951725DEST_PATH_IMAGE113
的点队列；对相邻两个路径 点间距
Figure 174896DEST_PATH_IMAGE128
小于
Figure 161175DEST_PATH_IMAGE112
时，在中点插入一个点，完成对航行路线 A 的预处理，得到预处理路线 A＇。
在本实施例中，通过插值法对航行路线 A 进行预处理，使后面自主航行更加精准。
在一实施例中，如图7所示，采集单元30包括：
自主航行模块31，用于控制无人船前往第一个路径点 r1，到达第一个路径点后更新无人船当前路径点索引index，随后无人船根据控制系统指令向路径点索引为index+1的位置运动，到达路径点索引为index+1 的位置后，更新路径点索引并根据控制系统指令继续向路径点索引为index+2的位置运动，重复此过程直至无人船到达预处理路线 A＇终点，此时自主航行结束。
在本实施例中，通过更新无人船当前路径点索引并沿更新索引的路径点所在位置运动，实现控制无人船沿预处理路线 A＇进行自主航行。
另一实施例中，如图8所示，采集单元30还包括：
世界坐标系模块32，用于以无人船初始位置为原点，正北方向为
Figure 153402DEST_PATH_IMAGE017
轴正方向，正东 方向为
Figure 786509DEST_PATH_IMAGE114
轴正方向，建立世界坐标系，形成环境栅格地图B。
平面坐标系模块33，用于基于无人船当前位置采集环境雷达点云 P，并以无人船 船艏朝向为
Figure 762555DEST_PATH_IMAGE017
轴正方向，无人船船艏右侧方向为 x 轴正方向，建立平面直角坐标系，作为雷 达坐标系。
雷达点云模块34，用于对时间上相邻的5包雷达点云
Figure 37679DEST_PATH_IMAGE018
根据相应时刻的坐标进行 方向角配准，并将5包雷达点云合并为1包，形成雷达点云 P＇，其中
Figure 133680DEST_PATH_IMAGE019
代表第
Figure 203267DEST_PATH_IMAGE019
包雷达点 云。
雷达点云滤波模块35，用于对合并的雷达点云 P＇进行滤波，得到滤波后的雷达点云 P＇＇。
计算和标记模块36，用于计算雷达坐标系与世界坐标系间的旋转矩阵 Rot 和平 移矩阵 Trans，根据旋转矩阵 Rot 和平移矩阵 Trans 计算滤波后的雷达点云 P＇＇在世界 坐标系中的位置，并在环境栅格地图 B 标记，使环境栅格地图 B 中包含当前相应雷达的 障碍，得到实时地图
Figure 401030DEST_PATH_IMAGE020
。
在本实施例中，通过采集无人船当前位置的位置坐标，进行方向角配准，以及基于 无人船当前位置采集环境雷达点云，构造实时地图
Figure 214265DEST_PATH_IMAGE115
，方便后续计算无人船未来路线上路径 点的障碍分布。
在本实施例中，如图9所示，雷达点云模块34包括：
雷达点云计算子模块341，用于以最后一包雷达点云时刻的坐标为对准点，方向角 为对准方向，分别计算前4包雷达点云相对最后一包雷达点云的平移向量
Figure 915505DEST_PATH_IMAGE021
和偏转角度
Figure 670840DEST_PATH_IMAGE022
， 其中
Figure 90320DEST_PATH_IMAGE023
正方向为逆时针。
投影子模块342，用于利用偏转角度
Figure 707246DEST_PATH_IMAGE023
计算每一包雷达点云投影到最后一包雷达 点云所在坐标系的旋转矩阵
Figure 528572DEST_PATH_IMAGE024
，平移向量
Figure 205541DEST_PATH_IMAGE025
，
Figure 377896DEST_PATH_IMAGE026
。
雷达点云子模块343，用于根据旋转矩阵
Figure 782202DEST_PATH_IMAGE027
和平移向量
Figure 458034DEST_PATH_IMAGE025
计算每一包雷达点 云相应最后一包雷达点云投影后的雷达点云
Figure 305904DEST_PATH_IMAGE028
，然后合并形成雷达点云
Figure 699976DEST_PATH_IMAGE029
，其中，计算投影 雷达点云
Figure 658705DEST_PATH_IMAGE030
，雷达点云
Figure 703890DEST_PATH_IMAGE031
。
在本实施例中，通过计算旋转矩阵
Figure 457082DEST_PATH_IMAGE027
、平移向量
Figure 338451DEST_PATH_IMAGE025
、投影并合并即可得到雷 达点云
Figure 100870DEST_PATH_IMAGE029
，计算量小，可以降低计算量。
在本实施例中，如图10所示，雷达点云滤波模块35包括：
第一滤波子模块351，用于对雷达点云
Figure 751295DEST_PATH_IMAGE032
使用直通滤波滤除近处由于无人船本身运 动带来水花产生的水杂波，得到雷达点云
Figure 190235DEST_PATH_IMAGE034
，
Figure 293320DEST_PATH_IMAGE035
其中,
Figure 593852DEST_PATH_IMAGE036
代表雷达点云在雷达坐标系下的
Figure 364361DEST_PATH_IMAGE037
轴，
Figure 724936DEST_PATH_IMAGE017
轴的距离，
Figure 564585DEST_PATH_IMAGE038
代表雷达点云与雷 达坐标系下原点之间的连线与
Figure 668807DEST_PATH_IMAGE037
轴组成的夹角，
Figure 293823DEST_PATH_IMAGE039
分别为相应物理量的阈值。
第二滤波子模块352，用于设置雷达点云信噪比阈值
Figure 825299DEST_PATH_IMAGE040
，在雷达点云
Figure 902976DEST_PATH_IMAGE041
中滤除低 信噪比的雷达点云，得到雷达点云
Figure 545310DEST_PATH_IMAGE042
，
Figure 8521DEST_PATH_IMAGE043
。
第三滤波子模块353，用于使用
Figure 710898DEST_PATH_IMAGE044
聚类的方法滤除雷达点云
Figure 275872DEST_PATH_IMAGE045
中的离群点，得 到雷达点云簇
Figure 721896DEST_PATH_IMAGE046
，
Figure 321505DEST_PATH_IMAGE047
。
对雷达点云簇
Figure 178471DEST_PATH_IMAGE048
中的每个雷达点云簇进行跟踪，将无法跟踪的雷达点云簇使用卡 尔曼滤波的方法滤除，得到雷达点云簇
Figure 496320DEST_PATH_IMAGE049
，其中
Figure 480457DEST_PATH_IMAGE050
，并将雷达点云簇
Figure 934572DEST_PATH_IMAGE051
中的 所有雷达点云合并，得到最终滤波后的雷达点云
Figure 713172DEST_PATH_IMAGE052
。
在本实施例中，通过一系列滤波，以降低杂波的干扰，提高自主航行的精度。
在一实施例中，如图11所示，第一计算单元40包括：
查找子模块41，用于无人船以路径索引
Figure 767585DEST_PATH_IMAGE053
查找当前无人船航行路径的路径点
Figure 289833DEST_PATH_IMAGE054
, 将路径点
Figure 864034DEST_PATH_IMAGE055
作为无人船当前位于路径上的点。
第一计算模块42，用于计算点
Figure 813535DEST_PATH_IMAGE054
沿路径前进方向一定距离内的所有点，形成局部 点队列
Figure 840397DEST_PATH_IMAGE056
。
在本实施例中，第一计算模块42为计算点
Figure 415604DEST_PATH_IMAGE055
沿路径前进方向10m内的所有点。
在另一实施例中，可以根据实际情况对前进距离进行设置比如9m、11m或12m等。
第二计算模块43，用于计算局部点队列
Figure 578732DEST_PATH_IMAGE056
中每个路径点
Figure 230293DEST_PATH_IMAGE057
在实时地图M中的坐标 C。
截取子模块44，用于以每个路径点
Figure 478872DEST_PATH_IMAGE058
为实时地图 M 中的局部中心点，裁取相应地 图，形成地图队列
Figure 608502DEST_PATH_IMAGE059
。
第三计算模块45，用于记第i个路径点的地图队列
Figure 140983DEST_PATH_IMAGE060
中障碍栅格的数量 ni，设置 障碍所占栅格阈值为 λ,当 ni >λ 时，路径点 i 判定为障碍点并记为 G，否则记为 F，局 部点队列 R' 中的所有障碍点信息构成障碍分布 D。
在本实施例中，通过计算局部点队列
Figure 432287DEST_PATH_IMAGE056
中每个路径点
Figure 433741DEST_PATH_IMAGE057
在实时地图M中的坐标 C， 在地图队列
Figure 101483DEST_PATH_IMAGE060
中计算对应每个路径点的障碍分布 D，计算量低，降低成本。
在一实施例中，如图12所示，判断单元50包括：
第一滑动模块51，用于对路径点障碍分布
Figure 504782DEST_PATH_IMAGE061
进行滑动，得到滑动窗口
Figure 232567DEST_PATH_IMAGE062
。
判断模块52，用于当 G的数量大于
Figure 705005DEST_PATH_IMAGE063
时，则判定此时路径上存在障碍，并记录窗口 第一个点的索引
Figure 176438DEST_PATH_IMAGE064
。
在本实施例中，索引
Figure 168665DEST_PATH_IMAGE116
所记录的路径点为障碍点，表示路线穿过障碍。
在一实施例中，如图13所示，第二计算单元60包括：
第二滑动模块61，用于从索引
Figure 332930DEST_PATH_IMAGE129
处继续滑动路径点障碍分布
Figure 43397DEST_PATH_IMAGE061
，得到滑动窗 口
Figure 36630DEST_PATH_IMAGE067
。
设置模块62，用于当 F 的数量大于
Figure 414521DEST_PATH_IMAGE068
时，记录窗口所在的最后一个路径点，将该路 径点作为绕行障碍的目标点
Figure 218529DEST_PATH_IMAGE069
。
在本实施例中，通过设置避障障碍所占栅格阈值
Figure 416292DEST_PATH_IMAGE130
，判断此时路径上是否存在障 碍，简单高效。
在一实施例中，如图14所示，第三计算单元70包括：
第四计算模块71，用于计算无人船实时位置在实时地图M中的坐标
Figure 229528DEST_PATH_IMAGE072
, 以此坐 标 Coord作为中心点，截取相应栅格地图
Figure 711194DEST_PATH_IMAGE073
。
障碍点滤波模块72，用于从栅格地图
Figure 951682DEST_PATH_IMAGE073
中筛选出所有的障碍点
Figure 636741DEST_PATH_IMAGE074
，对障碍点进行相 关滤波处理，得到
Figure 988088DEST_PATH_IMAGE075
。
扇区模块73，用于以正东方向为0度方向，顺时针旋转，均分为n个扇区。
在本实施例中，采用每5度形成一个扇区，共形成72个扇区。
在另一实施例中，可以根据实际情况进行均分，比如采用每4度或每6度等来均分。
第五计算模块74，用于将障碍点
Figure 74993DEST_PATH_IMAGE076
投影到所有的扇区中，分别计算每个扇区对于无 人船航行的危险程度
Figure 735650DEST_PATH_IMAGE077
，计算
Figure 642426DEST_PATH_IMAGE077
的方式：
Figure 797464DEST_PATH_IMAGE078
其中
Figure 738875DEST_PATH_IMAGE079
为每个扇区内障碍点到无人船的最小距离，
Figure 586746DEST_PATH_IMAGE080
为无人船的探测距离。
选取模块75，用于依据不同扇区的 T 值，选取绕行危险最低，速度最快的扇区
Figure 246397DEST_PATH_IMAGE081
作 为障碍绕行扇区。
绕行方向模块76，用于将选定扇区的中心方向角度作为障碍绕行方向
Figure 188814DEST_PATH_IMAGE082
。
在本实施例中，通过计算不同扇区的 T 值，以选取绕行危险最低，速度最快的扇 区
Figure 984732DEST_PATH_IMAGE081
作为障碍绕行扇区，从而将障碍绕行扇区的中心方向角度作为障碍绕行方向
Figure 737924DEST_PATH_IMAGE082
，简单 高效。
在一实施例中，如图15所示，选取模块75包括：
目标位置计算子模块751，用于计算目标位置所在的目标扇区。
筛选子模块752，用于设定威胁阈值
Figure 619293DEST_PATH_IMAGE084
，筛选威胁
Figure 381712DEST_PATH_IMAGE085
的扇区，视为安全扇区，
Figure 281404DEST_PATH_IMAGE086
的扇区视为危险扇区
Figure 205498DEST_PATH_IMAGE118
扇区连续性判断子模块753，用于滤除危险扇区以及危险扇区相邻的安全扇区，得到保留扇区。
选取子模块754，用于在保留扇区中选取距离目标扇区最近的扇区 c 作为障碍绕行扇区。
在本实施例中，通过设定威胁阈值
Figure 574162DEST_PATH_IMAGE119
，并滤除危险扇区以及危险扇区相邻的安全扇 区，得到保留扇区，实现更近一步降低危险扇区的干扰，精度高。
在一实施例中，如图16所示，更新单元80包括：
第一更新模块81，用于以当前路径点索引
Figure 874693DEST_PATH_IMAGE093
为标志，取距离路径点路径反方向 一定距离的路径点
Figure 645203DEST_PATH_IMAGE094
到路径正方向一定距离的路径点
Figure 989466DEST_PATH_IMAGE095
之间的所有点形成局部 点队列
Figure 845426DEST_PATH_IMAGE096
，此处
Figure 949649DEST_PATH_IMAGE097
代表距离第
Figure 309086DEST_PATH_IMAGE053
个路径点
Figure 840561DEST_PATH_IMAGE098
米的最近的路径点，其中路径方 向为正方向。
在本实施例中，以当前路径点索引
Figure 183818DEST_PATH_IMAGE093
为标志，取路径点沿路径反方向2m内，路径 正方向10m内的所有路径点
Figure 75419DEST_PATH_IMAGE120
到路径点
Figure 554942DEST_PATH_IMAGE121
之间的所有点形成局部点队列
Figure 257319DEST_PATH_IMAGE096
。
在另一实施例中，可根据实际情况进行反方向距离和正方向距离的设置，比如取反方向3m或4m，正方向11m或12m等多种设置。
第二更新模块82，用于计算
Figure 822293DEST_PATH_IMAGE096
中的每个点到无人船实时位置的距离
Figure 268317DEST_PATH_IMAGE099
，
Figure 117194DEST_PATH_IMAGE100
为局部点队列中第
Figure 724892DEST_PATH_IMAGE101
个点到无人船实时位置的距离。
第三更新模块83，用于根据
Figure 42741DEST_PATH_IMAGE102
计算距离无人船实 时位置最近点的索引
Figure 761299DEST_PATH_IMAGE103
，将索引
Figure 480993DEST_PATH_IMAGE104
作为当前点的索引。
第四更新模块84，用于以索引
Figure 508861DEST_PATH_IMAGE104
代表的路径点计算当前路径的障碍分布
Figure 314006DEST_PATH_IMAGE091
，将 障碍分布D' 替换为障碍分布 D。
在本实施例中，实时计算当前路径距离无人船的最近点
Figure 836254DEST_PATH_IMAGE122
，根据最近点
Figure 410455DEST_PATH_IMAGE123
计算障碍分布
Figure 359956DEST_PATH_IMAGE124
，更新避障的目标点
Figure 386818DEST_PATH_IMAGE125
，将障碍分布
Figure 962025DEST_PATH_IMAGE126
替换为障碍分布
Figure 390732DEST_PATH_IMAGE108
，实现无人船自 主规划路线，绕开障碍继续沿原路线航行，精确避障的同时降低计算量。
需要说明的是，所属领域的技术人员可以清楚地了解到，上述无人船避障的航行装置和各单元的具体实现过程，可以参考前述方法实施例中的相应描述，为了描述的方便和简洁，在此不再赘述。
请参阅图17，图17是本申请实施例提供的一种计算机设备的示意性框图。该计算机设备500可以是终端，也可以是服务器，其中，终端可以是智能手机、平板电脑、笔记本电脑、台式电脑、个人数字助理和穿戴式设备等具有通信功能的电子设备。服务器可以是独立的服务器，也可以是多个服务器组成的服务器集群。
参阅图17，该计算机设备500包括通过系统总线501连接的处理器502、存储器和网络接口505，其中，存储器可以包括非易失性存储介质503和内存储器504。
该非易失性存储介质503可存储操作系统5031和计算机程序5032。该计算机程序5032包括程序指令，该程序指令被执行时，可使得处理器502执行一种无人船避障的航行方法。
该处理器502用于提供计算和控制能力，以支撑整个计算机设备500的运行。
该内存储器504为非易失性存储介质503中的计算机程序5032的运行提供环境，该计算机程序5032被处理器502执行时，可使得处理器502执行一种无人船避障的航行方法。
该网络接口505用于与其它设备进行网络通信。本领域技术人员可以理解，图17中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备500的限定，具体的计算机设备500可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
其中，所述处理器502用于运行存储在存储器中的计算机程序5032。
应当理解，在本申请实施例中，处理器502可以是中央处理单元 (CentralProcessing Unit，CPU)，该处理器502还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路 (Application Specific IntegratedCircuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。其中，通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
本领域普通技术人员可以理解的是实现上述实施例的方法中的全部或部分流程，是可以通过计算机程序来指令相关的硬件来完成。该计算机程序包括程序指令，计算机程序可存储于一存储介质中，该存储介质为计算机可读存储介质。该程序指令被该计算机系统中的至少一个处理器执行，以实现上述方法的实施例的流程步骤。
因此，本发明还提供一种存储介质。该存储介质可以为计算机可读存储介质。
所述存储介质可以是U盘、移动硬盘、只读存储器（Read-Only Memory，ROM）、磁碟或者光盘等各种可以存储程序代码的计算机可读存储介质。
本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行，取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的几个实施例中，应该理解到，所揭露的装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的。例如，各个单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式。例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。
本发明实施例方法中的步骤可以根据实际需要进行顺序调整、合并和删减。本发明实施例装置中的单元可以根据实际需要进行合并、划分和删减。另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。
该集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备（可以是个人计算机，终端，或者网络设备等）执行本发明各个实施例所述方法的全部或部分步骤。
上述仅以实施例来进一步说明本发明的技术内容，以便于读者更容易理解，但不代表本发明的实施方式仅限于此，任何依本发明所做的技术延伸或再创造，均受本发明的保护。本发明的保护范围以权利要求书为准。

Claims (15)

1.一种无人船避障的航行方法，其特征在于，包括以下步骤：
步骤一、获取无人船航行路线A；
步骤二、对所述航行路线A进行预处理，得到预处理路线A′；
步骤三、控制无人船沿所述预处理路线A′进行自主航行，并采集无人船当前位置的位置坐标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地图M；
步骤四、根据所述实时地图M，实时计算无人船未来路线上的障碍分布D；
步骤五、根据所述障碍分布D，判断无人船未来路线是否穿过障碍；
步骤六、若无人船未来路线穿过障碍，则计算绕行障碍的目标点(a1，b1)；
步骤七、根据无人船实时位置(a，b)、所述实时地图M和所述目标点(a1，b1)，计算障碍绕行方向dire；
步骤八、控制无人船沿所述障碍绕行方向dire绕行，并实时计算当前路径距离无人船的最近点(a′，b′)，根据所述最近点(a′，b′)计算障碍分布D′，更新避障的目标点(a2，b2)，将障碍分布D′替换为障碍分布D，并重复执行步骤五至步骤八，直到无人船到达目标点为止。
2.根据权利要求1所述的无人船避障的航行方法，其特征在于，所述对所述航行路线A进行预处理，得到预处理路线A′的步骤，包括：
设定航行路线A两点间距小于d，从航行路线A的第一个点开始遍历，对相邻两个路径点间距d′大于2d插入间距为d的点队列；对相邻两个路径点间距d′小于2d，在中点插入一个点，完成对航行路线A的预处理，得到预处理路线A′。
3.根据权利要求1所述的无人船避障的航行方法，其特征在于，所述控制无人船沿所述预处理路线A′进行自主航行的步骤，包括：
控制无人船前往第一个路径点r1，到达第一个路径点后更新无人船当前路径点索引index，随后无人船根据控制系统指令向路径点索引为index+1位置运动，到达索引index+1的位置后，更新路径点索引并根据控制系统指令继续向index+2的路径点所在位置运动，重复此过程直至无人船到达所述预处理路线A′终点，此时自主航行结束。
4.根据权利要求1所述的无人船避障的航行方法，其特征在于，所述采集无人船当前位置的位置坐标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地图M的步骤，包括：
以无人船初始位置为原点，正北方向为y轴正方向，正东方向为x轴正方向，建立世界坐标系，形成环境栅格地图B；
基于无人船当前位置采集环境雷达点云P，并以无人船船艏朝向为y轴正方向，无人船船艏右侧方向为x轴正方向，建立平面直角坐标系，作为雷达坐标系；
对相邻5包的雷达点云
Figure FDA0003094742930000021
根据相应时刻的坐标，进行方向角配准，并将5包雷达点云合并为1包，形成雷达点云P′，其中iρc代表第ipc包雷达点云；
对合并的雷达点云P′进行滤波，得到滤波后的雷达点云P″；
计算雷达坐标系与世界坐标系间的旋转矩阵Rot和平移矩阵Trans，根据所述旋转矩阵Rot和所述平移矩阵Trans计算滤波后的雷达点云P″在世界坐标系中的位置，并在环境栅格地图B标记，使环境栅格地图B中包含当前相应雷达的障碍，得到实时地图M。
5.根据权利要求4所述的无人船避障的航行方法，其特征在于，所述对相邻5包的雷达点云
Figure FDA0003094742930000022
根据相应时刻的坐标，进行方向角配准，并将5包雷达点云合并为1包，形成雷达点云P′，其中ipc代表第ipc包雷达点云的步骤，包括：
以最后一包雷达点云时刻的坐标为对准点，方向角为对准方向，分别计算前4包雷达点云相对最后一包雷达点云的平移向量
Figure FDA0003094742930000023
和偏转角度
Figure FDA0003094742930000024
其中
Figure FDA0003094742930000031
正方向为逆时针；
利用偏转角度
Figure FDA0003094742930000032
计算每一包雷达点云投影到最后一包雷达点云所在坐标系的旋转矩阵
Figure FDA0003094742930000033
平移向量
Figure FDA0003094742930000034
Figure FDA0003094742930000035
根据所述旋转矩阵
Figure FDA0003094742930000036
和所述平移向量
Figure FDA0003094742930000037
计算每一包雷达点云相应最后一包雷达点云投影后的雷达点云
Figure FDA0003094742930000038
然后合并形成雷达点云P′，其中，计算投影雷达点云
Figure FDA0003094742930000039
雷达点云
Figure FDA00030947429300000310
Figure FDA00030947429300000311
6.根据权利要求4所述的无人船避障的航行方法，其特征在于，所述对合并的雷达点云P′进行滤波，得到滤波后的雷达点云P″的步骤，包括：
对雷达点云P′使用直通滤波滤除近处由于无人船本身运动带来水花产生的水杂波，得到雷达点云P′1，
P′1＝{pi||xi|＞x0，|yi|＞y0,|θi-90|＜θ0，pi∈P′}
其中，x，y代表雷达点云在雷达坐标系下的x轴，y轴的距离，θ代表雷达点云与雷达坐标系下原点之间直线的倾角，x0，y0，θ0分别为相应物理量的阈值；
设置雷达点云信噪比阈值SNR0，在雷达点云P′1中滤除低信噪比的雷达点云，得到雷达点云P′2，P′2＝{pi|SNRi＞SNR0，pi∈P′1}；
使用DBSCAN聚类的方法滤除雷达点云P′2中的离群点，得到雷达点云簇P′3，
P′3＝DBSCAN(P′2，eps，minPts)；
对雷达点云簇P′3中的每个雷达点云簇进行跟踪，将无法跟踪的雷达点云簇滤除，得到雷达点云簇P′4，其中P′4＝kalmanFilter(P′3，)，并将雷达点云簇P′4中的所有雷达点云合并，得到最终滤波后的雷达点云P”。
7.根据权利要求1所述的无人船避障的航行方法，其特征在于，所述根据所述实时地图M，实时计算无人船未来路线上的障碍分布D的步骤，包括：
无人船以路径索引index查找当前无人船航行路径的路径点rindex，将路径点rindex作为无人船当前位于路径上的点；
计算点rindex沿路径前进方向一定距离内的所有点，形成局部点队列R′；
计算局部点队列R′中每个路径点r′在实时地图M中的坐标C；
以每个路径点r′为实时地图M中的局部中心点，裁取相应地图，形成地图队列M′；
记第i个路径点的地图队列M′中障碍栅格的数量ni，设置障碍所占栅格阈值为λ，当ni＞λ时，路径点i判定为障碍点并记为T，否则记为F，局部点队列R′中的所有障碍点信息构成障碍分布D。
8.根据权利要求7所述的无人船避障的航行方法，其特征在于，所述根据所述障碍分布D，判断无人船未来路线是否穿过障碍的步骤，包括：
对路径点障碍分布D进行滑动，得到滑动窗口W＝w1；
当所述T的数量大于
Figure FDA0003094742930000041
时，则判定此时路径上存在障碍，并记录窗口第一个点的索引indexobs。
9.根据权利要求8所述的无人船避障的航行方法，其特征在于，所述若无人船未来路线穿过障碍，则计算绕行障碍的目标点(a1,b1)的步骤，包括：
从索引indexobs处继续滑动路径点障碍分布D，得到滑动窗口W＝w2；
当所述F的数量大于
Figure FDA0003094742930000042
时，记录窗口所在的最后一个路径点，将该路径点作为绕行障碍的目标点(a1，b1)。
10.根据权利要求1所述的无人船避障的航行方法，其特征在于，所述根据无人船实时位置(a,b)、所述实时地图M和所述目标点(a1,b1)，计算障碍绕行方向dire的步骤，包括：
计算无人船实时位置在实时地图M中的坐标Coord，以此坐标Coord作为中心点，截取相应栅格地图M1；
从所述栅格地图M1中筛选出所有的障碍点O，对障碍点进行相关滤波处理，得到O′；
以正东方向为0度方向，顺时针旋转，均分为n个扇区；
将障碍点O′投影到所有的扇区中，分别计算每个扇区对于无人船航行的危险程度T，计算T的方式：
Figure FDA0003094742930000051
其中disc为每个扇区内障碍点到无人船的最小距离，dmax为无人船的探测距离；
依据不同扇区的T值，选取绕行危险最低，速度最快的扇区c作为障碍绕行扇区；
将所述障碍绕行扇区的中心方向角度作为障碍绕行方向dire。
11.根据权利要求10所述的无人船避障的航行方法，其特征在于，所述依据不同扇区的T值，选取绕行危险最低，速度最快的扇区c作为障碍绕行扇区的步骤，包括：
计算目标点所在的目标扇区；
设定威胁阈值T0，筛选威胁T＜T0的扇区，视为安全扇区，T＞T0的扇区视为危险扇区；
滤除所述危险扇区以及危险扇区相邻的安全扇区，得到保留扇区；
在所述保留扇区中选取距离所述目标扇区最近的扇区c作为障碍绕行扇区。
12.根据权利要求1所述的无人船避障的航行方法，其特征在于，所述控制无人船沿所述障碍绕行方向dire绕行，并实时计算当前路径距离无人船的最近点(a′，b′)，根据所述最近点(a′，b′)计算障碍分布D′，更新避障的目标点(a2，b2)，将障碍分布D′替换为障碍分布D的步骤，包括：
以当前路径点索引index为标志，取距离路径点路径反方向一定距离的路径点rindex-dis1到路径正方向一定距离的路径点rindex+dis2的所有点形成局部点队列Rpartial，此处index±dis代表距离第index个路径点dis米的最近的路径点，其中路径方向为正方向；
计算Rpartial中的每个点到无人船实时位置的距离dispartial，dispartial(i)为局部点队列中第i个点到无人船实时位置的距离；
根据{imindis|dispartial＝min(dispartial(imindis))}计算距离无人船实时位置最近点的索引imindis，将索引imindis作为当前点的索引；
以索引imindis代表的路径点计算当前路径的障碍分布D′，将障碍分布D′替换为障碍分布D。
13.一种无人船自主避障的航行装置，其特征在于，包括：
获取单元，用于获取无人船航行路线A；
预处理单元，用于对所述航行路线A进行预处理，得到预处理路线A′；
采集单元，用于控制无人船沿所述预处理路线A′进行自主航行，并采集无人船当前位置的位置坐标和方向角，以及基于无人船当前位置采集环境雷达点云，以构造实时地图M；
第一计算单元，用于根据所述实时地图M，实时计算无人船未来路线上的障碍分布D；
判断单元，用于根据所述障碍分布D，判断无人船未来路线是否穿过障碍；
第二计算单元，用于若无人船未来路线穿过障碍，则计算绕行障碍的目标点(a1,b1)；
第三计算单元，用于根据无人船实时位置(a,b)、所述实时地图M和所述目标点(a1,b1)，计算障碍绕行方向dire；
更新单元，用于控制无人船沿所述障碍绕行方向dire绕行，并实时计算当前路径距离无人船的最近点(a′，b′)，根据所述最近点(a′，b′)计算障碍分布D′，更新避障的目标点(a2，b2)，将障碍分布D′替换为障碍分布D，并重复执行步骤五至步骤八，直到无人船到达目标点为止。
14.一种计算机设备，其特征在于，所述计算机设备包括存储器及处理器，所述存储器上存储有计算机程序，所述处理器执行所述计算机程序时实现如权利要求1至12中任一项所述的无人船避障的航行方法。
15.一种存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序被处理器执行时可实现如权利要求1至12中任一项所述的无人船避障的航行方法。
CN202110304729.8A 2021-03-23 2021-03-23 一种无人船避障的航行方法、装置、计算机设备及存储介质 Active CN112797987B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110304729.8A CN112797987B (zh) 2021-03-23 2021-03-23 一种无人船避障的航行方法、装置、计算机设备及存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110304729.8A CN112797987B (zh) 2021-03-23 2021-03-23 一种无人船避障的航行方法、装置、计算机设备及存储介质

Publications (2)

Publication Number Publication Date
CN112797987A CN112797987A (zh) 2021-05-14
CN112797987B true CN112797987B (zh) 2021-07-23

Family

ID=75815613

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110304729.8A Active CN112797987B (zh) 2021-03-23 2021-03-23 一种无人船避障的航行方法、装置、计算机设备及存储介质

Country Status (1)

Country Link
CN (1) CN112797987B (zh)

Families Citing this family (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114202684B (zh) * 2021-11-29 2023-06-16 哈尔滨工程大学 一种适用于水面环境的点云数据投影方法、系统及装置
CN114594771A (zh) * 2022-03-07 2022-06-07 陕西欧卡电子智能科技有限公司 一种水面机器人的卡位检测和脱困方法及其装置
CN114814796B (zh) * 2022-07-01 2022-09-30 陕西欧卡电子智能科技有限公司 基于高精度地图的水面可行驶区域提取方法、装置及设备
CN116225032B (zh) * 2023-05-09 2023-07-25 陕西欧卡电子智能科技有限公司 基于已知航迹的无人船集群协同避障规划方法
CN116430906B (zh) * 2023-06-13 2023-10-03 西安羚控电子科技有限公司 一种基于凸点平移的无人机动态避障方法、系统、设备及介质
CN116878471B (zh) * 2023-09-06 2023-11-10 湖南湘船重工有限公司 一种水上无人测绘系统

Family Cites Families (16)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JPH05165523A (ja) * 1991-12-13 1993-07-02 Meidensha Corp 無人搬送車の制御装置
CN103455034B (zh) * 2013-09-16 2016-05-25 苏州大学张家港工业技术研究院 一种基于最近距离向量场直方图的避障路径规划方法
CN105549589A (zh) * 2015-12-15 2016-05-04 吴宝举 一种基于前视雷达的无人船避碰方法
CN106371445B (zh) * 2016-11-17 2019-05-14 浙江大学 一种基于拓扑地图的无人车规划控制方法
WO2018226253A1 (en) * 2016-12-16 2018-12-13 The Government Of The United States Of America As Represented By The Secretary Of The Navy Millimeter-wave terrain aided navigation system
CN108363395A (zh) * 2018-02-11 2018-08-03 智久(厦门)机器人科技有限公司 一种agv自主避障的方法
CN108549385B (zh) * 2018-05-22 2021-05-04 东南大学 一种结合a*算法和vfh避障算法的机器人动态路径规划方法
CN109085838A (zh) * 2018-09-05 2018-12-25 南京理工大学 一种基于激光定位的动态障碍物剔除算法
CN109460035B (zh) * 2018-12-18 2021-10-15 国家海洋局北海海洋工程勘察研究院（青岛环海海洋工程勘察研究院） 一种无人艇高速状态下的二级自主避障方法
CN109828566B (zh) * 2019-01-30 2022-05-24 华南理工大学 一种水面无人艇自主航行方法
CN110032184A (zh) * 2019-03-12 2019-07-19 哈尔滨工程大学 一种基于遗传与滑动窗口法的水上无人艇避碰方法
CN110208816B (zh) * 2019-06-04 2023-05-16 浙江海洋大学 用于海上无人艇的自动障碍物识别系统及识别方法
CN110609570A (zh) * 2019-07-23 2019-12-24 中国南方电网有限责任公司超高压输电公司天生桥局 一种基于无人机的自主避障巡检方法
CN110608740B (zh) * 2019-09-06 2024-08-06 遵义师范学院 一种无人船舰路径规划方法
CN111220160B (zh) * 2020-02-27 2023-07-04 智慧航海（青岛）科技有限公司 基于D*Lite优化算法的自主驾驶船舶航路规划方法
CN112506225B (zh) * 2020-12-15 2023-09-01 广州极飞科技股份有限公司 一种无人机避障方法、装置、无人机和存储介质

Also Published As

Publication number Publication date
CN112797987A (zh) 2021-05-14

Similar Documents

Publication Publication Date Title
CN112797987B (zh) 一种无人船避障的航行方法、装置、计算机设备及存储介质
Wang et al. Polarized light compass-aided visual-inertial navigation under foliage environment
CN108073175B (zh) 基于虚拟艇自适应规划的欠驱动无人艇编队智能控制方法
CN110673115A (zh) 雷达与组合导航系统的联合标定方法、装置、设备及介质
CN110930495A (zh) 基于多无人机协作的icp点云地图融合方法、系统、装置及存储介质
CN103411609A (zh) 一种基于在线构图的飞行器返航路线规划方法
Cai et al. Mobile robot localization using gps, imu and visual odometry
CN109461208A (zh) 三维地图处理方法、装置、介质和计算设备
CN111090283B (zh) 一种无人艇组合定位定向方法和系统
CN112764074B (zh) 一种定位导航轨迹的方法、装置及电子设备
CN107526087A (zh) 一种获取水下3d断层图像的方法及系统
CN114046792B (zh) 一种无人船水面定位与建图方法、装置及相关组件
CN110673107A (zh) 基于多线激光雷达的路沿检测方法及装置
CN111665868A (zh) 基于虚拟航道的无人船返航方法、装置、设备及存储介质
CN114077249B (zh) 一种作业方法、作业设备、装置、存储介质
CN114485698A (zh) 一种交叉路口引导线生成方法及系统
CN114593739A (zh) 基于视觉检测与参考线匹配的车辆全局定位方法及装置
CN108241368B (zh) 一种无人船路线导航方法及设备
CN114063622A (zh) 无人船自主停泊定位方法、装置及相关组件
Ding et al. Coordinated sensing and tracking for mobile camera platforms
CN115760930A (zh) 基于多摄像头的水面目标跟踪方法、装置、计算机设备及存储介质
CN114646317A (zh) 车辆视觉定位导航控制方法、装置及计算机设备、介质
CN111207748B (zh) 基于感知信息的水面/水下无人航行器目标环绕跟踪方法
CN109164797B (zh) 一种用于船舶智能控制的航迹制导方法及系统
WO2022188174A1 (zh) 可移动平台、可移动平台的控制方法及存储介质

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant