具体实施方式

下面将结合本申请实施例中的附图，对本申请实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本申请一部分实施例，而不是全部的实施例。基于本申请中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本申请保护的范围。

需要说明的是，本申请的说明书和权利要求书及上述附图中的术语“第一”、“第二”等是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的本申请的实施例。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

为了使本领域的技术人员更好的了解本申请，下面先对本申请实施例中出现的部分技术术语进行解释如下：

可移动物体：是指车辆、移动机器人、飞行器等可进行地图采集的物体，可移动物体上可以搭载各类型传感器，如激光雷达、相机等。

ICP：Iterative Closest Point，迭代最近点算法，主要用于计算机视觉中深度图像的精确拼合，通过不断迭代最小化源数据与目标数据对应点来实现精确地拼合。目前已经有很多变种，主要热点是怎样高效、鲁棒的获得较好地拼合效果。

GNSS：Global Navigation Satellite System，全球导航卫星系统。

GPS：Global Positioning System，全球定位系统。

IMU：Inertial Measurement Unit，惯性测量单元，是测量物体三轴姿态角(或角速率)以及加速度的装置。

高精度地图：不同于传统的导航地图，高精度地图包含大量的驾驶辅助信息，最重要的信息是依托道路网的精确三维表征，例如交叉路口布局和路标位置等。另外，高精度地图还包含很多语义信息，地图上可能会报告通信交通灯上不同颜色的含义，它可能指示道路的速度限制，以及左转车道开始的位置等。高精地图最重要的特征之一是精度，高精度地图能使车辆达到厘米级的精度，这对确保自动驾驶汽车的安全至关重要。

建图(Mapping)：根据估计得到的车辆或移动机器人实时位姿以及激光雷达等视觉传感器的采集数据，构建出描述当前场景的高精度地图。

位姿(Pose)：位置和朝向的总称，包含6个自由度，其中包括3个位置自由度和3个朝向自由度。朝向的3个自由度通常用俯仰角(Pitch)、翻滚角(Roll)、偏航角(Yaw)来表示。

帧(Frame)：传感器完成一次观测所接收到的测量数据，如相机的一帧数据为一张图片，激光雷达的一帧数据为一组激光点云。

子地图(Submap)：全局地图由若干个子地图组成，每个子地图包含连续多帧的观测结果。

配准(Registration)：对同一区域在不同时刻、不同位置的观测结果进行匹配，得到两个观测时刻间的相对位姿关系。

NDT：Normal Distributions Transform，即正态分布变换算法，是一种配准算法，它应用于三维点的统计模型，使用标准最优化技术来确定两个点云间的最优的匹配。

NovAtel：精密全球导航卫星系统(GNSS)及其子系统领域中，处于领先地位的产品与技术供应商。本申请实施例中表示NovAtel的组合导航系统。

LOAM：LiDAR odometry and mapping，激光测距与测绘。

KD-tree：K-Dimensional树，是一种分割k维数据空间的数据结构。主要应用于多维空间关键数据的搜索(如：范围搜索和最近邻搜索)。

SVD：Singular Value Decomposition，奇异值分解，是线性代数中一种重要的矩阵分解。

LIO-Mapping：Lidar Inertial Odometry and Mapping，激光雷达惯性里程测量与测绘。

里程计(Odometry)：一种利用从可移动物体的传感器获得的数据来估计物体位姿的方法。

在本申请的一些实施例中，术语“车辆”广泛地解释为包括任何移动物体，包括例如飞行器、船只、航天器、汽车、卡车、厢式货车、半挂车、摩托车、高尔夫球车、越野车辆、仓库运输车辆或农用车以及行驶在轨道上的运输工具，例如电车或火车以及其它有轨车辆。本申请中的“车辆”通常可以包括：动力系统、传感器系统、控制系统、外围设备和计算机系统。在其它实施例中，车辆可以包括更多、更少或者不同的系统。

