CN109765569B - 一种基于激光雷达实现虚拟航迹推算传感器的方法 - Google Patents
一种基于激光雷达实现虚拟航迹推算传感器的方法 Download PDFInfo
- Publication number
- CN109765569B CN109765569B CN201711095018.4A CN201711095018A CN109765569B CN 109765569 B CN109765569 B CN 109765569B CN 201711095018 A CN201711095018 A CN 201711095018A CN 109765569 B CN109765569 B CN 109765569B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- laser radar
- theta
- max
- angle
- 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.)
- Expired - Fee Related
Links
- 238000000034 method Methods 0.000 title claims abstract description 27
- 239000002245 particle Substances 0.000 claims abstract description 19
- 238000001914 filtration Methods 0.000 claims abstract description 4
- 238000005259 measurement Methods 0.000 claims description 9
- 238000009827 uniform distribution Methods 0.000 claims description 6
- 230000014509 gene expression Effects 0.000 claims description 3
- 230000007547 defect Effects 0.000 description 1
- 238000005305 interferometry Methods 0.000 description 1
- 238000004088 simulation Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
本发明涉及一种基于激光雷达实现虚拟航迹推算传感器的方法,其核心是根据时间上相邻的两次激光雷达扫描数据的配准来计算移动机器人在该时间段的位姿变化;首先利用粒子滤波方法对移动机器人的位姿变化进行初始估计,在此基础上利用迭代最近点方法对移动机器人位姿变化进行精确估计,最后根据要求位姿变化计算出移动机器人在该时间段内的线速度和角速度。
Description
技术领域
本发明涉及一种基于激光雷达实现虚拟航迹推算传感器的方法,属于移动机器人技术领域。
背景技术
航迹推算传感器是移动机器人导航控制的重要传感器,为移动机器人提供线速度和角速度的量测。一方面,由于一般的编码器和陀螺仪在测量机器人线速度和角速度时通常存在较大的误差;另一方面,有的机器人没有配备编码器或者陀螺仪。激光雷达是一种高精度的传感器,利用激光雷达仿真实现高精度的虚拟航迹推算传感器对于提高移动机器人高精度导航控制具有重要意义。
现有方法中,文献(参见“Mariano Jaimez,Javier G.Monroy,Javier Gonzalez-Jimenez:《Planar Odometry from a Radial Laser Scanner.A Range Flow-basedApproach》,IEEE International Conference on Robotics and Automation(ICRA),2016”))提出了一种基于激光雷达实现航迹推算的方法,该方法基于扫描地图梯度进行密度扫描配准,该方法的主要缺陷在于对于动态环境精度较低。
发明内容
本发明的目的在于提出一种激光雷达实现虚拟航迹推算传感器的方法,其核心是根据时间上相邻的两次激光雷达扫描数据的配准来计算移动机器人在该时间段的位姿变化;首先利用粒子滤波方法对移动机器人的位姿变化进行初始估计,在此基础上利用迭代最近点方法对移动机器人位姿变化进行精确估计,最后根据精确估计的位姿变化计算出移动机器人在该时间段的线速度和角速度;
为了实现上述目的,本发明的技术方案如下:
输入:当前时刻激光雷达扫描Rc,前一时刻激光雷达扫描Rp,时间间隔τ;其中 表示Rc的第j条射线,表示射线测量的距离,表示射线的角度;表示Rp的第j条射线,表示射线测量的距离,表示射线的角度;width表示激光雷达扫描射线数;
输出:移动机器人线速度ν,移动机器人角速度ω;
步骤1、设置参数:粒子数目N,激光雷达扫描角度分辨率angle_inc,,激光雷达扫描第一条射线的角度angle_min,移动机器人最大线速度v_max,移动机器人最大角速度omega_max,高似然射线分位数阈值η;迭代最近点方法误差阈值ε;
步骤2、循环变量i从1至N循环,执行步骤3至步骤10
步骤3、按均匀分布产生一个-v_max到+v_max之间的随机数,并把该随机数赋值给lv[i];按均匀分布产生一个-omega_max到+omega_max之间的随机数,并把该随机数赋值给av[i];其中lv[i]表示第i个粒子的线速度,av[i]表示第i个粒子的角速度;
步骤4、根据式(1)计算x[i],y[i],theta[i],其中(x[i],y[i],theta[i])为第i个粒子,表示移动机器人的位姿:
步骤5:j从1至width循环执行步骤6
步骤6:
xcj=cos(theta[i])*xpj-sin(theta[i])*ypj+x[i]; (4)
ycj=sin(theta[i])*xpj+cos(theta[i])*ypj+y[i] (5)
dista=sqrt(xcj*xcj+ycj*ycj) (6)
anglej=atan2(ycj,xcj) (7)
index=(anglej-angle_min)/angle_inc+1 (8)
if(index<=0||index>width-1)wall[j]=1..0e-99;
else{
wall[j]=exp(-0.5*(distb-dista)*(distb-dista));
} (9)
其中xpj、ypj、xcj、ycj、dista、anglej、index、wall、distb为临时变量,其含义由式(2)至式(9)确定;
步骤7:
将wall的η分位数赋值给hlray;w[i]=1;j从1至width循环执行步骤8;其中hlray为临时变量,表示wall的η分位数;
步骤8:
if(wall[j]>=hlray)w[i]=w[i]*wall[j];
其中,w[i]表示第i个粒子的权重
步骤9:估计机器人位姿(bestx,besty,bestheta):
if(i==1){bestx=x[1];besty=y[1];bestheta=theta[1],bestw=w[1]}
if(i>1&&w[i]>bestw){
bestx=x[i];besty=y[i];bestheta=theta[i],bestw=w[i]
}
步骤10:
以(bestx,besty,bestheta)为初始位置估计,利用迭代最近点方法根据Rc和Rp计算移动机器人在该时间段的位姿估计(bestxicp,bestyicp,besthetaicp),
(bestxicp,bestyicp,besthetaicp)=icp((bestx,besty,bestheta),Rp,Rc,ε)
步骤11:
根据(bestxicp,bestyicp,besthetaicp)估计移动机器人在该时间段的线速度和角速度:
v=bestxicp/τ
ω=besthetaicp/τ。
具体实现方式:
根据时间上相邻的两次激光雷达扫描数据的配准来计算移动机器人在该时间段的位姿变化;首先利用粒子滤波方法对移动机器人的位姿变化进行初始估计,在此基础上利用迭代最近点方法对移动机器人位姿变化进行精确估计,最后根据精确估计的位姿变化计算出移动机器人在该时间段的线速度和角速度;下面加以具体说明;
输入:当前时刻激光雷达扫描Rc,前一时刻激光雷达扫描Rp,时间间隔τ;其中 表示Rc的第j条射线,表示射线测量的距离,表示射线的角度;表示Rp的第j条射线,表示射线则量的距离,表示射线的角度;width表示激光雷达扫描射线数;
输出:移动机器人线速度v,移动机器人角速度ω;
步骤1、设置参数:粒子数目N,激光雷达扫描角度分辨率angle_inc,激光雷达扫描第一条射线的角度angle_min,移动机器人最大线速度v_max,移动机器人最大角速度omega_max,高似然射线分位数阈值η;迭代最近点方法误差阈值ε;
步骤2、循环变量i从1至N循环,执行步骤3至步骤10
步骤3、按均匀分布产生一个-v_max到+v_max之间的随机数,并把该随机数赋值给lv[i];按均匀分布产生一个-omega_max到+omega_max之间的随机数,并把该随机数赋值给av[i];其中lv[i]表示第i个粒子的线速度,av[i]表示第i个粒子的角速度;
步骤4、根据式(1)计算x[i],y[i],theta[i],其中(x[i],y[i],theta[i])为第i个粒子,表示移动机器人的位姿:
步骤5:j从1至width循环执行步骤6
步骤6:
xcj=cos(theta[i])*xpj-sin(theta[i])*ypj+x[i]; (4)
ycj=sin(theta[i])*xpj+cos(theta[i])*ypj+y[i] (5)
dista=sqrt(xcj*xcj+ycj*ycj) (6)
anglej=atan2(ycj,xcj) (7)
index=(anglej-angle_min)/angle_inc+1 (8)
if(index<=0||index>width-1)wall[j]=1.0e-99;
else{
wall[j]=exp(-0.5*(distb-dista)*(distb-dista));
} (9)
其中xpj、ypj、xcj、ycj、dista、anglej、index、wall、distb为临时变量,其含义由式(2)至式(9)确定;
步骤7:
将wall的η分位数赋值给hlray;w[i]=1;j从1至width循环执行步骤8;其中hlray为临时变量,表示wall的η分位数;
步骤8:
if(wall[j]>=hlray)w[i]=w[i]*wall[j];
其中,w[i]表示第i个粒子的权重
步骤9:估计机器人位姿(bestx,besty,bestheta):
if(i==1){bestx=x[1];besty=y[1];bestheta=theta[1],bestw=w[1]}
if(i>1&&w[i]>bestw){
bestx=x[i];besty=y[i];bestheta=theta[i],bestw=w[i]
}
步骤10:
以(bestx,besty,bestheta)为初始位置估计,利用迭代最近点方法根据Rc和Rp计算移动机器人在该时间段的位姿估计(bestxicp,bestyicp,besthetaicp),
(bestxicp,bestyicp,besthetaicp)=icp((bestx,besty,bestheta),Rp,Rc,ε)
步骤11:
根据(bestxicp,bestyicp,besthetaicp)估计移动机器人在该时间段的线速度和角速度:
v=bestxicp/τ
ω=besthetaicp/τ
以上是本发明的一个具体实施案例,需要说明的是,任何二维激光雷达,都可以利用上述方法实现虚拟的航迹推算传感器。
Claims (1)
1.一种激光雷达实现虚拟航迹推算传感器的方法,其核心是根据时间上相邻的两次激光雷达扫描数据的配准来计算移动机器人在该时间段的位姿变化;首先利用粒子滤波方法对移动机器人的位姿变化进行初始估计,在此基础上利用迭代最近点方法对移动机器人位姿变化进行精确估计,最后根据精确估计的位姿变化计算出移动机器人在该时间段的线速度和角速度;具体实现步骤如下:
输入:当前时刻激光雷达扫描Rc,前一时刻激光雷达扫描Rp,时间间隔τ;其中 表示Rc的第j条射线,表示射线则量的距离,表示射线的角度;表示Rp的第j条射线,表示射线测量的距离,表示射线的角度;width表示激光雷达扫描射线数;
输出:移动机器人线速度v,移动机器人角速度ω;
步骤1、设置参数:粒子数目N,激光雷达扫描角度分辨率angle_inc,激光雷达扫描第一条射线的角度angle_min,移动机器人最大线速度v_max,移动机器人最大角速度omega_max,高似然射线分位数阈值η;迭代最近点方法误差阈值ε;
步骤2、循环变量i从1至N循环,执行步骤3至步骤10
步骤3、按均匀分布产生一个-v_max到+v_max之间的随机数,并把该随机数赋值给lv[i];按均匀分布产生一个-omega_max到+omega_max之间的随机数,并把该随机数赋值给av[i];
其中lv[i]表示第i个粒子的线速度,av[i]表示第i个粒子的角速度;
步骤4、根据式(1)计算x[i],y[i],theta[i],其中(x[i],y[i],theta[i])为第i个粒子,表示移动机器人的位姿:
步骤5:j从1至width循环执行步骤6
步骤6:
xcj=cos(theta[i])*xpj-sin(theta[i])*ypj+x[i]; (4)
ycj=sin(theta[i])*xpj+cos(theta[i])*ypj+y[i] (5)
dista=sqrt(xcj*xcj+ycj*ycj) (6)
anglej=atan2(ycj,xcj) (7)
index=(anglej-angle_min)/angle_inc+1 (8)
其中xpj、ypj、xcj、ycj、dista、anglej、index、wall、distb为临时变量,其含义由式(2)至式(9)确定;
步骤7:
将wall的η分位数赋值给hlray;w[i]=1;j从1至width循环执行步骤8;其中hlray为临时变量,表示wall的η分位数;
步骤8:
if(wall[j]>=hlray)w[i]=w[i]*wall[j];
其中,w[i]表示第i个粒子的权重
步骤9:估计机器人位姿(bestx,besty,bestheta):
if(i==1){bestx=x[1];besty=y[1];bestheta=theta[1],bestw=w[1]}
if(i>1&&w[i]>bestw){
bestx=x[i];besty=y[i];bestheta=theta[i],bestw=w[i]
}
步骤10:
以(bestx,besty,bestheta)为初始位置估计,利用迭代最近点方法根据Rc和Rp计算移动机器人在该时间段的位姿估计(bestxicp,bestyicp,besthetaicp),
(bestxicp,bestyicp,besthetaicp)=icp((bestx,besty,bestheta),Rp,Rc,ε)
步骤11:
根据(bestxicp,bestyicp,besthetaicp)估计移动机器人在该时间段的线速度和角速度:
v=bestxicp/τ
ω=besthetaicp/τ。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711095018.4A CN109765569B (zh) | 2017-11-09 | 2017-11-09 | 一种基于激光雷达实现虚拟航迹推算传感器的方法 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201711095018.4A CN109765569B (zh) | 2017-11-09 | 2017-11-09 | 一种基于激光雷达实现虚拟航迹推算传感器的方法 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN109765569A CN109765569A (zh) | 2019-05-17 |
CN109765569B true CN109765569B (zh) | 2022-05-17 |
Family
ID=66449126
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201711095018.4A Expired - Fee Related CN109765569B (zh) | 2017-11-09 | 2017-11-09 | 一种基于激光雷达实现虚拟航迹推算传感器的方法 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN109765569B (zh) |
Families Citing this family (3)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110333526B (zh) * | 2019-05-22 | 2021-11-02 | 电子科技大学中山学院 | 一种基于gps的仿真航迹推算传感器方法 |
CN110634161B (zh) * | 2019-08-30 | 2023-05-05 | 哈尔滨工业大学(深圳) | 一种基于点云数据的工件位姿快速高精度估算方法及装置 |
CN112965082B (zh) * | 2021-02-26 | 2024-01-12 | 浙江大学 | 一种基于激光雷达的自适应沿墙导航方法 |
Family Cites Families (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
DE102013102153A1 (de) * | 2012-03-15 | 2013-09-19 | GM Global Technology Operations LLC | Verfahren zur Registrierung von Entfernungsbildern von mehreren LiDAR-Sensoren |
CN103344963B (zh) * | 2013-07-12 | 2016-05-18 | 电子科技大学中山学院 | 一种基于激光雷达的鲁棒航迹推算方法 |
CN103487047B (zh) * | 2013-08-06 | 2016-05-11 | 重庆邮电大学 | 一种基于改进粒子滤波的移动机器人定位方法 |
US9491585B2 (en) * | 2014-05-31 | 2016-11-08 | Apple Inc. | Location determination using dual statistical filters |
CN104166989B (zh) * | 2014-07-04 | 2017-02-15 | 电子科技大学中山学院 | 一种用于二维激光雷达点云匹配的快速icp方法 |
CN105865449B (zh) * | 2016-04-01 | 2020-05-05 | 深圳市杉川机器人有限公司 | 基于激光和视觉的移动机器人的混合定位方法 |
CN105867373B (zh) * | 2016-04-07 | 2018-09-11 | 重庆大学 | 一种基于激光雷达数据的移动机器人位姿推算方法及*** |
CN107246873A (zh) * | 2017-07-03 | 2017-10-13 | 哈尔滨工程大学 | 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法 |
-
2017
- 2017-11-09 CN CN201711095018.4A patent/CN109765569B/zh not_active Expired - Fee Related
Also Published As
Publication number | Publication date |
---|---|
CN109765569A (zh) | 2019-05-17 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN111207774B (zh) | 一种用于激光-imu外参标定的方法及*** | |
CN107239076B (zh) | 基于虚拟扫描与测距匹配的agv激光slam方法 | |
WO2018054080A1 (zh) | 一种机器人规划路径的更新方法及装置 | |
CN106197428B (zh) | 一种利用测量信息优化分布式ekf估计过程的slam方法 | |
CN103257342B (zh) | 三维激光传感器与二维激光传感器的联合标定方法 | |
CN112882053B (zh) | 一种主动标定激光雷达和编码器外参的方法 | |
CN109765569B (zh) | 一种基于激光雷达实现虚拟航迹推算传感器的方法 | |
CN106885576B (zh) | 一种基于多点地形匹配定位的auv航迹偏差估计方法 | |
CN109807911B (zh) | 基于gnss、uwb、imu、激光雷达、码盘的室外巡逻机器人多环境联合定位方法 | |
CN107741745A (zh) | 一种实现移动机器人自主定位与地图构建的方法 | |
CN106969784B (zh) | 一种并发建图定位与惯性导航的组合误差融合*** | |
CN104166989B (zh) | 一种用于二维激光雷达点云匹配的快速icp方法 | |
CN105865452A (zh) | 一种基于间接卡尔曼滤波的移动平台位姿估计方法 | |
CN111913484B (zh) | 一种变电站巡检机器人在未知环境下的路径规划方法 | |
CN111380573A (zh) | 用于校准运动的对象传感器的取向的方法 | |
CN114440928A (zh) | 激光雷达与里程计的联合标定方法、机器人、设备和介质 | |
CN110132281B (zh) | 一种基于询问应答模式的水下高速目标高精度自主声学导航方法 | |
CN101907705B (zh) | 通用的多源遥感影像几何校正模型联合平差方法 | |
Chen et al. | 3D LiDAR-GPS/IMU calibration based on hand-eye calibration model for unmanned vehicle | |
CN116642482A (zh) | 基于固态激光雷达和惯性导航的定位方法、设备和介质 | |
CN110703205B (zh) | 基于自适应无迹卡尔曼滤波的超短基线定位方法 | |
CN112147599B (zh) | 基于样条函数的3d激光雷达和惯性传感器外参标定方法 | |
CN115540850A (zh) | 一种激光雷达与加速度传感器结合的无人车建图方法 | |
CN109031339B (zh) | 一种三维点云运动补偿方法 | |
CN104517297A (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 | ||
CF01 | Termination of patent right due to non-payment of annual fee | ||
CF01 | Termination of patent right due to non-payment of annual fee |
Granted publication date: 20220517 |