CN112305558A - 一种利用激光点云数据的移动机器人轨迹确定方法及装置 - Google Patents

Abstract

本发明涉及一种利用激光点云数据的移动机器人轨迹确定方法及装置，属于定位技术领域。方法包括：获取每帧的扫描点云数据；提取点特征和平面特征；根据相邻两帧的点特征确定点特征关联，平面特征确定相邻两帧是否存在平面特征关联；若存在平面特征关联，则根据这两帧中关联的平面对的法向量确定相关矩阵；若相关矩阵的秩小于满秩，或者秩等于满秩、且条件数大于条件数设定阈值，则结合平面特征关联和点特征关联确定帧间位姿变换矩阵；若秩等于满秩、且条件数小于等于条件数设定阈值，则根据相关矩阵确定帧间位姿变换矩阵。本发明通过对平面特征的系统约束进行判断，进而选择不同的方法确定移动机器人的帧间位姿变换矩阵，提高了定位的精度。

Description

一种利用激光点云数据的移动机器人轨迹确定方法及装置

技术领域

本发明涉及一种利用激光点云数据的移动机器人轨迹确定方法及装置，属于定位技术领域。

背景技术

随着机器人技术的快速发展，移动机器人正逐步走入人们的日常生活中。为此，需要赋予移动机器人与人、环境自由交互的能力，提高其自主性和智能化程度，对于移动机器人而言，实现真正自主的前提和基础是在运行环境中能够准确的自我定位，而自我定位有赖于其装载的各类传感器。

经历了多轮技术更新，激光雷达(Light Detection and Ranging，LiDAR)作为当前最先进的环境感知传感器之一，具有测量精度高、速度快、视场范围大、受环境影响小等优点，可为移动机器人提供强大的环境感知能力，为此LiDAR越来越多的应用于多种移动机器人平台。基于LiDAR的定位是测绘、计算机视觉、机器人学等众多领域共同关心的热点问题，具有十分广阔的应用前景。基于LiDAR的定位技术包括2D LiDAR的定位技术和LiDAR里程计技术，其中2D LiDAR的定位技术已相对较为成熟，但在复杂场景下的3D定位的鲁棒性、精度、和效率仍有待提高，因此LiDAR里程计技术成为实现移动机器人定位的关键技术。

不同于车辆上安装的轮式里程计传感器，移动机器人提出的里程计(Odometry)是一种算法上的概念，它指的是利用运动传感器(包括视觉传感器和LiDAR等)的数据估计载体位置随时间的变化，是一种相对定位技术，它估计的是相对于某个初始位置当前移动机器人所处的位置。LiDAR里程计的性能对移动机器人的感知、定位和地图构建等任务而言是至关重要的，现有的LiDAR里程计通过提取点特征，利用相邻观测信息估计相对运动并推算得到机器人相对于某个起始点的位姿，进而得到移动机器人的运动轨迹。

现有方法仅利用点特征进行运动估计，是一种采用单一的扫描匹配方法估计帧间运动的方法，该方法属于一种增量式定位技术，将会导致位姿误差会不断累积，在某些情况下采用这种方法估计帧间运动可能会失败或存在欠约束方向，同时在大规模场景或长期运行条件下只利用点特征进行单一的扫描匹配，使得机器人的估计位姿并不准确，最终导致得到的移动机器人的轨迹并不准确。

发明内容

本申请的目的在于提供一种利用激光点云数据的移动机器人轨迹确定方法及装置，用以解决现有技术轨迹确定不准确的问题。

为实现上述目的，本申请提出了一种利用激光点云数据的移动机器人轨迹确定方法的技术方案，包括以下步骤：

1)获取每帧的扫描点云数据；

2)对每帧的扫描点云数据进行点特征和平面特征的提取；所述点特征包括边缘点特征和平面点特征；

3)根据相邻两帧的点特征确定所述相邻两帧的点特征关联，并且根据平面特征确定所述相邻两帧是否存在平面特征关联；

4)若存在平面特征关联，则根据这两帧中关联的平面对的法向量确定相关矩阵，若相关矩阵的秩小于满秩，则进入步骤6)，若相关矩阵的秩等于满秩，则进入步骤5)；

