CN114419147A - 一种救援机器人智能化远程人机交互控制方法及系统 - Google Patents

Abstract

本发明公开了一种救援机器人智能化远程人机交互控制方法，包括：利用三维激光雷达和惯性测量单元IMU传感器，在无人平台行进过程中估计无人平台的位姿并优化；利用单目摄像机自身进行360°旋转，在较短的时间间隔内环形采集周围场景的图像信息，将拍摄的照片序列进行空间匹配对准，构造一幅包含图像序列信息的全景图像；基于获取的点云数据与全景图像，利用时间同步和空间同步方法，获得同一时刻的一帧点云和一帧图像，同时联合标定结果确定三维激光雷达和单目摄像机两个坐标系位置关系以及单目摄像机的内参和畸变参数；构建无人平台周围局部环境的三维地图模型并且进行优化；进行救援机器人智能化远程人机交互控制。还公开了对应的控制系统。

Description

一种救援机器人智能化远程人机交互控制方法及系统

技术领域

本发明属于机器人控制技术领域，尤其涉及一种救援机器人智能化远程人 机交互控制方法及系统。

背景技术

高机动救援机器人从工程角度上需要在救援过程中模仿人类固有的动态行 为，如步行、学习功能等，并且由于面临复杂的灾害救援环境，其行走和作业 过程中的稳定度高于其他类型的机器人。

智能控制采用各种智能技术从而达到对复杂系统控制的目标，其产生和发 展反映当代自动控制以及科学技术的发展趋势。目前具有四种基本的机器人行 走及作业稳定控制方法，并且有相互之间融合的趋势，包括：

(1)机器人变结构控制：具有完全和理想鲁棒性，尽管含有不确定性，但 系统在滑动模态时仍具有对外部环境的不变性，因此适合机器人的控制。变结 构控制不需要精确的系统模型，只需要获知模型中参数的误差范围或变化范围 即可。对于有界干扰和参数变化具有不敏感性，从而消除由于哥氏力及粘性摩 擦力的作用而产生的影响。控制算法简单，容易在线实现，但是抖振现象阻碍 了其在救援机器人上的实际应用，目前提出的动态调整滑模参数、在线估计滑 模参数的削弱抖振改进算法效果都不好。

(2)机器人模糊控制：用机器模拟人对系统的控制，包括模糊化、模糊决 策和精确化计算，模糊系统为不依赖模型的估计器，主要依赖模糊规则和模糊 变量的隶属度函数，无需知道输入和输出之间的数学依赖关系，是解决不确定 系统控制的有效方法，但是对信息进行简单的模糊处理会导致被控系统的精度 降低，动态品质变差，而提高系统精度必然增加量化等级，导致规则迅速增多， 影响规则库的最佳生成，且增加系统的复杂和推理时间。

(3)机器人分层递阶控制：实现传统控制功能外的规划、决策和学习等智 能功能，上层作用为模仿人的行为功能，主要基于知识的系统，所实现的规划 决策、学习、数据的存取、任务的协调主要是对知识进行处理；下层的作用是 执行机器人具体的控制任务。

(4)机器人的神经网络控制：复杂非线性系统的识别和控制，能够充分逼 近任意复杂的非线性系统，学习与适应不确定系统的动态特性，具有很强的鲁 棒性和容错性。然而防灾救援环境多变，因此在机器人神经网络动力学控制方 法中，对于多自由度机器人控制，输入参数多，学习时间长，实时性较低，对 于计算力矩控制和分解运动加速度控制不友好，因此无法在高机动救援机器人 中应用。

以上控制方法属于单向控制，并未实现人机交互，并且对于当前较为复杂 的工业现场环境，针对三维目标图像检测技术，图像数据具有丰富的颜色信息 以及稠密的像素点等优势，但仅仅依靠单一摄像头很难获取准确三维信息，而 双目或三目视觉技术的准确性仍然不高，且摄像头性能受环境影响较大。另一 方面，激光雷达能够获取目标准确三维信息、稳定性高，但存在点云线束稀疏、 特征提取困难等劣势。点云是三维激光扫描最主要的数据，其空间分布呈现为 离散型。由于数据采集具有一定的盲目性，激光点的位置是随机的，不能保证 地物特征点有数据。虽然扫描仪还能提供激光束的回波强度信息，但也存在着 噪声严重的问题，而且利用强度生成的灰度影像在视觉上并不直观。另外，点 云数据的海量性给数据的存储、处理带来了巨大的挑战。点云的这些特点导致 单纯利用点云难以对地物进行判读、分类、测量，制约着点云的应用。目前基 于二维激光雷达估计无人平台位姿的技术已经非常成熟，但是二维激光雷达过 分地依赖帧间的匹配，误差累积明显，三维激光雷达的优势在于测量频率高， 测量结果准确，测量距离远，不易受环境影响。但是现有技术如何使用三维激 光雷达结合IMU信息对地面无人平台的六自由度姿态进行实时的估计还没有成 熟的方案。

发明内容

本发明的目的是提供一种救援机器人智能化远程人机交互控制方法及系统， 面向实际现场环境，利用多传感器间的互补特性，提出基于激光雷达与摄像头 的三维目标检测技术，安装于无人平台上的传感器通过环形扫描得到多视角下 的环境信息，在得到目标场景的三维信息和色彩信息后，基于时间同步和空间 同步将两种传感器获得的信息进行数据级别的融合，进行三维场景重建。

本发明一方面提供了一种救援机器人智能化远程人机交互控制方法，包括：

步骤1，利用三维激光雷达和惯性测量单元IMU传感器，在无人平台行进过 程中估计无人平台的位姿；其中基于所述三维激光雷达进行位姿估计，然后使 用惯性测量单元IMU传感器获得一次运动前的信息，辅助描述高频率下无人平 台的运动，提高姿态估计的准确度；

步骤2，利用安装在所述三维激光雷达正上方的单目摄像机自身进行360° 旋转，在较短的时间间隔内环形采集周围场景的图像信息，将拍摄的照片序列 进行空间匹配对准，构造一幅包含图像序列信息的全景图像。

步骤3，基于获取的点云数据与所述全景图像，利用时间同步和空间同步方 法，获得同一时刻的一帧点云和一帧图像，同时联合标定结果确定所述三维激 光雷达和所述单目摄像机两个坐标系的位置关系以及所述单目摄像机的内参和 畸变参数；

步骤4，构建无人平台周围局部环境的三维地图模型并且进行优化；

步骤5，基于优化的三维地图模型进行救援机器人智能化远程人机交互控制。

优选的，所述基于所述三维激光雷达进行位姿估计包括：

步骤11，利用雷达里程计在高频率、低保真度下进行点云特征提取、点云 特征匹配以及建立约束方程来估计所述三维激光雷达的位姿；

步骤12，在低频率、高精度下使用点云与已建立地图匹配并结合回环检测 的方法，实现所述雷达里程计以及所述位姿的优化。

