CN108709560A - 基于直线段特征的载运机器人高精度定位导航方法 - Google Patents

Abstract

本发明一种基于直线段特征的载运机器人高精度定位导航方法，针对工业载运机器人的室内应用特点，以直线段替代传统的点作为建图特征，由激光雷达测量的环境或障碍物距离、角度，计算其世界坐标，并通过最小二乘做直线段近似，一方面消除了测量误差，同时降低了系统的状态维度，提高了运算速度；利用当前测量与已建图数据的关联性，并通过扩展卡尔曼滤波算法进一步精确估计机器人的运动状态，二次提高了系统的定位精度和可靠性。

Description

基于直线段特征的载运机器人高精度定位导航方法

技术领域

本发明涉及智能控制技术，特别涉及AGV智能控制技术领域，具体的，其展示一种基于直线段特征的载运机器人高精度定位导航方法。

背景技术

SLAM技术，即实时定位与地图构建，其是AGV智能控制技术的核心，广泛应用于VR/AR、无人机、无人驾驶、机器人定位导航等领域。

SLAM主要分为两种：激光slam和视觉slam；两种方法广泛应用在定位导航领域。

但是工业环境中，现场的纹理单一，难以建立稳定的图像特征点，所以视觉slam不适合工业现场；激光雷达由于在线扫描干扰、以及自身精度等问题，不可避免导致了点云的测量误差。

此外，目前激光SLAM技术采用的激光雷达+前端ICP匹配+后端优化算法积累误差大，导致定位精度以及地图构建的准确度较低，无法应用于对精度、准确度要求较高的工业领域。

因此，有必要提供一种基于直线段特征的载运机器人高精度定位导航方法来解决上述问题。

发明内容

本发明的目的是提供一种基于直线段特征的载运机器人高精度定位导航方法，其针对工业载运机器人的室内应用特点，以直线段替代传统的点作为建图特征，由激光雷达测量的环境或障碍物距离、角度，计算其世界坐标，并通过最小二乘做直线段近似，一方面消除了测量误差，同时降低了系统的状态维度，提高了运算速度；利用当前测量与已建图数据的关联性，并通过扩展卡尔曼滤波算法进一步精确估计机器人的运动状态，二次提高了系统的定位精度和可靠性。

技术方案如下：

一种基于直线段特征的载运机器人高精度定位导航方法，包括如下步骤：

S1）自主定位导航小车动力学模型：

(1)

(2)

(3)

(4)

(5)

下一时刻小车中心O’的位置采用二阶的Runge-Kutta迭代公式（公式6）得到：

(6)

其中， _L和 _R分别代表左轮和右轮的角速度，v_L和v_R分别代表左轮和右轮的线速度，v代表小车中心O’点的线速度， _E为小车的旋转角速度，r为小车轮子半径，L为两个轮子之间的距离， _G为陀螺仪测得的角速度；

S2）使用垂直距离方法提取线特征：

S2-1）选取一系列点，并将选取的第一个点和最后一个点用直线连接；

S2-2）计算其余点到这条直线的距离，取得到达直线距离最大的这个点；

S2-3）设置一个距离阈值，如果得到的最大距离超过这个距离阈值，则提取该点，设为P_L，P_SP_L和P_LP_E为提取的线特征，如果得到的最大距离小于距离阈值，则放弃提取的点，P_SP_E为提取的线特征；

S2-4）通过最小二乘法对点集进行拟合，最终得到线特征；

S3）利用几何匹配方法进行匹配：

在全局地图和局部地图中分别寻找四个点，并连成线段，组成一种特定图式；

S4）基于最小二乘法的ICP方法：

G1、G2表示从全局地图中提取的几何特征，l为局部地图中的有向线段，P_I ⁱ和P_l ^e为线段l的两端，C_l为l中心，则,用点法式直线方程来描述G2的函数为：

P·u_l=r_l (7)

其中P为G2上的一个点，u_l为法向量，r_l为实数；

线段l相对于旋转中心旋转并且平移（）后：

(8)

匹配错误为：

(9)