5)对相关矩阵进行特征值分解，计算条件数，若条件数大于条件数设定阈值，则进入步骤6)；若条件数小于等于条件数设定阈值，则根据相关矩阵确定移动机器人的帧间位姿变换矩阵；得到移动机器人轨迹；所述条件数为最大特征值和最小特征值的比值；

6)结合平面特征关联和点特征关联确定移动机器人的帧间位姿变换矩阵；得到移动机器人轨迹。

另外，本申请还提出一种利用激光点云数据的移动机器人轨迹确定装置，包括处理器、存储器以及存储在所述存储器中并可在处理器上运行的计算机程序，所述处理器在执行所述计算机程序时实现上述的利用激光点云数据的移动机器人轨迹确定方法的技术方案。

本发明的利用激光点云数据的移动机器人轨迹确定方法及装置的技术方案的有益效果是：本发明对获取的扫描点云数据进行点特征和平面特征的提取，进而确定相邻两帧的点特征关联以及判断相邻两帧是否存在平面特征关联，若存在平面特征关联，则确定相关矩阵，若相关矩阵的秩小于满秩，则表明此时的平面特征的系统约束不够强，则需要结合平面特征关联和点特征关联确定移动机器人的帧间位姿变换矩阵；若相关矩阵的秩等于满秩，而其条件数大于条件数设定阈值，则表明此时的平面特征的系统约束不够强，则需要结合平面特征关联和点特征关联确定移动机器人的帧间位姿变换矩阵；若相关矩阵的秩等于满秩，且其条件数小于等于条件数设定阈值，则表明此时的平面特征的系统约束足够强，直接根据相关矩阵确定移动机器人的帧间位姿变换矩阵。本发明通过对平面特征的系统约束进行判断，进而选择不同的方法确定移动机器人的帧间位姿变换矩阵，这种混合式的扫描匹配方法，充分利用场景中可能存在的平面特征所提供的约束信息，提高了定位的鲁棒性和精度。

进一步的，上述利用激光点云数据的移动机器人轨迹确定方法及装置中，为了提高移动机器人轨迹的准确性，还包括将当前帧的点特征和平面特征与局部地图进行配准，优化帧间位姿变换矩阵的步骤，所述局部地图根据当前帧之前的扫描点云数据构建。

进一步的，上述利用激光点云数据的移动机器人轨迹确定方法及装置中，为了提高移动机器人轨迹的准确性，所述步骤1)中的扫描点云数据为经过运动畸变消除的扫描点云数据，运动畸变消除的过程为：利用预测位姿提供的观测位姿，通过线性插值和球面线性插值计算出每帧扫描点云数据中每个扫描点的位置和姿态，随后将每帧扫描点云数据转换到该帧点云的初始时刻坐标系下，以实现运动畸变的消除。

进一步的，上述利用激光点云数据的移动机器人轨迹确定方法及装置中，为了提高移动机器人轨迹的准确性，还包括采用因子图模型对移动机器人的轨迹进行优化的过程，所述因子图模型的建立过程为：将步骤2)提取的平面特征作为平面路标，建立平面路标节点；将局部地图优化后的位姿作为位姿节点，通过里程计因子和观测因子连接各节点，完成因子图模型的建立，所述里程计因子用于描述帧间运动；所述观测因子用于描述从某一位姿对平面路标的观测。

附图说明

图1是本发明相对定位的原理图；

图2是本发明利用激光点云数据的移动机器人轨迹确定方法实施例1的流程图；

图3是本发明点特征提取时的分区示意图；

图4是本发明利用激光点云数据的移动机器人轨迹确定方法实施例2的流程图；

图5是本发明基于平面路标的因子图模型示意图；

图6是本发明利用激光点云数据的移动机器人轨迹确定装置的结构示意图。

具体实施方式

利用激光点云数据的移动机器人轨迹确定方法实施例1：

