CN114353780B - 姿态优化方法及设备 - Google Patents
姿态优化方法及设备 Download PDFInfo
- Publication number
- CN114353780B CN114353780B CN202111666849.9A CN202111666849A CN114353780B CN 114353780 B CN114353780 B CN 114353780B CN 202111666849 A CN202111666849 A CN 202111666849A CN 114353780 B CN114353780 B CN 114353780B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- cloud data
- environment
- detectors
- environment detectors
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 137
- 238000005457 optimization Methods 0.000 title claims abstract description 31
- 230000008569 process Effects 0.000 claims abstract description 95
- 230000007613 environmental effect Effects 0.000 claims description 31
- 238000004590 computer program Methods 0.000 claims description 29
- 238000012937 correction Methods 0.000 claims description 23
- 239000013598 vector Substances 0.000 claims description 12
- 230000014509 gene expression Effects 0.000 claims description 10
- 238000013178 mathematical model Methods 0.000 claims description 9
- 238000000605 extraction Methods 0.000 abstract description 9
- 238000001514 detection method Methods 0.000 description 35
- 230000036544 posture Effects 0.000 description 35
- 238000010586 diagram Methods 0.000 description 21
- 230000003287 optical effect Effects 0.000 description 18
- 239000011159 matrix material Substances 0.000 description 14
- 238000012545 processing Methods 0.000 description 11
- 238000003860 storage Methods 0.000 description 10
- 238000004891 communication Methods 0.000 description 9
- 238000005516 engineering process Methods 0.000 description 9
- 230000006870 function Effects 0.000 description 8
- 238000013519 translation Methods 0.000 description 7
- 238000004364 calculation method Methods 0.000 description 5
- 238000013480 data collection Methods 0.000 description 5
- 230000000694 effects Effects 0.000 description 4
- 230000005236 sound signal Effects 0.000 description 4
- 230000009471 action Effects 0.000 description 3
- 125000003275 alpha amino acid group Chemical group 0.000 description 3
- 238000004458 analytical method Methods 0.000 description 3
- 238000006243 chemical reaction Methods 0.000 description 3
- 150000001875 compounds Chemical class 0.000 description 3
- 230000005540 biological transmission Effects 0.000 description 2
- 238000009826 distribution Methods 0.000 description 2
- 230000009977 dual effect Effects 0.000 description 2
- 230000006872 improvement Effects 0.000 description 2
- 230000010365 information processing Effects 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000007726 management method Methods 0.000 description 2
- 238000013507 mapping Methods 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000003672 processing method Methods 0.000 description 2
- 230000003068 static effect Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- CLOMYZFHNHFSIQ-UHFFFAOYSA-N clonixin Chemical compound CC1=C(Cl)C=CC=C1NC1=NC=CC=C1C(O)=O CLOMYZFHNHFSIQ-UHFFFAOYSA-N 0.000 description 1
- 238000013499 data model Methods 0.000 description 1
- 230000003247 decreasing effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 239000000428 dust Substances 0.000 description 1
- 239000004973 liquid crystal related substance Substances 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 239000000523 sample Substances 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000002366 time-of-flight method Methods 0.000 description 1
- 230000009466 transformation Effects 0.000 description 1
Landscapes
- Length Measuring Devices By Optical Means (AREA)
Abstract
本申请实施例提供一种姿态优化方法及设备。在本申请实施例中，可利用不同环境探测器在不同时刻可扫描到同一对象的特点，对多个环境探测器采集的各帧点云数据进行特征提取，得到各帧点云数据中多个环境探测器对应的特征点；通过对多个环境探测器对应的特征点进行匹配，得到多个环境探测器对应的特征点中的同名点；并以多个环境探测器对应的同名点在同一坐标下的坐标之间的距离最小化为目标，根据同名点的点云坐标和多个环境探测器在采集同名点过程中记录的位姿信息，对多个环境探测器在采集点云数据过程中记录的姿态信息进行矫正，实现了对环境探测器的姿态进行优化，有助于提高环境探测器的姿态精度。
Description
技术领域
本申请涉及计算机技术领域，尤其涉及一种姿态优化方法及设备。
背景技术
随着智能终端的不断发展和普及，地图应用类软件被广泛安装与应用。基于数据采集设备沿路采集的资料数据，进行电子地图数据的制作被广泛应用。基于资料数据构建地图过程中，需要从资料数据中识别出相应的地理要素及其几何数据，比如，道路及其几何数据(包括道路的形状、方向和位置等数据)。为了从资料数据中，能够准确地获取相应地理要素及其几何数据，需要获取数据采集设备的位姿信息。
电子地图的精度很大程度上依赖数据采集设备的轨迹精度，轨迹精度分为位置精度和姿态精度。因此，数据采集设备的姿态精度对电子地图的精度具有至关重要的影响。综前所述，如何提高数据采集设备的姿态精度成为本领域技术人员持续研究的问题。
发明内容
本申请的多个方面提供一种姿态优化方法及设备，用以对环境探测器的姿态进行优化，有助于提高环境探测器的姿态精度。
本申请实施例提供一种姿态优化方法，包括：
获取多个环境探测器采集的各帧点云数据；
对所述各帧点云数据进行特征提取，以从所述各帧点云数据中获取所述多个环境探测器对应的特征点；
对所述多个环境探测器对应的特征点进行匹配，以确定所述多个环境探测器对应的特征点中的同名点；其中，所述同名点为所述多个环境探测器采集的所述各帧点云数据中与现实世界的同一物理点相对应的数据点；
获取所述多个环境探测器采集所述同名点过程中记录的位姿信息；
根据所述多个环境探测器对应同名点的点云坐标和所述多个环境探测器采集所述同名点过程中记录的位姿信息，对多个环境探测器采集所述点云数据过程中记录的姿态进行优化。
本申请实施例还提供一种计算机设备，包括：存储器和处理器；其中，所述存储器，用于存储计算机程序；
所述处理器耦合至所述存储器，用于执行所述计算机程序以用于执行上述姿态优化方法中的步骤。
本申请实施例还提供一种数据采集设备，包括：机械本体；所述机械本体上设置有存储器、处理器及多个环境探测器；
所述多个环境探测器，用于采集点云数据；
所述存储器，用于存储计算机程序；
所述处理器耦合至所述存储器，用于执行所述计算机程序以用于执行上述姿态优化方法中的步骤。
本申请实施例还提供一种存储有计算机指令的计算机可读存储介质，当计算机指令被一个或多个处理器执行时，致使一个或多个处理器执行上述姿态优化方法中的步骤。
本申请实施例还提供一种计算机程序产品，包括：计算机程序。其中，计算机程序产品被处理器执行可实现上述姿态优化方法。
在本申请实施例中，利用不同环境探测器在不同时刻可扫描到同一对象的特点，对多个环境探测器采集的各帧点云数据进行特征提取，得到各帧点云数据中多个环境探测器对应的特征点；通过对多个环境探测器对应的特征点进行匹配，得到多个环境探测器对应的特征点中的同名点；并以多个环境探测器对应的同名点在同一坐标下的坐标之间的距离最小化为目标，根据多个探测器对应的同名点的点云坐标和多个探测器采集同名点过程中的位姿信息，对多个环境探测器采集所述点云数据过程中记录的姿态信息进行矫正，实现了对环境探测器的姿态进行优化，有助于提高环境探测器的姿态精度。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1a为本申请实施例提供的数据采集设备的工作环境示意图；
图1b为本申请实施例提供的数据采集设备的结构示意图；
图2为本申请实施例提供的环境探测器的设置方式示意图；
图3为本申请实施例提供的双环境探测器相邻帧扫描示意图；
图4为本申请实施例提供的双单线激光雷达扫描的点云数据特征示意图；
图5为利用优化前后的位姿对图4扫描的点云数据坐标转换的效果示意图；
图6为本申请实施例提供的姿态优化方法的流程示意图；
图7为本申请实施例提供的计算机设备的结构示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请具体实施例及相应的附图对本申请技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
在电子地图领域，为了制作电子地图数据，经常使用数据采集设备沿路采集环境资料数据，并基于这些环境资料数据制作电子地图数据。为了从资料数据中，能够准确地获取相应地理要素及其几何数据，需要获取数据采集设备的位姿信息。
电子地图的精度很大程度上依赖数据采集设备的轨迹精度，轨迹精度分为位置精度和姿态精度。因此，数据采集设备的姿态精度对电子地图的精度具有至关重要的影响。在现有技术中，可采用雷达测距和建图(Lidar odometry and mapping，LOAM)方法可对激光雷达的姿态进行校正，但是LOAM方法要求激光雷达采集的前后帧数据重叠，不适用于采集的前后帧数据不重叠的激光雷达的姿态校正。
综前所述，亟需提供一种对激光雷达采集的前后帧数据无限制的姿态优化方法。为了解决上述技术问题，在本申请一些实施例中，利用不同环境探测器在不同时刻可扫描到同一对象的特点，对多个环境探测器采集的各帧点云数据进行特征提取，得到各帧点云数据中多个环境探测器对应的特征点；通过对多个环境探测器对应的特征点进行匹配，得到多个环境探测器对应的特征点中的同名点；并以多个环境探测器对应的同名点在同一坐标下的坐标之间的距离最小化为目标，根据多个探测器对应的同名点的点云坐标和所述多个环境探测器采集所述同名点过程中的位姿信息，对多个环境探测器采集所述点云数据过程中的姿态进行矫正，实现了对环境探测器的姿态进行优化，有助于提高环境探测器的姿态精度。
另一方面，本申请实施例利用不同环境探测器在不同时刻可扫描到同一对象的特点，对环境探测器的姿态进行优化，对同一环境探测器采集的前后帧点云数据是否重叠无限制，提供了一种对激光雷达采集的前后帧数据无限制的姿态优化方法。
以下结合附图，详细说明本申请各实施例提供的技术方案。
应注意到：相同的标号在下面的附图以及实施例中表示同一物体，因此，一旦某一物体在一个附图或实施例中被定义，则在随后的附图和实施例中不需要对其进行进一步讨论。
图1a为本申请实施例提供的数据采集设备的工作环境示例图。图1b为本申请实施例提供的数据采集设备的结构示意图。如图1a和图1b所示，数据采集设备S1包括：机械本体10及设置于机械本体上的多个环境探测器11。多个是指2个或2个以上。多个环境探测器11可呈一定角度设置于机械本体10上，其设置方式如图2所示。图1a和图2仅以环境探测器11的数量为2个进行图示，但不构成限定。
机械本体10是数据采集设备的执行机构，主要是指数据采集设备的机身。可以在确定的环境中执行指定的操作。其中，机械本体10一定程度上体现了数据采集设备的外观形态。在本实施例中，并不限定数据采集设备的外观形态。例如，数据采集设备可以数据采集车、数据采集用无人机，或者人形或非人形机器人等。
在本实施例中，环境探测器11主要是指可采集环境信息的电子器件。环境探测器11可为视觉传感器或雷达等。其中，视觉传感器可为摄像头等；雷达可为微波雷达、毫米波雷达或激光雷达等。激光雷达又可为单线激光雷达或多线激光雷达。
在本实施例中，如图1b所示，机械本体10上还设置有数据采集设备的一些基本组件，如驱动组件12等。可选地，驱动组件12可以包括驱动轮、驱动电机、万向轮等。
在本实施例中，如图1a所示，多个环境探测器11可采集环境信息，得到点云数据。每个环境探测器11均可采集点云数据，得到多个环境探测器11各自采集的点云数据。环境探测器11可随数据采集设备的移动而移动，并在数据采集设备移动过程中采集环境信息，得到点云数据。一帧点云数据为环境探测器11一次环境探测得到的数据集合，包括：探测点在环境探测器11坐标系下的空间坐标。其中，对于视觉传感器来说，视觉传感器采集的环境图像的像素点组成点云数据，每个像素点对应一个探测点，每个像素点可作为点云数据中的一个数据点。对于雷达来说，点云数据为雷达扫描一周得到的数据集合。下面以激光雷达为例，对点云数据进行说明。
对于激光雷达来说，可对数据采集设备当前位置的周围环境进行扫描，得到点云数据。在本实施例中，激光雷达包括激光发射机、光接收机和信息处理***，其中，激光发射机用于发射激光探测信号，该探测激光信号遇到障碍物时会反射回来光回波信号，光接收机可接收该光回波信号。之后，信息处理***便可根据反射回来的光回波信号获得目标的有关信息，例如激光雷达与目标之间的距离、目标的方位、高度和形状等参数。
在本实施例中，激光探测信号遇到障碍物其实质为：激光探测信号遇到障碍物上的某一点。为了便于描述和区分，将激光探测信号在传播过程中实际遇到的障碍点，定义为探测点。在本实施例中，探测点可能为目标对象上的某一点，也可能属于目标对象之外的其它对象，例如空气中的灰尘等等。其中，每个激光探测信号在遇到一个探测点时，可返回对应的光回波信号。雷达便可根据激光探测信号与光回波信号之间的差异，获取激光雷达与探测点之间的距离。其中，雷达所发射的激光探测信号不同，获取激光雷达与探测点之间的距离的方式也有所差异。
例如，若激光雷达发射的激光探测信号为激光脉冲信号，则可根据雷达发出的探测的激光脉冲信号与接收到的光回波信号之间的时间差，计算雷达与探测点之间的距离。即利用飞行时间法计算激光雷达与探测点之间的距离。可选地，已知激光脉冲信号和光回波信号在大气传播中的速度，则根据激光雷达发出的激光脉冲信号与接收到的光回波信号之间的时间差以及激光秒冲信号和光回波信号在大气传播中的速度，便可计算出激光雷达与探测点之间的距离。又例如，若激光雷达发射的激光探测信号为连续光信号，则可根据雷达发出的连续光信号与接收到的光回波信号之间的频率差，计算激光雷达与探测点之间的距离。可选地，连续波为调频连续波(Frequency Modulated Continuous Wave，FMCW)。其中，调频方式可以为三角波调频、锯齿波调频、编码调制或者噪声调频等，但不限于此。
进一步，可基于激光雷达发射的激光探测信号和接收到的光回波信号，得到探测点的空间坐标信息，多个探测点的空间坐标信息便组成点云数据，即点云数据为一系列的空间坐标点组成的集合。可选地，可根据激光雷达与探测点之间的距离以及激光雷达的位姿，计算探测点对应的数据点。其中，激光雷达的位姿是指激光雷达的位置和姿态。进一步，激光雷达的姿态可以是指激光发射机发射的激光光束的方向性。进一步，根据激光发射机发射的激光光束的方向性，可获得探测点相较于激光雷达的方向；进而根据探测点相较于激光雷达的方向、激光雷达与探测点之间的距离以及激光雷达的位置，可计算出探测点在激光雷达坐标系下的空间坐标，作为探测点对应的点云数据中的一个数据点。
在一些实施例中，数据采集设备可单纯进行数据采集，将采集到的点云数据提供给其它计算机设备处理。在另一些实施例中，数据采集设备也可具有数据处理功能。例如，数据采集设备可为自主移动设备，如自动驾驶车辆、机器人或无人机等。自主移动设备在移动过程中，可采集环境信息，构建环境地图；并基于构建的环境地图进行路线规划等等。当然，数据采集设备也可为其它可移动设备，如需要驾驶员驾驶的车辆等。
下面以具有数据处理功能的数据采集设备为例，对本申请实施例提供的姿态优化方法进行示例性说明。
如图1b所示，数据采集设备S1还可包括：设置于机械本体10上的存储器13和处理器14。值得说明的是，存储器13和处理器14可设置于机械本体10内部，也可以设置于机械本体10的表面。
在本实施例中，存储器13可存储计算机程序。计算机程序可被处理器14执行，致使处理器14实现相应功能或控制数据采集设备完成相应动作或任务等。
处理器14可以看作是数据采集设备的控制***，可用于执行存储器13中存储的计算机程序，以控制数据采集设备实现相应功能、完成相应动作或任务等。
在本实施例中，处理器14可获取多个环境探测器11采集的各帧点云数据。其中，每个环境探测器11对应一帧点云数据。考虑到一帧点云数据的数据量较大，且存在噪声点，为了降低计算量和噪声，在本实施例中，处理器14可对各帧点云数据进行特征提取，以从各帧点云数据中获取多个环境探测器11对应的特征点。其中，从哪个环境探测器采集的点云数据中获取的特征点，即为该环境探测器11对应的特征点。
可选地，处理器14可根据各帧点云数据中的数据点的点云坐标，计算数据点的曲率。可选地，针对任一数据点A，处理器14可根据数据点A所属的点云数据中的数据点的点云坐标，计算与数据点A属于同一帧点云数据中的其它数据点B与数据点A之间的距离；并从其它数据点B中，选择与数据点A之间的距离满足设定要求的数据点C。可选地，可从数据点B中，选择与数据点A之间的距离小于或等于设定的距离阈值的数据点，作为数据点C。或者，也可按照与数据点A之间的距离从小到大的顺序，选择设定数量的数据点，作为数据点C。进一步，可根据数据点C和数据点A的坐标，计算数据点A的曲率。
基于各帧点云数据中数据点的曲率，处理器14可从每帧点云数据中获取曲率满足设定要求的数据点，作为采集该帧点云数据的环境探测器11对应的特征点。可选地，处理器14可从每帧点云数据中获取曲率大于或等于设定的第一曲率阈值的边缘点，作为采集该帧点云数据的环境探测器11对应的特征点；和/或，从每帧点云数据中获取曲率小于设定的第二曲率阈值的平面点，作为采集该帧点云数据的环境探测器11对应的特征点；其中，第一曲率阈值大于第二曲率阈值。
进一步，多个环境探测器11呈一定角度设置于机械本体10上，可在不同时刻观测到同一对象。如图3所示，虚线表示第一环境探测器11a在T1和T2时刻的扫描示意图；实线表示第二环境探测器11b在T1和T2时刻的扫描示意图。从图3可得，第一环境探测器11a和第二环境探测器11b可分别在T1时刻和T2时刻观测到对象P1；且分别在T2时刻和T1时刻观测到对象P2。基于此，在本实施例中，处理器14可对多个环境探测器11对应的特征点进行匹配，确定多个环境探测器11对应的特征点中的同名点。其中，同名点是指多个环境探测器11采集的各帧点云数据中与现实世界的同一物理点相对应的数据点。如图4中2个环境探测器采集的隧道场景下的点云数据特征示意图所示，数据点A和B为一对同名点。其中，图4中所示的点云数据可由双单线激光雷达扫描得到，但不进行限定。
由于一对同名点在现实世界的物理点为同一物理点，因此，处理器14在对多个环境探测器11对应的特征点进行匹配时，可先将不同环境探测器11采集的点云数据转换至同一设定坐标系下，如世界坐标系。针对任一环境探测器11采集的点云数据的任一数据点A，可采用下述式(1)将其点云坐标转换至设定坐标系下，式(1)可表示为：
因此，在理想条件下，对于一对同名点A与B来说，其在设定坐标系下可满足：
在式(1)中，Pl表示数据点A在点云数据中的点云坐标，即数据点A在采集该帧点云数据的环境探测器坐标系下的坐标；和/>表示环境探测器坐标系与设定坐标系(如世界坐标系)之间的姿态矩阵与平移矩阵。在式(1)和式(2)中，Pw表示数据点A在设定坐标系下的坐标，Pw0表示数据点A的同名点B在设定坐标系下的坐标。
在一些实施例中，如图1b所示，数据采集设备S1的机械本体10上设置有组合导航模块15。组合导航模块15可采集环境探测器11的位姿(即位置和姿态)。组合导航模块15可包括：惯性传感器(IMU)、定位单元和轮速计等，但不限于此。组合导航模块15采集的环境探测器11的位姿为IMU坐标系与世界坐标系之间的姿态矩阵和平移矩阵。因此，为了降低计算量，可将环境探测器11采集的点云数据转换到IMU坐标系下，其中，转换公式可表示为：
在式(3)中，Pl表示数据点A在点云数据中的点云坐标，即数据点A在采集该帧点云数据的环境探测器坐标系下的坐标；Pi表示数据点A在IMU坐标系下的坐标；和/>表示环境探测器坐标系与IMU坐标系之间的姿态矩阵与平移矩阵。其中，由于环境探测器11的旋转平面与组合导航模块15之间的相对位置关系固定，因此，/>和/>可提前标定得到。
基于上述式(3)可环境探测器11采集的点云数据转换到IMU坐标系下，在对多个环境探测器11对应的特征点进行匹配时，可先将不同环境探测器11采集的数据点在IMU坐标系下的坐标转换至同一设定坐标系下，如世界坐标系，则针对任一数据点A，可采用下述式(4)将其点云坐标转换至设定坐标系下，式(4)可表示为：
在式(4)中，和/>表示IMU坐标系与设定坐标系(如世界坐标系)之间的姿态矩阵与平移矩阵。
在理论条件下，对于一对同名点A和B来说，可满足下述条件：
在式(5)中，Pi和Pi0表示数据点A和数据点A的同名点B在IMU坐标系下的坐标。第一环境探测器11a为采集数据点A的环境探测器；第二环境探测器11b为采集数据点A的同名点B的环境探测器，则在式(5)中，和/>分别为第一环境探测器11a采集数据点A过程中的姿态和位置(即位姿)，即第一环境探测器11a对应的IMU坐标系与设定坐标系(如世界坐标系)之间的姿态矩阵与平移矩阵；/>和/>分别为第二环境探测器11b采集数据点A的同名点B过程中的姿态和位置(即位姿)，即第二环境探测器11b对应的IMU坐标系与设定坐标系(如世界坐标系)之间的姿态矩阵与平移矩阵。通过式(5)等号左右侧的公式可将Pi和Pi0转换到同一设定坐标系下，如世界坐标系，即同名点A和B在同一设定坐标系下的坐标相等，为同一点。
基于上述分析，在本实施例中，组合导航模块15可采集多个环境探测器11在采集点云数据过程中的位姿信息。处理器14可获取多个环境探测器11在采集点云数据过程中的位姿信息。
在对多个环境探测器11对应的特征点进行匹配时，处理器14可将多个环境探测器采集的各帧点云数据中的特征点进行匹配。但是，这样计算量较大。考虑到采集位置距离较远的点云数据所采集的地理区域的不同，针对不同的地理区域或者同一地理区域中的不同子区域所采集的点云数据之间的特征点之间的匹配度较低。基于此，为了降低计算量，可处理器14可获取多个环境探测器11在采集点云数据过程中的位姿信息；并根据位姿信息中的位置信息，计算不同环境探测器对应的点云数据的采集位置之间的距离；并从不同环境探测器对应的点云数据中，选择采集位置之间的距离小于或等于设定的距离阈值的目标点云数据。环境探测器对应的点云数据，即为该环境探测器采集的点云数据。
进一步，可从多个环境探测器11对应的特征点中获取属于目标点云数据的目标特征点；并根据多个环境探测器在采集目标特征点过程中的位姿信息及目标特征点的点云坐标，计算目标特征点在设定坐标系下的位置信息。在本实施例中，可采用上述式(3)将目标特征点的点云坐标，转换为目标特征点在IMU坐标系下的坐标；进一步，根据上述式(4)计算出目标特征点在设定坐标系下的位置信息。在计算目标特征点在设定坐标系下的位置信息时，式(4)中的和/>分别采用环境探测器在采集目标特征点过程中的姿态和位置信息。
进一步，处理器14可根据目标特征点在设定坐标系下的位置信息，确定多个环境探测器11对应的目标特征点中的同名点。
可选地，处理器14可根据目标特征点在设定坐标系下的位置信息，计算不同环境探测器11对应的目标特征点之间的距离；并确定目标特征点之间的距离小于或等于设定的距离阈值的目标特征点为不同环境探测器11对应的目标特征点中的同名点。或者；处理器14可根据多个环境探测器11对应的目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点的法向量之间的夹角；并确定法向量之间的夹角小于或等于设定的角度阈值的目标特征点为不同环境探测器对应的目标特征点中的同名点。或者，处理器14还可根据目标特征点在设定坐标系下的位置信息，计算不同环境探测器11对应的目标特征点之间的距离；并根据多个环境探测器11对应的目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点的法向量之间的夹角；进一步，处理器14可确定目标特征点之间的距离小于或等于设定的距离阈值，且法向量之间的夹角小于或等于设定的角度阈值的目标特征点，为不同环境探测器对应的目标特征点中的同名点。
在确定多个环境探测器11对应的特征点中的同名点之后，处理器14可根据多个环境探测器11对应同名点的关联信息，对多个环境探测器11在采集上述点云数据的过程中的姿态进行矫正。
其中，同名点的关联信息可包括：同名点的点云坐标及多个环境探测器11在采集同名点过程中记录的位姿信息等。其中，多个环境探测器11在采集同名点过程中记录的位姿信息可为组合导航模块15采集的多个环境探测器11在采集点云数据过程中的位姿信息。
相应地，处理器14可根据多个环境探测器11分别对应的同名点的点云坐标及多个环境探测器在采集同名点过程中的位姿信息，对多个环境探测器11在采集上述点云数据的过程中的姿态进行矫正。由于一对同名点A和B，在理论条件下满足上述式(5)，即一对同名点A和B在理论条件下在同一设定坐标系中的坐标是相同的。因此，可以多个环境探测器11对应的同名点在同一坐标系中对应坐标之间的距离最小化为目标，根据同名点在各帧点云数据中的点云坐标和多个环境探测器11采集同名点过程中记录的位姿信息，对多个环境探测器采集点云数据过程中记录的姿态信息进行矫正。这样，在利用矫正后的姿态信息计算点云数据在设定坐标系下的位置信息时，有助于使得计算得到的位置信息尽可能的接近实际位置信息，有助于提高确定出的位置信息的准确度。
可选地，处理器14可从组合导航模块采集的多个环境探测器11在采集点云数据过程中的位姿信息，获取多个环境探测器11在采集同名点过程中记录的位姿信息；并根据同名点在各帧点云数据的点云坐标和多个环境探测器在采集同名点过程中记录的位姿信息，计算对多个环境探测器11进行姿态优化的姿态纠偏量。
在本实施例中，不限定根据同名点在各帧点云数据中的点云坐标和多个环境探测器在采集同名点过程中记录的位姿信息，计算对多个环境探测器11进行姿态优化的姿态纠偏量具体实施方式。可选地，可根据上述式(4)，将同名点的点云坐标转换为在IMU坐标系下的坐标，并根据同名点在IMU坐标系下的坐标，计算对多个环境探测器11进行姿态优化的姿态纠偏量。在一些实施例中，由于一对同名点A和B，在理论条件下满足上述式(5)，即一对同名点A和B在理论条件下在同一设定坐标系中的坐标是相同的。因此，计算得到的对环境探测器进行姿态优化的姿态纠偏量，应使得姿态优化后的同名点A和B在同一坐标系中的坐标之间的差异尽可能的小。
基于上述分析，可以姿态纠偏参数为待求量，根据同名点在各帧点云数据中的点云坐标和多个环境探测器在采集同名点过程中记录的位姿信息，确定多个环境探测器对应的同名点在设定坐标系下的坐标表达式。具体地，基于上述式(3)，可将同名点在各帧点云数据中的点云坐标，转换为同名点在IMU坐标系下的坐标；进一步，基于上述式(4)，可以姿态纠偏参数ΔR为待求量，以同名点在IMU坐标系下的坐标和多个环境探测器在采集同名点过程中记录的位姿信息为已知量，确定多个环境探测器对应的同名点在设定坐标系下的坐标表达式为：
在式(6.1)中，ΔR表示待求解的姿态纠偏参数；Pw表示同名点i在设定坐标系下的坐标。
进一步，可根据多个环境探测器对应的同名点在设定坐标系下的坐标表达式，构建反映多个环境探测器对应的同名点在设定坐标系下的坐标之间的距离的数学模型。其中，数学模型的求解模型可表示为：
其中，在式(6.2)中，ΔR表示待求解的姿态纠偏参数；n表示第一环境探测器11a和第二环境探测器11b采集的点云数据中同名点的总对数；k表示第k对同名点；其中，k＝1，2，...，n。argminf(x)表示函数f(x)取最小值时，自变量x的取值。相应地，上述式(6)表示f(ΔR)等于函数取最小值时的ΔR的取值。
在式(6)中，表示第k对同名点在设定坐标系下的坐标之间的距离。相应地，/>表示第k对同名点在设定坐标系下的坐标点之间的距离的平方。因此，/>表示n对同名点在设定坐标系下的坐标点之间的距离的平方和，即可为上述反映多个环境探测器11对应的同名点在设定坐标系下的坐标之间的距离的数学模型。
由于一对同名点在理论条件下对应设定坐标系中的同一点，因此，姿态纠偏量ΔR使得n对同名点在设定坐标系下的坐标点之间的距离的平方和越小，后续利用姿态纠偏量ΔR计算出的点云数据在设定坐标系下的坐标的准确度越高。因此，可以多个环境探测器对应的同名点在设定坐标下的坐标之间的距离最小化为目标，对上述反映多个环境探测器11对应的同名点在设定坐标系下的坐标之间的距离的数学模型进行求解，即对上述式(6.2)进行求解，可得到ΔR的值，即为上述姿态纠偏量。
进一步，在得到姿态纠偏量ΔR之后，可利用姿态纠偏量ΔR对多个环境探测器11在采集点云数据过程中记录的姿态进行矫正，得到多个环境探测器在采集点云数据过程中的优化后的姿态信息。其中，优化公式可表示为：
在式(7)中，表示组合导航模块采集的环境探测器在采集第i个数据点时记录的姿态，即优化前的姿态；/>表示环境探测器在采集第i个数据点时的优化后的姿态。
本实施例提供的数据采集设备，可利用不同环境探测器在不同时刻可扫描到同一对象的特点，对多个环境探测器采集的点云数据进行特征提取，得到点云数据中的特征点；通过对多个环境探测器对应的特征点进行匹配，得到多个环境探测器对应的特征点中的同名点；并根据多个环境探测器对应的同名点的点云坐标和多个探测器采集同名点过程中的位姿信息，对多个环境探测器在采集点云数据过程中的姿态进行矫正，实现了对环境探测器的姿态进行优化，有助于提高环境探测器的姿态精度。
在对环境探测器11在采集点云坐标的过程中记录的姿态进行矫正之后，处理器14还可根据多个环境探测器11在采集点云坐标的过程中的优化后的姿态信息及点云数据中的点云坐标，计算多个环境探测器11采集的点云数据在设定坐标系下的空间信息。其中，点云数据的空间信息可以为点云数据在设定坐标系下的位置坐标分布。
可选地，可将环境探测器11在采集点云坐标的过程中优化后的姿态信息及点云数据对应的点云坐标带入下式(8)中，得到点云数据在设定坐标系下的坐标表达。式(8)可表示为：
对于图4所示的隧道场景的点云数据来说，利用优化前的姿态信息及点云数据对应的点云坐标，得到点云数据在设定坐标系下的空间信息的效果图如图5左图所示，利用环境探测器优化后的姿态信息及点云数据对应的点云坐标，得到点云数据在设定坐标系下的空间信息的效果图如图5右图所示。
根据图5所示的对环境探测器的姿态优化前后的场景探测效果对比，可以看出优化后得到的隧道形状的还原精度较高，可更加真实的还原现实场景。
进一步，处理器14还可根据环境探测器11采集的点云数据在设定坐标系下的空间信息，构建电子地图。由于环境探测器11的姿态使用的为优化后的姿态，因此环境探测器11采集的点云数据在设定坐标系下的空间信息的精度更高，进而构建的电子地图精度也更高。
对于自主移动设备来说，在构建完电子地图之后，可基于电子地图进行自主路线规划，有助于提高路线规划的精度，从而提高后续导航准确度。
值得说明的是，数据采集设备的实现形态不同，所包含的这些基本组件以及基本组件的构成均会有所不同，本申请实施例列举的仅是部分示例，不意味着数据采集设备必须包含图1a和图1b所示全部组件，也不意味着数据采集设备只能包括图1a和图1b所示组件。
还值得说明的是，上述数据采集设备执行的数据处理方法，也可由其它计算机设备实现。下面对本申请实施例提供的数据处理方法进行示例性说明。
图6为本申请实施例提供的姿态优化方法的流程示意图。如图6所示，该方法包括：
601、获取多个环境探测器采集的各帧点云数据。
602、对各帧点云数据进行特征提取，以从各帧点云数据中获取多个环境探测器对应的特征点。
603、对多个环境探测器对应的特征点进行匹配，以确定多个环境探测器对应的特征点中的同名点。
604、获取多个环境探测器采集同名点过程中记录的位姿信息。
605、以多个环境探测器对应的同名点在同一坐标系中对应坐标之间的距离最小化为目标，根据多个环境探测器对应的同名点在各帧点云数据中的点云坐标和多个环境探测器采集同名点过程中记录的位姿信息，对多个环境探测器在采集点云数据过程中记录的姿态信息进行矫正。
在本实施中，多个环境探测器设置于数据采集设备上，并呈一定的角度。多个环境探测器可采集环境信息，得到点云数据。关于环境探测器的实现形态和设置方式的描述，可参见上述实施例的相关内容。
多个环境探测器可随数据采集设备的移动而移动，并在数据采集设备移动过程中采集环境信息，得到点云数据。一帧点云数据为环境探测器一次环境探测得到的数据集合，包括：探测点在环境探测器坐标系下的空间坐标。
在本实施例中，为了对环境探测器的姿态进行优化，在步骤601中，可获取多个环境探测器采集的各帧点云数据。其中，每个环境探测器对应一帧点云数据。考虑到一帧点云数据的数据量较大，且存在噪声点，为了降低计算量和噪声，在步骤602中，可对各帧点云数据进行特征提取，以从各帧点云数据中获取多个环境探测器对应的特征点。其中，每个环境探测器对应的特征点，为从该环境探测器采集的点云数据中获取的特征点。
可选地，可根据各帧点云数据中的数据点的点云坐标，计算各帧点云数据中的数据点的曲率。关于数据点的曲率的计算方式，可参见上述实施例的相关内容，在此不再赘述。
进一步，可从每帧点云数据中获取曲率满足设定要求的数据点，作为采集该帧点云数据的环境探测器对应的特征点。可选地，可从每帧点云数据中获取曲率大于或等于设定的第一曲率阈值的边缘点，作为采集该帧点云数据的环境探测器对应的特征点；和/或，从每帧点云数据中获取曲率小于设定的第二曲率阈值的平面点，作为采集该帧点云数据的环境探测器对应的特征点；其中，第一曲率阈值大于第二曲率阈值。
进一步，由于多个环境探测器呈一定角度设置于数据采集设备上，因此，多个环境探测器可在不同时刻观测到同一对象。基于此，在步骤603中，可对多个环境探测器对应的特征点进行匹配，确定多个环境探测器对应的特征点中的同名点。其中，同名点是指多个环境探测器采集的点云数据中与现实世界的的同一物理点相对应的数据点。
由于一对同名点在现实世界的物理点为同一物理点，因此，在对多个环境探测器对应的特征点进行匹配时，可先将不同环境探测器采集的点云数据转换至同一设定坐标系下，如世界坐标系，转换过程可参见式(1)的相关内容，在此不再赘述。在理想条件下，对于一对同名点来说，在同一设定坐标系下的坐标相等。
在一些实施例中，数据采集设备上设置有组合导航模块。组合导航模块可采集环境探测器的位姿(即位置和姿态)。组合导航模块可包括：惯性传感器(InertialMeasurement Unit，IMU)、定位单元和轮速计等，但不限于此。组合导航模块采集的环境探测器的位姿为IMU坐标系与世界坐标系之间的姿态矩阵和平移矩阵。因此，为了降低计算量，可将环境探测器采集的点云数据转换到IMU坐标系下，具体转换过程可参见上述式(3)的相关内容。
基于上述式(3)可环境探测器采集的点云数据转换到IMU坐标系下，在对多个环境探测器对应的特征点进行匹配时，可先将不同环境探测器采集的数据点在IMU坐标系下的坐标转换至同一设定坐标系下，如世界坐标系，则针对任一数据点，可采用上述式(4)将其点云坐标转换至设定坐标系下。基于上述分析，在本实施例中，组合导航模块可采集多个环境探测器在采集点云数据过程中的位姿信息。相应地，在步骤603之前，可获取多个环境探测器在采集点云数据过程中记录的位姿信息。其中，多个环境探测器在采集点云数据过程中记录的位姿信息可为上述组合导航模块采集的多个环境探测器在采集点云数据过程中的位姿信息。
在对多个环境探测器对应的特征点进行匹配时，可将不同环境探测器采集的点云数据中的特征点进行匹配。但是，这样计算量较大。考虑到采集位置距离较远的点云数据所采集的地理区域的不同，针对不同的地理区域或者同一地理区域中的不同子区域所采集的点云数据之间的特征点之间的匹配度较低。基于此，为了降低计算量，可获取多个环境探测器在采集点云数据过程中记录的位姿信息；并根据该位姿信息中的位置信息，计算不同环境探测器对应的点云数据的采集位置之间的距离；并从不同环境探测器对应的点云数据中，选择采集位置之间的距离小于或等于设定的距离阈值的目标点云数据。其中，环境探测器对应的点云数据为该环境探测器采集的点云数据。
进一步，可从多个环境探测器对应的特征点中，获取属于目标点云数据的目标特征点；并根据多个环境探测器在采集目标特征点过程中记录的位姿信息及目标特征点的点云坐标，计算目标特征点在设定坐标系下的位置信息。在本实施例中，可采用上述式(3)将目标特征点的点云坐标，转换为目标特征点在IMU坐标系下的坐标；进一步，根据上述式(4)计算出目标特征点在设定坐标系下的位置信息。在计算目标特征点在设定坐标系下的位置信息时，式(4)中的和/>分别采用环境探测器在采集目标特征点过程中记录的姿态和位置信息。
进一步，可根据目标特征点在设定坐标系下的位置信息，确定多个环境探测器对应的目标特征点中的同名点。
可选地，可根据目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点之间的距离；并确定目标特征点之间的距离小于或等于设定的距离阈值的目标特征点为不同环境探测器对应的目标特征点中的同名点。或者；可根据多个环境探测器对应的目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点的法向量之间的夹角；并确定法向量之间的夹角小于或等于设定的角度阈值的目标特征点为不同环境探测器对应的目标特征点中的同名点。或者，还可根据目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点之间的距离；并根据多个环境探测器对应的目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点的法向量之间的夹角；进一步，可确定目标特征点之间的距离小于或等于设定的距离阈值，且法向量之间的夹角小于或等于设定的角度阈值的目标特征点，为不同环境探测器对应的目标特征点中的同名点。
在确定多个环境探测器对应的特征点中的同名点之后，在步骤604中，可获取多个环境探测器采集同名点过程中记录的位姿信息。可选地，可从上述组合导航模块采集的多个环境探测器在采集点云数据过程中的位姿信息中，获取多个环境探测器采集同名点过程中记录的位姿信息。
进一步，在步骤605中，以多个环境探测器对应的同名点在同一坐标系中对应坐标之间的距离最小化为目标，根据多个环境探测器对应的同名点的点云坐标和多个探测器在采集同名点过程中记录的位姿信息，对多个环境探测器采集点云数据过程中记录的姿态信息进行优化，得到多个探测器采集点云数据过程中的优化后的姿态。
可选地，可以姿态纠偏参数为待求量，根据同名点在各帧点云数据中的点云坐标和多个环境探测器在采集同名点过程中记录的位姿信息，确定多个环境探测器对应的同名点在设定坐标系下的坐标表达式。具体实施方式可参见上述式(6.1)的相关内容，在此不再赘述。
进一步，可根据多个环境探测器对应的同名点在设定坐标系下的坐标表达式，构建反映多个环境探测器对应的同名点在设定坐标系下的坐标之间的距离的数学模型；并以多个环境探测器对应的同名点在设定坐标下的坐标之间的距离最小化为目标，对上述反映多个环境探测器对应的同名点在设定坐标系下的坐标之间的距离的数学模型进行求解，得到姿态纠偏参数的值，即为上述姿态纠偏量。具体实施方式和原理可参见上述实施例式(6.2)的相关内容，在此不再赘述。
进一步，在得到姿态纠偏量ΔR之后，可利用姿态纠偏量ΔR对多个环境探测器在采集点云数据过程中记录的姿态信息进行矫正，以得到多个探测器采集各帧点云数据过程中的优化后的姿态信息。其中，优化公式可参见上述式(7)。
在本实施例中，可利用不同环境探测器在不同时刻可扫描到同一对象的特点，对多个环境探测器采集的点云数据进行特征提取，得到点云数据中的特征点；通过对多个环境探测器对应的特征点进行匹配，得到多个环境探测器对应的特征点中的同名点；并以多个环境探测器对应的同名点在同一坐标下的坐标之间的距离最小化为目标，根据多个环境探测器对应的同名点的点云坐标及多个环境探测器在采集同名点过程中的位姿信息，对多个环境探测器在采集点云数据过程中的姿态进行优化，实现了对环境探测器的姿态进行优化，有助于提高环境探测器的姿态精度。
在对环境探测器的姿态进行优化之后，还可根据多个环境探测器在采集点云数据过程中的优化后的姿态信息及点云数据中的点云坐标，计算多个环境探测器采集的点云数据在设定坐标系下的空间信息。其中，点云数据的空间信息可以为点云数据在设定坐标系下的位置坐标分布。其中，计算多个环境探测器采集的点云数据在设定坐标系下的空间信息的具体实施方式，可参见上述式(8)的相关内容。
进一步，还可根据环境探测器采集的点云数据在设定坐标系下的空间信息，构建电子地图。由于环境探测器的姿态使用的为优化后的姿态，因此环境探测器采集的点云数据在设定坐标系下的空间信息的精度更高，进而构建的电子地图精度也更高。
对于自主移动设备来说，在构建完电子地图之后，可基于电子地图进行自主路线规划，有助于提高路线规划的精度，从而提高后续导航准确度。
需要说明的是，上述实施例所提供方法的各步骤的执行主体均可以是同一设备，或者，该方法也由不同设备作为执行主体。比如，步骤601和602的执行主体可以为设备A；又比如，步骤601的执行主体可以为设备A，步骤602的执行主体可以为设备B；等等。
另外，在上述实施例及附图中的描述的一些流程中，包含了按照特定顺序出现的多个操作，但是应该清楚了解，这些操作可以不按照其在本文中出现的顺序来执行或并行执行，操作的序号如601、602等，仅仅是用于区分开各个不同的操作，序号本身不代表任何的执行顺序。另外，这些流程可以包括更多或更少的操作，并且这些操作可以按顺序执行或并行执行。
相应地，本申请实施例还提供一种存储有计算机指令的计算机可读存储介质，当计算机指令被一个或多个处理器执行时，致使一个或多个处理器执行上述姿态优化方法中的步骤。
本申请实施例还提供一种计算机程序产品，包括：计算机程序。其中，计算机程序产品被处理器执行可实现上述姿态优化方法。本实施例提供的计算机程序产品可以为地图数据制作软件等。
图7为本申请实施例提供的计算机设备的结构示意图。计算机设备可为服务器、云化的服务器阵列等服务端设备；也可为电脑等终端设备。如图7所示，计算机设备包括：存储器70a和处理器70b；其中，存储器70a，用于存储计算机程序。
处理器70b耦合至存储器70a，用于执行计算机程序以用于：获取多个环境探测器采集的各帧点云数据；对各帧点云数据进行特征提取，以从各帧点云数据中获取多个环境探测器对应的特征点；对多个环境探测器对应的特征点进行匹配，以确定多个环境探测器对应的特征点中的同名点；其中，同名点为多个环境探测器采集的点云数据中与现实世界的同一物理点相对应的数据点；以及，获取多个环境探测器采集同名点过程中记录的位姿信息；并以多个环境探测器对应的同名点在同一坐标下的坐标之间的距离最小化为目标，根据同名点在各帧点云数据中的点云坐标和多个环境探测器采集同名点过程中记录的位姿信息，对多个环境探测器采集点云数据过程中记录的姿态信息进行矫正。
可选地，处理器70b在对各帧点云数据进行特征提取时，具体用于：根据各帧点云数据中数据点的点云坐标，计算各帧点云数据中的数据点的曲率；并从每帧点云数据中获取曲率满足设定要求的数据点，作为采集该帧点云数据的环境探测器对应的特征点。
可选地，处理器70b在从每帧点云数据中获取曲率满足设定要求的数据点时，具体用于：从每帧点云数据中获取曲率大于或等于设定的第一曲率阈值的边缘点，作为采集该帧点云数据的环境探测器对应的特征点；和/或，从每帧点云数据中获取曲率小于设定的第二曲率阈值的平面点，作为采集该帧点云数据的环境探测器对应的特征点；第一曲率阈值大于第二曲率阈值。
在一些实施例中，处理器70b还用于：获取组合导航模块采集的多个环境探测器在采集各帧点云数据过程中的位姿信息；根据位姿信息中的位置信息，计算不同环境探测器对应的点云数据的采集位置之间的距离；并从不同环境探测器对应的点云数据中，选择采集位置之间的距离小于或等于设定的第一距离阈值的目标点云数据。
相应地，处理器70b在对多个环境探测器对应的特征点进行匹配时，具体用于：从多个环境探测器对应的特征点中获取属于目标点云数据的目标特征点；目标点云数据是指所述各帧点云数据中对应的多个环境探测器的采集位置之间的距离小于或等于设定的第一距离阈值的数据点；进一步，可根据多个环境探测器在采集目标特征点过程中的位姿信息及目标特征点的点云坐标，计算目标特征点在设定坐标系下的位置信息；以及，根据目标特征点在设定坐标系下的位置信息，确定多个环境探测器对应的目标特征点中的同名点。
可选地，处理器70b在确定多个环境探测器对应的目标特征点中的同名点时，具体用于：根据目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点之间的距离；确定目标特征点之间的距离小于或等于设定的第二距离阈值的目标特征点为不同环境探测器对应的目标特征点中的同名点；和/或；根据多个环境探测器对应的目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点的法向量之间的夹角；确定法向量之间的夹角小于或等于设定的角度阈值的目标特征点为不同环境探测器对应的目标特征点中的同名点。
在另一些实施例中，处理器70b在获取多个环境探测器在采集同名点过程中记录的位姿信息时，具体用于：从组合导航模块采集的多个环境探测器在采集点云数据过程中的位姿信息中，获取多个环境探测器在采集同名点过程中记录的位姿信息。相应地，处理器70b在对多个环境探测器在采集各帧点云数据过程中记录的姿态进行矫正时，具体用于：以对多个环境探测器进行姿态优化的姿态纠偏量为待求量，根据同名点在各帧点云数据中的点云坐标和多个环境探测器在采集同名点过程中记录的位姿信息，确定多个环境探测器对应的同名点在设定坐标系下的坐标表达式；根据多个环境探测器对应的同名点在设定坐标系下的坐标表达式，构建反映多个环境探测器对应的同名点在设定坐标系下的坐标之间的距离的数学模型；以多个环境探测器对应的同名点在设定坐标系下的坐标之间的距离最小化为目标，对数学模型进行求解，以得到姿态纠偏量；以及利用求解到的姿态纠偏量对多个环境探测器在采集点云数据过程中记录的姿态进行矫正，以得到多个环境探测器在采集点云数据过程中的优化后的姿态信息。
可选地，处理器70b还用于：根据多个环境探测器在采集各帧点云数据过程中的优化后的姿态信息及各帧点云数据对应的点云坐标，计算各帧点云数据在设定坐标系下的空间信息。
可选地，处理器70b还用于：根据各帧点云数据在设定坐标系下的空间信息，构建电子地图。
在一些可选实施方式中，如图7所示，计算机设备还可以包括：通信组件70c、电源组件70d等可选组件。在一些实施例中，计算机设备可实现为电脑等终端设备，还可包括：显示组件70e及音频组件70f等可选组件。图7中仅示意性给出部分组件，并不意味着计算机设备必须包含图7所示全部组件，也不意味着计算机设备只能包括图7所示组件。
本实施例提供的计算机设备，可利用不同环境探测器在不同时刻可扫描到同一对象的特点，对多个环境探测器采集的各帧点云数据进行特征提取，得到各帧点云数据中多个环境探测器对应的特征点；通过对多个环境探测器对应的特征点进行匹配，得到多个环境探测器对应的特征点中的同名点；并以多个环境探测器对应的同名点在同一坐标下的坐标之间的距离最小化为目标，根据同名点在点云数据中的点云坐标和多个环境探测器在采集同名点过程中记录的位姿信息，对多个环境探测器在采集点云数据过程中的姿态信息进行矫正，实现了对环境探测器的姿态进行优化，有助于提高环境探测器的姿态精度。
在本申请实施例中，存储器用于存储计算机程序，并可被配置为存储其它各种数据以支持在其所在设备上的操作。其中，处理器可执行存储器中存储的计算机程序，以实现相应控制逻辑。存储器可以由任何类型的易失性或非易失性存储设备或者它们的组合实现，如静态随机存取存储器(SRAM)，电可擦除可编程只读存储器(EEPROM)，可擦除可编程只读存储器(EPROM)，可编程只读存储器(PROM)，只读存储器(ROM)，磁存储器，快闪存储器，磁盘或光盘。
在本申请实施例中，处理器可以为任意可执行上述方法逻辑的硬件处理设备。可选地，处理器可以为中央处理器(Central Processing Unit，CPU)、图形处理器(GraphicsProcessing Unit，GPU)或微控制单元(Microcontroller Unit，MCU)；也可以为现场可编程门阵列(Field-Programmable Gate Array，FPGA)、可编程阵列逻辑器件(ProgrammableArray Logic，PAL)、通用阵列逻辑器件(General Array Logic，GAL)、复杂可编程逻辑器件(Complex Programmable Logic Device，CPLD)等可编程器件；或者为先进精简指令集(RISC)处理器(Advanced RISC Machines，ARM)或***芯片(System on Chip，SOC)等等，但不限于此。
在本申请实施例中，通信组件被配置为便于其所在设备和其他设备之间有线或无线方式的通信。通信组件所在设备可以接入基于通信标准的无线网络，如WiFi，2G或3G，4G，5G或它们的组合。在一个示例性实施例中，通信组件经由广播信道接收来自外部广播管理***的广播信号或广播相关信息。在一个示例性实施例中，所述通信组件还可基于近场通信(NFC)技术、射频识别(RFID)技术、红外数据协会(IrDA)技术、超宽带(UWB)技术、蓝牙(BT)技术或其他技术来实现。
在本申请实施例中，显示组件可以包括液晶显示器(LCD)和触摸面板(TP)。如果显示组件包括触摸面板，显示组件可以被实现为触摸屏，以接收来自用户的输入信号。触摸面板包括一个或多个触摸传感器以感测触摸、滑动和触摸面板上的手势。所述触摸传感器可以不仅感测触摸或滑动动作的边界，而且还检测与所述触摸或滑动操作相关的持续时间和压力。
在本申请实施例中，电源组件被配置为其所在设备的各种组件提供电力。电源组件可以包括电源管理***，一个或多个电源，及其他与为电源组件所在设备生成、管理和分配电力相关联的组件。
在本申请实施例中，音频组件可被配置为输出和/或输入音频信号。例如，音频组件包括一个麦克风(MIC)，当音频组件所在设备处于操作模式，如呼叫模式、记录模式和语音识别模式时，麦克风被配置为接收外部音频信号。所接收的音频信号可以被进一步存储在存储器或经由通信组件发送。在一些实施例中，音频组件还包括一个扬声器，用于输出音频信号。例如，对于具有语言交互功能的设备，可通过音频组件实现与用户的语音交互等。
需要说明的是，本文中的“第一”、“第二”等描述，是用于区分不同的消息、设备、模块等，不代表先后顺序，也不限定“第一”和“第二”是不同的类型。
本领域内的技术人员应明白，本申请的实施例可提供为方法、***、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(***)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现信息存储。信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的相同要素。
以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。
Claims (12)
1.一种姿态优化方法，其中，包括：
获取多个环境探测器采集环境信息得到的各帧点云数据；
对所述各帧点云数据进行特征提取，以从所述各帧点云数据中获取所述多个环境探测器对应的特征点；
对所述多个环境探测器对应的特征点进行匹配，以确定所述多个环境探测器对应的特征点中的同名点，其中，所述同名点为所述多个环境探测器采集的所述各帧点云数据中与现实世界的同一物理点相对应的数据点；
获取所述多个环境探测器采集所述同名点过程中记录的位姿信息；
以所述多个环境探测器对应的同名点在同一坐标系中对应坐标之间的距离最小化为目标，根据所述同名点在所述各帧点云数据中的点云坐标和所述多个环境探测器采集所述同名点过程中记录的位姿信息，对多个环境探测器采集所述各帧点云数据过程中记录的姿态信息进行矫正。
2.根据权利要求1所述的方法，其中，所述对所述各帧点云数据进行特征提取，以从所述各帧点云数据中获取所述多个环境探测器对应的特征点，包括：
根据所述各帧点云数据中数据点的点云坐标，计算所述各帧点云数据中的数据点的曲率；
从每帧点云数据中获取曲率满足设定要求的数据点，作为采集该帧点云数据的环境探测器对应的特征点。
3.根据权利要求2所述的方法，其中，所述从每帧点云数据中获取曲率满足设定要求的数据点，作为所述采集该帧点云数据的环境探测器对应的特征点，包括：
从每帧点云数据中获取曲率大于或等于设定的第一曲率阈值的边缘点，作为所述采集该帧点云数据的环境探测器对应的特征点；
和/或，
从每帧点云数据中获取曲率小于设定的第二曲率阈值的平面点，作为所述采集该帧点云数据的环境探测器对应的特征点；
所述第一曲率阈值大于所述第二曲率阈值。
4.根据权利要求1所述的方法，其中，所述对所述多个环境探测器对应的特征点进行匹配，以确定所述多个环境探测器对应的特征点中的同名点，包括：
从多个环境探测器对应的特征点中获取属于目标点云数据的目标特征点；所述目标点云数据是指所述各帧点云数据中对应的所述多个环境探测器的采集位置之间的距离小于或等于设定的第一距离阈值的数据点；
根据所述多个环境探测器在采集所述目标特征点过程中的位姿信息及所述目标特征点的点云坐标，计算所述目标特征点在设定坐标系下的位置信息；
根据所述目标特征点在设定坐标系下的位置信息，确定所述多个环境探测器对应的目标特征点中的同名点。
5.根据权利要求4所述的方法，其中，还包括：
根据组合导航模块采集的所述多个环境探测器在采集所述各帧点云数据过程中的位姿信息中的位置信息，计算不同环境探测器对应的点云数据的采集位置之间的距离；
从所述不同环境探测器对应的点云数据中，选择采集位置之间的距离小于或等于设定的第一距离阈值的目标点云数据。
6.根据权利要求4所述的方法，其中，所述根据所述目标特征点在设定坐标系下的位置信息，确定所述多个环境探测器对应的目标特征点中的同名点，包括：
根据所述目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点之间的距离；
确定目标特征点之间的距离小于或等于设定的第二距离阈值的目标特征点为所述不同环境探测器对应的目标特征点中的同名点；
和/或；
根据所述多个环境探测器对应的目标特征点在设定坐标系下的位置信息，计算不同环境探测器对应的目标特征点的法向量之间的夹角；
确定法向量之间的夹角小于或等于设定的角度阈值的目标特征点为所述不同环境探测器对应的目标特征点中的同名点。
7.根据权利要求1所述的方法，其中，所述获取所述多个环境探测器采集所述同名点过程中记录的位姿信息，包括：
从组合导航模块采集的所述多个环境探测器在采集所述点云数据过程中的位姿信息，获取所述多个环境探测器在采集所述同名点过程中记录的位姿信息；
所述根据所述同名点在所述各帧点云数据中的点云坐标和所述多个环境探测器采集所述同名点过程中的位姿信息，对多个环境探测器采集所述点云数据过程中记录的姿态信息进行矫正，包括：
以对所述多个环境探测器进行姿态优化的姿态纠偏量为待求量，根据所述同名点在所述各帧点云数据中的点云坐标和所述多个环境探测器在采集所述同名点过程中记录的位姿信息，确定所述多个环境探测器对应的同名点在设定坐标系下的坐标表达式；
根据所述多个环境探测器对应的同名点在设定坐标系下的坐标表达式，构建反映所述多个环境探测器对应的同名点在设定坐标系下的坐标之间的距离的数学模型；
以所述多个环境探测器对应的同名点在设定坐标系下的坐标之间的距离最小化为目标，对所述数学模型进行求解，以得到所述姿态纠偏量；
利用求解到的姿态纠偏量对所述多个环境探测器在采集所述点云数据过程中记录的姿态进行矫正，以得到所述多个环境探测器在采集各帧点云数据过程中的优化后的姿态信息。
8.根据权利要求1-7任一项所述的方法，其中，还包括：
根据所述多个环境探测器在采集所述各帧点云数据过程中的优化后的姿态信息及各帧点云数据对应的点云坐标，计算所述各帧点云数据在设定坐标系下的空间信息。
9.根据权利要求8所述的方法，其中，还包括：
根据所述各帧点云数据在设定坐标系下的空间信息，构建电子地图。
10.一种计算机设备，其中，包括：存储器和处理器；其中，所述存储器，用于存储计算机程序；
所述处理器耦合至所述存储器，用于执行所述计算机程序以用于执行权利要求1-9任一项所述方法中的步骤。
11.一种数据采集设备，其中，包括：机械本体；所述机械本体上设置有存储器、处理器及多个环境探测器；
所述多个环境探测器，用于采集环境信息，得到点云数据；
所述存储器，用于存储计算机程序；
所述处理器耦合至所述存储器，用于执行所述计算机程序以用于执行权利要求1-9任一项所述方法中的步骤。
12.根据权利要求11所述的设备，其中，所述数据采集设备为自主移动设备。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111666849.9A CN114353780B (zh) | 2021-12-31 | 2021-12-31 | 姿态优化方法及设备 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202111666849.9A CN114353780B (zh) | 2021-12-31 | 2021-12-31 | 姿态优化方法及设备 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN114353780A CN114353780A (zh) | 2022-04-15 |
| CN114353780B true CN114353780B (zh) | 2024-04-02 |
Family
ID=81104925
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202111666849.9A Active CN114353780B (zh) | 2021-12-31 | 2021-12-31 | 姿态优化方法及设备 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN114353780B (zh) |
Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107133325A (zh) * | 2017-05-05 | 2017-09-05 | 南京大学 | 一种基于街景地图的互联网照片地理空间定位方法 |
| CN107657656A (zh) * | 2017-08-31 | 2018-02-02 | 成都通甲优博科技有限责任公司 | 同名点匹配及三维重建方法、***和光度立体摄像终端 |
| CN107767440A (zh) * | 2017-09-06 | 2018-03-06 | 北京建筑大学 | 基于三角网内插及约束的文物序列影像精细三维重建方法 |
| CN110473239A (zh) * | 2019-08-08 | 2019-11-19 | 刘秀萍 | 一种三维激光扫描的高精度点云配准方法 |
| CN112241010A (zh) * | 2019-09-17 | 2021-01-19 | 北京新能源汽车技术创新中心有限公司 | 定位方法、装置、计算机设备和存储介质 |
| CN112862894A (zh) * | 2021-04-12 | 2021-05-28 | 中国科学技术大学 | 一种机器人三维点云地图构建与扩充方法 |
| CN113240740A (zh) * | 2021-05-06 | 2021-08-10 | 四川大学 | 基于相位引导双目视觉密集标记点匹配的姿态测量方法 |
| WO2021189468A1 (zh) * | 2020-03-27 | 2021-09-30 | 深圳市速腾聚创科技有限公司 | 激光雷达的姿态校正方法、装置和*** |
| CN113608170A (zh) * | 2021-07-07 | 2021-11-05 | 云鲸智能(深圳)有限公司 | 雷达标定方法、雷达、机器人、介质及计算机程序产品 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP6030549B2 (ja) * | 2011-04-13 | 2016-11-24 | 株式会社トプコン | 三次元点群位置データ処理装置、三次元点群位置データ処理システム、三次元点群位置データ処理方法およびプログラム |
-
2021
- 2021-12-31 CN CN202111666849.9A patent/CN114353780B/zh active Active
Patent Citations (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107133325A (zh) * | 2017-05-05 | 2017-09-05 | 南京大学 | 一种基于街景地图的互联网照片地理空间定位方法 |
| CN107657656A (zh) * | 2017-08-31 | 2018-02-02 | 成都通甲优博科技有限责任公司 | 同名点匹配及三维重建方法、***和光度立体摄像终端 |
| CN107767440A (zh) * | 2017-09-06 | 2018-03-06 | 北京建筑大学 | 基于三角网内插及约束的文物序列影像精细三维重建方法 |
| CN110473239A (zh) * | 2019-08-08 | 2019-11-19 | 刘秀萍 | 一种三维激光扫描的高精度点云配准方法 |
| CN112241010A (zh) * | 2019-09-17 | 2021-01-19 | 北京新能源汽车技术创新中心有限公司 | 定位方法、装置、计算机设备和存储介质 |
| WO2021189468A1 (zh) * | 2020-03-27 | 2021-09-30 | 深圳市速腾聚创科技有限公司 | 激光雷达的姿态校正方法、装置和*** |
| CN112862894A (zh) * | 2021-04-12 | 2021-05-28 | 中国科学技术大学 | 一种机器人三维点云地图构建与扩充方法 |
| CN113240740A (zh) * | 2021-05-06 | 2021-08-10 | 四川大学 | 基于相位引导双目视觉密集标记点匹配的姿态测量方法 |
| CN113608170A (zh) * | 2021-07-07 | 2021-11-05 | 云鲸智能(深圳)有限公司 | 雷达标定方法、雷达、机器人、介质及计算机程序产品 |
Non-Patent Citations (1)
| Title |
|---|
| 基于空间拓扑关系的目标自动跟踪与位姿测量技术;晏晖;胡丙华;;中国测试(第04期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN114353780A (zh) | 2022-04-15 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| EP3542182B1 (en) | Methods and systems for vehicle environment map generation and updating | |
| US11002840B2 (en) | Multi-sensor calibration method, multi-sensor calibration device, computer device, medium and vehicle | |
| US11474247B2 (en) | Methods and systems for color point cloud generation | |
| CN108419446B (zh) | 用于激光深度图取样的***及方法 | |
| CN110889808B (zh) | 一种定位的方法、装置、设备及存储介质 | |
| US20200353914A1 (en) | In-vehicle processing device and movement support system | |
| CN111121754A (zh) | 移动机器人定位导航方法、装置、移动机器人及存储介质 | |
| US11227395B2 (en) | Method and apparatus for determining motion vector field, device, storage medium and vehicle | |
| JP2016080460A (ja) | 移動体 | |
| EP4213128A1 (en) | Obstacle detection device, obstacle detection system, and obstacle detection method | |
| CN112630798B (zh) | 用于估计地面的方法和装置 | |
| CN114353780B (zh) | 姿态优化方法及设备 | |
| US11645762B2 (en) | Obstacle detection | |
| CN114115263B (zh) | 用于agv的自主建图方法、装置、移动机器人及介质 | |
| JP2023510507A (ja) | ナビゲーション用の順次マッピング及び位置推定（ｓｍａｌ） | |
| CN116222544B (zh) | 一种面向饲养场的投料车自动导航定位方法及装置 | |
| CN116630923B (zh) | 道路消失点的标注方法、装置及电子设备 | |
| CN115019167B (zh) | 基于移动终端的融合定位方法、***、设备及存储介质 | |
| CN118225078A (zh) | 交通工具的定位方法、装置、交通工具及存储介质 | |
| Tahir | Development of an Autonomous Vehicle Platform | |
| CN118310531A (zh) | 一种由粗到细点云配准的机器人跨场景定位方法及*** | |
| CN113625271A (zh) | 基于毫米波雷达和双目相机的同时定位与建图方法 | |
| CN114663605A (zh) | 即时定位地图构建方法、无人移动设备及计算机程序产品 | |
| CN115409986A (zh) | 一种基于点云语义激光slam回环检测方法、装置及机器人 |
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 |