所有点的匹配错误为：

（10）

其中n_l为l上点的数量：

(11)

通过得到S的最小值，即可得到（）；

S5）利用扩展卡尔曼滤波预测方法（EKF），进一步提高小车定位的精度：

S5-1）设定小车初始位置(x,y,θ)和协方差P；

S5-2）通过动力学模型预测小车的下一时刻位置；

S5-3）通过更新协方差P：

(12)

(13)

(14)

(15)

(16)

(17)

S5-4)评估

S5-4-1）计算卡尔曼增益：

(18)

S5-4-2）更新位置坐标评估：

(19)

S5-4-3）更新协方差：

(20)

S5-4-4）增加新的标记点：

(21)

(22)

(23)

(24)

(25)

(26)。

进一步的，步骤S3）中，四对特征点需满足以下条件：

1）；

2）LA1=GA1,LA2=GA2；

3）由这四个点组成的图形在局部地图和全局地图中必须是独一无二的。

与现有技术相比，本发明针对工业载运机器人的室内应用特点，以直线段替代传统的点作为建图特征，由激光雷达测量的环境或障碍物距离、角度，计算其世界坐标，并通过最小二乘做直线段近似，一方面消除了测量误差，同时降低了系统的状态维度，提高了运算速度；利用当前测量与已建图数据的关联性，并通过扩展卡尔曼滤波算法进一步精确估计机器人的运动状态，二次提高了系统的定位精度和可靠性。

附图说明

图1是本发明的实施例中，AGV小车的示意图；

图2是本发明的实施例中，线性特征提取示意图；

图3是本发明的实施例中，四对特征点的示意图；

图4是本发明的实施例中，最小二乘法的ICP方法的示意图。

具体实施方式

实施例：

本实施例展示一种基于直线段特征的载运机器人高精度定位导航方法，包括如下步骤：

S1）自主定位导航小车动力学模型：

(1)

(2)

(3)

(4)

(5)

下一时刻小车中心O’的位置采用二阶的Runge-Kutta迭代公式（公式6）得到：

(6)

参阅图1，其中， _L和 _R分别代表左轮和右轮的角速度，v_L和v_R分别代表左轮和右轮的线速度，v代表小车中心O’点的线速度， _E为小车的旋转角速度，r为小车轮子半径，L为两个轮子之间的距离， _G为陀螺仪测得的角速度；

S2）使用垂直距离方法提取线特征，参阅图2：

S2-1）选取一系列点，并将选取的第一个点和最后一个点用直线连接；

S2-2）计算其余点到这条直线的距离，取得到达直线距离最大的这个点；

S2-3）设置一个距离阈值，如果得到的最大距离超过这个距离阈值，则提取该点，设为P_L，P_SP_L和P_LP_E为提取的线特征，如果得到的最大距离小于距离阈值，则放弃提取的点，P_SP_E为提取的线特征；

S2-4）通过最小二乘法对点集进行拟合，最终得到线特征；

S3）利用几何匹配方法进行匹配：

参阅图3，在全局地图和局部地图中分别寻找四个点，并连成线段，组成一种特定图式；

S4）基于最小二乘法的ICP方法：

参阅图4；G1、G2表示从全局地图中提取的几何特征，l为局部地图中的有向线段，P_I ⁱ和P_l ^e为线段l的两端，C_l为l中心，则,用点法式直线方程来描述G2的函数为：

P·u_l=r_l (7)

其中P为G2上的一个点，u_l为法向量，r_l为实数；

线段l相对于旋转中心旋转并且平移（）后：

(8)

匹配错误为：

(9)

所有点的匹配错误为：

（10）

其中n_l为l上点的数量：

(11)

通过得到S的最小值，即可得到（）；

S5）利用扩展卡尔曼滤波预测方法（EKF），进一步提高小车定位的精度：

S5-1）设定小车初始位置(x,y,θ)和协方差P；

S5-2）通过动力学模型预测小车的下一时刻位置；

S5-3）通过更新协方差P：

(12)

(13)

(14)

(15)

(16)