其中，动力系统是为车辆提供动力运动的系统，包括：引擎/马达、变速器和车轮/轮胎、能源单元。

控制系统可以包括控制车辆及其组件的装置的组合，例如转向单元、节气门、制动单元。

外围设备可以是允许车辆与外部传感器、其它车辆、外部计算设备和/或用户进行交互的设备，例如无线通信系统、触摸屏、麦克风和/或扬声器。

基于上述描述的车辆，自动驾驶车辆中还配置有传感器系统和自动驾驶控制装置。

传感器系统可以包括用于感测车辆所处环境的信息的多个传感器，以及改变传感器的位置和/或方向的一个或多个致动器。传感器系统可以包括全球定位系统传感器、惯性测量单元、无线电检测和测距(RADAR)单元、相机、激光测距仪、光检测和测距(LIDAR)单元和/或声学传感器等传感器的任何组合；传感器系统还可以包括监视车辆内部系统的传感器(例如O₂监视器、燃油表、引擎温度计等)。

自动驾驶控制装置可以包括一个处理器和存储器，存储器中存储有至少一条机器可执行指令，处理器执行至少一条机器可执行指令实现包括地图引擎、定位模块、感知模块、导航或路径模块、以及自动控制模块等的功能。地图引擎和定位模块用于提供地图信息和定位信息。感知模块用于根据传感器系统获取到的信息和地图引擎提供的地图信息感知车辆所处环境中的事物。导航或路径模块用于根据地图引擎、定位模块和感知模块的处理结果，为车辆规划行驶路径。自动控制模块将导航或路径模块等模块的决策信息输入解析转换成对车辆控制系统的控制命令输出，并通过车载网(例如通过CAN总线、局域互联网络、多媒体定向系统传输等方式实现的车辆内部电子网络系统)将控制命令发送给车辆控制系统中的对应部件，实现对车辆的自动控制；自动控制模块还可以通过车载网来获取车辆中各部件的信息。

目前，在自动驾驶领域中常用的激光里程计方法和激光惯性里程计方法，在一些特征稀疏、GPS信号较差的场景(如隧道、跨海大桥以及沙漠戈壁等)中，通常会出现退化，无法进行准确的位姿估计，随着时间的推移可能出现位姿估计的结果丢失的情况，不能满足自动驾驶中对于高精度地图构建的需求。

本申请实施例旨在提出一种基于多传感器融合的里程计方法及装置，以克服现有技术中自动驾驶常用的激光里程计方法和激光惯性里程计方法在特征稀疏、GPS信号较差的场景中无法准确的进行位姿估计的问题。

如图1所示，本申请实施例提供一种基于多传感器融合的里程计方法，该方法应用于一种搭载有多种传感器的可移动物体上，该方法包括：

步骤101、实时获得可移动物体上搭载的各种传感器采集的传感器数据。

步骤102、对各种传感器采集的传感器数据分别进行建模，建立可移动物体位姿的约束关系。

步骤103、对可移动物体位姿的约束关系进行联合优化求解，确定可移动物体的位姿结果。

为了使本领域的技术人员更好的了解本发明，下面结合具体的实施例来对本申请实施例进行阐述，如图2所示，本申请实施例提供一种基于多传感器融合的里程计方法，该方法应用于一种搭载有多种传感器的可移动物体上，该多种传感器可以包括惯性测量单元IMU、轮速计、激光雷达和气压计；其中，所述IMU包括加速度计和陀螺仪。

该方法包括：

步骤201、实时获得加速度计测量的三轴加速度数据、陀螺仪测量的三轴角速度数据、轮速计测量的可移动物体轮速数据、激光雷达测量的点云数据和气压计测量的高度观测数据。

在步骤201之后，继续执行步骤202至步骤205。

步骤202、根据加速度计测量的三轴加速度数据进行建模，建立可移动物体的横滚角约束和俯仰角约束。

