1.一种基于激光与视觉异步融合的SLAM算法，其特征在于，包括以下步骤：步骤1)对于由视觉传感器反馈得到的位姿增量，根据相机小孔成像原理以及刚体变换T公式，假设空间点P在世界坐标系下的齐次坐标为P＝(XW,YW,ZW,1) ；则其在相机的像素坐标系下的坐标为：其中(u,v)为点p在像素坐标系下的坐标值；s为点p在相机坐标系下的深度值；K为相机的内参矩阵；R为旋转矩阵；t为平移矩阵；

步骤2)通过ORB特征提取算法得到当前帧中图像的特征点，将rBRIEF描述符得到的当前帧中特征点的描述符与上一时刻图像中的特征点进行描述符之间的匹配，建立对应的匹配关系；将上一时刻的匹配点通过相机的内参矩阵以及初始位姿投影到当前帧图像中；由于相机的位姿通常存在偏差，所以投影点与当前地图上的匹配点之间通常会存在偏差；假T设特征点P在当前帧图像中的坐标为P＝(u1,v1) ，对应上一帧中的匹配点在当前帧中的匹T配点坐标为P’＝(u2,v2) ；通常由于相机位姿的偏差导致这两个点的坐标并不相同，因此定义误差函数：其中e是误差值，(u,v)是当前帧中特征点的像素坐标，P是当前帧特征点对应的匹配点；将其改写为李代数形式：其中exp(δ)为相机坐标系的李代数表达式，e为空间点P投影到当前图像中与匹配点之间的投影误差；

通常一帧图像中会存在若干个匹配点，根据所有的匹配点得到投影误差函数：其中ui表示当前帧中第i个特征点；Pi表示其对应的匹配点的世界坐标；si表示该点的深度；K表示相机的内参矩阵；exp(δ)表示相机坐标系的李代数表达式；

步骤3)利用高斯牛顿法或者列文伯格‑马夸尔特方法求解公式(4)，得到位姿估计为：其中z1(k1)为视觉传感器在k1时刻的最优估计，R为旋转矩阵，t为平移矩阵；v1(k1)为相机估计噪声且假设服从高斯分布；

步骤4)假设k2时刻得到的激光传感器数据为 令Si(δ2)为世界坐标，M(Si(δ2))为坐标点Si(δ2)的占用概率， 为M(Si(δ2))的导数；M(Si(δ2))与的值可通过对世界坐标Si(δ2)进行双线性插值求得；那么机器人从上一时刻到当前时刻的位姿增量可由下式给出：其中

步骤5)根据位姿增量得到机器人在激光传感器位姿估计下的坐标为：z2(k2)＝z2(k2‑1)+Δδ2+v2(k2)    (8)其中z2(k2)为激光传感器在k2时刻的最优位姿估计；z2(k2‑1)为其在上一时刻的最优位姿估计；v2(k2)为观测噪声且假设服从高斯分布，满足如下关系：T

其中vi(ki)，vj(kj)为测量噪声，E代表均值，δij， 为克罗内克函数，Ri(ki)为k时刻来自视觉或者激光传感器的协方差矩阵；

步骤6)采用了异步融合方式对来自两个传感器的数据进行融合；

首先将传感器的数据按较快的传感器采样频率划分为m个采样区间，每个采样区间内会存在两种情况：

1)只拥有相机的位姿估计；

2)同时拥有来自激光传感器和视觉传感器的位姿估计；

结合Kalman滤波器及多传感器异步融合的方法，得到了改进后的基于激光与视觉异步融合位姿估计算法如下：其中 为根据上一时刻机器人位姿的最优估计得到的预测位姿；P0(k|k)为预测位姿的协方差矩阵；zi(ki)为k时刻来自视觉或者激光传感器的最优位姿估计；Ri(k)为k时刻来自视觉或者激光传感器的协方差矩阵，下标i代表传感器数据的个数；

当采样区间内只有相机的数据时，i＝1，此时式(10)变为：此时估计模型转变为以较快的传感器采样频率下对单个传感器数据进行位姿估计的Kalman滤波器；

当采样区间内同时存在相机的数据以及激光传感器的位姿估计结果时，此时式(10)变为式(12)、(13)的形式：此时估计模型变为以较慢的传感器采样速率下对激光传感器与视觉传感器进行融合估计的Kalman滤波算法；算法以上一时刻的最优估计作为预测量，首先融合当前时刻传感器一估计的位姿，然后再融合传感器二估计的位姿结果，最终以两次融合后的传感器估计值作为最终位姿估计的结果；

步骤7)为了有效利用每个传感器不同的特性，引入置信度概念；

使用置信度w表示对两个传感器位姿估计的置信度，将式(10)改为：其中wi代表第i个传感器的位姿估计置信度；

步骤8)传感器的特性通常与机器人运动过程相关，因此wi应与机器人的运动状态相关；

当机器人运动沿直线行驶时，算法增大对视觉的置信度；当机器人出现旋转时，算法增大对激光的置信度；

每次融合时算法会根据激光传感器最新的位姿估计判断机器人当前运动过程中是否发生旋转；其由式(6)得到，这里记为Δα2；

将角度变化量代入wi得到：

其中Δα2为上一时刻到当前时刻估计的机器人旋转角度；根据当前机器人运行效果，每个采样周期内机器人旋转角度不超过0.25rad，因此式(15)修改为：将式(16)代入式(14)得到：