利索能及
我要发布
收藏
专利号: 2019107457129
申请人: 中山大学
专利类型:发明专利
专利状态:已下证
更新日期:2025-11-27
缴费截止日期: 暂无
联系人

摘要:

权利要求书:

1.一种基于占据栅格地图且结合imu的移动机器人定位方法,其特征在于,包括位姿推算、特征提取、数据融合以及icp匹配四个部分,其中,位姿推算基于imu所读出的三轴姿态角以及加速度,先通过滤波处理,然后去除零偏,积分得到航向角差;

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

数据融合基于位姿推算得到的航向角差和位移差,将之转化为位姿变换矩阵T,以T为初值,将两帧栅格地图配准,建立非线性最小二乘模型,求解出最优姿态变换矩阵T1,再使用T1将两帧点云合并成一个micromap,如此重复,直至将五帧点云组成一个micromap;

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

具体包括以下步骤:

S1.读取imu的数据,对于角速度,只用到yaw角,读取出来的数据是一轴角速度,计算公式如下:W=Wlast‑Wshift

其中,W为这一时刻所读入的角速度,Wshift为零偏,Wlast为上一时刻的角速度,计算航向角变化值所用的是龙格‑库塔法;

S2.读取imu在x,y轴方向加速度的分量,计为 据此可得x,y轴方向的位移量Δx,Δy,计算公式如下:S3.根据步骤S1和步骤S2获得的航向角差以及位移差,构建位姿变换矩阵T,公式如下:S4.读取激光雷达数据,读取到点云,对hk使用分辨率为0.1cm×0.1cm的栅格地图分割,再对每个栅格求取质心,以质心处的点代替该栅格内所有点,计算公式如下:其中,Pk是某一栅格中的所有点;

S5.建立非线性最小二乘模型,以步骤S3求得的位姿变换矩阵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)为当前点的世界坐标。

2.根据权利要求1所述的基于占据栅格地图且结合imu的移动机器人定位方法,其特征在于,在求取当前点世界坐标,通过借助占据栅格地图以过滤掉移动物体。