LiDAR里程计的相对定位的原理如图1所示，它不依赖于其他传感器，仅利用具有一定重叠的序列帧LiDAR扫描点云估计出机器人在不同时刻的位姿。本发明的主要构思在于，在获取激光点云数据后，进行点特征提取和平面特征提取，进而确定点特征关联和平面特征关联；计算平面特征的系统约束，对平面特征的约束质量进行分析，若平面特征的系统约束不强，则联合平面特征关联和点特征关联通过迭代优化的方式求解帧间位姿变换，得到初始定位结果；若平面特征的系统约束足够强，则采用平面特征关联求解帧间位姿变换，得到初始定位结果；同时通过平面特征和点特征构建局部地图，将局部地图和初始定位结果进行位姿融合，优化初始定位结果，最终得到机器人的定位轨迹。

具体的，利用激光点云数据的移动机器人轨迹确定方法如图2所示，包括以下步骤：

1)通过激光雷达获取每帧的扫描点云数据，并对点云数据进行运动畸变的消除。

激光雷达接收到新到来的一帧扫描点云后，利用预测位姿或者其他传感器提供的观测位姿通过线性插值和球面线性插值计算出该帧点云中每个点的位置和姿态，随后将所有扫描点云转换到该帧点云的初始时刻坐标系下，以实现运动畸变的改正。

2)对消除畸变后的每帧的扫描点云数据进行点特征和平面特征的提取。

对消除畸变后的点云数据进行特征提取包括点特征提取和平面特征提取，利用LiDAR的测量特性和点云线束结构的平面特征提取方法实现。

提取的点特征中包括两类点特征：局部平滑度较高的平面点和局部平滑度较低的边缘点，具体点特征的提取过程为：对各扫描线上进行并行处理，依靠左右相邻点深度变化计算出的每个点的局部平滑度，局部平滑度较高的点为平面点，局部光滑度较低的点为边缘点。并且，为了使点特征分布较为均匀，如图3所示，将360°扫描点云划分为6个子区域，保证从各个子区域中提取特定数量的点特征，得到特征点集

其中

为边缘点集、

为平面点集。

在特征点集中进行显著特征点的提取，得到显著特征点集

其中

为显著边缘点集、

为显著平面点集，

包含

包含

平面特征的提取过程为：将平面特征多边形化，将平面内点投影到2D平面上，采用2D凸包检测算法快速检测边界点，将平面边界点、质心等信息作为平面属性予以保存。

3)根据相邻两帧的点特征确定相邻两帧的点特征关联

并且根据平面特征确定相邻两帧是否存在平面特征关联

假设当前帧的扫描点云为S_k+1，前一帧的扫描点云为S_k，每一帧中的平面参数采用海森标准形式表示：

其中n为法向量、ρ为原点到平面的距离，因此，当前帧中包含平面

前一帧中包含平面

由于LiDAR扫描频率较高，相邻帧间位姿变化较小，因此采用最近邻相似平面的原则进行平面特征关联，如果相邻两帧中有一对平面满足以下条件，则表明这一对平面特征关联

n_k+1·n_k＞τ_∠；

其中，τ_∠为平面平行设定阈值，若n_k+1·n_k＞τ_∠，则表明两平面近似平行；τ_dis为平面距离设定阈值，若

则表明两平面相距较近；若两平面满足近似平行且相距较近，则两平面特征关联。

点特征关联

包括点-线和点-面关联，利用KD树搜索建立点特征关联的对应关系，具体地，确定当前帧中的点在上一帧显著特征点集中对应的点，进而确定当前帧中的显著边缘点和上一帧中对应的边缘线的对应关系，以及当前帧中的显著平面点和上一帧中对应的平面的对应关系，假设当前帧中的显著边缘点集为

显著平面点集为

上一帧中的显著边缘点集为

显著平面点集为

则：

当前帧中的显著边缘点

那么p_i,k+1对应的上一帧中的边缘线由p_i,k+1对应的上一帧中点的附近的两个点确定，附近的两个点包括：最临近点p_ai,k和位于其相邻扫描线上的次邻近点p_bi,k，p_ai,k、

当前帧中的显著平面点

