一种惯性导航系统的误差校正方法
【专利摘要】本发明公开了一种惯性导航系统的误差校正方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；包括以下步骤：1）设置全局坐标系；2）机器人从起点开始不断扫描环境信息；3）对扫描数据点进行滤波处理；4）将滤波后数据点分割为不同的类别；5）采用IEP算法对每一个类别进行直线特征识别；6）利用最小二乘准则从直线特征的数据点集中拟合出直线，并计算其特征参数；7）从上述步骤中得到的激光扫描数据中提取两组直线特征参数；8）将两组直线特征参数变换到全局坐标系下，确定两直线是否来自同一物体；9）上述步骤中L1，L2为同一直线时，利用机器人的位姿变化量来校正位姿。
【专利说明】一种惯性导航系统的误差校正方法
【技术领域】
[0001]本发明属于移动机器人定位【技术领域】，特别涉及一种惯性导航系统的误差校正方法。
【背景技术】
[0002]在移动机器人的应用中，导航定位是机器人系统的关键功能。导航是指移动机器人通过传感器获得环境信息和自身位姿状态，实现在障碍物的环境中自主运动到目的地。导航的关键和前提是定位，定位是指实时确定机器人自身在环境中的位置和姿态。
[0003]目前较为常见的机器人定位技术主要是根据先验的环境信息，结合当前机器人位置信息和传感器获取的信息确定机器人位姿状态。定位技术主要分为绝对定位和相对定位，相对定位通过计算相对于初始位姿的位姿变化来确定当前的位姿状态，常见的有里程计、惯性导航系统。里程计、惯性导航系统等长时间或大尺度下累积误差较大，定位精度较差，无法准确、省力地为机器人提供位姿信息。
【发明内容】
[0004]本发明的目的是为了解决上述问题，提供一种能够对惯性导航系统的航位误差进行校正，减少误差的误差校正方法。
[0005]为此，本发明的技术方案是:一种惯性导航系统的误差校正方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；其特征在于:它包括以下步骤:
1)首先在新的未知环境下，设置起点位姿Z0，并以此为基准设置全局坐标系，即以起点位置为坐标系原点，以起点运动朝向为I轴正向，按右手法则确定X轴正向；
2)机器人从起点开始运动，机器人所载激光传感器不断扫描环境信息得到以激光传感器为原点的角度、距离极坐标点数据；机器人每运动一定距离就要处理一次激光扫描数据；惯性导航系统不断输出机器人当前的航位信息；
3)对杂乱无规律的激光扫描数据点进行滤波处理，去除在有效计算距离范围之外的无用点；
4)将滤波后数据点根据其分布的连续特性聚类处理，分割为不同的类别；方法为计算相邻两激光扫描点距离nd_laser，若该距离小于等于设定阈值thr_cate则判定为同一类另IJ，同一类别代表该点集来自障碍物的连续表面；同时设置合适的阈值thr_cate，将数据点较少的类别剔除以加快数据处理速度；
5)采用IEP算法对每一个类别进行直线特征识别，在每个类别中找出构成直线特征的数据点集；设置合理的阈值thr_cate，将数据点较少的直线点集剔除；
6)利用最小二乘准则从直线特征的数据点集中拟合出直线，并计算其特征参数；
以机器人为中心坐标系与激光传感器为中心的坐标系重合，所述参数为在激光传感器坐标系下，直线到原点距离d、直线与坐标原点的垂线与极坐标轴夹角theta、直线端点坐标pi (x, y)，p2(x, y)和线性相关系数r ;其中d，theta直线方程参数，利用线段端点pl，p2计算线段长度和直线匹配；利用线性系数r作为判别其直线特征相似程度，优先利用线性系数高的直线特征进行定位计算；
7)从上述步骤中得到的激光扫描数据中提取两组直线特征参数；提取激光扫描数据中Tl时刻的某一直线参数LI (rl，thetal)，此时刻机器人位姿坐标为Zl (XI，Yl，THETAI);提取激光扫描数据中T2时刻的某一直线参数L2(r2，theta2)，此时刻机器人位姿为Z2(X1，Yl，THETAI)；
8)将步骤7)中提取的两组直线特征参数变换到全局坐标系下，根据判别标准确定两直线是否来自同一物体；
采用的判别标准为:两直线距离差绝对值小于设定阈值lthr_d，且两直线角度差绝对值小于设定阈值lthr_theta，且两直线对应端点距离小于设定阈值lthr_pd ；
9)上述步骤中LI，L2为同一直线时，则Tl时刻到T2时刻机器人位姿变化量为deltaZ，利用该位姿变化量来校正位姿Z2=Z I+de I taZ。
[0006]本发明以惯性导航的输出位姿为基础，利用激光扫描获取自然环境中的直线特征，通过以相同直线的两次激光扫描数据提取的特征参数为基准，反推计算坐标系变换关系，激光扫描器坐标系下特征直线位姿变化即为两次激光扫描间机器人在全局坐标系下位姿变化量，以此校正惯性导航误差，创建高精度的环境特征地图。
【专利附图】
【附图说明】
[0007]以下结合附图和本发明的实施方式来做进一步详细说明。
[0008]图1本发明的直线特征提取流程图。
[0009]图2本发明的误差校正流程图。
【具体实施方式】
[0010]参见附图。本实施例在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；它包括以下步骤:
1)首先在新的未知环境下，设置起点位姿Z0，并以此为基准设置全局坐标系，即以起点位置为坐标系原点，以起点运动朝向为I轴正向，按右手法则确定X轴正向；
2)机器人从起点开始运动，机器人所载激光传感器不断扫描环境信息得到以激光传感器为原点的角度、距离极坐标点数据；机器人每运动一定距离就要处理一次激光扫描数据；
惯性导航系统不断输出机器人当前的航位信息，利用该航位信息不断计算当前点与上一次扫描数据处理位置的距离delta_d，当该距离大于等于设定的阈值delta_dthreshod时则进行下面的激光扫描数据处理，并将delta_d清0，从当前点重新开始计算delta_d ；
3)如图1所示从激光扫描数据中提取直线特征过程:对杂乱无规律的激光扫描数据点进行滤波处理，去除在有效计算距离范围之外的无用点；
4)将滤波后数据点根据其分布的连续特性聚类处理，分割为不同的类别；方法为计算相邻两激光扫描点距离nd_laser，若该距离小于等于设定阈值thr_cate则判定为同一类另IJ，同一类别代表该点集来自障碍物的连续表面；同时设置合适的阈值thr_cate，将数据点较少的类别剔除以加快数据处理速度；
5)采用IEP算法对每一个类别进行直线特征识别，在每个类别中找出构成直线特征的数据点集；设置合理的阈值thr_cate，将数据点较少的直线点集剔除；
6)利用最小二乘准则从直线特征的数据点集中拟合出直线，并计算其特征参数； 假设以机器人为中心坐标系与激光传感器为中心的坐标系重合，所述参数为在激光传感器坐标系下，直线到原点距离d、直线与坐标原点的垂线与极坐标轴夹角theta、直线端点坐标pi (X，y)，p2(x，y)和线性相关系数r ;其中d、theta直线方程参数，利用线段端点pl，p2计算线段长度和直线匹配；利用线性系数!■作为判别其直线特征相似程度，优先利用线性系数高的直线特征进行定位计算；
7)从上述步骤中得到的激光扫描数据中提取两组直线特征参数；提取激光扫描数据中Tl时刻的某一直线参数LI (rl, thetal)，此时刻机器人位姿坐标为Zl (XI，Yl，THETAI);提取激光扫描数据中T2时刻的某一直线参数L2(r2，theta2)，此时刻机器人位姿为Z2(X1，Yl，THETAI)；
8)将步骤7)中提取的两组直线特征参数变换到全局坐标系下，根据判别标准确定两直线是否来自同一物体；
采用的判别标准为:两直线距离差绝对值小于设定阈值lthr_d，且两直线角度差绝对值小于设定阈值lthr_theta，且两直线对应端点距离小于设定阈值lthr_pd ；
9)上述步骤中LI，L2为同一直线时，则Tl时刻到T2时刻机器人位姿变化量为deltaZ，利用该位姿变化量来校正位姿Z2=Z I+de I taZ。
[0011]如图2所示为误差校正过程，其中LI表示提取的特征直线在激光传感器坐标系下参数，{LI，...}表示特征直线集，{LI’，...}表示将直线集{LI，...}变换到全局坐标系下，{ (LI’’，L2’’)，...}表示由{LI，...}到{L2，...}的对应直线对映射集；
在两次激光扫描中相同的特征直线变换全局坐标系下其特征参数会存在差异，主要原因为惯性导航导致的当前位姿存在误差，即坐标变换关系存在误差，因而导致环境中相同直线在全局坐标下有不同参数；因而以相同直线的两次激光扫描数据提取的特征参数为基准，反推计算坐标系变换关系，激光扫描器坐标系下特征直线位姿变化即为两次激光扫描间机器人在全局坐标系下位姿变化量，以此校正惯性导航输出的位姿状态；其中采用最近邻法判别两次不同扫描数据中是否为同一直线。
【权利要求】
1.一种惯性导航系统的误差校正方法，在至少具有运动控制系统、惯性导航系统、激光扫描传感器的机器人上使用；其特征在于:它包括以下步骤: 1)首先在新的未知环境下，设置起点位姿Z0，并以此为基准设置全局坐标系，即以起点位置为坐标系原点，以起点运动朝向为I轴正向，按右手法则确定X轴正向； 2)机器人从起点开始运动，机器人所载激光传感器不断扫描环境信息得到以激光传感器为原点的角度、距离极坐标点数据；机器人每运动一定距离就要处理一次激光扫描数据；惯性导航系统不断输出机器人当前的航位信息； 3)对杂乱无规律的激光扫描数据点进行滤波处理，去除在有效计算距离范围之外的无用点； 4)将滤波后数据点根据其分布的连续特性聚类处理，分割为不同的类别；方法为计算相邻两激光扫描点距离nd_laser，若该距离小于等于设定阈值thr_cate则判定为同一类另IJ，同一类别代表该点集来自障碍物的连续表面；同时设置合适的阈值thr_cate，将数据点较少的类别剔除以加快数据处理速度； 5)采用IEP算法对每一个类别进行直线特征识别，在每个类别中找出构成直线特征的数据点集；设置合理的阈值thr_cate，将数据点较少的直线点集剔除； 6)利用最小二乘准则从直线特征的数据点集中拟合出直线，并计算其特征参数； 以机器人为中心坐标系与激光传感器为中心的坐标系重合，所述参数为在激光传感器坐标系下，直线到原点距离d、直线与坐标原点的垂线与极坐标轴夹角theta、直线端点坐标pi (X，y)，p2(x, y)和线性相关系数r ;其中d，theta直线方程参数，利用线段端点pl，p2计算线段长度和直线匹配；利用线性系数r作为判别其直线特征相似程度，优先利用线性系数高的直线特征进行定位计算； 7)从上述步骤中得到的激光扫描数据中提取两组直线特征参数；提取激光扫描数据中Tl时刻的某一直线参数LI (rl，thetal)，此时刻机器人位姿坐标为Zl (XI，Yl，THETAI);提取激光扫描数据中T2时刻的某一直线参数L2(r2，theta2)，此时刻机器人位姿为Z2(X1，Yl，THETAI)； 8)将步骤7)中提取的两组直线特征参数变换到全局坐标系下，根据判别标准确定两直线是否来自同一物体； 采用的判别标准为:两直线距离差绝对值小于设定阈值lthr_d，且两直线角度差绝对值小于设定阈值lthr_theta，且两直线对应端点距离小于设定阈值lthr_pd ； 9)上述步骤中LI，L2为同一直线时，则Tl时刻到T2时刻机器人位姿变化量为deltaZ，利用该位姿变化量来校正位姿Z2=Z I+de I taZ。
【文档编号】G01C25/00GK104501829SQ201410677883
【公开日】2015年4月8日 申请日期:2014年11月24日 优先权日:2014年11月24日
【发明者】陈如申, 黎勇跃, 王海瑞, 马丽艳 申请人:杭州申昊科技股份有限公司