CN115629374B - 基于毫米波雷达的无人船桥下定位方法及相关设备 - Google Patents

基于毫米波雷达的无人船桥下定位方法及相关设备 Download PDF

Info

Publication number
CN115629374B
CN115629374B CN202211620321.2A CN202211620321A CN115629374B CN 115629374 B CN115629374 B CN 115629374B CN 202211620321 A CN202211620321 A CN 202211620321A CN 115629374 B CN115629374 B CN 115629374B
Authority
CN
China
Prior art keywords
bridge
ship
positioning result
point cloud
calculating
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
CN202211620321.2A
Other languages
English (en)
Other versions
CN115629374A (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 CN202211620321.2A priority Critical patent/CN115629374B/zh
Publication of CN115629374A publication Critical patent/CN115629374A/zh
Application granted granted Critical
Publication of CN115629374B publication Critical patent/CN115629374B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • 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/88—Radar or analogous systems specially adapted for specific applications
    • 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
    • G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
    • G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00

Abstract

本发明实施例公开了一种基于毫米波雷达的无人船桥下定位方法及相关设备，方法包括：基于毫米波雷达点云计算出桥下船只的方向角；基于毫米波雷达点云推算出无人船的初步定位结果；对无人船的初步定位结果进行优化，以得到无人船的最终定位结果。本发明基于毫米波雷达的无人船桥下定位，实现了高精度的无人船的桥下定位，由于毫米波雷达对天气、光照等条件的鲁棒性，该定位方式可适应于不同的天气情况，且成本低廉，具有较高的应用价值。

Description

基于毫米波雷达的无人船桥下定位方法及相关设备
技术领域
本发明涉及无人船定位技术领域，更具体地说是基于毫米波雷达的无人船桥下定位方法及相关设备。
背景技术
近年来，无人船被广泛应用于水环境监测、水面垃圾收集、水面运输、水面巡检等领域，与无人船相关的技术也取得了较大进步。对于无人船的智能运行，实时准确的定位有着至关重要的作用。传统的无人船定位主要依赖于卫星定位系统，然而，随着无人船的应用场景从海面、海岸区域，扩展至城市内河、湖泊，无人船的运行场景变得更为复杂。城市内河中，无人船行驶至桥下时，卫星定位信号将被桥梁阻隔，卫星信号减弱，卫星定位结果不可用，此时，就需要依赖于其他传感器完成桥下定位。
常用的辅助定位的传感器为视觉传感器、激光雷达、轮速计、惯性测量单元（IMU）等，然而，桥下光线昏暗，对视觉传感器的应用很不友好；同时，平整、狭长的桥梁也对基于2D和3D激光雷达点云的定位方式，带来了巨大挑战，导致定位结果的退化；轮速记在水面无法使用；IMU的累积误差则使得单独依赖IMU的定位方式也不可用。
发明内容
本发明的目的在于克服现有技术的不足，提供一种基于毫米波雷达的无人船桥下定位方法及相关设备，以能够适用于不同的天气状况，提升鲁棒性与定位精度。
为实现上述目的，本发明采用以下技术方案：
第一方面，基于毫米波雷达的无人船桥下定位方法，包括：
基于毫米波雷达点云计算出桥下船只的方向角；
基于毫米波雷达点云推算出无人船的初步定位结果；
对无人船的初步定位结果进行优化，以得到无人船的最终定位结果。
其进一步技术方案为：所述基于毫米波雷达点云计算出桥下船只的方向角，包括：
对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率；
根据桥梁两侧的直线的斜率推算出船只与桥梁方向的夹角。
其进一步技术方案为：所述对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率，包括：
采用随机采样一致性算法，结合最小二乘拟合，拟合出单侧桥梁直线分布曲线；
从单侧桥梁直线分布曲线中筛选出单侧桥梁点云；
利用单侧桥梁点云拟合出桥梁的另一单侧桥梁点云。
其进一步技术方案为：所述基于毫米波雷达点云推算出无人船的初步定位结果，包括：
基于毫米波雷达点云的多普勒信息推算出船只的速度；
根据船只的速度和船只的方向角计算得出无人船的初步定位结果。
其进一步技术方案为：所述根据船只的速度和船只的方向角计算得出无人船的初步定位结果，包括：
根据船只的速度和船只的方向角计算出船只在当前固定时间差内的位移；
将当前固定时间差内的位移的计算结果叠加至上一帧对应的位移结果，以得到当前帧的初步定位结果。
其进一步技术方案为：所述对无人船的初步定位结果进行优化，以得到无人船的最终定位结果，包括：
根据两侧桥梁的点云集合以及对应的拟合直线结果，识别出桥梁特征；
根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果。
其进一步技术方案为：所述根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果，包括：
计算船只距离桥梁两侧侧边的垂直距离，以得到侧方距离；
利用得到的侧方距离代替初步定位结果中的X方向的定位结果，以得到最终的X方向的定位结果；
计算当前帧与上一帧的相对位姿初值；
采用相对位姿初值进行点云配准，以得到相对位姿计算值；
基于得到的相对位姿计算值更新初步定位结果中的Y方向的定位结果，以得到最终的Y方向的定位结果。
第二方面，基于毫米波雷达的无人船桥下定位装置，包括第一计算单元、第二计算单元以及优化单元；
所述第一计算单元，用于基于毫米波雷达点云计算出桥下船只的方向角；
所述第二计算单元，用于基于毫米波雷达点云推算出无人船的初步定位结果；
所述优化单元，用于对无人船的初步定位结果进行优化，以得到无人船的最终定位结果。
第三方面，一种计算机设备，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如上述的基于毫米波雷达的无人船桥下定位方法。
第四方面，一种计算机可读存储介质，所述存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令被处理器执行时，使得所述处理器执行如上述的基于毫米波雷达的无人船桥下定位方法。
本发明与现有技术相比的有益效果是：本发明通过基于毫米波雷达点云计算出桥下船只的方向角；基于毫米波雷达点云推算出无人船的初步定位结果；对无人船的初步定位结果进行优化，以得到无人船的最终定位结果。基于毫米波雷达的无人船桥下定位，实现了高精度的无人船的桥下定位，由于毫米波雷达对天气、光照等条件的鲁棒性，该定位方式可适应于不同的天气情况，且成本低廉，具有较高的应用价值。
上述说明仅是本发明技术方案的概述，为了能够更清楚了解本发明技术手段，可依照说明书的内容予以实施，并且为了让本发明的上述和其它目的、特征及优点能够更明显易懂，以下特举较佳实施例，详细说明如下。
附图说明
为了更清楚地说明本发明实施例技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动的前提下，还可以根据这些附图获得其他的附图。
图1为本发明具体实施例提供的基于毫米波雷达的无人船桥下定位方法应用场景示意图；
图2为本发明具体实施例提供的基于毫米波雷达的无人船桥下定位方法的流程图；
图3为本发明具体实施例提供的基于毫米波雷达的无人船桥下定位装置的示意性框图；
图4为本发明具体实施例提供的一种计算机设备的示意性框图；
图5为本发明具体实施例提供的单侧桥梁点云示意图；
图6为本发明具体实施例提供的双侧桥梁点云示意图；
图7为本发明具体实施例提供的单个毫米波雷达点多普勒速度与船只速度以及点坐标的关系示意图；
图8为本发明具体实施例提供的桥内船只方向角与速度和位移的转换关系示意图；
图9为本发明具体实施例提供的不同类型桥梁点云示意图；
图10为本发明具体实施例提供的船只距离桥侧方垂直距离示意图。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
应当理解，当在本说明书和所附权利要求书中使用时，术语“包括”和 “包含”指示所描述特征、整体、步骤、操作、元素和/或组件的存在，但并不排除一个或多个其它特征、整体、步骤、操作、元素、组件和/或其集合的存在或添加。
还应当理解，在本发明说明书中所使用的术语仅仅是出于描述特定实施例的目的而并不意在限制本发明。如在本发明说明书和所附权利要求书中所使用的那样，除非上下文清楚地指明其它情况，否则单数形式的“一”、“一个”及“该”意在包括复数形式。
还应当进一步理解，在本发明说明书和所附权利要求书中使用的术语“和/ 或”是指相关联列出的项中的一个或多个的任何组合以及所有可能组合，并且包括这些组合。
请参阅图1和图2，图1为本发明实施例提供的基于毫米波雷达的无人船桥下定位方法的应用场景示意图；图2为本发明实施例提供的基于毫米波雷达的无人船桥下定位方法的流程示意图，毫米波雷达将数据发送至服务器，该基于毫米波雷达的无人船桥下定位方法应用于服务器中，该方法通过安装于服务器中的应用软件进行执行。
如图2所示，基于毫米波雷达的无人船桥下定位方法，包括以下步骤：S10-S30。
S10、基于毫米波雷达点云计算出桥下船只的方向角。
由于桥下空间狭窄，毫米波雷达点云内会出现较多杂波干扰，因此，首先对桥下毫米波雷达点云进行桥梁点云提取，去除雷达点云中的杂波，保留有用的桥梁点信息。一般较长长度的桥梁，内部或为沿着河道的平整的桥面，或为沿着河道方向的数个桥墩，分布均为两条平行的直线，利用此特征对桥梁点云进行提取。
在一实施例中，步骤S10具体包括以下步骤：S101-S102。
S101、对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率。
在一实施例中，步骤S101具体包括以下步骤：S1011-S1013。
S1011、采用随机采样一致性算法，结合最小二乘拟合，拟合出单侧桥梁直线分布曲线。
S1012、从单侧桥梁直线分布曲线中筛选出单侧桥梁点云。
在本实施例中，设一帧毫米波雷达点云数据表示为
Figure 838489DEST_PATH_IMAGE001
，
Figure 11981DEST_PATH_IMAGE002
，其中
Figure 429187DEST_PATH_IMAGE003
表示
Figure 662722DEST_PATH_IMAGE001
中的一个点云，对每个点云
Figure 24433DEST_PATH_IMAGE003
，
Figure 876983DEST_PATH_IMAGE004
，其中
Figure 273329DEST_PATH_IMAGE005
分别表示当 前点的
Figure 943345DEST_PATH_IMAGE006
坐标，以及多普勒速度（多普勒速度即为目标相对船的径向速度速度）。毫米 波雷达安装在船只前侧，
Figure 402139DEST_PATH_IMAGE007
方向为船只右方，
Figure 183013DEST_PATH_IMAGE008
方向为船只前方，
Figure 433866DEST_PATH_IMAGE009
方向为船只上方。
设当前
Figure 9204DEST_PATH_IMAGE010
时刻，单侧桥梁的点云直线，在雷达坐标系下可以表示为
Figure 220874DEST_PATH_IMAGE011
，其中斜率
Figure 539860DEST_PATH_IMAGE012
反映了桥梁和船头的夹角，
Figure 645219DEST_PATH_IMAGE012
与
Figure 266824DEST_PATH_IMAGE013
共同反映了侧面 桥梁与船只的距离。由于点云中，部分为杂波点，部分为桥梁点，因此，采用随机采样一致性 算法，即RANSAC算法，结合最小二乘拟合，该算法可从含有干扰噪声数据中，拟合出期望形 式的曲线。利用该算法，可以同时拟合出单侧桥梁直线分布，同时筛选出单侧桥梁点云。
如图5所示，图中未填充的点为提取出的桥梁点云
Figure 90424DEST_PATH_IMAGE014
，该部分点云与拟合出的
Figure 213100DEST_PATH_IMAGE011
直线距离不超过设定的阈值
Figure 313912DEST_PATH_IMAGE015
；带填充的点为外点
Figure 231052DEST_PATH_IMAGE016
，该部分点 云与拟合出的
Figure 276368DEST_PATH_IMAGE011
直线距离超过了设定的阈值
Figure 937157DEST_PATH_IMAGE015
。
S1013、利用单侧桥梁点云拟合出桥梁的另一单侧桥梁点云。
在本实施例中，通过上述计算得到了单侧点云拟合出的直线
Figure 155124DEST_PATH_IMAGE011
，以 及该直线的外点集合
Figure 243166DEST_PATH_IMAGE016
。由于两侧桥面相互平行，因此，依据拟合出的斜率
Figure 775778DEST_PATH_IMAGE012
，在外点集 合
Figure 115624DEST_PATH_IMAGE016
中，利用RANSAC算法，再拟合出另一侧点云直线
Figure 50082DEST_PATH_IMAGE017
。如图6所示，即在 上述得到的外点集合
Figure 309025DEST_PATH_IMAGE016
中，提取出了另一侧桥梁点云
Figure 469879DEST_PATH_IMAGE018
。
S102、根据桥梁两侧的直线的斜率推算出船只与桥梁方向的夹角。
在本实施例中，由上述拟合得到的两侧桥梁直线的斜率
Figure 738049DEST_PATH_IMAGE019
, 即可推算得出船只 与桥方向的夹角
Figure 527014DEST_PATH_IMAGE020
，也即船只的方向角。设
Figure 956858DEST_PATH_IMAGE021
为
Figure 605008DEST_PATH_IMAGE007
方向与桥走向方向的 夹角，则
Figure 411290DEST_PATH_IMAGE022
。
S20、基于毫米波雷达点云推算出无人船的初步定位结果。
假设桥下目标均为静止，仅有船只本身运动，则毫米波雷达点云中，由每个点的多普勒信息与点的位置之间的关系，可推到出船只本身的运动情况，
在一实施例中，步骤S20具体包括以下步骤：S201-S202。
S201、基于毫米波雷达点云的多普勒信息推算出船只的速度。
在本实施例中，单个点云
Figure 54761DEST_PATH_IMAGE004
，其中
Figure 796452DEST_PATH_IMAGE023
为点云在雷达坐标系 的坐标，
Figure 525374DEST_PATH_IMAGE024
为点云的多普勒速度，设该点云与
Figure 135347DEST_PATH_IMAGE025
点的连线与
Figure 633324DEST_PATH_IMAGE008
轴负向的夹角为
Figure 811496DEST_PATH_IMAGE026
，
Figure 27713DEST_PATH_IMAGE027
。设此时船速度为
Figure 175798DEST_PATH_IMAGE028
，与
Figure 669227DEST_PATH_IMAGE008
轴正向的夹角为
Figure 877355DEST_PATH_IMAGE029
，如图7所示，多普勒速 度与船只运动速度的大小关系为：
Figure 518552DEST_PATH_IMAGE030
；反之，可由多个点的多普勒速度以及坐标，拟合出 船只的速度
Figure 470327DEST_PATH_IMAGE028
（包括速度大小
Figure 821193DEST_PATH_IMAGE031
以及速度的朝向
Figure 465801DEST_PATH_IMAGE029
）。拟合方法可使用现有的拟合算法， 本实施例中，采用采样非线性最小二乘法对其进行拟合，可得出船只速度大小
Figure 656611DEST_PATH_IMAGE031
以及速 度的朝向
Figure 21864DEST_PATH_IMAGE029
。
S202、根据船只的速度和船只的方向角计算得出无人船的初步定位结果。
在一实施例中，步骤S202具体包括以下步骤：S2021-S2022。
S2021、根据船只的速度和船只的方向角计算出船只在当前固定时间差内的位移。
在本实施例中，通过上述计算得到了船只速度大小
Figure 614519DEST_PATH_IMAGE031
以及速度的朝向
Figure 898870DEST_PATH_IMAGE029
，基于 此，结合计算的方向角
Figure 576976DEST_PATH_IMAGE020
，如图8所示，可计算出船只在固定时间差
Figure 745920DEST_PATH_IMAGE032
内的位移：
Figure 193082DEST_PATH_IMAGE033
其中，
Figure 179493DEST_PATH_IMAGE028
为计算得到的
Figure 954682DEST_PATH_IMAGE034
时刻的船只速度，
Figure 786372DEST_PATH_IMAGE028
代表船只速度大小，
Figure 353619DEST_PATH_IMAGE029
为计算得到 的船只当前方向角，
Figure 245352DEST_PATH_IMAGE032
代表当前帧毫米波雷达数据对应时刻与上一帧毫米波雷达数据对 应时刻之间的时间差。上式将船只位移投影到桥梁区域对应的坐标系的坐标轴
Figure 773416DEST_PATH_IMAGE035
与
Figure 408797DEST_PATH_IMAGE036
上，得到了船只在
Figure 830551DEST_PATH_IMAGE032
时间内，在
Figure 768551DEST_PATH_IMAGE035
与
Figure 642966DEST_PATH_IMAGE036
方向上分别的位移。
S2022、将当前固定时间差内的位移的计算结果叠加至上一帧对应的位移结果，以得到当前帧的初步定位结果。
在本实施例中，将当前位移计算结果，叠加至上一帧定位结果
Figure 82038DEST_PATH_IMAGE037
，即 可得到当前帧初步定位结果
Figure 233665DEST_PATH_IMAGE038
，即
Figure 467200DEST_PATH_IMAGE039
。其中
Figure 828911DEST_PATH_IMAGE037
代表上一帧，即
Figure 806094DEST_PATH_IMAGE040
对应时刻，船只在
Figure 812228DEST_PATH_IMAGE041
坐标系下对应的位移，
Figure 216664DEST_PATH_IMAGE042
为船只在
Figure 331251DEST_PATH_IMAGE032
时间内，在
Figure 781299DEST_PATH_IMAGE035
与
Figure 641939DEST_PATH_IMAGE036
方向上分别的位移，该位移叠加在
Figure 217277DEST_PATH_IMAGE037
上，得到即是当前帧 初步定位结果
Figure 553580DEST_PATH_IMAGE043
。
S30、对无人船的初步定位结果进行优化，以得到无人船的最终定位结果。
由于里程计存在累积误差，因此，通过对桥梁特征信息的提取与判别，基于初步定 位结果
Figure 13511DEST_PATH_IMAGE043
，来优化桥下定位结果。
在一实施例中，步骤S30具体包括以下步骤：S301-S302。
S301、根据两侧桥梁的点云集合以及对应的拟合直线结果，识别出桥梁特征。
在本实施例中， 在上述得到了两侧桥梁的点云集合
Figure 118871DEST_PATH_IMAGE014
和
Figure 865110DEST_PATH_IMAGE018
，以及对应的拟合 直线结果
Figure 688709DEST_PATH_IMAGE011
和
Figure 421173DEST_PATH_IMAGE017
，接下来，首先识别桥梁特征。
具体的，将两侧桥梁点云，分别投影至拟合出的桥梁直线上，得到每个点在拟合出 的直线上的投影坐标
Figure 646618DEST_PATH_IMAGE044
。对
Figure 829338DEST_PATH_IMAGE044
，利用基于密度的空间聚类算法 DBSCAN，采用一定的聚类参数进行聚类。得到的聚类团数目大于设定阈值
Figure 750020DEST_PATH_IMAGE045
，则认为该 桥梁为多个分离桥墩支撑的，否则认为该桥梁为竖直墙面，两种桥的点云示意图分别如图9 所示。
S302、根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果。
在一实施例中，步骤S302具体包括以下步骤：
S3021、计算船只距离桥梁两侧侧边的垂直距离，以得到侧方距离。
S3022、利用得到的侧方距离代替初步定位结果中的X方向的定位结果，以得到最终的X方向的定位结果。
S3023、计算当前帧与上一帧的相对位姿初值。
S3024、采用相对位姿初值进行点云配准，以得到相对位姿计算值。
S3025、基于得到的相对位姿计算值更新初步定位结果中的Y方向的定位结果， 以得到最终的Y方向的定位结果。
在本实施例中，侧向定位结果优化如下：
上述得到的拟合直线结果
Figure 410809DEST_PATH_IMAGE011
和
Figure 490760DEST_PATH_IMAGE017
，如图10所示，可 推算出船只距离桥梁两侧侧边的垂直距离
Figure 578802DEST_PATH_IMAGE046
，
Figure 252360DEST_PATH_IMAGE047
；
Figure 716839DEST_PATH_IMAGE048
；
由于该侧方距离结果为实时计算，不存在累积误差，因此，直接用到左侧桥壁的侧 方距离
Figure 651297DEST_PATH_IMAGE046
，替代上述计算的
Figure 785607DEST_PATH_IMAGE007
方向定位结果
Figure 805515DEST_PATH_IMAGE049
，从而得到最终的
Figure 808106DEST_PATH_IMAGE007
方向定位结果
Figure 738016DEST_PATH_IMAGE050
。
前向定位结果优化如下：
由于上述识别出为竖直墙面的桥，其在
Figure 433440DEST_PATH_IMAGE008
方向为一列点云，不具备结构性特征，无 法进行
Figure 940644DEST_PATH_IMAGE008
方向定位结果的优化，其最终定位结果即为
Figure 481347DEST_PATH_IMAGE051
。初始时刻
Figure 545991DEST_PATH_IMAGE052
时，
Figure 146737DEST_PATH_IMAGE053
，其中
Figure 141238DEST_PATH_IMAGE054
为
Figure 360997DEST_PATH_IMAGE052
时刻，计算出的点云至侧方桥壁距 离。
上述识别出为多个分离桥墩支撑的桥，可利用桥墩的分布情况，对
Figure 124554DEST_PATH_IMAGE008
方向定位结 果进行优化。
具体地，首先，基于当前计算出的位姿，包括
Figure 161780DEST_PATH_IMAGE007
，
Figure 518943DEST_PATH_IMAGE008
方向的位移以及方向角
Figure 667028DEST_PATH_IMAGE055
与上一帧位姿
Figure 285091DEST_PATH_IMAGE056
，计算当前帧与上一帧之 间的相对位姿初值
Figure 493219DEST_PATH_IMAGE057
。
然后对当前帧
Figure 337678DEST_PATH_IMAGE010
时刻内点集合
Figure 23874DEST_PATH_IMAGE058
，与上一帧
Figure 762023DEST_PATH_IMAGE040
时刻点云内点集合
Figure 16418DEST_PATH_IMAGE059
，利用常用的点云配准算法，例如ICP算法与NDT算法，进行点云配准,配准中 需要设定的相对位姿初值，即为上述得到的
Figure 207228DEST_PATH_IMAGE057
。通过配准，得到 ICP的相对位姿计算值
Figure 697115DEST_PATH_IMAGE060
。
最后基于ICP得到的
Figure 165137DEST_PATH_IMAGE060
，更新
Figure 715067DEST_PATH_IMAGE008
方向定位结果，即
Figure 393173DEST_PATH_IMAGE061
，由此，即得到了当前时刻优化后的定位结果
Figure 686751DEST_PATH_IMAGE062
。
本发明基于毫米波雷达的无人船桥下定位，实现了高精度的无人船的桥下定位，由于毫米波雷达对天气、光照等条件的鲁棒性，该定位方式可适应于不同的天气情况，且成本低廉，具有较高的应用价值。
图3是本发明实施例提供的基于毫米波雷达的无人船桥下定位装置100的示意性框图。对应于上述的基于毫米波雷达的无人船桥下定位方法，本发明具体实施例还提供了一种基于毫米波雷达的无人船桥下定位装置100。该基于毫米波雷达的无人船桥下定位装置100包括用于执行上述基于毫米波雷达的无人船桥下定位方法的单元，该装置可以被配置于服务器中。
如图3所示，基于毫米波雷达的无人船桥下定位装置100，包括第一计算单元110、第二计算单元120以及优化单元130。
第一计算单元110，用于基于毫米波雷达点云计算出桥下船只的方向角。
在一实施例中，第一计算单元110包括提取模块以及第一计算模块。
提取模块，用于对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率。
在一实施例中，提取模块包括第一拟合子模块、筛选子模块以及第二拟合子模块。
第一拟合子模块，用于采用随机采样一致性算法，结合最小二乘拟合，拟合出单侧桥梁直线分布曲线。
筛选子模块，用于从单侧桥梁直线分布曲线中筛选出单侧桥梁点云。
第二拟合子模块，用于利用单侧桥梁点云拟合出桥梁的另一单侧桥梁点云
第一计算模块，用于根据桥梁两侧的直线的斜率推算出船只与桥梁方向的夹角。
第二计算单元120，用于基于毫米波雷达点云推算出无人船的初步定位结果。
在一实施例中，第二计算单元120包括第二计算模块以及第三计算模块。
第二计算模块，用于基于毫米波雷达点云的多普勒信息推算出船只的速度。
第三计算模块，用于根据船只的速度和船只的方向角计算得出无人船的初步定位结果。
在一实施例中，第三计算模块包括第一计算子模块以及第二计算子模块。
第一计算子模块，用于根据船只的速度和船只的方向角计算出船只在当前固定时间差内的位移。
第二计算子模块，用于将当前固定时间差内的位移的计算结果叠加至上一帧对应的位移结果，以得到当前帧的初步定位结果。
优化单元130，用于对无人船的初步定位结果进行优化，以得到无人船的最终定位结果。
在一实施例中，优化单元130包括识别模块以及优化模块。
识别模块，用于根据两侧桥梁的点云集合以及对应的拟合直线结果，识别出桥梁特征。
优化模块，用于根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果。
在一实施例中，优化模块包括第三计算子模块、第一更新子模块、第四计算子模块、点云配准子模块以及第二更新子模块。
第三计算子模块，用于计算船只距离桥梁两侧侧边的垂直距离，以得到侧方距离。
第一更新子模块，用于利用得到的侧方距离代替初步定位结果中的X方向的定位结果，以得到最终的X方向的定位结果。
第四计算子模块，用于计算当前帧与上一帧的相对位姿初值。
点云配准子模块，用于采用相对位姿初值进行点云配准，以得到相对位姿计算值。
点云配准子模块，用于基于得到的相对位姿计算值更新初步定位结果中的Y方向的定位结果，以得到最终的Y方向的定位结果。
上述基于毫米波雷达的无人船桥下定位装置可以实现为计算机程序的形式，该计算机程序可以在如图4所示的计算机设备上运行。
请参阅图4，图4是本申请实施例提供的一种计算机设备的示意性框图。该计算机设备700可以是服务器，其中，服务器可以是独立的服务器，也可以是多个服务器组成的服务器集群。
如图4所示，该计算机设备，包括存储器、处理器以及存储在存储器上并可在处理器上运行的计算机程序，处理器执行计算机程序时实现如上述的基于毫米波雷达的无人船桥下定位方法步骤。
该计算机设备700可以是终端或服务器。该计算机设备700包括通过系统总线710连接的处理器720、存储器和网络接口750，其中，存储器可以包括非易失性存储介质730和内存储器740。
该非易失性存储介质730可存储操作系统731和计算机程序732。该计算机程序732被执行时，可使得处理器720执行任意一种基于毫米波雷达的无人船桥下定位方法。
该处理器720用于提供计算和控制能力，支撑整个计算机设备700的运行。
该内存储器740为非易失性存储介质730中的计算机程序732的运行提供环境，该计算机程序732被处理器720执行时，可使得处理器720执行任意一种基于毫米波雷达的无人船桥下定位方法。
该网络接口750用于进行网络通信，如发送分配的任务等。本领域技术人员可以理解，图4中示出的结构，仅仅是与本申请方案相关的部分结构的框图，并不构成对本申请方案所应用于其上的计算机设备700的限定，具体的计算机设备700可以包括比图中所示更多或更少的部件，或者组合某些部件，或者具有不同的部件布置。其中，所述处理器720用于运行存储在存储器中的程序代码，以实现以下步骤：
第一方面，基于毫米波雷达的无人船桥下定位方法，包括：
基于毫米波雷达点云计算出桥下船只的方向角；
基于毫米波雷达点云推算出无人船的初步定位结果；
对无人船的初步定位结果进行优化，以得到无人船的最终定位结果。
在一实施例中：所述基于毫米波雷达点云计算出桥下船只的方向角，包括：
对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率；
根据桥梁两侧的直线的斜率推算出船只与桥梁方向的夹角。
在一实施例中：所述对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率，包括：
采用随机采样一致性算法，结合最小二乘拟合，拟合出单侧桥梁直线分布曲线；
从单侧桥梁直线分布曲线中筛选出单侧桥梁点云；
利用单侧桥梁点云拟合出桥梁的另一单侧桥梁点云。
在一实施例中：所述基于毫米波雷达点云推算出无人船的初步定位结果，包括：
基于毫米波雷达点云的多普勒信息推算出船只的速度；
根据船只的速度和船只的方向角计算得出无人船的初步定位结果。
在一实施例中：所述根据船只的速度和船只的方向角计算得出无人船的初步定位结果，包括：
根据船只的速度和船只的方向角计算出船只在当前固定时间差内的位移；
将当前固定时间差内的位移的计算结果叠加至上一帧对应的位移结果，以得到当前帧的初步定位结果。
在一实施例中：所述对无人船的初步定位结果进行优化，以得到无人船的最终定位结果，包括：
根据两侧桥梁的点云集合以及对应的拟合直线结果，识别出桥梁特征；
根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果。
在一实施例中：所述根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果，包括：
计算船只距离桥梁两侧侧边的垂直距离，以得到侧方距离；
利用得到的侧方距离代替初步定位结果中的X方向的定位结果，以得到最终的X方向的定位结果；
计算当前帧与上一帧的相对位姿初值；
采用相对位姿初值进行点云配准，以得到相对位姿计算值；
基于得到的相对位姿计算值更新初步定位结果中的Y方向的定位结果，以得到最终的Y方向的定位结果。
应当理解，在本申请实施例中，处理器720可以是中央处理单元 (CentralProcessing Unit，CPU)，该处理器720还可以是其他通用处理器、数字信号处理器(Digital Signal Processor，DSP)、专用集成电路 (Application Specific IntegratedCircuit，ASIC)、现成可编程门阵列 (Field-Programmable Gate Array，FPGA) 或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件等。其中，通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。
本领域技术人员可以理解，图4中示出的计算机设备700结构并不构成对计算机设备700的限定，可以包括比图示更多或更少的部件，或者组合某些部件，或者不同的部件布置。
在本发明的另一实施例中提供了一种计算机可读存储介质。该计算机可读存储介质可以为非易失性的计算机可读存储介质。该计算机可读存储介质存储有计算机程序，其中计算机程序被处理器执行时实现本发明实施例公开的基于毫米波雷达的无人船桥下定位方法。
所属领域的技术人员可以清楚地了解到，为了描述的方便和简洁，上述描述的设备、装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。本领域普通技术人员可以意识到，结合本文中所公开的实施例描述的各示例的单元及算法步骤，能够以电子硬件、计算机软件或者二者的结合来实现，为了清楚地说明硬件和软件的可互换性，在上述说明中已经按照功能一般性地描述了各示例的组成及步骤。这些功能究竟以硬件还是软件方式来执行取决于技术方案的特定应用和设计约束条件。专业技术人员可以对每个特定的应用来使用不同方法来实现所描述的功能，但是这种实现不应认为超出本发明的范围。
在本发明所提供的几个实施例中，应该理解到，所揭露的设备、装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为逻辑功能划分，实际实现时可以有另外的划分方式，也可以将具有相同功能的单元集合成一个单元，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另外，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口、装置或单元的间接耦合或通信连接，也可以是电的，机械的或其它的形式连接。
所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本发明实施例方案的目的。
另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以是两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。
所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分，或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)执行本发明各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，可轻易想到各种等效的修改或替换，这些修改或替换都应涵盖在本发明的保护范围之内。因此，本发明的保护范围应以权利要求的保护范围为准。

