CN103995152A - 一种外场环境三维测量加速度计误差无奇异估计方法 - Google Patents

Abstract

一种外场环境三维测量加速度计无奇异误差估计方法，本发明涉及一种无需专门设备、在现场或者外场环境无奇异估计三维加速度计误差的方法。该方法首先确定外场估计三维加速度计误差所需采集的全部姿态信息；其次，建立加速度计误差模型，明确需要估计的未知参数；第三，将三维加速度计误差模型未知参数估计问题转化为椭球几何模型参数的拟合问题；第四，在三维加速度计误差模型参数估计中采用一种避免奇异点的椭球拟合算法；第五，在三维加速度计误差模型参数估计中采用修正椭球模型解决了标度因子和安装误差矩阵不唯一问题。本发明适用于以低成本、小体积、参数稳定性较低的加速度计作为传感器的小型无人机、机器人、手持终端等系统。

Description

一种外场环境三维测量加速度计误差无奇异估计方法

技术领域

本发明涉及一种外场环境三维测量加速度计误差无奇异估计方法，是一种简便易行、无需专门设备的惯性传感器组合外场误差估计方法，同时估计三维测量加速度计的零偏、标度因子和三维安装误差。估计过程中设定约束条件避免因奇异而无法估计三维测量加速度计误差模型参数，采用修正椭球模型，避免所估计模型参数不唯一的问题。本发明适用于采用低成本、小体积、参数稳定性较低的加速度计作为传感器的小型无人机、机器人、手持终端等系统中，提高其实际使用精度，具有很高的工程实用价值。

背景技术

三维测量加速度计经常作为一种重要传感器用于小型无人机、机器人、手持终端等多种系统中实现运动测量。为了实现系统的小型化、低成本、低功耗，传感器也必须具备低成本、小体积、低功耗等特征，其性能往往偏低，主要体现为误差模型参数不稳定。为了提高三维测量加速度计的实际使用精度，迫切需要一种现场估计误差并进行补偿的技术。为了提高操作的便捷性、降低对系统资源的耗费，估计补偿算法应该具有在线运行、占用系统资源少的特点。目前尚无现场估计三维测量加速度计误差模型参数的有效途径。

传统的加速度计误差模型估计需要借助分度头等专门仪器设备，在实验室环境下进行，即提前离线确定误差模型的各个参数，并在使用前进行固定的补偿。一旦加速度计误差模型参数因为使用时间、环境温度、压力等发生变化，固定补偿必然导致较大误差残留，严重影响使用精度，必须进行现场误差估计和补偿。这一点对小体积、低成本传感器尤其重要。

三维测量磁罗盘在使用中常常受到周围磁环境的干扰，因此该领域较早使用了现场误差估计技术：转动三维测量磁罗盘并采集各个姿态下地磁场在三维磁传感器上的分量。不同之处在于磁罗盘可以连续旋转连续采集，本发明方法需要三维测量加速度计在各个姿态下保持静止，去除转动过程中的数据。另一个区别在于目前的三维测量磁罗盘现场误差模型参数估计方法未能考虑避免估计奇异值的出现，常常导致估计失败。少数方法仅考虑了二维测量情况的去奇异条件。为此，本发明针对三维测量加速度计提出了一种外场环境三维测量加速度计误差无奇异估计方法。

发明内容

本发明的技术解决问题是：克服现有技术的不足，提供一种适用于以三维测量加速度计作为传感器的小型无人机、机器人、手持终端等系统的外场环境三维测量加速度计误差无奇异估计方法。

本发明的技术解决方案为：一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于包括以下步骤：

(1)确定外场估计三维加速度计误差模型参数所需采集的全部姿态；

(2)建立加速度计误差模型和被估计参数；

(3)将三维加速度计误差模型参数估计问题转化为椭球几何模型参数拟合问题，建立加速度计误差模型参数与椭球几何模型参数的对应关系；

(4)确定避免奇异的参数估计约束条件；

(5)建立约束条件下的最小二乘参数估计算法；

(6)在三维加速度计误差模型参数估计中采用修正椭球模型，避免所估计模型参数不唯一的问题；

(7)利用加速度计误差模型参数与椭球几何模型参数的对应关系，解算三维正交加速度计误差模型参数。

