CN103471594B - 基于ahrs的精对准算法 - Google Patents

Abstract

本发明公开了一种基于AHRS的精对准算法，主要解决了现有技术中存在的精对准技术收敛较慢、误差估计精度不高，不能满足人们需求的问题。该基于AHRS的精对准算法采用两个方向上的加速计的输出和陀螺仪三个方向上的输出做观测值，并采用卡尔曼滤波求姿态角的误差，进而进行精对准。通过上述方案，本发明达到了收敛较快，精度较高的目的，具有很高的实用价值和推广价值。

Description

基于AHRS的精对准算法

技术领域

本发明涉及一种导航领域的对准算法，具体地说，是涉及一种基于AHRS的精对准算法。

背景技术

传统的精对准技术观测量比较少，大都只用东向和北向的速度做观测值或只用东向和北向加速度计做观测值，该种对准方式虽然对姿态角的误差有一定的估计作用，但是收敛很慢，且误差估计精度不高。现有技术中大都采用多传感器的输出做观测值、采用东向和北向的速度和陀螺仪的三个方向的角速度共五个值做观测值，以此提高收敛速度和精度，但这种方式的对准度仍然较为有限，不能满足人们需求。

发明内容

本发明的目的在于提供一种基于AHRS的精对准算法，主要解决现有技术中存在的精对准技术收敛较慢、误差估计精度不高，不能满足人们需求的问题。

为了实现上述目的，本发明采用的技术方案如下：

基于AHRS的精对准算法，包括以下步骤：

(1)采集当前的纬度信息和重力加速度信息；

(2)根据AHRS中加速度计和陀螺仪的输出测得当前的姿态转移矩阵C_bp；

(3)测算出地球相对于惯性坐标系的旋转角速度w_ie；

(4)确定当前X轴向的角度误差、Y轴向的角度误差、Z轴向的角度误差、陀螺仪在X轴向的常值偏移、陀螺仪在Y轴向的常值偏移、陀螺仪在Z轴向的常值偏移、加速度计在X轴向的常值偏移和加速度计在Y轴向的常值偏移的被估计值；

(5)根据步骤(4)中被估计值前后两个时刻的关系得出连续卡尔曼滤波下的状态转移矩阵A，并根据公式F＝eye(8)+A×t；将其转换为离散卡尔曼滤波下的状态转移矩阵，其中，t为滤波间隔；

(6)根据当前噪声和观测值前后两个时刻间的关系求出噪声驱动矩阵G，根据被估计值和观测值之间的关系求出观测矩阵H，根据陀螺仪的随机误差确定AHRS中的***噪声方差矩阵，并根据加速度计的随机误差确定观测噪声方差矩阵；

(7)将被估计值的初始滤波值设置为零；

(8)开始卡尔曼滤波，每次取两个方向上的加速度计的输出和陀螺仪的三个方向上的输出作为观测值；

(9)根据求得的滤波结果和方差更新被估计值和观测噪声方差的数据，并再次取观测值进行卡尔曼滤波；

(10)返回步骤(8)，直至两次滤波结果小于用户设定的阈值；

(11)使用求得的X轴向、Y轴向和Z轴向上的姿态角的误差对姿态转移矩阵C_bp进行纠正。

具体地说，所述步骤(2)具体包括以下步骤：

(2a)根据 ${\stackrel{\→}{g}}^t=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$ 得出重力加速度在地理坐标系下的投影其中g为重力加速度；根据 ${\stackrel{\→}{w}}_{ie}^t=\left[\begin{array}{c}0\\ w_{ie}\cos L\\ w_{ie}\cos L\end{array}\right]$ 得出地球绕惯性坐标系自转角速度在地理坐标系下的投影其中w_iε为地球绕惯性轴的自转角速度，L为纬度；

(2b)根据公式构造坐标系下的三个向量和

(2c)根据方程式 $\left\{\begin{array}{c}\stackrel{\→}{{r_1}^b}=C_t^b\stackrel{\→}{{r_1}^t}\\ \stackrel{\→}{{r_2}^b}=C_t^b\stackrel{\→}{{r_2}^t}\\ \stackrel{\→}{{r_3}^b}=C_t^b\stackrel{\→}{{r_3}^t}\end{array}\right.$ 解得矩阵其中，t为时间间隔，将以平台坐标系表示；

(2d)根据公式计算出姿态转移矩阵C_bp。

与现有技术相比，本发明具有以下有益效果：

(1)本发明中，巧妙地采用两个方向上的加速计的输出和陀螺仪三个方向上的输出做观测值，并采用卡尔曼滤波求姿态角的误差，从而大大提高了收敛速度，并有效地增加了对准精度，符合技术发展需求，具有突出的实质性特点和显著进步，适合大规模推广应用。

具体实施方式

下面结合实施例对本发明作进一步说明，本发明的实施方式包括但不限于下列实施例。

实施例

为了解决现有技术中存在的精对准技术收敛较慢、误差估计精度不高，不能满足人们需求的问题，本发明公开了一种基于AHRS的精对准算法，通过以下步骤实现：

第一步：通过外部设备，如经纬度测量仪和重力加速度测量仪获得纬度信息和重力加速度信息；

第二步：进行粗对准，即利用AHRS(航姿参考***)中的加速度计和陀螺仪的输出测得当前的姿态转移矩阵C_bp，此时该姿态转移矩阵的精度较差；

根据 ${\stackrel{\→}{g}}^t=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$ 得出重力加速度在地理坐标系下的投影其中g为重力加速度；根据 ${\stackrel{\→}{w}}_{ie}^t=\left[\begin{array}{c}0\\ w_{ie}\cos L\\ w_{ie}\cos L\end{array}\right]$ 得出地球绕惯性坐标系自转角速度在地理坐标系下的投影其中w_iε为地球绕惯性轴的自转角速度，L为纬度；根据构造载体坐标系下的三个向量根据方程式 $\left\{\begin{array}{c}\stackrel{\→}{{r_1}^b}=C_t^b\stackrel{\→}{{r_1}^t}\\ \stackrel{\→}{{r_2}^b}=C_t^b\stackrel{\→}{{r_2}^t}\\ \stackrel{\→}{{r_3}^b}=C_t^b\stackrel{\→}{{r_3}^t}\end{array}\right.$ 解得矩阵因为精度不高，这里写为平台坐标系更合适，则姿态转移矩阵 $C_{bp}={\left(C_p^b\right)}^T;$

第三步：确定地球相对于惯性坐标系的旋转角速度w_ie；

第四步：确定被估计值X＝[φ_xφ_yφ_zε_xε_yε_z▽_x▽_y]^T，其中，前三个值为要求的三个轴向，即X轴向、Y轴向、Z轴向的角度误差，中间三个值为陀螺仪常值偏移，开机后这个值既可作为定值，但每次开机不一样，可以多次测量取均值，最后两个量为加速度计的常值偏移，为了进行卡尔曼滤波，还需求出以下几个关键矩阵。

(1)求状态转移矩阵：

A＝zeros(8,8)；

A(1,2)＝w_ie×sin(L)；

A(1,3)＝-w_ie×cos(L)；

A(2,1)＝-w_ie×sin(L)；

A(3,1)＝w_ie×cos(L)；

A(1,4)＝-C_bp(1,1)；

A(1,5)＝-C_bp(1,2)；

A(1,6)＝-C_bp(1,3)；

A(2,4)＝-C_bp(2,1)；

A(2,5)＝-C_bp(2,2)；

A(2,6)＝-C_bp(2,3)；

A(3,4)＝-C_bp(3,1)；

A(3,5)＝-C_bp(3,2)；

A(3,6)＝-C_bp(3,3)；根据被估计值前后两个时刻的关系求出连续卡尔曼滤波下的状态转移矩阵A，并将它转换为离散卡尔曼滤波下的状态转移矩阵，以便于运算；

(2)根据F＝eye(8)+A×t；求出离散状态下的状态转移矩阵，其中，t为滤波间隔；

(3)根据当前噪声和观测值前后两个时刻间的关系，求出噪声驱动矩阵G，即噪声对状态转移的贡献矩阵，也为连续状态下的矩阵，同样离散化；

G＝zeros(8,8)；

G(1,1)＝-C_bp(1,1)；

G(1,2)＝-C_bp(1,2)；

G(1,3)＝-C_bp(1,3)；

G(2,1)＝-C_bp(2,1)；

G(2,2)＝-C_bp(2,2)；

G(2,3)＝-C_bp(2,3)；

G(3,1)＝-C_bp(3,1)；

G(3,2)＝-C_bp(3,2)；

G(3,3)＝-C_bp(3,3)；

G(4,4)＝1；

G(5,5)＝1；

G(6,6)＝1；

G(7,7)＝1；

G(8,8)＝1；

G0＝G×t；

(4)根据被估计值与观测值之间的关系求出观测矩阵H；

H＝zeros(5,8)；

H(1,2)＝-g；

H(1,7)＝1；

H(2,1)＝g；

H(2,8)＝1；

H(3,2)＝-w_ie×sin(L)；

H(3,3)＝w_ie×cos(L)；

H(3,4)＝1；

H(4,1)＝w_ie×sin(L)；

H(4,5)＝1；

H(5,1)＝-w_ie×cos(L)；

H(5,6)＝1；

(5)根据陀螺仪随机误差确定***噪声方差矩阵；

Q＝{zeros(8,8)+diag(q²,q²,q²,0,0,0,0,0)}/t其中q为陀螺仪产生的白噪声的方差；

(6)根据加速度计随机误差确定观测噪声方差阵，虽然理论上来说初始方差是随意的，在进行卡尔曼滤波的过程中会趋近真实值，但在具体应用时优选取为和各观测值误差相关的值，以利于加快收敛速度；

R＝{zeros(5,5)+diag(s²,s²,q²,q²,q²)}/t，其中s为加速度计产生的白噪声的方差。

在测算出上述值的基础上，进行以下步骤便可实现精对准：

第五步：设置被估计值X的初始滤波值为全零；

第六步：开始卡尔曼滤波，每次取两个方向上的加速度计的输出和陀螺仪的三个方向上的输出作为观测值，如 $Z={\left[\begin{array}{ccccc}\widehat{f_x^p}& \widehat{f_y^p}& \widehat{w_{ibE}^p}& \widehat{w_{ibN}^p}& \widehat{w_{ibU}^p}\end{array}\right]}^T;$

第七步：根据求得的滤波结果和方差更新被估计值X和观测噪声方差R的数据，并再次取观测值进行卡尔曼滤波；

第八步：反复进行第六步和第七部步，直到两次滤波结果小于一个阈值，该阈值由用户根据实际需求自行设定；

第九步：用求得的三个方向上的姿态角的误差纠正姿态转移矩阵C_bp，求得精度较高的姿态矩阵

按照上述实施例，便可很好地实现本发明。

Claims (2)

1.基于AHRS的精对准算法，其特征在于，包括以下步骤：

(1)采集当前的纬度信息和重力加速度信息；

(2)根据AHRS中加速度计和陀螺仪的输出测得当前的姿态转移矩阵C_bp；

(3)测算出地球相对于惯性坐标系的旋转角速度w_ie；

(4)确定当前X轴向的角度误差、Y轴向的角度误差、Z轴向的角度误差、陀螺仪在X轴向的常值偏移、陀螺仪在Y轴向的常值偏移、陀螺仪在Z轴向的常值偏移、加速度计在X轴向的常值偏移和加速度计在Y轴向的常值偏移的被估计值；

(5)根据步骤(4)中被估计值前后两个时刻的关系得出连续卡尔曼滤波下的状态转移矩阵A，并根据公式F＝eye(8)+A×t；将其转换为离散卡尔曼滤波下的状态转移矩阵，其中，t为滤波间隔，eye(8)为维度为8的单位矩阵；

(6)根据当前噪声和观测值前后两个时刻间的关系求出噪声驱动矩阵G，根据被估计值和观测值之间的关系求出观测矩阵H，根据陀螺仪的随机误差确定AHRS中的***噪声方差矩阵，并根据加速度计的随机误差确定观测噪声方差矩阵；

(7)将被估计值的初始滤波值设置为零；

(8)开始卡尔曼滤波，每次取两个方向上的加速度计的输出和陀螺仪的三个方向上的输出作为观测值；

(9)根据求得的滤波结果和方差更新被估计值和观测噪声方差的数据，并再次取观测值进行卡尔曼滤波；

(10)返回步骤(8)，直至两次滤波结果小于用户设定的阈值；

(11)使用求得的X轴向、Y轴向和Z轴向上的姿态角的误差对姿态转移矩阵C_bp进行纠正。

2.根据权利要求1所述的基于AHRS的精对准算法，其特征在于，所述步骤(2)具体包括以下步骤：

(2a)根据 ${\stackrel{\→}{g}}^t=\left[\begin{array}{c}0\\ 0\\ -g\end{array}\right]$ 得出重力加速度在地理坐标系下的投影其中g为重力加速度；根据 ${\stackrel{\→}{w}}_{ie}^t=\left[\begin{array}{c}0\\ w_{ie}\cos L\\ w_{ie}\cos L\end{array}\right]$ 得出地球绕惯性坐标系自转角速度在地理坐标系下的投影其中w_iε为地球绕惯性轴的自转角速度，L为纬度；

(2b)根据公式构造坐标系下的三个向量和

(2c)根据方程式 $\left\{\begin{array}{c}\stackrel{\→}{{r_1}^b}=C_t^b\stackrel{\→}{{r_1}^t}\\ \stackrel{\→}{{r_2}^b}=C_t^b\stackrel{\→}{{r_2}^t}\\ \stackrel{\→}{{r_3}^b}=C_t^b\stackrel{\→}{{r_3}^t}\end{array}\right.$ 解得矩阵其中，t为时间间隔，将以平台坐标系表示；

(2d)根据公式计算出姿态转移矩阵C_bp。