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/τ。