互补滤波与T265位置获取结合的车辆状态估计

互补滤波与T265位置获取结合的车辆状
态估计
摘要:针对汽车状态估计过程中传感器数据获取准确性难以保证的特性，提
出一种多传感器数据融合与互补滤波解算的车辆状态观测器。

引言
车辆的安全一直是社会关注的重点问题，实时了解车辆当前准确的姿态信息
可以帮助提升车辆的操纵性和稳定性，从而降低事故的发生概率。

车辆的实时姿
态获取，主要是获取车辆的横摆角速度、纵向速度、侧向速度等。

车辆实时状态
参数的获取主要又两条路线，其一是基于传感器进行测量，另一种则是更多采用
的算法估计。

因此本文提出了一种基于BMI088六轴传感器、Interl T265数据融合与互补
算法解算的车辆状态实时检测装置，实现对运动状态下车辆横滚角、俯仰角、偏
航角准确输出的车辆状态观测器。

1.陀螺仪的零飘抑制
零飘指机器人在静止状态下，采集出来的角速度不是0，而是在某个数值上
下跳动。

从原理上来讲，陀螺仪上电或开始工作时会出现漂移，又分为常值漂移
和随机漂移两种，其中常值漂移就叫零偏。

通过获取陀螺仪的零偏，在后续系统
使用中对其进行补偿,因此在本设计中在设备上电后并保证陀螺仪在静止状态下，采集1000帧陀螺仪原始数据的方差得到预估零飘值，达到抑制零飘的目的。

图1 零点消除代码
2.互补滤波算法
2.1互补滤波的作用
在多传感器数据融合过程中进行误差修正是非常必要的。

互补滤波器从频域的角度来分辨噪声。

由于加速度、陀螺仪的工作原理特性决定了这些惯性测量器件本身存在一些不可避免的缺陷。

其中，加速度计动态响应特性较差，但测量过程中不会存在累积误差，而陀螺仪动态响应特性良好，角速度瞬时精度高，但测量使用过程中由于零飘和积分运算，计算出的姿态角会产生累积误差。

因此采用互补滤波算法融合这两种传感器的数据，可以有效提高测量精度和系统的动态性能。

2.2方向余弦阵
为了描述车辆的姿态信息，本文按照两个不同的三维空间设定了两个不同的空间三维坐标系，分别为参考坐标系On和载体坐标系Ob,如图二所示。

图2 参考系坐标系与载体坐标系
载体在静止状态下参考坐标系和载体坐标系为固连状态，当Ob通过旋转与On不再重合即载体的姿态发生了变化。

在坐标系转换中，任意一个动坐标系都可以通过分别绕 3 个轴旋转得到的 3 个角度，进而确定动坐标系与静坐标系之间的相对位置关系。

以载体坐标系为动坐标系，以导航坐标系为定坐标系的话，姿态角即为一组欧拉角，按一定的转动顺序得到导航坐标系到载体坐标系的关系，常以表示导航坐标系到载体坐标系转换矩阵即方向余弦阵。

=
图3 方向余弦阵
=
图4 欧拉角表示的方向余弦阵
2.3姿态融合
综上可知，姿态更新算法主要是对陀螺仪数据进行迭代运算。

虽然陀螺仪的动态性能好，短时间内数据精度高，但随着时间的延长，其容易受外界环境、温度等影响而产生随机漂移，出现积分误差。

而加速度计虽然短时间内精度差，但长时间测量不会产生积分误差，所以可通过重力加速度在当前载体坐标系的分量与加速度计测得的当前重力加速度分量做向量积运算，修正陀螺仪引起的误差。

取定导航坐标系n中标准重力加速度g定义为的转置，结合图四可得：
==
图5 重力加速度处理后的欧拉角矩阵
将加速度计输出做归一化处理与做向量叉乘得到陀螺仪的校正误差e(w):
==
图6 校正误差获得
采用PI控制器来消除误差:
图7 PI修正后的较正误差
1.T265位置获取
使用 T265相机进行定位， T265 内部自带 SLAM 模块，它可以实时向 z 板载计算机发送位置信息，实现精准定位。

使用 px4＿command 开源项目 ROS功
能包。

系统中包含 MAVROS，T265，px4＿pos＿estimator，px4＿pos＿control 等多个节点，其中 MAVROS 负责将输出的车辆当前状态、姿态及位置信息的MAVROS 消息转换成具有特定格式和标识符的 ROS 话题以供任务管理系统进行订阅；另外 MAVROS 节点还负责将任务控制器发布的包含无人机控制指令的话题
和服务转换成 MAVROS消息格式并发送到主控制器，T265 节点负责将 T265 位置信息以话题形式传送给 px4＿pos＿estimator，px4＿pos＿estimator 节点订阅 T265 和发布的位置信息，并进行坐标转换，并且在
state＿from＿mavros 中已订阅飞控发布的位置、速度、欧拉角信息，根据订阅的数据，发布相应的位置、偏航角给主控，从而达到位置获取这一目的。

结语
提出了一种的车辆侧翻报警与定位系统的构思，以姿态传感器做为嵌入式系统的核心，基于倾斜角报警作为程序的主干算法，通过研究加速度、倾斜角对车辆的影响，完成侧翻预警系统的设计。

帮助疲劳、反应懈怠的驾驶员可以及时的判断汽车行驶过程中是否存在侧翻的危险，使驾驶员可以在侧翻即将发生时可以快速的做出反应，可以减少侧翻事故的发生。

基金项目编号：202210379016 “宿州学院国家级大学生创新创业训练计划项目资助”。