互补滤波姿态解算方法

繁星夏若离

互补滤波顾名思义就是多组数据结合互补，并进行滤波处理稳定输出得到可 靠性较高的姿态解算结果。我们平时进行姿态角测量时多使用加速度计和陀螺仪， 加速度计用于测量加速度，具有较好的静态稳定特性，但在运动环境中其数据相 对不可靠。陀螺仪用于测量角加速度，动态稳定性较好，但在静止时数据相对不 可靠。所以我们可以通过互补滤波方法用加速度输出的数据来实时修正陀螺仪的 漂移误差，提高在各种运动状态下的姿态测量精度。基于四元数的互补滤波法(CF) 可以有效利用陀螺仪、加速度计、磁力计的频域互补特性，实现多传感器姿态融 合，解算姿态信息。
互补滤波器设计中最重要的一个步骤就是 PID 控制器中的比例环节系数 Kp的动态自适应选择，该值直接影响滤波器的截止频率 f = KP/2pi。即在大于f 的高频段，陀螺仪对姿态解算起主要作用，在小于f的低频段，加速度传感器对 解算结果起主要作用。在本项目中研究人员根据陀螺仪的转动速率设置了两个不 同的 Kp 值，以适应在不同的晃动速率下进行更好的数据融合。根据长期室内 试验调试，实验人员发现，当 BMX160 的陀螺仪在10ms内转速超过5°时选取 Kp=0.001（快速晃动/转动条件），否则选取Kp=0.8（快速晃动/转动条件），这 种参数设置在海洋波浪晃动条件下的姿态测量效果最为理想。比例环节系数Kp 选择后还包括，加速度数据归一化，获取重力方向和变迁的评估参数，计算评估 参数与加速度数据的误差，对误差进行误差积分，误差积分通过PI控制后补偿到 陀螺仪，然后进行四元数微分方程解算，四元数归一化，通过四元数归一化计算 姿态角，通过姿态角计算姿态补偿矩阵，通过姿态补偿矩阵计算北西天坐标系的 加速度值。
姿态解算的第一步是指定坐标系，通常将陀螺仪、加速度计和磁力计定义到 一个坐标系，即载体坐标系（Ob 系）。ObXb 与载体纵轴重合；ObYb 位于载 体纵轴向对称面内与 ObXb 轴重合；ObZb 垂直于 ObXbYb 平面。与载体坐标 系对应的是导航坐标系（On），本项目选用的导航坐标系是“东北天 ENU”坐标 系，选取载体重心为原点，OnXn 指向正北方向，OnYn 指向正东方向，OnZn 指向垂直天空方向。
姿态解算以及姿态补偿矩阵计算前还包括，判断姿态输出心跳间隔数 OutputNum 是否等于预设值，判断定时时间是否大于或等于预设值，若均是， 则读取 6 轴 mems 惯导单元和 3 轴磁力计单元的数据，否则继续执行判断动 作，根据返回的陀螺仪数据和加速度数据，动态选择 PID 误差控制中的比例环 节系数 Kp。根据陀螺仪的转动速率设置了两个不同的 Kp 值，以适应 在不同的晃动速率下进行更好的数据融合。根据长期室内试验调试，实验人员发 现，当 ADIS16485 的陀螺仪在 10ms 内转速超过 5°时选取 Kp=0.001，否则 选取 Kp=1，这种参数设置最为理想。 比例环节系数 Kp 选择后还包括，加速度数据归一化，获取重力方向和变 迁的评估参数，计算评估参数与加速度数据的误差，对误差进行误差积分，误差 积分通过 PI 控制后补偿到陀螺仪，然后进行四元数微分方程解算，四元数归一 化，通过四元数归一化计算姿态角，通过姿态角计算姿态补偿矩阵，通过姿态补 偿矩阵计算北西天坐标系的加速度值。258 姿态角解算以及姿态补偿矩阵计算的结果按照指定格式通过串口依次输出， 输出参数包括：横滚角（roll）、俯仰角(pitch)、航向角(yaw)、四元数（q0、q1、 q2、q3）,载体坐标系->北西天坐标系的姿态转换矩阵 Cbn、载体系下的三轴加速 度（axb，ayb，azb），北西天坐标系下的三轴加速度（axn，ayn，azn）。每次 输出固定的 92 字节数据，其中头码占据 2 字节为 0x55,0xaa，校验码占据 2 个字节，每个参数均占 4 个字节的 float 类型，且按照高位在前低位在后的格式 发送。其中姿态补偿矩阵 Cbn 为 3*3 的矩阵，其数字式姿态补偿的主要功能 可用来对载体系上的矢量测量进行姿态校正，将载体系的矢量测量值校正到北西 天坐标系，获取自然地理坐标系下的矢量真实值。科勘海洋波浪传感器可应用在对晃动平台，如无人 机、无人艇、Glider、海洋浮标等晃动平台上对三维矢量测量的姿态校正。如对浮标晃动平台上的三维风速校正，假设锚定浮标是只有晃动而没有运动发生，浮 标上搭载的三维风速测量值为 Vb = [Ub, Vb,Wb]T，则校正到北西天系后的三维 风速测量值为 Vn=Vb*Cbn。
————————————————
版权声明：本文为博主原创文章，遵循 CC 4.0 BY-SA 版权协议，转载请附上原文出处链接和本声明。
原文链接：https://blog.csdn.net/yuhaiyan1979/article/details/148451570

执成瘾i

抢完橘子抢银行，现在要来抢沙发！