1.基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于包括以下步骤：

步骤1：读取视觉传感器、激光雷达传感器、惯性传感器的数据，对不同源的图像、激光点云和imu位姿数据进行时间同步，获得统一的时间系统下的待融合数据，并利用惯性传感器采集到的前20帧数据进行初始化，建立初始位姿估计；

步骤2：基于imu传感器的高工作频率，惯性传感器选用imu传感器，对imu传感器采集到的惯性数据进行前向传播，得到各个时刻机器人位姿的先验估计；得到各个时刻间对机器人的位姿的先验估计后，通过对时刻进行插值，计算出每个激光点采集时刻和激光点云帧采集结束时刻间机器人的相对运动，将激光点从采集时刻的机器人坐标系转移到点云帧采集结束时刻的坐标系中，对当前帧中每一个激光点进行上述操作后，得到消除帧间运动畸变的点云帧采集结束时刻点云；

步骤3：使用误差状态迭代卡尔曼滤波器对不同帧间采集到的信息进行比较，在机器人运动过程中，一个地标物往往会在连续很多帧上同时存在，并且和机器人的相对位置不断变化，对新采集到的点云帧中的每一点，在地图中提取和当前位置最近的五个点拟合出一个小平面，通过迭代优化，不断缩小激光点和它对应的拟合平面间的距离，直到小于阈值，从而得到点云帧采集结束时刻对机器人运动的最优估计；利用计算出的最优估计，将点云帧采集结束时刻视觉传感器采集到的图像转移归一化坐标系并最终转移到雷达坐标系中，对当前帧中的每一个点，利用预标定过的激光雷达传感器和视觉传感器的旋转平移关系，从三维空间坐标转移到像素坐标系，给视觉传感器视场范围内的每个激光点赋予RGB信息，得到经过RGB渲染的彩色点云；使用基于时间的插帧处理，利用激光雷达传感器采集时刻的前后两帧图片，使用稠密光流法估计出激光雷达传感器采集时刻的视觉数据，实现激光雷达传感器和视觉传感器的时间同步；

步骤4：对经过RGB渲染的彩色点云通过点云帧采集结束时刻对机器人运动的最优估计，将点云从点云帧采集结束时刻的机器人运动坐标系转移到独立的世界坐标系，从而构建出当前帧的彩色点云地图；将地图中的点以K维树的形式组织起来，使得新采集到的每一个点都可以用最短的时间找到它在地图中的最邻近点；通过时间累积，将机器人采集到的多帧信息累加，得到连续各帧间的机器人运动最优估计和各帧的彩色点云地图，实现机器人的自我定位并建立全局彩色地图。

2.根据权利要求1所述的基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于所述步骤2具体包括以下步骤：步骤2.1：建立imu运动学模型，对于机器人运动的描述中最重要的两个物理量是当前时刻的位姿和相对于坐标系原点的位移量，使用imu传感器测量得到的角速度和加速度数据对时间维度积分得到所求的位姿和位移状态，因此建立在世界坐标系下的状态表达方程：在建立世界坐标系的状态表达方程中一共有6个状态量，分别是机器人在世界坐标系下的位姿RI，即机器人相对于初始状态的旋转量，使用一个3*3的旋转矩阵来表示；机器人在世界坐标系下相对于原点的位移量pI，使用一个3*1的平移向量来表示；机器人在x、y、z三个轴上的瞬时速度vI,使用用一个3*1的速度向量来表示；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差bω和ba；机器人世界坐标系相对于真实世界坐标系下的重力矢量g；

步骤2.2：对于建立好的状态方程，对于imu数据进行预积分，建立imu运动学连续模型；

其中，机器人在世界坐标系下的位姿的一阶微分量等于机器人相对于初始状态的旋转矩阵右乘消除噪音后的角速度向量的反对称矩阵；机器人在世界坐标系下的位移的一阶微分量等于机器人在x、y、z三个轴上的瞬时速度；机器人的速度的一阶微分量等于机器人相对于初始状态的旋转矩阵乘以消除噪音后的加速度向量再加上机器人世界坐标系下的重力矢量；机器人惯性imu传感器采集数据的在角速度和加速度上的偏差bω和ba的模型为带高斯噪声nbω和nba的随机游动过程；机器人世界坐标系下的重力矢量g的一阶微分量为0，由此可得机器人的imu运动学方程如下：对上述方程组离散化可得

