技术领域
[0001] 本发明属于航姿参数检测技术领域，具体地说，涉及一种航姿参数估计方法。
相关背景技术
[0002] 航姿参数可以定量描述物体在参考坐标系中的角位置，在生产生活中应用广泛。加速度计、陀螺及磁传感器是常用的航姿参数传感器，它们的测量特性与测量信息各不相同，即单独使用又可通过信息融合技术组合应用。
[0003] 文献改进型互补滤波在三维姿态估计中的应用(陈薇等.控制工程，2019,26(5)：910‑915)中修改了互补滤波器中的误差补偿成分，去掉X、Y轴的磁力计误差补偿、添加了Z轴磁力计误差补偿。文献小型无人飞行器机动过程中航姿互补滤波算法研究(王勇军等.电子测量与仪器学报，2020,34(7)：141‑149)中将基于不同传递函数及各种航姿表示形式的姿态互补滤波归纳为统一的广义互补滤波算法，分析该类算法中的乘性姿态误差，并引入运动加速度补偿方法。文献基于递推最小二乘与互补滤波的姿态估计(陈光武.控制理论与应用，2019,36(7)：1096‑1102)中在传统互补滤波器的基础上通过自适应调整比例积分参数来调整滤波器的交接频率。
[0004] 上述文献所采用的方法均存在以下缺点：(1)航姿估计方法只有一种工作模式，需要加速度计、陀螺与磁传感器配合工作，不能适应其它传感器组合形式。(2)没有考虑加速度计与磁传感器低频率波特性，传感器数据频域处理不足，对航姿估计效果差。(3)采用比例积分环节实现二阶互补滤波器，调整参数较多。
具体实施方式
[0065] 下面，通过示例性的实施方式对本发明进行具体描述。然而应当理解，在没有进一步叙述的情况下，一个实施方式中的元件、结构和特征也可以有益地结合到其他实施方式中。
[0066] 参见图1，本发明提供了一种航姿参数估计方法，具有三种工作模式，工作模式一：以加速度计与磁传感器测量数据为输入采用矢量法利用矢量坐标变换关系对测量和数据进行处理，计算得到俯仰角估计值 横滚角估计值 航向角估计值 实现对航姿参数进行估计。工作模式二：以陀螺测量数据为输入，采用积分法利用航姿四元数动态递推方程由航姿初值逐步计算各时刻航姿四元数，并利用航姿参数转换关系计算得到航向角估计值俯仰角估计值 横滚角估计值 实现对航姿参数进行估计。工作模式三：以加速度计、陀螺及磁传感器测量数据为输入，使用互补滤波器对矢量法与积分法估计结果进行组合，得到航姿参数组合估计结果，即得到航向角估计值 俯仰角估计值 及横滚角估计值实现对航姿参数进行估计。以下以对上述方法的三种工作模式进行详细说明。
[0067] 采用矢量法(即工作模式一)对航姿参数进行估计，其具体步骤为：
[0068] S1、获取加速度计测量值a、磁传感器测量值m。
[0069] S2、以加速度计测量值a、磁传感器测量值m为输入，利用矢量坐标变换关系，通过参考坐标系中的重力加速度矢量计算俯仰角估计值 及横滚角估计值 将物体坐标系中的地磁矢量转换至水平坐标系，利用参考坐标系中的地磁矢量和水平坐标系中的地磁矢量计算航向角估计值 完成对航姿参数的估计。
[0070] 具体地，利用矢量坐标变换计算俯仰角估计值 横滚角估计值 及航向角估计值 的具体步骤为：
[0071] 设参考坐标系为or‑xryrzr，物体坐标系为ob‑xbybzb，空间矢量在参考坐标系的坐标为vr，在物体坐标系的坐标为vb，则两坐标的转换关系为：
[0072]
[0073] 式中， 为参考坐标系至物体坐标系的变换矩阵；
[0074] 利用欧拉角构建变换矩阵 表示为：
[0075]
[0076] 式中，cos表示余弦函数，sin表示正弦函数，ψ为航向角，为俯仰角，γ为横滚角，航向角ψ、俯仰角 横滚角γ三者构成一组欧拉角；
[0077] 设参考坐标系中重力加速度矢量为ar,ar＝[0 0 9.8]，地磁矢量为mr，由地磁模型计算得到，则有：
[0078]
[0079] 将公式(2)带入公式(3)，得到：
[0080]
[0081]
[0082] 式中，ax为加速度计测量值a的x轴分量，ay为加速度计测量值a的y轴分量，az为加速度计测量值a的z轴分量；
[0083] 利用俯仰角估计值 和横滚角估计值 构建物体坐标系至水平坐标系变换矩阵变换矩阵 表示为：
[0084]
[0085] 将物体坐标系中的地磁矢量mb变换为水平坐标系中的地磁矢量mp，则有：
[0086]
[0087] 将公式(6)带入公式(7)，得到：
[0088]
[0089] 式中，mpx为地磁矢量mp的x轴分量，mpy为地磁矢量mp的y轴分量，mrx为地磁矢量mr的x轴分量，mry为地磁矢量mr的y轴分量；
[0090] 由公式(4)、公式(5)及公式(8)计算得到俯仰角估计值 横滚角估计值 航向角估计值
[0091] 具体地，加速度计测量值a、磁传感器测量值m进行矢量坐标变换之前，通过补偿滤波器分别对加速度计测量值a、磁传感器测量值m进行频域补偿。具体地，通过补偿滤波器进行频域补偿的具体方法为：将加速度计测量值a输入补偿滤波器，补偿滤波器的输出即为补偿后的加速度计测量值a，将磁传感器测量值m输入补偿滤波器，补偿滤波器的输出即为补偿后的加速度计测量值m，补偿滤波器的传递函数为(T1s+1)/(T2s+1)，在应用中可根据加速度计与磁传感器测量特性确定T1与T2取值。通过补偿滤波器函数对测量信息进行频域补偿，补偿信息损失，有效抑制测量噪声，提升了高频段参数估计结果，进而提高航姿估计效果。
[0092] 采用积分法(即工作模式二)对航姿参数进行估计，其具体步骤为：
[0093] S1、获取陀螺测量值ω。
[0094] S2、以陀螺测量值ω为输入，利用航姿四元数动态递推方程构建四元数递推计算模型，计算各时刻航姿四元数，利用航姿四元数计算航向角估计值 俯仰角估计值 及横滚角估计值
[0095] 具体地，利用航姿四元数计算航向角估计值 俯仰角估计值 及横滚角估计值的具体步骤为：
[0096] 以陀螺测量值ω为输入，利用航姿四元数动态递推方程构建四元数递推计算模型，四元数递推计算模型表示为：
[0097]
[0098] 式中，qk为k采样时刻的四元数，qk+1为k+1采样时刻的四元数，k为采样时刻，k＝0,1,2,3，Δt为采样间隔，ωk为k采样时刻的陀螺测量值，[ωk×]为陀螺测量值ωk的反对称矩阵；
[0099] 利用四元数构建变换矩阵 表示为：
[0100]
[0101] 式中，q0为四元数的实数分量，q1为四元数虚数部分的第一分量，q2为四元数虚数部分的第二分量，q3为四元数虚数部分的第三分量；
[0102] 联立公式(2)和公式(4)得到：
[0103]
[0104]
[0105]
[0106] 将公式(9)计算得到的四元数带入公式(12)‑(14)得到航向角估计值 俯仰角估计值 横滚角估计值
[0107] 使用互补滤波器对矢量法与积分法估计结果进行组合(即工作模式三)对航姿参数进行估计，其具体步骤为：
[0108] S1、获取加速度计测量值a、磁传感器测量值m及陀螺测量值ω。
[0109] S2、以加速度计测量值a、磁传感器测量值m为输入，利用矢量坐标变换关系，通过参考坐标系中的重力加速度矢量计算俯仰角估计值 及横滚角估计值 将物体坐标系中的地磁矢量转换至水平坐标系，利用参考坐标系中的地磁矢量和水平坐标系中的地磁矢量计算航向角估计值 完成对航姿参数的估计。
[0110] 具体地，利用矢量坐标变换计算俯仰角估计值 横滚角估计值 及航向角估计值 的具体步骤为：
[0111] 设参考坐标系为or‑xryrzr，物体坐标系为ob‑xbybzb，空间矢量在参考坐标系的坐标为vr，在物体坐标系的坐标为vb，则两坐标的转换关系为：
[0112]
[0113] 式中， 为参考坐标系至物体坐标系的变换矩阵；
[0114] 利用欧拉角构建变换矩阵 表示为：
[0115]
[0116] 式中，cos表示余弦函数，sin表示正弦函数，ψ为航向角，为俯仰角，γ为横滚角，航向角ψ、俯仰角 横滚角γ三者构成一组欧拉角；
[0117] 设参考坐标系中重力加速度矢量为ar,ar＝[0 0 9.8]，地磁矢量为mr，由地磁模型计算得到，则有：
[0118]
[0119] 将公式(2)带入公式(3)，得到：
[0120]
[0121]
[0122] 式中，ax为加速度计测量值a的x轴分量，ay为加速度计测量值a的y轴分量，az为加速度计测量值a的z轴分量；
[0123] 利用俯仰角估计值 和横滚角估计值 构建物体坐标系至水平坐标系变换矩阵变换矩阵 表示为：
[0124]
[0125] 将物体坐标系中的地磁矢量mb变换为水平坐标系中的地磁矢量mp，则有：
[0126]
[0127] 将公式(6)带入公式(7)，得到：
[0128]
[0129] 式中，mpx为地磁矢量mp的x轴分量，mpy为地磁矢量mp的y轴分量，mrx为地磁矢量mr的x轴分量，mry为地磁矢量mr的y轴分量；
[0130] 由公式(4)、公式(5)及公式(8)计算得到俯仰角估计值 横滚角估计值 航向角估计值
[0131] 具体地，加速度计测量值a、磁传感器测量值m进行矢量坐标变换之前，通过补偿滤波器分别对加速度计测量值a、磁传感器测量值m进行频域补偿。具体地，通过补偿滤波器进行频域补偿的具体方法为：将加速度计测量值a输入补偿滤波器，补偿滤波器的输出即为补偿后的加速度计测量值a，将磁传感器测量值m输入补偿滤波器，补偿滤波器的输出即为补偿后的加速度计测量值m，补偿滤波器的传递函数为(T1s+1)/(T2s+1)，在应用中可根据加速度计与磁传感器测量特性确定T1与T2取值。通过补偿滤波器函数对测量信息进行频域补偿，补偿信息损失，有效抑制测量噪声，提升了高频段参数估计结果，进而提高航姿估计效果。
[0132] S3、以陀螺测量值ω为输入，利用航姿四元数动态递推方程构建四元数递推计算模型，计算各时刻航姿四元数，利用航姿四元数计算航向角估计值 俯仰角估计值 及横滚角估计值
[0133] 具体地，利用航姿四元数计算航向角估计值 俯仰角估计值 及横滚角估计值的具体步骤为：
[0134] 以陀螺测量值ω为输入，利用航姿四元数动态递推方程构建四元数递推计算模型，四元数递推计算模型表示为：
[0135]
[0136] 式中，qk为k采样时刻的四元数，qk+1为k+1采样时刻的四元数，k为采样时刻，k＝0,1,2,3，Δt为采样间隔，ωk为k采样时刻的陀螺测量值，[ωk×]为陀螺测量值ωk的反对称矩阵；
[0137] 利用四元数构建变换矩阵 表示为：
[0138]
[0139] 式中，q0为四元数的实数分量，q1为四元数虚数部分的第一分量，q2为四元数虚数部分的第二分量，q3为四元数虚数部分的第三分量；
[0140] 联立公式(2)和公式(4)得到：
[0141]
[0142]
[0143]
[0144] 将公式(9)计算得到的四元数带入公式(12)‑(14)得到航向角估计值 俯仰角估计值 横滚角估计值
[0145] S4、依据传递函数和为1的原则分别构建低通滤波器与高通滤波器，低通滤波器以航向角估计值 俯仰角估计值 及横滚角估计值 为输入，高通滤波器以航向角估计值俯仰角估计值 横滚角估计值 为输入，将低通滤波器处理后的航姿参数估计结果与高通滤波器处理后的航姿参数估计结果相加得到航向角估计值 俯仰角估计值 及横滚角估计值
[0146] 具体地，得到航向角估计值 俯仰角估计值 及横滚角估计值 的具体步骤为：
[0147] 依据传递函数和为1的原则应用一阶环节平方构建单参数二阶的低通滤波器和高通滤波器，低通滤波器的传递函数为Fl，高通滤波器的传递函数为Fh；
[0148] 低通滤波器以航向角估计值 俯仰角估计值 及横滚角估计值 为输入，高通滤波器以航向角估计值 俯仰角估计值 横滚角估计值 为输入，将低通滤波器处理后的航姿参数估计结果与高通滤波器处理后的航姿参数估计结果相加，则有：
[0149]
[0150]
[0151]
[0152] 由公式(15)‑(17)计算得到航向角估计值 俯仰角估计值 及横滚角估计值[0153] 所述低通滤波器和高通滤波器的传递函数分别表示为：
[0154]
[0155]
[0156] 式中，τ为频域特性调节参数，s为复频域变量。
[0157] 以下以具体的实施例对上述方法的估计效进行说明。设置加速度频域补偿参数T1为0.18，T2为0.05，磁传感器频域补偿参数T1为0.27，T2为0.05，互补滤波器参数τ为0.8。以通过BWT901CL型传感器(内含加速度计、陀螺及磁传感器)获取加速度计、磁传感器及陀螺测量数据为例，航姿估计结果参见图2‑4，图中，点画线为工作模式一的航姿估计结果，虚线为工作模式二的航姿估计结果，实现为工作模式三的航姿估计结果。
[0158] 上述实施例用来解释本发明，而不是对本发明进行限制，在本发明的精神和权利要求的保护范围内，对本发明做出的任何修改和改变，都落入本发明的保护范围。