Claims (4)

1.基于毫米波雷达的无人船桥下定位方法，其特征在于，包括：
基于毫米波雷达点云计算出桥下船只的方向角；
基于毫米波雷达点云推算出无人船的初步定位结果；
对无人船的初步定位结果进行优化，以得到无人船的最终定位结果；
所述基于毫米波雷达点云计算出桥下船只的方向角，包括：
对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率；
根据桥梁两侧的直线的斜率推算出船只与桥梁方向的夹角；
所述对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率，包括：
采用随机采样一致性算法，结合最小二乘拟合，拟合出单侧桥梁直线分布曲线；
从单侧桥梁直线分布曲线中筛选出单侧桥梁点云；
利用单侧桥梁点云拟合出桥梁的另一单侧桥梁点云；
所述基于毫米波雷达点云推算出无人船的初步定位结果，包括：
基于毫米波雷达点云的多普勒信息推算出船只的速度；
根据船只的速度和船只的方向角计算得出无人船的初步定位结果；
所述根据船只的速度和船只的方向角计算得出无人船的初步定位结果，包括：
根据船只的速度和船只的方向角计算出船只在当前固定时间差内的位移；
将当前固定时间差内的位移的计算结果叠加至上一帧对应的位移结果，以得到当前帧的初步定位结果；
所述对无人船的初步定位结果进行优化，以得到无人船的最终定位结果，包括：
根据两侧桥梁的点云集合以及对应的拟合直线结果，识别出桥梁特征；
根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果；
所述根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果，包括：
计算船只距离桥梁两侧侧边的垂直距离，以得到侧方距离；
利用得到的侧方距离代替初步定位结果中的X方向的定位结果，以得到最终的X方向的定位结果；
计算当前帧与上一帧的相对位姿初值；
采用相对位姿初值进行点云配准，以得到相对位姿计算值；
基于得到的相对位姿计算值更新初步定位结果中的Y方向的定位结果，以得到最终的Y方向的定位结果。
2.基于毫米波雷达的无人船桥下定位装置，其特征在于，包括第一计算单元、第二计算单元以及优化单元；
所述第一计算单元，用于基于毫米波雷达点云计算出桥下船只的方向角；
所述第二计算单元，用于基于毫米波雷达点云推算出无人船的初步定位结果；
所述优化单元，用于对无人船的初步定位结果进行优化，以得到无人船的最终定位结果；
所述第一计算单元包括提取模块以及第一计算模块；
所述提取模块，用于对桥下毫米波雷达点云进行桥梁点云提取，以得到桥梁两侧的直线的斜率；
所述第一计算模块，用于根据桥梁两侧的直线的斜率推算出船只与桥梁方向的夹角；
所述提取模块包括第一拟合子模块、筛选子模块以及第二拟合子模块；
所述第一拟合子模块，用于采用随机采样一致性算法，结合最小二乘拟合，拟合出单侧桥梁直线分布曲线；
所述筛选子模块，用于从单侧桥梁直线分布曲线中筛选出单侧桥梁点云；
所述第二拟合子模块，用于利用单侧桥梁点云拟合出桥梁的另一单侧桥梁点云；
所述第二计算单元包括第二计算模块以及第三计算模块；
所述第二计算模块，用于基于毫米波雷达点云的多普勒信息推算出船只的速度；
所述第三计算模块，用于根据船只的速度和船只的方向角计算得出无人船的初步定位结果；
所述第三计算模块包括第一计算子模块以及第二计算子模块；
所述第一计算子模块，用于根据船只的速度和船只的方向角计算出船只在当前固定时间差内的位移；
所述第二计算子模块，用于将当前固定时间差内的位移的计算结果叠加至上一帧对应的位移结果，以得到当前帧的初步定位结果；
所述优化单元包括识别模块以及优化模块；
所述识别模块，用于根据两侧桥梁的点云集合以及对应的拟合直线结果，识别出桥梁特征；
所述优化模块，用于根据桥梁特征进行侧向定位结果和前向定位结果的优化，以得到最终定位结果；
所述优化模块包括第三计算子模块、第一更新子模块、第四计算子模块、点云配准子模块以及第二更新子模块；
所述第三计算子模块，用于计算船只距离桥梁两侧侧边的垂直距离，以得到侧方距离；
所述第一更新子模块，用于利用得到的侧方距离代替初步定位结果中的X方向的定位结果，以得到最终的X方向的定位结果；
所述第四计算子模块，用于计算当前帧与上一帧的相对位姿初值；
所述点云配准子模块，用于采用相对位姿初值进行点云配准，以得到相对位姿计算值；
所述点云配准子模块，用于基于得到的相对位姿计算值更新初步定位结果中的Y方向的定位结果，以得到最终的Y方向的定位结果。
3.一种计算机设备，其特征在于，包括存储器、处理器以及存储在所述存储器上并可在所述处理器上运行的计算机程序，所述处理器执行所述计算机程序时实现如权利要求1所述的基于毫米波雷达的无人船桥下定位方法。
4.一种计算机可读存储介质，其特征在于，所述存储介质存储有计算机程序，所述计算机程序包括程序指令，所述程序指令被处理器执行时，使得所述处理器执行如权利要求1所述的基于毫米波雷达的无人船桥下定位方法。
CN202211620321.2A 2022-12-16 2022-12-16 基于毫米波雷达的无人船桥下定位方法及相关设备 Active CN115629374B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211620321.2A CN115629374B (zh) 2022-12-16 2022-12-16 基于毫米波雷达的无人船桥下定位方法及相关设备

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211620321.2A CN115629374B (zh) 2022-12-16 2022-12-16 基于毫米波雷达的无人船桥下定位方法及相关设备

