CN110926485B - 一种基于直线特征的移动机器人定位方法及系统 - Google Patents

Abstract

本发明属于移动对象导航定位相关技术领域，并公开了一种基于直线特征的移动机器人定位方法及系统，其首先采集移动机器人周围的环境特征以得到点云数据，处理得到直线并构建地图；然后采集移动机器人控制量数据，并得到移动机器人在相应控制量下的预测位姿，同时采集周围的局部点云信息并构建观测直线；再将观测直线转换至预测位姿状态对应的全局坐标系中，并将观测直线与地图直线进行匹配，利用观测直线和地图直线的对应关系构建惩罚函数并求解获得移动机器人的实际位姿。通过本发明，无需对环境进行特殊改造，无需使用大量辅助工具条件下，即可实现位姿定位，同时还能解决移动机器人绑架问题，实现移动机器人的精确、高效定位。

Description

一种基于直线特征的移动机器人定位方法及系统

技术领域

本发明属于移动对象导航定位相关技术领域，更具体地，涉及一种基于直线特征的移动机器人定位方法及系统。

背景技术

移动机器人的定位导航技术广泛使用于自主AGV，无人驾驶，无人机等领域。目前的导航定位方案主要是以下几种，使用特殊的标志物作为定位导航的路标，如反光板定位，二维码导航定位等，需要对场景进行特殊的布置，多使用在小范围的室内区域。另一类是不使用特殊的标志物进行定位，通过移动机器人自身的传感器，完成定位导航，典型的有使用激光雷达利用卡尔曼滤波算法实现定位导航，如经典的EKF-SLAM；还有研究比较深入的GMAPPING；在这类方法中，经过较长的时间累积后可能会存在误差的累积，导致定位的精度下降。也有使用优化策略进行定位导航的相关方法，如图优化。使用现有的优化方法，计算的时间较长，计算的复杂度较高，不利于满足定位的实时性要求。

现有技术中已经有使用直线结合卡尔曼滤波方法，实现定位导航，但是在实际的使用过程中，存在着定位精度不高，参数调节困难，经过长时间的运行之后，由于误差积累导致定位失败等问题。相应的，本领域亟需一种更为妥善解决方式，以满足日益提高的工程项目中对移动机器人的精度要求。

发明内容

针对现有技术的以上缺陷或改进需求，本发明提供了一种基于直线特征的移动机器人定位方法及系统，其通过直线具有方向性的性质，对定位算法进行了重新设计，无需对周围环境进行特殊改造的情况下，即可实现对不同情况下的位姿定位，并且能够满足实时性强、定位精度高、定位过程鲁棒性强的要求。

为实现上述目的，按照本发明的一个方面，提出了一种基于直线特征的移动机器人定位方法，其包括如下步骤：

S1采集移动机器人周围的环境特征以得到点云数据，对点云数据进行数据处理得到相关的直线，利用这些直线构建得到地图；

S2采集移动机器人运行过程中的控制量数据，通过构建移动机器人的运动学或者动力学模型得到移动机器人在相应控制量下的预测位姿，同时在移动机器人运动过程中还采集周围的局部点云信息，利用这些局部点云信息构建得到观测直线；

S3将得到的所述观测直线转换至所述预测位姿状态对应的全局坐标系中，并将全局坐标系下的观测直线与构建的所述地图中的直线进行匹配，利用观测直线和地图中直线一一对应的关系构建惩罚函数并求解获得移动机器人的实际位姿，以此完成移动机器人的定位导航。

作为进一步优选的，步骤S3中，在观测直线与地图中的直线进行匹配时，若全局坐标系下的观测直线与地图中某条直线的差值的绝对值小于设定的阈值，则认为匹配成功，并记录匹配成功的直线数量N；若未匹配成功，则认为此观测直线为环境中的新特征，将其添加至步骤S1构建的地图中。

作为进一步优选的，当N＜2时将出现绑架问题，优选利用直线的有向性解决该问题。

作为进一步优选的，具体采用如下步骤利用直线的有向性解决绑架问题：

首先，将观测直线中的角度部分分别同世界坐标系中直线中的角度特征进行相减得到角度差的矩阵