IMU中的加速度计可以实时测量IMU坐标系下的三轴加速度数据，测量得到的三轴加速度数据一般由重力加速度和可移动物体自身加速度两部分组成，但由于可移动物体自身的加速度通常远小于重力加速度，因此可以忽略可移动物体自身加速度的影响。

具体的，此处步骤202可以采用如下方式实现：

根据加速度计测量的三轴加速度数据进行建模。

其中，所建立的数学模型有如下关系：

在以上数学模型中，a_x、a_y、a_z表示加速度计测量的三轴加速度数据；为IMU坐标系到世界坐标系的旋转矩阵；g表示归一化后的重力加速度；a_r表示车体加速度。

通过化简以上数学模型，可以确定IMU在世界坐标系下的横滚角估计值θ_roll和俯仰角估计值θ_pitch；其中，a_x、a_y、a_z表示加速度计测量的三轴加速度数据。

为降低后续步骤中联合优化的自由度，避免里程计方法在隧道、跨海大桥等场景下因特征稀疏而快速退化，本申请提出将上述横滚角估计值θ_roll和俯仰角估计值θ_pitch作为固定约束加入到后续的联合优化过程中。此外，由于联合优化中姿态的状态变量需要用四元数表示，所以需先将四元数转换为旋转矩阵，之后再将旋转矩阵转为欧拉角形式，从而可以根据横滚角估计值θ_roll和俯仰角估计值θ_pitch，建立可移动物体的横滚角约束r_Roll(X)和俯仰角约束r_Pitch(X)；其中，r_Roll(X)＝θ_roll-arcsin(-R₁₃)；r_Pitch(X)＝θ_pitch-arctan2(R₂₃，R₃₃)；X表示IMU在世界坐标系下的位姿，X为待优化的状态变量，X包括位置p和姿态q；R为待优化的状态变量X中的姿态q的旋转矩阵形式，R₂₃、R₃₃、R₁₃分别为旋转矩阵R中对应行列的元素。

步骤203、根据陀螺仪测量的三轴角速度数据和轮速计测量的可移动物体轮速数据，采用阿克曼模型进行运动学建模，建立可移动物体的水平位置和偏航角的阿克曼模型约束。

具体的，此处步骤203可以采用如下方式实现：

本申请可以基于阿克曼模型进行可移动物体的运动学建模。其中，为便于计算，在阿克曼运动学模型中，一般以可移动物体的后轴(例如车辆后轴)的中心为原点建立车体坐标系。

一般情况下，阿克曼运动学模型的默认输入为可移动物体速度和方向盘转角，但在实际应用中，发明人发现方向盘转角的精度通常难以保证，为提高整个里程计方法的精度和鲁棒性，本申请应用世界坐标系下可移动物体前进方向与y轴夹角的角度积分值来替换方向盘转角。因此，此处需要根据陀螺仪测量的三轴角速度数据，确定世界坐标系下可移动物体前进方向与y轴夹角的角度积分值： 其中，θ_i表示第i时刻的可移动物体前进方向与y轴夹角的角度积分值；t表示第t时刻；为预先获知的车体坐标系到IMU坐标系的旋转变换关系；为第t时刻陀螺仪测量的三轴角速度数据中的偏航角。

之后，在阿克曼运动学模型中，可以根据轮速计测量的第i时刻的可移动物体左后轮在车体坐标系下的速度和右后轮在车体坐标系下的速度确定可移动物体后轴中心在车体坐标系下的速度v_i；其中， 为预先获知的速度噪声。

之后，采用阿克曼模型的运动学建模，可以确定世界坐标系下可移动物体位姿传递方程：

X_i+1＝x_i+v_i.Δt·sinθ_i

y_i+1＝y_i+v_i·Δt·cosθ_i

其中，Δt为轮速计两个相邻测量时刻的时间差；x_i、y_i表示可移动物体在世界坐标系下的水平位置。