步骤2.3：步骤1中进行初始化，得到了机器人运动状态的初始估计，当机器人收到imu传感器的输入时，即可根据步骤2.2中建立的离散运动学模型进行前向传播，通过初始估计和imu传感器每一帧采集到的加速度和角速度数据，迭代出每一帧时刻的机器人位姿的先验估计；具体的迭代方程如下xi+1＝xi+(Dtf(xi,ui,wi))

其中控制量ui为imu传感器采集到的的加速度数据am和角速度数据ωm,误差量w为imu传感器的累积误差nω、na和imu传感器采集到的数据的误差nbω、nba；

步骤2.4：激光雷达传感器采集完完整一帧点云后，根据imu传感器数据前向传播得到的各个时刻的机器人运动状态进行插值计算，得到每个激光点采集时刻对应的机器人运动状态，并计算出每个激光点采集时刻到点云帧采集结束时刻的机器人相对运动状态其中j为每个雷达点的采集时刻，k为点云帧的采集时刻；将j时刻采集到的激光点先从j时刻的激光雷达坐标系转移到imu传感器坐标系，再利用j时刻到k时刻的机器人的相对运动将激光点从j时刻的imu传感器坐标系转移到k时刻的imu传感器坐标系，最后再将激光点先从k时刻的imu传感器坐标系转移到激光雷达坐标系，激光点的坐标系转换过程如下公式所示：I

其中 TL是激光雷达坐标系和imu传感器坐标系间的旋转平移关系，由于激光雷达传感I

器和imu传感器二者之间有固定的物理连接关系，所以 TL是一个固定量，并通过预标定得到。

3.根据权利要求2所述的基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于所述步骤3具体包括以下步骤：步骤3.1：构建一个误差状态迭代卡尔曼滤波器来优化机器人的运动状态，减小传感器导致的误差；将点云帧采集结束时刻k的点云 转换到imu传感器坐标，再利用前向传播得到的k时刻机器人运动的先验估计，可以得到激光点j的在激光雷达点云帧采集结束时刻k的估计全局坐标系坐标点最近的平面或边缘应该由其地图上附近的点来定义，残差 定义为激光点j的在激光雷达点云帧采集结束时刻k的估计全局坐标系坐标点 与地图上最近的平面或边缘的距离：G

其中Gj等于地图上与激光点j最近的5个点所拟合出的小平面的法向量 qj为估计全局坐标系坐标点 在拟合平面上的投影点；

步骤3.2：当传感器误差为0时，imu数据前向传播得到的机器人状态先验估计即为机器G

人的实际运动状态，此时估计全局坐标系坐标点 即为实际地图上的点，因此 与qj处于同一拟合平面，残差 等于0，由于误差影响任何时刻下 都不等于0，使用一个误差迭代卡尔曼滤波器来迭代优化 从而得到当残差小于阈值时的机器人运动状态的估计作为最佳估计；根据步骤2.3构建的机器人运动状态迭代方程，取运动状态的真实值为xi,建立误差状态的动态模型如下将白噪声w的协方差记为Q，则前向传播的协方差 可按下式迭代计算得到假设激光点测量值 对应的真实值为 真实值与观测值之间的误差原始测量噪声为 则有 将 代入残差计算方程可得在 处进行一阶泰勒展开，可得残差方程的一阶近似

将激光点对应真实值的残差方程表达为观测值残差方程与真实值残差方程的雅克比矩阵与vj之和的形式，其中vj代表原始测量噪声 所产生的观测误差，是要通过迭代优化消除掉的干扰量；

