[发明专利]一种基于环境直线特征的定位方法

摘要

本发明公开了一种基于环境直线特征的定位方法，包括以下步骤：1）设置全局坐标系；2）创建环境特征地图；3）机器人从起点开始不断扫描环境信息；4）对扫描数据点进行滤波处理；5）将滤波后数据点分割为不同的类别；6）采用IEP算法对每一个类别进行直线特征识别；7）从直线特征的数据点集中拟出直线，计算特征参数；8）将直线特征从激光坐标系下变换到全局坐标系下，并利用激光扫描数据校正惯性导航的航位误差；9）将得到的直线特征添加到环境特征地图中；10）重复上述步骤，得到环境特征地图；11）机器人从起点开始运动，通过不断扫描到的环境特征与已创建的环境特征地图进行匹配以实现自定位。

主权项

一种基于环境直线特征的定位方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；其特征在于：它包括以下步骤：1）首先在新的未知环境下，设置起点位姿Z0，并以此为基准设置全局坐标系，即以起点位置为坐标系原点，以起点运动朝向为y轴正向，按右手法则确定x轴正向；2）控制机器人在环境中运动以自主创建环境特征地图；3）机器人从起点开始运动，机器人所载激光传感器不断扫描环境信息得到以激光传感器为原点的角度、距离极坐标点数据；机器人每运动一定距离就要处理一次激光扫描数据；惯性导航系统不断输出机器人当前的航位信息，利用该航位信息不断计算当前点与上一次扫描数据处理位置的距离delta_d，当该距离大于等于设定的阈值delta_dthreshod时则进行下面的激光扫描数据处理,并将delta_d清0，从当前点重新开始计算delta_d；4）对杂乱无规律的激光扫描数据点进行滤波处理，去除在有效计算距离范围之外的无用点；5）将滤波后数据点根据其分布的连续特性聚类处理，分割为不同的类别；计算相邻两激光扫描点距离nd_laser，若该距离小于等于设定阈值thr_cate则判定为同一类别，同一类别代表该点集来自障碍物的连续表面；同时设置合适的阈值thr_cate，将数据点较少的类别剔除以加快数据处理速度；6）采用IEP算法对每一个类别进行直线特征识别，在每个类别中找出构成直线特征的数据点集；设置合理的阈值thr_cate，将数据点较少的直线点集剔除；7）利用最小二乘准则从直线特征的数据点集中拟合出直线，并计算其特征参数；以机器人为中心坐标系与激光传感器为中心的坐标系重合，所述参数为在激光传感器坐标系下，直线到原点距离d、直线与坐标原点的垂线与极坐标轴夹角theta、直线端点坐标p1(x,y)，p2(x,y)和线性相关系数r；其中d，theta直线方程参数，利用线段端点p1，p2计算线段长度和直线匹配；利用线性系数r作为判别其直线特征相似程度，优先利用线性系数高的直线特征进行定位计算；8）将上述步骤中得到的直线特征从激光坐标系下变换到全局坐标系下，并利用激光扫描数据校正惯性导航的航位误差；以相同直线的两次激光扫描数据提取的特征参数为基准，反推计算坐标系变换关系，激光扫描器坐标系下特征直线位姿变化即为两次激光扫描间机器人在全局坐标系下位姿变化量，以此校正惯性导航输出的位姿状态；9）将上述步骤校正得到的直线特征添加到环境特征地图中，所述环境特征地图为记录特征直线参数的数据库，每条记录为一特征直线，记录的属性分别为直线的特征参数d，theta，p1，p2；10）使机器人在整个环境中运动重复上述步骤，以得到整个环境的特征地图；11）机器人从起点开始运动，通过不断扫描到的环境特征与已创建的环境特征地图进行匹配以实现自定位；机器人从起点开始运动，惯性导航输出当前位姿的粗略值，激光不断当前扫描当前环境，每隔一定距离按照上述特征提取过程得到特征直线变换到全局坐标系，在已创建地图中查找相对应的直线特征，判别地图中是否已有直线与当前特征直线相对应。