由于IMU、轮速计的测量频率通常高于激光雷达频率，因此可以根据激光雷达的测量频率，对相邻两个激光雷达第k时刻和第k+1时刻之间的x_i、y_i、θ_i进行积分，确定在世界坐标系下x_i、y_i、θ_i各自的改变量δx_k(k+1)、δy_k(k+1)、δθ_k(k+1)。

之后，可以根据车体坐标系和IMU坐标系之间的外参确定IMU坐标系到车体坐标系的位姿变换关系并确定第k时刻到第k+1时刻之间的IMU在世界坐标系下的位姿变换关系其中：

从而，可以建立可移动物体的阿克曼模型约束r_Akerman(X)；其中：

X表示IMU在世界坐标系下的位姿，X为待优化的状态变量。例如式中，X_k、X_k+1分别为IMU在世界坐标系下在第k、第k+1时刻的位姿。

步骤204、根据激光雷达测量的点云数据进行建模，建立可移动物体的激光雷达位姿约束。

此处，该步骤204可以采用如下方式实现，例如包括如下步骤：

步骤2041、将激光雷达测量的各帧点云数据进行运动补偿，确定各帧点云数据中的点的运动补偿后的位置。

需要进行运动补偿的原因为：激光雷达一般为机械式结构，完成一帧扫描需要一定时间(通常为0.1s或0.05s)，而该段时间内由于可移动物体(例如车辆)高速运动，采集得到的激光雷达原始数据会受运动影响，使得测量值与真实值存在偏差。为减小可移动运动的影响，本申请可以根据上述阿克曼模型估计得到的IMU在世界坐标系下的位姿变换关系对激光雷达测量的原始数据进行运动补偿。由于两次扫描之间的时间间隔很短，所以可假设两帧之间的运动为线性运动，通过时间戳插值即可得到一帧内的激光雷达所采集的点相对于该帧起始时刻的位姿，从而将一帧内的所有激光雷达所采集的点都转换到该帧的起始时刻，确定各点运动补偿后的位置。

步骤2042、对进行运动补偿后的各帧点云数据进行特征提取，根据各帧点云数据中的点的曲率信息，将各帧点云数据中的点划分为线特征点和平面特征点。

该步骤2042具体可以通过如下方式实现：

在进行运动补偿后的一帧点云数据中获得一个线束上的任一点及该线束上的任一点预设范围内的若干点。此处，由于激光雷达测量得到的激光点是按照线束排列的，所以可以按照线束为每一个激光点找到其预设范围内的若干点，如在线束上其左右两侧的若干激光点(如左右两侧各取5个激光点，但不仅局限于此)。

根据该任一点在激光雷达坐标系下的坐标和该线束上的任一点预设范围内的若干点在激光雷达坐标系下的坐标，确定该任一点处的曲率。例如，可以采用如下曲率计算公式来确定该任一点处的曲率：其中，c表示点处的曲率；分别表示当前帧中第k条线上的第i、j个点在激光雷达坐标系下的坐标，S表示第i个点左右两侧若干个点组成的点集，|S|表示该点集所包含点的个数。

根据预先设置的曲率阈值，在一点的曲率大于曲率阈值时，将该一点作为线特征点，在一点的曲率小于曲率阈值时，将该一点作为平面特征点。

步骤2043、将当前帧点云数据之前的预设帧点云数据按照已经进行位姿估计得到的位姿进行叠加，确定当前帧点云数据对应的局部线特征地图和局部面特征地图。

具体的，位姿估计是增量式进行的，因此当前帧之前的各帧点云的线特征点、面特征点及对应的位姿均为已知量，所以可以将当前帧点云数据之前的预设帧点云数据(如15帧点云数据)按照已经进行位姿估计得到的位姿进行叠加，即可得到对应的局部线特征地图(由线特征点组成)和局部面特征地图(由平面特征点组成)。