优选的，所述步骤2包括：利用基于SIFT特征匹配的图像拼接算法，在不 降低图像分辨率的条件下将拍摄的照片序列进行空间匹配对准，构造一幅包含 图像序列信息的全景图像。

优选的，所述步骤4包括：

步骤41，根据所述位置关系、内参预计畸变参数，将所述三维激光雷达三 维坐标系下的点投影至二维像素平面，获得所述二维像素平面相应位置的颜色 信息；

步骤42，利用激光雷达估计的无人平台位姿，将具有彩色信息的单帧点云 转换到世界坐标系下，与已经建立的地图叠加，实时构建无人平台周围局部环 境的三维地图模型；

步骤43，利用高阶条件随机场对已建立所述三维地图模型进行更新优化， 提升所述三维地图模型的精度。

优选的，所述步骤1实施前还包括：对点云进行预处理，所述预处理包括 运动畸变校正和地面点分割；其中：

所述运动畸变校正包括：

根据扫描到的点云角度，按照不同的扫描线将其分类，分别保存到不同的 点云指针中；通过计算每一个点的俯仰角ω来确定其所属的线，对俯仰角四舍五 入，根据俯仰角排列激光线号；对初始和结束时刻两个位置的雷达坐标系进行 转化，对激光雷达点云进行运动畸变消除；使用其他传感器获得运动速度、位 移信息去除运动畸变，或者使用IMU直接测量机器人的角度、位移和速度后使 用线性插值去除运动畸变；在点云畸变校正前，首先记录点云起始位置的速度 (V_sx,V_sy,V_sz)，位移(X_s,Y_s,Z_s)和角度(Roll_s,Pitch_s,Yaw_s)，在获取点云的时间t_i后，使 用IMU数据线性插值求解出当前点p_i对应的速度(V_ix,V_iy,V_iz)，位移(X_i,Y_i,Z_i)和角 度(Roll_i,Pitch_i,Yaw_i)，计算当前点p_i相对于第一个点由于运动产生的位移畸变，并 对点云中的每个点位置信息重新补偿校正消除运动畸变；所述畸变校正过程分为三步：首先计算当前时刻在世界坐标系下由于加减速造成的位移畸变 (Δ_X,Δ_Y,Δ_Z)；然后将运动畸变值绕Y、X、Z轴旋转后得到起始点坐标系下的运动 畸变；最后将当前点p_i先转换到世界坐标系下，再由世界坐标转换到点云起始点 坐标系下，减去加减速造成的非匀速畸变的值，去除点云产生的位移畸变；

所述地面点分割包括：标记所有地面点；将非地面点分组为若干类，同一 聚类的点被分配相同的标签，而地面点是一种特殊的聚类。

作为优选的实施方式，所述步骤11的所述点云特征提取包括：将一次扫描 中获得的点进行遍历求取点云曲率，按照曲率c值大小进行排序，使用阈值c_th来 判断不同类型的特征点，c＞c_th的点即为边缘特征，c＜c_th的点即为平面特征；在 点云预处理时，已经将点云标记为地面点和非地面点，只对地面点提取平面特 征F_p，只对非地面点提取边缘特征F_e；同时在点云预处理时，已经按照不同的扫 描线将点云分类，分别保存到不同的点云指针中；把每个扫描线平均分成6段， 每一段按照曲率的升序排列，根据阈值分别提取边缘点和平面点，将特征点平 均分配到环境中；

所述步骤11的所述点云特征匹配包括：

设t_k为第k次扫描的起始时刻。在每次扫描的结束时刻，本次采集的点云P_k将会根据雷达里程计结果投影到时间t_k+1下，投影后的点云为

设第k+1次扫 描的点云为P_k+1，分别获得P_k和P_k+1中的边缘特征点和平面特征点，

和

为 第k次扫描的边缘特征和平面特征，投影到扫描结束时刻为

和

和

分别为第k+1次扫描的边缘特征和平面特征；在第k+1次扫描起始时P_k+1为空， 随着扫描的进行P_k+1中的点云不断增加，同时激光雷达里程计也在不断估计扫描 过程中的雷达的姿态；在每次迭代过程中根据目前估计的姿态变换将

和

投影到这次扫描的起始时刻t_k+1，

和

表示投影后的特征点集，用于与上一 帧数据进行匹配；对

和

中的每个点，在对应的

和

中寻找与其最相 近的点；建立这两个特征点集的一一对应关系，完成特征点云的匹配工作；

所述步骤11的所述建立约束方程包括：找到特征点对应的边缘线或对应平 面后，找到前后两帧点云的对应关系，建立点云的约束方程，并通过特征点距 离最小化来估计所述三维激光雷达的运动；其中利用两步L-M算法来估计所述 三维激光雷达的运动姿态，计算每一个特征点的最小二乘权值，通过一次迭代 更新姿态变换矩阵，如果迭代结果趋于收敛，或者达到了最大迭代次数，则非 线性优化过程结束。当里程计算法达到扫描的结束时刻，当前位置通过姿态变 换被投影到下一个时间点上，为下一次扫描做准备。

作为优选的实施方式，所述步骤12包括：

步骤121，雷达建图优化，使用与已建立地图进行匹配的方式来优化位姿；

步骤122，回环检测和位姿图优化；所述回环检测是指无人平台识别曾经到 达过的场景，并与之前的某一帧建立位姿约束关系，从而修正之前对方位的判 断，起到了减小累计误差的作用，所述回环检测是基于位置也就是里程计的几 何关系来实现的，在位姿估计过程中不断保存雷达的轨迹，当前雷达运动到了 某个位置时，寻找附近5m内的历史轨迹，检测它们是否有回环关系；所述位姿 图优化包括：根据雷达姿态构建图，激光雷达每个关键帧的传感器位姿v_i作为图 中的顶点，而不同时刻位姿之间的关系构成边e_ij。在激光雷达运动过程中，通过 不断累积而成的顶点v_i和边构e_ij成图结构，图优化的目标就是通过调整顶点的位 姿最大可能的满足边之间的约束；如果当前帧位姿与之前的位姿匹配成功，那么检测到回环，则针对位姿图进行优化，调整机器人位姿顶点尽量满足边的约 束。

优选的，所述步骤3所述空间同步包括：

三维激光雷达和摄像机的空间同步，即建立它们的空间位置关系，通过对 两个传感器进行联合标定获取摄像机与激光雷达外参，在对摄像机和激光雷达 联合标定前需要获取摄像机的内参(f_u,f_v,u₀,v₀)，因此先对摄像机进行标定；获取 摄像机的内参的方法为使用移动棋盘格的摄像机标定方法，其基本原理是：使 用摄像机拍摄n幅棋盘格的照片，提取棋盘格角点坐标，计算单应性矩阵以及 内参和外参矩阵，构造似然函数使用L-M算法进行迭代，对摄像机的径向畸变 和切向畸变进行优化。不受棋盘格的运动影响，就可以直接获得摄像机的内参 (f_u,f_v,u₀,v₀)和畸变参数(k₁,k₂,k₃,p₁,p₂)。

