CN114063622B - 无人船自主停泊定位方法、装置及相关组件 - Google Patents

无人船自主停泊定位方法、装置及相关组件 Download PDF

Info

Publication number
CN114063622B
CN114063622B CN202210009065.7A CN202210009065A CN114063622B CN 114063622 B CN114063622 B CN 114063622B CN 202210009065 A CN202210009065 A CN 202210009065A CN 114063622 B CN114063622 B CN 114063622B
Authority
CN
China
Prior art keywords
unmanned ship
coordinate system
marker
point cloud
point
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
CN202210009065.7A
Other languages
English (en)
Other versions
CN114063622A (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 CN202210009065.7A priority Critical patent/CN114063622B/zh
Publication of CN114063622A publication Critical patent/CN114063622A/zh
Application granted granted Critical
Publication of CN114063622B publication Critical patent/CN114063622B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • 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
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
    • G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
    • G01S13/02—Systems using reflection of radio waves, e.g. primary radar systems; Analogous systems
    • G01S13/06—Systems determining position data of a target

Abstract

本发明公开了一种无人船自主停泊定位方法、装置及相关组件。该方法包括测量标志物与返航终点的相对位置关系；使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云；以各标志物的中心点为坐标原点，建立北东坐标系，计算出无人船在北东坐标系下的坐标，并根据北东坐标系与地理坐标系的转换关系，转换得到无人船的当前经纬度坐标；以无人船的当前经纬度坐标为控制起点，控制无人船向返航终点的经纬度坐标行驶，并使得无人船在到达返航终点后停止行驶。该方法在无人船停泊场景下对船只与岸基相对位置的定位精度较高，能够基于岸基上的标志物对船只实现精准定位，确保无人船准确的停在返航终点。

Description

无人船自主停泊定位方法、装置及相关组件
技术领域
本发明涉及运载设备的感知与控制领域，尤其涉及一种无人船自主停泊定位方法、装置及相关组件。
背景技术
近年来，随着智能技术的快速发展、硬件设备的不断迭代，无人驾驶的智能化程度不断提高，当前船只的停泊方式仍然以人工控制为主，即便有部分船只实现自主停泊，也必须是在非常理想的环境下进行的，有些是要求岸基有较长的延伸臂来辅助船只停泊，有些是需要在岸基上安装大量传感器来辅助船只进行定位。如何能在减少对岸基依赖的情况下，依靠船只自身的定位系统完成停泊任务是无人船控制领域的一项技术难点，也就是说，现有的无人船的停泊定位精度较差，无法将无人船准确的停靠在指定的返航终点。
发明内容
本发明的目的是提供一种无人船自主停泊定位方法、装置及相关组件，旨在解决现有无人船的停泊定位精度差的问题。
为解决上述技术问题，本发明的目的是通过以下技术方案实现的：提供一种无人船自主停泊定位方法，其包括：
测量标志物与返航终点的相对位置关系，其中，所述标志物有多个，且布置在预设的返航区域对应的岸基上；
在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系；
以各所述标志物的中心点为坐标原点，建立北东坐标系，基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，并根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标；
以所述无人船的当前经纬度坐标为控制起点，控制所述无人船向所述返航终点的经纬度坐标行驶，并使得所述无人船在到达返航终点后停止行驶。
另外，本发明要解决的技术问题是还在于提供一种无人船自主停泊定位装置，其包括：
布置单元，用于测量标志物与返航终点的相对位置关系，其中，所述标志物有多个，且布置在预设的返航区域对应的岸基上；
检测单元，用于在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系；
转换单元，用于以各所述标志物的中心点为坐标原点，建立北东坐标系，基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，并根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标；
控制单元，用于以所述无人船的当前经纬度坐标为控制起点，控制所述无人船向所述返航终点的经纬度坐标行驶，并使得所述无人船在到达返航终点后停止行驶。
另外，本发明实施例又提供了一种计算机设备，其包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现上述第一方面所述的无人船自主停泊定位方法。
另外，本发明实施例还提供了一种计算机可读存储介质，其中所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行上述第一方面所述的无人船自主停泊定位方法。
本发明实施例公开了一种无人船自主停泊定位方法、装置及相关组件，其中，方法包括：
测量标志物与返航终点的相对位置关系，其中，所述标志物有多个，且布置在预设的返航区域对应的岸基上；在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系；以各所述标志物的中心点为坐标原点，建立北东坐标系，基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，并根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标；以所述无人船的当前经纬度坐标为控制起点，控制所述无人船向所述返航终点的经纬度坐标行驶，并使得所述无人船在到达返航终点后停止行驶。
该方法在无人船停泊场景下对船只与岸基相对位置的定位精度较高，能够基于岸基上的标志物对船只实现精准定位，确保无人船准确的停在返航终点。
附图说明
为了更清楚地说明本发明实施例技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明实施例提供的无人船自主停泊定位方法的流程示意图；
图2为本发明实施例提供的无人船自主停泊定位方法的子流程示意图；
图3为本发明实施例提供的无人船自主停泊定位装置的示意性框图；
图4为本发明实施例提供的计算机设备的示意性框图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和 “包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在此本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/ 或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
请参阅图1，图1为本发明实施例提供的无人船自主停泊定位方法的流程示意图；
如图1所示，该方法包括步骤S101~S104。
S101、测量标志物与返航终点的相对位置关系，其中，所述标志物有多个，且布置在预设的返航区域对应的岸基上；
S102、在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系；
S103、以各所述标志物的中心点为坐标原点，建立北东坐标系，基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，并根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标；
S104、以所述无人船的当前经纬度坐标为控制起点，控制所述无人船向所述返航终点的经纬度坐标行驶，并使得所述无人船在到达返航终点后停止行驶。
在本实施例中，先根据水域与岸基的实际位置情况，设定适合的返航终点，并以返航终点为中心圈划出对应的返航区域，例如可以将返航终点设置在离岸基20m的位置上，然后以返航终点为圆心，返航终点与岸基之间的距离长度为半径，得到返航区域，也可以将返航终点直接设置在岸基上，并以返航终点为中心，取如20m为半径的扇形区域为返航区域，需要说明的是，返航终点以及返航区域可以设置成其他的尺寸，只要雷达可以检测到岸基的标志物即可，故本申请不再做过多阐述。
在本实施例中，由于在无人船离岸边较远时，控制无人船停泊不需要太高的定位精度，目前的GPS信号给出的位置精度可以满足该阶段的精度需求，根据GPS得到的位置信息，使用PID算法（结合比例、积分和微分三种环节于一体的控制算法）来控制无人船向岸边的返航终点行驶，直到无人船举例返航终点20m左右的位置。
在本实施例中，雷达的种类包括但不局限于毫米波雷达，其中，本申请优先使用载频为77GHz、宽度为4GHz的毫米波雷达，该毫米波雷达有三个发射天线、四个接收天线，在距离维度上有0.04m的分辨率。
在无人船行驶到以所述返航终点为中心的返航区域后，首先利用毫米波雷达来检测岸基上的标志物，并得到各标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系；然后选择标志物的中心点作为坐标原点，建立北东坐标系，再基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，再将北东坐标系转化到经纬度坐标的地理坐标系下，即可得到无人船的相应的经纬度坐标；最后，以返航终点的经纬度坐标作为控制终点，以步骤S103计算得到的当前的经纬度坐标作为控制起点，使用PID算法控制无人船向返航终点行驶，最终完成无人船停泊任务。
本申请的无人船自主停泊定位方法在无人船停泊场景下对船只与岸基相对位置的定位精度较高，能够基于岸基上的标志物对船只实现精准定位，确保无人船准确的停在返航终点
需要注意的是，本申请各标志物的中心点即岸基上预先布置的所有标志物的中间点，例如在岸基上布置有两个标志物，则两个标志物之间的点为两者的中心点，但在一具体实施例中，所述标志物包括至少三个金属类，各所述标志物等距且共线布置在返航区域对应的岸基上，各所述标志物的摆放位置与岸基呈平行关系，需要说明的是，本申请不对标志物与返航终点之间的距离做限定，只要符合雷达的检测距离即可。
参照图2，在一实施例中，所述步骤S102包括以下步骤：
S10、在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达探测当前区域，得到当前帧中的标志物的点云；
S11、搜索点云中是否存在满足标志物几何约束条件的点云簇，将满足几何约束条件的点云簇设定为疑似标志物组合，并对其进行跟踪；其中，所述几何约束条件根据各所述标志物在岸边的摆放方式确定；
S12、统计各疑似标志物组合连续存在的点云帧数，如果所述连续存在的点云帧数达到帧数阈值时，则认定该组疑似标志物组合为真正的标志物的点云；
S13、对以上确定的标志物的点云进行跟踪，同时通过坐标系转换后输出无人船当前位置坐标。
由于岸边金属物质较多，可能会检测到多组满足几何约束条件的标志物的点云的中心点，或者最初检测到的一组中心点并不是标志物的点云的中心点，从而造成标志物检测错误，影响后续的定位精度，所以无人船行驶到以所述返航终点为中心的返航区域后，先将所有满足几何约束条件的点云簇设定为疑似标志物组合，然后统计各疑似标志物组合连续存在的点云帧数，如果所述连续存在的点云帧数达到帧数阈值时，则认定该组疑似标志物组合为真正的标志物的点云。
需要说明的是，步骤S12中帧数阈值为人为设定的，由于具体的帧数阈值需要提前在不同的航行场景进行测试得到，所以本申请不对帧数阈值进行限定，但根据历史经验可知：在较为简单的航行场景下，帧数阈值可设置为10~20帧的范围，在较为复杂的航行环境下，帧数阈值可以设置为30~40帧的范围。
具体一实施例中，所述步骤S11包括：
S110、对当前帧中的点云进行杂波滤除、点云聚类，并计算每类点云的中心点坐标；
S111、检测当前帧中是否存在满足几何约束条件的点云簇，并且将满足几何约束条件的点云簇设定为疑似标志物的点云；
S112、利用跟踪算法对疑似标志物的点云进行跟踪，并记录每组疑似标志物的点云出现的帧数。
由于雷达检测点云的稳定性较差，所以需要提高毫米波雷达检测标志物的稳定性，本申请通过先对雷达检测到的标志物的点云进行聚类来滤除杂波点云，然后利用已知的标志物摆放的几何关系来搜索标志物的点云中心满足该几何约束条件的点云，最后通过追踪算法来确认检测到的点云是否属于标志物，并且对标志物的点云进行持续跟踪，从而提高雷达检测标志物的稳定性。
需要说明的是，在所述步骤S13中是利用DBScan算法（Density-Based SpatialClustering of Applications with Noise，具有噪声的基于密度的聚类方法）聚类来滤除杂波，同时，所述几何约束条件根据各所述标志物共线摆放、相邻标志物之间的间距确定。
同时，本申请使用基于EKF的目标跟踪算法来跟踪最初检测到的满足标志物集合约束的多组点云中心点，通过记录被检测到的、满足几何约束条件的标志物的点云在后续多帧点云中出现的帧数，当这些中心点出现帧数超过预设的出现帧数阈值，且满足跟踪算法的条件时，可以确定这组标志物的点云的中心点是标志物的点云的中心点。
具体一实施例中，所述步骤S110之前包括：
S113、判断当前帧中的点云在上一帧是否被判定为真正的标志物的点云，若当前帧中的点云在上一帧被判定为真正的标志物的点云，则执行步骤S13，若当前帧中的点云在上一帧未被判定为真正的标志物的点云，则执行步骤S110。
若是在上一帧被判定为真正的标志物的点云，说明该标志物的点云中心满足该几何约束条件的点云，且连续存在的点云帧数达到帧数阈值。
具体一实施例中，所述步骤S12中检测当前帧中是否存在满足几何约束条件的点云簇，包括：
S20、按下式得到当前帧的各所述标志物的点云的中心点坐标M：
Figure DEST_PATH_IMAGE001
其中，k表示当前帧的第k类标志物的点云，n表示当前帧的第k类标志物的点云中共有n个点云，x、y分别表示雷达点云坐标系下的x轴和y轴；
S21、基于所述几何约束条件，遍历所有的中心点坐标并筛选出符合所有所述几何约束条件的中心点构成的中心点组合。
在本实施例中，对标志物的点云进行杂波滤除处理后，计算当前帧每一类点云的平均值得到各所述标志物的点云的中心点坐标，例如第k类标志物的点云有n个点云；然后利用标志物的共线摆放，并且相邻标志物之间等距的几何特点来对各标志物的点云进行几何约束检测，最终得到满足标志物摆放的几何约束条件的点云中心点，在本申请实施例中，将三个标志物放置在一条直线上，且相邻两个标志物之间的距离相等，由此可以通过遍历所有中心点的方式选出满足该几何约束条件的中心点组合。
在一具体实施例中，所述步骤S13，包括以下步骤：
S30、根据无人船的运动方式，设置无人船的状态矩阵为：
Figure 325497DEST_PATH_IMAGE002
S31、设置控制无人船的输入矩阵为：
Figure DEST_PATH_IMAGE003
S32、设置状态噪声矩阵为：
Figure 280815DEST_PATH_IMAGE004
其中，a1表示控制量给HA-t的x轴坐标添加的噪声，a2表示控制量给HA-t的y轴坐标添加的噪声；
S33、按下式对当前帧的状态估计值进行计算：
Figure DEST_PATH_IMAGE005
其中，
Figure 710134DEST_PATH_IMAGE006
表示上一帧标志物的点云的中心点坐标，
Figure DEST_PATH_IMAGE007
表示当前帧的无人船的速度；
S34、设置状态模型噪声协方差矩阵为：
Figure 321375DEST_PATH_IMAGE008
其中，q1表示x轴方向速度的方差，q2表示y轴方向速度的方差；
S35、按下式预测当前帧的状态协方差估计为：
Figure DEST_PATH_IMAGE009
S36、设置雷达的观测误差向量为：
Figure 989117DEST_PATH_IMAGE010
其中，b1表示观测量x轴坐标包含的噪声，b2表示观测量y轴坐标包含的噪声；
S37、按下式计算当前帧的观测值与状态估计值的差值：
Figure DEST_PATH_IMAGE011
其中，
Figure 267783DEST_PATH_IMAGE012
表示当前帧的标志物的点云的中心点坐标；
S38、设置雷达观测噪声协方差矩阵为：
Figure DEST_PATH_IMAGE013
其中，r1表示x轴方向观测量，r2表示y轴方向观测量；
S39、按下式计算测量残差协方差矩阵为：
Figure 342705DEST_PATH_IMAGE014
S40、按下式计算EKF的增益矩阵为：
Figure DEST_PATH_IMAGE015
S41、按下式更新状态协方差估计：
Figure 910084DEST_PATH_IMAGE016
S42、按下式计算出当前帧的所述无人船当前位置坐标：
Figure DEST_PATH_IMAGE017
需要说明的是，步骤32中w向量表示步骤S33中计算HA-t时控制量包含的噪声，其中，本申请的控制量为速度；步骤S34中的Q矩阵是相邻两帧中输入的控制量的方差，显示了控制输入量的可信度，Q矩阵中元素越大，说明输入的控制量杂声越大、越不可信；步骤S36中u向量表示步骤S37中计算
Figure 256882DEST_PATH_IMAGE018
时这一帧的观测值包含的噪声；步骤S38中R矩阵表示相邻两帧中输入的观测量即本申请检测到的中心点坐标的方差，显示了观测输入量的可信度，R矩阵中元素越大，说明输入的观测量的噪声越大、越不可信。
在本实施例中，毫米波雷达是10秒10帧，以每一帧为一个时刻计算；由于岸基上环境复杂，经常会出现在某一帧点云图中有标志物的点云未被检测到的情况，因此本申请利用标志物之间的几何约束关系来计算未检测到的标志物在雷达点云坐标系下的位置，通过对所述步骤S21中得到的各标志物的点云的中心点坐标进行跟踪，可以确定各标志物在每一帧点云中的位置，以三个标志物为例，但某个标志物未被检测到时，可以通过其他两个标志物来推测该标志物的坐标，当两个及两个以上的标志物未被检测到时，可以纯粹使用跟踪算法来预测未检测到的标志物的点云的中心点坐标，从而提高毫米波雷达检测标志物的稳定性。
在一实施例中，所述步骤S103中基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，包括以下步骤：
S50、基于获得的当前帧的各所述标志物的点云的当前位置坐标，求得各所述标志物的点云的中心点坐标；
S51、按下式将雷达点云坐标系下的原点移动至各所述标志物的点云的中心点坐标，使得无人船的中心点的坐标从（0，0）变成S1：
Figure DEST_PATH_IMAGE019
其中，（0，0）表示雷达点云坐标系的原点，
Figure 183863DEST_PATH_IMAGE020
表示在雷达点云坐标系下各所述标志物的中心点坐标；
S52、将雷达点云坐标系下的x轴进行反转，并得到更新后的无人船的中心点的坐标
Figure DEST_PATH_IMAGE021
；
S53、设无人船的船头朝向y轴，通过GPS+IMU得到无人船的船头朝向与x轴之间的夹角
Figure 630019DEST_PATH_IMAGE022
；
S54、基于得到的
Figure 809327DEST_PATH_IMAGE022
，旋转雷达点云坐标系，得到北东坐标系，其中，按下式对无人船的中心点的坐标进行转换：
Figure DEST_PATH_IMAGE023
；
其中，R表示旋转矩阵。
需要说明的是，各标志物的点云的当前位置坐标是在无人船的雷达点云坐标系下的，该坐标系是以无人船中心为原点，船前方为y轴，船右侧为x轴，以三个标志物为例，在所述步骤S42中得到三个标志物的点云的当前位置坐标即三个标志物的点云的中心点坐标，设其中中间标志物的坐标为
Figure 225396DEST_PATH_IMAGE024
，同时通过GPS+IMU估计出无人船的船头朝向与正北方向的夹角
Figure DEST_PATH_IMAGE025
。
在本实施例中，先将雷达点云坐标系的原点移动到中间标志物处，此时无人船的中心点的坐标从（0，0）变成S1；由于雷达点云坐标系下的x轴、y轴是逆时针顺序，北东坐标系下x轴、y轴是顺时针顺序，所以需要将反转x轴的坐标，此时无人船的中心点的坐标更新为S2；再根据GPS+IMU给出的夹角
Figure 469865DEST_PATH_IMAGE022
旋转反转x轴后的坐标系，得到最终在标志物的北东坐标系下无人船的中心点的坐标Smark。
在一实施例中，所述步骤S103中根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标，包括以下步骤：
S60、将所述北东坐标系绕y轴逆时针旋转
Figure 601769DEST_PATH_IMAGE026
，使所述北东坐标系的z轴与所述地理坐标系的z轴平行，并按下式更新无人船的中心点的坐标：
Figure DEST_PATH_IMAGE027
其中，
Figure 206057DEST_PATH_IMAGE028
表示各所述标志物的点云的中心点的经纬度坐标，经度为
Figure DEST_PATH_IMAGE029
，纬度为
Figure 629079DEST_PATH_IMAGE030
；
将所述北东坐标系绕z轴逆时针旋转
Figure DEST_PATH_IMAGE031
，使所述北东坐标系的三个坐标轴与所述地理坐标系的对应的三个坐标轴平行，并按下式更新无人船的中心点的坐标：
Figure 736844DEST_PATH_IMAGE032
将所述北东坐标系进行平移，使所述北东坐标系与所述地理坐标系重合，并按下式更新所述无人船的中心点的坐标：
Figure DEST_PATH_IMAGE033
将更新后的所述北东坐标系转化为极坐标系，并按下式将所述无人船的中心点的坐标S5转换成所述无人船的当前经纬度坐标：
Figure 977332DEST_PATH_IMAGE034
其中，s1、s2、s3分别为更新后的所述无人船的中心点的坐标S5在北东坐标系下的x轴、y轴、z轴的坐标位置，Re为该地区地球半径。
在本实施例中，北东坐标系中的x轴、y轴坐标是顺时针顺序，三维北东坐标系下的z轴是指向地球中心的，此时可以通过两次旋转加一次平移，从而将北东坐标系转换到以地球中心为原点的直角坐标系中，最后将该直角坐标系转化为极坐标系，其中的两个角度值就代表了经纬度。
同时，需要说明的是，在步骤S54中，R表示旋转矩阵，即表征了该坐标旋转的方式，例如步骤S60的R1表示的旋转矩阵为针对经度方向的旋转，而R2表示的旋转矩阵为针对纬度方向的旋转。另外，鉴于地球本身不是一个规范的球体，在不同地区具有不同的海拔，所以本申请为提高经纬度的计算结果，Re设置为该地区地球半径。
具体的，已知北东坐标系原点是中间标志物的坐标，同时中间标志物的经纬度坐标通过所述步骤S101得到即
Figure DEST_PATH_IMAGE035
，首先将北东坐标系绕y轴逆时针旋转
Figure 269249DEST_PATH_IMAGE036
，使北东坐标系的z轴与地理坐标系的z轴平行，此时无人船坐标从Smark变成S3，然后将绕y轴逆时针旋转后的北东坐标系绕z轴逆时针旋转
Figure DEST_PATH_IMAGE037
，使得该坐标系的三个坐标轴与地理坐标系下的三个坐标轴完全平行，此时无人船的中心点的坐标为S4，再将该坐标系平移，使其与地理坐标系重合，得到无人船的中心点的坐标S5，最后简化S5的坐标为（S1，S2，S3），将直角坐标系转化为极坐标系，得到无人船的经纬度坐标。
本发明实施例还提供一种无人船自主停泊定位装置，该无人船自主停泊定位装置用于执行前述无人船自主停泊定位方法的任一实施例。具体地，请参阅图3，图3是本发明实施例提供的无人船自主停泊定位装置的示意性框图。
如图3所示，无人船自主停泊定位装置500，包括：
布置单元501，用于测量标志物与返航终点的相对位置关系，其中，所述标志物有多个，且布置在预设的返航区域对应的岸基上；
检测单元502，用于在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系；
转换单元503，用于以各所述标志物的中心点为坐标原点，建立北东坐标系，基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，并根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标；
控制单元504，用于以所述无人船的当前经纬度坐标为控制起点，控制所述无人船向所述返航终点的经纬度坐标行驶，并使得所述无人船在到达返航终点后停止行驶。
该装置在无人船停泊场景下对船只与岸基相对位置的定位精度较高，能够基于岸基上的标志物对船只实现精准定位，确保无人船准确的停在返航终点。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
上述无人船自主停泊定位装置可以实现为计算机程序的形式，该计算机程序可以在如图4所示的计算机设备上运行。
请参阅图4，图4是本发明实施例提供的计算机设备的示意性框图。该计算机设备1100是服务器，服务器可以是独立的服务器，也可以是多个服务器组成的服务器集群。
参阅图4，该计算机设备1100包括通过系统总线1101连接的处理器1102、存储器和网络接口1105，其中，存储器可以包括非易失性存储介质1103和内存储器1104。
该非易失性存储介质1103可存储操作系统11031和计算机程序11032。该计算机程序11032被执行时，可使得处理器1102执行无人船自主停泊定位方法。
该处理器1102用于提供计算和控制能力，支撑整个计算机设备1100的运行。
该内存储器1104为非易失性存储介质1103中的计算机程序11032的运行提供环境，该计算机程序11032被处理器1102执行时，可使得处理器1102执行无人船自主停泊定位方法。
该网络接口1105用于进行网络通信，如提供数据信息的传输等。本领域技术人员可以理解，图4中示出的结构，仅仅是与本发明方案相关的部分结构的框图，并不构成对本发明方案所应用于其上的计算机设备1100的限定，具体的计算机设备1100可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。
本领域技术人员可以理解，图4中示出的计算机设备的实施例并不构成对计算机设备具体构成的限定，在其他实施例中，计算机设备可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。例如，在一些实施例中，计算机设备可以仅包括存储器及处理器，在这样的实施例中，存储器及处理器的结构及功能与图4所示实施例一致，在此不再赘述。
应当理解，在本发明实施例中，处理器1102可以是中央处理单元 (CentralProcessing Unit，CPU)，该处理器1102还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路 (Application Specific IntegratedCircuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。其中，通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
在本发明的另一实施例中提供计算机可读存储介质。该计算机可读存储介质可以为非易失性的计算机可读存储介质。该计算机可读存储介质存储有计算机程序，其中计算机程序被处理器执行时实现本发明实施例的无人船自主停泊定位方法。
所述存储介质为实体的、非瞬时性的存储介质，例如可以是U盘、移动硬盘、只读存储器（Read-Only Memory，ROM）、磁碟或者光盘等各种可以存储程序代码的实体存储介质。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的设备、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (10)

1.一种无人船自主停泊定位方法，其特征在于，包括：
测量标志物与返航终点的相对位置关系，其中，所述标志物有多个，且布置在预设的返航区域对应的岸基上；
在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系；
以各所述标志物的中心点为坐标原点，建立北东坐标系，基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，并根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标；
以所述无人船的当前经纬度坐标为控制起点，控制所述无人船向所述返航终点的经纬度坐标行驶，并使得所述无人船在到达返航终点后停止行驶。
2.根据权利要求1所述的无人船自主停泊定位方法，其特征在于，所述标志物包括至少三个金属类，各所述标志物等距且共线布置在返航区域对应的岸基上，各所述标志物的摆放位置与岸基呈平行关系。
3.根据权利要求2所述的无人船自主停泊定位方法，其特征在于，所述在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系，包括：
在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，得到当前帧中的标志物的点云；
搜索点云中是否存在满足标志物几何约束条件的点云簇，将满足几何约束条件的点云簇设定为疑似标志物组合，并对其进行跟踪；其中，所述几何约束条件根据各所述标志物在岸边的摆放方式确定；
统计各疑似标志物组合连续存在的点云帧数，如果所述连续存在的点云帧数达到帧数阈值时，则认定该组疑似标志物组合为真正的标志物的点云；
对以上确定的标志物的点云进行跟踪，同时通过坐标系转换后输出无人船当前位置坐标。
4.根据权利要求3所述的无人船自主停泊定位方法，其特征在于，所述搜索点云中是否存在满足标志物几何约束条件的点云簇，包括：
按下式得到当前帧的各所述标志物的点云的中心点坐标M：
Figure DEST_PATH_IMAGE002
其中，k表示当前帧的第k类标志物的点云，n表示当前帧的第k类标志物的点云中共有n个点云，x、y分别表示雷达点云坐标系下的x轴和y轴；
基于所述几何约束条件，遍历所有的中心点坐标并筛选出符合所有所述几何约束条件的中心点构成的中心点组合。
5.根据权利要求4所述的无人船自主停泊定位方法，其特征在于，所述对以上确定的标志物的点云进行跟踪，同时通过坐标系转换后输出无人船当前位置坐标，包括：
根据无人船的运动方式，设置无人船的状态矩阵为：
Figure DEST_PATH_IMAGE004
设置控制无人船的输入矩阵为：
Figure DEST_PATH_IMAGE006
其中，dt表示更新一次无人船位置的时间间隔；
设置状态噪声矩阵为：
Figure DEST_PATH_IMAGE008
其中，a1表示控制量给HA-t的x轴坐标添加的噪声，a2表示控制量给HA-t的y轴坐标添加的噪声；
按下式对当前帧的状态估计值进行计算：
Figure DEST_PATH_IMAGE010
其中，
Figure DEST_PATH_IMAGE012
表示上一帧标志物的点云的中心点坐标，
Figure DEST_PATH_IMAGE014
表示当前帧的无人船的速度；
设置状态模型噪声协方差矩阵为：
Figure DEST_PATH_IMAGE016
其中，q1表示x轴方向速度的方差，q2表示y轴方向速度的方差；
按下式预测当前帧的状态协方差估计为：
Figure DEST_PATH_IMAGE018
设置雷达的观测误差向量为：
Figure DEST_PATH_IMAGE020
其中，b1表示观测量x轴坐标包含的噪声，b2表示观测量y轴坐标包含的噪声；
按下式计算当前帧的观测值与状态估计值的差值：
Figure DEST_PATH_IMAGE022
其中，
Figure DEST_PATH_IMAGE024
表示当前帧的标志物的点云的中心点坐标；
设置雷达观测噪声协方差矩阵为：
Figure DEST_PATH_IMAGE026
其中，r1表示x轴方向观测量，r2表示y轴方向观测量；
按下式计算测量残差协方差矩阵为：
Figure DEST_PATH_IMAGE028
按下式计算EKF的增益矩阵为：
Figure DEST_PATH_IMAGE030
按下式更新状态协方差估计：
Figure DEST_PATH_IMAGE032
按下式计算出当前帧的所述无人船当前位置坐标：
Figure DEST_PATH_IMAGE034
。
6.根据权利要求5所述的无人船自主停泊定位方法，其特征在于，所述基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，包括：
基于获得的当前帧的各所述标志物的点云的当前位置坐标，求得各所述标志物的点云的中心点坐标；
按下式将雷达点云坐标系下的原点移动至各所述标志物的点云的中心点坐标，使得无人船的中心点的坐标从（0，0）变成S1：
Figure DEST_PATH_IMAGE036
其中，（0，0）表示雷达点云坐标系的原点，
Figure DEST_PATH_IMAGE038
表示在雷达点云坐标系下各所述标志物的中心点坐标，xpr和ypr分别为坐标Pr的横坐标与纵坐标；
将雷达点云坐标系下的x轴进行反转，并得到更新后的无人船的中心点的坐标
Figure DEST_PATH_IMAGE040
；
设无人船的船头朝向y轴，通过GPS+IMU得到无人船的船头朝向与x轴之间的夹角
Figure DEST_PATH_IMAGE042
；
基于得到的
Figure DEST_PATH_IMAGE043
，旋转雷达点云坐标系，得到北东坐标系，其中，按下式对无人船的中心点的坐标进行转换：
Figure DEST_PATH_IMAGE045
Figure DEST_PATH_IMAGE047
其中，R表示旋转矩阵。
7.根据权利要求6所述的无人船自主停泊定位方法，其特征在于，所述根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标，包括：
将所述北东坐标系绕y轴逆时针旋转
Figure DEST_PATH_IMAGE049
，使所述北东坐标系的z轴与所述地理坐标系的z轴平行，并按下式更新无人船的中心点的坐标：
Figure DEST_PATH_IMAGE051
Figure DEST_PATH_IMAGE053
其中，
Figure DEST_PATH_IMAGE055
表示各所述标志物的点云的中心点的经纬度坐标，经度为
Figure DEST_PATH_IMAGE057
，纬度为
Figure DEST_PATH_IMAGE059
；
将所述北东坐标系绕z轴逆时针旋转
Figure DEST_PATH_IMAGE061
，使所述北东坐标系的三个坐标轴与所述地理坐标系的对应的三个坐标轴平行，并按下式更新无人船的中心点的坐标：
Figure DEST_PATH_IMAGE063
Figure DEST_PATH_IMAGE065
将所述北东坐标系进行平移，使所述北东坐标系与所述地理坐标系重合，并按下式更新所述无人船的中心点的坐标：
Figure DEST_PATH_IMAGE067
Figure DEST_PATH_IMAGE069
将更新后的所述北东坐标系转化为极坐标系，并按下式将所述无人船的中心点的坐标S5转换成所述无人船的当前经纬度坐标：
Figure DEST_PATH_IMAGE071
其中，s1、s2、s3分别为更新后的所述无人船的中心点的坐标S5在北东坐标系下的x轴、y轴、z轴的坐标位置，Re为该地区地球半径。
8.一种无人船自主停泊定位装置，其特征在于，包括：
布置单元，用于测量标志物与返航终点的相对位置关系，其中，所述标志物有多个，且布置在预设的返航区域对应的岸基上；
检测单元，用于在控制无人船行驶到以所述返航终点为中心的返航区域后，使用雷达检测每一标志物，并得到对应标志物在雷达点云坐标系下的点云，以确定各所述标志物的中心点与无人船的相对位置关系；
转换单元，用于以各所述标志物的中心点为坐标原点，建立北东坐标系，基于各所述标志物的中心点与无人船的相对位置关系，计算出所述无人船在所述北东坐标系下的坐标，并根据所述北东坐标系与地理坐标系的转换关系，转换得到所述无人船的当前经纬度坐标；
控制单元，用于以所述无人船的当前经纬度坐标为控制起点，控制所述无人船向所述返航终点的经纬度坐标行驶，并使得所述无人船在到达返航终点后停止行驶。
9.一种计算机设备，包括存储器、处理器及存储在所述存储器上并可在所述处理器上运行的计算机程序，其特征在于，所述处理器执行所述计算机程序时实现如权利要求1至7中任一项所述的无人船自主停泊定位方法。
10.一种计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机程序，所述计算机程序当被处理器执行时使所述处理器执行如权利要求1至7任一项所述的无人船自主停泊定位方法。
CN202210009065.7A 2022-01-06 2022-01-06 无人船自主停泊定位方法、装置及相关组件 Active CN114063622B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202210009065.7A CN114063622B (zh) 2022-01-06 2022-01-06 无人船自主停泊定位方法、装置及相关组件

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202210009065.7A CN114063622B (zh) 2022-01-06 2022-01-06 无人船自主停泊定位方法、装置及相关组件

Publications (2)

Publication Number Publication Date
CN114063622A CN114063622A (zh) 2022-02-18
CN114063622B true CN114063622B (zh) 2022-05-17

Family

ID=80230653

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202210009065.7A Active CN114063622B (zh) 2022-01-06 2022-01-06 无人船自主停泊定位方法、装置及相关组件

Country Status (1)

Country Link
CN (1) CN114063622B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114879685B (zh) * 2022-05-25 2023-04-28 合肥工业大学 一种用于无人船的河岸线检测及自主巡航方法

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110275153A (zh) * 2019-07-05 2019-09-24 上海大学 一种基于激光雷达的水面目标检测与跟踪方法
CN111427979A (zh) * 2020-01-15 2020-07-17 深圳市镭神智能系统有限公司 基于激光雷达的动态地图构建方法、系统及介质
CN111679669A (zh) * 2020-06-01 2020-09-18 陕西欧卡电子智能科技有限公司 一种无人船自主精准停靠方法及系统
CN113050121A (zh) * 2021-03-22 2021-06-29 上海海事大学 一种船舶导航系统和船舶导航方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2018112045A1 (en) * 2016-12-13 2018-06-21 Oceaneering International, Inc. System and method for using a combination of multiple autonomous vehicles with different abilities, working together as a system for subsea oil and gas exploration
CN109443356A (zh) * 2019-01-07 2019-03-08 大连海事大学 一种含测量噪声的无人船位置与速度估计结构及设计方法
CN109828570B (zh) * 2019-02-18 2021-10-01 哈尔滨工程大学 一种自适应边界层水面无人艇控制导引方法
CN113433933B (zh) * 2021-04-16 2022-05-10 安徽中科合鼎科技发展有限公司 一种智能水面保洁无人船的自主巡航方法

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110275153A (zh) * 2019-07-05 2019-09-24 上海大学 一种基于激光雷达的水面目标检测与跟踪方法
CN111427979A (zh) * 2020-01-15 2020-07-17 深圳市镭神智能系统有限公司 基于激光雷达的动态地图构建方法、系统及介质
CN111679669A (zh) * 2020-06-01 2020-09-18 陕西欧卡电子智能科技有限公司 一种无人船自主精准停靠方法及系统
CN113050121A (zh) * 2021-03-22 2021-06-29 上海海事大学 一种船舶导航系统和船舶导航方法

Also Published As

Publication number Publication date
CN114063622A (zh) 2022-02-18

Similar Documents

Publication Publication Date Title
CN107782321B (zh) 一种基于视觉和高精度地图车道线约束的组合导航方法
JP6504316B2 (ja) 走行車線推定システム
US9978161B2 (en) Supporting a creation of a representation of road geometry
US11525682B2 (en) Host vehicle position estimation device
CN109059964B (zh) 一种基于“重力峰”的惯性导航与重力测量双校准方法
US11555705B2 (en) Localization using dynamic landmarks
CN114046792B (zh) 一种无人船水面定位与建图方法、装置及相关组件
CN114111774B (zh) 车辆的定位方法、系统、设备及计算机可读存储介质
JP7113134B2 (ja) 車両制御装置
CN113933818A (zh) 激光雷达外参的标定的方法、设备、存储介质及程序产品
CN114063622B (zh) 无人船自主停泊定位方法、装置及相关组件
CN110969643A (zh) 地面目标移动轨迹的星上自主预测方法
CN112859133A (zh) 一种基于雷达与北斗数据的船舶深度融合定位方法
CN115164936A (zh) 高精地图制作中用于点云拼接的全局位姿修正方法及设备
CN101464155B (zh) 航位推算中初始方向的确定方法
CN112764074B (zh) 一种定位导航轨迹的方法、装置及电子设备
RU2617147C1 (ru) Способ начального ориентирования гироскопической навигационной системы для наземных подвижных объектов
WO2005066652A1 (en) Method and system for mobile location
JPS63221217A (ja) 相対位置ナビゲーションシステム
CN111123323B (zh) 一种提高便携设备定位精度的方法
CN114897942A (zh) 点云地图的生成方法、设备及相关存储介质
Niknejad et al. Multi-sensor data fusion for autonomous vehicle navigation and localization through precise map
CN110057383B (zh) 一种auv推位导航系统杆臂误差标校方法
Banachowicz et al. A comparison of the least squares with Kalman filter methods used in algorithms of fusion with dead reckoning navigation data
JP2022098635A (ja) 自車位置信頼度演算装置、自車位置信頼度演算方法、車両制御装置、及び車両制御方法

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