对

中元素进行排序；

然后，设定间隔为

的窗口，滑动窗口，当窗口内包含的数据最多时将该窗口内的数据求和，求和后获得结果即为移动机器人的姿态角；

最后，通过姿态角即可得到移动机器人的姿态值，根据移动机器人的观测直线与地图直线之间关系，实现移动机器人的位置求解，以此解决绑架问题。

作为进一步优选的，步骤S3中，移动机器人的实际位姿具体采用如下步骤求解：

S31提取已匹配上的观测直线

和地图中的直线

并建立惩罚函数J：

其中，X_k为待求解的移动机器人在k时刻的实际位姿；h为在X_k下从观测直线到地图直线的映射；

S32设定求解域范围，在求解域范围内求解使惩罚函数J取得最小值时对应的解即为待求解的移动机器人在k时刻的实际位姿X_k。

作为进一步优选的，在求解使惩罚函数J取得最小值时对应的解时，第一次使用大步长ΔX₁进行大范围求解得到最优解

然后以

为中心，以

为半径的邻域，再以小步长ΔX₂求解得到最优解

以该最优解

作为X_k的最优解。

作为进一步优选的，所述移动机器人为自主AGV、无人驾驶车辆、无人船或无人机。

按照本发明的另一方面，提供了一种基于直线特征的移动机器人定位系统，其包括如下模块：

第一采集模块，用于采集移动机器人周围的环境特征以得到点云数据，并对点云数据进行数据处理得到相关的直线，利用这些直线构建得到地图；

第二采集模块，用于采集移动机器人运行过程中的控制量数据，并通过构建移动机器人的运动学或者动力学模型得到移动机器人在相应控制量下的预测位姿，同时在移动机器人运动过程中还采集周围的局部点云信息，利用这些局部点云信息构建得到观测直线；

匹配求解模块，用于将得到的所述观测直线转换至所述预测位姿状态对应的全局坐标系中，并将全局坐标系下的观测直线与构建的所述地图中的直线进行匹配，利用观测直线和地图中直线一一对应的关系构建惩罚函数并求解获得移动机器人的实际位姿，以此完成移动机器人的定位导航。

总体而言，通过本发明所构思的以上技术方案与现有技术相比，主要具备以下的技术优点：

1.本发明的移动机器人构图和位姿定位过程不需要外界的专门辅助设备或者装置，也不需要对环境进行特殊的改造，只需依赖自身的传感器即可实现建图和姿态定位，相应的具备便于操作、适应性强。

2.本发明所设计的移动机器人的定位算法，采用直线进行建图，利用直线进行定位，在良好的结构化环境中可以达到定位误差e＜2cm，具备很高的定位；同时由于算法的性质决定了这个方法不存在累积误差，只存在单步误差，也就是说精度较高。

3.本发明中对移动机器人的绑架问题也提出了解决方法，因此本发明方法具有较好的鲁棒性。

4.本发明通过使用大步长和小步长的求解过程搭配使用，降低了计算的复杂度，提高计算的速度，同时避免了错误解的发生；同时避免了由于映射函数的不连续而造成求解的复杂性，提高了计算的实时性。

附图说明

图1是按照本发明所构建的基于直线特征的移动机器人定位方法的流程图；

图2是定位过程中解决移动机器人绑架问题的流程图；

图3是用于示范性显示本发明中传感器对环境特征的提取、动坐标系和全局坐标系的关系；

图4是用于示范性显示本发明中动坐标系中直线和地图中直线的匹配过程；

图5是用于示范性显示本发明中求解过程中大步长和小步长的搭配使用理论示意图。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。

图1是按照本发明所构建的基于直线特征的移动机器人定位方法的流程图，所述的移动机器人譬如为自主AGV、无人驾驶车辆、无人船、无人机等。图2是定位过程中解决移动机器人绑架问题的流程图。如图1所示，所述基于直线特征的移动机器人定位方法包含了使用直线进行建图过程以及位姿定位过程，两者同时进行，具体包括如下步骤：

S1环境信息采集步骤