本发明的原理是：加速度计在静止情况下能够测量到重力加速度的投影分量，三维测量加速度计三个测量值的平方和在静止情况下应该等于重力加速度的平方。但三个测量值的具体数值会因为姿态不同而输出重力加速度不同的投影分量，换言之，三维测量加速度计三个测量值体现了三维测量加速度计当前的姿态信息，因此三维测量加速度计的误差对姿态测量精度影响很大，必须加以估计和补偿。

传统的加速度计误差模型估计需要借助分度头等专门仪器设备，在实验室环境下进行，即提前离线确定误差模型的各个参数，并在使用前进行固定的补偿。一旦加速度计误差模型参数因为使用时间、环境温度、压力等发生变化，固定补偿必然导致较大误差残留，严重影响使用精度，必须进行现场误差估计和补偿。三维测量加速度计的误差包括零偏、标度因子和三轴相互之间的非正交误差。V_ax，V_ay，V_az表示三维加速度计的输出测量值，B_ax，B_ay，B_az表示三维测量加速度计的零偏，K_ax，K_ay，K_az表示三维测量加速度计的标度因子，E_axy，E_axz，E_ayx，E_ayz，E_azx，E_azy表示三维测量加速度计三轴相互之间的非正交误差，A_x，A_y，A_z表示三轴真实输入加速度值。具体模型如下：

$\begin{array}{c}{V}_{\mathrm{ax}}=B_{\mathrm{ax}}+K_{\mathrm{ax}}(A_x+E_{\mathrm{axy}}{A}_y+E_{\mathrm{axz}}{A}_z)\\ V_{\mathrm{ay}}=B_{\mathrm{ay}}+K_{\mathrm{ay}}(E_{\mathrm{ayx}}{A}_x+A_y+E_{\mathrm{ayz}}{A}_z)\\ V_{\mathrm{az}}=B_{\mathrm{az}}+K_{\mathrm{az}}(E_{\mathrm{azx}}{A}_x+E_{\mathrm{azy}}{A}_y+A_z)\\ V=B+\left[\begin{array}{ccc}{K}_{\mathrm{ax}}& K_{\mathrm{ax}}{E}_{\mathrm{axy}}& K_{\mathrm{ax}}{E}_{\mathrm{axz}}\\ K_{\mathrm{ay}}{E}_{\mathrm{axy}}& K_{\mathrm{ay}}& K_{\mathrm{ay}}{E}_{\mathrm{ayz}}\\ K_{\mathrm{az}}{E}_{\mathrm{azx}}& K_{\mathrm{az}}{E}_{\mathrm{azy}}& K_{\mathrm{az}}\end{array}\right]\·\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=B+\mathrm{KA}\\ A=K^{-1}(V-B)\end{array}---\left(1\right)$

三维测量加速度计误差估计是通过多个姿态测量值V＝[V_axV_ayV_az]^T，估计B_ax，B_ay，B_az，K_ax，K_ay，K_az，E_axy，E_axz，E_ayx，E_ayz，E_azx，E_azy。假设没有测量误差，以三维加速度计测量值为坐标构成空间矢量，转动三维测量加速度计，其矢端在空间的轨迹位于标准球面上，球心位于原点，半径为重力加速度数值的平方||g||²。

$V^{T}V=V_{\mathrm{az}}^2+V_{\mathrm{ay}}^2+V_{\mathrm{az}}^2={\left|\right|g\left|\right|}^2$

存在误差的实际情况下以三维加速度计测量值为坐标构成空间矢量，转动三维测量加速度计，其矢端在空间的轨迹位于椭球面上，椭球几何参数与B_ax，B_ay，B_az，K_ax，K_ay，K_az，E_axy，E_axz，E_ayx，E_ayz，E_azx，E_azy有函数关系。

$\begin{array}{c}\left|\right|A^T\·A\left|\right|=\left|\right|{\left(K^{-1}\right[V-B\left]\right)}^T\·\left(K^{-1}\right[V-B\left]\right)\left|\right|={\left|\right|g\left|\right|}^2\\ {[V-B]}^T{\left(K^{-1}\right)}^T\·K^{-1}[V-B]=g^2\end{array}$

(2)式等价为椭球方程

