一种基于占据栅格地图且结合imu的移动机器人定位方法与流程

本发明涉及机器人自主导航技术领域，更具体地，涉及一种基于占据栅格地图且结合imu的移动机器人定位方法。

背景技术：

在现代的无人车中，定位是必不可少的一个技术部分。要实现自动驾驶，或者说自动导航，无人车需要获得精准的定位，才能够精准地导航。犹如人行走，必须知道自己的位置和方向，才能够知道要往哪里去。现有的定位技术有许多种，有单点gps定位，有差分gps定位，有激光雷达定位，还有用计算机视觉的方法来定位。

每种定位技术都有各自的优缺点，例如：单点gps依靠卫星的数量来衡量定位的质量；差分gps需要两个站来维持分米乃至厘米级的定位，往往来说，需要基站和移动站收发信号的稳定才能保证质量，而且信号的传输往往带来技术成本的增加；激光雷达定位，需要很大的运算量来计算点云匹配，而且定位效果往往不佳；计算机视觉的定位方法十分依赖摄像头的质量，还有光线的限制，等等。往往单靠一种技术是不够得到鲁棒性强的定位效果，需要融合多种传感器设备数据。

imu(inertialmeasurementunit)，是指惯性测量单元；slam(simultaneouslocalizationandmapping)，是指即时定位与地图构建。

技术实现要素：

本发明为克服上述现有技术中定位技术的缺陷，提供一种基于占据栅格地图且结合imu的移动机器人定位方法，能够在短时间获得精确的定位效果。

为解决上述技术问题，本发明采用的技术方案是：一种基于占据栅格地图且结合imu的移动机器人定位方法，包括位姿推算、特征提取、数据融合以及icp匹配四个部分，其中，

位恣推算基于imu所读出的三轴姿态角以及加速度，先通过滤波处理，然后去除零偏，积分得到航向角差；

特征提取基于激光雷达所读出的数据，将激光雷达所读出的一帧点云分成若干栅格，对每一格栅格中的点云求取质心，以质心点代表该栅格中的所有点云；

数据融合基于位恣推算得到的航向角差，将之转化为位姿变换矩阵t，以t为初值，将两帧栅格地图配准，建立非线性最小二乘模型，求解出最优姿态变换矩阵t1，再使用t1将两帧点云合并成一个micromap，如此重复，直至将五帧点云组成一个micromap；由于micromap每个格点都有一个概率值，设定阈值，即可过滤掉移动物体。

icp匹配基于数据融合提供的micromap，通过两个micromap，得到位姿变换矩阵，从而求得当前点世界坐标。

进一步的，具体包括以下步骤：

s1.读取imu的数据，对于角速度，只用到yaw角，读取出来的数据是一轴角速度，计算公式如下：

w＝w-wshift

其中，w为这一时刻所读入的角速度，wshift为零偏，wlast为上一时刻的角速度，计算航向角变化值所用的是龙格-库塔法；

s2.读取imu在x，y轴方向加速度的分量，计为据此可得x，y轴方向的位移量δx,δy，计算公式如下：

s3.根据步骤s1和步骤s2获得的航向角以及位移差，构建位姿变换矩阵t，公式如下：

s4.读取激光雷达数据，读取到点云，对hk使用分辨率为0.1cm×0.1cm的栅格地图分割，再对每个栅格求取质心，以质心处的点代替该栅格内所有点，计算公式如下：

其中，pk是某一栅格中的所有点；

s5.建立非线性最小二乘模型，以步骤二求得的位姿变换矩阵t位初始值，求得最优位恣变换矩阵t1，计算公式如下：

其中，m(k)代表栅格k处的micromap的odd值；

s6.根据最优变换矩阵，将hk重新投影到micromap所在的局部坐标系中，并根据如下公式重新计算odd值，并将该值保存为micromap栅格地图格点的odd值，回至步骤s3，直至此micromap中加入了五帧点云；

其中，z代表观测值，odd(s)代表栅格k处原本的odd值，p(z|s＝0)代表栅格处实际上没有障碍物z的概率，p(z|s＝1)代表栅格处实际上有障碍物z的概率，odd(s|z)代表z发生的条件下s的状态；

s7.将当前时刻的micromap与上一帧micromap进行icp配准，得到位姿变换矩阵t2，假设上一帧micromap所在点的世界坐标位为(x,y)，可根据以下公式直接求得当前点世界坐标：

其中，(xnow，ynow)为当前点的世界坐标。

进一步的，在求取当前点世界坐标，通过借助占据栅格地图以过滤掉移动物体。

