多线激光定位方法及定位装置、计算机设备、存储介质与流程

1.本发明涉及机器人定位技术，尤其涉及采用多线激光与imu紧耦合的定位方法及适于运行该方法的定位装置、计算机设备、存储介质。


背景技术：

2.工业场景下的使用多线激光在预先构建的点云地图中进行定位时，通常使用ndt（正态分布点云算法,即normal distribution transform）进行点云配准，配准时需将最近的机器人位姿（即机器人在当前地图坐标系下的位置与朝向）作为初值，配准后再将配准结果与里程计进行融合的得到当前机器人位姿。
3.因此，ndt配准的初值来自融合里程计后的机器人位姿，如果车辆发生打滑，将不可避免的对ndt配准造成较大的影响，甚至导致配准失败，进而引发定位丢失。
4.为此发明人发现，现有技术曾经提出了“一种激光雷达与imu紧耦合的定位方法”（中国专利申请号202010197086.7），其通过imu对系统实时运动状态进行估计，通过激光雷达的点云数据提取平面特征进行跟踪，并利用平面特征的观测模型对imu估计的运动状态进行纠正。此外该方法利用msckf框架对激光雷达和imu两种传感器实现了优势互补，一方面imu高频的运动估计能够对系统的实时状态进行估计，另一方面激光雷达点云提取的平面特征能够鲁棒地在环境中被跟踪，从而能够定时对系统状态进行纠正。因此在一定程度上能够解决定位会受到车体打滑影响的问题。
5.然而该现有技术也存在一些问题，如：该技术中采用的平面检测算法复杂度较高，如需要进行点云球面投影，计算每个点的法向量，执行生长聚类算法，再使用pca计算平面参数。此外该技术中使用滑窗中多个相邻状态下的平面特征作为约束来修正位姿结果，实际上也是局部定位状态，只是一定程度上提高了定位精度，无法保证全局一致定位。


技术实现要素：