步骤2044、根据激光雷达和IMU之间的外参，得到当前帧激光雷达在世界坐标系下的初始位姿：

其中，p_LiDAR为当前时刻激光雷达在世界坐标系下的初始位置，p_LiDAR为当前时刻激光雷达在世界坐标系下的初始姿态，R_IMU、t_IMU分别为当前时刻IMU在世界坐标系下的姿态和位置，和分别为预先通过激光雷达和IMU之间的外参标定得到姿态变换关系和位置变换关系。

步骤2045、根据预先采用KD-Tree算法为每个点建立的数据索引，在局部线特征地图中搜索得到当前帧点云数据中每个线特征点对应的若干近邻点，在局部面特征地图中搜索得到当前帧点云数据中每个平面特征点对应的若干近邻点。

步骤2046、根据当前帧点云数据中的线特征点x^l对应的若干近邻点(例如5个)拟合得到一条直线，将线特征点x^l与该直线的距离函数作为线特征点误差函数；

所述线特征点误差函数为：其中，和为该直线上的任意两点。

步骤2047、根据当前帧点云数据中的平面特征点x^p对应的若干近邻点(例如5个)拟合(例如通过SVD分解)得到一个平面Ax+By+Cz+D＝0，将面特征点x^p与该平面的距离函数作为面特征点误差函数。

其中，A、B、C和D表示拟合得到的平面的参数。

所述面特征点误差函数为：其中，n表示矩阵：n＝(A,B,C)。

步骤2048、根据线特征点误差函数和面特征点误差函数，建立可移动物体的激光雷达位姿约束r_LiDAR(X)。

其中：

X表示IMU在世界坐标系下的位姿，X为待优化的状态变量；n_line表示当前帧点云数据中的线特征点数目，n_plane表示当前帧点云数据中的平面特征点数目。

步骤205、根据气压计测量的高度观测数据进行建模，建立可移动物体的高度位置的气压计约束。

具体的，气压计可以通过测量大气压得到当前的海拔高度。虽然温度突变、气流冲击等因素会影响气压计高度测量的绝对精度，但气压计观测的相对精度通常较高。而高度估计精度低一直是目前主流里程计算法的一个突出问题，所以为提高里程计在高度方向的估计精度，降低系统累计误差，本申请实施例中可以采用如下方式：

根据气压计测量的当前时刻高度观测数据Z_k+1、气压计预先测量的初始时刻高度观测数据Z₀、IMU测量的当前时刻在世界坐标系下的高度估计值以及IMU预先测量的初始时刻在世界坐标系下的高度估计值进行建模，建立可移动物体的高度位置的气压计约束r_Altimeter(X)；其中：

X表示IMU在世界坐标系下的位姿，X为待优化的状态变量；分别为预先获知的当前时刻气压计坐标系到世界坐标系的旋转数据和平移数据。

步骤206、对所述横滚角约束、俯仰角约束、阿克曼模型约束、激光雷达位姿约束和气压计约束采用非线性优化方法进行联合优化求解，确定可移动物体的位姿结果。

具体的，此处可以对横滚角约束r_Roll(X)、俯仰角约束r_Pitch(X)、阿克曼模型约束r_Akerman(X)、激光雷达位姿约束r_LiDAR(X)和气压计约束r_Altimeter(X)，采用优化算法对联合优化代价函数求解非线性最小二乘问题，确定可移动物体的IMU在世界坐标系下的位姿结果(即当前待优化的状态变量X的最大后验概率估计)。其中，所采用的优化算法可以为高斯牛顿算法或者Levenberg-Marquardt算法(L-M算法，列文伯格-马夸尔特法)等，但不仅局限于此。

其中，联合优化代价函数为：

其中，分别为各约束项所对应的预先设置的信息矩阵。