优选的，所述步骤3所述时间同步包括：

所述三维激光雷达和摄像机的时间同步通常分为硬件同步和软件同步，硬 件同步是在摄像机上安装一个触发器，当激光雷达扫描一周时触发器触发，拍 摄一帧图像；将摄像机和雷达设置成相同频率(10HZ)，IMU设置成较大的频率 (50HZ)，通过时间同步机制对点云节点、摄像机节点、IMU节点的时间戳进行 比较，若时间戳小于一定的阈值，则统一接收这几个主题，并产生一个同步结 果的回调函数，在回调函数里对时间同步后的几种数据进行融合。

本发明的第二方面，提供一种救援机器人智能化远程人机交互控制系统， 包括：

无人平台的位姿及优化子系统，用于利用三维激光雷达和惯性测量单元IMU 传感器，在无人平台行进过程中估计无人平台的位姿；其中基于所述三维激光 雷达进行位姿估计，然后使用惯性测量单元IMU传感器获得一次运动前的信息， 辅助描述高频率下无人平台的运动，提高姿态估计的准确度；

单目摄像机，所述单目摄像机安装在所述三维激光雷达正上方，自身进行 360°旋转，在较短的时间间隔内环形采集周围场景的图像信息，利用基于SIFT 特征匹配的图像拼接算法，在不降低图像分辨率的条件下把拍摄的照片序列进 行空间匹配对准，构造一幅包含图像序列信息的全景图像。

点云和图像获取子系统，基于获取的点云数据与所述全景图像，利用时间 同步和空间同步方法，获得同一时刻的一帧点云和一帧图像；

联合标定子系统，用于联合标定，并根据联合标定结果确定所述三维激光 雷达和所述单目摄像机两个坐标系的位置关系以及所述单目摄像机的内参和畸 变参数。

三维地图模型构建及优化子系统，用于构建无人平台周围局部环境的三维 地图模型并且进行优化，包括：

救援机器人智能化远程人机交互控制子系统，用于基于优化的三维地图模 型进行救援机器人智能化远程人机交互控制。

本发明的第三方面提供一种电子设备，包括处理器和存储器，所述存储器 存储有多条指令，所述处理器用于读取所述指令并执行如第一方面所述的方法。

本发明的第四方面提供一种计算机可读存储介质，所述计算机可读存储介 质存储有多条指令，所述多条指令可被处理器读取并执行如第一方面所述的方 法。

本发明提供一种救援机器人智能化远程人机交互控制方法及系统和电子设 备，具有如下有益效果：

面向实际现场环境，利用多传感器间的互补特性，提出基于激光雷达与摄 像头的三维目标检测技术，安装于无人平台上的传感器通过环形扫描得到多视 角下的环境信息，在得到目标场景的三维信息和色彩信息后，基于时间同步和 空间同步将两种传感器获得的信息进行数据级别的融合，进行三维场景重建， 使用三维激光雷达结合IMU信息对地面无人平台的六自由度姿态进行实时的估 计。

附图说明

图1为本发明提供的救援机器人智能化远程人机交互控制方法流程图。

图2为本发明提供的激光雷达与摄像机安装示意图。

图3为本发明提供的三维激光雷达坐标系，图3(a)为三维激光雷达坐标 系正视图，Y为前进方向，X为右侧，Z指向上方，图3(b)为三维激光雷达坐 标系侧视图，图3(c)为三维激光雷达坐标系俯视图。

图4为本发明提供的运动畸变比较示意图，图4(a)为激光雷达扫描的真 实值，图4(b)为激光雷达的畸变示意图。

图5为本发明提供的雷达坐标系OXY平面分割示意图。

图6为本发明提供的3D点映射到2D点示意图。

图7为本发明提供的多自由度多功能高负载液压机械臂刚柔耦合分析方法 快速搬运下优化后三臂最大热点应力所在位置示意图。

图8为本发明提供的地面点分割结果可视化示意图，图8(a)和(b)分别 表示原始点云和地面点分割后的点云。

图9为本发明提供的将点云映射到每次扫描的结束时刻的示意图。

图10为本发明提供的特征点匹配原理示意图，其中图10(a)为寻找边缘 点对应的边缘线原理示意图，图10(b)为寻找平面点对应的特征平面原理示意 图。

图11为本发明提供的点线距离和点面距离示意图，其中图11(a)为点线 距离示意图，图11(b)为点面距离示意图。

图12为本发明提供的激光雷达里程计两步优化示意图。

图13为本发明提供的地图构建算法原理图。

图14为本发明提供的回环检测原理图，其中图14(a)表示无回环，图14 (b)表示局部回环，图14(c)表示大回环。

图15所示为本发明提供的位姿图优化原理图。

图16为本发明提供的摄像机与三维激光雷达的位置关系图。

图17为本发明提供的电子设备一种实施例的结构示意图。

具体实施方式

下面结合附图和实施例，对本发明的具体实施方式作进一步详细描述。以 下实施例用于说明本发明，但不用来限制本发明的范围。

实施例一

如图1所示，本实施例一提供了一种救援机器人智能化远程人机交互控制 方法，包括：

步骤1，利用三维激光雷达和惯性测量单元IMU传感器，在无人平台行进过 程中估计无人平台的位姿；其中基于所述三维激光雷达进行位姿估计，然后使 用惯性测量单元IMU传感器获得一次运动前的信息，辅助描述高频率下无人平 台的运动，提高姿态估计的准确度；

基于所述三维激光雷达进行位姿估计包括：

步骤11，利用雷达里程计在高频率、低保真度下进行点云特征提取、点云 特征匹配以及建立约束方程来估计所述三维激光雷达的位姿；

步骤12，在低频率、高精度下使用点云与已建立地图匹配并结合回环检测 的方法，实现所述雷达里程计以及所述位姿的优化。

步骤2，利用安装在所述三维激光雷达正上方的单目摄像机自身进行360° 旋转，在较短的时间间隔内环形采集周围场景的图像信息，利用基于SIFT特征 匹配的图像拼接算法，在不降低图像分辨率的条件下把拍摄的照片序列进行空 间匹配对准，构造一幅包含图像序列信息的全景图像。

步骤3，基于获取的点云数据与所述全景图像，利用时间同步和空间同步方 法，获得同一时刻的一帧点云和一帧图像，同时联合标定结果确定所述三维激 光雷达和所述单目摄像机两个坐标系的位置关系以及所述单目摄像机的内参和 畸变参数。

步骤4，构建无人平台周围局部环境的三维地图模型并且进行优化，包括：

步骤41，根据所述位置关系、内参预计畸变参数，将所述三维激光雷达三 维坐标系下的点投影至二维像素平面，获得所述二维像素平面相应位置的颜色 信息；

步骤42，利用激光雷达估计的无人平台位姿，将具有彩色信息的单帧点云 转换到世界坐标系下，与已经建立的地图叠加，实时构建无人平台周围局部环 境的三维地图模型；

