[发明专利]一种基于激光雷达实现虚拟航迹推算传感器的方法

摘要

本发明涉及一种基于激光雷达实现虚拟航迹推算传感器的方法，其核心是根据时间上相邻的两次激光雷达扫描数据的配准来计算移动机器人在该时间段的位姿变化；首先利用粒子滤波方法对移动机器人的位姿变化进行初始估计，在此基础上利用迭代最近点方法对移动机器人位姿变化进行精确估计，最后根据要求位姿变化计算出移动机器人在该时间段内的线速度和角速度。

主权项

1.一种激光雷达实现虚拟航迹推算传感器的方法，其核心是根据时间上相邻的两次激光雷达扫描数据的配准来计算移动机器人在该时间段的位姿变化；首先利用粒子滤波方法对移动机器人的位姿变化进行初始估计，在此基础上利用迭代最近点方法对移动机器人位姿变化进行精确估计，最后根据要求位姿变化计算出移动机器人在该时间段内的线速度和角速度；具体实现步骤如下：输入：当前时刻激光雷达扫描，前一时刻激光雷达扫描，时间间隔;其中，表示的第j条射线，表示射线测量的距离，表示射线的角度；表示的第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个粒子表示的移动机器人的位姿:                     (1)步骤5：j从1至width循环执行步骤6步骤6：xpj=*cos()                       （2）ypj=*sin()                       （3）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 {distb=(anglej‑angle_min‑angle_inc*index)*(‑)/angle_inc+;wall[j]=exp(‑.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）为初始位置估计，利用迭代最近点方法根据和计算移动机器人在该时间段的位姿估计（bestxicp,bestyicp,besthetaicp），（bestxicp,bestyicp,besthetaicp）=icp((bestx，besty，bestheta）,,,）步骤11：根据（bestxicp,bestyicp,besthetaicp）估计移动机器人在该时间段的线速度和角速度：=bestxicp/=besthetaicp/。