选择适当的位置(原则上可以选择任意位置)建立移动机器人所在环境的世界坐标系的原点，针对作为定位对象的移动机器人，对环境信息的采集，使用AGV自身的位姿状态建立动坐标系，直接使用固定在其自身的车载传感器实时地对周围环境进行采集，并且得到反应环境的多轮廓离散点以及其形成的点云图，即采集移动机器人周围的环境特征以得到点云数据，对点云数据进行数据处理得到相关的直线，利用这些直线构建得到地图，如图3所示传感器获取点云，拟合点云数据得到的环境图(即地图)；具体的，根据离散点的在AGV的车载动坐标系的坐标信息

转为直角坐标系表示为P＝{(x_i,y_i)}；对点云信息进行拟合得到多条直线，记录直线；并使用霍夫变换表示直线，将一个特征表征为

其中ρ_i表示直线距离坐标系的最近距离，

表示形成ρ_i的极线同坐标系的x轴的角度；

S2数据采集及位姿的预测

S21移动机器人的自身控制量及局部点云信息的采集步骤

采集移动机器人运行过程中的控制量数据，具体的通过移动机器人自身的传感器以一定的频率采样获取其运动过程中的控制量并记录；同时在移动机器人运动过程中还采集周围的局部点云信息，利用这些局部点云信息构建得到观测直线，具体使用移动机器人上的固设的传感器获取得到局部点云信息；

S22移动机器人的自身预测位姿的计算步骤

通过构建移动机器人的运动学或者动力学模型得到移动机器人在相应控制量下的预测位姿，具体的根据移动机器人的结构特点，建立移动机器人的运动学或者是动力学模型X_k＝f(X_k-1,U_k)，根据S21中获取得到的移动机器人的控制量的数据

以及上一时刻的机器人位姿X_k-1，带入到移动机器人的模型中，可以求解得到在相应控制量作用下的在k时刻的预测位姿

通常预测位姿是存在误差的，并且仅使用该方法，会引起误差的累计，导致后期的误差非常大，无法满足定位的精度要求，因此需要进一步的处理；

S3匹配与求解步骤

首先将得到的所述观测直线转换至所述预测位姿状态对应的全局坐标系中，具体的先将点云信息转为动坐标系下的直线，然后使用旋转矩阵将动坐标系下的直线转为全局(世界)坐标系下的直线；

然后将全局坐标系下的观测直线与构建的所述地图中的直线进行匹配，利用观测直线和地图中直线一一对应的关系构建惩罚函数并求解获得移动机器人的实际位姿，以此完成移动机器人的定位导航。

具体的，步骤S3中，移动机器人的实际位姿具体采用如下步骤求解：

S31提取已匹配上的观测直线

和地图中的直线

并建立惩罚函数J：

其中，X_k为待求解的移动机器人在k时刻的实际位姿；h为在X_k下从观测直线到地图直线的映射；

S32首先设定求解域范围，在本发明中需要设定合适的求解域，降低计算的复杂度，结合建立的预测位姿

设定求解域范围为[X_k+ΔX,X_k-ΔX]，为了使得计算的准确性，避免意外的情况出现，通常给求解范围添加一个安全系数[X_k+ηΔX,X_k-ηΔX]；然后在求解域范围内求解使惩罚函数J取得最小值时对应的解即为待求解的移动机器人在k时刻的实际位姿X_k。

由于映射h的非线性，同时存在点的不连续特点，使用穷举法求解数值解，为了进一步降低计算的复杂度，在第一次使用大步长ΔX₁进行大范围求解得到最优解

为了提高求解的精度，以

为中心，以

为半径的邻域，再以一个小步长ΔX₂求解得到最优解

以

作为X_k的最优解。其中，大步长ΔX₁和小步长ΔX₂可根据需要进行设定。

进一步的，步骤S3中，在观测直线与地图中的直线进行匹配时，若全局坐标系下的观测直线

与地图中某条直线

的差值的绝对值小于设定的阈值，即满足

则认为匹配成功，并记录匹配成功的直线数量N；若未匹配成功，则认为此观测直线为环境中的新特征，将其添加至步骤S1构建的地图中。