步骤43，利用高阶条件随机场对已建立所述三维地图模型进行更新优化， 提升所述三维地图模型的精度。

步骤5，基于优化的三维地图模型进行救援机器人智能化远程人机交互控制。

三维激光雷达与单目摄像机安装示意图如图2所示。

优选的，所述步骤1实施前还包括：对点云进行预处理。

激光雷达扫描到的点是杂乱无章的，为了便于后续对点云的特征提取与配 准，需要对点云进行预处理操作。点云的预处理操作主要包括运动畸变校正和 地面点分割。

(1)运动畸变矫正：

定义雷达旋转一周的采集的点云为一帧，假设第k次扫描的点云为 P_k＝{p₁,p₂,...p_n}，如图3所示，(a)为激光雷达坐标系，Y为前进方向，X为右侧， Z指向上方，(b)为侧视图，(c)为俯视图。点p_i在雷达坐标系中的坐标为(x_i,y_i,z_i)。 首先根据扫描到的点云角度，按照不同的扫描线将其分类，分别保存到不同的 点云指针中。通过计算每一个点的俯仰角ω来确定其所属的线，对俯仰角四舍五 入，根俯仰角排列激光线号。

激光雷达获取一帧点云不是瞬时的，而是一个持续的过程，激光雷达在扫 描时还伴随着无人平台的运动，激光雷达有其自身的坐标系，当无人平台运动 到不同的位置时，雷达认为自己仍处于同一位置，所以会出现畸变。假设无人 平台以1m/s的速度直行，激光雷达扫描的频率是10HZ，即激光雷达旋转一周的 时间为0.1s，在这0.1秒内无人平台的运动距离为0.1m。激光雷达扫描返回的 点是位于其自身坐标系的，在扫描初始时刻和结束时刻，激光雷达对同一物体 的测量距离相差0.1m，因此会产生误差，如图4所示，(a)为激光雷达扫描的 真实值，(b)为激光雷达的畸变示意图。因此需要对初始和结束时刻两个位置 的雷达坐标系进行转化，对激光雷达点云进行运动畸变消除。通常的方法是使 用其他的传感器获得运动速度、位移等信息去除运动畸变，可以使用IMU直接 测量机器人的角度、位移和速度，IMU具有较高的局部角度测量精度和局部位置 测量精度，然后使用线性插值去除运动畸变。

定义P_k＝{p₁,p₂,...p_n}是第k次扫描的一帧点云，点p_i在当前时刻雷达坐标系 中的坐标为(x_i,y_i,z_i)。分别计算该次扫描起始时刻t_s和结束时刻t_e扫描点相对雷 达坐标系X正半轴的角度α_start和α_end。对于任意点p_i其夹角为α_i，α_i用于计算该 点在这帧点云中的相对位置，结合点云扫描起始和结束时间就可以得到每一个 点获取时的时间t_i。

在点云畸变校正时，首先记录点云起始位置的速度(V_sx,V_sy,V_sz)，位移 (X_s,Y_s,Z_s)和角度(Roll_s,Pitch_s,Yaw_s)，在获取点云的时间t_i后，使用IMU数据线性插 值求解出当前点p_i对应的速度(V_ix,V_iy,V_iz)，位移(X_i,Y_i,Z_i)和角度(Roll_i,Pitch_i,Yaw_i)， 计算当前点p_i相对于第一个点由于运动产生的位移畸变，并对点云中的每个点位 置信息重新补偿校正消除运动畸变。

畸变校正过程分为三步：首先计算当前时刻在世界坐标系下由于加减速造 成的位移畸变(Δ_X,Δ_Y,Δ_Z)；然后将运动畸变值绕Y、X、Z轴旋转后得到起始点坐 标系下的运动畸变；最后将当前点p_i先转换到世界坐标系下，再由世界坐标转换 到点云起始点坐标系下，减去加减速造成的非匀速畸变的值，去除点云产生的 位移畸变。

Δ_X＝X_i-X_s-V_sx×t_i

Δ_Y＝Y_i-Y_s-V_sy×t_i

Δ_Z＝Z_i-Z_s-V_sz×t_i (4)

(2)地面点分割：

无人平台在许多环境中运动时，地面并不是平坦的，并且存在倾斜的地形， 为了便于后续点云特征提取与配准，需要提取雷达点云的地面点，并对其余非 地面点分类。激光雷达第k次扫描获得的点云是P_k＝{p₁，p₂，...p_n}，将点云进行畸 变校正后，任意一点在第k次扫描初始时刻雷达坐标系中的坐标为p_i＝{x_i，y_i，z_i}。 如图5所示，以雷达坐标系0-XYZ的原点为圆心，将0XY平面划分成M个相等 的扇形S₁，S₂，...，S_M，每个扇形的圆周角α＝2π/M，并且依据雷达点与X轴的角度 将点云投影到不同的扇形分区。P_s表示映射到分区S_s的所有点的集合： P_s＝{p_i∈P|arctan(y_i，x_i)/α＝s}。

为了得到适合估计地面的点集，将同一分区等间隔划分为n段：

j＝1，2，…，n，

和

表示b_j半径范围的最大和最小值。如果点p_i满足

则 将点p_i映射到

区域内，使用

来表示

的所有点的集合。如图6所示，对于任 意一个分区S_s，将3D点集

映射到2D点集

可能包含多个点，也可能没有点。对于每个非空的点集

选择坐标z 最小的一个点为该集合的代表点

因为该点最有可能位于地平面上，因此使 用z坐标最小的点会对后续的直线拟合产生良好的结果。如图7雷达扫描示意 图所示。代表点

将3D点转换成2D点，提供了适合线性拟合的条件。对于每 个扇形分区，利用每个分段的代表点

应用增量算法进行线性拟合来估计地面， 拟合过程中直线y＝mx+b为地平面必须满足的条件是：斜率m不得超过阈值T_m， 即地平面不应该是垂直结构。并且对于斜率m＜T_{m_small}，直线的截距b的绝对值不 得超过某个阈值T_b，这个条件可以将高原排除。对满足上述斜率m和截距b的点， 使用最小二乘法进行线性拟合。当前区域S_s拟合的地平面线为L_s，对P_s中的每个 点计算它到L_s之间的距离，如果该距离低于阈值T_ground，则将该点记为地面，若 高于阈值T_ground，则记为非地面。

在标记了所有地面点后，下一步是将非地面点分组为若干类。通过简单的 点云聚类实现这一点，去除了少于30个点的聚类，同一聚类的点被分配相同的 标签，而地面点是一种特殊的聚类。假设无人平台在嘈杂的环境中，小的物体 (例如树叶)可能形成琐碎且不可靠的特征，因此将地面点分割方法应用于点 云可以在后面的特征提取与配准过程中提高精度和处理效率。图8(a)和(b) 分别表示原始点云和地面点分割后的点云，内圆代表地面点，外圆代表非地面 点。

