机器人在未知环境中依靠视觉同时定位和建图(SLAM)技术可以确定自身位置并且构建环境地图．在很多无人驾驶的应用场景中(如地下车库)，全球卫星导航系统(GNSS)系统不可用，因此须要用到SLAM技术．在众多SLAM解决方案中，视觉惯性SLAM以其低成本吸引了很多研究人员的关注．视觉惯性SLAM使用视觉观测和IMU测量估计位姿，其中视觉观测可以校正IMU数据积分时产生的累计误差，而IMU为融合提供一个良好的先验．MSCKF方法使用扩展卡尔曼滤波(EKF)融合IMU和视觉[1]，是经典的基于滤波的视觉惯性SLAM方法．在此基础上提出的S-MSCKF方法，使用双目相机替代单目[2]，提高了系统的精度和鲁棒性．虽然基于滤波的方案具有很好的实时性，但是缺乏局部建图和回环线程，导致位姿估计存在较大的累计误差．预积分[3]使得基于非线性优化视觉惯性融合成为可能，是目前视觉惯性SLAM中最常用的方法之一．文献[4]使用李代数和旋转群重新推导了预积分，克服了欧拉角表达旋转时产生的自锁问题．文献[5]基于四元数推导的预积分，提出VINS-Mono和VINS-Fusion，由于使用了滑动窗口优化和回环检测，因此VINS呈现很好的鲁棒性、实时性和精度．文献[6]基于ORB特征和共视后端提出了ORB-SLAM，后续在此基础上引入IMU设计了VIORB[7]，引入双目和RGBD设计了ORB-SLAM2[8]．文献[9]针对VIORB惯导初始化时间长的问题，提出了纯MAP的初始化方法，之后又基于VIORB和ORB-SLAM2提出了ORB-SLAM3，后者是目前定位精度最高的视觉SLAM系统之一．尽管研究人员提出了很多高精度的视觉惯性SLAM方案，但是目前视觉惯性SLAM的精度还是不能满足实际工程的需求．在地下车库这种低速无人驾驶场景中，车身运动激励不足，导致IMU加速度偏置估计存在误差，同时IMU加速度求解位移须经过两次积分，导致IMU位移预积分误差进一步放大．针对无人驾驶汽车的运动特点，文献[10]设计了动力学预积分，将车辆动力学信息融合到位姿优化中，在VINS-Mono的基础上设计了VINS-vehicle，但是所提出的车辆动力学预积分仅采用了方向盘转角和车速，所得到的车辆动力学预积分，其旋转预积分部分积分时的频率较低，且只能表达平面运动．为了进一步提高车辆动力学预积分的精度，本研究提出一种预积分方法，旋转预积分使用IMU角速度观测以IMU测量频率积分，平移预积分使用IMU角速度信息和车辆动力学信息以车辆动力学测量频率积分．这种方法使用了精度较高、频率较大的IMU角速度代替车辆方向盘转角进行旋转预积分，使得平移预积分可以表达非平面运动．同时，更高频率的旋转预积分可以利用更多的IMU角速度测量，达到更高的精度．最终，以此预积分为基础将车辆动力学信息融合到开源的双目惯性ORB-SLAM3(http://github.com/UZSLAMLAB/ORB_SLAM3)中，提高其在地下车库的定位精度．1 车辆动力学观测本研究使用的坐标系如图1所示，一共包含4个坐标系，即局部世界坐标系W、相机坐标系C、IMU坐标系B及车辆坐标系V，其中局部世界坐标系Z轴和重力方向重合，原点为系统观测到第一帧图像时的位置，车辆坐标系原点为整车质心．本研究使用二自由度汽车模型[11]，将从底盘得到的整车车速和方向盘转角转化成为整车质心的速度vmt，质心侧偏角βmt和角速度ωmt，根据车辆坐标系的定义可以得到车辆坐标系下的速度v˜V(t)和角速度ω˜V(t)分别为：v˜V(t)=[vmtcos βmt,  vmtsin βmt,  0]T;ω˜V(t)=[0,  0,  ωmt]. (1)10.13245/j.hust.220509.F001图1坐标系定义由式(1)和车辆坐标系和IMU坐标系之间的外参(RBV,pBV)可以得到IMU坐标系下的车速v˜B(t)为v˜B(t)=RBVv˜VB(t)=RBV(v˜V(t)+ω˜V(t)×pBV) ，(2)式中v˜VB(t)为车辆坐标系下IMU坐标系的速度，本研究假设车辆是刚体，因此可由刚体运动公式得到．假设角速度观测受到高斯白噪声ηg(t)和缓慢变化的偏置bg(t)影响，速度观测受到高斯白噪声ηv(t)的影响．IMU坐标系下的角速度观测ω˜B(t)和速度观测v˜B(t)可以表达为：ω˜B(t)=ωB(t)+bg(t)+ηg(t);v˜B(t)=vB(t)+ηv(t), (3)式中ωB(t)和vB(t)分别为IMU坐标系下真实的角速度和速度．2 车辆动力学预积分将构造预积分的两帧图像定义为帧i和帧j，两帧图像之间有频率不同的IMU信息和车辆动力学信息，如图2所示．假设帧i和帧j之间有M+1个IMU数据和N+1个轮速数据，且有i+M+1=j，i+N+1=j．为简化公式，用(Ri,pi)和(Rj,pj)代表世界坐标系下帧i和帧j的IMU位姿，即(RiWW,piWW)和(RjWW,pjWW)．10.13245/j.hust.220509.F002图2预积分示意图2.1　车辆动力学预积分公式帧j的位姿可以通过式(3)使用角速度和速度在帧i位姿的基础上积分得到，即Rj=Ri∏k=0Mexp[(ω˜kB-big-ηgd)∧Δtb];pj=pi+∑k=0NRi+k[(v˜kB-ηvd)Δtc], (4)式中：ω˜kB和v˜kB分别为第k个角速度和车速观测，其中ω˜kB通过IMU直接得到，v˜kB通过式(2)得到；∧为列向量对应的反对称矩阵；ηgd和ηvd分别为离散的角速度和车速观测噪声．和IMU预积分[4]相同，本研究也假定帧i和帧j之间的角速度偏置相同．将式(4)中帧i和帧j的位姿相关的变量移动到等式左边，再对等式两边取期望，可以得到车辆动力学预积分的观测，即车辆动力学预积分的定义：ΔR˜ij=RiTRj=∏k=0Mexp[(ω˜k-big)∧Δtb];Δp˜ij=RiT(pj-pi)=∑k=0NΔR˜ii+k(v˜kBΔtc). (5)2.2　车辆动力学预积分的观测噪声通过在真实预积分(式(5))中将高斯白噪声分离得到预积分观测的噪声δφij，其中旋转预积分的推导和文献[4]中的IMU预积分类似，直接给出结果：ΔRij≈ΔR˜ijexp(-δφ∧);δφij≈∑k=0NΔR˜i+kjJrkηgdΔtc, (6)式中Jrk=Jr((ω˜k-big)Δtc)，为(ω˜k-big)Δtc的右乘雅可比矩阵．旋转预积分噪声的更新及后文中雅可比的更新频率都和车辆动力学信息频率相同，这是为了便于更新平移预积分的噪声和雅可比．按照同样的思路可以推导平移预积分的噪声，即：      Δpij=∑k=0NΔR˜ii+kexp(-δφ∧)[(v˜kB-ηvd)Δtc]≈∑k=0NΔR˜ii+k(I-δφ∧)[(v˜kB-ηvd)Δtc]=Δp˜ij-δpij;    (7)         δpij≈∑k=0N(ΔR˜ii+kηvdΔtc-ΔR˜ii+kv˜kB∧Δtcδφij),式中I为3×3的单位矩阵．根据式(6)和式(7)中噪声的定义，可以得到噪声状态和协方差的传播方程为      δφijδpij=ΔR˜j-1jT0-ΔR˜j-1jTvj-1B∧ΔtcIδφij-1δpij-1+Jrj-1Δtc00ΔR˜ij-1Δtcηgdηvd.2.3　车辆动力学预积分对于角速度偏置的雅可比在预积分公式中对角速度偏置施加微小扰动δb，再将扰动分离，可以得到车辆动力学预积分关于角速度偏置的雅可比矩阵．旋转预积分雅可比矩阵和IMU预积分相似，即：ΔR˜ij(big+δbig)≈ΔR˜ij(big)exp∂R˜ij∂bigδbig∧;∂R˜ij∂big≈∑k=0N-ΔR˜i+kjTJrkΔtb.平移预积分对角速度偏置的雅可比为：Δp˜ij(bg+δbg)=∑k=0NΔR˜ii+k(bg)exp∂R˜ii+k∂bgδbg∧(v˜kBΔtc)≈∑k=0NΔR˜ii+kI+∂R˜ii+k∂bgδbg∧(v˜kBΔtc)=Δp˜ij+∂p˜ij∂bgδbg;∂p˜ij∂bg=-∑k=0NΔR˜ii+k∂R˜ii+k∂bgv˜kB∧Δtc.3 车辆动力学预积分残差根据式(5)得到车辆动力学的旋转残差rΔRij∈R3和平移残差rΔpij∈R3分别为：rΔRij=log(ΔR˜ijT(b^ig)R^iTR^j);rΔpij=R^iT(p^j-p^i)-Δp˜ij(b^ig), (8)式中R^i,R^j,p^i,p^j和b^ig为优化过程中的估计量．车辆动力学残差关联帧i和帧j的位移和旋转及帧i的角速度偏置，共五个优化变量．通过增加扰动再分离的方式，可以求出残差对这五个状态量的雅可比．扰动增加的方式[4]为：Ri→Riexp(δφi);Rj→Rjexp(δφj);pi→pi+Riδpi;pj→pj+Rjδpj;Δbig→Δbig+δbig, (9)式中Δbig为b^ig和预积分时使用的偏置的差值．旋转预积分残差的雅可比和IMU预积分相似，五个雅可比分别为：∂rΔRij∂δφi=-Jr-1(rΔRij)R^jTR^i；∂rΔRij∂δφj=Jr-1(rΔRij)； ∂rΔRij∂δpi=0； ∂rΔRij∂δpj=0；∂rΔRij∂δbig=-Jr-1(rΔRij)exp(rΔRij)TJr∂R˜ij∂bgbg∂R˜ij∂δbg．通过式(8)和式(9)，平移残差的雅可比对应的求解．平移残差对于帧i旋转的雅可比为：rΔpij[R^iexp(δφi)]=[R^iexp(δφi)]T(p^j-p^i)-Δpij=(I-δφi∧)R^iT(p^j-p^i)-Δpij=rΔpij(R^i)+∂rΔpij∂δφiδφi;  ∂rΔpij∂δφi=R^iT(p^j-p^i)∧.以上推导用到了性质exp(δφ)T=exp(-δφ)．由于平移残差公式中没有出现帧j的旋转，因此容易得到∂rΔpij/∂δφj=0.平移残差对于帧i平移的雅可比为：rΔpij(p^i+R^iδpi)=R^iT(p^j-p^i-R^iδpi)-Δpij=rΔpij(p^i)+∂rΔpij∂δpiδpi;  ∂rΔpij∂δpi=-I.平移残差对于帧j平移的雅可比为：rΔpij(p^i+R^jδpj)=R^iT(p^j+R^jδpj-p^i)-Δpij=rΔpij(p^i)+∂rΔpij∂δpjδpj;  ∂rΔpij∂δpi=R^iTR^j.平移残差对于帧i角速度偏置的雅可比为：rΔpij(Δbig+δbig)=R^iT(p^j-p^i)-Δpij+∂p˜ij∂bg(Δbig+δbig)=rΔpij(Δbig)+∂rΔpij∂δbigδbig;∂rΔpij∂δbig=-∂p˜ij∂bg.4 融合车辆动力学视觉惯性SLAMORB-SLAM3是目前定位精度最高的双目视觉惯性SLAM系统之一，本研究选择其作为基础开发SLAM算法，所构建的系统框架如图3所示．10.13245/j.hust.220509.F003图3视觉惯性车辆动力学SLAM系统框架本研究在ORB-SLAM3的IMU初始化集束调整(BA)、追踪局部地图BA、局部建图BA及全局BA中都添加车辆动力学残差．图4展示了这些位姿优化涉及到的因子图，图中：T为由旋转R和平移p组成的位姿；b为IMU偏置．因子图表明：车辆动力学残差和IMU残差类似，同时其不受重力和加速度偏置影响，可以为整个优化问题引入更多稳定的约束，提高整个SLAM系统的定位精度．10.13245/j.hust.220509.F004图4视觉惯性车辆动力学SLAM重要部分的因子图5 实车实验由于目前的公开数据集都没有包含车辆底盘信息，因此本研究使用TIEV无人驾驶平台(cs1.tongji.edu.cn/tiev)，在同济大学嘉定校区地下车库采集数据进行测试．本研究使用低成本量产成像稳定的ZED2相机(https://www.stereolabs.com/zed-2/)采集双目图像数据(15 FPS，640×360)和IMU数据(400 Hz)，从CAN总线获取车辆底盘数据(100 Hz)．由于地下车库没有GPS信号，因此使用Velodyne VLP-16激光雷达采集激光数据，使用SC-LeGO-LOAM[12]的定位结果作为轨迹真值．本研究一共采集了6个数据序列用于评估，整个实验的平均车速约为5 km/h．图5展示了测试地下车库的具体场景，图6展示了数据序列UG01，UG03和UG05的轨迹真值对比，图中：ORB-SLAM3代表开源双目惯性ORB-SLAM3；ORB-SLAM3-v代表本研究提出的系统．10.13245/j.hust.220509.F005图 5测试地下车库的具体场景10.13245/j.hust.220509.F006图 6UG01，UG03和UG05序列的轨迹真值对比本研究使用开源的evo轨迹评估工具(https://github.com/MichaelGrupp/evo)，通过绝对位置均方根误差(εAPE)评估系统的定位精度．表1列出本研究提出的系统和双目视觉惯性ORB-SLAM3在地库数据集上的精度εAPE对比．图7展示了UG01和UG03序列上的εAPE对比．可以看出：由于车辆动力学信息的引入，在全部的测试序列上，本研究提出的系统将ORB-SLAM3的平均定位精度提高了32%，而且在UG05序列上明显提高了ORB-SLAM3的鲁10.13245/j.hust.220509.T001表1系统定位精度εAPE对比序列ORB-SLAM3本方法UG01UG020.6210.5940.5100.507UG030.8430.395UG040.6700.386UG05—0.597注：—表示该项数据不存在．m10.13245/j.hust.220509.F007图7UG01和UG03序列的APE对比棒性．