这样，通过上述步骤201至步骤206实现的基于多传感器(包括激光雷达、IMU、轮速计和气压计)融合的里程计方法，可以得到相对于激光雷达采集的各帧间准确的相对位姿，能够满足例如隧道、跨海大桥等特征稀疏、GPS信号较差场景下的实时位姿估计，得到的位姿结果准确性和鲁棒性较好。

在本申请的一实施例中，发明人对上述本申请所实现的基于多传感器融合的里程计方法进行了实验验证，过程如下：

为验证本申请所实现的基于多传感器融合的里程计方法的准确性和鲁棒性，本申请实施例中使用装配了激光雷达、IMU、轮速计、气压计等传感器的数据采集车，采集了一段特长隧道数据进行实验验证，该隧道全长约9.2Km，如图3所示，隧道内场景特征稀疏、两侧墙壁均为光滑平面。

在上述图3所示的场景中采用本申请实施例的基于多传感器融合的里程计方法后，结合现有技术中最具代表性的激光里程计算法LOAM以及激光惯导里程计算法LIO-Mapping在相同数据上进行了对比实验。实验结果如图4所示，其中图4的横纵坐标用于表示IMU在世界坐标系下的位姿中的位置信息，Ground-Truth表示位姿的真实值，Sensor-Fusion-Odometry表示本申请实施例的基于多传感器融合的里程计方法。可见，在该实验场景下，LOAM、LIO-Mapping两种算法均发生严重退化，无法跑完全程，数据采集车搭载的IMU在世界坐标系下的位姿丢失，完全没有达到隧道建图的要求；而同样条件下，本发明所提出的基于多传感器融合的里程计方法可以跑完全程，最终得到的位姿估计结果虽然不可避免的存在累计误差，但得到了隧道内各帧间准确的相对位姿，为后续的隧道建图奠定了基础。

另外，如图5所示，本申请实施例提供一种基于多传感器融合的里程计装置，应用于一种搭载有多种传感器的可移动物体上，该基于多传感器融合的里程计装置，包括：

传感器数据获得单元31，用于实时获得可移动物体上搭载的各种传感器采集的传感器数据。

约束关系建立单元32，用于对各种传感器采集的传感器数据分别进行建模，建立可移动物体位姿的约束关系。

联合优化单元33，用于对可移动物体位姿的约束关系进行联合优化求解，确定可移动物体的位姿结果。

另外，本申请实施例还提供一种计算机可读存储介质，包括程序或指令，当所述程序或指令在计算机上运行时，实现上述图1和图2所述的基于多传感器融合的里程计方法。其具体实现过程请见上述方法实施例，此处不再赘述。

另外，本申请实施例还提供一种包含指令的计算机程序产品，当所述计算机程序产品在计算机上运行时，使得所述计算机执行上述图1和图2所述的基于多传感器融合的里程计方法。其具体实现过程请见上述方法实施例，此处不再赘述。

另外，本申请实施例还提供一种计算机服务器，包括存储器，以及与所述存储器通信连接的一个或多个处理器；所述存储器中存储有可被所述一个或多个处理器执行的指令，所述指令被所述一个或多个处理器执行，以使所述一个或多个处理器实现上述图1和图2所述的基于多传感器融合的里程计方法。其具体实现过程请见上述方法实施例，此处不再赘述。

本申请实施例提供的一种基于多传感器融合的里程计方法及装置，通过实时获得可移动物体上搭载的各种传感器采集的传感器数据，进而可以对各种传感器采集的传感器数据分别进行建模，建立可移动物体位姿的约束关系，从而可以对可移动物体位姿的约束关系进行联合优化求解，确定可移动物体的位姿结果。通过本申请实施例可以实现可移动物体在特征稀疏、GPS信号较差的场景中的实时位姿估计，其结果准确，鲁棒性较好。

本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、CD-ROM、光学存储器等)上实施的计算机程序产品的形式。

本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

本申请中应用了具体实施例对本申请的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本申请的方法及其核心思想；同时，对于本领域的一般技术人员，依据本申请的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本申请的限制。