更为具体的，匹配成功的数量N≥2时，则定位可以成功的进行下去，当N＜2，此时定位过程是不准确的，甚至是错误的，也就是出现“绑架”问题，此时位姿定位过程优选为解决移动机器人的“绑架”问题。

具体的，使用直线的性质，利用直线的有向性可有效解决移动机器人的姿态求解问题(也即解决上述绑架问题)，具体的包括如下步骤(即移动机器人绑架时的姿态求解步骤)：将观测直线中的角度部分分别同世界坐标系中直线中的角度特征进行相减可以得到角度差的矩阵

其中

对

中元素进行排序，设定一个间隔为

的窗口，滑动窗口，当窗口内包含的数据最多时候此时的将该窗口内的数据

求和取均值，即是移动机器人的最优姿态角即

其中n表示窗口内的数据个数。此时可以得到移动机器人的姿态值，那么可以根据移动机器人的姿态将观测直线的在世界坐标系的方向求解，使用转换后的直线方向，可以和地图中的直线进行匹配。

进一步的，采用如下方式进行匹配：建立地图中特征之间的矩阵表达，可以得到关于路标节点的关系矩阵Ω_n×n(l_i,j)，建立观测路标的关系矩阵ζ_m×m(l_k,l)，通过比较Ω_n×n(l_i,j)中每一行同ζ_m×m(l_k,l)中满足

的数量，得到数量矩阵￥_m×n，比较每一行满足￥(i,j)≥3的最大的数，数量越大，那么所占的权重越大，观测特征与地图特征匹配上的可能性越大，并且每个特征匹配上的权重是相等的，假设观测矩阵中每一行中有k个元素匹配成功，那么匹配上的权重记作

从而完成对平行直线的匹配过程与权重记录过程，选取其中满足条件的且权重较大的元素对应的相应直线分别对应的观测直线和地图直线。

进一步的，采用如下方式进行位姿求解：记录下移动机器人在最后一次匹配的直线个数N≤2时的位姿值，扩大移动机器人的位置求解的范围，按照步骤S32所述的方法实现对位姿的求解。即在构建惩罚函数之后，设定求解域时需调用出现N＜2时的位姿状态的数据，扩大求解域的范围，使得到的最优解更合理有效。

综上，按照本发明的包含的多种工况下的位姿确定过程具体可包括：①传感器对环境信息的采集以及传感器对移动机器人自身控制量的采集；②点云数据的处理，根据移动机器人的结构特点建立运动学或者动力学模型；③从点云数据中完成直线的提取，利用控制量和构建的数学模型，完成预测位姿的计算；④利用预测位姿，完成观测直线到世界坐标中的直线转换；⑤完成观测观测直线同地图中直线的匹配过程；⑥提取得到与观测直线对应的地图直线，完成惩罚函数的构建；⑦利用预测位姿和控制量的作用效果，设定求解域，降低求解的复杂度；⑧利用大步长和小步长搭配结合使用的方法，保证求解结果的精确性；⑨当出现了“绑架”现象，利用直线的方向性的特点，构建观测直线同地图直线的角度差矩阵，排序，利用滑动窗口的方法，得到最佳姿态解；⑩利用姿态信息求解完成，实现部分直线的匹配过程，完成惩罚函数的建立；

利用上述⑦⑧中相似的手段实现对求解域的设定以及位置的求解。

本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种基于直线特征的移动机器人定位方法，其特征在于，包括如下步骤：

S1采集移动机器人周围的环境特征以得到点云数据，对点云数据进行数据处理得到相关的直线，利用这些直线构建得到地图；

S2采集移动机器人运行过程中的控制量数据，通过构建移动机器人的运动学或者动力学模型得到移动机器人在相应控制量下的预测位姿，同时在移动机器人运动过程中还采集周围的局部点云信息，利用这些局部点云信息构建得到观测直线；

