[发明专利]基于点特征投影与激光点云融合的机器人位姿判断方法

摘要

本发明涉及一种基于点特征投影与激光点云融合的机器人位姿判断法。利用ORB算法来提取图像特征点，将特征点坐标投影到二维平面，与激光雷达点云信息进行融合，得到周围环境信息，与现有地图匹配，来对机器人的位姿进行判断，得到机器人的准确位姿。与现有算法相比，可以有效提高点云信息与地图的匹配度，提高机器人的定位精度，稳定性更好。

主权项

1.一种基于点特征投影与激光点云融合的机器人位姿判断法，其特征在于，包括以下步骤：/n步骤1、在运行环境的关键位置放置k个不同的标志物，利用双目摄像机分别对每个标志物进行特征点提取，获得k个特征点，每个特征点作为一个单词，由k个单词组成词典；/n步骤2、获得样本粒子；/n步骤3、对机器人进行定位，以上一步得到的一定数量的粒子在粒子滤波预测阶段估计机器人的位姿，利用里程计运动模型，根据状态转移函数对机器人的位姿进行状态转移变换，得到机器人位姿的建议分布，式中，表示t时刻粒子i的位姿状况，u_t-1表示t-1时刻输入的里程计数据；/n步骤4、机器人在运行环境中运行过程中，利用双目摄像机对周围环境进行扫描时对周围环境进行特征点提取，根据双目摄像机的内外参数，将特征点p_i的像素坐标转化为相机坐标系下的坐标，设为(x_i,y_i,z_i)；/n步骤5、将步骤4提取到的特征点p_i与步骤1建立的词典中的单词进行匹配，若匹配成功，则进入步骤6，否则进入步骤8；/n步骤6、将特征点p_i的三位空间坐标(x_i,y_i,z_i)投影到二维平面，则特征点p_i的坐标为(x_i,y_i)，在相同时刻，激光雷达收集的点云信息为M，设M_j(x_j,y_j)为点云信息为M中的一点，设激光雷达原点到M_j(x_j,y_j)的直线为L，若特征点p_i到直线L的距离小于阈值一，则表明特征点p_i落于该直线L上，进入步骤7，否则进入步骤8；/n步骤7、若特征点p_i和M_j(x_j,y_j)的欧式距离小于阈值二，则表明特征点p_i和M_j(x_j,y_j)为同一点，则视觉信息和激光雷达信息融合成功，获得t时刻的融合观测信息，将该融合观测信息作为t时刻的观测信息z_t，进入步骤9，若特征点p_i和M_j(x_j,y_j)的欧式距离不小于阈值二，进入步骤8；/n步骤8、将激光雷达收集的点云信息M作为t时刻的观测信息z_t，进入步骤9；/n步骤9、在粒子滤波重采样阶段输入上一步得到的观测信息z_t，计算当有一次迭代时在粒子滤波预测阶段所使用的粒子的权重，利用权重来判断每一个粒子的好坏，保留权重大的粒子，舍弃权重小的粒子，返回步骤3进行下一次的迭代，直至达到设定的迭代次数之后，得到机器人的正确位姿。/n