CN114485607A - 一种运动轨迹的确定方法、作业设备、装置、存储介质 - Google Patents

一种运动轨迹的确定方法、作业设备、装置、存储介质 Download PDF

Info

Publication number
CN114485607A
CN114485607A CN202111463035.5A CN202111463035A CN114485607A CN 114485607 A CN114485607 A CN 114485607A CN 202111463035 A CN202111463035 A CN 202111463035A CN 114485607 A CN114485607 A CN 114485607A
Authority
CN
China
Prior art keywords
current frame
point cloud
cloud data
determining
change information
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.)
Granted
Application number
CN202111463035.5A
Other languages
English (en)
Other versions
CN114485607B (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 CN202111463035.5A priority Critical patent/CN114485607B/zh
Publication of CN114485607A publication Critical patent/CN114485607A/zh
Application granted granted Critical
Publication of CN114485607B publication Critical patent/CN114485607B/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/005—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 with correlation of navigation data from several sources, e.g. map or contour matching

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Automation & Control Theory (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Radar Systems Or Details Thereof (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本申请实施例公开了一种运动轨迹的确定方法、作业设备、装置、存储介质，其中，所述方法包括：获取待作业区域当前帧的点云数据和当前帧的全局地图，其中，所述当前帧的点云数据是所述作业设备在所述作业区域内作业采集的点云数据，所述当前帧的全局地图是基于所述当前帧的前一帧的点云数据确定的；基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息；基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息；基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息。

Description

一种运动轨迹的确定方法、作业设备、装置、存储介质
技术领域
本申请实施例涉及作业设备技术领域，涉及但不限于一种运动轨迹的确定方法、作业设备、装置、存储介质。
背景技术
随着科学技术的发展，各行业对自动化作业的需求持续增长。当作业设备例如无人船或者室外清扫机器人在作业区域进行作业时，准确定位对作业设备的安全行驶和任务执行至关重要，而准确定位离不开作业者对作业设备的运动轨迹的准确判断。
在现实生活中，作业环境条件如恶劣的天气、强光照射、夜间等会影响作业者对作业设备运动轨迹的准确判断，进而影响作业设备的准确定位，导致作业设备不能进行正常作业，甚至出现破坏作业设备的现象。
发明内容
有鉴于此，本申请实施例提供一种运动轨迹的确定方法、作业设备、装置、存储介质。本申请实施例的技术方案是这样实现的：
第一方面、本申请实施例提供一种运动轨迹的确定方法，所述方法包括：获取待作业区域当前帧的点云数据和当前帧的全局地图，其中，所述当前帧的点云数据是所述作业设备在所述作业区域内作业采集的点云数据，所述当前帧的全局地图是基于所述当前帧的前一帧的点云数据确定的；基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息；基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息；基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息。
第二方面、本申请实施例提供一种作业装置，所述装置包括：第一获取模块，用于获取待作业区域当前帧的点云数据和当前帧的全局地图，其中，所述当前帧的点云数据是所述作业设备在所述作业区域内作业采集的点云数据，所述当前帧的全局地图是基于所述当前帧的前一帧的点云数据确定的；第一确定模块，用于基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息；第二确定模块，用于基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息；融合模块，用于基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息。
第三方面、本申请实施例提供一种作业设备，所述设备包括：存储器和处理器，
所述存储器存储有可在所述处理器上运行的计算机程序，
所述处理器执行所述计算机程序时实现上述方法中的步骤。
第四方面、本申请实施例提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述作业方法中的步骤。
本申请实施例中，由于所述作业设备的当前帧的位姿信息是基于数据融合算法，将第一位姿变化信息和第二位姿变化信息经数据融合后得到的，相比于经过单一的位姿变化信息的算法得到的当前帧的位姿信息而言，本申请实施例中得到的当前帧的位姿信息更加准确，由此可以使后续作业者对作业设备的运动轨迹做出准确的判断，进而提高了作业设备的准确定位，能够有效解决由于作业环境条件如恶劣的天气、强光照射、夜间等原因导致的作业设备定位精度差的问题，减少作业设备不能正常作业或出现破坏的现象。
附图说明
图1为本申请实施例提供的一种运动轨迹的确定方法的流程示意图；
图2为本申请实施例提供的一种确定当前帧的全局地图的流程示意图；
图3为本申请实施例提供的一种无人船的运动模型的示意图；
图4为本申请实施例提供的一种雷达坐标系的示意图；
图5为本申请实施例提供的一种无人船的运动速度和雷达的运动速度之间的关系示意图；
图6为本申请实施例提供的一种确定作业设备的第二位姿变化信息的流程示意图；
图7为本申请实施例提供的当前帧的局部地图、2D占据栅格图、匹配度度量图；
图8为本申请实施例提供的一种确定作业设备的当前帧的位姿信息的流程示意图；
图9为本申请实施例提供的另外一种运动轨迹的确定方法的流程示意图；
图10为本申请实施例提供的一种作业装置的组成结构示意图。
具体实施方式
下面结合附图和实施例对本申请的技术方案进一步详细阐述。
本申请实施例提供一种运动轨迹的确定方法，包括图1中的方法：
步骤S101、获取待作业区域当前帧的点云数据和当前帧的全局地图，其中，所述当前帧的点云数据是所述作业设备在所述作业区域内作业采集的点云数据，所述当前帧的全局地图是基于所述当前帧的前一帧的点云数据确定的；
本申请实施例中，所述待作业区域可以为空中、海面、河道和陆地中的部分区域；在所述待作业区域作业的作业方式可以为巡检监察、垃圾清扫、物流运输、浇灌或捕捞等。实施作业方式的设备(即作业设备)可以包括但不限于无人机、无人船、清洁机器人、洒水机器人和探测机器人等。例如：农业应用的无人机对农田进行喷药或浇灌等；船舶对海面某一区域进行捕捞；清洁机器人对室内外的某一区域进行清洁。需要说明的是，本申请实施例对待作业区域、作业方式以及应用的场景不做限定。
在一些实施例中，所述作业设备可以为电子设备，所述电子设备可以为服务器，所述服务器与作业设备之间进行数据交互，或者，终端分别与作业设备和服务器进行数据交互，以使作业设备按照一定的运动轨迹在待作业区域内进行作业。其中，终端和服务器都可以为具有信息处理能力的设备，例如笔记本电脑、平板电脑、手机、导航仪、台式电脑、服务器集群等设备。
在一些实施例中，点云数据(Point Cloud Data)是指在一个三维坐标系中的一组向量的集合，这些向量通常以x、y、z三维坐标的形式表示点云中每个点的几何位置，除此之外，点云数据还可以包括点云中一个点的颜色、灰度值、深度、分割结果和/或速度等。因此，可以理解地，所述当前帧的点云数据是指当前一帧点云中每个点的点云数据组成的集合，同理，所述当前帧的前一帧的点云数据是指所述位于当前一帧的前面一帧的点云中每个点的点云数据组成的集合。
在一些实施例中，当前帧的点云数据可以是数据获取装置采集的，所述数据获取装置可以为全球卫星定位系统(Global Positioning System，GPS)、视觉摄像头、相机、雷达和/或激光雷达等。
在相关技术中，当卫星信号受到遮挡时，GPS采集的数据精度较差；激光雷达、视觉摄像头等受恶劣天气和光照的影响较大。本申请实施例中，采用毫米波雷达作为数据获取装置来采集待作业区域当前帧的点云数据，相比于相关技术中的数据获取装置，毫米波雷达成本低廉、对恶劣天气和光照等的鲁棒性好，能更好地辅助作业设备实现不同工况下的准确定位。
在一些实施例中，全局地图可以为栅格图、特征地图、拓扑地图、平面轮廓图、三维立体图等。实施时，可以利用摄像头拍摄、激光扫描或传感器感应的方式，来获取待作业区域的周围环境信息，基于该环境信息建立待作业区域的地图。在本申请实施例中，所述当前帧的全局地图是指在当前一帧点云对应的时刻下的全局地图。
步骤S102、基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息；
这里，位姿代表位置和姿态角。一个刚体在空间坐标系中可以用位置和姿态角来精确、唯一的表示其位置状态，其中，位置用x、y和z坐标来表示，姿态角用刚体与x轴的夹角、刚体与y轴的夹角和刚体与z轴的夹角来表示。由此可知，位姿变化信息代表位移和姿态角的变化信息。
步骤S103、基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息；
本申请实施例中，所述作业设备的第一位姿变化信息和第二位姿变化信息是指所述作业设备的两个不同的位姿变化信息，其中，第一和第二只是为了表述的方便，没有其他实质性的含义例如表示两个不同位姿变化信息之间的大小关系的含义等。
步骤S104、基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息。
在一些实施例中，所述当前帧的位姿信息是指所述作业设备在当前一帧点云对应的时刻所处的位置和姿态角信息。
在一些实施例中，所述数据融合算法是指利用数据融合技术进行数据处理时所采用的算法。更多地，数据融合技术的原理是：在多源数据融合系统当中，各类传感器提供多源信息与数据，这些多源信息与数据可能具有各不相同的特性或特征，而这些特性或特征也是多样的，可能是相互支持或互补的，确定或者模糊的，还有可能是冲突矛盾的，具有差异性。当计算机获取到传感器观测到的信息后，通过有效筛选信息，把在时空上互补和冗余的信息按照某种组合规则进行优化处理，多方位、多层次的进行数据化处理，获得对监测目标的理解、认知以及更多有价值的信息，并且获得比它的各个部分所构成的系统更高质量的性能，作为最后协同作用的结果。
在相关技术中，数据融合技术可以包括数据层融合、特征层融合和决策层融合，其中，数据层融合是直接在采集到的原始数据层上进行的融合，在各种传感器的原始测报数据未经预处理之前就进行数据的综合与分析。数据层融合一般采用集中式融合体系进行融合处理过程。这是低层次的融合，如成像传感器中通过对包含某一像素的模糊图像进行图像处理来确认目标属性的过程就属于数据层融合；特征层融合是先对来自传感器的原始信息进行特征提取(特征可以是目标的边缘、方向、速度等)，然后对特征信息进行综合分析和处理，最后得出融合结果的方法，特征层融合一般采用分布式或集中式的融合体系。特征层融合可分为两大类:一类是目标状态融合；另一类是目标特性融合；决策层融合是通过对不同类型的初步采集数据分别进行特征提取和初步判断，以建立对所观察目标的初步结论，然后通过关联处理进行决策层融合判决，最终获得联合推断结果的方法。
本申请实施例中，由于所述作业设备的当前帧的位姿信息是基于数据融合算法，将第一位姿变化信息和第二位姿变化信息经数据融合后得到的，相比于经过单一的位姿变化信息的算法得到的当前帧的位姿信息而言，本申请实施例中得到的当前帧的位姿信息更加准确，由此可以使后续作业者对作业设备的运动轨迹做出准确的判断，进而提高了作业设备的准确定位，能够有效解决由于作业环境条件如恶劣的天气、强光照射、夜间等原因导致的作业设备定位精度差的问题，减少作业设备不能正常作业或出现破坏的现象。
在一些实施例中，步骤S101中“获取当前帧的全局地图”可以采用图2中的方法来实现，该方法包括：
步骤S201、获取所述当前帧的前一帧的点云数据、所述前一帧的位姿信息以及所述前一帧的全局地图；
在一些实施例中，所述当前帧的前一帧的点云数据是在以装配在作业设备上的毫米波雷达为原点建立的雷达坐标系中表示的，其中，毫米波雷达右朝向为x轴正方向，毫米波雷达前朝向为y轴正方向。
在一些实施例中，通过毫米波雷达来获取点云数据。更详细地，首先采用4D-FMCW毫米波雷达，由模数转换器采样得到原始点云数据，其中原始点云数据包括点云中每个点与雷达之间的距离以及点云中每个点的多普勒速度；其次通过傅里叶变换得到距离-多普勒矩阵(Range Doppler Matrix，RDM)；接下来对RDM通过恒虚警率检测(Constant FalseAlarm Rate，CFAR)检测其中的目标单元，也叫做有效单元格；之后对目标单元通过到达角(Direction Of Arrival，DOA)估计，得到目标的方向角和高度角；最后将点云中每个点与雷达之间的距离、每个点的多普勒速度、方向角和高度角转换为点云中每个点的空间坐标信息和多普勒速度。
步骤S202、基于所述前一帧的位姿信息，将所述当前帧的前一帧的点云数据转换到世界坐标系下，得到第一点云数据；
在一些实施例中，所述世界坐标系是以在所述待作业区域内作业的所述作业设备为原点建立的，正东方向为x轴正方向，正北方向为y轴正方向。
在一些实施例中，所述“将所述当前帧的前一帧的点云数据转换到世界坐标系下，得到第一点云数据”的步骤可以包括：通过确定作业设备坐标系到世界坐标系的旋转矩阵和平移向量，将所述当前帧的前一帧的点云数据转换到世界坐标系中，其中，所述旋转矩阵和平移向量是基于所述前一帧的位姿信息确定的。
更进一步地，以水域为例来说明“将所述当前帧的前一帧的点云数据转换到世界坐标系下，得到第一点云数据”的实现方法。例如，在待作业水域中，所述当前帧的前一帧的点云数据处于雷达坐标系下，将所述当前帧的前一帧的点云数据转换到世界坐标系下，得到世界坐标系下的第一点云数据的实现方法参见公式(2-1)：
P′t-1(trans)＝Rt-1P′t-1+Tt-1 (2-1)；
其中，t代表当前帧的点云数据对应的时刻，t-1代表当前帧的前一帧的点云数据对应的时刻，trans代表将点云数据转换到世界坐标系的操作，P′t-1(trans)代表第一点云数据，P′t-1代表当前帧的前一帧的点云数据，Rt-1代表旋转矩阵，Tt-1代表平移向量。
这里，当前帧的前一帧的平移向量Tt-1的值为lt-1，其中，lt-1代表当前帧的前一帧的位置信息。
实施时，作业设备在待作业水域中进行作业，获取当前帧的前一帧的点云数据P′t-1，以及前一帧的位姿信息，其中，前一帧的位置信息用lt-1来表示，前一帧的姿态角用yawt-1来表示，根据姿态角yawt-1得到旋转矩阵Rt-1，这里，旋转矩阵Rt-1可表示为式(2-2)：
Figure BDA0003390001560000051
步骤S203、基于所述第一点云数据和所述前一帧的全局地图，确定所述当前帧的全局地图。
在一些实施例中，全局地图可以采用同步定位与建图方法(SimultaneousLocalization And Mapping，SLAM)形成，详细理解为，可以同步进行作业设备的位置确定和环境地图的构建，其中，位置信息可以用来构造地图和改进地图，而准确的环境地图又可以用来得到精确的位置信息。
又以水域为例来说明全局地图的构建过程。例如，根据步骤S202得到了第一点云数据，前一帧的全局地图表示为Pmap(t-1)，因此，当前帧的全局地图Pmap(t)的表达式参见公式(2-3)：
Pmap(t)＝Pmap(t-1)∪P′t-1(trans) (2-3)；
其中，map代表地图，∪代表并集。
这里，需要指出的是，若当前帧为第一帧，则直接将滤波后的当前帧点云数据P1′存入当前帧的全局地图Pmap(t)中，即
Figure BDA0003390001560000061
同时结束当前帧位置和姿态角估计，直接得l1＝(0,0)，yaw1＝0即设置当前帧位置为零点，当前帧姿态角为0度，其中，
Figure BDA0003390001560000062
代表空集，换句话说，就是表示没有点云数据，l1代表第一帧的位置信息，yaw1代表第一帧的姿态角。
本申请实施例中，由于世界坐标系是一个相对固定的坐标系，因此将处于雷达坐标系下的点云数据转换到世界坐标系下，并结合处于世界坐标系下的位姿信息，确定了处于世界坐标系下的全局地图，由此，可以将处于不同坐标系下的数据信息统一到同一相对固定的坐标系下，便于后期对不同数据信息进行处理、运算。
在一些实施例中，步骤S102中“所述基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息”的实现方法包括：
步骤S301、获取所述作业设备的运动模型，其中，所述运动模型包括所述作业设备沿所述作业设备的中轴方向的轴向运动、沿水平方向的水平运动和环绕所述作业设备的中心的圆周运动；
在一些实施例中，以在待作业水域中作业的无人船为例来说明所述作业设备的运动模型。详细地，在图3中，以无人船为原点建立世界坐标系，其中，在世界坐标系中正东方向也可以理解为水平方向为x轴正方向，正北方向也可以理解为无人船的中轴方向为y轴正方向。无人船的运动包括沿无人船的中轴方向的轴向运动，沿无人船的水平方向的水平运动和环绕无人船的中心的圆周运动；其中，轴向运动的运动速度可表示为
Figure BDA0003390001560000063
水平运动的运动速度可表示为
Figure BDA0003390001560000064
圆周运动的运动速度可表示为ωt。
在一些实施例中，基于所述作业设备的运动模型，可以得到在Δt时间内，所述作业设备的位移可用公式(3-1)来表示，姿态角的变化量可以用公式(3-2)来表示：
Figure BDA0003390001560000065
Δyawt＝Δt*ωt (3-2)；
其中，Δlt代表当前帧的位移，
Figure BDA0003390001560000066
代表当前帧作业设备沿水平方向运动的水平位移，
Figure BDA0003390001560000067
代表当前帧作业设备沿轴向运动的轴向位移，Δyawt代表当前帧作业设备绕圆周运动的姿态角的变化量。
更多地，
Figure BDA0003390001560000068
和
Figure BDA0003390001560000069
分别可以表示为公式(3-3)和公式(3-4)：
Figure BDA00033900015600000610
Figure BDA00033900015600000611
其中，
Figure BDA00033900015600000612
代表当前帧作业设备沿水平方向运动的水平位移，
Figure BDA00033900015600000613
代表当前帧作业设备沿轴向运动的轴向位移，Δyawt代表当前帧作业设备绕圆周运动的姿态角的变化量。
步骤S302、基于所述当前帧的点云数据，确定所述作业设备的运动速度，其中，所述作业设备的运动速度包括所述作业设备分别沿所述中轴方向、水平方向和圆周方向的速度；
在一些实施例中，所述当前帧的点云数据包括点云中每个点i的数据信息，所述每个点i的数据信息可表示为
Figure BDA0003390001560000071
其中，上标i表示当前帧中的第i个点，
Figure BDA0003390001560000072
分别代表该点在雷达坐标系中的x，y，z方向的坐标，
Figure BDA0003390001560000073
代表该点在雷达坐标系中的多普勒速度。
在一些实施例中，当前帧点云数据Pt′是对毫米波雷达获得的点云数据Pt进行预处理后得到的点云数据，预处理过程如下：
首先，基于直通滤波对点云数据Pt进行滤波，得到点云数据
Figure BDA0003390001560000074
在一些实施例中，直通滤波的原理为：在点云的指定维度上设置一个阈值范围，将这个维度上的数据分为在阈值范围内数据和不在阈值范围内的数据，将不在阈值范围内的数据进行过滤去除，得到滤波后的数据。
本申请实施例中，以无人船在待作业水域中作业时，毫米波雷达获得的点云数据为例来说明滤波步骤。
这里，毫米波雷达获得的点云数据为Pt，对于无人船运行前方由于水面波纹而产生的水面杂波点云数据，设定水面上的距离阈值Rthreshold，对在此阈值范围内的点云数据进行直接滤除，得到滤除后的点云数据
Figure BDA0003390001560000075
式(3-5)为点云数据
Figure BDA0003390001560000076
的表达式：
Figure BDA0003390001560000077
其中，
Figure BDA0003390001560000078
代表当前帧点云数据中的点i在雷达坐标系中对应的x轴的坐标值；
Figure BDA0003390001560000079
代表当前帧点云数据中的点i在雷达坐标系中对应的y轴的坐标值，
Figure BDA00033900015600000710
代表点i的数据信息，Rthreshold代表设定的水面上的距离阈值。
在本申请实施例中，Rthreshold＝0.5米(m)。
同时，对于z方向绝对值较大的点，设定z方向的距离阈值为Hthreshold，z方向绝对值大于此阈值的点均被滤除，得到滤除后的点云数据
Figure BDA00033900015600000711
式(3-6)为点云数据
Figure BDA00033900015600000712
的表达式：
Figure BDA00033900015600000713
其中，
Figure BDA00033900015600000714
代表了当前帧点云数据中点i在雷达坐标系中对应的z轴的坐标值，Hthreshold代表设定的z方向的距离阈值。
在本申请实施例中，Hthreshold＝2m。
其次，基于密度聚类滤波对点云数据
Figure BDA00033900015600000715
进行滤波，得到点云数据
Figure BDA00033900015600000716
在本申请实施例中，对邻域点云数据中个数较少的点、分布较为离散的点云进行滤除，详细实施为，计算每个点到其他点之间的距离矩阵Dt，式(3-7)为距离矩阵Dt的表达式为：
Figure BDA00033900015600000717
进一步地，d(pk,pj)的表达式参见公式(3-8)：
Figure BDA0003390001560000081
其中，d(pk,pj)代表当前帧点云中两个点pk和pj之间的欧式距离。
因此，基于密度聚类滤波后的点云集合
Figure BDA0003390001560000082
可表示为式(3-9)：
Figure BDA0003390001560000083
其中，N(·)代表满足该条件的元素个数，tneighbour为设定的邻域大小阈值，tnumber为设定的邻域点云数据中点的个数阈值，Dt(i,∶)为距离矩阵，实施时，若邻域点云数据中的点的数目少于设定阈值，即对其进行滤除，滤除后得到的点云数据为
Figure BDA0003390001560000084
最后，滤除点云数据
Figure BDA0003390001560000085
中的动态目标，得到当前帧的点云数据Pt′。
其中，点云数据中的动态目标是指毫米波雷达采集到的一些不是所有时刻都能观测到的点云数据中的点。
在一些实施例中，滤除点云数据
Figure BDA0003390001560000086
中的动态目标采用的方法是随机采样一致性算法。
通过以上过程，便可得到最终的滤波后点云数据，也就是当前帧点云数据Pt′。
本申请实施例中，首先，采用直通滤波、基于密度聚类滤波方法对毫米波雷达获取的点云数据进行滤波处理，得到点云数据
Figure BDA0003390001560000087
在此基础上，对点云数据
Figure BDA0003390001560000088
中的动态目标进行滤除，得到当前帧的点云数据Pt′。通过这些滤波方法对原始点云数据进行滤波，可以去除掉一些杂波点云数据，减少杂波点云数据对后续作业设备的运动速度计算的影响，便于得到更为精确的作业设备的路径点。
步骤S303、基于所述作业设备的运动模型和所述作业设备的运动速度，确定所述作业设备的第一位姿变化信息，其中，所述第一位姿变化信息包括沿所述中轴方向、所述水平方向的位移信息和姿态角的变化信息。
在一些实施例中，所述作业设备的第一位移信息可用式(3-10)表示，第一姿态角的变化信息可用式(3-11)表示：
Figure BDA0003390001560000089
Figure BDA00033900015600000810
其中，
Figure BDA00033900015600000811
代表当前帧的第一位移，
Figure BDA00033900015600000812
代表当前帧的第一姿态角的变化量，
Figure BDA00033900015600000813
代表当前帧的所述水平方向的位移，
Figure BDA00033900015600000814
代表当前帧的所述中轴方向的位移。
进一步地，
Figure BDA00033900015600000815
可用式(3-12)表示，
Figure BDA00033900015600000816
可用式(3-13)表示：
Figure BDA00033900015600000817
Figure BDA00033900015600000818
其中，沿中轴方向的位移表示为
Figure BDA0003390001560000091
沿水平方向的位移表示为
Figure BDA0003390001560000092
沿圆周方向的姿态角的变化量表示为
Figure BDA0003390001560000093
这里，需要理解的是：式(3-10)中当前帧的第一位移
Figure BDA0003390001560000094
的计算公式和式(3-11)中当前帧的第一姿态角的变化量
Figure BDA0003390001560000095
的计算公式是由步骤S301中获取的所述作业设备的运动模型确定的，而所述计算公式中包括的作业设备的运动速度是由步骤S302求解得到的。
在一些实施例中，步骤S302中“基于所述当前帧的点云数据，确定所述作业设备的运动速度，其中，所述作业设备的运动速度包括所述作业设备分别沿所述中轴方向、水平方向和圆周方向的速度”的实现包括如下步骤：
步骤S3021、基于所述当前帧的点云数据，确定所述作业设备上雷达的运动速度；
在一些实施例中，作业设备上装配毫米波雷达来获取点云数据，图4为以毫米波雷达为原点建立的雷达坐标系，其中，毫米波雷达右朝向为x轴正方向，毫米波雷达前朝向为y轴正方向。可以实例地，在图4中，在雷达坐标系中存在点云中的一个点
Figure BDA0003390001560000096
设
Figure BDA0003390001560000097
为xOy平面上由点pi连接到原点(0,0)的向量与x轴正向的夹角，
Figure BDA0003390001560000098
则点pi的多普勒速度
Figure BDA0003390001560000099
运动可表示为式(4-1)：
Figure BDA00033900015600000910
其中，
Figure BDA00033900015600000911
代表毫米波雷达沿x轴运动的速度；
Figure BDA00033900015600000912
代表毫米波雷达沿y轴运动的速度。
进一步地，在雷达坐标系中存在当前帧的点云数据中的一个点
Figure BDA00033900015600000913
设
Figure BDA00033900015600000914
为xOy平面上由点pk连接到原点(0,0)的向量与x轴负向的夹角，其中，
Figure BDA00033900015600000915
步骤S3022、获取所述雷达的运动速度和所述作业设备的运动速度之间的运动关系；
在一些实施例中，以图5中在待作业水域中作业的无人船为例说明如何得到所述雷达的运动速度和所述作业设备的运动速度之间的运动关系。
在图5中，在无人船的一定位置装配了毫米波雷达，则无人船的运动速度与雷达的运动速度之间的关系式为式(5-1)：
Figure BDA00033900015600000916
其中，dF为雷达装配位置到无人船设定中心的距离，ωt为无人船运动的角速度，vt为无人船运动的线速度，包括无人船沿轴向运动的轴向运动速度
Figure BDA00033900015600000917
和无人船沿水平方向运动的水平速度
Figure BDA00033900015600000918
为雷达相对自身坐标系的运动，包括沿雷达坐标系内x方向的水平速度分量
Figure BDA00033900015600000919
和沿雷达坐标系内y方向的垂直速度分量
Figure BDA00033900015600000920
进一步地，设船只坐标系x方向和雷达坐标系y方向的夹角为θS,雷达安装位置与船体中心的连线与船只坐标系x方向的夹角为θF，则可以推导出雷达的运动速度和和无人船的运动速度之间的运动关系为式(5-2)和(5-3)：
Figure BDA0003390001560000101
Figure BDA0003390001560000102
其中，
Figure BDA0003390001560000103
为沿雷达坐标系内x方向的水平速度分量，
Figure BDA0003390001560000104
为沿雷达坐标系内y方向的垂直速度分量，dF为雷达装配位置到无人船设定中心的距离，ωt为无人船运动的角速度，
Figure BDA0003390001560000105
为无人船沿水平方向运动的水平速度，
Figure BDA0003390001560000106
为无人船沿轴向运动的轴向运动速度。
在一些实施例中，由公式(4-1)联立公式(5-1)和公式(5-2)可以得到每个点pi和无人船运动速度
Figure BDA0003390001560000107
ωt之间的关系，进而通过多点拟合可以求得毫米波雷达沿x轴方向的运动速度
Figure BDA0003390001560000108
和沿y轴方向的运动速度
Figure BDA0003390001560000109
步骤S3023、基于所述雷达的运动速度和所述运动关系，确定所述作业设备的运动速度。
在一些实施例中，通过随机采样一致性算法并通过拟合算法得到作业设备的运动速度。
在一些实施例中，通过对当前点云集合
Figure BDA00033900015600001010
中的点云进行随机选取采样，再结合最小二乘拟合方法，用点云集合
Figure BDA00033900015600001011
中点云信息拟合得出雷达两个方向的速度
Figure BDA00033900015600001012
和
Figure BDA00033900015600001013
以及作业设备的角速度大小ωt，之后根据雷达运动速度与作业设备运动速度之间的运动关系，得到作业设备的运动速度
Figure BDA00033900015600001014
本申请实施例中，利用毫米波雷达获取的当前帧的点云数据，确定了作业设备的运动速度，由作业设备的运动速度结合作业设备的运动模型，得到了作业设备的第一位姿信息，该方法依据运动学原理，由作业设备的运动速度获得作业设备的第一位姿信息，简单便捷，易于实现。
在一些实施例中，步骤S103中“基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息”的实现包括图6中的方法：
步骤S601、从所述当前帧的全局地图提取子地图，得到当前帧的局部地图；
在一些实施例中，局部地图是指从所述当前帧的全局地图中提取的子地图。
在本申请实施例中，当前帧的局部地图P(submap,t)是基于当前帧对应的第一位移
Figure BDA00033900015600001015
初步计算一个当前帧对应的第一位置
Figure BDA00033900015600001016
在第一位置lt′周围，从已建立的当前帧的全局地图Pmap中提取大小为25m的正方形子地图，也就是局部地图P(submap,t)＝{(x1,y1),(x2,y2),……,(xN,yN)}，其中，以点N为例，xN代表点N在局部地图P(submap,t)中的横坐标，yN代表点N在局部地图P(submap,t)中的纵坐标。
步骤S602、确定所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度；
在一些实施例中，步骤S602的实施可以包括以下步骤：
步骤S6021、将所述当前帧的点云数据转换到世界坐标系下，得到第二点云数据；
在一些实施例中，将点云数据转换到世界坐标系下的方法为：
步骤S60211、基于当前帧对应的第一位移
Figure BDA00033900015600001017
初步计算当前帧点云数据对应的第一位置lt′以及第一姿态角yawt′，如公式(6-1)和公式(6-2)所示：
Figure BDA0003390001560000111
其中，t代表当前帧的点云数据对应的时刻，l′t代表当前帧点云数据对应的第一位置lt′，lt-1代表当前帧的前一帧点云数据对应的位置lt′，
Figure BDA0003390001560000112
代表当前帧点云数据对应的第一位置变化。
Figure BDA0003390001560000113
其中，t代表当前帧的点云数据对应的时刻，yawt-1代表当前帧的前一帧的点云数据对应的第一姿态角yawt-1，
Figure BDA0003390001560000114
代表当前帧点云数据对应的第一姿态变化。
步骤S60212、基于所述当前帧的第一位置lt′和姿态角yawt′，将所述当前帧的点云数据Pt′转换到世界坐标系下，得到第二点云数据P′t(trans)，其中，将所述当前帧的点云数据Pt′转换到世界坐标系下，得到世界坐标系下的第二点云数据P′t(trans)的实现方法参见公式(6-3)：
P′t(trans)＝Rt′P′t+Tt′ (6-3)；
其中，t代表当前帧的点云数据对应的时刻，trans代表将点云数据转换到世界坐标系的操作，P′t(trans)代表世界坐标系下的点云数据，P′t代表当前帧的点云数据，Rt′代表旋转矩阵，Tt′代表平移向量，Tt′＝l′t。
这里，Rt′代表旋转矩阵，可表示为式(6-4)：
Figure BDA0003390001560000115
步骤S6022、将所述当前帧的局部地图进行转换，得到匹配度度量图；
在一些实施例中，匹配度度量图的获得包括：首先需要将局部地图转换为2D占据栅格图M(occ,t)，转换方法为：设定栅格分辨率为0.05m，根据栅格分辨率，将局部地图划分为多个栅格区域，对于栅格中包含点云中点的栅格，设定栅格图值对应为1；对不包含点云中点的栅格，设定栅格图对应值为0。紧接着，便可以将2D占据栅格图M(occ,t)转换为匹配度度量图。
在本申请实施例中，匹配度度量图采用的是基于距离度量的匹配度度量图M(occ,t)。将2D占据栅格图M(occ,t)转换为基于距离度量的匹配度度量图M(occ,t)的方法为：设定与2D占据栅格图M(occ,t)保持同样的栅格分辨率和栅格划分方式，根据栅格分辨率和栅格划分方式，得到基于距离度量的匹配度度量图M(occ,t)。对于M(occ,t)中每个栅格的值，由如下方式决定：若该栅格与其最为临近的被点云中的点占据的栅格的距离大于等于disT，则设定该栅格值为disT；若该栅格与其最为临近的被点云中的点占据的栅格的距离小于disT，则设定该栅格值为对应的点与点之间的距离。其中，disT为设定的距离阈值，该值被设定为10m。
图7分别展示了当前帧的局部地图、由局部地图转换出的2D占据栅格图M(occ,t)，以及由2D占据栅格图M(occ,t)转换的基于距离度量的匹配度度量图M(mat,t)，其中，图7(a)表示当前帧的局部地图，在局部地图中包括有很多点云中的点的区域以及没有点的区域；图7(b)表示2D占据栅格图M(occ,t)，在2D占据栅格图M(occ,t)中，因为包含点云中点的栅格被设定栅格图值为1，因此在图中为亮点部分，而不包含点云中点的栅格被设定栅格图值为0，因此在图中为乌黑部分；图7(c)表示基于距离度量的匹配度度量图M(mat,t)，在基于距离度量的匹配度度量图M(mat,t)中，通过将所述栅格与其最为临近的被点云中的点占据的栅格的距离和距离阈值disT进行比较，确定了所述栅格的栅格值，得到了图7(c)。
步骤S6023、确定所述第二点云数据在所述匹配度度量图中的坐标信息；
在一些实施例中，第二点云数据中每个点对应在基于距离度量的匹配度度量图M(mat,t)中的坐标可表示为(h1,w1),(h2,w2),…,(hn,wn)，其中，以点n为例，hn代表点n的横坐标，wn代表点n的纵坐标。
步骤S6024、基于预设的匹配度度量算法和所述坐标信息，确定所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度。
在一些实施例中，首先，根据第二点云数据中每个点对应在基于距离度量的匹配度度量图M(mat,t)中的坐标(h1,w1),(h2,w2),…,(hn,wn)对对应的单元格取值，分别表示为M(mat,t)(h1,w1),M(mat,t)(h2,w2),…,M(mat,t)(hn,wn)；其次，对单元格值根据式(6-5)求和，得到M；最后根据式(6-6)进行计算，得到匹配度得分(Match Score)。
Figure BDA0003390001560000121
Figure BDA0003390001560000122
其中，disT为设定的距离阈值，n代表n个点。
这里，匹配度得分便可代表所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度。
步骤S603、基于所述匹配度和所述当前帧的初始位姿信息，确定所述作业设备的预期位姿信息；
这里，所述初始位姿信息是指设定的作业设备初始的位置和姿态角信息，所述预期位姿信息是指当前帧的点云数据与所述当前帧的局部地图之间匹配度最高时得到的作业设备的位姿信息。
在一些实施例中，所述预期位姿信息采用搜索算法得到。搜索算法是指利用计算机的高性能来有目的的穷举一个问题解空间的部分或所有的可能情况，从而求出问题的解的一种方法。相关技术中，搜索算法包括枚举算法、深度优先搜索、广度优先搜索、回溯算法、蒙特卡洛树搜索、散列函数等算法。本申请实施例中，搜索算法采用分支定界法。
实施时，首先，在每次搜索前，需要设定作业设备的初始位置和姿态角，设定方法为：设定搜索时的初始位置和姿态角为前一帧推导出的位置和姿态角，加上当前帧的前面两帧的位置和姿态角的变化量，在这里，初始位置可表示为式(6-7)，姿态角可表示为式(6-8)：
l′t＝lt-1+(lt-1-lt-2) (6-7)；
yaw′t＝yawt-1+(yawt-1-yawt-2) (6-8)；
其中，l′t表示当前帧的初始位置，lt-1表示当前帧的前一帧的位置，lt-2表示当前帧的前二帧的位置，yaw′t表示当前帧的初始姿态角，yawt-1表示当前帧的前一帧的姿态角，yawt-2表示当前帧的前二帧的姿态角。
其次，在初始位置l′t和姿态角yaw′t附近采用分支定界法进行搜索，得到使得当前帧的点云数据与所述当前帧的局部地图之间匹配度最高时的作业设备的位置
Figure BDA0003390001560000131
和姿态角
Figure BDA0003390001560000132
步骤S604、基于所述作业设备的预期位姿信息和所述前一帧的位姿信息，确定所述作业设备的第二位姿变化信息。
在一些实施例中，采用式(6-9)至式(6-10)的方法来求解第二位姿变化信息：
Figure BDA0003390001560000133
Figure BDA0003390001560000134
其中，
Figure BDA0003390001560000135
代表当前帧作业设备的第二位移，Δyawmatch代表当前帧作业设备姿态角的变化量，
Figure BDA0003390001560000136
代表当前帧的点云数据与所述当前帧的局部地图之间匹配度最高时的作业设备的位置，
Figure BDA0003390001560000137
当前帧的点云数据与所述当前帧的局部地图之间匹配度最高时的作业设备的姿态角。
本申请实施例中，采用基于当前帧的点云数据和当前帧的局部地图的匹配度度量的方法来确定作业设备的第二位姿信息。采用这种匹配度度量的方法来求解作业设备的第二位姿变化信息，可以有效解决当采集到的帧间点云数据差距较大时，由单纯的依据运动学原理求解作业设备的位姿信息不准确的问题。
在一些实施例中，步骤S104中“基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息”的实现方法包括图8中的方法：
步骤S801、获取所述作业设备的前一帧的位姿信息；
这里，所述作业设备的前一帧的位置可表示为lt-1，前一帧的姿态角可表示为yawt-1。
步骤S802、基于所述数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述当前帧的位姿变化信息；
在一些实施例中，采用所述数据融合算法进行数据融合时，需要融合权重系数。
在本申请实施例中，所述当前帧的位移用
Figure BDA0003390001560000138
表示，当前帧的姿态角的变化用
Figure BDA0003390001560000139
表示，式(8-1)和式(8-2)代表所述数据融合算法：
Figure BDA00033900015600001310
Figure BDA00033900015600001311
其中，η代表第一权重系数，Ψ代表第二权重系数，
Figure BDA0003390001560000141
代表第一位移信息，
Figure BDA0003390001560000142
代表第一姿态角的变化信息，
Figure BDA0003390001560000143
代表第二位移信息，Δyawmatch代表第二姿态角的变化信息，
Figure BDA0003390001560000144
代表当前帧最终的位移，
Figure BDA0003390001560000145
代表当前帧最终的姿态角的变化量。
在一些实施例中，步骤S802可以包括：
步骤S8021、获取所述当前帧的点云数据和所述当前帧的前N帧的点云数据，其中，N大于1；
在一些实施例中，N的取值需根据实际实施情况设定，不做统一的限制。
步骤S8022、分别一一确定所述前N帧中每一帧的点云数据与所述当前帧的点云数据之间的N个匹配度；
在一些实施例中，首先，基于步骤S6022方法，将当前帧的点云数据转换为基于距离度量的匹配度度量图
Figure BDA0003390001560000146
其次，分别将前N帧的点云数据P′t-N，P′t-N+1，……，P′t-1一一投影至距离度量图
Figure BDA0003390001560000147
基于步骤S6023至步骤S6024的方法，分别一一确定所述前N帧中每一帧的点云数据与所述当前帧的点云数据之间的N个匹配度结果。
步骤S8023、基于预设的相似度算法和所述N个匹配度，确定N+1帧所有的点云数据的相似度；
在一些实施例中，相似度拟合算法设定为一个三次拟合函数ft(x)。
在本实施例中，首先，基于N个匹配度结果值，采用线性最小二乘拟合算法得到三次拟合函数ft(x)；其次，求三次拟合函数ft(x)在(0，N)范围内的最后一个谷值ft(c)min，这里，最后一个谷值ft(c)min指的是最后一个极小值点，ft(c)min则可代表N+1帧所有的点云数据的相似度。
步骤S8024、基于预设的所述第一位姿变化信息对应的融合权重系数算法和所述点云数据的相似度，确定所述第一位姿变化信息对应的第一权重；
在一些实施例中，式(8-3)表示所述第一位姿变化信息对应的融合权重系数算法：
Figure BDA0003390001560000148
其中，η代表第一权重，ft(x)min代表三次拟合函数的最小值，DisT代表距离阈值。
在本申请实施例中，第一权重
Figure BDA0003390001560000149
步骤S8025、基于预设的所述第二位姿变化信息对应的融合权重系数算法和所述相似度，确定所述第二位姿变化信息对应的第二权重；
在一些实施例中，式(8-4)表示所述第二位姿变化信息对应的融合权重系数算法：
Figure BDA00033900015600001410
其中，Ψ代表第二权重，ft(x)min代表三次拟合函数的最小值，DisT代表距离阈值。
在本申请实施例中，第二权重
Figure BDA0003390001560000151
步骤S8026、基于所述第一位姿变化信息、所述第一权重、所述第二位姿变化信息和所述第二权重进行数据融合，得到所述当前帧的位姿变化信息。
在一些实施例中，所述当前帧的位移可表示为式(8-5)，当前帧的姿态角变化可表示为式(8-6)：
Figure BDA0003390001560000152
Figure BDA0003390001560000153
其中，
Figure BDA0003390001560000154
代表当前帧最终的位移，
Figure BDA0003390001560000155
代表当前帧最终的姿态角的变化量，
Figure BDA0003390001560000156
代表第二位移信息，
Figure BDA0003390001560000157
代表第一位移信息，Δyawmatch代表第二姿态角的变化信息，Δyawmotion代表第一姿态角的变化信息。
步骤S803、基于所述当前帧的位姿变化信息和所述前一帧的位姿信息，确定所述当前帧的位姿信息。
在一些实施例中，当前帧的位置lt可表示为式(8-7)，当前帧的姿态角yawt可表示为式(8-8)：
Figure BDA0003390001560000158
Figure BDA0003390001560000159
其中，lt-1代表当前帧的前一帧的位置，yawt-1代表当前帧的前一帧的姿态角。
本申请实施例中，采用决策级融合的数据融合算法，通过作业设备上装配的毫米波雷达采集多帧点云数据，确定作业设备的第一位姿变化信息和第二位姿变化信息，采用数据融合算法，基于第一位姿变化信息、第一权重、第二位姿变化信息和第二权重进行数据融合，得到当前帧的位姿变化信息。通过决策级融合的数据融合算法，不仅比单一来源决策更加准确可靠，而且还具有较好的鲁棒性以及容错性。
本申请实施例还提供了一种运动轨迹的确定方法，该方法包括图9中的方法：
步骤S901、获取待作业区域当前帧的点云数据和当前帧的全局地图，其中，所述当前帧的点云数据是所述作业设备在所述作业区域内作业采集的点云数据，所述当前帧的全局地图是基于所述当前帧的前一帧的点云数据确定的；
步骤S902、基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息；
步骤S903、基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息；
步骤S904、基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息。
步骤S905、基于所述作业设备的当前帧的位姿信息和所述当前帧的全局地图，确定所述当前帧的下一帧的全局地图；
这里，在一些实施例中，所述当前帧的下一帧的全局地图的确定方法跟当前帧的全局地图的确定方法相同。
步骤S906、基于获取的下一帧的点云数据和所述下一帧的全局地图，确定所述作业设备的下一帧的位姿信息；
在一些实施例中，所述作业设备的下一帧的位姿信息的确定方法跟当前帧的位姿信息的确定方法相同。
步骤S907、基于持续获取的点云数据和持续更新的全局地图，确定所述作业设备的路径点集合，其中，每一所述路径点的属性信息包括对应帧的位姿信息和时间戳；
这里，在一些实施例中，时间戳里包含了每个路径点形成的时间信息。
在一些实施例中，所述点云数据是由作业设备在持续作业过程中不断获得的，与此同时，全局地图也在持续不断的更新，因此，最终可持续得到所述作业设备的多个路径点，多个路径点形成的集合被称为路径点集合。
步骤S908、基于所述路径点集合中的每一所述路径点的时间戳，连接各个所述路径点，得到所述作业设备的运动轨迹。
在一些实施例中，由于时间戳里包含了每个路径点形成的时间信息，因此按照每个路径点的时间戳中时间的先后顺序，将各个路径点连接在一起，便可得到所述作业设备的运动轨迹。
本申请实施例中，通过持续获取的点云数据和持续更新的全局地图，确定了作业设备的路径点集合，并进一步基于所述路径点集合中的每一所述路径点的时间戳，连接各个所述路径点，得到所述作业设备的运动轨迹，采用这种实时获取点云数据，并实时更新全局地图和实时获取路径点的方法，能够更准确地对作业设备进行定位，用于满足自动化作业需求。
本申请实施例提供一种运动轨迹的确定方法，应用于作业设备，实施步骤如下：
步骤S1100、通过毫米波雷达获取待作业水域当前帧的点云数据Pt′和当前帧的全局地图Pmap(t)，其中，所述当前帧的点云数据Pt′是所述无人船在所述水域内作业采集的点云数据，所述当前帧的全局地图Pmap(t)是基于所述当前帧的前一帧的点云数据Pt-1′和所述当前帧的前一帧的全局地图Pmap(t-1)确定的；
步骤S1200、基于所述当前帧的点云数据Pt′，确定所述无人船的第一位姿变化信息；
其中，第一位姿变化信息包括第一位移
Figure BDA0003390001560000161
和第一姿态角的变化量
Figure BDA0003390001560000162
步骤S1300、基于所述当前帧的点云数据Pt′和所述当前帧的全局地图Pmap(t)，确定所述无人船的第二位姿变化信息；
其中，第二位姿变化信息包括第二位移
Figure BDA0003390001560000163
和第二姿态角的变化量Δyawmatch。
步骤S1400、基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述无人船的当前帧的位置lt和姿态角yawt；
步骤S1500、基于所述无人船的当前帧的位置lt、姿态角yawt以及所述当前帧的全局地图Pmap(t)，确定所述当前帧的下一帧的全局地图Pmap(t+1)；
步骤S1600、基于获取的下一帧的点云数据Pt+1′和所述下一帧的全局地图Pmap(t+1)，确定所述无人船的下一帧的位置lt+1和姿态角yawt+1；
步骤S1700、基于持续获取的点云数据和持续更新的全局地图，确定所述无人船的路径点集合{r1,r2,r3,……,rn}，其中，每一所述路径点rn的属性信息包括对应帧的位姿信息和时间戳；
步骤S1800、基于所述路径点集合中的每一所述路径点的时间戳，连接各个所述路径点r，得到所述无人船的运动轨迹Route＝{r1,r2,r3,……,rn}。
在一些实施例中，步骤S1100中“获取当前帧的全局地图Pmap(t)”可以采用如下步骤：
步骤S11001、获取所述当前帧的前一帧的点云数据Pt-1′、所述前一帧的位置lt-1、姿态角yawt-1以及所述前一帧的全局地图Pmap(t-1)；
步骤S11002、基于所述前一帧的位置lt-1和姿态角yawt-1，将所述当前帧的前一帧的点云数据Pt-1′转换到世界坐标系下，得到第一点云数据P′t-1(trans)，其中，将所述当前帧的前一帧的点云数据Pt-1′转换到世界坐标系下，得到世界坐标系下的第一点云数据P′t-1(trans)的实现方法参见公式(10-1)：
P′t-1(trans)＝Rt-1P′t-1+Tt-1 (10-1)；
其中，t代表当前帧的点云数据对应的时刻，t-1代表当前帧的前一帧的点云数据对应的时刻，trans代表将点云数据转换到世界坐标系的操作，P′t-1(trans)代表第一点云数据，P′t-1代表当前帧的前一帧的点云数据，Rt-1代表旋转矩阵，Tt-1代表平移向量
这里，Rt-1代表旋转矩阵，可表示为式(10-2)：
Figure BDA0003390001560000171
步骤S11003、基于所述第一点云数据P′t-1(trans)和所述前一帧的全局地图P′t-1，确定所述当前帧的全局地图Pmap(t)。
这里，当前帧的全局地图Pmap(t)的表达式参见公式(10-3)：
Pmap(t)＝Pmap(t-1)∪P′t-1(trans) (10-3)；
其中，map代表地图，∪代表并集。
需要指出的是，若当前帧为第一帧，则直接将滤波后的当前帧点云数据P1′存入当前帧的全局地图Pmap(t)中，即
Figure BDA0003390001560000181
在一些实施例中，步骤S1200中“基于所述当前帧的点云数据Pt′，确定所述无人船的第一位姿变化信息”的实施包括：
步骤S12001、获取所述无人船的运动模型，其中所述运动模型包括所述无人船沿所述无人船的中轴方向的轴向运动、沿水平方向的水平运动和环绕所述作业设备的中心的圆周运动；
这里，以无人船为原点建立世界坐标系，无人船的运动包括沿无人船的中轴方向的轴向运动，沿无人船的水平方向的水平运动和环绕无人船的中心的圆周运动；其中，轴向运动的运动速度可表示为
Figure BDA0003390001560000182
水平运动的运动速度可表示为
Figure BDA0003390001560000183
圆周运动的运动速度可表示为ωt。
因此，基于所述无人船的运动模型，可以得到在Δt时间内，所述无人船的位移可用公式(10-4)来表示，姿态角的变化量可以用公式(10-5)来表示：
Figure BDA0003390001560000184
Δyawt＝Δt*ωt (10-5)；
其中，Δlt代表当前帧的位移，
Figure BDA0003390001560000185
代表当前帧作业设备沿水平方向运动的水平位移，
Figure BDA0003390001560000186
代表当前帧作业设备沿轴向运动的轴向位移，Δyawt代表当前帧作业设备绕圆周运动的姿态角的变化量。
更多地，
Figure BDA0003390001560000187
和
Figure BDA0003390001560000188
分别可以表示为公式(10-6)和公式(10-7)：
Figure BDA0003390001560000189
Figure BDA00033900015600001810
其中，
Figure BDA00033900015600001811
代表当前帧作业设备沿水平方向运动的水平位移，
Figure BDA00033900015600001812
代表当前帧作业设备沿轴向运动的轴向位移，Δyawt代表当前帧作业设备绕圆周运动的姿态角的变化量。
步骤S12002、基于所述当前帧的点云数据Pt′，确定所述作业设备上雷达的运动速度，其中，毫米波雷达沿x轴运动的速度为
Figure BDA00033900015600001813
毫米波雷达沿y轴运动的速度
Figure BDA00033900015600001814
在实施时，当前帧点云数据Pt′是对毫米波雷达获得的点云数据Pt进行预处理后得到的点云数据，预处理过程如下：
首先，基于直通滤波对点云数据Pt进行滤波，得到点云数据
Figure BDA00033900015600001815
这里，毫米波雷达获得的点云数据为Pt，对于无人船运行前方由于水面波纹而产生的水面杂波点云数据，设定水面上的距离阈值Rthreshold，对在此阈值范围内的点云数据进行直接滤除，得到滤除后的点云数据
Figure BDA00033900015600001816
式(10-8)为点云数据
Figure BDA00033900015600001817
的表达式：
Figure BDA0003390001560000191
其中，
Figure BDA0003390001560000192
代表当前帧点云数据中的点i在雷达坐标系中对应的x轴的坐标值；
Figure BDA0003390001560000193
代表当前帧点云数据中的点i在雷达坐标系中对应的y轴的坐标值，
Figure BDA0003390001560000194
代表点i的数据信息，Rthreshold代表设定的水面上的距离阈值。
其中，Rthreshold＝0.5m。
同时，对于z方向绝对值较大的点，设定z方向的距离阈值为Hthreshold，z方向绝对值大于此阈值的点均被滤除，得到滤除后的点云数据
Figure BDA0003390001560000195
式(10-9)为点云数据
Figure BDA0003390001560000196
的表达式：
Figure BDA0003390001560000197
其中，
Figure BDA0003390001560000198
代表了当前帧点云数据中点i在雷达坐标系中对应的z轴的坐标值，Hthreshold代表设定的z方向的距离阈值。
其中，Hthreshold＝2m。
其次，基于密度聚类滤波对点云数据
Figure BDA0003390001560000199
进行滤波，得到点云数据
Figure BDA00033900015600001910
这里，对邻域点云数据中个数较少的点、分布较为离散的点云进行滤除，详细实施为，计算每个点到其他点之间的距离矩阵Dt，式(10-10)为距离矩阵Dt的表达式为：
Figure BDA00033900015600001911
进一步地，d(pk,pj)的表达式参见公式(10-11)：
Figure BDA00033900015600001912
其中，d(pk,pj)代表当前帧点云中两个点pk和pj之间的欧式距离。
因此，基于密度聚类滤波后的点云集合
Figure BDA00033900015600001913
可表示为式(10-12)：
Figure BDA00033900015600001914
其中，N(·)代表满足该条件的元素个数，tneighbour为设定的邻域大小阈值，tnumber为设定的邻域点云数据中点的个数阈值，Dt(i,∶)为距离矩阵，实施时，若邻域点云数据中的点的数目少于设定阈值，即对其进行滤除，滤除后得到的点云数据为
Figure BDA00033900015600001915
当得到当前帧的点云数据
Figure BDA00033900015600001916
后，可以示例地，在雷达坐标系中存在点云中的一个点
Figure BDA00033900015600001917
Figure BDA00033900015600001918
设
Figure BDA00033900015600001919
为xOy平面上由点pi连接到原点(0,0)的向量与x轴正向的夹角，
Figure BDA00033900015600001920
则点pi对应的多普勒速度可表示为式(10-13)：
Figure BDA00033900015600001921
其中，
Figure BDA0003390001560000201
代表毫米波雷达沿x轴运动的速度；
Figure BDA0003390001560000202
代表毫米波雷达沿y轴运动的速度。
进一步地，在雷达坐标系中存在当前帧的点云数据中的一个点
Figure BDA0003390001560000203
设
Figure BDA0003390001560000204
为xOy平面上由点pk连接到原点(0,0)的向量与x轴负向的夹角，其中，
Figure BDA0003390001560000205
步骤S12003、获取所述雷达的运动速度和所述作业设备的运动速度之间的运动关系；
这里，在无人船的一定位置装配了毫米波雷达，则无人船的运动速度与雷达的运动速度之间的关系式为式(10-14)：
Figure BDA0003390001560000206
其中，dF为雷达装配位置到无人船设定中心的距离，ωt为无人船运动的角速度，vt为无人船运动的线速度，包括无人船沿轴向运动的轴向运动速度
Figure BDA0003390001560000207
和无人船沿水平方向运动的水平速度
Figure BDA0003390001560000208
为雷达相对自身坐标系的运动，包括沿雷达坐标系内x方向的水平速度分量
Figure BDA0003390001560000209
和沿雷达坐标系内y方向的垂直速度分量
Figure BDA00033900015600002010
进一步地，设船只坐标系x方向和雷达坐标系y方向的夹角为θS,雷达安装位置与船体中心的连线与船只坐标系x方向的夹角为θF，则可以推导出雷达的运动速度和和无人船的运动速度之间的运动关系为式(10-15)和(10-16)：
Figure BDA00033900015600002011
Figure BDA00033900015600002012
其中，
Figure BDA00033900015600002013
为沿雷达坐标系内x方向的水平速度分量，
Figure BDA00033900015600002014
为沿雷达坐标系内y方向的垂直速度分量，dF为雷达装配位置到无人船设定中心的距离，ωt为无人船运动的角速度，
Figure BDA00033900015600002015
为无人船沿水平方向运动的水平速度，
Figure BDA00033900015600002016
为无人船沿轴向运动的轴向运动速度。
步骤S12004、基于所述雷达的运动速度和所述运动关系，确定所述作业设备的运动速度。
这里，通过对当前点云集合
Figure BDA00033900015600002017
中的点云通过随机采样一致性算法，再结合最小二乘拟合方法，用点云集合
Figure BDA00033900015600002018
中点云信息拟合得出雷达两个方向的速度
Figure BDA00033900015600002019
和
Figure BDA00033900015600002020
以及作业设备的角速度大小ωt，同时由于动态目标点不满足S12002中提到的雷达点云多普勒速度
Figure BDA00033900015600002021
与雷达运动
Figure BDA00033900015600002022
之间的关系，通过随机采样一致性算法，可以同时滤除点云集合中的动态目标点，得到滤除动态目标后的点云集合Pt′，之后根据雷达运动速度与作业设备运动速度之间的运动关系，得到作业设备的运动速度
Figure BDA00033900015600002023
在一些实施例中，步骤S1300中所述“基于所述当前帧的点云数据Pt′和所述当前帧的全局地图Pmap(t)，确定所述无人船的第二位姿变化信息”的实施包括：
步骤S13001、从所述当前帧的全局地图Pmap(t)提取子地图，得到当前帧的局部地图P(submap,t)；
这里，当前帧的局部地图P(submap，t)是基于当前帧对应的第一位移
Figure BDA00033900015600002024
初步计算一个当前帧对应的第一位置
Figure BDA0003390001560000211
在第一位置lt′周围，从已建立的当前帧的全局地图Pmap中提取大小为25m的正方形子地图，也就是局部地图P(submap,t)＝{(x1,y1),(x2,y2),……,(xN,yN)}。
步骤S13002、将所述当前帧的点云数据Pt′转换到世界坐标系下，得到第二点云数据；
在一些实施例中，将点云数据转换到世界坐标系下的方法为：
步骤S130021、基于当前帧对应的第一位移
Figure BDA0003390001560000212
初步计算当前帧点云数据对应的第一位置lt′以及第一姿态角yawt′，如公式(10-17)和公式(10-18)所示：
Figure BDA0003390001560000213
其中，t代表当前帧的点云数据对应的时刻，l′t代表当前帧点云数据对应的第一位置lt′，lt-1代表当前帧的前一帧点云数据对应的位置lt′，
Figure BDA0003390001560000214
代表当前帧点云数据对应的第一位置变化。
Figure BDA0003390001560000215
其中，t代表当前帧的点云数据对应的时刻，yawt-1代表当前帧的前一帧的点云数据对应的第一姿态角yawt-1，
Figure BDA0003390001560000216
代表当前帧点云数据对应的第一姿态变化。
步骤S130022、基于所述当前帧的第一位置lt′和姿态角yawt′，将所述当前帧的点云数据Pt′转换到世界坐标系下，得到第二点云数据P′t(trans)，其中，将所述当前帧的点云数据Pt′转换到世界坐标系下，得到世界坐标系下的第二点云数据P′t(trans)的实现方法参见公式(10-19)：
P′t(trans)＝Rt′P′t+Tt′ (10-19)；
其中，t代表当前帧的点云数据对应的时刻，trans代表将点云数据转换到世界坐标系的操作，P′t(trans)代表世界坐标系下的点云数据，P′t代表当前帧的点云数据，Rt′代表旋转矩阵，Tt'代表平移向量，Tt′＝l′t。
这里，Rt′代表旋转矩阵，可表示为式(10-20)：
Figure BDA0003390001560000217
步骤S13003、将所述当前帧的局部地图P(submap,t)进行转换，得到基于距离度量的匹配度度量图M(occ,t)；
这里，基于距离度量的匹配度度量图M(occ,t)的获得包括：
首先，需要将局部地图转换为2D占据栅格图M(occ,t)，转换方法为：设定栅格分辨率为0.05m，根据栅格分辨率，将局部地图划分为多个栅格区域，对于栅格中包含点云中点的栅格，设定栅格图值对应为1；对不包含点云中点的栅格，设定栅格图对应值为0。
其次，将2D占据栅格图M(occ,t)转换为基于距离度量的匹配度度量图M(occ,t)。转换方法为：设定与2D占据栅格图M(occ,t)保持同样的栅格分辨率和栅格划分方式，根据栅格分辨率和栅格划分方式，得到基于距离度量的匹配度度量图M(occ,t)。对于M(occ,t)中每个栅格的值，由如下方式决定：若该栅格与其最为临近的被点云中的点占据的栅格的距离大于等于disT，则设定该栅格值为disT；若该栅格与其最为临近的被点云中的点占据的栅格的距离小于disT，则设定该栅格值为对应的点与点之间的距离。其中，disT为设定的距离阈值，该值被设定为10m。
步骤S13004、确定所述第二点云数据在所述匹配度度量图中的坐标信息；
这里，首先，第二点云数据中每个点对应在基于距离度量的匹配度度量图M(mat,t)中的坐标可表示为(h1,w1),(h2,w2),…,(hn,wn)。
其次，根据第二点云数据中每个点对应在基于距离度量的匹配度度量图M(mat,t)中的坐标(h1,w1),(h2,w2),…,(hn,wn)对对应的单元格取值，分别表示为M(mat,t)(h1,w1),M(mat,t)(h2,w2),…,M(mat,t)(hn,wn)；
最后，对单元格值根据式(10-21)求和，得到M；最后根据式(10-22)进行计算，得到匹配度得分(Match Score)。
Figure BDA0003390001560000221
Figure BDA0003390001560000222
其中，disT为设定的距离阈值，n代表n个点。
匹配度得分便可代表所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度。
步骤S13005、基于所述匹配度和所述当前帧的初始位置l′t和初始姿态角yaw′t，确定所述作业设备的预期位置
Figure BDA0003390001560000223
和预期姿态角
Figure BDA0003390001560000224
这里，所述预期位置
Figure BDA0003390001560000225
和预期姿态角
Figure BDA0003390001560000226
采用搜索算法得到。
实施时，首先，在每次搜索前，需要设定作业设备的初始位置和姿态角，设定方法为：设定搜索时的初始位置和姿态角为前一帧推导出的位置和姿态角，加上当前帧的前面两帧的位置和姿态角的变化量，在这里，初始位置可表示为式(10-23)，姿态角可表示为式(10-24)：
l′t＝lt-1+(lt-1-lt-2) (10-23)；
yaw′t＝yawt-1+(yawt-1-yawt-2) (10-24)；
其中，l′t表示当前帧的初始位置，lt-1表示当前帧的前一帧的位置，lt-2表示当前帧的前二帧的位置，yaw′t表示当前帧的初始姿态角，yawt-1表示当前帧的前二帧的姿态角，yawt-2表示当前帧的前二帧的姿态角。
其次，在初始位置l′t和姿态角yaw′t附近采用分支定界法进行搜索，得到使得当前帧的点云数据与所述当前帧的局部地图之间匹配度最高时的作业设备的位置
Figure BDA0003390001560000227
和姿态角
Figure BDA0003390001560000228
步骤S13006、基于所述作业设备的预期位姿信息和所述前一帧的位姿信息，确定所述作业设备的第二位姿变化信息。
这里，采用式(10-25)至(10-26)的方法来求解第二位姿变化信息：
Figure BDA0003390001560000231
Figure BDA0003390001560000232
其中，
Figure BDA0003390001560000233
代表当前帧作业设备的第二位移，Δyawmatch代表当前帧作业设备姿态角的变化量，
Figure BDA0003390001560000234
代表当前帧的点云数据与所述当前帧的局部地图之间匹配度最高时的作业设备的位置，
Figure BDA0003390001560000235
当前帧的点云数据与所述当前帧的局部地图之间匹配度最高时的作业设备的姿态角。
在一些实施例中，步骤S1400中所述“基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述无人船的当前帧的位置lt和姿态角yawt”的实施包括：
步骤S14001、获取所述作业设备的前一帧的位置lt-1和前一帧的姿态角yawt-1；
步骤S14002、获取所述当前帧的点云数据和所述当前帧的前N帧的点云数据，其中，N大于1；
步骤S14003、分别一一确定所述前N帧中每一帧的点云数据与所述当前帧的点云数据之间的N个匹配度；
这里，首先，将当前帧的点云数据转换为基于距离度量的匹配度度量图
Figure BDA0003390001560000236
其次，分别将前N帧的点云数据P′t-N，P′t-N+1，……，P′t-1一一投影至距离度量图
Figure BDA0003390001560000237
之后，分别一一确定所述前N帧中每一帧的点云数据与所述当前帧的点云数据之间的N个匹配度结果。
步骤S14004、基于预设的相似度算法和所述N个匹配度，确定N+1帧所有的点云数据的相似度；
这里，相似度拟合算法设定为一个三次拟合函数ft(x)。
实施时，首先，基于N个匹配度结果值，采用线性最小二乘拟合算法得到三次拟合函数ft(x)；其次，求三次拟合函数ft(x)在(0，N)范围内的最后一个谷值ft(c)min，这里，最后一个谷值ft(c)min指的是最后一个极小值点，ft(c)min则可代表N+1帧所有的点云数据的相似度。
步骤S14005、基于预设的所述第一位姿变化信息对应的融合权重系数算法和所述点云数据的相似度，确定所述第一位姿变化信息对应的第一权重η；
这里，式(10-27)表示所述第一位姿变化信息对应的融合权重系数算法：
Figure BDA0003390001560000238
其中，η代表第一权重，ft(x)min代表三次拟合函数的最小值，DisT代表距离阈值。
步骤S14006、基于预设的所述第二位姿变化信息对应的融合权重系数算法和所述相似度，确定所述第二位姿变化信息对应的第二权重；
式(10-28)表示所述第二位姿变化信息对应的融合权重系数算法：
Figure BDA0003390001560000241
其中，Ψ代表第二权重，ft(x)min代表三次拟合函数的最小值，DisT代表距离阈值。
步骤S14007、基于所述第一位姿变化信息、所述第一权重、所述第二位姿变化信息和所述第二权重进行数据融合，得到所述当前帧的位姿变化信息。
这里，当前帧的位移可表示为式(10-29)，当前帧的姿态角变化可表示为式(10-30)：
Figure BDA0003390001560000242
Figure BDA0003390001560000243
其中，其中，
Figure BDA0003390001560000244
代表当前帧最终的位移，
Figure BDA0003390001560000245
代表当前帧最终的姿态角的变化量，
Figure BDA0003390001560000246
代表第二位移信息，
Figure BDA0003390001560000247
代表第一位移信息，Δyawmatch代表第二姿态角的变化信息，Δyawmotion代表第一姿态角的变化信息。
步骤S14008、基于所述当前帧的位姿变化信息和所述前一帧的位姿信息，确定所述当前帧的位姿信息。
当前帧的位置lt可表示为式(10-31)，当前帧的姿态角yawt可表示为式(10-32)：
Figure BDA0003390001560000248
Figure BDA0003390001560000249
其中，lt-1表示当前帧的前一帧的位置，yawt-1表示当前帧的前一帧的姿态角，
Figure BDA00033900015600002410
代表当前帧最终的位移，
Figure BDA00033900015600002411
代表当前帧最终的姿态角的变化量。
基于前述的实施例，本申请实施例提供一种作业装置，该装置包括所包括的各模块、以及各模块所包括的各子模块，各子模块所包括的各单元，以及各单元所包括的各子单元，都可以通过作业设备来实现；当然也可通过具体的逻辑电路实现；在实施的过程中，处理器可以为中央处理器(CPU)、微处理器(MPU)、数字信号处理器(DSP)或现场可编程门阵列(FPGA)等。
图10为本申请实施例提供的一种作业装置的组成结构示意图，如图10所示，所述作业装置1000包括第一获取模块1010、第一确定模块1020、第二确定模块1030和融合模块1040，其中：
第一获取模块1000，用于获取待作业区域当前帧的点云数据和当前帧的全局地图，其中，所述当前帧的点云数据是所述作业设备在所述作业区域内作业采集的点云数据，所述当前帧的全局地图是基于所述当前帧的前一帧的点云数据确定的；
第一确定模块1020，用于基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息；
第二确定模块1030，用于基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息；
融合模块1040，用于基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息。
在一些实施例中，所述装置还包括第三确定模块，用于基于所述作业设备的当前帧的位姿信息和所述当前帧的全局地图，确定所述当前帧的下一帧的全局地图；第四确定模块，用于基于获取的下一帧的点云数据和所述下一帧的全局地图，确定所述作业设备的下一帧的位姿信息；第五确定模块，用于基于持续获取的点云数据和持续更新的全局地图，确定所述作业设备的路径点集合，其中，每一所述路径点的属性信息包括对应帧的位姿信息和时间戳；连接模块，用于基于所述路径点集合中的每一所述路径点的时间戳，连接各个所述路径点，得到所述作业设备的运动轨迹。
在一些实施例中，所述装置还包括第二获取模块，用于获取所述当前帧的前一帧的点云数据、所述前一帧的位姿信息以及所述前一帧的全局地图；转换模块，用于基于所述前一帧的位姿信息，将所述当前帧的前一帧的点云数据转换到世界坐标系下，得到第一点云数据，其中，所述世界坐标系是以在所述待作业区域内作业的所述作业设备为原点建立的；第六确定模块，用于基于所述第一点云数据和所述前一帧的全局地图，确定所述当前帧的全局地图。
在一些实施例中，所述第一确定模块包括：第一获取子模块，用于获取所述作业设备的运动模型，其中所述运动模型包括所述作业设备沿所述作业设备的中轴方向的轴向运动、沿水平方向的水平运动和环绕所述作业设备的中心的圆周运动；第一确定子模块，用于基于所述当前帧的点云数据，确定所述作业设备的运动速度，其中，所述作业设备的运动速度包括所述作业设备分别沿所述中轴方向、水平方向和圆周方向的速度；第二确定子模块，用于基于所述作业设备的运动模型和所述作业设备的运动速度，确定所述作业设备的第一位姿变化信息，其中，所述第一位姿变化信息包括沿所述中轴方向、所述水平方向的位移信息和姿态角的变化信息。
在一些实施例中，所述第一确定子模块包括：第一确定单元，用于基于所述当前帧的点云数据，确定所述作业设备上雷达的运动速度；第一获取单元，用于获取所述雷达的运动速度和所述作业设备的运动速度之间的运动关系；第二确定单元，用于基于所述雷达的运动速度和所述运动关系，确定所述作业设备的运动速度。
在一些实施例中，所述第二确定模块包括：提取子模块，用于从所述当前帧的全局地图提取子地图，得到当前帧的局部地图；第三确定子模块，用于确定所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度；第四确定子模块，用于基于所述匹配度和所述当前帧的初始位姿信息，确定所述作业设备的预期位姿信息；第五确定子模块，用于基于所述作业设备的预期位姿信息和所述前一帧的位姿信息，确定所述作业设备的第二位姿变化信息。
在一些实施例中，所述第三确定子模块包括：第一转换单元，用于将所述当前帧的点云数据转换到世界坐标系下，得到第二点云数据；第二转换单元，用于将所述当前帧的局部地图进行转换，得到匹配度度量图；第三确定单元，用于确定所述第二点云数据在所述匹配度度量图中的坐标信息；第四确定单元，用于基于预设的匹配度度量算法和所述坐标信息，确定所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度。
在一些实施例中，所述融合模块包括：第二获取子模块，用于获取所述作业设备的前一帧的位姿信息；融合子模块，用于基于所述数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述当前帧的位姿变化信息；第六确定子模块，用于基于所述当前帧的位姿变化信息和所述前一帧的位姿信息，确定所述当前帧的位姿信息。
在一些实施例中，所述融合子模块包括：第二获取单元，用于获取所述当前帧的点云数据和所述当前帧的前N帧的点云数据，其中，N大于1；第五确定单元，用于分别一一确定所述前N帧中每一帧的点云数据与所述当前帧的点云数据之间的N个匹配度；第六确定单元，用于基于预设的相似度算法和所述N个匹配度，确定N+1帧所有的点云数据的相似度；第七确定单元，用于基于预设的所述第一位姿变化信息对应的融合权重系数算法和所述点云数据的相似度，确定所述第一位姿变化信息对应的第一权重；第八确定单元，用于基于预设的所述第二位姿变化信息对应的融合权重系数算法和所述相似度，确定所述第二位姿变化信息对应的第二权重；融合单元，用于基于所述第一位姿变化信息、所述第一权重、所述第二位姿变化信息和所述第二权重进行数据融合，得到所述当前帧的位姿变化信息。
需要说明的是，本申请实施例中，如果以软件功能模块的形式实现上述的作业方法，并作为独立的产品销售或使用时，也可以存储在一个计算机可读取存储介质中。基于这样的理解，本申请实施例的技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得作业设备执行本申请各个实施例所述方法的全部或部分。而前述的存储介质包括：U盘、移动硬盘、只读存储器(Read Only Memory，ROM)、磁碟或者光盘等各种可以存储程序代码的介质。这样，本申请实施例不限制于任何特定的硬件和软件结合。对应地，本申请实施例提供一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现上述实施例中提供的作业方法中的步骤。
这里需要指出的是：以上存储介质和设备实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本申请存储介质和设备实施例中未披露的技术细节，请参照本申请方法实施例的描述而理解。
本申请实施例还可以提供一种芯片，芯片包括处理器，处理器可以从存储器中调用并运行计算机程序，以实现本申请实施例中的作业方法。芯片还可以包括存储器。其中，处理器可以从存储器中调用并运行计算机程序，以实现本申请实施例中的作业方法。其中，存储器可以是独立于处理器的一个单独的器件，也可以集成在处理器中。
本申请实施例还提供了一种计算机程序产品，所述计算机程序产品包括计算机存储介质，所述计算机存储介质存储计算机程序，所述计算机程序包括能够由至少一个处理器执行的指令，当所述指令由所述至少一个处理器执行时实现本申请实施例中的作业方法。
这里需要指出的是：以上作业设备、计算机存储介质、芯片、计算机程序产品、计算机程序实施例的描述，与上述方法实施例的描述是类似的，具有同方法实施例相似的有益效果。对于本申请作业设备、计算机存储介质、芯片、计算机程序产品、计算机程序实施例中未披露的技术细节，请参照本申请方法实施例的描述而理解。上述作业装置、芯片或处理器可以包括以下任一个或多个的集成：特定用途集成电路(Application SpecificIntegrated Circuit，ASIC)、数字信号处理器(Digital Signal Processor，DSP)、数字信号处理装置(Digital Signal Processing Device，DSPD)、可编程逻辑装置(ProgrammableLogic Device，PLD)、现场可编程门阵列(Field Programmable Gate Array，FPGA)、中央处理器(Central Processing Unit，CPU)、图形处理器(Graphics Processing Unit，GPU)、嵌入式神经网络处理器(neural-network processing units，NPU)、控制器、微控制器、微处理器、可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。本领域普通技术人员可以理解：实现上述方法实施例的全部或部分步骤可以通过程序指令相关的硬件来完成，前述的程序可以存储于计算机存储介质中，该程序在执行时，执行包括上述方法实施例的步骤；而前述的存储介质包括：移动存储设备、只读存储器(Read Only Memory，ROM)、磁碟或者光盘等各种可以存储程序代码的介质。或者，本申请上述集成的单元如果以软件功能模块的形式实现并作为独立的产品销售或使用时，也可以存储在一个计算机存储介质中。基于这样的理解，本申请实施例的技术方案本质上或者说对相关技术做出贡献的部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机、服务器、或者网络设备等)执行本申请各个实施例所述方法的全部或部分。而前述的存储介质包括：移动存储设备、ROM、磁碟或者光盘等各种可以存储程序代码的介质。
以上所述，仅为本申请的实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应以所述权利要求的保护范围为准。

Claims (12)

1.一种运动轨迹的确定方法，应用于作业设备，其特征在于，所述方法包括：
获取待作业区域当前帧的点云数据和当前帧的全局地图，其中，所述当前帧的点云数据是所述作业设备在所述作业区域内作业采集的点云数据，所述当前帧的全局地图是基于所述当前帧的前一帧的点云数据确定的；
基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息；
基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息；
基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息。
2.根据权利要求1所述的方法，其特征在于，所述方法还包括：
基于所述作业设备的当前帧的位姿信息和所述当前帧的全局地图，确定所述当前帧的下一帧的全局地图；
基于获取的下一帧的点云数据和所述下一帧的全局地图，确定所述作业设备的下一帧的位姿信息；
基于持续获取的点云数据和持续更新的全局地图，确定所述作业设备的路径点集合，其中，每一所述路径点的属性信息包括对应帧的位姿信息和时间戳；
基于所述路径点集合中的每一所述路径点的时间戳，连接各个所述路径点，得到所述作业设备的运动轨迹。
3.根据权利要求1所述的方法，其特征在于，所述方法还包括：
获取所述当前帧的前一帧的点云数据、所述前一帧的位姿信息以及所述前一帧的全局地图；
基于所述前一帧的位姿信息，将所述当前帧的前一帧的点云数据转换到世界坐标系下，得到第一点云数据，其中，所述世界坐标系是以在所述待作业区域内作业的所述作业设备为原点建立的；
基于所述第一点云数据和所述前一帧的全局地图，确定所述当前帧的全局地图。
4.根据权利要求1所述的方法，其特征在于，所述基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息，包括：
获取所述作业设备的运动模型，其中，所述运动模型包括所述作业设备沿所述作业设备的中轴方向的轴向运动、沿水平方向的水平运动和环绕所述作业设备的中心的圆周运动；
基于所述当前帧的点云数据，确定所述作业设备的运动速度，其中，所述作业设备的运动速度包括所述作业设备分别沿所述中轴方向、水平方向和圆周方向的速度；
基于所述作业设备的运动模型和所述作业设备的运动速度，确定所述作业设备的第一位姿变化信息，其中，所述第一位姿变化信息包括沿所述中轴方向、所述水平方向的位移信息和姿态角的变化信息。
5.根据权利要求4所述的方法，其特征在于，所述基于所述当前帧的点云数据，确定所述作业设备的运动速度，其中，所述作业设备的运动速度包括所述作业设备分别沿所述中轴方向、水平方向和圆周方向的速度，包括：
基于所述当前帧的点云数据，确定所述作业设备上雷达的运动速度；
获取所述雷达的运动速度和所述作业设备的运动速度之间的运动关系；
基于所述雷达的运动速度和所述运动关系，确定所述作业设备的运动速度。
6.根据权利要求1所述的方法，其特征在于，所述基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息，包括：
从所述当前帧的全局地图提取子地图，得到当前帧的局部地图；
确定所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度；
基于所述匹配度和所述当前帧的初始位姿信息，确定所述作业设备的预期位姿信息；
基于所述作业设备的预期位姿信息和所述前一帧的位姿信息，确定所述作业设备的第二位姿变化信息。
7.根据权利要求6所述的方法，其特征在于，所述当前帧的点云数据是在所述雷达的雷达坐标系下表示的，所述确定所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度，包括：
将所述当前帧的点云数据转换到世界坐标系下，得到第二点云数据；
将所述当前帧的局部地图进行转换，得到匹配度度量图；
确定所述第二点云数据在所述匹配度度量图中的坐标信息；
基于预设的匹配度度量算法和所述坐标信息，确定所述当前帧的点云数据与所述当前帧的局部地图之间的匹配度。
8.根据权利要求1所述的方法，其特征在于，所述基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息，包括：
获取所述作业设备的前一帧的位姿信息；
基于所述数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述当前帧的位姿变化信息；
基于所述当前帧的位姿变化信息和所述前一帧的位姿信息，确定所述当前帧的位姿信息。
9.根据权利要求8所述的方法，其特征在于，所述基于所述数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述当前帧的位姿变化信息，包括：
获取所述当前帧的点云数据和所述当前帧的前N帧的点云数据，其中，N大于1；
分别一一确定所述前N帧中每一帧的点云数据与所述当前帧的点云数据之间的N个匹配度；
基于预设的相似度算法和所述N个匹配度，确定N+1帧所有的点云数据的相似度；
基于预设的所述第一位姿变化信息对应的融合权重系数算法和所述点云数据的相似度，确定所述第一位姿变化信息对应的第一权重；
基于预设的所述第二位姿变化信息对应的融合权重系数算法和所述相似度，确定所述第二位姿变化信息对应的第二权重；
基于所述第一位姿变化信息、所述第一权重、所述第二位姿变化信息和所述第二权重进行数据融合，得到所述当前帧的位姿变化信息。
10.一种作业装置，其特征在于，所述装置包括：
第一获取模块，用于获取待作业区域当前帧的点云数据和当前帧的全局地图，其中，所述当前帧的点云数据是所述作业设备在所述作业区域内作业采集的点云数据，所述当前帧的全局地图是基于所述当前帧的前一帧的点云数据确定的；
第一确定模块，用于基于所述当前帧的点云数据，确定所述作业设备的第一位姿变化信息；
第二确定模块，用于基于所述当前帧的点云数据和所述当前帧的全局地图，确定所述作业设备的第二位姿变化信息；
融合模块，用于基于预设的数据融合算法，将所述第一位姿变化信息和所述第二位姿变化信息进行数据融合，得到所述作业设备的当前帧的位姿信息。
11.一种作业设备，其特征在于，包括：存储器和处理器，
所述存储器存储有可在所述处理器上运行的计算机程序，
所述处理器执行所述计算机程序时实现权利要求1至9任一项所述方法中的步骤。
12.一种计算机可读存储介质，其上存储有计算机程序，其特征在于，该计算机程序被处理器执行时实现权利要求1至9任一项所述作业方法中的步骤。
CN202111463035.5A 2021-12-02 2021-12-02 一种运动轨迹的确定方法、作业设备、装置、存储介质 Active CN114485607B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111463035.5A CN114485607B (zh) 2021-12-02 2021-12-02 一种运动轨迹的确定方法、作业设备、装置、存储介质

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111463035.5A CN114485607B (zh) 2021-12-02 2021-12-02 一种运动轨迹的确定方法、作业设备、装置、存储介质

Publications (2)

Publication Number Publication Date
CN114485607A true CN114485607A (zh) 2022-05-13
CN114485607B CN114485607B (zh) 2023-11-10

Family

ID=81492838

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111463035.5A Active CN114485607B (zh) 2021-12-02 2021-12-02 一种运动轨迹的确定方法、作业设备、装置、存储介质

Country Status (1)

Country Link
CN (1) CN114485607B (zh)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116359938A (zh) * 2023-05-31 2023-06-30 未来机器人（深圳）有限公司 对象检测方法、装置及运载装置

Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN108921925A (zh) * 2018-06-27 2018-11-30 广州视源电子科技股份有限公司 基于激光雷达和视觉融合的语义点云生成方法和装置
CN109073385A (zh) * 2017-12-20 2018-12-21 深圳市大疆创新科技有限公司 一种基于视觉的定位方法及飞行器
CN109556611A (zh) * 2018-11-30 2019-04-02 广州高新兴机器人有限公司 一种基于图优化和粒子滤波的融合定位方法
CN110954114A (zh) * 2019-11-26 2020-04-03 苏州智加科技有限公司 电子地图的生成方法、装置、终端及存储介质
CN110992401A (zh) * 2019-11-25 2020-04-10 上海眼控科技股份有限公司 目标跟踪方法、装置、计算机设备和存储介质
CN111442722A (zh) * 2020-03-26 2020-07-24 达闼科技成都有限公司 定位方法、装置、存储介质及电子设备
WO2020150896A1 (zh) * 2019-01-22 2020-07-30 深圳大学 视频目标的数据关联方法、装置及存储介质
WO2020259481A1 (zh) * 2019-06-27 2020-12-30 Oppo广东移动通信有限公司 定位方法及装置、电子设备、可读存储介质
WO2021097983A1 (zh) * 2019-11-21 2021-05-27 广州文远知行科技有限公司 定位的方法、装置、设备及存储介质
CN113324542A (zh) * 2021-06-07 2021-08-31 北京京东乾石科技有限公司 一种定位方法、装置、设备和存储介质

Patent Citations (10)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109073385A (zh) * 2017-12-20 2018-12-21 深圳市大疆创新科技有限公司 一种基于视觉的定位方法及飞行器
CN108921925A (zh) * 2018-06-27 2018-11-30 广州视源电子科技股份有限公司 基于激光雷达和视觉融合的语义点云生成方法和装置
CN109556611A (zh) * 2018-11-30 2019-04-02 广州高新兴机器人有限公司 一种基于图优化和粒子滤波的融合定位方法
WO2020150896A1 (zh) * 2019-01-22 2020-07-30 深圳大学 视频目标的数据关联方法、装置及存储介质
WO2020259481A1 (zh) * 2019-06-27 2020-12-30 Oppo广东移动通信有限公司 定位方法及装置、电子设备、可读存储介质
WO2021097983A1 (zh) * 2019-11-21 2021-05-27 广州文远知行科技有限公司 定位的方法、装置、设备及存储介质
CN110992401A (zh) * 2019-11-25 2020-04-10 上海眼控科技股份有限公司 目标跟踪方法、装置、计算机设备和存储介质
CN110954114A (zh) * 2019-11-26 2020-04-03 苏州智加科技有限公司 电子地图的生成方法、装置、终端及存储介质
CN111442722A (zh) * 2020-03-26 2020-07-24 达闼科技成都有限公司 定位方法、装置、存储介质及电子设备
CN113324542A (zh) * 2021-06-07 2021-08-31 北京京东乾石科技有限公司 一种定位方法、装置、设备和存储介质

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
HU YIN等: "Robust tracking algorithm based on model matching and region correlation fusion", 《 COMPUTER ENGINEERING AND APPLICATIONS 43 (33) 》 *
吴玮;郑娟毅;杜乐;: "多特征融合的Camshift运动目标跟踪算法", 计算机科学, no. 07 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN116359938A (zh) * 2023-05-31 2023-06-30 未来机器人（深圳）有限公司 对象检测方法、装置及运载装置
CN116359938B (zh) * 2023-05-31 2023-08-25 未来机器人（深圳）有限公司 对象检测方法、装置及运载装置

Also Published As

Publication number Publication date
CN114485607B (zh) 2023-11-10

Similar Documents

Publication Publication Date Title
US20220028163A1 (en) Computer Vision Systems and Methods for Detecting and Modeling Features of Structures in Images
Aldera et al. Fast radar motion estimation with a learnt focus of attention using weak supervision
Liu et al. Ground surface filtering of 3D point clouds based on hybrid regression technique
CN112740225B (zh) 一种路面要素确定方法及装置
US20230236280A1 (en) Method and system for positioning indoor autonomous mobile robot
CN113743385A (zh) 一种无人船水面目标检测方法、装置及无人船
CN114325634A (zh) 一种基于激光雷达的高鲁棒性野外环境下可通行区域提取方法
CN111811502B (zh) 一种运动载体多源信息融合导航方法及系统
Omrani et al. Dynamic and static object detection and tracking in an autonomous surface vehicle
Xu et al. Dynamic vehicle pose estimation and tracking based on motion feedback for LiDARs
CN114485607A (zh) 一种运动轨迹的确定方法、作业设备、装置、存储介质
CN112802195B (zh) 一种基于声呐的水下机器人连续占据建图方法
Yao et al. LiDAR based navigable region detection for unmanned surface vehicles
Chen et al. High-Precision Positioning, Perception and Safe Navigation for Automated Heavy-duty Mining Trucks
Xiao et al. Detection and tracking method of maritime moving targets based on geosynchronous orbit satellite optical images
CN113483769B (zh) 基于粒子滤波器的车辆自定位方法、系统、设备和介质
CN113554705B (zh) 一种变化场景下的激光雷达鲁棒定位方法
CN112731335B (zh) 一种基于全区域激光扫描的多无人机协作定位方法
CN112747752B (zh) 基于激光里程计的车辆定位方法、装置、设备和存储介质
Eynard et al. UAV Motion Estimation using Hybrid Stereoscopic Vision.
CN111239761B (zh) 一种用于室内实时建立二维地图的方法
Šuľaj et al. Examples of real-time UAV data processing with cloud computing
Wang et al. Simultaneous Location of Rail Vehicles and Mapping of Environment with Multiple LiDARs
Baig et al. Advances in the Bayesian Occupancy Filter framework using robust motion detection technique for dynamic environment monitoring
Wang et al. Raillomer: Rail vehicle localization and mapping with LiDAR-IMU-odometer-GNSS data fusion

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