与现有技术相比，有益效果是：本发明提供的一种基于占据栅格地图且结合imu的移动机器人定位方法，没有假设环境是静态的，而是通过占据栅格地图去消除动态物的影响，以期能建立静态地图，同时利用imu进行姿态推算，并不是简单的使用imu推算的位恣变换矩阵，而是建立非线性最小二乘法，结合imu推算的位恣以及栅格地图得到最优位恣变换矩阵；本发明利用占据栅格地图对动态物进行滤波，后融合imu数据得到一个micromap，以此进行计算，是一个鲁棒性好的slam方法，能够在一定时间内获得精度高的定位。

附图说明

图1是本发明总体框架示意图。

图2是本发明构建micromap的流程图。

图3是本发明处理imu数据的流程图。

图4是本发明数据融合的流程图。

具体实施方式

附图仅用于示例性说明，不能理解为对本发明的限制；为了更好说明本实施例，附图某些部件会有省略、放大或缩小，并不代表实际产品的尺寸；对于本领域技术人员来说，附图中某些公知结构及其说明可能省略是可以理解的。附图中描述位置关系仅用于示例性说明，不能理解为对本发明的限制。

如图1至4所示，一种基于占据栅格地图且结合imu的移动机器人定位方法，包括位姿推算、特征提取、数据融合以及icp匹配四个部分，其中，

位恣推算基于imu所读出的三轴姿态角以及加速度，先通过滤波处理，然后去除零偏，积分得到航向角差；

特征提取基于激光雷达所读出的数据，将激光雷达所读出的一帧点云分成若干栅格，对每一格栅格中的点云求取质心，以质心点代表该栅格中的所有点云；

数据融合基于位恣推算得到的航向角差，将之转化为位姿变换矩阵t，以t为初值，将两帧栅格地图配准，建立非线性最小二乘模型，求解出最优姿态变换矩阵t1，再使用t1将两帧点云合并成一个micromap，如此重复，直至将五帧点云组成一个micromap；由于micromap每个格点都有一个概率值，设定阈值，即可过滤掉移动物体。

icp匹配基于数据融合提供的micromap，通过两个micromap，得到位姿变换矩阵，从而求得当前点世界坐标。

进一步的，具体包括以下步骤：

s1.读取imu的数据，对于角速度，只用到yaw角，读取出来的数据是一轴角速度，计算公式如下：

w＝w-wshift

其中，w为这一时刻所读入的角速度，wshift为零偏，wlast为上一时刻的角速度，计算航向角变化值所用的是龙格-库塔法；

s2.读取imu在x，y轴方向加速度的分量，计为据此可得x，y轴方向的位移量δx,δy，计算公式如下：

s3.根据步骤s1和步骤s2获得的航向角以及位移差，构建位姿变换矩阵t，公式如下：

s4.读取激光雷达数据，读取到点云，对hk使用分辨率为0.1cm×0.1cm的栅格地图分割，再对每个栅格求取质心，以质心处的点代替该栅格内所有点，计算公式如下：

其中，pk是某一栅格中的所有点；

s5.建立非线性最小二乘模型，以步骤二求得的位姿变换矩阵t位初始值，求得最优位恣变换矩阵t1，计算公式如下：

其中，m(k)代表栅格k处的micromap的odd值；

s6.根据最优变换矩阵，将hk重新投影到micromap所在的局部坐标系中，并根据如下公式重新计算odd值，并将该值保存为micromap栅格地图格点的odd值，回至步骤s3，直至此micromap中加入了五帧点云；

其中，z代表观测值，odd(s)代表栅格k处原本的odd值，p(z|s＝0)代表栅格处实际上没有障碍物z的概率，p(z|s＝1)代表栅格处实际上有障碍物z的概率，odd(s|z)代表z发生的条件下s的状态；

前面步骤所求得变换矩阵都是局部的、相对的，需要与上一时刻的micromap配准，才能得到当前时刻的世界坐标。

s7.将当前时刻的micromap与上一帧micromap进行icp配准，得到位姿变换矩阵t2，假设上一帧micromap所在点的世界坐标位为(x,y)，可根据以下公式直接求得当前点世界坐标：

其中，(xnow，ynow)为当前点的世界坐标。

具体的，在求取当前点世界坐标，通过借助占据栅格地图以过滤掉移动物体。建立非线性最小二乘模型是关键一步，根据模型，由位恣推算得到变换矩阵和特征提取得到的占据栅格地图可求得最优变换矩阵。

显然，本发明的上述实施例仅仅是为清楚地说明本发明所作的举例，而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说，在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明权利要求的保护范围之内。