[发明专利]一种附有GNSS速度矢量辅助的车载姿态估计方法

权利要求书

1.一种附有GNSS速度矢量辅助的车载姿态估计方法，包括以下步骤：

步骤1：利用陀螺仪数据进行姿态推算；

步骤2：利用加速度计和陀螺仪对GNSS速度进行检测：

步骤2-1：利用长度为l的滑动递推窗口对GNSS速度进行p阶多项式拟合，进一步对其求导，获得运动加速度拟合多项式；对GNSS速度得到的运动加速度与陀螺仪和加速度计计算的运动加速度进行比较，检验GNSS速度是否异常；

步骤2-2：利用GNSS速度进行俯仰角求解，将其与陀螺仪获得的俯仰角进行比较，检验GNSS速度是否异常；

步骤3：利用GNSS速度对运动加速度进行补偿，进行陀螺仪/加速度计一级滤波；

取3维姿态误差q_ξ，陀螺漂移误差b_g和加速度计零偏误差b_a构建滤波状态矢量并建立传感器漂移噪声模型：

其中，顶标·表示一阶导数运算，表示陀螺漂移模型噪声，表示噪声服从0均值方差为的高斯分布；表示加速度计零偏模型噪声，表示噪声服从0均值方差为的高斯分布；

记状态转移模型噪声可建立系统状态模型：

其中，转移矩阵B与系统噪声方差Q分别为：

其中，R_g表示陀螺随机噪声的方差；表示陀螺漂移模型噪声的方差；表示加速度计零偏模型的噪声方差；[·×]表示反对称矩阵运算；进一步地，在系统状态模型的基础上，离散化的系统状态模型可写为：

其中，x_k表示k时刻的状态矢量，表示离散化的系统状态模型噪声，状态转移矩阵为Φ_k,k-1＝I_9×9+BΔt_s；

利用加速度计建立量测模型：

其中，表示k时刻的量测信息，H_a,k表示k时刻的加速度计量测模型矩阵，z_a,k表示k时刻的加速度计输出，表示利用陀螺估计的姿态四元数，表示k时刻的运动加速度估计；基于线性卡尔曼滤波进行状态估计，获得融合加速度计信息后的状态及方差P_a,k；将一级滤波的结果更新至姿态四元数并做归一化处理：

其中，表示一级滤波k时刻融合加速度计信息后的姿态误差，表示一级滤波k时刻融合加速度计信息后的状态估计量，表示取矢量的第1至第3个分量；表示k时刻利用陀螺估计的姿态四元数；表示一级滤波k时刻融合加速度计信息后的姿态四元数；用更新并对状态矢量中q_ξ置零；

步骤4：利用GNSS速度检测排除磁力计模型中的磁场干扰；

步骤5：进行二级滤波，融合磁力计数据；

进行二级滤波状态及其方差一步预测：

其中，表示k时刻融合加速度计信息后的状态估计，在步骤3中获得，其方差为P_a,k；表示k时刻融合磁力计信息后的状态一步预测，表示状态方差，P_a,k(1:3,1:3)表示取矩阵P_a,k第1至3行，1至3列的矩阵；

建立磁力计量测模型：

其中，z_m表示磁力计输出，mⁿ表示n系下的地磁参考矢量；ε_m为磁力计噪声，将模型记为

其中，量测信息量测矩阵融合加速度计后的姿态四元数通过步骤3中(23)获得；为保证磁力计数据仅用于偏航角修正，而不影响俯仰和滚转，引入约束矩阵计算系统滤波增益K_m,k：

其中，R_m表示磁力计的噪声方差，一步预测方差由公式(8)获得；约束矩阵Λ_m,k为：

进行二级滤波观测修正：

其中，表示k时刻融合磁力计信息后的状态估计，P_m,k表示的方差；一步预测值由(7)计算获得，增益K_m,k利用(11)计算获得；利用二级滤波的结果更新姿态四元数并做归一化处理：

其中，表示二级滤波k时刻融合磁力计信息后的姿态误差，表示二级滤波k时刻融合磁力计信息后的状态估计量，见(13)；为矢量的第1至第3个分量；表示一级滤波k时刻融合加速度计信息后的姿态四元数，见(6)；表示二级滤波k时刻融合磁力计信息后的姿态四元数；用更新并对状态矢量中q_ξ置零；

步骤6：进行三级滤波，融合GNSS速度信息；具体步骤如下：

步骤6-1：计算三级滤波状态及其方差的一步预测：

对于三级滤波，计算状态及其方差的一步预测：

其中，表示k时刻融合GNSS速度信息后的状态一步预测，表示k时刻融合磁力计信息后的状态估计量；和P_m,k分别为和的方差；

步骤6-2：利用GNSS速度建立载体速度矢量对方程，进一步建立三级滤波量测方程；

利用GNSS速度建立载体速度矢量对方程：

其中，表示b系下的速度矢量估计，v^e表示e系下的速度矢量，表示由e系到b系的旋转矩阵，ε₁表示噪声，b系下载体的速度可表示为

式(37)中，噪声ε₁包含两类影响：一为车轮轨迹和车辆b系X轴的定义方向存在偏差；二为由路面引起车辆滑动或跳跃，导致在Y轴或Z轴存在速度；GNSS速度观测模型有：

其中，表示GNSS速度观测，ε₂表示噪声，将(39)带入(37)，得GNSS速度矢量对方程：

记式(40)可写作

根据误差传播定律确定ε_s的方差R_s：

其中，R₁和R₂分别表示噪声ε₁和ε₂的方差；利用GNSS速度建立三级滤波量测方程

其中，观测信息为观测矩阵为z_s,k表示k时刻的z_s；为步骤5中融合磁力计信息后的姿态四元数；

步骤6-3：建立三级滤波滚转/偏航-滚转约束方程：

利用Z轴角速率输出z_g(3)(t)检测载体是否存在转弯：

由于GNSS速度无法测量载体滚转角，在三级滤波中，GNSS速度不对二级滤波的姿态滚转角修正；据此可建立滚转约束方程：

其中，矩阵下标(2,3)表示取矩阵第2行第3列的元素；(3,3)表示取矩阵第3行第3列的元素；

其中，表示三级滤波中融合GNSS速度后的姿态四元数估计；表示与姿态真值间的误差四元数；表示三级滤波中融合GNSS速度后的状态矢量估计；向量下标(1)，(2)，(3)，以及下文出现的(4)分别表示取向量的第1、第2、第3、第4个元素；

将滚转约束方程改写成如下形式

其中，约束矩阵为

在车辆转弯时，GNSS速度方向与载体偏航角方向存在偏差；为抑制其对偏航角的影响，在转弯情况下建立偏航-滚转约束方程，约束矩阵为

步骤6-4：基于贝叶斯理论进行最优估计，得到含滚转/偏航-滚转约束的三级滤波状态估计值，完成姿态估计：

基于贝叶斯理论建立如下最优问题：

其中，V_k和分别表示观测和预测的残差，

引入拉格朗日算子，

对其求一阶偏导数，有

其中，表示求偏导数运算；最终得有无滚转约束的状态估计和间的关系

其中，表示未进行滚转约束的滤波估计值，其方差为表示进行滚转约束的滤波估计值，其方差为P_s,k；利用三级滤波的结果更新姿态四元数并做归一化处理：

其中，表示三级滤波中融合GNSS速度后的姿态四元数估计；表示三级滤波姿态误差四元数；表示三级滤波中融合GNSS速度后的状态矢量估计；为的第1至第3个元素；表示二级滤波中融合磁力计信息后的姿态四元数。