CN110361010B - 一种基于占据栅格地图且结合imu的移动机器人定位方法 - Google Patents
一种基于占据栅格地图且结合imu的移动机器人定位方法 Download PDFInfo
- Publication number
- CN110361010B CN110361010B CN201910745712.9A CN201910745712A CN110361010B CN 110361010 B CN110361010 B CN 110361010B CN 201910745712 A CN201910745712 A CN 201910745712A CN 110361010 B CN110361010 B CN 110361010B
- Authority
- CN
- China
- Prior art keywords
- grid
- micromap
- imu
- transformation matrix
- point
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
- G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
- G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
- G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
- G01C21/20—Instruments for performing navigational calculations
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及机器人自主导航技术领域,更具体地,涉及一种基于占据栅格地图且结合imu的移动机器人定位方法。包括位姿推算、特征提取、数据融合以及icp匹配四个部分;本发明通过占据栅格地图去消除动态物的影响,以期能建立静态地图,同时利用imu进行姿态推算,并不是简单的使用imu推算的位恣变换矩阵,而是建立非线性最小二乘法,结合imu推算的位恣以及栅格地图得到最优位恣变换矩阵;是一个鲁棒性好的slam方法,能够在一定时间内获得精度高的定位。
Description
技术领域
本发明涉及机器人自主导航技术领域,更具体地,涉及一种基于占据栅格地图且结合imu的移动机器人定位方法。
背景技术
在现代的无人车中,定位是必不可少的一个技术部分。要实现自动驾驶,或者说自动导航,无人车需要获得精准的定位,才能够精准地导航。犹如人行走,必须知道自己的位置和方向,才能够知道要往哪里去。现有的定位技术有许多种,有单点GPS定位,有差分GPS定位,有激光雷达定位,还有用计算机视觉的方法来定位。
每种定位技术都有各自的优缺点,例如:单点GPS依靠卫星的数量来衡量定位的质量;差分GPS需要两个站来维持分米乃至厘米级的定位,往往来说,需要基站和移动站收发信号的稳定才能保证质量,而且信号的传输往往带来技术成本的增加;激光雷达定位,需要很大的运算量来计算点云匹配,而且定位效果往往不佳;计算机视觉的定位方法十分依赖摄像头的质量,还有光线的限制,等等。往往单靠一种技术是不够得到鲁棒性强的定位效果,需要融合多种传感器设备数据。
IMU(Inertial measurement unit),是指惯性测量单元;slam(simultaneouslocalization and mapping),是指即时定位与地图构建。
发明内容
本发明为克服上述现有技术中定位技术的缺陷,提供一种基于占据栅格地图且结合imu的移动机器人定位方法,能够在短时间获得精确的定位效果。
为解决上述技术问题,本发明采用的技术方案是:一种基于占据栅格地图且结合imu的移动机器人定位方法,包括位姿推算、特征提取、数据融合以及icp匹配四个部分,其中,
位姿推算基于imu所读出的三轴姿态角以及加速度,先通过滤波处理,然后去除零偏,积分得到航向角差;
特征提取基于激光雷达所读出的数据,将激光雷达所读出的一帧点云分成若干栅格,对每一格栅格中的点云求取质心,以质心点代表该栅格中的所有点云;
数据融合基于位姿推算得到的航向角差和位移差,将之转化为位姿变换矩阵T,以T为初值,将两帧栅格地图配准,建立非线性最小二乘模型,求解出最优姿态变换矩阵T1,再使用T1将两帧点云合并成一个micromap,如此重复,直至将五帧点云组成一个micromap;由于micromap每个格点都有一个概率值,设定阈值,即可过滤掉移动物体。
icp匹配基于数据融合提供的micromap,通过两个micromap,得到位姿变换矩阵,从而求得当前点世界坐标。
进一步的,具体包括以下步骤:
S1.读取imu的数据,对于角速度,只用到yaw角,读取出来的数据是一轴角速度,计算公式如下:
W=Wlast-Wshift
其中,W为这一时刻所读入的角速度,Wshift为零偏,Wlast为上一时刻的角速度,计算航向角变化值所用的是龙格-库塔法;
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)为当前点的世界坐标。
进一步的,在求取当前点世界坐标,通过借助占据栅格地图以过滤掉移动物体。
与现有技术相比,有益效果是:本发明提供的一种基于占据栅格地图且结合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=Wlast-Wshift
其中,W为这一时刻所读入的角速度,Wshift为零偏,Wlast为上一时刻的角速度,计算航向角变化值所用的是龙格-库塔法;
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的状态;
前面步骤所求得变换矩阵都是局部的、相对的,需要与上一时刻的micromap配准,才能得到当前时刻的世界坐标。
S7.将当前时刻的micromap与上一帧micromap进行icp配准,得到位姿变换矩阵T2,假设上一帧micromap所在点的世界坐标位为(x,y),可根据以下公式直接求得当前点世界坐标:
其中,(xnow,ynow)为当前点的世界坐标。
具体的,在求取当前点世界坐标,通过借助占据栅格地图以过滤掉移动物体。建立非线性最小二乘模型是关键一步,根据模型,由位姿推算得到变换矩阵和特征提取得到的占据栅格地图可求得最优变换矩阵。
显然,本发明的上述实施例仅仅是为清楚地说明本发明所作的举例,而并非是对本发明的实施方式的限定。对于所属领域的普通技术人员来说,在上述说明的基础上还可以做出其它不同形式的变化或变动。这里无需也无法对所有的实施方式予以穷举。凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等,均应包含在本发明权利要求的保护范围之内。
Claims (2)
1.一种基于占据栅格地图且结合imu的移动机器人定位方法,其特征在于,包括位姿推算、特征提取、数据融合以及icp匹配四个部分,其中,
位姿推算基于imu所读出的三轴姿态角以及加速度,先通过滤波处理,然后去除零偏,积分得到航向角差;
特征提取基于激光雷达所读出的数据,将激光雷达所读出的一帧点云分成若干栅格,对每一格栅格中的点云求取质心,以质心点代表该栅格中的所有点云;
数据融合基于位姿推算得到的航向角差和位移差,将之转化为位姿变换矩阵T,以T为初值,将两帧栅格地图配准,建立非线性最小二乘模型,求解出最优姿态变换矩阵T1,再使用T1将两帧点云合并成一个micromap,如此重复,直至将五帧点云组成一个micromap;
icp匹配基于数据融合提供的micromap,通过两个micromap,得到位姿变换矩阵,从而求得当前点世界坐标;
具体包括以下步骤:
S1.读取imu的数据,对于角速度,只用到yaw角,读取出来的数据是一轴角速度,计算公式如下:
W=Wlast-Wshift
其中,W为这一时刻所读入的角速度,Wshift为零偏,Wlast为上一时刻的角速度,计算航向角变化值所用的是龙格-库塔法;
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的移动机器人定位方法,其特征在于,在求取当前点世界坐标,通过借助占据栅格地图以过滤掉移动物体。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910745712.9A CN110361010B (zh) | 2019-08-13 | 2019-08-13 | 一种基于占据栅格地图且结合imu的移动机器人定位方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910745712.9A CN110361010B (zh) | 2019-08-13 | 2019-08-13 | 一种基于占据栅格地图且结合imu的移动机器人定位方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110361010A CN110361010A (zh) | 2019-10-22 |
CN110361010B true CN110361010B (zh) | 2022-11-22 |
Family
ID=68224592
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910745712.9A Active CN110361010B (zh) | 2019-08-13 | 2019-08-13 | 一种基于占据栅格地图且结合imu的移动机器人定位方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110361010B (zh) |
Families Citing this family (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110596683B (zh) * | 2019-10-25 | 2021-03-26 | 中山大学 | 一种多组激光雷达外参标定系统及其方法 |
CN110910498B (zh) * | 2019-11-21 | 2021-07-02 | 大连理工大学 | 一种利用激光雷达和双目相机构建栅格地图的方法 |
CN110879400A (zh) * | 2019-11-27 | 2020-03-13 | 炬星科技(深圳)有限公司 | 激光雷达与imu融合定位的方法、设备及存储介质 |
CN110849363B (zh) * | 2019-12-03 | 2021-09-21 | 深圳市镭神智能系统有限公司 | 激光雷达与组合惯导的位姿标定方法、系统及介质 |
CN111142117B (zh) * | 2019-12-31 | 2021-11-05 | 芜湖哈特机器人产业技术研究院有限公司 | 融合折角板和占据栅格的混合导航地图构建方法及系统 |
CN111207753A (zh) * | 2020-02-13 | 2020-05-29 | 苏州大学 | 一种多玻璃隔断环境下的同时定位与建图的方法 |
CN111461982B (zh) * | 2020-03-30 | 2023-09-22 | 北京百度网讯科技有限公司 | 用于拼接点云的方法和装置 |
CN111457902B (zh) * | 2020-04-10 | 2021-10-29 | 东南大学 | 基于激光slam定位的水域测量方法及系统 |
CN112465970B (zh) * | 2020-11-27 | 2024-03-19 | 北京斯年智驾科技有限公司 | 导航地图构建方法、装置、系统、电子装置和存储介质 |
CN112729283A (zh) * | 2020-12-21 | 2021-04-30 | 西北工业大学 | 一种基于深度相机/mems惯导/里程计组合的导航方法 |
CN113093221A (zh) * | 2021-03-31 | 2021-07-09 | 东软睿驰汽车技术(沈阳)有限公司 | 占据栅格地图的生成方法及装置 |
Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105806344A (zh) * | 2016-05-17 | 2016-07-27 | 杭州申昊科技股份有限公司 | 一种基于局部地图拼接的栅格地图创建方法 |
CN105973265A (zh) * | 2016-05-19 | 2016-09-28 | 杭州申昊科技股份有限公司 | 一种基于激光扫描传感器的里程估计方法 |
CN107193012A (zh) * | 2017-05-05 | 2017-09-22 | 江苏大学 | 基于imm‑mht算法的智能车激光雷达机动多目标跟踪方法 |
CN107450577A (zh) * | 2017-07-25 | 2017-12-08 | 天津大学 | 基于多传感器的无人机智能感知系统和方法 |
CN108036797A (zh) * | 2017-11-30 | 2018-05-15 | 深圳市隐湖科技有限公司 | 基于四轮独立驱动且结合imu的里程推算方法 |
CN108908330A (zh) * | 2018-06-28 | 2018-11-30 | 中国人民解放军国防科技大学 | 一种基于虚拟现实的机器人行为控制方法 |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
Family Cites Families (2)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN106170676B (zh) * | 2015-07-14 | 2018-10-09 | 深圳市大疆创新科技有限公司 | 用于确定移动平台的移动的方法、设备以及系统 |
CN108230379B (zh) * | 2017-12-29 | 2020-12-04 | 百度在线网络技术(北京)有限公司 | 用于融合点云数据的方法和装置 |
-
2019
- 2019-08-13 CN CN201910745712.9A patent/CN110361010B/zh active Active
Patent Citations (7)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105806344A (zh) * | 2016-05-17 | 2016-07-27 | 杭州申昊科技股份有限公司 | 一种基于局部地图拼接的栅格地图创建方法 |
CN105973265A (zh) * | 2016-05-19 | 2016-09-28 | 杭州申昊科技股份有限公司 | 一种基于激光扫描传感器的里程估计方法 |
CN107193012A (zh) * | 2017-05-05 | 2017-09-22 | 江苏大学 | 基于imm‑mht算法的智能车激光雷达机动多目标跟踪方法 |
CN107450577A (zh) * | 2017-07-25 | 2017-12-08 | 天津大学 | 基于多传感器的无人机智能感知系统和方法 |
CN108036797A (zh) * | 2017-11-30 | 2018-05-15 | 深圳市隐湖科技有限公司 | 基于四轮独立驱动且结合imu的里程推算方法 |
CN108908330A (zh) * | 2018-06-28 | 2018-11-30 | 中国人民解放军国防科技大学 | 一种基于虚拟现实的机器人行为控制方法 |
CN109993113A (zh) * | 2019-03-29 | 2019-07-09 | 东北大学 | 一种基于rgb-d和imu信息融合的位姿估计方法 |
Non-Patent Citations (3)
Title |
---|
Vision-aided inertial navigation for resource constrained systems;Mingyang Li等;《2012 IEEE/RSJ International Conference on Intelligent Robots and Systems》;20121012;1057-1063 * |
基于ROS的室内移动机器人实时定位与地图创建研究;李敏;《中国优秀硕士学位论文全文数据库信息科技辑》;20190615(第6期);1-65 * |
基于激光雷达的二维即时定位与制图技术研究;李昊;《中国优秀硕士学位论文数据库信息科技辑》;20190515(第5期);1-80 * |
Also Published As
Publication number | Publication date |
---|---|
CN110361010A (zh) | 2019-10-22 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110361010B (zh) | 一种基于占据栅格地图且结合imu的移动机器人定位方法 | |
CN110446159B (zh) | 一种室内无人机精确定位与自主导航的系统及方法 | |
CN110243358B (zh) | 多源融合的无人车室内外定位方法及系统 | |
CN109341706B (zh) | 一种面向无人驾驶汽车的多特征融合地图的制作方法 | |
CN106017463B (zh) | 一种基于定位传感装置的飞行器定位方法 | |
CN111156998B (zh) | 一种基于rgb-d相机与imu信息融合的移动机器人定位方法 | |
CN112362051B (zh) | 一种基于信息融合的移动机器人导航定位系统 | |
WO2021026850A1 (zh) | 一种基于二维码的导航定姿定位方法和系统 | |
CN110411457B (zh) | 基于行程感知与视觉融合的定位方法、系统、终端和存储介质 | |
CN107478214A (zh) | 一种基于多传感器融合的室内定位方法及系统 | |
CN111380514A (zh) | 机器人位姿估计方法、装置、终端及计算机存储介质 | |
CN105424010A (zh) | 一种无人机视频地理空间信息注册方法 | |
CN110458885B (zh) | 基于行程感知与视觉融合的定位系统和移动终端 | |
CN111025366B (zh) | 基于ins及gnss的网格slam的导航系统及方法 | |
CN115272596A (zh) | 一种面向单调无纹理大场景的多传感器融合slam方法 | |
CN115371665B (zh) | 一种基于深度相机和惯性融合的移动机器人定位方法 | |
CN112254729A (zh) | 一种基于多传感器融合的移动机器人定位方法 | |
CN111504323A (zh) | 基于异源图像匹配与惯性导航融合的无人机自主定位方法 | |
CN110533719A (zh) | 基于环境视觉特征点识别技术的增强现实定位方法及装置 | |
CN113296133B (zh) | 一种基于双目视觉测量与高精度定位融合技术实现位置标定的装置及方法 | |
CN110749308B (zh) | 使用消费级gps和2.5d建筑物模型的面向slam的室外定位方法 | |
CN109143303B (zh) | 飞行定位方法、装置及固定翼无人机 | |
Fang et al. | A motion tracking method by combining the IMU and camera in mobile devices | |
CN115930948A (zh) | 一种果园机器人融合定位方法 | |
CN114529585A (zh) | 基于深度视觉和惯性测量的移动设备自主定位方法 |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
PB01 | Publication | ||
PB01 | Publication | ||
SE01 | Entry into force of request for substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |