基于约束优化的多传感器车载定位方法

张子腾，盛传贞，蔚保国，王星星，易卿武

(1.中国电子科技集团公司第五十四研究所，河北 石家庄 050081；2.卫星导航系统与装备技术国家重点实验室，河北 石家庄 050081)

0 引言

实时连续、泛在无缝的高精度导航定位是保障各类无人平台安全行进、智能作业的重要基础，坍塌、滑坡和泥石流等城市或山区灾害救援现场，复杂遮蔽地形导致应急救援平台行进过程定位信号失联，精度与稳定性下降。现阶段，能够应用于应急救援的小型化智能作业机器人仅限于其配属的合作空间，例如商场、超市[1]，其导航定位有效范围难以支持大型应急救援装备在突发城市火灾、地质灾害等重大自然灾害现场处置[2]。

视觉特征跟踪受限于大尺度、光照等空旷自然环境；激光雷达场景辨识能力差，难以闭环全局修正；低成本惯性导航累积误差时间漂移；GNSS信号抗干扰能力弱，易受遮挡等[3-5]。发展联合多传感器松/紧耦合定位方法是解决受限环境下高精度导航定位难题的主流手段，另外，在主动式定位方面，相较于视觉相机，激光雷达在精度上具有较大优势。通过激光雷达NDT配准，融合IMU的高精度定位方法虽然具有较高精度，但其计算复杂度较高，应用空间场景受限[6-8]；细化激光雷达点云特征可以提高算法的场景适用能力，然而IMU仅提供畸变矫正并不参与算法的优化过程，松耦合的算法模型难以保持较高的定位精度[9]；紧耦合则通过联合其他传感器进行误差同步优化获得更高的精度，本文多传感器融合方法以GNSS/IMU/激光雷达紧耦合的算法模型为基础[10-12]，可保持较高的车载定位精度。

本文约束优化在于:① 进一步分析车辆通过连续起伏区域受力过程，给出受力方程，约束低成本MEMS高程震荡；② 以RTK解算状态监测筛选有效的GNSS全局解；③ 构建3类传感器全局的联合误差约束，通过全局最小误差获得最终解。

1 传感器量测

1.1 GNSS量测

GNSS量测通过单基准站RTK(Real Time Kinematic)[5]进行解算无人平台位置rG，并手动测量天线杆臂值进行补偿，GNSS天线杆臂是由IMU中心指向GNSS天线相位中心的向量，卫星天线相对于IMU处在前侧、右侧和上侧时，杆臂值l以该向量投影在IMU坐标系下的3个分量l=(lx,ly,lz)T来表示：

rG=ra-l，

(1)

式中，ra为天线杆测量值。已知rS为系统位置测量值，GNSS量测误差约束fG：

fG=min{rS-rG}。

(2)

1.2 IMU预积分

IMU量测计算无人平台t+Δt时刻运动状态速度vt+Δt、位置rt+Δt以及旋转qt+Δt的估计模型为：

(3)

IMU预积分误差约束fB为：

(4)

fB=argmin{δv,δr,δq},

(5)

式中，Δtij表示连续时刻i,j的时间间隔;vi,vj表示间隔Δtij的对应时刻i,j的速度;ri,rj表示间隔Δtij的对应时刻i,j的位置;qi,qj表示间隔Δtij的对应时刻i,j的位姿;R为惯性本体坐标系到世界坐标系的旋转;δv,δr,δq分别表示预积分速度偏差、预积分位置偏差和预积分位姿偏差。

1.3 激光雷达里程计

激光雷达里程计通过计算曲率提取边缘点、平面点特征[13]，执行点到边缘特征、点到平面特征的连续扫描匹配估计传感器运动，de,dh分别表示点到边缘特征、点到平面特征的距离，其中点到边缘的特征约束为距离argmin{de}，点到平面的特征约束为距离argmin{dh}。

(6)

将上述状态转移过程使用6自由度[x,y,z,roll,pitch,yaw]表示，进行欧拉角到旋转矩阵的变换，使用罗德里格斯公式进行变换：

(7)

式中，欧拉角

为旋转方向单位向量，T=[x,y,z]为转动幅度。

激光雷达匹配距离约束fL为：

fL=argmin{∑de+∑dh}，

(8)

式中，∑de,∑dh分别表示当前激光帧所有de,dh的累积和。

2 状态约束

2.1 惯导高程约束

