在对方网站做友情链接,直播视频素材,网站开发技术部绩效考核,公司设计网站推荐本文由来#xff1a;前一篇文章“零速更新(ZUPT)辅助INS定位”#xff0c;并通过开源方案ZUPT-aided-INS进行了算法验证。在验证过程中#xff0c;意识到在进行多源传感器融合算法中#xff0c;利用载体自身运动信息进行约束修正非常重要#xff0c;因为这不需要额外增加传…本文由来前一篇文章“零速更新(ZUPT)辅助INS定位”并通过开源方案ZUPT-aided-INS进行了算法验证。在验证过程中意识到在进行多源传感器融合算法中利用载体自身运动信息进行约束修正非常重要因为这不需要额外增加传感器在现有系统上进行ZUPT/ZIHR可以提高系统精度和鲁棒性特别是在部分场景中多源传感器中部分传感器退化的情况下在日常工作中也常称为“engineer trick”。 在阅读相关论文时发现论文“基于车轮安装MEMS IMU的航迹推算算法研究”发现文章写的很好收获很大很有参考意义算是阅读记录一下吧。
文章参考来源论文“基于车轮安装MEMS IMU的航迹推算算法研究”。
由于Wheel-IMU跟随车轮不断旋转安装方式和imu坐标系定义不同在算法中的处理方式也不同。基于旋转效应对捷联惯导误差的调制作用充分挖掘Wheel-IMU的定位能力。基于陀螺间接测量得到的车轮速度进一步探讨由车轮速度信息构建不同观测值用于滤波量测更新的算法并基于载体自身运动信息约束构建零速更新和零航向角速率观测值设计一种性能最优、计算最简洁的约束算法。
一、Wheel-IMU的安装方案和安装角的标定方法
为了避免俯仰角出现奇异问题将imu的旋转轴定义为X轴指向车体右侧且认定imu与载体之间的偏航角偏差固定为90°。imu与载体系坐标定义如下图 1.旋转调制效应分析
姿态误差和速度误差受陀螺和加速度计误差影响而周期性旋转可以抑制误差累计以陀螺为例假设车轮imu的X轴指向正北且在短时间内车轮转速固定则imu角速度误差在n系下的投影为 假定在车轮旋转一圈的时间内误差恒定对上式积分得到由于陀螺误差引起的姿态误差 从上式可以看出由于车轮旋转imu非旋转轴的测量误差在n系下被调制成正弦波经过一圈积分后累计误差为零。同理加速度计测量误差所引起的速度误差在经过一圈的积分后非旋转轴累计误差为零。 因此在一圈旋转周期内造成的东向和地向的累计速度误差以及俯仰、航向误差为零imu安装在车轮上在一定程度上抵消了imu的常值偏差。
2.车轮imu安装角标定
由于imu安装无法与车轮完全对齐存在imu中心与车轮中心不重合的误差以及imu轴系与车轮坐标系不平行的误差分别称为杠臂误差和安装角误差。 由于imu随着车轮一直旋转车轮坐标系和imu坐标系的y/z轴指向会周期性地变化所以忽略imu与车轮坐标系之间的横滚角只对航向角和俯仰安装角进行校准。 忽略地球自转在载体沿直线形式或者车轮空转时可以认为imu测量的角速度完全来自于车轮角速度的投影则有 仅考虑航向角和俯仰角忽略无关项有 代入上试得到 求解可以得到imu相对于车轮的安装角 实际应用中可以将轮架空提前利用静止时段数据扣除陀螺零偏然后是车轮空转利用一段时间数据求平均值。对于不能架空车轮的机器可以沿着既定轨迹走一段直线并采集wheel-imu数据求平均结果。 另杠臂误差的校准后续会增广到卡尔曼滤波的状态向量中进行实时估计。
二、基于滤波航迹推算算法
1.算法整体框架
a.初始化载体位置、速度和姿态PVA后利用wheel-imu测量数据进行机械编排推算载体状态 b.利用imu角速度和比力测量值探测载体状态判断载体是否进入静止状态若进入则进行ZUPT和ZIHR量测更新 c.当载体开始运动时采用速度信息作为量测更新。速度约束观测值有三种形式1利用imu角速度和车轮半径计算车轮前进速度结合NHC在v系进行约束即传统的ODO/INS组合导航思路2将车轮前进速度投影到n系积分得到imu在一段时间内的位移以此作为观测值3通过杠臂、车轮半径和imu水平姿态将imu速度投影到车轮上的触地点利用“当载体不发生侧滑和颠簸等状况时车轮上与地面接触点相对于n系速度为零”这一条件构建约束。 d.利用滤波后得到的imu误差估计值补偿imu原始数据同时载体运动误差反馈给载体状态得到滤波更新之后的运动信息。并将ekf输出的航向结果结合车轮速度得到平面DR推算位置结果。 三种观测值构建的航迹推算系统框架如下
2.系统误差状态方程视觉里程计比例因子、imu的零偏误差和比例因子误差、杠臂误差
考虑车辆行驶过程中由于自重、温度等变化导致轮胎压强变化造成车轮实际半径与标称半径的偏差将该误差建模为与车轮半径按比例变化视为里程计比例因子误差sv则 由于杠臂误差直接会影响到imu的向心加速度和速度投影与其它误差有较好的区分度所以imu y、z轴的杠臂误差有较强的客观性。将上述误差增光到卡尔曼滤波系统状态中可以得到基于车轮安装imu的航迹的推算算法滤波模型的状态向量如下 将imu的零偏误差和比例因子误差建模为一阶高斯-马尔科夫过程可得imu零偏和比例因子误差的微分方程 将里程计比例因子和杠臂误差建模为随机游走其微分方程为
3.车轮速度信息约束算法
a.车轮中心速度约束
补偿imu安装角和陀螺误差后可以更准确由陀螺x轴测量值和车轮半径计算车轮前进速度 对于一般车辆其在平面上正常行驶假设不发生颠簸和侧滑车体只有沿着前进方向的速度侧向和垂向速度为零。这一基本假设称为非完整性约束NHC。车辆满足该约束时载体坐标系下的观测向量可以表示为 车辆运动约束和非完整性约束均属于载体坐标系下的观测值因此与imu的状态构建约束时需要获得b系和v系的转换关系。 在传统的车载里程计、惯导组合导航算法中imu安装在车身需要提前标定或在线标定imu安装角并视为固定值。而在该方案中imu安装在车轮上imu相对于载体的横滚角程周期性变化无法感受载体的水平姿态即仅仅依靠车轮imu无法判断车辆是上坡还是下坡。 因此假设车辆行驶在水平面上v系相对于n系只存在航向角误差则有 载体的航向角误差可认为与imu的航向角误差相等。 由imu速度经过杠臂投影得到的车轮速度为 则车轮中心速度约束方程误差方程可以表示为 ps状态向量的误差方程有固定思路如下 该推导方式下误差等于预测减去观测值。当然也可以观测减去预测。 b.里程增量约束算法
思路由陀螺角速度和车轮半径可以计算车轮在任意时刻的速度经过积分之后可以得到位移增量。 n系下的里程速度观测 由imu速度推算得到的车轮在n系下的速度为 两式相减得到 两边积分并认为积分区间内误差恒定则 忽略地球自转则 综上得到里程增量约束算法的观测方程
c.车轮接触地点零约束算法
根据非完整性约束轮式机器人正常行驶在路面上时测向和垂向的速度为零。 假设某个瞬间车轮中心相对于地面前进的速度为v此时由于车轮子在旋转车轮上与地面接触点相对于车轮中心的速度也为v方向与车辆前进的方向相反则当载体不发生颠簸和侧滑时该点相对于地面的速度始终为零。 不同于传统的里程计速度约束该方案无需获得车轮速度而是通过杠臂将车轮imu速度投影换算至车轮与地面接触点这时该点速度v(p2n) [0 0 0]^T。 车轮接触点零速投影示意图如下 由imu速度计算得到的车轮触地点p的速度为 l(p2b)表示p点在b系下的杠臂。随着车轮转动p点在b系下的位置动态地变化需要根据imu横滚角实时计算。假设车辆在水平面上运动则 p点速度观测值可以表示为 因此速度投影约束算法的误差方程可以表示为
4.零速修正和静止航向锁定
在不外加传感器的情况下依靠载体自身的运动信息引入约束抑制INS误差发散主要包括ZUPT/ZIHR。 以文章零速更新(ZUPT)辅助INS定位一样流程1.零速检测2.零速更新成立进行误差修正。 本文采用固定长度滑动窗口内的加速度、速度和角速度三个量的峰峰值来和阈值对比判断载体是否处于静止状态。实际功能工程应用中可以针对不同的运动载体和环境选用不同阈值。 当判断零速更新成立时imu速度观测值可以表示为 零速更新的观测方程可以表示为 由误差扰动分析imu推算得到的载体姿态矩阵和航向角可以分别表示为 静止航向锁定的观测方程可以表示为 三、基于车轮速度信息构建的三种观测约束算法讨论
从以上推导可以看出三种观测值更新算法本质上都是利用载体车轮速度信息对系统状态进行修正。其中 1.车轮中心速度约束算法将imu速度经杠臂和姿态矩阵C_b2n)投影到车轮中心联合NHC与陀螺数据推算的v系下的车轮速度构建约束。引起观测误差的系统误差状态主要包括速度误差、姿态误差、陀螺误差、杠臂误差和里程计比例因子误差。 2.里程计增量约束算法是将载体速度投影在n系下进行积分得到一段时间内的位移增量并假设在积分区间内系统误差状态不变。引起观测误差的系统状态包括速度误差、姿态误差、陀螺误差、杠臂误差、加速度计误差和车轮比例因子误差。 3.车轮触地点投影约束并不需要利用陀螺数据计算车轮中心的速度而是利用“当载体不发生颠簸、侧滑等现象时车轮触地点相对于n系的速度为零”这一条件构件约束。引起观测误差的系统状态包括速度误差、姿态误差、陀螺误差、杠臂误差和车轮比例因子误差。
同时三种算法都依赖非完整性约束和载体水平面运动假设。对于车轮中心速度约束算法和里程增量约束算法比较直观因其观测方程建立时需要载体在v系下的速度和载体姿态。而对于车轮触地点投影约束载体水平面运动约束提现在利用车轮旋转角和半径进行速度投影。此时若载体运动在斜面上则速度投影还应该在imu横滚角上补偿载体俯仰角。对于里程计增量约束算法在载体动态较大时不满足积分区间内系统保持不变这一假设同时速度观测噪声也在无形中进行了积分一定程度上破坏了卡尔曼中观测噪声服从高斯分布这一条件并且里程计增量约束对NHC的假设更强一些需要保证在积分区间内载体垂向速度和侧向速度均为零。对于车轮中心约束由于载体航向也来自于imu姿态因此在速度投影时引入了额外的航向误差。而对于车轮触地点零点约束则在速度投影引入了横滚角误差。
综上三种观测值约束算法利用的信息本质上是相同的只是观测值加工的手段不同不同的观测值引入不同的微小误差因此最终的导航结果也不应有明显的差异。
缺陷1.单体车轮安装imu受限于水平面运动假设在起伏不平的路段精度会下降同时单个系统的鲁棒性有待提高。
改进针对缺陷1可以通过车体安装imu检测载体的水平姿态信息针对缺陷2考虑在两个非转向轮同时安装imu进行两次观测去平均。同时车身安装imu可以用imu之间的空间位姿关系来修正系统误差。