那么p_j,k+1对应的上一帧中的平面由p_j,k+1对应的上一帧中点的附近的三个点确定，附近的三个点包括：最近邻点p_aj,k和位于其邻近扫描线上的最近点p_bj,k和p_cj,k三个点，p_aj,k、p_bj,k、

点特征关联

是无需进行判断的，原因在于，相邻两帧是一定存在点关联的，本发明建立的点特征关联关系是点-线和点-面关联，是稳定可靠的，而对于点-点间的关联是不一定存在的。

4)通过平面特征关联(即存在平面特征关联)计算平面特征的系统约束，若平面特征的系统约束不够强，则联合平面特征关联和点特征关联求解帧间位姿变换矩阵，得到初始定位结果；若平面特征的系统约束足够强，则采用平面特征关联求解帧间位姿变换矩阵，得到初始定位结果。

初始定位结果即初始定位轨迹，平面特征的系统约束是否足够强的判断过程为：

a.步骤3)中得到的存在平面特征关联

中包含的关联的平面对的数量为K_c对，分别罗列每对平面的法向量，若前一帧k中的平面i和当前帧k+1中的平面j关联，那么平面i的法向量为

平面j的法向量为

分别构成K_c×3矩阵N_k和N_k+1，定义3×3相关矩阵

其中

为权矩阵，是一种对角矩阵，

为对角矩阵对角线上的元素，表示为权值，权值w可根据多种方式确定，如平面对的面积、平面拟合不确定度等；N_k+1为当前帧的罗列法向量；N_k为前一帧的罗列法向量；

b.若相关矩阵Q的秩小于3(3×3的矩阵满秩为3)，则平面特征的系统约束不够强，联合平面特征关联和点特征关联求解帧间位姿变换矩阵，得到初始定位结果；

c.若相关矩阵Q的秩等于3，则对相关矩阵Q进行特征值分解，得到降序排列的特征值λ₁,λ₂,λ₃，计算条件数C，条件数C为最大特征值和最小特征值的比值，即C＝λ₁/λ₃，若条件数C大于条件数设定阈值，则平面特征的系统约束不够强，联合平面特征关联和点特征关联求解帧间位姿变换矩阵，得到初始定位结果；

d.若条件数C小于等于条件数设定阈值，则平面特征的系统约束足够强，采用平面特征关联求解帧间位姿变换矩阵，得到初始定位结果。

采用平面特征关联求解帧间位姿变换矩阵的过程为：对相关矩阵Q进行奇异值分解，得到Q＝UΛV^T，其中：U为左奇异向量；Λ为对角矩阵；V为右奇异向量；进而计算得到相邻两帧之间移动机器人位姿的旋转矩阵的估计值

罗列

和Δρ＝ρ_i,k-ρ_j,k+1可得到N_kt_k,k+1＝Δρ，进而可由下式计算相邻两帧之间移动机器人位姿的平移向量的估计值

其中，

为前一帧中，平面i的法向量；Δρ为距离变化量；ρ_i,k为原点到前一帧中平面i的距离；ρ_j,k+1当原点到前帧中平面j的距离；t_k,k+1为平移向量(也即平移矩阵)；Δρ为距离变化向量。

帧间位姿变换矩阵包括旋转矩阵和平移矩阵，通过移动机器人的旋转和平移体现帧间位姿变换量的估计值

联合平面特征关联和点特征关联求解帧间位姿变换的过程为：联合平面特征关联

和点特征关联

通过迭代优化方式求解帧间位姿变换。其中，平面配准残差f_pl(x)形式如下：

其中，d₁存在平面特征关联中第一对平面的平面距离；

存在平面特征关联中，第K_c对平面的平面距离；d_m为存在平面特征关联中，第m对平面的平面距离；n_m,k+1为第m对平面中，当前帧平面的法向量；n_m,k为第m对平面中，前一帧平面的法向量；ρ_m,k+1为原点到第m对平面中当前帧片面的距离；ρ_m,k为原点到第m对平面中前一帧平面的距离。

点配准残差f_pt(x)包含两类，分别为显著边缘点到其对应的边缘线的距离残差和显著平面点到其对应的平面的距离残差。

显著边缘点到其对应的边缘线的距离残差d_E为(这里的显著边缘点为提取出的所有的显著边缘点)：

显著平面点到其对应的平面的距离残差d_P为：

平面配准残差f_pl(x)是显式的包含帧间位姿变换量T_k,k+1，点配准残差f_pt(x)隐式包含了帧间位姿变换量T_k,k+1，所谓隐式就是无法直接列出其表达式，但可在非线性优化求解过程中联合上述两种距离残差求出帧间位姿变换量T_k,k+1，求解过程可以根据现有技术推理得出，这里不做过多赘述。

根据

和

联立所有残差式f_pl(x)和f_pt(x)构成最优化问题，利用Ceres库通过Levenberg-Marquardt算法进行优化求解，得到帧间位姿变换量的估计值

(具体求解过程可以根据现有技术推理得出，这里不做过多赘述)：

上述根据步骤1)-步骤4)得到的初始定位结果是由初始定位线程得到。并且上述初始定位的结果是建立在存在平面特征关联的基础上进行定位的，那么如果不存在平面特征关联，直接采用点特征关联进行定位，点特征关联进行定位为现有技术这里不做过多赘述。

5)根据步骤2)得到的当前帧的点特征和平面特征与局部地图进行配准，优化帧间位姿变换矩阵，进而优化初始定位结果，局部地图根据当前帧之前的扫描点云数据构建。

本步骤通过地图构建线程进行单帧扫描与局部地图(scan-to-map)间的匹配(简称子图匹配)，通过将当前帧的平面特征、显著特征点集

与局部地图

配准，对初始定位线程估计的位姿变换估计矩阵

进一步优化，同时还利用优化后的位姿变换和当前帧特征点集更新局部地图得到

最终得到机器人的定位轨迹。

在地图构建线程中，利用多分辨率体素来管理地图。地图中的点保存于体素中，局部地图

由载体(这里的载体为机器人)附近的体素构成；当随着载体的运动，载体靠近地图边缘时，类似于环形存储器，沿前进方向添加新的体素单元以扩展地图，同时将相反方向的最远体素及保存的点删除。局部地图

中的每个体素

又划分为更小尺寸的体素，标记利用当前运动估计进行转换后的

和

中的显著特征点所占据的体素，从中提取的点组成点集Q_k，子图匹配即利用

和Q_k实现，利用体素和KD树搜索实现显著点特征对应关系的建立。同时，局部地图还包含相应范围内观测到的平面特征，需建立当前帧扫描中的平面特征与局部地图中平面特征的对应关系，而位姿变换估计方法与初始定位线程相同。完成子图匹配后，利用

对被标记的占据体素进行更新，同时更新平面特征属性信息，进而实现地图更新，同时对体素中点进行下采样，以维持相对固定的点密度，防止地图尺寸增长过快。

为保证运行效率，该线程的运行频率较低，频率可根据硬件资源进行动态调整，同时将该线程得到的侧滚角和俯仰角作为反馈信息传递给初始定位线程，从而有效增强定位系统的鲁棒性。

上述实施例中，局部地图的建立是为了优化初始定位结果，作为其他实施方式，在初始定位结果准确的情况下，也可以不进行优化。

上述实施例中，为了提高定位效率，将提取的点特征和平面特征进行了显著点特征和显著平面特征的提取，作为其他实施方式，也可以直接采用提取的点特征和平面特征进行后续的定位过程。

本发明通过对平面特征的系统约束进行判断，选择不同的扫描匹配方法进行帧间位姿变换矩阵的求解，在平面特征的系统约束足够强的情况下，采用平面特征关联求解帧间位姿变换矩阵，在平面特征的系统约束不够强的情况下，联合平面特征关联和点特征关联求解帧间位姿变换矩阵；这种混合式的扫描匹配方法，充分利用场景中可能存在的平面特征所提供的约束信息，提高了定位的鲁棒性和精度。

利用激光点云数据的移动机器人轨迹确定方法实施例2：

本实施例提出的利用激光点云数据的移动机器人轨迹确定方法与实施例1的不同之处在于，增加了第三个线程——后端优化线程。初始定位线程和地图构建线程通过累积序列帧间匹配得到的位姿变换估计，进而得到定位轨迹，然而由于测量噪声导致的误差不断累积，最终的位姿估计将出现严重漂移，本实施例中，通过后端优化线程降低累计误差。

具体的，本实施例提出的利用激光点云数据的移动机器人轨迹确定方法如图4所示。关于初始定位线程和地图构建线程的具体定位过程在上述实施例1中已经介绍，这里不做赘述。

后端优化线程将所提取的平面特征作为平面路标，建立位姿节点和平面路标节点，利用帧间运动约束和平面路标观测约束进行因子图优化，所采用的因子图模型如图5所示，图5中x₁、x₂、x₃、x₄、x₅、x₆、x₇…表示各扫描帧时刻的位姿节点，l₁、l₆、l₈、l₁₀、l₁₆、l₂₀…表示平面路标节点(包括平面参数)，通过两种因子实现节点间的连接，分别为描述帧间运动的里程计因子和描述从某一位姿对平面路标观测的观测因子。

对于两个位姿节点x_a和x_b，给定里程计因子u(里程计因子u即实施例1中步骤4)中计算出的帧间位姿变换量的估计值

)，里程计因子u提供如下误差：

其中，e(x_a,x_b|u)为位姿误差，

为位姿节点x_a位姿变换的求逆运算，

为位姿节点x_a和x_b之间的位姿相对变化量。

对于位姿x和平面p，某一位姿对平面路标观测的观测因子描述了在x处将对p的观测与实际观测z的差，提供如下平面路标的观测误差：

e(x,p|u)＝z-x^-1p；

其中，e(x,p|u)为平面路标的观测误差；x和平面p都处于全局坐标系下，x^-1p为位姿x在一个局部坐标系下假想的平面观测；z为实际观测。

优化过程为使得各误差趋于0的过程，将误差趋于0对局部地图优化后的定位轨迹进行优化。

因子图模型的建立过程为：在关联平面路标时，采用比初始定位线程更为严苛的对应匹配条件，对于新观测的平面和已建立的平面路标，除了需要满足平面特征关联的条件外，还需要利用平面边界信息计算两平面的重叠度，当重叠度超过重叠度设定阈值时，两者成功关联，完成平面路标关联后得到对应关系C_pp。在每个扫描帧时刻，算法往因子图中插入一个新的位姿节点，并通过里程计因子提供的位姿变换与上一节点相连；然后，对C_pp包含的每个平面对应，在该位姿节点和相应的路标节点间插入一个新的观测因子，具体处理方式为：如果该平面对应是未知的(即路标地图中不存在相应路标)，且被连续观测多次，则在插入因子前为该平面创建一个新的路标；否则，若平面对应关联的平面路标已知，则在新位姿和该已知平面路标间建立因子。在定义了变量和因子后，即可根据扫描匹配得到的序列帧间位姿变换估计和平面路标对应C_pp构建因子图。

根据因子图模型利用iSAM2优化器进行在线更新求解，得到优化后的定位轨迹。本发明通过后端优化线程，利用基于平面路标的因子图对定位轨迹进行进一步的优化，增强了定位的一致性。同时本发明可构建一致性较好的平面与点构成的混合地图，该地图可用于机器人的路径规划及其他感知任务。

利用激光点云数据的移动机器人轨迹确定装置实施例：

利用激光点云数据的移动机器人轨迹确定装置，如图6所示，包括处理器、存储器以及存储在所述存储器中并可在处理器上运行的计算机程序，所述处理器在执行所述计算机程序时实现利用激光点云数据的移动机器人轨迹确定方法。

利用激光点云数据的移动机器人轨迹确定方法的具体实施过程以及效果在上述利用激光点云数据的移动机器人轨迹确定方法实施例1、2中介绍，这里不做赘述。

也就是说，以上利用激光点云数据的移动机器人轨迹确定方法实施例中的方法应理解可由计算机程序指令实现利用激光点云数据的移动机器人轨迹确定方法的流程。可提供这些计算机程序指令到处理器(如通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备等)，使得通过处理器执行这些指令产生用于实现上述方法流程所指定的功能。