(V-X₀)^TM(V-X₀)＝1      (3)三轴加速度计的零偏B＝[B_axB_ayB_az]^T对应椭球中心的位移X₀＝B， $M=\frac{1}{g^2}{\left(K^{-1}\right)}^T\·K^{-1}.$ 若将展开(3)式

$\begin{array}{c}{m}_{11}{v}_x^2+m_{22}{v}_y^2+m_{33}{v}_z^2+{2m}_{12}{v}_x{v}_y+{2m}_{13}{v}_x{v}_z+{2m}_{23}{v}_y{v}_z\\ -2(m_{11}{v}_{x0}+m_{12}{v}_{y0}+m_{13}{v}_{z0})v_x\\ -2(m_{12}{v}_{x0}+m_{22}{v}_{y0}+m_{23}{v}_{z0})v_y\\ -2(m_{13}{v}_{x0}+m_{23}{v}_{y0}+m_{33}{v}_{z0})v_z\\ +\left[\begin{array}{ccc}{v}_{x0}& v_{y0}& v_{z0}\end{array}\right]\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}{v}_{x0}\\ v_{y0}\\ v_{z0}\end{array}\right]=1\end{array}$ 若写做，

$\begin{array}{c}{\mathrm{av}}_x^2+{\mathrm{bv}}_y^2+{\mathrm{cv}}_z^2+2{\mathrm{dv}}_x{v}_y+{2\mathrm{ev}}_x{v}_z+{2\mathrm{fv}}_y{v}_z-{2\mathrm{pv}}_x-{2\mathrm{qv}}_y-{2\mathrm{rv}}_z+l=\stackrel{\→}{a}\·\stackrel{\→}{x}=0\\ \stackrel{\→}{a}=\left[\begin{array}{cccccccccc}a& b& c& d& e& f& p& q& r& l\end{array}\right]\\ \stackrel{\→}{x}=\left[\begin{array}{cccccccccc}{v}_x^2& v_y^2& v_z^2& v_x{v}_y& v_x{v}_z& v_y{v}_z& v_x& v_x& v_x& 1\end{array}\right]\end{array}---\left(4\right)$

则， $M=\frac{1}{g^2}{\left(K^{-1}\right)}^T\·K^{-1}=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]=\left[\begin{array}{ccc}a& d& e\\ d& b& f\\ e& f& c\end{array}\right],X_0=B=M^{-1}\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$

根据采集的三维测量加速度计的输出组合拟合出椭球方程(4)，即可计算出K和B，带入(1)式实现对加速度计误差的估计和补偿。

传统椭球方程估计过程中常常出现以下问题：

●全零的奇异现象；

●现有二维椭圆拟合抗奇异方法无法保证三维椭球拟合无奇异；

●在没有发生奇异的情况下，同一批测量值进行多次拟合结果不相同，表现为成比例关系M1＝c₂·M2＝c₃·M3＝......。

本发明在(4)式的拟合条件之外，增加两个约束条件：

●保证拟合结果为椭球方程，式中

