CN109765569B - 一种基于激光雷达实现虚拟航迹推算传感器的方法 - Google Patents

一种基于激光雷达实现虚拟航迹推算传感器的方法 Download PDF

Info

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
Application number
CN201711095018.4A
Other languages
English (en)
Other versions
CN109765569A (zh
Inventor
段琢华
许文杰
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
University of Electronic Science and Technology of China Zhongshan Institute
Original Assignee
University of Electronic Science and Technology of China Zhongshan Institute
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by University of Electronic Science and Technology of China Zhongshan Institute filed Critical University of Electronic Science and Technology of China Zhongshan Institute
Priority to CN201711095018.4A priority Critical patent/CN109765569B/zh
Publication of CN109765569A publication Critical patent/CN109765569A/zh
Application granted granted Critical
Publication of CN109765569B publication Critical patent/CN109765569B/zh
Expired - Fee Related legal-status Critical Current
Anticipated expiration legal-status Critical

Links

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，时间间隔τ；其中
Figure GDA0003571810470000011
Figure GDA0003571810470000012
表示Rc的第j条射线，
Figure GDA0003571810470000013
表示射线
Figure GDA0003571810470000014
测量的距离，
Figure GDA0003571810470000015
表示射线
Figure GDA0003571810470000016
的角度；
Figure GDA0003571810470000017
表示Rp的第j条射线，
Figure GDA0003571810470000018
表示射线
Figure GDA0003571810470000019
测量的距离，
Figure GDA00035718104700000110
表示射线
Figure GDA00035718104700000111
的角度；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个粒子，表示移动机器人的位姿:
Figure GDA0003571810470000021
步骤5：j从1至width循环执行步骤6
步骤6：
Figure GDA0003571810470000022
Figure GDA0003571810470000023
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{
Figure GDA0003571810470000024
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，时间间隔τ；其中
Figure GDA0003571810470000031
Figure GDA0003571810470000032
表示Rc的第j条射线，
Figure GDA0003571810470000033
表示射线
Figure GDA0003571810470000034
测量的距离，
Figure GDA0003571810470000035
表示射线
Figure GDA0003571810470000036
的角度；
Figure GDA0003571810470000037
表示Rp的第j条射线，
Figure GDA0003571810470000041
表示射线
Figure GDA0003571810470000042
则量的距离，
Figure GDA0003571810470000043
表示射线
Figure GDA0003571810470000044
的角度；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个粒子，表示移动机器人的位姿：
Figure GDA0003571810470000045
步骤5：j从1至width循环执行步骤6
步骤6：
Figure GDA0003571810470000046
Figure GDA0003571810470000047
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{
Figure GDA0003571810470000048
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，时间间隔τ；其中
Figure FDA0003571810460000011
Figure FDA0003571810460000012
表示Rc的第j条射线，
Figure FDA0003571810460000013
表示射线
Figure FDA0003571810460000014
则量的距离，
Figure FDA0003571810460000015
表示射线
Figure FDA0003571810460000016
的角度；
Figure FDA0003571810460000017
表示Rp的第j条射线，
Figure FDA0003571810460000018
表示射线
Figure FDA0003571810460000019
测量的距离，
Figure FDA00035718104600000110
表示射线
Figure FDA00035718104600000111
的角度；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个粒子，表示移动机器人的位姿：
Figure FDA00035718104600000112
步骤5：j从1至width循环执行步骤6
步骤6：
Figure FDA00035718104600000113
Figure FDA00035718104600000114
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)
Figure FDA0003571810460000021
其中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/τ。
CN201711095018.4A 2017-11-09 2017-11-09 一种基于激光雷达实现虚拟航迹推算传感器的方法 Expired - Fee Related CN109765569B (zh)

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)

* Cited by examiner, † Cited by third party
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)

* Cited by examiner, † Cited by third party
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 哈尔滨工程大学 一种基于改进的粒子滤波的移动机器人同时定位与地图构建的方法

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