本实施例所指的处理器是指微处理器MCU或可编程逻辑器件FPGA等的处理装置；

本实施例所指的存储器用于存储实现利用激光点云数据的移动机器人轨迹确定方法而形成的计算机程序指令，包括用于存储信息的物理装置，通常是将信息数字化后再以利用电、磁或者光学等方式的媒体加以存储。例如：利用电能方式存储信息的各式存储器，RAM、ROM等；利用磁能方式存储信息的的各式存储器，硬盘、软盘、磁带、磁芯存储器、磁泡存储器、U盘；利用光学方式存储信息的各式存储器，CD或DVD。当然，还有其他方式的存储器，例如量子存储器、石墨烯存储器等等。

通过上述存储有实现利用激光点云数据的移动机器人轨迹确定方法而形成的计算机程序指令的存储器、处理器构成的利用激光点云数据的移动机器人轨迹确定装置，在计算机中由处理器执行相应的程序指令来实现，计算机可使用windows操作系统、linux系统、或其他，例如使用android、iOS系统程序设计语言在智能终端实现，以及基于量子计算机的处理逻辑实现等。

作为其他实施方式，利用激光点云数据的移动机器人轨迹确定装置还可以包括其他的处理硬件，如数据库或多级缓存、GPU等，本发明并不对利用激光点云数据的移动机器人轨迹确定装置的结构做具体的限定。

Claims (5)

1.一种利用激光点云数据的移动机器人轨迹确定方法，其特征在于，包括以下步骤：

1)获取每帧的扫描点云数据；

2)对每帧的扫描点云数据进行点特征和平面特征的提取；所述点特征包括边缘点特征和平面点特征；

3)根据相邻两帧的点特征确定所述相邻两帧的点特征关联，并且根据平面特征确定所述相邻两帧是否存在平面特征关联；

4)若存在平面特征关联，则根据这两帧中关联的平面对的法向量确定相关矩阵，若相关矩阵的秩小于满秩，则进入步骤6)，若相关矩阵的秩等于满秩，则进入步骤5)；

5)对相关矩阵进行特征值分解，计算条件数，若条件数大于条件数设定阈值，则进入步骤6)；若条件数小于等于条件数设定阈值，则根据相关矩阵确定移动机器人的帧间位姿变换矩阵；得到移动机器人轨迹；所述条件数为最大特征值和最小特征值的比值；

6)结合平面特征关联和点特征关联确定移动机器人的帧间位姿变换矩阵；得到移动机器人轨迹。

2.根据权利要求1所述的利用激光点云数据的移动机器人轨迹确定方法，其特征在于，还包括将当前帧的点特征和平面特征与局部地图进行配准，优化帧间位姿变换矩阵的步骤，所述局部地图根据当前帧之前的扫描点云数据构建。

3.根据权利要求1所述的利用激光点云数据的移动机器人轨迹确定方法，其特征在于，所述步骤1)中的扫描点云数据为经过运动畸变消除的扫描点云数据，运动畸变消除的过程为：利用预测位姿提供的观测位姿，通过线性插值和球面线性插值计算出每帧扫描点云数据中每个扫描点的位置和姿态，随后将每帧扫描点云数据转换到该帧点云的初始时刻坐标系下，以实现运动畸变的消除。

4.根据权利要求2所述的利用激光点云数据的移动机器人轨迹确定方法，其特征在于，还包括采用因子图模型对移动机器人的轨迹进行优化的过程，所述因子图模型的建立过程为：将步骤2)提取的平面特征作为平面路标，建立平面路标节点；将局部地图优化后的位姿作为位姿节点，通过里程计因子和观测因子连接各节点，完成因子图模型的建立，所述里程计因子用于描述帧间运动；所述观测因子用于描述从某一位姿对平面路标的观测。

5.一种利用激光点云数据的移动机器人轨迹确定装置，其特征在于，包括处理器、存储器以及存储在所述存储器中并可在处理器上运行的计算机程序，所述处理器在执行所述计算机程序时实现如权利要求1-4中任一项所述的利用激光点云数据的移动机器人轨迹确定方法。

Images