6.为此，本发明的主要目的在于提供一种多线激光定位方法及定位装置、计算机设备、存储介质，以解决背景技术中所提到的现有技术的缺陷。
7.为了实现上述目的，根据本发明第一个方面，提供了一种多线激光定位方法，步骤包括：步骤s100定义imu数据的状态分量、系统输入数据、系统过程噪声，以依据系统输入数据对每个状态分量带来的变化来建立系统状态模型；步骤s200设定系统状态模型的初始值，以初始化系统状态模型，并建立误差状态的协方差矩阵；步骤s300预测阶段：对每个接收到的imu数据进行卡尔曼滤波标准预测计算，步骤包括：将imu数据的时间间隔数据、系统输入数据，带入初始化后的系统状态模型，计算每个imu数据的状态预测值，并更新误差状态的协方差矩阵；步骤s400观测阶段：根据观测模型计算当前状态预测值下，激光数据点落在地图
点云中相应的平面上的残差，若残差不为零，则判断状态预测值与真值之间存在误差，该误差为误差状态；之后进行误差状态卡尔曼滤波法迭代计算，直至误差状态的值达标，以根据达标的误差状态的值输出对应的状态分量。
8.在可能的优选实施方式中，步骤s100中，定义的imu数据的状态分量包括：当前地图坐标系中imu的位置、姿态、速度、角速度偏置、加速度偏置、全局坐标系下重力向量；且上述每个单独的状态都在一种光滑流型上，将所有的状态组合成一个状态变量：则该状态变量所在的复合流型为：；其中为三维的特殊正交群，3d旋转可以用此群表示、为3维的欧式空间、为2-sphere，是一个半球定义如下：系统输入数据包括：imu自身坐标系下的角速度、加速度，则该系统输入数据为： ；系统过程噪声包括：角速度测量噪声、加速度测量噪声、陀螺仪的偏置随机游走过程噪声、加速度计的偏置随机游走过程噪声，则系统过程噪声为 ；所述系统状态模型为：，其中为18维的欧式空间，其中为imu的采样周期。
9.在可能的优选实施方式中，步骤s400中所述观测模型为：其中从激光雷达获取到的数据点为，为真实位置，为测量噪声，落在地图点云平面上的点，满足如下条件：满足如下条件：为该激光数据点所在平面在地图坐标系下的法向量，为当前状态下imu在地图坐标下的位姿，为激光坐标系到imu坐标系的变换矩阵，是将激光坐标系下的点变换到地图坐标系下，为该平面上的一个地图坐标系下的点，表示该平面上的一个向量；设imu一个采样周期 内的测量值一样，复合流型上的离散过程定义为：
每个imu采样周期引入变化可以看作是对复合流型系统上叠加的外部扰动，使用表示复合流型上叠加外部扰动，使用表示扰动所在空间与切空间结构相同时的叠加操作。
10.在可能的优选实施方式中，步骤s400中所述计算当前状态预测值下，激光数据点落在地图点云中相应的平面上的残差的步骤包括：遍历当前激光数据点，根据imu在地图中的位姿以及激光与imu相对位姿，将激光坐标系下的点变换到地图坐标系下：搜索距离激光数据点最近的n个地图点，判断能否组成平面，若为是，则计算该激光数据点落在该平面上的残差。
11.在可能的优选实施方式中，步骤s200设定系统状态模型的初始值，以初始化系统状态模型的步骤包括：设定初始时刻为k，该系统状态模型的初始值为：初始定位时保持静止，获取当前的位置与姿态、以设置为初始定位结果，将速度设置为、以当前状态下的角速度偏置均值赋予，同理计算加速度的均值赋值给、将加速度偏置设置为。
12.在可能的优选实施方式中，步骤s200中建立误差状态的协方差矩阵的步骤包括：将误差状态的协方差定义在系统状态模型的切空间上，初始协方差为：，其中是一个的方阵，其中的分量是一个的方阵，r为旋转分量；分量是一个的方阵，p为旋转分量；分量是一个的方阵，v为旋转分量；分量是一个的方阵，为旋转分量；分量是一个的方阵，为旋转分量；分量是一个的方阵，g为旋转分量；中其余位置均为0。
13.在可能的优选实施方式中，所述进行误差状态卡尔曼滤波法迭代计算的步骤包括：以预测阶段所获状态预测值为始，通过观测误差，计算出当前误差状态值，若该值未达标，则将该误差状态值更新为计算开始时的状态预测值，再次观测误差并计算此刻的误差状态值，多次迭代计算直至误差状态值达标，并以该达标的误差状态值更新误差状态的协方差矩阵。
14.为了实现上述目的，本发明第二个方面，还提供了一种多线激光定位装置，其包括：
存储单元，用于存储包括如上任一所述多线激光定位方法步骤的程序，以供控制单元，激光扫描单元，imu测量单元，处理单元，信息传输单元，适时调取执行；所述存储单元还存有地图点云数据；其中所述控制单元，用于协调：imu测量单元，用于在采样周期内采集每一帧激光数据之前的imu数据；处理单元，用于设定系统状态模型的初始值，以初始化系统状态模型，并建立误差状态的协方差矩阵；处理单元进一步将imu数据中的角速度、加速度数据，以及对应的每个imu数据的时间间隔数据带入初始化后的系统状态模型，计算每个imu数据的状态预测值，并更新误差状态的协方差矩阵；激光扫描单元，用于扫描场景获取激光点云数据；处理单元，进一步根据观测模型计算当前状态预测值下，激光点云的数据点落在地图点云中相应平面上的残差，若残差不为零，则判断状态预测值与真值之间存在误差，该误差为误差状态；之后进行误差状态卡尔曼滤波法迭代计算，直至误差状态的值达标，以据该值输出对应的状态分量；信息传输单元，用于传输处理单元处理后的状态分量数据。
15.为了实现上述目的，本发明第三个方面，还提供了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其中，所述处理器执行所述计算机程序时实现如上任一所述多线激光定位方法的步骤。
16.为了实现上述目的，本发明第四个方面，还提供了一种计算机可读存储介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现如上任一项所述多线激光定位方法的步骤。
17.通过本发明提供的该多线激光定位方法及定位装置、计算机设备、计算机可读存储介质，其有益效果如下：相比现有技术中，需要进行点云球面投影，计算每个点的法向量，执行生长聚类算法，再使用pca计算平面参数，本案中只需搜索每个点的最近的n个地图点，如5个点，通过评估这5个地图点是否组成平面即可，从而降低了平面检测算法的复杂度。
18.此外相比现有技术中，使用滑窗中多个相邻状态下的平面特征作为约束来修正位姿结果，实际上也是局部定位状态，只是一定程度上提高了定位精度，无法保证全局一致定位。本案直接使用全局地图中的平面特征作为约束，使位姿可以收敛到全局最优状态，可以保证全局一致性定位结果。
19.另一方面，相比现有技术中的姿态使用了四元数的表达，采用msckf框架进行优化计算，计算过程中姿态的增量更新后还需要进行归一化计算，保证其满足四元数要求，因此解精度会受这个过程的影响。本案整个模型在建立在流型上，可以通过指数映射将流型对应切空间上的增量变换回流型上，这个过程没有精度损失，从而整个计算精度也会更高。
附图说明
20.构成本技术的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：图1为本发明的多线激光定位方法的步骤示意图；
图2为本发明的多线激光定位方法的步骤逻辑示意图；图3为本发明的多线激光定位装置的结构示意图。
具体实施方式
21.为了使本领域的技术人员能够更好的理解本发明的技术方案，下面将结合实施例来对本发明的具体技术方案进行清楚、完整地描述，以助于本领域的技术人员进一步理解本发明。显然，本案所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。应当指出的是，对本领域的普通技术人员来说，在不脱离本发明构思及相互不冲突的前提下，本技术中的实施例及实施例中的特征可以相互组合。基于本发明中的实施例，在本领域普通技术人员没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明的揭露及保护范围。
22.此外本发明的说明书和权利要求书及附图中的术语“第一”、“第二”、“s1”、“s2”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本发明的实施例能够以除了在这里描述的那些以外的顺序实施。同时本发明中的术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含。
23.术语解释imu：inertialmeasurementuni，惯性测量单元。
24.激光与imu紧耦合：各传感器数据共同参与计算过程，并且部分传感器的内参作为待优化变量在计算过程中被优化。与之对应的是松耦合。
25.流型：流型是一种局部空间为线性空间的拓扑空间。光滑流型则表明该拓扑空间任何点处的切空间（即局部空间）唯一，也就是说流型的表面光滑、连续。复合流型则是由多个光滑流型组成的复合拓扑空间。
26.参考文献：1.b.m.bellandf.w.cathey,theiteratedkalmanfilterupdateasagauss-newtonmethod,ieeetrans.automat.contr.,vol.38,no.2,pp.294
–
297,1993.本发明提供的该多线激光定位方法，发明构思上，主要采用激光与imu紧耦合方式，将待求解的状态构建在复合流型（manifold）上，以imu的角速度与加速度作为控制输入，将当前激光数据中的平面点落在点云地图中平面上的程度做为观测模型，利用误差状态迭代卡尔曼滤波器求解最优状态，籍此获取当前机器人在地图中的位置与姿态。
27.（一）为此，如图1至图2所示，本发明实施例一提供了一种多线激光定位方法，其步骤包括s100-s400：步骤s100：定义imu数据的状态分量、系统输入数据、系统过程噪声，以依据系统输入数据对每个状态分量带来的变化来建立系统状态模型。
28.具体来说，该定义的imu数据的状态分量，就是本方案中待求解的状态，如包括：当前地图坐标系（也可以看作全局坐标系）中imu的：位置、
姿态、速度、角速度偏置、加速度偏置、全局坐标系下重力向量。
29.且每个单独的状态都在一种光滑流型上，将所有的状态组合成一个状态变量： 。
30.则该状态变量所在的复合流型为： 。
31.其中系统输入数据为imu自身坐标系下的：角速度、加速度、则该系统输入数据为：。
32.其中系统过程噪声为imu的：角速度测量噪声、加速度测量噪声、陀螺仪的偏置随机游走过程噪声、加速度计的偏置随机游走过程噪声、则系统过程噪声为。
[0033]-系统状态模型建立构思假定imu一个采样周期内的测量值一样，复合流型上的离散过程定义为：复合流型上的离散过程定义为：每个imu采样周期引入变化可以看作是对复合流型系统上叠加的外部扰动，注意扰动所在的空间并不一定与流型上的切空间结构相同，使用表示复合流型上叠加外部扰动。使用表示扰动所在空间与切空间结构相同时的叠加操作。
[0034]
下面说明下imu输入对每个状态分量带来的变化。
[0035]
对于速度来说，首先需要将imu自身坐标系下的测量数据转到全局坐标系下，表示去除偏置与噪声的加速度值。
[0036]
由于imu的读取的数据是包含重力分量，为的负重力分量（这样设置是为了后面计算偏导数方便），因此表示消去重力分量，变化量为：对于位置来说，变化量为：在实际使用时会忽略加速度部分：对于姿态来说，其变化量在切空间上为：
对于imu偏置有：重力 不发生变化：由此整理得系统状态模型：之后便可采用标准的迭代扩展卡尔曼滤波方法计算上述系统状态。
[0037]
步骤s200：设定系统状态模型的初始值，以初始化系统状态模型，并建立误差状态的协方差矩阵。
[0038]
具体来说，在实际的使用场景，假设点云地图已经提前构建好了，首先需要设置系统状态的初值，假设初始时刻为k。
[0039]
则：其中初始位置与姿态可以通过ndt或者粒子滤波等方法获取、此为现有技术不在此处赘述，籍此将当前状态下的与设置为初始定位结果。
[0040]
初始定位的时候车辆都是静止的，将 设置为。角速度偏置初值可以通过在采集如20个（或者更多）静止状态下的imu数据，计算角速度的平均值赋值给，同理计算加速度的均值赋值给，加速度的偏置通常设置为。
[0041]
则误差状态的协方差定义在其切空间上，初始协方差为：
ꢀꢀ
。
[0042]
其中位置误差、姿态误差与速度误差的协方差初始化为：。
[0043]
角速度偏置与加速度偏置误差的协方差初始化为： 。
[0044]
重力误差的协方差初始化为：。
[0045]
步骤s300：预测阶段，对每个接收到的imu数据进行卡尔曼滤波标准预测计算，步骤包括：将imu数据的时间间隔数据、系统输入数据，带入初始化后的系统状态模型，计算每
个imu数据的状态预测值，并更新误差状态的协方差矩阵。
[0046]
具体来说，在系统初始化完成后，开始缓存接收到的imu数据，直到接收到一帧激光数据。由于imu数据频率通常都远高于激光数据频率，在接收到一帧激光数据之前，往往都会缓存了若干个imu数据，每个imu数据都可以做一次预测。
[0047]
即卡尔曼滤波标准预测过程为：具体示例：假设第一个imu数据的时间间隔为 ，角速度为 ，加速度为 ，则有：此时。
ꢀꢀ
代表的意义为复合流型上各个分量切空间的扰动，就是积分的过程。
[0048]
对于分量、、 、 来说，他们属于，扰动可以直接叠加计算，即：对于分量
ꢀꢀ
，其中：对于分量 ，这样就可以计算出使用一个imu数据的状态预测值。
[0049]
然后是更新误差状态的协方差矩阵： 。
[0050]
例如，对于第一个imu数据来说。
[0051][0051]
其中 为状态的误差对应在该点处的切空间上表达。
[0052]
其中：其中：其中：其中：其中： 。
[0053]
至此每一个imu数据都可以执行一次状态预测以及计算误差状态的协方差，之后
需要把当前激光数据之前所有的接收到imu执行过预测阶段之后，需要执行更新程序将状态修正到最优。
[0054]
步骤s400观测阶段：根据观测模型计算当前状态预测值下，激光数据点落在地图点云中相应的平面上的残差，若残差不为零，则判断状态预测值与真值之间存在误差，该误差为误差状态；之后进行误差状态卡尔曼滤波法迭代计算，直至误差状态的值达标，以据该值输出对应的状态分量。
[0055]
具体来说，假设点云地图建的足够完美，与实际场景基本一致，且场景不发生任何变化，其次当前定位位置足够精确，那么在该位置处采集到的点云变换到地图坐标系下就应该与点云地图完全贴合。通常环境中都存在较多的平面，比如地面，墙面以及表面包含平面的物体，显然在这样的假设条件下，采集到的激光点云中的平面点也都应该落在地图中相应的平面上。
[0056]
假设从激光获取到的数据点假设从激光获取到的数据点为真实位置。
[0057]
为测量噪声，落在平面上的点，满足如下条件： 为该点所在平面在地图坐标系下的法向量。
[0058] 为当前状态下imu在地图坐标下的位姿。
[0059]
为激光坐标系到imu坐标系的变换矩阵（传感器安装后就已经确定，可以通过标定得到精确值，标定方法属于现有技术在此不再赘述）。
[0060] 就是将激光坐标系下的点变换到地图坐标系下。
[0061]
为该平面上的一个地图坐标系下的点。
[0062]
表示该平面上的一个向量。
[0063]
由平面上的任意向量与该平面的法向量垂直可知乘积为零。实际情况由于地图与激光点云均存在误差，导致平面法向量与平面上的向量计算都有误差，两个向量相乘的乘积不为零，此外当前的定位位置如果存在偏差，该乘积也不为零，定义观测模型为：每一帧激光数据都应该有很多平面点，假设有m个，则观测方程为：则观测方程为：为观测噪声。
[0064]
在更新程序中，使用迭代方法进一步优化状态，定义为第j次迭代的状态估计。当时（即刚从预测阶段执行完毕，第一次执行更新计算），为预测阶段结束后的状态估计。
[0065]
将第j次迭代的观测残差定义为：
其中 为观测值，定义为第j次迭代的状态估计与真值之间的误差，该误差在处切空间上表示。
[0066]
在可以进行上述计算之前，其实还需要解决如何从当前激光数据中筛选出属于某个平面上的点（上面的 ）以及该平面的法向量是多少（上面的）。
[0067]
实现方法如下：遍历当前激光数据点，首先根据imu在地图中的位姿以及激光与imu相对位姿，将激光坐标系下的点变换到地图坐标系下：然后使用kdtree（可以选择使用常用的pcl库中kdtree或者nanoflann）搜索距离该点最近的n个地图点，如本实施例下优选为5个地图点，为了避免选取的地图点距离过近，需要对点云地图做分辨率是 0.2的下采样（使用pcl库中的voxelgrid实现）。
[0068]
获取到最近的5个点后，需要判断这5个点是否可以组成平面。假设这5个点在一个平面方程为 的平面上，则有：求解此超定方程（可以使用eigen求解）即可得到该平面的系数。
[0069]
然后将每个点逐个带入该方程，必须全部满足该方程才认定是一个平面，该平面的法向量即为。
[0070]
由于误差的存在，即使点位于平面上，方程计算的的数值可能不为，设定一个门限，只要在附近就可以。此外，在迭代计算过程中由于估计的状态与真值之间存在误差，此时的点也不会落在平面上，此时残差为：
之后按照迭代卡尔曼更新计算（参考文献1）：以预测阶段所获状态预测值为始，通过观测误差，计算出当前误差状态值，若该值大于门限，则将该误差状态值更新为计算开始时的状态预测值，再次计算观测误差以及此刻的误差状态值，多次迭代计算直至状态误差达标，此时将该求解出的状态值更新误差状态的协方差矩阵，并根据状态值输出当前定位位置与姿态。
[0071]
实现方法如下：误差状态，。
[0072] 为每次迭代后的误差，其中：对于不同的流型类型有不同的计算公式：有不同的计算公式：最终每次迭代的更新可计算为：每次迭代完成后，会判断当前的误差是否已经足够小，判断的方法就是如果 中每分量值都小于0.001（即一个很小的门限）就认为当前状态已经优化求解完毕。
[0073]
最后更新误差状态的协方差即可，更新如下：对于不同的流型类型 有不同的计算公式：
至此一次完整的状态误差迭代扩展卡尔曼滤波计算完成。待接收到下次imu数据与激光数据后依照上述方法继续计算，如图2流程图所示。
[0074]
由此值得一提的是，相比其他定位方法，每次计算前都需要结合当前里程计结果提供初值，本方案完全不依赖里程计，这样就可以很好解决打滑问题。
[0075]
另一方面，相比背景技术中相近的现有技术，本案中只需搜索每个点的最近的5个地图点，通过评估这5个地图点是否组成平面即可，从而降低了平面检测算法的复杂度。
[0076]
同时本案能直接使用全局地图中的平面特征作为约束，使位姿可以收敛到全局最优状态，可以保证全局一致性定位结果。
[0077]
此外本案整个模型在建立在流型上，可以通过指数映射将流型对应切空间上的增量变换回流型上，这个过程没有精度损失，从而整个计算精度也会更高。
[0078]
（二）请参阅图3所示，对应实施例一的多线激光定位方法，本发明还提供了一种多线激光定位装置，其包括：存储单元，用于存储包括如实施例一所述多线激光定位方法步骤的程序，以供控制单元，激光扫描单元，imu测量单元，处理单元，信息传输单元，适时调取执行；所述存储单元还存有地图点云数据。
[0079]
其中所述控制单元，用于协调：imu测量单元，用于在采样周期内采集每一帧激光数据之前的imu数据。
[0080]
处理单元，用于设定系统状态模型的初始值，以初始化系统状态模型，并建立误差状态的协方差矩阵；处理单元进一步将imu数据中的角速度、加速度数据，以及对应的每个imu数据的时间间隔数据带入初始化后的系统状态模型，计算每个imu数据的状态预测值，并更新误差状态的协方差矩阵。
[0081]
激光扫描单元，用于扫描场景获取激光点云数据。
[0082]
处理单元，进一步根据观测模型计算当前状态预测值下，激光点云的数据点落在地图点云中相应平面上的残差，若残差不为零，则判断状态预测值与真值之间存在误差，该误差为误差状态；之后进行误差状态卡尔曼滤波法迭代计算，直至误差状态的值达标，以据该值输出对应的状态分量。
[0083]
信息传输单元，用于传输处理单元处理后的状态分量数据，以供定位。
[0084]
（三）对应实施例一的多线激光定位方法，本发明还提供了一种计算机设备，包括存储器和处理器，所述存储器存储有计算机程序，其中，所述处理器执行所述计算机程序时实现如实施例一中所述多线激光定位方法的步骤。
[0085]
（四）对应实施例一的多线激光定位方法，本发明还提供了一种计算机可读存储介质，其上存储有计算机程序，其中，所述计算机程序被处理器执行时实现如实施例一中所述多线激光定位方法的步骤。
[0086]
综上所述，以上公开的本发明优选实施例只是用于帮助阐述本发明。优选实施例并没有详尽叙述所有的细节，也不限制该发明仅为所述的具体实施方式。显然，根据本说明书的内容，可作很多的修改和变化。本说明书选取并具体描述这些实施例，是为了更好地解释本发明的原理和实际应用，从而使所属技术领域技术人员能很好地理解和利用本发明。本发明仅受权利要求书及其全部范围和等效物的限制，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。
[0087]
本领域技术人员可以理解，除了以纯计算机可读程序代码方式实现本发明提供的系统、装置及其各个模块以外，完全可以通过将方法步骤进行逻辑编程来使得本发明提供的系统、装置及其各个模块以逻辑门、开关、专用集成电路、可编程逻辑控制器以及嵌入式微控制器等的形式来实现相同程序。所以，本发明提供的系统、装置及其各个模块可以被认为是一种硬件部件，而对其内包括的用于实现各种程序的模块也可以视为硬件部件内的结构；也可以将用于实现各种功能的模块视为既可以是实现方法的软件程序又可以是硬件部件内的结构。
[0088]
此外实现上述实施例方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序存储在一个存储介质中，包括若干指令用以使得单片机、芯片或处理器（processor）执行本技术各个实施例所述方法的全部或部分步骤。而前述的存储介质包括：u盘、移动硬盘、只读存储器（rom，read-only memory）、随机存取存储器（ram，random access memory）、磁碟或者光盘等各种可以存储程序代码的介质。
[0089]
此外，本发明实施例的各种不同的实施方式之间也可以进行任意组合，只要其不违背本发明实施例的思想，其同样应当视为本发明实施例所公开的内容。