Publications (2)

Publication Number Publication Date
CN115629374A CN115629374A (zh) 2023-01-20
CN115629374B true CN115629374B (zh) 2023-03-10

Family

ID=84910616

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211620321.2A Active CN115629374B (zh) 2022-12-16 2022-12-16 基于毫米波雷达的无人船桥下定位方法及相关设备

Country Status (1)

Country Link
CN (1) CN115629374B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117523568B (zh) * 2024-01-08 2024-03-29 陕西欧卡电子智能科技有限公司 一种基于无人船的桥高检测方法、装置、设备及介质

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111859518A (zh) * 2020-08-03 2020-10-30 交通运输部天津水运工程科学研究所 一种基于几何关系的斜桥通航有效通航净宽确定方法

Family Cites Families (6)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN104952107A (zh) * 2015-05-18 2015-09-30 湖南桥康智能科技有限公司 基于车载LiDAR点云数据的桥梁三维重建方法
CN108189040B (zh) * 2018-03-09 2023-06-27 成都圭目机器人有限公司 一种污水管线检测机器人系统
CN111290396A (zh) * 2020-03-12 2020-06-16 上海圭目机器人有限公司 一种管道检测无人船自动控制方法
CN212322115U (zh) * 2020-07-31 2021-01-08 南京开天眼无人机科技有限公司 一种无人船巡检避障系统
CN113340295B (zh) * 2021-06-16 2021-12-21 广东工业大学 一种多个测距传感器的无人艇近岸实时定位与建图方法
CN114046792B (zh) * 2022-01-07 2022-04-26 陕西欧卡电子智能科技有限公司 一种无人船水面定位与建图方法、装置及相关组件

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111859518A (zh) * 2020-08-03 2020-10-30 交通运输部天津水运工程科学研究所 一种基于几何关系的斜桥通航有效通航净宽确定方法