$\text{C=}\left[\begin{array}{cccccccccc}0& 0& 2& 0& 0& 0& 0& 0& 0& 0\\ 0& -1& 0& 0& 0& 0& 0& 0& 0& 0\\ 2& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

●修改[V-B]^T(K^-1)^T·K^-1[V-B]＝g²等式右面g²为

的实际值，避免椭球参数估计的不唯一性。

本发明与现有技术相比的优点在于：本发明方法确定外场估计三维加速度计误差所需采集的全部姿态信息；第二，将三维加速度计误差模型未知参数估计问题转化为椭球几何模型参数的拟合问题；第三，在三维加速度计误差模型参数估计中采用一种避免奇异点的递推式椭球拟合算法；第四，在三维加速度计误差模型参数估计中采用修正椭球模型解决了标度因子和安装误差矩阵不唯一的问题。

附图说明

图1为本发明的外场环境三维测量加速度计无奇异误差估计方法流程图。

具体实施方式

如图1所示，本发明的具体方法如下：

1、首先确定外场估计三维加速度计误差所需采集的全部姿态信息；

即保证每维加速度计尽可能在铅垂平面内重力加速度最大、最小、零值等多个位置静止停留采集数据。

2、建立加速度计误差模型，明确需要估计的未知参数；

三维测量加速度计的误差包括零偏、标度因子和三轴相互之间的非正交误差。V_ax，V_ay，V_az表示三维加速度计的输出测量值，B_ax，B_ay，B_az表示三维测量加速度计的零偏，K_ax，K_ay，K_az表示三维测量加速度计的标度因子，E_axy，E_axz，E_ayx，E_ayz，E_azx，E_azy表示三维测量加速度计三轴相互之间的非正交误差，A_x，A_y，A_z表示三轴真实输入加速度值。具体模型如下：

$\begin{array}{c}{V}_{\mathrm{ax}}=B_{\mathrm{ax}}+K_{\mathrm{ax}}(A_x+E_{\mathrm{axy}}{A}_y+E_{\mathrm{axz}}{A}_z)\\ V_{\mathrm{ay}}=B_{\mathrm{ay}}+K_{\mathrm{ay}}(E_{\mathrm{ayx}}{A}_x+A_y+E_{\mathrm{ayz}}{A}_z)\\ V_{\mathrm{az}}=B_{\mathrm{az}}+K_{\mathrm{az}}(E_{\mathrm{azx}}{A}_x+E_{\mathrm{azy}}{A}_y+A_z)\\ V=B+\left[\begin{array}{ccc}{K}_{\mathrm{ax}}& K_{\mathrm{ax}}{E}_{\mathrm{axy}}& K_{\mathrm{ax}}{E}_{\mathrm{axz}}\\ K_{\mathrm{ay}}{E}_{\mathrm{axy}}& K_{\mathrm{ay}}& K_{\mathrm{ay}}{E}_{\mathrm{ayz}}\\ K_{\mathrm{az}}{E}_{\mathrm{azx}}& K_{\mathrm{az}}{E}_{\mathrm{azy}}& K_{\mathrm{az}}\end{array}\right]\·\left[\begin{array}{c}{A}_x\\ A_y\\ A_z\end{array}\right]=B+\mathrm{KA}\\ A=K^{-1}(V-B)\end{array}---\left(1\right)$

三维测量加速度计误差估计是通过多个姿态测量值V＝[V_axV_ayV_az]^T，估计B_ax，B_ay，B_az，K_ax，K_ay，K_az，E_axy，E_axz，E_ayx，E_ayz，E_azx，E_azy。

3、将三维加速度计误差模型未知参数估计问题转化为椭球几何模型参数的拟合问题；

存在误差的实际情况下以三维加速度计测量值为坐标构成空间矢量，转动三维测量加速度计，其矢端在空间的轨迹位于椭球面上，椭球几何参数与B_ax，B_ay，B_az，K_ax，K_ay，K_az，E_axy，E_axz，E_ayx，E_ayz，E_azx，E_azy有函数关系。

$\begin{array}{c}\left|\right|A^T\·A\left|\right|=\left|\right|{\left(K^{-1}\right[V-B\left]\right)}^T\·\left(K^{-1}\right[V-B\left]\right)\left|\right|={\left|\right|g\left|\right|}^2\\ {[V-B]}^T{\left(K^{-1}\right)}^T\·K^{-1}[V-B]=g^2\end{array}$

(2)式等价为椭球方程

(V-X₀)^TM(V-X₀)＝1        (3)三轴加速度计的零偏B＝[B_axB_ayB_az]^T对应椭球中心的位移X₀＝B， $M=\frac{1}{g^2}{\left(K^{-1}\right)}^T\·K^{-1}.$ 若将展开(3)式

$\begin{array}{c}{m}_{11}{v}_x^2+m_{22}{v}_y^2+m_{33}{v}_z^2+{2m}_{12}{v}_x{v}_y+{2m}_{13}{v}_x{v}_z+{2m}_{23}{v}_y{v}_z\\ -2(m_{11}{v}_{x0}+m_{12}{v}_{y0}+m_{13}{v}_{z0})v_x\\ -2(m_{12}{v}_{x0}+m_{22}{v}_{y0}+m_{23}{v}_{z0})v_y\\ -2(m_{13}{v}_{x0}+m_{23}{v}_{y0}+m_{33}{v}_{z0})v_z\\ +\left[\begin{array}{ccc}{v}_{x0}& v_{y0}& v_{z0}\end{array}\right]\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]\left[\begin{array}{c}{v}_{x0}\\ v_{y0}\\ v_{z0}\end{array}\right]=1\end{array}$ 若写做，