作为优选的实施方式，所述步骤11的所述点云特征提取包括：

定义一帧扫描点云为P_k＝{p₁,p₂,...,p_n}，对P_k中的点提取边缘特征和平面特征。设p_i是P_k中的一点，在当前雷达坐标系下点p_i的坐标表示为

S是一次扫描 过程中扫描到的点，S包含的点平均分布在p_i的两侧，将S设置为10，计算p_i与 其相邻的10个点所构成的平面在该点处的曲率c。

将一次扫描中获得的点进行遍历求取点云曲率，按照曲率c值大小进行排 序，使用阈值c_th来判断不同类型的特征点。c＞c_th的点即为边缘特征，c＜c_th的点 即为平面特征。在点云预处理时，已经将点云标记为地面点和非地面点，因此 只对地面点提取平面特征F_p，只对非地面点提取边缘特征F_e，基于地面点分割的 特征提取方法更高效、更准确。同时在点云预处理时，已经按照不同的扫描线 将点云分类，分别保存到不同的点云指针中。为了防止特征点聚集，把每个扫 描线平均分成6段，每一段按照曲率的升序排列，根据阈值分别提取边缘点和 平面点，将特征点平均分配到环境中。

优选的，所述步骤11的所述点云特征匹配包括：

雷达里程计算法能够估计一次扫描过程中激光雷达的运动。设t_k为第k次扫 描的起始时刻。在每次扫描的结束时刻，本次采集的点云P_k将会根据雷达里程计 结果投影到时间t_k+1下，投影后的点云为

如图9所示。设第k+1次扫描的点 云为P_k+1，利用上一小节的点云特征提取分别获得P_k和P_k+1中的边缘特征点和平 面特征点。

和

为第k次扫描的边缘特征和平面特征，投影到扫描结束时刻 为

和

和

分别为第k+1次扫描的边缘特征和平面特征。

在第k+1次扫描起始时P_k+1为空，随着扫描的进行P_k+1中的点云不断增加， 同时激光雷达里程计也在不断估计扫描过程中的雷达的姿态。在每次迭代过程 中根据目前估计的姿态变换将

和

投影到这次扫描的起始时刻t_k+1，

和

表示投影后的特征点集，用于与上一帧数据进行匹配。对

和

中的每 个点，在对应的

和

中寻找与其最相近的点。下面建立这两个特征点集的一 一对应关系，完成特征点云的匹配工作。

图10(a)表示寻找边缘点对应的边缘线。设

找到边缘点p_i对应的 上一帧数据中最近的两个点p_j和p_l，并且这两个点都属于

同时要求p_j和p_l是不同扫描线上的两个点。则p_j和p_l组成了边缘点p_i在上一帧的对应线。图10 (b)表示寻找平面点对应的特征平面。设

它在上一帧数据中的对应用 三个点所构成的平面表示。首先找到平面点p_i对应上一帧数据最近点p_j，并在 该扫描线上找到次近点p_l，再在相邻扫描线上找最近点p_m，这三个点都属于

并且保证三点不在一条线上组成一个平面。

作为优选的实施方式，所述步骤11的所述建立约束方程包括：找到特征点 对应的边缘线或对应平面后，下一步找到前后两帧点云的对应关系，建立点云 的约束方程。如图11(a)所示，(p_j,p_l)是边缘点p_i的对应线，点到线的距离可 以用式(7)表示：

其中

分别为点p_i，pj，p_l在当前雷达坐标系下的坐标。 如图11(b)所示，(p_j,p_l,p_m)是平面点p_i对应的特征平面。点到面的距离就可由 式(8)求得，其中

为点p_m在当前雷达坐标系下的坐标。

在建立了两帧特征点云的对应关系后，下面将通过特征点距离最小化来估 计激光雷达的运动。如上图11所示，在第k+1次扫描时，设t为当前时间点， 假设

是[t_k+1，t]之间激光雷达的姿态变换矩阵，

包含激 光雷达6自由度刚体运动的信息，其中t_x,t_y,t_z为雷达坐标系x,y,z轴方向的平移， θ_x,θ_y,θ_z为x,y,z轴方向的旋转角度。给定一个点p_i∈P_k+1，它对应的时间点为t_i，

为[t_k+1，t_i]时间段的姿态变换矩阵，对

进行线性插值求出

利用公式(9)可以得出：

其中R是根据罗德里格斯方程定义的旋转矩阵：

R＝e^ω'θ＝I+ω'sinθ+ω'(1-cosθ) (11)

其中θ是旋转向量的模：

其中ω是旋转方向的单位向量，ω'是ω的反对称矩阵：

根据式(7)和(9)-(13)，可以建立下面的方程：

根据式(8)和(9)-(13)，可以建立下面的方程：

如图12所示，利用两步L-M算法来估计雷达的运动姿态。通过匹配从地面 点提取的平面特征来获得t_z,θ_x,θ_y，使用t_z,θ_x,θ_y作为约束条件，利用从非地面点 提取的边缘特征来估计t_x,t_y,θ_z。首先求f_p关于

的雅克比矩阵，记为J_p，通 过非线性迭代使d_p趋近于0解出t_z,θ_x,θ_y，其中λ是L-M算法中的一个参数。

使用上一步获得的结果

作为约束，对式(14)求f_e关于

的雅克比 矩阵，记为J_e，通过非线性迭代使d_e趋近于0解出t_x,t_y,θ_z。

雷达里程计算法的输入为第k次扫描得到的点云

当前扫描得到不断增 长的点云P_k+1、上一次迭代所得的姿态变换矩阵

如果是扫描的开始时刻， 那么

设为0。里程计算法从P_k+1中提取平面特征点和边缘特征点，在

中寻 找对应点，计算每一个特征点的最小二乘权值，使用L-M的方法进行优化，运 动估计过程是一个稳健的拟合过程。通过一次迭代更新姿态变换矩阵

如果 迭代结果趋于收敛，或者达到了最大迭代次数，则非线性优化过程结束。当里 程计算法达到扫描的结束时刻，P_k+1通过姿态变换被投影到时间点t_k+2上，为下 一次扫描做准备。

作为优选的实施方式，所述步骤12包括：

步骤121，雷达建图优化

雷达里程计能够完成点云数据特征点提取、配准和位姿估计等功能，可以 产生相对粗糙的里程计信息，能够粗略地估计出雷达的位姿。但在实际应用中， 雷达里程计会受到测量测噪声的影响，导致位姿估计结果产生一定的漂移。雷 达里程计当前估计的结果若不准确，后续结果依赖当前结果，误差将会不断累 积，点云地图不断发散。由于点云特征的提取仅依赖曲率信息，并且上一帧点 云中的离散点无法保证在下一帧中仍然被采集到。因此，需要采用其他方法优 化雷达里程计估计的位姿。由于与已经建立地图匹配的准确度要高于帧间的匹 配，因此使用与已建立地图进行匹配的方式来优化位姿。

建图过程无需高速的更新频率，因此使用建图方法进行位姿优化的过程在 低频(1HZ)下进行。如图13所示，在第k次扫描的结束时刻，定义Q_k为世界 坐标系下已建立的地图，蓝色曲线

为此时激光雷达在世界坐标系下的姿态。 第k+1次扫描的末时刻，激光里程计产生一帧点云

将

映射到世界坐标 系中，记为

同时生成一个姿态变换矩阵

黄色曲线表示[t_k+1，t_k+2]时间内 激光雷达坐标系下的位姿变化，利用里程计算法的输出，在第k+1次扫描周期 中将

扩展成

地图构建算法通过

与Q_k特征点云配准将激光雷达的姿 态

最优化。

特征点的提取仍然使用之前所述的方法，但是点云的提取数量是原来的10 倍。为了寻找特征点的对应关系，将点云地图Q_k保存在边长为10米的立方体内， 并将其分割成边长为10cm的子立方体，将立方体中与当前帧点云

有重叠的 点保存在KD树中，将

中的边缘点和平面点分别映射到每个小立方体内，匹 配过程是在特征点附近宽为10cm的子立方体邻域内搜索。因此不需要对庞大的 地图点云进行处理，只需要处理这些邻域子立方体内的地图特征点即可，可以 节省大量的运算资源。

利用KD树来寻找子立方体内与特征点最邻近的5个点，设S为特征点周围 点集。构建这5个点在(x,y,z)方向的3×3协方差矩阵M，求解出M的特征值V和 特征向量E，利用主成份分析法来判断是否能拟合成直线或平面。

其中协方差:

c_ij＝Cov(X_i,X_j)＝E{[X_i-E(X_i)][X_j-E(X_j)]} (21)

如果S分布在一条边缘线上，特征值V中包含元素显著大于其余两个，E中 与该特征值相关的特征向量表示边缘线的方向。如果S分布在一个平面上，特 征值V中包含元素存在一个显著小的元素，并且E中与该特征值相关的特征向 量表示平面的法线方向。因此根据特征向量找到直线上两点或者平面上三点， 从而利用前文中点线距离、点面距离公式构建雅克比矩阵，利用L-M算法非线 性优化求解雷达位姿

步骤122，回环检测和位姿图优化；

随着无人平台行进过程运动轨迹不断增加，位姿估计产生的误差会逐渐累 加，通常使用回环检测的方法来去除误差，提升位姿估计结果。雷达里程计的 前端实现了特征点提取、匹配、位姿估计以等功能，而后端的回环检测负责对 所有这些数据进行优化。如果雷达里程计仅考虑相邻时间上的关邻，那么之前 的误差将不可避免地累积到下一时刻，这样整个过程会出现累积误差，长期的 结果将不可靠。回环检测是指无人平台识别曾经到达过的场景，并与之前的某 一帧建立位姿约束关系，从而修正之前对方位的判断，起到了减小累计误差的 作用。如图14所示，(a)表示无回环，(b)表示局部回环，(c)表示大回环。 本发明研究过程中的回环检测是基于位置也就是里程计的几何关系来实现的， 在位姿估计过程中不断保存雷达的轨迹，当前雷达运动到了某个位置时，寻找 附近5m内的历史轨迹，检测它们是否有回环关系。

如图15所示，图优化是解决回环检测和位姿优化问题的主流方法，通常会 把常规的问题通过构建图的形式来表述。图G＝{V,E}由顶点V和边E组成。根据 雷达姿态构建图，激光雷达每个关键帧的传感器位姿v_i作为图中的顶点，而不同 时刻位姿之间的关系构成边e_ij。在激光雷达运动过程中，通过不断累积而成的顶 点v_i和边构e_ij成图结构，图优化的目标就是通过调整顶点的位姿最大可能的满足 边之间的约束。

如果当前帧位姿与之前的位姿匹配成功，那么检测到回环，则针对位姿图 进行优化，调整机器人位姿顶点尽量满足边的约束。由于边T_ij存在误差，当出现 回环时，所有边给出的数据并不一致，因此可以优化一个不一致误差E：

其中x_i是优化变量，在优化过程中根据误差E对x的梯度调整x的值使得E 不断缩小。

通过摄像机扫描得到地物的影像数据，将影像与点云数据融合，可以提供 具有真实色彩的点云，大大提高点云可视化的效果，便于判读，还可以帮助后 续三维建模进行纹理映射，甚至还能直接在影像上进行测量。因此，在三维数 据的获取中，将影像与点云数据融合，获取每一个点的真实颜色纹理是三维激 光扫描技术中的一个重要需求。

作为优选的实施方式，所述步骤3包括：多传感器的信息融合技术最早来 源于声纳信号处理系统，信息融合技术根据系统中众多传感器采集的信息，获 取信息之间的关联，最后完成多传感器信息的有效融合。

本发明采用数据级别的融合方法，数据级别的融合方法难度非常大，需要 解决两个关键性的问题，一是获取传感器之间准确的位置关系，实现传感器对 环境观测信息的准确融合。二是传感器采集的数据需要进行时间同步，并且激 光雷达在行进过程中的扫描需要补偿自我运动。

其中：空间同步包括：

三维激光雷达和摄像机的空间同步，即建立它们的空间位置关系。可以通 过对两个传感器进行联合标定获取摄像机与激光雷达外参，在对摄像机和激光 雷达联合标定前需要获取摄像机的内参(f_u,f_v,u₀,v₀)，因此先对摄像机进行标定。

本发明获取摄像机的内参的方法为使用移动棋盘格的摄像机标定方法。基 本原理是：使用摄像机拍摄n幅棋盘格的照片，提取棋盘格角点坐标，计算单 应性矩阵以及内参和外参矩阵，构造似然函数使用L-M算法进行迭代，对摄像 机的径向畸变和切向畸变进行优化。不受棋盘格的运动影响，就可以直接获得 摄像机的内参(f_u,f_v,u₀,v₀)和畸变参数(k₁,k₂,k₃,p₁,p₂)。

如图16所示，点云三维坐标系为(X_l,Y_l,Z_l)，摄像机三维坐标系为(X_c,Y_c,Z_c)， (R,t)表示激光雷达和摄像机之间的外参矩阵。联合标定过程使用开源的工具实 现。首先利用摄像机拍摄标定板，提取角点的像素坐标；利用激光雷达提取标 定板平面，在平面上提取四条边，计算标定板四个顶点的三维坐标，从而推算 出标定板角点的三维坐标。根据不同姿态下标定板平面，可得到一系列的线性 方程，计算出摄像机相对激光雷达的旋转向量和平移向量，解得标定参数。

作为优选的实施方式，所述时间同步包括：

在多种传感器采集的数据实现融合时，需要获取同一时刻激光雷达和摄像 机采集的数据。鉴于每个传感器获取数据的时间不统一，需要将传感器数据进 行时间同步后才能实现融合。激光雷达和摄像机的时间同步通常分为硬件同步 和软件同步，硬件同步是在摄像机上安装一个触发器，当激光雷达扫描一周时 触发器触发，拍摄一帧图像。考虑到硬件问题和触发拍摄与读取数据的时间延 迟问题，利用软件进行时间同步。ROS系统在订阅不同的需要融合的传感器的主 题时，可以记录采集时的时间戳。在研究过程中将摄像机和雷达设置成相同频 率(10HZ)，IMU设置成较大的频率(50HZ)，通过时间同步机制对点云节点、 摄像机节点、IMU节点的时间戳进行比较，若时间戳小于一定的阈值，则统一接 收这几个主题，并产生一个同步结果的回调函数，在回调函数里对时间同步后 的几种数据进行融合。

实施例二，

一种救援机器人智能化远程人机交互控制系统，包括：

无人平台的位姿及优化子系统，用于利用三维激光雷达和惯性测量单元IMU 传感器，在无人平台行进过程中估计无人平台的位姿；其中基于所述三维激光 雷达进行位姿估计，然后使用惯性测量单元IMU传感器获得一次运动前的信息， 辅助描述高频率下无人平台的运动，提高姿态估计的准确度；

单目摄像机，所述单目摄像机安装在所述三维激光雷达正上方，自身进行 360°旋转，在较短的时间间隔内环形采集周围场景的图像信息，利用基于SIFT 特征匹配的图像拼接算法，在不降低图像分辨率的条件下把拍摄的照片序列进 行空间匹配对准，构造一幅包含图像序列信息的全景图像。

点云和图像获取子系统，基于获取的点云数据与所述全景图像，利用时间 同步和空间同步方法，获得同一时刻的一帧点云和一帧图像；

联合标定子系统，用于联合标定，并根据联合标定结果确定所述三维激光 雷达和所述单目摄像机两个坐标系的位置关系以及所述单目摄像机的内参和畸 变参数。

三维地图模型构建及优化子系统，用于构建无人平台周围局部环境的三维 地图模型并且进行优化，包括：

救援机器人智能化远程人机交互控制子系统，用于基于优化的三维地图模 型进行救援机器人智能化远程人机交互控制。

该系统可实现上述实施例一提供的控制方法，具体的实现方法可参见实施 例一中的描述，在此不再赘述。

本发明还提供了一种存储器，存储有多条指令，所述指令用于实现如实施 例一所述的方法。

如图17所示，本发明还提供了一种电子设备，包括处理器301和与所述处 理器301连接的存储器302，所述存储器302存储有多条指令，所述指令可被所 述处理器加载并执行，以使所述处理器能够执行如实施例一所述的方法。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基 本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要 求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。显然， 本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和 范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术 的范围之内，则本发明也意图包含这些改动和变型在内。

Claims (10)

1.一种救援机器人智能化远程人机交互控制方法，其特征在于，包括：

步骤1，利用三维激光雷达和惯性测量单元IMU传感器，在无人平台行进过程中估计无人平台的位姿；其中基于所述三维激光雷达进行位姿估计，然后使用惯性测量单元IMU传感器获得一次运动前的信息，辅助描述高频率下无人平台的运动，提高姿态估计的准确度；

步骤2，利用安装在所述三维激光雷达正上方的单目摄像机自身进行360°旋转，在较短的时间间隔内环形采集周围场景的图像信息，将拍摄的照片序列进行空间匹配对准，构造一幅包含图像序列信息的全景图像；

步骤3，基于获取的点云数据与所述全景图像，利用时间同步和空间同步方法，获得同一时刻的一帧点云和一帧图像，同时联合标定结果确定所述三维激光雷达和所述单目摄像机两个坐标系的位置关系以及所述单目摄像机的内参和畸变参数；

步骤4，构建无人平台周围局部环境的三维地图模型并且进行优化；

步骤5，基于优化的三维地图模型进行救援机器人智能化远程人机交互控制。

2.根据权利要求1所述的一种救援机器人智能化远程人机交互控制方法，其特征在于，所述基于所述三维激光雷达进行位姿估计包括：

步骤11，利用雷达里程计在高频率、低保真度下进行点云特征提取、点云特征匹配以及建立约束方程来估计所述三维激光雷达的位姿；

步骤12，在低频率、高精度下使用点云与已建立地图匹配并结合回环检测的方法，实现所述雷达里程计以及所述位姿的优化。

3.根据权利要求1所述的一种救援机器人智能化远程人机交互控制方法，其特征在于，所述步骤2包括：利用基于SIFT特征匹配的图像拼接算法，在不降低图像分辨率的条件下将拍摄的照片序列进行空间匹配对准，构造一幅包含图像序列信息的全景图像。

4.根据权利要求1所述的一种救援机器人智能化远程人机交互控制方法，其特征在于，所述步骤4包括：

步骤41，根据所述位置关系、内参预计畸变参数，将所述三维激光雷达三维坐标系下的点投影至二维像素平面，获得所述二维像素平面相应位置的颜色信息；

步骤42，利用激光雷达估计的无人平台位姿，将具有彩色信息的单帧点云转换到世界坐标系下，与已经建立的地图叠加，实时构建无人平台周围局部环境的三维地图模型；

步骤43，利用高阶条件随机场对已建立所述三维地图模型进行更新优化，提升所述三维地图模型的精度。

5.根据权利要求1所述的一种救援机器人智能化远程人机交互控制方法，其特征在于，所述步骤1实施前还包括：对点云进行预处理，所述预处理包括运动畸变校正和地面点分割；其中：

所述运动畸变校正包括：

根据扫描到的点云角度，按照不同的扫描线将其分类，分别保存到不同的点云指针中；通过计算每一个点的俯仰角ω来确定其所属的线，对俯仰角四舍五入，根据俯仰角排列激光线号；对初始和结束时刻两个位置的雷达坐标系进行转化，对激光雷达点云进行运动畸变消除；使用其他传感器获得运动速度、位移信息去除运动畸变，或者使用IMU直接测量机器人的角度、位移和速度后使用线性插值去除运动畸变；在点云畸变校正前，首先记录点云起始位置的速度(V_sx,V_sy,V_sz)，位移(X_s,Y_s,Z_s)和角度(Roll_s,Pitch_s,Yaw_s)，在获取点云的时间t_i后，使用IMU数据线性插值求解出当前点p_i对应的速度(V_ix,V_iy,V_iz)，位移(X_i,Y_i,Z_i)和角度(Roll_i,Pitch_i,Yaw_i)，计算当前点p_i相对于第一个点由于运动产生的位移畸变，并对点云中的每个点位置信息重新补偿校正消除运动畸变；所述畸变校正过程分为三步：首先计算当前时刻在世界坐标系下由于加减速造成的位移畸变(Δ_X,Δ_Y,Δ_Z)；然后将运动畸变值绕Y、X、Z轴旋转后得到起始点坐标系下的运动畸变；最后将当前点p_i先转换到世界坐标系下，再由世界坐标转换到点云起始点坐标系下，减去加减速造成的非匀速畸变的值，去除点云产生的位移畸变；

所述地面点分割包括：标记所有地面点；将非地面点分组为若干类，同一聚类的点被分配相同的标签，而地面点是一种特殊的聚类。

6.根据权利要求2所述的一种救援机器人智能化远程人机交互控制方法，其特征在于，所述步骤11的所述点云特征提取包括：将一次扫描中获得的点进行遍历求取点云曲率，按照曲率c值大小进行排序，使用阈值c_th来判断不同类型的特征点，c＞c_th的点即为边缘特征，c＜c_th的点即为平面特征；在点云预处理时，已经将点云标记为地面点和非地面点，只对地面点提取平面特征F_p，只对非地面点提取边缘特征F_e；同时在点云预处理时，已经按照不同的扫描线将点云分类，分别保存到不同的点云指针中；把每个扫描线平均分成6段，每一段按照曲率的升序排列，根据阈值分别提取边缘点和平面点，将特征点平均分配到环境中；

所述步骤11的所述点云特征匹配包括：

设t_k为第k次扫描的起始时刻。在每次扫描的结束时刻，本次采集的点云P_k将会根据雷达里程计结果投影到时间t_k+1下，投影后的点云为

设第k+1次扫描的点云为P_k+1，分别获得P_k和P_k+1中的边缘特征点和平面特征点，

和

为第k次扫描的边缘特征和平面特征，投影到扫描结束时刻为

和

和

分别为第k+1次扫描的边缘特征和平面特征；在第k+1次扫描起始时P_k+1为空，随着扫描的进行P_k+1中的点云不断增加，同时激光雷达里程计也在不断估计扫描过程中的雷达的姿态；在每次迭代过程中根据目前估计的姿态变换将

和

投影到这次扫描的起始时刻t_k+1，

和

表示投影后的特征点集，用于与上一帧数据进行匹配；对

和

中的每个点，在对应的

和

中寻找与其最相近的点；建立这两个特征点集的一一对应关系，完成特征点云的匹配工作；

所述步骤11的所述建立约束方程包括：找到特征点对应的边缘线或对应平面后，找到前后两帧点云的对应关系，建立点云的约束方程，并通过特征点距离最小化来估计所述三维激光雷达的运动；其中利用两步L-M算法来估计所述三维激光雷达的运动姿态，计算每一个特征点的最小二乘权值，通过一次迭代更新姿态变换矩阵，如果迭代结果趋于收敛，或者达到了最大迭代次数，则非线性优化过程结束。当里程计算法达到扫描的结束时刻，当前位置通过姿态变换被投影到下一个时间点上，为下一次扫描做准备。

7.根据权利要求1所述的一种救援机器人智能化远程人机交互控制方法，其特征在于，所述步骤12包括：

步骤121，雷达建图优化，使用与已建立地图进行匹配的方式来优化位姿；

步骤122，回环检测和位姿图优化；所述回环检测是指无人平台识别曾经到达过的场景，并与之前的某一帧建立位姿约束关系，从而修正之前对方位的判断，起到了减小累计误差的作用，所述回环检测是基于位置也就是里程计的几何关系来实现的，在位姿估计过程中不断保存雷达的轨迹，当前雷达运动到了某个位置时，寻找附近5m内的历史轨迹，检测它们是否有回环关系；所述位姿图优化包括：根据雷达姿态构建图，激光雷达每个关键帧的传感器位姿v_i作为图中的顶点，而不同时刻位姿之间的关系构成边e_ij。在激光雷达运动过程中，通过不断累积而成的顶点v_i和边构e_ij成图结构，图优化的目标就是通过调整顶点的位姿最大可能的满足边之间的约束；如果当前帧位姿与之前的位姿匹配成功，那么检测到回环，则针对位姿图进行优化，调整机器人位姿顶点尽量满足边的约束。

8.根据权利要求1所述的一种救援机器人智能化远程人机交互控制方法，其特征在于，所述步骤3所述空间同步包括：

三维激光雷达和摄像机的空间同步，即建立它们的空间位置关系，通过对两个传感器进行联合标定获取摄像机与激光雷达外参，在对摄像机和激光雷达联合标定前需要获取摄像机的内参(f_u,f_v,u₀,v₀)，因此先对摄像机进行标定；获取摄像机的内参的方法为使用移动棋盘格的摄像机标定方法，其基本原理是：使用摄像机拍摄n幅棋盘格的照片，提取棋盘格角点坐标，计算单应性矩阵以及内参和外参矩阵，构造似然函数使用L-M算法进行迭代，对摄像机的径向畸变和切向畸变进行优化。不受棋盘格的运动影响，就可以直接获得摄像机的内参(f_u,f_v,u₀,v₀)和畸变参数(k₁,k₂,k₃,p₁,p₂)。

9.根据权利要求1所述的一种救援机器人智能化远程人机交互控制方法，其特征在于，所述步骤3所述时间同步包括：

所述三维激光雷达和摄像机的时间同步通常分为硬件同步和软件同步，硬件同步是在摄像机上安装一个触发器，当激光雷达扫描一周时触发器触发，拍摄一帧图像；将摄像机和雷达设置成相同频率(10HZ)，IMU设置成较大的频率(50HZ)，通过时间同步机制对点云节点、摄像机节点、IMU节点的时间戳进行比较，若时间戳小于一定的阈值，则统一接收这几个主题，并产生一个同步结果的回调函数，在回调函数里对时间同步后的几种数据进行融合。

10.一种实施如权利要求1-9任一所述救援机器人智能化远程人机交互控制方法的系统，其特征在于，包括：

无人平台的位姿及优化子系统，用于利用三维激光雷达和惯性测量单元IMU传感器，在无人平台行进过程中估计无人平台的位姿；其中基于所述三维激光雷达进行位姿估计，然后使用惯性测量单元IMU传感器获得一次运动前的信息，辅助描述高频率下无人平台的运动，提高姿态估计的准确度；

单目摄像机，所述单目摄像机安装在所述三维激光雷达正上方，自身进行360°旋转，在较短的时间间隔内环形采集周围场景的图像信息，利用基于SIFT特征匹配的图像拼接算法，在不降低图像分辨率的条件下把拍摄的照片序列进行空间匹配对准，构造一幅包含图像序列信息的全景图像。

点云和图像获取子系统，基于获取的点云数据与所述全景图像，利用时间同步和空间同步方法，获得同一时刻的一帧点云和一帧图像；

联合标定子系统，用于联合标定，并根据联合标定结果确定所述三维激光雷达和所述单目摄像机两个坐标系的位置关系以及所述单目摄像机的内参和畸变参数。

三维地图模型构建及优化子系统，用于构建无人平台周围局部环境的三维地图模型并且进行优化，包括：

救援机器人智能化远程人机交互控制子系统，用于基于优化的三维地图模型进行救援机器人智能化远程人机交互控制。

Images