S3将得到的所述观测直线转换至所述预测位姿状态对应的全局坐标系中，并将全局坐标系下的观测直线与构建的所述地图中的直线进行匹配，利用观测直线和地图中直线一一对应的关系构建惩罚函数并求解获得移动机器人的实际位姿，以此完成移动机器人的定位，具体的，在观测直线与地图中的直线进行匹配时，若全局坐标系下的观测直线与地图中某条直线的差值的绝对值小于设定的阈值，则认为匹配成功，并记录匹配成功的直线数量N；若未匹配成功，则认为此观测直线为环境中的新特征，将其添加至步骤S1构建的地图中，当N＜2时将出现绑架问题，利用直线的有向性解决该问题，其中，具体采用如下步骤利用直线的有向性解决绑架问题：

首先，将观测直线中的角度部分分别同世界坐标系中直线中的角度特征进行相减得到角度差的矩阵

对

中元素进行排序；

然后，设定间隔为

的窗口，滑动窗口，当窗口内包含的数据最多时将该窗口内的数据求和，求和后获得结果即为移动机器人的姿态角；

最后，通过姿态角即可得到移动机器人的姿态值，根据移动机器人的观测直线与地图直线之间关系，实现移动机器人的位置求解，以此解决绑架问题；

移动机器人的实际位姿具体采用如下步骤求解：

S31提取已匹配上的观测直线

和地图中的直线

并建立惩罚函数J：

其中，X_k为待求解的移动机器人在k时刻的实际位姿；h为在X_k下从观测直线到地图直线的映射；

S32设定求解域范围，在求解域范围内求解使惩罚函数J取得最小值时对应的解即为待求解的移动机器人在k时刻的实际位姿X_k。

2.如权利要求1所述的基于直线特征的移动机器人定位方法，其特征在于，在求解使惩罚函数J取得最小值时对应的解时，第一次使用大步长ΔX₁进行大范围求解得到最优解

然后以

为中心，以

为半径的邻域，再以小步长ΔX₂求解得到最优解

以该最优解

作为X_k的最优解。

3.如权利要求1或2所述的基于直线特征的移动机器人定位方法，其特征在于，所述移动机器人为自主AGV、无人驾驶车辆、无人船或无人机。

4.一种基于直线特征的移动机器人定位系统，其特征在于，包括如下模块：

第一采集模块，用于采集移动机器人周围的环境特征以得到点云数据，并对点云数据进行数据处理得到相关的直线，利用这些直线构建得到地图；

第二采集模块，用于采集移动机器人运行过程中的控制量数据，并通过构建移动机器人的运动学或者动力学模型得到移动机器人在相应控制量下的预测位姿，同时在移动机器人运动过程中还采集周围的局部点云信息，利用这些局部点云信息构建得到观测直线；

匹配求解模块，用于将得到的所述观测直线转换至所述预测位姿状态对应的全局坐标系中，并将全局坐标系下的观测直线与构建的所述地图中的直线进行匹配，利用观测直线和地图中直线一一对应的关系构建惩罚函数并求解获得移动机器人的实际位姿，以此完成移动机器人的定位，具体的，在观测直线与地图中的直线进行匹配时，若全局坐标系下的观测直线与地图中某条直线的差值的绝对值小于设定的阈值，则认为匹配成功，并记录匹配成功的直线数量N；若未匹配成功，则认为此观测直线为环境中的新特征，将其添加至第一采集模块构建的地图中，当N＜2时将出现绑架问题，利用直线的有向性解决该问题，其中，具体采用如下步骤利用直线的有向性解决绑架问题：

首先，将观测直线中的角度部分分别同世界坐标系中直线中的角度特征进行相减得到角度差的矩阵

对

中元素进行排序；

然后，设定间隔为

的窗口，滑动窗口，当窗口内包含的数据最多时将该窗口内的数据求和，求和后获得结果即为移动机器人的姿态角；

最后，通过姿态角即可得到移动机器人的姿态值，根据移动机器人的观测直线与地图直线之间关系，实现移动机器人的位置求解，以此解决绑架问题；

移动机器人的实际位姿具体采用如下步骤求解：

S31提取已匹配上的观测直线

和地图中的直线

并建立惩罚函数J：

其中，X_k为待求解的移动机器人在k时刻的实际位姿；h为在X_k下从观测直线到地图直线的映射；

S32设定求解域范围，在求解域范围内求解使惩罚函数J取得最小值时对应的解即为待求解的移动机器人在k时刻的实际位姿X_k。

Images