$\begin{array}{c}{\mathrm{av}}_x^2+{\mathrm{bv}}_y^2+{\mathrm{cv}}_z^2+2{\mathrm{dv}}_x{v}_y+{2\mathrm{ev}}_x{v}_z+{2\mathrm{fv}}_y{v}_z-{2\mathrm{pv}}_x-{2\mathrm{qv}}_y-{2\mathrm{rv}}_z+l=\stackrel{\→}{a}\·\stackrel{\→}{x}=0\\ \stackrel{\→}{a}=\left[\begin{array}{cccccccccc}a& b& c& d& e& f& p& q& r& l\end{array}\right]\\ \stackrel{\→}{x}=\left[\begin{array}{cccccccccc}{v}_x^2& v_y^2& v_z^2& v_x{v}_y& v_x{v}_z& v_y{v}_z& v_x& v_x& v_x& 1\end{array}\right]\end{array}---\left(4\right)$

则， $M=\frac{1}{g^2}{\left(K^{-1}\right)}^T\·K^{-1}=\left[\begin{array}{ccc}{m}_{11}& m_{12}& m_{13}\\ m_{21}& m_{22}& m_{23}\\ m_{31}& m_{32}& m_{33}\end{array}\right]=\left[\begin{array}{ccc}a& d& e\\ d& b& f\\ e& f& c\end{array}\right],X_0=B=M^{-1}\left[\begin{array}{c}p\\ q\\ r\end{array}\right]$ 根据采集的三维测量加速度计的输出组合拟合出椭球方程(4)，即可计算出K和B，带入(1)式实现对加速度计误差的估计和补偿。

4、在三维加速度计误差模型参数估计中采用一种避免奇异点的递推式椭球拟合算法；

增加约束条件：保证拟合结果为椭球方程，式中

$\text{C=}\left[\begin{array}{cccccccccc}0& 0& 2& 0& 0& 0& 0& 0& 0& 0\\ 0& -1& 0& 0& 0& 0& 0& 0& 0& 0\\ 2& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0& 0& 0& 0& 0& 0\end{array}\right]$

5、在三维加速度计误差模型参数估计中采用修正椭球模型解决了标度因子和安装误差矩阵不唯一的问题。

修改[V-B]^T(K^-1)^T·K^-1[V-B]＝g²等式右面g²为

的实际值，避免椭球参数估计的不唯一性。

Claims (8)

1.一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于包括以下步骤：

(1)确定外场估计三维加速度计误差模型参数所需的全部姿态；

(2)建立加速度计误差模型和被估计参数；

(3)将三维加速度计误差模型参数估计问题转化为椭球几何模型参数拟合问题，建立加速度计误差模型参数与椭球几何模型参数的对应关系；

(4)确定避免奇异的参数估计约束条件；

(5)修正椭球模型；

(6)建立约束条件下修正椭球模型的最小二乘估计算法；

(7)解算三维正交加速度计误差模型参数。

2.根据权利要求1所述的一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于：外场估计三维加速度计误差模型参数所需采集姿态集的确定原则是保证每个加速度计在铅垂平面内遍历重力加速度最大、最小、零值位置。

3.根据权利要求1所述的一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于：所采用的误差模型可以在外场同时估计出三维测量加速度计的零偏、标度因子和三维安装误差。

4.根据权利要求1所述的一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于：外场估计三维加速度计误差所需采集的全部姿态信息均为翻转完成后的静态姿态数据，去除任何翻转过程中的动态数据。

5.根据权利要求1所述的一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于：翻转过程中无需任何专用设备。

6.根据权利要求1所述的一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于：建立三维加速度计误差模型与椭球几何模型的参数对应关系，可根据椭球几何模型参数计算全部三维加速度计误差模型参数。

7.根据权利要求1所述的一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于：采用拉格朗日乘子法将避免奇异的约束条件与常规最小二乘算法结合，将有约束的三维测量加速度计误差估计最小二乘算法转化为无约束的最小二乘算法。

8.根据权利要求1所述的一种外场环境三维测量加速度计误差无奇异估计方法，其特征在于：采用修正椭球模型，避免所估计模型参数不唯一的问题。