Non-Patent Citations (3)

* Cited by examiner, † Cited by third party
Title
H. L. Zhang et al..Validation of Extended Kirchhoff Approximation and Small Slope Approximation for Electromagnetic Scattering from Ship Wake Surfaces.2016,全文. *
朱健楠等.无人水面艇感知技术发展综述.2020,41(10),全文. *
田明辉等.机载单站固定目标测向交叉定位研究.2012,第35卷(第35期),全文. *

Also Published As

Publication number Publication date
CN115629374A (zh) 2023-01-20

Similar Documents

Publication Publication Date Title
CN109143207B (zh) 激光雷达内参精度验证方法、装置、设备及介质
CN109239709B (zh) 一种无人船的局部环境地图自主构建方法
CN103197308B (zh) 基于多波束相控阵声纳系统的三维声纳可视化处理方法
CN115629374B (zh) 基于毫米波雷达的无人船桥下定位方法及相关设备
CN109001725B (zh) 一种海上无人艇海上多目标跟踪方法
US20210304491A1 (en) Ground map generation
CN114046792B (zh) 一种无人船水面定位与建图方法、装置及相关组件
Kim et al. Artificial intelligence vision-based monitoring system for ship berthing
CN115079168B (zh) 基于激光雷达与毫米波雷达融合的建图方法、装置及设备
CN114926726B (zh) 基于多任务网络的无人船感知方法及相关设备
Lenart Collision threat parameters for a new radar display and plot technique
CN113743385A (zh) 一种无人船水面目标检测方法、装置及无人船
Wang et al. Estimation of ship berthing parameters based on Multi-LiDAR and MMW radar data fusion
CN113177593B (zh) 一种水上交通环境中雷达点云与影像数据的融合方法
CN113759364A (zh) 一种基于激光地图的毫米波雷达连续定位方法及装置
CN117389305A (zh) 一种无人机巡检路径规划方法、系统、设备及介质
CN112965076A (zh) 一种用于机器人的多雷达定位系统及方法
CN114821494B (zh) 船舶信息匹配方法及装置
CN116907509A (zh) 基于图像匹配的auv水下辅助导航方法、系统、设备及介质
CN114063622B (zh) 无人船自主停泊定位方法、装置及相关组件
Iscar et al. Autonomous surface vehicle 3D seafloor reconstruction from monocular images and sonar data
CN114862938A (zh) 雪堆体积的检测方法、电子设备及存储介质
CN114913500A (zh) 位姿确定方法、装置、计算机设备和存储介质
CN112050830B (zh) 一种运动状态估计方法及装置
Krupiński et al. Positioning aiding using LiDAR in GPS signal loss scenarios

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