(17)

S5-4)评估

S5-4-1）计算卡尔曼增益：

(18)

S5-4-2）更新位置坐标评估：

(19)

S5-4-3）更新协方差：

(20)

S5-4-4）增加新的标记点：

(21)

(22)

(23)

(24)

(25)

(26)。

进一步的，步骤S3）中，四对特征点需满足以下条件：

1）；

2）LA1=GA1,LA2=GA2；

3）由这四个点组成的图形在局部地图和全局地图中必须是独一无二的。

与现有技术相比，本实施例针对工业载运机器人的室内应用特点，以直线段替代传统的点作为建图特征，由激光雷达测量的环境或障碍物距离、角度，计算其世界坐标，并通过最小二乘做直线段近似，一方面消除了测量误差，同时降低了系统的状态维度，提高了运算速度；利用当前测量与已建图数据的关联性，并通过扩展卡尔曼滤波算法进一步精确估计机器人的运动状态，二次提高了系统的定位精度和可靠性。

以上所述的仅是本发明的一些实施方式。对于本领域的普通技术人员来说，在不脱离本发明创造构思的前提下，还可以做出若干变形和改进，这些都属于本发明的保护范围。

Claims (2)

1.一种基于直线段特征的载运机器人高精度定位导航方法，其特征在于：包括如下步骤：

S1）自主定位导航小车动力学模型：

(1)

(2)

(3)

(4)

(5)

下一时刻小车中心O’的位置采用二阶的Runge-Kutta迭代公式（公式6）得到：

(6)

其中， _L和 _R分别代表左轮和右轮的角速度，v_L和v_R分别代表左轮和右轮的线速度，v代表小车中心O’点的线速度， _E为小车的旋转角速度，r为小车轮子半径，L为两个轮子之间的距离， _G为陀螺仪测得的角速度；

S2）使用垂直距离方法提取线特征：

S2-1）选取一系列点，并将选取的第一个点和最后一个点用直线连接；

S2-2）计算其余点到这条直线的距离，取得到达直线距离最大的这个点；

S2-3）设置一个距离阈值，如果得到的最大距离超过这个距离阈值，则提取该点，设为P_L，P_SP_L和P_LP_E为提取的线特征，如果得到的最大距离小于距离阈值，则放弃提取的点，P_SP_E为提取的线特征；

S2-4）通过最小二乘法对点集进行拟合，最终得到线特征；

S3）利用几何匹配方法进行匹配：

在全局地图和局部地图中分别寻找四个点，并连成线段，组成一种特定图式；

S4）基于最小二乘法的ICP方法：

G1、G2表示从全局地图中提取的几何特征，l为局部地图中的有向线段，P_I ⁱ和P_l ^e为线段l的两端，C_l为l中心，则,用点法式直线方程来描述G2的函数为：

P·u_l=r_l (7)

其中P为G2上的一个点，u_l为法向量，r_l为实数；

线段l相对于旋转中心旋转并且平移（）后：

(8)

匹配错误为：

(9)

所有点的匹配错误为：

（10）

其中n_l为l上点的数量：

(11)

通过得到S的最小值，即可得到（）；

S5）利用扩展卡尔曼滤波预测方法（EKF），进一步提高小车定位的精度：

S5-1）设定小车初始位置(x,y,θ)和协方差P；

S5-2）通过动力学模型预测小车的下一时刻位置；

S5-3）通过更新协方差P：

(12)

(13)

(14)

(15)

(16)

(17)

S5-4)评估

S5-4-1）计算卡尔曼增益：

(18)

S5-4-2）更新位置坐标评估：

(19)

S5-4-3）更新协方差：

(20)

S5-4-4）增加新的标记点：

(21)

(22)

(23)

(24)

(25)

(26)。

2.根据权利要求1所述的一种基于直线段特征的载运机器人高精度定位导航方法，其特征在于：步骤S3）中，四对特征点需满足以下条件：

1）；

2）LA1=GA1,LA2=GA2；

3）由这四个点组成的图形在局部地图和全局地图中必须是独一无二的。