将得到的观测误差方程与建立的状态误差方程联立，得到需要优化的观测误差的最大后验估计值 对观测误差的最大后验估计值用卡尔曼滤波器进行最小二乘优化，得到收敛并小于阈值的 此时对应的运动状态估计值 即为最优状态估计

步骤3.3：将步骤3.2中得到的最优状态估计 假设为这一点云帧采集结束时刻k的机器人运动状态真实值，可得k时刻的机器人相对于世界坐标系状态转换矩阵 包括旋转矩阵 和平移向量 利用摄像头采集到的像素点 的时间戳信息计算出摄像头图片采集时刻q与激光雷达点云帧采集结束时刻k之间机器人的相对运动状态，并得到对应摄像头图片采集时刻q与激光雷达点云帧采集结束时刻k之间的状态转换矩阵 将k时刻激光雷达坐标系中的激光点 转移到q时刻的激光雷达坐标系中，公式如下得到q时刻的激光点云 后，利用预标定过的激光雷达传感器和视觉摄像头传感器C

间的坐标转换关系TI和相机内参I将激光点从激光雷达传感器的xyz坐标系转移到相机的归一化坐标系，最后再转移到相机的像素坐标系，公式如下：使用稠密光流法得到激光雷达扫描结束时刻的相机数据，由于相邻两帧间采集间隔时间较短，可以假设两帧中同一物体的灰度不变，通过前后两帧中各个光流场的位移，估计出激光雷达时刻的相机数据，公式如下根据激光点对应的像素点坐标进行筛选，像素坐标超出图片尺寸的激光点即为落在摄像头视野盲区外的点，将这部分激光点筛除掉，并将视觉摄像头采集到的像素点用双线性插值法计算出剩余的激光点的像素坐标处对应的RGB值，公式如下将插值计算出的RGB值作为激光点云 的属性，保存为彩色激光点云

4.根据权利要求3所述的基于多传感器的机器人实时定位和彩色地图融合映射方法，其特征在于所述步骤4具体包括以下步骤：步骤4.1：将得到RGB属性的激光点云 再依次转换回k时刻的雷达坐标系中，得到点云 随着状态 的更新，可得k时刻的机器人惯性坐标系相对于全局框架世界坐标I系状态转换矩阵 同时已知预标定过的TL是激光雷达坐标系和imu传感

器坐标系间的旋转平移关系，将点云从k时刻雷达坐标系转移到全局框架下的世界坐标系，即可得到用于建图的彩色点云步骤4.2：将地图以k维树的形式组织起来，k维树是二叉树的变种，用于储存维度为k的空间中的点，我们的激光雷达点依照xyz三个维度进行分割，构造k＝3的k维树；

在第一帧彩色点云 存放入地图中后，对这一帧中每一个点的x坐标值进行遍历并排序，将中值点作为根节点，所有x坐标值值小于根节点的点放入左侧的子树，所有x坐标值大于根节点的点放入右侧的子树；完成第一层的分割对左右两棵子树中点的y坐标值进行分别的遍历和排序，分别将中值点作为第二层的根节点，所有y坐标值值小于对应根节点的点放入对应根节点左侧的子树，所有y坐标值大于对应根节点的点放入对应根节点右侧的子树；完成第二层的分割后再分别对四棵子树的z坐标值进行遍历和排序，依次循环，直到最后一层的每个子树都包含一个点，即完成了k维树的初始化构建；

从第二帧开始，每当有新的点云存放入地图，点云中的每个点递归地从根节点向下搜索，并将新点的除法轴上的坐标与存储在树节点上的点进行比较，直到发现一个叶节点然后追加一个新的树节点；当一帧中的所有点都完成更新后，从叶节点向上判断子树是否已经不平衡，即一侧子树中的节点数是否远多于另一侧，如果检测到不平衡，则将对应子树重建；

步骤4.3：随着机器人的运动，将多帧间的机器人运动状态最优估计 可视化显示出来，即可得到机器人运动中的实时定位效果，将多帧用于建图的彩色点云 累加在同一张地图中，即可得到对应地图，当机器人完成对当前环境的遍历后，即可建立全局彩色地图。