应急救援车辆在高速通过减速带、坑洼和坎坡等起伏地段时，低成本MEMS震荡抖动引起测量数据与车辆实际状态偏离，造成高程方向误差累积快速漂移，通过分析车辆连续起伏过程，给出受力方程，约束MEMS高程震荡，实现对高程量测误差的抑制；系统重力加速度为G，MEMS行进方向加速度为ay，俯仰角为pitch，则垂直方向加速度az：

az=(G-ay·sin(pitch))·cos(pitch)。

(9)

2.2 GNSS状态解约束

GNSS量测通过单基准站RTK获取纬度、经度和高程，如图1所示。

(a) RTK解状态

受遮蔽环境影响，GNSS量测难以保持固定解，利用多系统GPS/BDS信号解状态监测对GNSS量测可用性进行评估，相较于水平方向，RTK在高程方向的漂移更为明显，单独对高程的可用性进行分析，进一步约束全局高程跳变：

① 未解析：解算过程中检测周跳而未解析最终定位结果，GNSS不可用；

② 浮点解：当前接收卫星信号质量可用，已成功解算定位结果，但整周模糊度未固定，此时定位水平精度满足车道级可用，但垂直精度不可用；

③ 固定解：当前接受卫星信号质量良好，已成功解算定位结果，且整周模糊度固定，此时定位精度满足车道级可用。

3 多源融合系统

3.1 系统描述

本文多传感器融合系统构建GNSS/IMU/激光雷达紧耦合的算法模型，如图2所示，并进一步分析车辆通过连续起伏区域受力过程，给出受力方程，约束低成本MEMS高程震荡；以RTK解算状态监测筛选有效的GNSS全局解；构建3类传感器全局的联合误差约束，通过全局最小误差获得最终解。

图2 多传感器融合框架Fig.2 Multi-sensor fusion framework

3.2 坐标转换

首先将IMU坐标系转入雷达坐标系[14]，然后将原始GNSS坐标系(纬度、经度、高程)转为UTM(Universal Transverse Mercator)网格坐标系(xUTM,yUTM,zUTM)，采用扩展卡尔曼滤波估计器(Extended Kalman Filter,EKF)[15-16]融合原始IMU将GNSS结果再次转入世界坐标系(xW,yW,zW),实现3类传感器坐标统一。

(10)

(11)

(12)

(13)

(14)

式中，φ,θ,φ分别表示车载系统的滚转角、俯仰角和偏航角，滚转角、俯仰角可通过IMU获取，偏航角可结合GPS全局信息，获取真实NEU(北-东-天)坐标。

3.3 系统优化

利用集束调整[17-18]进行全局约束优化，建立最小误差多因子概率权重模型。已知3类传感器误差约束包含IMU预积分误差约束、GNSS量测定位误差约束、激光雷达匹配距离误差约束。全局约束优化是将单一信源置信的后验概率作为误差约束的权重进行赋值，累计联合形成全局约束，利用LM(Levenberg-Marquarelt)算法进行非线性参数求解，优化各信源运动估计模型，获得多源融合最佳置信定位解：

min{∑P(A)f(A)},A=G,B,L，

(15)

式中，P(A)为传感器可用性度量概率值。

4 实验结果

4.1 平台搭建

多传感器车载平台配备Velodyne VLP-16激光雷达，INS-D-E1组合导航系统，内置GNSS板卡和MEMS惯性器件，通过接入测量天线，可输出多系统GNSS以及惯性导航原始数据，系统平台如图3所示，各传感器部分性能参数如表1所示，中央处理器为4核心i7-8550U；室内测试平台搭载轮式小车，室外测试平台搭载商务汽车。

图3 多传感器车载平台Fig.3 Multi-sensor vehicle platform

表1 传感器性能参数Tab.1 Sensor parameters

4.2 室内环境定位测试

室内定位测试场地为某实验室导航试验场，试验场大小约为24 m×27 m，室内测试所选路径绕行试验场外侧及内侧，并尽可能回归起点区域，验证室内运动轨迹全程累积误差，其中外侧穿行大门走廊，通过震荡区域(过地板门槛0.01 m)，系统测试结果如图4所示。图4(a)表示室内场景构建图；图4(b)，图4(c)分别表示优化前后水平运动轨迹图，蓝色线表示运动轨迹，黄色线表示检测回环，红色椭圆表示振荡区域，绿色椭圆表示起点和终点区域；图4(d)，图4(e)分别表示图4(b)，图4(c)垂直方向局部放大；图4(f)分别表示X，Y，Z方向轨迹定位解，红色线为优化前解，蓝色线为本文算法解；算法优化前如图4(b),图4(d)所示，小车经过震荡点(红色椭圆)时轨迹存在明显偏移，高程跳变严重，小车回归起点区域(绿色椭圆区域)后，高程方向存在明显偏移；算法经过约束优化后如图4(c),图4(e)所示，运动轨迹跳变频率减少，运动轨迹平滑，起点至终点高程累计误差显著降低；图4(f)对优化前后定位结果进行标定点统计，震荡后优化前水平误差为1.93 m，垂直误差为0.81 m，震荡后优化后水平误差为0.31 m，垂直误差为0.09 m，震荡发生前2算法轨迹基本一致。

(a) 室内场景构建图

(a) 社区卫星图、实时建图及运动轨迹(左->右)

4.3 城市环境定位测试

测试地点选取石家庄市某居民社区，驾驶测试车辆通过社区居民小区，并绕行社区内主干道，全程约2 km，道路两侧为小区居民楼、商超和树木等，多传感器车载平台接入卫星导航测量型天线，并通过单基准站RTK差分解算获取GNSS定位结果，系统测试结果如图5所示。

图5(a)表示城市社区卫星图、建图及运动轨迹(左->右)；图5(b)表示GNSS周跳监测；图5(c)，图5(d)分别表示优化前、后RTK重新固定建图连续性；图5(f)分别表示X，Y，Z方向轨迹定位解，绿色线为IMU/Lidar耦合定位解，紫色线为EKF定位解，红色线为紧耦合优化前解，蓝色线为本文算法解。算法经过优化前如图5(c)所示，由于存在浮点解和周跳影响，导致建图偏移，GNSS信号重新固定后，建图连接断裂，运动轨迹急剧抖动；算法经过约束优化后，全程检测约束周跳6次(参见图1RTK解状态统计，例如：图5(a)右红色椭圆,图5(b))，降低了GNSS信号周跳等引起的运动轨迹和场景建图的影响，如图5(d)所示，信号中断重新固定后，建图结构连续，避免结构断裂现象；图5(a)右中绿色椭圆，满足回环检测，表明全程轨迹误差较低，具备车道级定位能力；图5(e)对优化前后定位结果进行统计，本文算法平滑性及准确性均优于其他算法，楼层遮挡环境下，最大定位误差小于3.5 m。

(a) 卫星图

4.4 山区环境定位测试

为进一步验证算法在复杂山区环境的实用性和鲁棒性，选取西山森林公园进行山区环境定位测试，起点为游客停车场，驾驶测试车辆穿过山间道路，途径转盘、服务区、林荫道路以及上下坡等，全程约2 km，道路两侧为山壁、树木以及景观建筑等，多传感器车载平台通过单基准站RTK差分解算获取GNSS定位结果，系统测试结果如图6所示。

图6(a)表示西山森林公园卫星图；图6(b)表示定位系统实时建图；图6(c)分别表示X，Y，Z方向轨迹定位解，绿色线为IMU+Lidar耦合定位解，紫色线为EKF定位解，红色线为紧耦合优化前解(在椭圆处漂移)，蓝色线为本文算法解。多传感器车载平台实时定位系统在道路崎岖、山壁、林木遮挡的山区自然环境下，运动轨迹跳变较少，轨迹平滑，实时建图完整，表明算法具有良好的实用性和鲁棒性。图6(c)对优化前后定位结果进行统计，本文算法平滑性及准确性均优于其他算法，平均误差为1.56 m。

4.5 定位测试结果

3类环境定位测试结果如表2所示。

表2 定位结果(RMS)Tab.2 Positioning results (RMS) 单位：m

由表2可以看出，算法具备亚米级室内定位和车道级室外定位性能，在稳定性和精确性上均优于现有算法。

5 结束语

复杂场景下多传感器融合定位的鲁棒性、准确性是导航定位的研究热点与难点，本文给出一种基于约束优化的多传感器车载定位方法，通过设计搭建一套GNSS/IMU/激光雷达定位系统，进行实时跑车测试，结果表明本文算法可实现亚米级室内定位和车道级室外定位性能。该方法有助于应急救援类车载平台在室内、城市峡谷、山区自然环境等广域室内外定位能力提升，后续将通过引入环境信息检测与识别，以实现多信源概率自适应调整和广域无缝定位切换。