CN116465393A - 基于面阵激光传感器的同步定位和建图方法及装置 - Google Patents
基于面阵激光传感器的同步定位和建图方法及装置 Download PDFInfo
- Publication number
- CN116465393A CN116465393A CN202310475104.7A CN202310475104A CN116465393A CN 116465393 A CN116465393 A CN 116465393A CN 202310475104 A CN202310475104 A CN 202310475104A CN 116465393 A CN116465393 A CN 116465393A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- point cloud
- coordinate system
- determining
- map
- 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.)
- Pending
Links
- 238000000034 method Methods 0.000 title claims abstract description 73
- 238000013507 mapping Methods 0.000 title claims abstract description 36
- 230000001360 synchronised effect Effects 0.000 title claims abstract description 33
- 238000012937 correction Methods 0.000 claims abstract description 61
- 238000005259 measurement Methods 0.000 claims abstract description 48
- 230000033001 locomotion Effects 0.000 claims abstract description 44
- 238000007781 pre-processing Methods 0.000 claims abstract description 17
- 238000006243 chemical reaction Methods 0.000 claims description 32
- 239000011159 matrix material Substances 0.000 claims description 32
- 238000001914 filtration Methods 0.000 claims description 11
- 238000005070 sampling Methods 0.000 claims description 9
- 238000005295 random walk Methods 0.000 claims description 7
- 238000009826 distribution Methods 0.000 claims description 6
- 230000000694 effects Effects 0.000 abstract 1
- 238000012800 visualization Methods 0.000 abstract 1
- 238000003860 storage Methods 0.000 description 15
- 238000004364 calculation method Methods 0.000 description 11
- 238000004590 computer program Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 11
- 238000012545 processing Methods 0.000 description 11
- 230000008569 process Effects 0.000 description 8
- 230000006870 function Effects 0.000 description 7
- 238000012163 sequencing technique Methods 0.000 description 6
- 238000010276 construction Methods 0.000 description 3
- 230000007613 environmental effect Effects 0.000 description 3
- 230000003287 optical effect Effects 0.000 description 3
- 230000000007 visual effect Effects 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 2
- 238000004422 calculation algorithm Methods 0.000 description 2
- 238000005516 engineering process Methods 0.000 description 2
- 230000003993 interaction Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 230000002093 peripheral effect Effects 0.000 description 2
- 230000009466 transformation Effects 0.000 description 2
- 238000003491 array Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000004927 fusion Effects 0.000 description 1
- 230000004807 localization Effects 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 238000011022 operating instruction Methods 0.000 description 1
- 238000012634 optical imaging Methods 0.000 description 1
- 230000003068 static effect Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000010408 sweeping Methods 0.000 description 1
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/38—Electronic maps specially adapted for navigation; Updating thereof
- G01C21/3804—Creation or updating of map data
- G01C21/3833—Creation or updating of map data characterised by the source of data
- G01C21/3841—Data obtained from two or more sources, e.g. probe vehicles
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本申请涉及一种基于面阵激光传感器的同步定位和建图方法及装置，其方法包括：对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云，根据移动机器人的姿态测量单元采集的运动信息，确定移动机器人的状态估计量，根据观测点云和状态估计量构建误差修正量，并对移动机器人的状态估计量进行修正，得到移动机器人的修正状态量，根据移动机器人的修正状态量，将机器人坐标系下的观测点云转换到世界坐标系下，从而确定移动机器人的位姿并同步更新三维概率体素地图。本申请有效降低错误点对于定位和建图精度的影响，构建可视化效果更好的3D地图。
Description
技术领域
本申请涉及定位和建图技术领域，具体涉及一种基于面阵激光传感器的同步定位和建图方法及装置。
背景技术
随着移动机器人智能化的发展，其对于环境感知能力提出了更高的要求。目前，移动机器人自主导航作业的能力依赖于机器人的同时定位与建图(SimultaneousLocalization and Mapping，SLAM)的能力。SLAM能力很大程度上依赖于使用的传感器。当今主流的算法主要基于激光雷达实现，即移动机器人通过激光扫描获得所处环境的2D或3D点云，获取周围障碍物的信息，以确定自身在环境中所处的位置并同时构建所处环境的地图。常用的激光雷达包括两种：机械式激光雷达和固态激光雷达。其中，机械式激光雷达体积较大，价格较为昂贵；固态激光雷达尺寸较机械式激光雷达更小，成本也更低一些，但由于视场角有限，通常需要使用更多台来达到较大的视场角，也意味着较高的成本。
发明内容
本申请实施例针对上述情况，提出了一种基于面阵激光传感器的同步定位和建图方法及装置，以构建可视化效果更好的3D地图，有效降低错误点对于定位和建图精度的影响。
第一方面，本申请实施例提供了一种基于面阵激光传感器的同步定位和建图方法，包括：
对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云；
根据移动机器人的姿态测量单元采集的运动信息，确定移动机器人的状态估计量；
根据观测点云和状态估计量构建误差修正量，并对移动机器人的状态估计量进行修正，得到移动机器人的修正状态量；
根据移动机器人的修正状态量，将机器人坐标系下的观测点云转换到世界坐标系下，从而确定移动机器人的位姿并同步更新三维概率体素地图。
可选的，在上述方法中，对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云，包括：
创建三维体素网格，三维体素网格的尺寸是根据点云数据在传感器坐标系下的坐标值以及预设的采样目标确定的；
确定落于三维体素网格的每个三维空间立方体的点云数据的第一质心；
基于传感器坐标系到机器人坐标系的第一转换矩阵，将各个第一质心从传感器坐标系转换到机器人坐标系，得到观测点云。
可选的，在上述方法中，所述姿态测量单元包括：轮式里程计和惯性测量单元；根据移动机器人的姿态测量单元采集的运动信息，确定移动机器人的状态估计量，包括：
读取轮式里程计采集的第一运动信息，第一运动信息包括线速度和第一测量白噪声；
读取惯性测量单元采集的第二运动信息，第二运动信息包括角速度、第二测量白噪声和随机游走高斯白噪声；
根据第一运动信息和第二运动信息，确定移动机器人的名义状态量和误差状态量；
对名义状态量和误差状态量作和，得到移动机器人的状态估计量。
可选的，在上述方法中，根据观测点云和状态估计量构建误差修正量，并对移动机器人的状态估计量进行修正，得到移动机器人的修正状态量，包括：
基于后验高斯分布，构建误差修正量，误差修正量包括状态量误差项和观测误差项；
通过卡尔曼滤波增益计算误差修正量，以更新移动机器人的状态估计量；
确定更新后的误差修正量是否小于预设的误差阈值，若是，则将更新后的状态估计量作为修正状态量。
可选的，在上述方法中，观测误差项是根据下述方法确定的：
根据移动机器人的状态估计量，确定机器人坐标系到世界坐标系的第二转换矩阵，根据第二转换矩阵，将观测点云从机器人坐标系转换到世界坐标系，得到第一空间点云；
估计第一空间点云中的任意一个目标点的曲率，从而确定第一空间点云的特征类型；
根据任意一个目标点、以及与任意一个目标点的特征类型相同的三维概率体素地图的地图点的匹配度确定观测误差项。
可选的，在上述方法中，根据任意一个目标点、以及与任意一个目标点的特征类型相同的三维概率体素地图的地图点的匹配度确定观测误差项，包括：
对于任意一个目标点，从三维概率体素地图中查找与目标点的距离小于预设的距离阈值、且与目标点的特征类型相同的地图点；
确定地图点的数量是否大于预设的数量阈值，若是，则按照概率权重的大小对地图点进行排序，得到排序结果；
截取排序结果中排序在前的多个地图点拟合为与特征类型对应的参考线或参考平面；
确定目标点到参考线或参考平面的距离，并根据距离更新观测误差项。
可选的，在上述方法中，根据移动机器人的修正状态量，将机器人坐标系下的观测点云转换到世界坐标系下，从而确定移动机器人的位姿并同步更新三维概率体素地图，包括：
根据移动机器人的修正状态量，确定机器人坐标系到世界坐标系的第三转换矩阵，根据第三转换矩阵，将观测点云从机器人坐标系转换到世界坐标系，得到第二空间点云；
根据三维概率体素地图的分辨率，对第二空间点云进行离散化处理，并确定第二空间点云中的各数据点在三维概率体素地图中的坐标；
基于第二空间点云中的各数据点的坐标确定移动机器人的位姿并同步更新三维概率体素地图。
可选的，在上述方法中，基于第二空间点云中的各数据点的坐标确定移动机器人的位姿并同步更新三维概率体素地图，包括：
判断更新的三维概率体素地图的一个网格单元包含的数据点的数量是否大于预设的数量阈值；
若是，则采用网格单元包括的多个数据点的第二质心替换多个数据点并标注第二质心的特征类型。
第二方面，本申请实施例还提供了一种基于面阵激光传感器的同步定位和建图装置，包括：
观测点云确定单元，用于对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云；
位姿估计单元，用于根据移动机器人的姿态测量单元采集的运动信息，确定移动机器人的状态估计量；
位姿修正单元，用于根据观测点云和状态估计量构建误差修正量，并对移动机器人的状态估计量进行修正，得到移动机器人的修正状态量；
地图更新单元，用于根据移动机器人的修正状态量，将机器人坐标系下的观测点云转换到世界坐标系下，从而确定移动机器人的位姿并同步更新三维概率体素地图。
第三方面，本申请实施例还提供了一种移动机器人，包括：处理器；以及存储器，用于存储处理器的可执行指令，其中，处理器配置为经由可执行指令来执行上述任一的基于面阵激光传感器的同步定位和建图方法。
第四方面，本申请实施例还提供了一种计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现如上任一所述基于面阵激光传感器的同步定位和建图方法。
本申请实施例采用的方法至少能够达到以下有益效果：
本申请提供了一种采用面阵激光传感器的同步定位和建图方法，对面阵激光传感器采集的点云数据进行体素滤波降采样等预处理，确定移动机器人在机器人坐标系下的观测点云，读取轮式里程计和惯性测量单元采集的运动信息估计移动机器人的状态估计量，然后通过观测点云和状态估计量构建的误差修正量对移动机器人的状态估计量进行修正，从而得到移动机器人的修正状态量，基于修正状态量将观测点云从机器人坐标系转换到世界坐标系下，融合于三维概率体素地图，实现了地图的实时构建和更新，同时实现了移动机器人位姿的精准确定。本申请通过有效的点云数据预处理，在保持点云空间结构特征的同时降低了噪点、高反点和离群点对后续定位和建图精度的影响，同时减少了数据处理的数量，保证了计算的实时性。对于3D ToF深度相机容易产生多路径干扰、边缘飞点等问题，使用三维概率体素地图，维护地图点的置信度，有效降低了错误点对于定位和建图精度的影响。相比基于传统单线激光雷达构建的2D SLAM，基于面阵激光传感器的3D SLAM能够利用更丰富的环境表面信息，减少长走廊等情况下地图定位错误的问题，同时能估计多个自由度的状态量，在移动机器人颠簸、翘起等情况下依然保持良好的定位。本申请的方法构建的地图可视化效果更高，能够用户交互提供更多信息。
附图说明
此处所说明的附图用来提供对本申请的进一步理解，构成本申请的一部分，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：
图1示出了根据本申请的一个实施例的基于面阵激光传感器的同步定位和建图方法的流程示意图；
图2示出了根据本申请的另一个实施例的基于面阵激光传感器的同步定位和建图方法的流程示意图；
图3示出了根据本申请的一个实施例的基于面阵激光传感器的同步定位和建图装置的结构示意图；
图4为本申请实施例中一种移动机器人的结构示意图。
具体实施方式
为使本申请的目的、技术方案和优点更加清楚，下面将结合本申请具体实施例及相应的附图对本申请技术方案进行清楚、完整地描述。显然，所描述的实施例仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。
以下结合附图，详细说明本申请各实施例提供的技术方案。
图1示出了根据本申请的一个实施例的基于面阵激光传感器的同步定位和建图方法的流程示意图，从图1可以看出，本申请至少包括步骤S110～步骤S140：
步骤S110：对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云。
本申请可以适用于各种自主导航机器人，尤其适用于可以移动机器人在平面执行任务时的情景，或者是平面运动的机器人在执行任务时的情景，以下为了方便说明，以扫地机器人进行地面清扫为例。
自主导航是指机器人可实时获取周围障碍物信息，确定自身在环境中所处的位置并同时构建所处环境的地图。现有技术中，常用机械式激光雷达和固态激光雷达实现，但这种雷达的成本均较高。
对此，本申请提供了一种，可通过面阵激光传感器实现移动机器人的实时定位和地图的构建。在移动机器人上安装面阵激光传感器。面阵激光传感器主要包括激光发射阵列和信息采集模块。面阵激光传感器中的激光发射阵列经其前方的光学成像系统向外发射光源，发射出的光源在到达物体表面后，一部分反射回来被信息采集模块所采集接收。而由于发射点到物体表面，以及物体表面到返回点的距离不同，飞行时间(TOF)不同，通过对飞行时间的测量，可获得对应的距离信息。
本申请中对面阵激光传感器的类型不作限定，可以为现有技术中的任意一种，入可以为spot型或者flood型ToF传感器，以下为了方便说明，均以面阵激光传感器为3D面阵ToF传感器进行说明，3D面阵ToF传感器又称3D ToF深度相机，是基于飞行时间测距法，采用类似于相机的一次性全局成像的工作模式，并直接测得每个像素深度方向距离信息的传感器。与普通相机相比，3D ToF深度相机能测量得到精度较高的像素深度信息；与激光雷达相比，3D ToF深度相机无需逐点扫描、一次成像，避免了数据产生运动畸变。3D ToF深度相机提供了丰富的高精度的环境点云信息，能为机器人SLAM模块提供可靠的数据输入。本申请在移动机器人的移动过程中，通过3D ToF深度相机实时采集移动机器人所处环境的三维点云信息，这里即为点云数据，点云数据是在传感器坐标系下的。3D ToF深度相机在移动机器人上的安装数量可以为一个也可以为多个。当移动机器人上安装有多个3D ToF深度相机时，只要不影响移动机器人的其他功能件即可。
3D ToF深度相机采集的点云数据可能包含无效点，需要对点云数据进行预处理，以对点云数据进行过滤，避免对系统准确度和鲁棒性造成影响。对点云数据的预处理可以包括但不限于通过以下方式实现。
比如，可以通过3D ToF深度相机采集点云数据的光强度信息，滤除光强度小于或者大于一定阈值的点，因为光强度小于或者大于一定阈值范围的点可能是多次折射的噪点或者反光产生的异常点。比如，可以通过3D ToF深度相机内置算法标志的多路径干涉产生的点，过滤点云数据中的标志点。比如，可以将超出3D ToF深度相机视场角或者三维坐标数值异常大的点滤除。比如，还可以通过预先标定确定的传感器坐标系与机器人坐标系的转换关系(即第一转换矩阵Trt)，将点云数据从传感器坐标系转换到机器人坐标系，滤除在机器人坐标系下高度小于一定阈值范围或者大于一定阈值范围的点。
当点云数据的数据量过于庞大时，还可以通过体素滤波器对点云数据进行降采样，以减少后续需要处理的数据量的同时，还能够保持点云数据的特征和空间结构信息，减小系统计算开销，保证系统计算的实时性。
通过上述方式进行预处理之后的点云数据，在基于预先标定确定的传感器坐标系与机器人坐标系的第一转换矩阵Trt，从传感器坐标系转换到机器人坐标系，从而得到机器人坐标系下的观测点云。观测点云的数据量不大于点云数据的数据量。
当移动机器人同时搭载多个3D ToF深度相机以扩大视场角时，可以对每个3D ToF深度相机采集到的不同时刻的点云数据(可以是经过预处理的)进行同步，再基于预先标定确定的各个传感器坐标系与机器人坐标系的转换关系，将不同3D ToF深度相机采集来源的点云数据(可以是经过预处理的)从自身的传感器坐标系转换到机器人坐标系下，进行融合和拼接后，构成更大视场角的在机器人坐标系下的观测点云。
具体的，在本申请的一些实施例中，对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云，包括：创建三维体素网格，该三维体素网格的尺寸是根据点云数据在传感器坐标系下的坐标值以及预设的采样目标确定的；确定落于该三维体素网格的每个三维空间立方体的点云数据的第一质心；基于传感器坐标系到机器人坐标系的第一转换矩阵，将各个第一质心从传感器坐标系转换到机器人坐标系，得到观测点云。
三维体素网格指的是三维空间立方体的集合，是三维空间数据的一种离散化的表示方式。在本实施例中，对点云数据进行降采样预处理，采用的是固定体素网格大小或固定观测点云数量的方式。固定体素网格大小是指三维体素网格包含的每个三维空间立方体代表的空间大小是固定不变的，固定观测点云数量是指降采样预处理后观测点云的目标数量。
在降采样之前，首先计算在传感器坐标系下，所有点云数据的x坐标的最小值xmin、所有点云数据的x坐标的最大值xmax、所有点云数据的y坐标的最小值ymin、所有点云数据的y坐标的最大值ymax。根据上述数据确定所有点云数据的宽度width＝xmax-xmin、以及高度heigth＝ymax-ymin。预设的采样目标为预设降采样后观测点云的目标数量为n′、降采样后点云在x坐标方向上和y坐标方向上点云数量的比例为r′，那么在降采样预处理后x坐标方向上点的个数为：
y坐标方向上点的个数为：
从而确定在x坐标方向和y坐标方向上，三维体素网格包含的每个三维空间立方体的尺寸为：
和/>
确定三维体素网格包含的每个三维空间立方体的尺寸后创建三维体素网格。将所有点云数据进行离散化处理后，计算其落到三维体素网格的每个三维空间立方体的坐标。对于落到同一三维空间立方体的点云数据的集合，求解该集合的点云数据的第一质心。计算公式为：
其中n表示落到同一三维空间立方体的点云数据的数量，xi，yi，zi表示落到该同一三维空间立方体的其中一个点云数据的坐标。这样，落到同一三维空间立方体的点云数据的集合就只用其第一质心来表示，达到了降采样的目的。
降采样后，基于预先标定的传感器坐标系到机器人坐标系的第一转换矩阵Trt，将各个第一质心从传感器坐标系转换到机器人坐标系，得到移动机器人在机器人坐标系下的观测点云。
步骤S120：根据移动机器人的姿态测量单元采集的运动信息，确定移动机器人的状态估计量。
在获取到移动机器人是在机器人坐标系下的观测点云后，可根据移动机器人的运动信息对移动机器人当前在三维概率体素地图上所处位姿进行实时的初步估计。对于运动信息可以包括但不限于从装载在移动机器人上现有的姿态测量单元获得，姿态测量单元通常为但不限于移动机器人上装载有高频率的轮式里程计和惯性测量单元等，姿态测量单元可以为但不限于轮式里程计以及惯性测量单元等，运动数据包括但不限于移动机器人的线速度和角速度，以及测量值的误差等。
在实际场景中，轮式里程计常用于采集移动机器人的线速度，惯性测量单元常用于采集移动机器人的角速度。在对移动机器人的状态量进行估计时，可读取轮式里程计和惯性测量单元采集到的数据，从而进行移动机器人的状态量进行估计，得到状态估计量。
具体的，在本申请的一些实施例中，根据移动机器人的姿态测量单元采集的运动信息，确定移动机器人的状态估计量，包括：读取装载于移动机器人的轮式里程计采集的第一运动信息，第一运动信息包括线速度和第一测量白噪声；读取装载于移动机器人的惯性测量单元采集的第二运动信息，第二运动信息包括角速度、第二测量白噪声和随机游走高斯白噪声；根据第一运动信息和第二运动信息确定移动机器人的名义状态量和误差状态量；对名义状态量和误差状态量作和，得到移动机器人的状态估计量。
理想情况下，通过轮式里程计和惯性测量单元数据直接累积得到的状态量(通常称为名义状态量)就是机器人真正的状态量，也就是说，在对移动机器人的状态量进行估计时，输入量为：
其中表示来自于轮式里程计测量的线速度，/>表示来源于惯性测量单元测量的角速度。
在不考虑噪声的情况，输出的为移动机器人的名义状态量，可定义为：
x＝[p，θ，bg]T，
其中，其中p表示位置状态量，θ表示旋转状态量，bg表示角度偏置状态量，这三个状态量即可表征移动机器人的位姿(即移动机器人的名义状态量)。在离散时间下，用i表示轮式里程计和惯性测量单元数据采样点。
那么最新第i+1帧的名义状态量xi+1可以表示为式(1)：
式(1)表示的名义状态量没有考虑噪声，名义状态量结果在长时间下会发生漂移。即在数据采集的过程中，轮式里程计和惯性测量单元也会产生一些误差，其中轮式里程计和惯性测量单元会产生测量白噪声，惯性测量单元还会产生随机游走高斯白噪声。在包含轮式里程计的第一测量白噪声和惯性测量单元的第二测量白噪声、以及惯性测量单元的随机游走高斯白噪声时，这些噪声可定义为：
其中nv表示第一测量白噪声，ng表示第二测量白噪声，nbg表示惯性测量单元的随机游走高斯白噪声。
因此，把误差部分独立出来作为误差状态量δx，定义为：
δx＝[δp，δθ，δbg]T，
那么最新第i+1帧的误差状态量δxi+1可以表示为式(2)：
则状态估计量等于名义状态量和误差状态量之和：
pT＝p+δp，θT＝θ+δθ，
名义状态量xi+1和瞬间误差状态量δxi+1作和，即可得到移动机器人的状态估计量，记为：
这里需要说明的是，这里对得到的移动机器人的状态估计量是某一时刻下相对粗略的估计。还需要说明的是，对于移动机器人的状态估计量是不断和实时进行的，即3D ToF深度相机获取到一帧数据，即进行一次移动机器人位姿状态的估计。
步骤S130：根据观测点云和状态估计量构建误差修正量，并对移动机器人的状态估计量进行修正，得到移动机器人的修正状态量。
移动机器人的状态估计量在长时间下会存在漂移，为此可以通过将前述得到的观测点云与事先加载的三维概率体素地图进行配准，如采用迭代最近点方法(ICP)，得到更为精确的移动机器人在机器人坐标系下的状态估计量，以修正移动机器人的状态估计量表征的位姿信息。可以通过构建以及最小化误差修正量，来实现对移动机器人的状态估计量的修正，将修正后的状态估计量记为移动机器人的修正状态量。
具体的，在本申请的一些实施例中，根据观测点云和状态估计量构建误差修正量，并对移动机器人的状态估计量进行修正，得到移动机器人的修正状态量，包括：基于后验高斯分布，构建误差修正量，误差修正量包括状态量误差项和观测误差项；通过卡尔曼滤波增益计算误差修正量，以更新移动机器人的状态估计量；确定更新后的误差修正量是否小于预设的误差阈值，若是，则将更新后的状态估计量作为修正状态量。
当对3D ToF深度相机采集的点云数据进行预处理降采样，得到第k帧观测点云时，利用观测信息进行最大化先验概率估计误差状态的后验高斯分布，用于修正估计的名义状态量。构建目标函数——误差修正量。误差修正量的具体形式如式(3)所示：
其中，表示状态量误差项，即修正后状态量到状态估计量之间的误差；
表示观测误差项，即实际观测和估计状态量下的观测之间的误差；
下标k表示第k帧，上标κ表示当前迭代轮次，xk表示第k帧的名义状态量，表示第k帧第κ次迭代的误差修正量，/>表示基于第k帧第κ-1次迭代更新之后的修正状态量，Jκ表示修正后状态量和状态估计量之间的误差相对于误差状态量的导数，j表示观测点云的当前点(即第j个观测点云)，m表示观测点云的数据量，/>表示实际观测和估计状态下的观测之间的误差，/>表示/>相对于当前误差状态量的导数。
求解以上目标函数解的过程，实际上是卡尔曼滤波的更新过程，卡尔曼增益计算式为：K＝PHT(HPHT+R)-1,其中，P表示误差状态量的先验概率，且P＝(Jκ)-1Pk(Jκ)-T。
在每次计算完卡尔曼增益后，均对移动机器人的状态估计量进行更新，误差修正量被更新为状态估计量被更新为/>
当计算出的误差修正量的大小小于预设的误差阈值时，认为目标函数已经优化完毕，且移动机器人的状态估计量最终被修正为修正状态量先验概率P被更新为
其中，对于实际观测和估计状态下的观测之间的误差，是通过观测点云到三维概率体素地图的距离来衡量的。在本申请的一些实施例中，观测误差项是根据下述方法确定的：根据移动机器人的状态估计量，确定机器人坐标系到世界坐标系的第二转换矩阵，根据第二转换矩阵，将观测点云从机器人坐标系转换到世界坐标系，得到第一空间点云；估计第一空间点云中的任意一个目标点的曲率，从而确定第一空间点云的特征类型；根据任意一个目标点、以及与任意一个目标点的特征类型相同的三维概率体素地图的地图点的匹配度确定观测误差项。
当对3D ToF深度相机采集的点云数据进行预处理降采样，得到第k帧观测点云后，根据移动机器人的线速度和角速度确定移动机器人的状态估计量xk。该状态估计量也就是通过误差修正量修正之前的估计的状态估计量。根据该状态估计量，确定机器人坐标系到世界坐标系的第二转换矩阵，根据第二转换矩阵，将观测点云从机器人坐标系转换到世界坐标系，得到第一空间点云。
由于3D ToF深度相机的点云数据类似于图像数据是有序的。因此可以利用第一空间点云中的每一个点与其邻域范围内的其他点之间的相对关系估计当前点的曲率，用于确定当前点是属于线特征还是面特征。
对于第一空间点云中的当前点aj，其曲率表示为：其中，ai是属于aj邻域的点，邻域可以是当前点aj同一行前n个和后n个的点，亦或者是n*n范围内的点。当当前点的曲率大于一定阈值时，当前点的特征类型为线特征；否则当前点的特征类型为面特征。
其中，对于观测误差项中包括的各个系数，是通过任意一个目标点、以及与任意一个目标点的特征类型相同的三维概率体素地图的地图点的匹配度确定观测误差项确定的。在本申请的一些实施例中，根据任意一个目标点、以及与任意一个目标点的特征类型相同的三维概率体素地图的地图点的匹配度确定观测误差项，包括：对于任意一个目标点，从三维概率体素地图中查找与目标点的距离小于预设的距离阈值、且与目标点的特征类型相同的地图点；确定地图点的数量是否大于预设的数量阈值，若是，则按照概率权重的大小对地图点进行排序，得到排序结果；截取排序结果中排序在前的多个地图点拟合为与特征类型对应的参考线或参考平面；确定目标点到参考线或参考平面的距离，并根据距离更新观测误差项。
对于任意一个目标点，基于点到点之间的欧式距离，在三维概率体素地图上进行最近邻查找，查找距离该目标点距离小于预设的距离阈值且同为线特征或面特征的地图点。也就是说，如果目标点是线特征，则在三维概率体素地图中查找距离该目标点最近的一定数量的且同为线特征的地图点；如果目标点是面特征，则在三维概率体素地图中查找距离该目标点距离最近的一定数量的且同为面特征的地图点。
对于不能找到足够数量能够匹配的地图点的目标点，认为该目标点是无法有效匹配的点，则跳过该目标点，不进行计算。
对于能够找到足够数量能够匹配的地图点的目标点，将该目标点匹配的地图点按照概率加权进行排序，取出其中排序在前(概率较高)的多个地图点，这些地图点最有可能是环境障碍物表面的最近邻的地图点。然后将这些地图点依据特征类型进行拟合，若这些地图点是线特征则拟合为参考线，若这些地图点是面特征则拟合为参考平面。最后计算目标点到参考线或参考平面的距离，并根据距离更新前述的观测误差项，即更新实际观测和估计状态下的观测之间的误差以及/>相对于当前误差状态量的导数/>
步骤S140：根据移动机器人的修正状态量，将机器人坐标系下的观测点云转换到世界坐标系下，从而确定移动机器人的位姿并同步更新三维概率体素地图。
在得到了移动机器人更加精准的的修正状态量后，以该修正状态量为基准，将机器人坐标系下的观测点云转换到世界坐标系下，从而同步定位和建图。
具体的，在本申请的一些实施例中，根据移动机器人的修正状态量，将机器人坐标系下的观测点云转换到世界坐标系下，从而确定移动机器人的位姿并同步更新三维概率体素地图，包括：根据移动机器人的修正状态量，确定机器人坐标系到世界坐标系的第三转换矩阵，根据第三转换矩阵，将观测点云从机器人坐标系转换到世界坐标系，得到第二空间点云；根据三维概率体素地图的分辨率，对第二空间点云进行离散化处理，并确定第二空间点云中的各数据点在三维概率体素地图中的坐标；基于第二空间点云中的个数据点的坐标确定移动机器人的位姿并同步更新三维概率体素地图。
获得精准的修正状态量后，根据该修正状态量确定机器人坐标系到世界坐标系的第三转换矩阵。采用该第三转换矩阵与观测点云中的各个数据点相乘，即可将观测点云中的各个数据点转换到世界坐标系下，得到第二空间点云。使用预先加载的三维概率体素地图的分辨率，对第二空间点云进行离散化处理，求解得到第二空间点云中各个数据点在三维概率体素地图的坐标值。基于各数据点的坐标值，将各数据点增加到三维概率体素地图中，实现对三维概率体素地图的更新，同时同步确定移动机器人的精准位姿。
在一些情况下，第二空间点云中的数据点可能过于稠密，而全局三维概率体素地图的分辨率较高时，可能会出现多个数据点落在同一个三维体素网格内的情况。对于这种情况，在本申请的一些实施例中，基于第二空间点云中的各数据点的坐标确定移动机器人的位姿并同步更新三维概率体素地图，包括：判断更新的三维概率体素地图的一个网格单元包含的数据点的数量是否大于预设的数量阈值；若是，则采用网格单元包括的多个数据点的第二质心替换多个数据点并标注第二质心的特征类型。
将落在同一个网格单元的多个数据点的集合求质心，用这第二质心代替该网格单元的多个数据点，这样的处理降低了三维概率体素地图中地图点的稠密程度，也提高了后续计算处理的速度。另外，在更新三维概率体素地图各网格单元的第二质心的同时，将第二质心所属的线特征或面特征进行标志，用于与第一空间点云进行配准时使用。
此外，3D ToF深度相机由于多路径干扰、边缘飞点等问题，容易产生测量错误的点。即使在预处理环节对点云数据进行了滤除操作，可能还是有较多的异常点。为此，需要利用三维概率体素网格来维护每个地图点的置信度，以进一步提高地图的准确性和鲁棒性。维护的整体思路是，对多个轮次构建的三维概率体素地图的每个网格单元计算置信度，置信度大的网格单元涉及的数据点进行保留；而对于置信度小的网格单元涉及的数据点进行剔除。对置信度的计算，是根据每个网格单元中的数据点的累积观测概率得到的，所谓“观测概率”可以理解为每一轮次检测到一个数据点，则为该数据点分配一个概率，多个轮次的概率之后，就被定义为对应网格单元中数据点的累积观测概率。
对应一个网格单元而言，根据其中的数据点的观测概率，确定其置信度，当一个网格单元的置信度高于预设的置信度阈值，则将该网格单元的数据点固定保留下来，也就是说，对于一个数据点，其每一轮次观测概率够大，多轮次的概率被多次累加(可以理解为被观测到的次数够多)，对应的网格单元的置信度就越高，形成的地图点就会被固定保留下来；而对于观测数量较少的数据点，对应的网格单元的置信度较低，在一段时间后，就会被从全局地图中删除。
另外，各个网格单元的置信度信息也可以用于第一空间点云的目标点到三维概率体素地图的配准中，使得目标点能尽可能地与置信度较高的网格单元中的数据点构成的参考线或参考平面进行配准。
由图1所示的方法可以看出，本申请提供了一种采用面阵激光传感器的同步定位和建图方法，对面阵激光传感器采集的点云数据进行体素滤波降采样等预处理，确定移动机器人在机器人坐标系下的观测点云，读取轮式里程计和惯性测量单元采集的运动信息估计移动机器人的状态估计量，然后通过观测点云和状态估计量构建的误差修正量对移动机器人的状态估计量进行修正，从而得到移动机器人的修正状态量，基于修正状态量将观测点云从机器人坐标系转换到世界坐标系下，融合于三维概率体素地图，实现了地图的实时构建和更新，同时实现了移动机器人位姿的精准确定。本申请通过有效的点云数据预处理，在保持点云空间结构特征的同时降低了噪点、高反点和离群点对后续定位和建图精度的影响，同时减少了数据处理的数量，保证了计算的实时性。对于3D ToF深度相机容易产生多路径干扰、边缘飞点等问题，使用三维概率体素地图，维护地图点的置信度，有效降低了错误点对于定位和建图精度的影响。相比基于传统单线激光雷达构建的2D SLAM，基于面阵激光传感器的3D SLAM能够利用更丰富的环境表面信息，减少长走廊等情况下地图定位错误的问题，同时能估计多个自由度的状态量，在移动机器人颠簸、翘起等情况下依然保持良好的定位。本申请的方法构建的地图可视化效果更高，能够用户交互提供更多信息。
图2示出了根据本申请的另一个实施例的基于面阵激光传感器的同步定位和建图方法的流程示意图，从图2可以看出，本实施例的基于面阵激光传感器的同步定位和建图方法包括以下步骤。
步骤S201，基于面阵激光传感器采集点云数据，过滤点云数据中的无效点。
步骤S202，创建三维体素网格，该三维体素网格的尺寸是根据面阵激光传感器采集的点云数据在传感器坐标系下的坐标值以及预设的采样目标确定的；确定落于三维体素网格的每个三维空间立方体的点云数据的第一质心；基于传感器坐标系到机器人坐标系的第一转换矩阵，将各个第一质心从传感器坐标系转换到机器人坐标系，得到观测点云。
步骤S203，读取装载于移动机器人的轮式里程计采集的第一运动信息，读取装载于移动机器人的惯性测量单元采集的第二运动信息；根据第一运动信息和第二运动信息确定移动机器人的名义状态量和误差状态量；对名义状态量和误差状态量作和，得到移动机器人的状态估计量。
步骤S204，基于后验高斯分布，构建误差修正量，该误差修正量包括状态量误差项和观测误差项。
步骤S205，根据移动机器人的状态估计量，确定机器人坐标系到世界坐标系的第二转换矩阵；根据第二转换矩阵将观测点云从机器人坐标系转换到世界坐标系，得到第一空间点云；估计第一空间点云中的任意一个目标点的曲率，从而确定第一空间点云的特征类型；
步骤S206，对于任意一个目标点，从三维概率体素地图中查找与目标点的距离小于预设的距离阈值、且与目标点的特征类型相同的地图点；确定地图点的数量是否大于预设的数量阈值；若是，则按照概率权重的大小对地图点进行排序得到排序结果；若否，则跳过该目标点，不进行计算。
步骤S207，截取排序结果中排序在前的多个地图点拟合为与特征类型对应的参考线或参考平面；确定目标点到参考线或参考平面的距离，并根据距离更新观测误差项。
步骤S208，通过卡尔曼滤波增益计算误差修正量，以更新移动机器人的状态估计量；确定更新后的误差修正量是否小于预设的误差阈值；若是，则将更新后的状态估计量作为修正状态量；若否，则继续迭代计算。
步骤S209，根据移动机器人的修正状态量，确定机器人坐标系到世界坐标系的第三转换矩阵，根据第三转换矩阵将观测点云从机器人坐标系转换到世界坐标系，得到第二空间点云；根据三维概率体素地图的分辨率，对第二空间点云进行离散化处理，并确定第二空间点云中的各数据点在三维概率体素地图中的坐标。
步骤S210，判断更新的三维概率体素地图的一个网格单元包含的数据点的数量是否大于预设的数量阈值；若是，则采用网格单元包括的多个数据点的第二质心替换多个数据点并标注第二质心的特征类型；若否，则不作处理。
步骤S211，基于第二空间点云中的各数据点或者第二质心的坐标，确定移动机器人的位姿并同步更新三维概率体素地图。
图3示出了根据本申请的一个实施例的基于面阵激光传感器的同步定位和建图装置的结构示意图。从图3可以看出，本实施例的基于面阵激光传感器的同步定位和建图装置300包括：
观测点云确定单元310，用于对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云；
位姿估计单元320，用于根据移动机器人的姿态测量单元采集的运动信息，确定移动机器人的状态估计量；
位姿修正单元330，用于根据观测点云和状态估计量构建误差修正量，并对移动机器人的状态估计量进行修正，得到移动机器人的修正状态量；
地图更新单元340，用于根据移动机器人的修正状态量，将机器人坐标系下的观测点云转换到世界坐标系下，从而确定移动机器人的位姿并同步更新三维概率体素地图。
在本申请的一些实施例中，在上述装置300中，观测点云确定单元310具体用于：创建三维体素网格，三维体素网格的尺寸是根据点云数据在传感器坐标系下的坐标值以及预设的采样目标确定的；确定落于三维体素网格的每个三维空间立方体的点云数据的第一质心；基于传感器坐标系到机器人坐标系的第一转换矩阵，将各个第一质心从传感器坐标系转换到机器人坐标系，得到观测点云。
在本申请的一些实施例中，在上述装置300中，姿态测量单元包括轮式里程计和惯性测量单元；位姿估计单元320具体用于：读取轮式里程计采集的第一运动信息，第一运动信息包括线速度和第一测量白噪声；读取惯性测量单元采集的第二运动信息，第二运动信息包括角速度、第二测量白噪声和随机游走高斯白噪声；根据第一运动信息和第二运动信息，确定移动机器人的名义状态量和误差状态量；对名义状态量和误差状态量作和，得到移动机器人的状态估计量。
在本申请的一些实施例中，在上述装置300中，位姿修正单元330具体用于：基于后验高斯分布，构建误差修正量，误差修正量包括状态量误差项和观测误差项；通过卡尔曼滤波增益计算误差修正量，以更新移动机器人的状态估计量；确定更新后的误差修正量是否小于预设的误差阈值，若是，则将更新后的状态估计量作为修正状态量。
在本申请的一些实施例中，在上述装置300中，位姿修正单元330具体还用于：根据移动机器人的状态估计量，确定机器人坐标系到世界坐标系的第二转换矩阵，根据第二转换矩阵，将观测点云从机器人坐标系转换到世界坐标系，得到第一空间点云；估计第一空间点云中的任意一个目标点的曲率，从而确定第一空间点云的特征类型；根据任意一个目标点、以及与任意一个目标点的特征类型相同的三维概率体素地图的地图点的匹配度确定观测误差项。
在本申请的一些实施例中，在上述装置300中，位姿修正单元330具体还用于：对于任意一个目标点，从三维概率体素地图中查找与目标点的距离小于预设的距离阈值、且与目标点的特征类型相同的地图点；确定地图点的数量是否大于预设的数量阈值，若是，则按照概率权重的大小对地图点进行排序，得到排序结果；截取排序结果中排序在前的多个地图点拟合为与特征类型对应的参考线或参考平面；确定目标点到参考线或参考平面的距离，并根据距离更新观测误差项。
在本申请的一些实施例中，在上述装置300中，地图更新单元340具体用于：根据移动机器人的修正状态量，确定机器人坐标系到世界坐标系的第三转换矩阵，根据第三转换矩阵，将观测点云从机器人坐标系转换到世界坐标系，得到第二空间点云；根据三维概率体素地图的分辨率，对第二空间点云进行离散化处理，并确定第二空间点云中的各数据点在三维概率体素地图中的坐标；基于第二空间点云中的各数据点的坐标确定移动机器人的位姿并同步更新三维概率体素地图。
在本申请的一些实施例中，在上述装置300中，地图更新单元340具体还用于：判断更新的三维概率体素地图的一个网格单元包含的数据点的数量是否大于预设的数量阈值；若是，则采用网格单元包括的多个数据点的第二质心替换多个数据点并标注第二质心的特征类型。
需要说明的是，上述的基于面阵激光传感器的同步定位和建图装置可一一实现前述的基于面阵激光传感器的同步定位和建图方法，这里不再一一赘述。
图4是本申请的一个实施例移动机器人的结构示意图。请参考图4，在硬件层面，该移动机器人包括处理器，可选地还包括内部总线、网络接口、存储器。其中，存储器可能包含内存，例如高速随机存取存储器(Random-Access Memory，RAM)，也可能还包括非易失性存储器(non-volatile memory)，例如至少1个磁盘存储器等。当然，该移动机器人还可能包括其他业务所需要的硬件。
处理器、网络接口和存储器可以通过内部总线相互连接，该内部总线可以是ISA(Industry Standard Architecture，工业标准体系结构)总线、PCI(PeripheralComponent Interconnect，外设部件互连标准)总线或EISA(Extended Industry StandardArchitecture，扩展工业标准结构)总线等。所述总线可以分为地址总线、数据总线、控制总线等。为便于表示，图4中仅用一个双向箭头表示，但并不表示仅有一根总线或一种类型的总线。
存储器，用于存放程序。具体地，程序可以包括程序代码，所述程序代码包括计算机操作指令。存储器可以包括内存和非易失性存储器，并向处理器提供指令和数据。
处理器从非易失性存储器中读取对应的计算机程序到内存中然后运行，在逻辑层面上形成基于面阵激光传感器的同步定位和建图装置。处理器，执行存储器所存放的程序，并具体用于执行前述基于面阵激光传感器的同步定位和建图方法。
上述如本申请图3所示实施例揭示的基于面阵激光传感器的同步定位和建图装置执行的方法可以应用于处理器中，或者由处理器实现。处理器可能是一种集成电路芯片，具有信号的处理能力。在实现过程中，上述方法的各步骤可以通过处理器中的硬件的集成逻辑电路或者软件形式的指令完成。上述的处理器可以是通用处理器，包括中央处理器(Central Processing Unit，CPU)、网络处理器(Network Processor，NP)等；还可以是数字信号处理器(Digital Signal Processor，DSP)、专用集成电路(Application SpecificIntegrated Circuit，ASIC)、现场可编程门阵列(Field－Programmable Gate Array，FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。可以实现或者执行本申请实施例中的公开的各方法、步骤及逻辑框图。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。结合本申请实施例所公开的方法的步骤可以直接体现为硬件译码处理器执行完成，或者用译码处理器中的硬件及软件模块组合执行完成。软件模块可以位于随机存储器，闪存、只读存储器，可编程只读存储器或者电可擦写可编程存储器、寄存器等本领域成熟的存储介质中。该存储介质位于存储器，处理器读取存储器中的配置信息，结合其硬件完成上述基于面阵激光传感器的同步定位和建图方法的步骤。
该移动机器人还可执行图3中基于面阵激光传感器的同步定位和建图装置执行的方法，并实现基于面阵激光传感器的同步定位和建图装置在图3所示实施例的功能，本申请实施例在此不再赘述。
本申请实施例还提出了一种计算机可读存储介质，该计算机可读存储介质存储一个或多个程序，该一个或多个程序包括指令，该指令当被包括多个应用程序的移动机器人执行时，能够使该移动机器人执行图3所示实施例中基于面阵激光传感器的同步定位和建图装置执行的方法，并具体用于执行前述的基于面阵激光传感器的同步定位和建图方法。
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。
在一个典型的配置中，计算设备包括一个或多个处理器(CPU)、输入/输出接口、网络接口和内存。
内存可能包括计算机可读介质中的非永久性存储器，随机存取存储器(RAM)和/或非易失性内存等形式，如只读存储器(ROM)或闪存(flash RAM)。内存是计算机可读介质的示例。
计算机可读介质包括永久性和非永久性、可移动和非可移动媒体可以由任何方法或技术来实现配置信息存储。配置信息可以是计算机可读指令、数据结构、程序的模块或其他数据。计算机的存储介质的例子包括，但不限于相变内存(PRAM)、静态随机存取存储器(SRAM)、动态随机存取存储器(DRAM)、其他类型的随机存取存储器(RAM)、只读存储器(ROM)、电可擦除可编程只读存储器(EEPROM)、快闪记忆体或其他内存技术、只读光盘只读存储器(CD-ROM)、数字多功能光盘(DVD)或其他光学存储、磁盒式磁带，磁带磁磁盘存储或其他磁性存储设备或任何其他非传输介质，可用于存储可以被计算设备访问的配置信息。按照本文中的界定，计算机可读介质不包括暂存电脑可读媒体(transitory media)，如调制的数据信号和载波。
还需要说明的是，术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、商品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、商品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、商品或者设备中还存在另外的同一要素。
本领域技术人员应明白，本申请的实施例可提供为方法、系统或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。
以上所述仅为本申请的实施例而已，并不用于限制本申请。对于本领域技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原理之内所作的任何修改、等同替换、改进等，均应包含在本申请的权利要求范围之内。
Claims (10)
1.一种基于面阵激光传感器的同步定位和建图方法，其特征在于，包括：
对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云；
根据所述移动机器人的姿态测量单元采集的运动信息，确定所述移动机器人的状态估计量；
根据所述观测点云和所述状态估计量构建误差修正量，并对所述移动机器人的状态估计量进行修正，得到所述移动机器人的修正状态量；
根据所述移动机器人的修正状态量，将所述机器人坐标系下的观测点云转换到世界坐标系下，从而确定所述移动机器人的位姿并同步更新三维概率体素地图。
2.根据权利要求1所述的方法，其特征在于，所述对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云，包括：
创建三维体素网格，所述三维体素网格的尺寸是根据所述点云数据在传感器坐标系下的坐标值以及预设的采样目标确定的；
确定落于所述三维体素网格的每个三维空间立方体的所述点云数据的第一质心；
基于所述传感器坐标系到所述机器人坐标系的第一转换矩阵，将各个所述第一质心从所述传感器坐标系转换到所述机器人坐标系，得到所述观测点云。
3.根据权利要求1所述的方法，其特征在于，所述姿态测量单元包括：轮式里程计和惯性测量单元；
所述根据所述移动机器人的姿态测量单元采集的运动信息，确定所述移动机器人的状态估计量，包括：
读取所述轮式里程计采集的第一运动信息，所述第一运动信息包括线速度和第一测量白噪声；
读取所述惯性测量单元采集的第二运动信息，所述第二运动信息包括角速度、第二测量白噪声和随机游走高斯白噪声；
根据所述第一运动信息和所述第二运动信息，确定所述移动机器人的名义状态量和误差状态量；
对所述名义状态量和所述误差状态量作和，得到所述移动机器人的状态估计量。
4.根据权利要求1所述的方法，其特征在于，所述根据所述观测点云和所述状态估计量构建误差修正量，并对所述移动机器人的状态估计量进行修正，得到所述移动机器人的修正状态量，包括：
基于后验高斯分布，构建所述误差修正量，所述误差修正量包括状态量误差项和观测误差项；
通过卡尔曼滤波增益计算误差修正量，以更新所述移动机器人的状态估计量；
确定更新后的所述误差修正量是否小于预设的误差阈值，若是，则将更新后的所述状态估计量作为所述修正状态量。
5.根据权利要求4所述的方法，其特征在于，所述观测误差项是根据下述方法确定的：
根据所述移动机器人的状态估计量，确定所述机器人坐标系到所述世界坐标系的第二转换矩阵，根据所述第二转换矩阵，将所述观测点云从所述机器人坐标系转换到所述世界坐标系，得到第一空间点云；
估计所述第一空间点云中的任意一个目标点的曲率，从而确定所述第一空间点云的特征类型；
根据所述任意一个目标点、以及与所述任意一个目标点的特征类型相同的三维概率体素地图的地图点的匹配度确定所述观测误差项。
6.根据权利要求5所述的方法，其特征在于，所述根据所述任意一个目标点、以及与所述任意一个目标点的特征类型相同的三维概率体素地图的地图点的匹配度确定所述观测误差项，包括：
对于所述任意一个目标点，从所述三维概率体素地图中查找与所述目标点的距离小于预设的距离阈值、且与所述目标点的特征类型相同的地图点；
确定所述地图点的数量是否大于预设的数量阈值，若是，则按照概率权重的大小对所述地图点进行排序，得到排序结果；
截取所述排序结果中排序在前的多个地图点拟合为与特征类型对应的参考线或参考平面；
确定所述目标点到所述参考线或所述参考平面的距离，并根据所述距离更新所述观测误差项。
7.根据权利要求1所述的方法，其特征在于，所述根据所述移动机器人的修正状态量，将所述机器人坐标系下的观测点云转换到世界坐标系下，从而确定所述移动机器人的位姿并同步更新三维概率体素地图，包括：
根据所述移动机器人的修正状态量，确定所述机器人坐标系到所述世界坐标系的第三转换矩阵，根据所述第三转换矩阵，将所述观测点云从所述机器人坐标系转换到所述世界坐标系，得到第二空间点云；
根据所述三维概率体素地图的分辨率，对所述第二空间点云进行离散化处理，并确定所述第二空间点云中的各数据点在所述三维概率体素地图中的坐标；
基于所述第二空间点云中的各数据点的坐标确定所述移动机器人的位姿并同步更新所述三维概率体素地图。
8.根据权利要求7所述的方法，其特征在于，所述基于所述第二空间点云中的各数据点的坐标确定所述移动机器人的位姿并同步更新所述三维概率体素地图，包括：
判断更新的所述三维概率体素地图的一个网格单元包含的数据点的数量是否大于预设的数量阈值；
若是，则采用所述网格单元包括的多个数据点的第二质心替换所述多个数据点并标注所述第二质心的特征类型。
9.一种基于面阵激光传感器的同步定位和建图装置，其特征在于，所述装置包括：
观测点云确定单元，用于对面阵激光传感器采集的点云数据进行预处理，确定移动机器人在机器人坐标系下的观测点云；
位姿估计单元，用于根据所述移动机器人的姿态测量单元采集的运动信息，确定所述移动机器人的状态估计量；
位姿修正单元，用于根据所述观测点云和所述状态估计量构建误差修正量，并对所述移动机器人的状态估计量进行修正，得到所述移动机器人的修正状态量；
地图更新单元，用于根据所述移动机器人的修正状态量，将所述机器人坐标系下的观测点云转换到世界坐标系下，从而确定所述移动机器人的位姿并同步更新三维概率体素地图。
10.一种移动机器人，包括：
处理器；以及
存储器，用于存储所述处理器的可执行指令；
其中，所述处理器配置为经由执行所述可执行指令来执行权利要求1～8任一项所述的基于面阵激光传感器的同步定位和建图方法。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310475104.7A CN116465393A (zh) | 2023-04-27 | 2023-04-27 | 基于面阵激光传感器的同步定位和建图方法及装置 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310475104.7A CN116465393A (zh) | 2023-04-27 | 2023-04-27 | 基于面阵激光传感器的同步定位和建图方法及装置 |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN116465393A true CN116465393A (zh) | 2023-07-21 |
Family
ID=87185088
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310475104.7A Pending CN116465393A (zh) | 2023-04-27 | 2023-04-27 | 基于面阵激光传感器的同步定位和建图方法及装置 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116465393A (zh) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116929338A (zh) * | 2023-09-15 | 2023-10-24 | 深圳市智绘科技有限公司 | 地图构建方法、设备及存储介质 |
-
2023
- 2023-04-27 CN CN202310475104.7A patent/CN116465393A/zh active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116929338A (zh) * | 2023-09-15 | 2023-10-24 | 深圳市智绘科技有限公司 | 地图构建方法、设备及存储介质 |
| CN116929338B (zh) * | 2023-09-15 | 2024-01-09 | 深圳市智绘科技有限公司 | 地图构建方法、设备及存储介质 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN108921947B (zh) | 生成电子地图的方法、装置、设备、存储介质以及采集实体 | |
| CN110889808B (zh) | 一种定位的方法、装置、设备及存储介质 | |
| CN108319655B (zh) | 用于生成栅格地图的方法和装置 | |
| EP2133662B1 (en) | Methods and system of navigation using terrain features | |
| EP2463680B1 (en) | Systems and methods for navigation using cross correlation on evidence grids | |
| CN116608847A (zh) | 基于面阵激光传感器和图像传感器的定位和建图方法 | |
| US8818722B2 (en) | Rapid lidar image correlation for ground navigation | |
| JP2002511614A (ja) | 物体位置の追跡・検知方法 | |
| CN110895408B (zh) | 一种自主定位方法、装置及移动机器人 | |
| CN113721248B (zh) | 一种基于多源异构传感器的融合定位方法及系统 | |
| CN112729321A (zh) | 一种机器人的扫图方法、装置、存储介质和机器人 | |
| CN112731337B (zh) | 地图构建方法、装置和设备 | |
| WO2024001649A1 (zh) | 机器人定位方法、装置和计算可读存储介质 | |
| CN115200572B (zh) | 三维点云地图构建方法、装置、电子设备及存储介质 | |
| CN116465393A (zh) | 基于面阵激光传感器的同步定位和建图方法及装置 | |
| CN110989619B (zh) | 用于定位对象的方法、装置、设备和存储介质 | |
| CN118168545A (zh) | 基于多源传感器融合的除草机器人定位导航系统及方法 | |
| CN118310531A (zh) | 一种由粗到细点云配准的机器人跨场景定位方法及系统 | |
| CN116429090A (zh) | 基于线激光的同步定位和建图方法、装置、移动机器人 | |
| CN113376638A (zh) | 一种无人物流小车环境感知方法及系统 | |
| CN110794434A (zh) | 一种位姿的确定方法、装置、设备及存储介质 | |
| CN111095024A (zh) | 高度确定方法、装置、电子设备和计算机可读存储介质 | |
| KR102624644B1 (ko) | 벡터 맵을 이용한 이동체의 맵 매칭 위치 추정 방법 | |
| JP7302966B2 (ja) | 移動体 | |
| CN118247313B (zh) | 一种融合动平台里程信息的被动传感器多目标跟踪方法 |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination |