一种基于扩展卡尔曼滤波的云台姿态估计方法

本发明涉及卡尔曼滤波技术，具体涉及一种基于四元数扩展卡尔曼滤波的云台姿态估计算法。

背景技术：

近年来，随着手持云台等智能设备的广泛应用，人们对其的稳定性和精确性都提出了更高的要求，要使这些设备能够更加稳定工作，获取更为精准的姿态估计信息尤为重要。在大多数姿态估计算法中，都是利用陀螺仪更新姿态信息，利用加速度计和磁力计对姿态信息进行修正。目前应用较广的姿态估计算法有卡尔曼滤波算法和互补滤波算法，这两种算法均能减少高斯噪声等对姿态估计的影响，也能减少由于陀螺仪漂移所造成的累计误差。

但是，由于加速度计和磁力计都是极易受到外界干扰的传感器，采用互补滤波算法精度往往有所欠缺，采用传统卡尔曼滤波算法的效果也差强人意。因此，衍生出了扩展卡尔曼滤波算法，扩展卡尔曼滤波算法相较于传统卡尔曼滤波算法更适用于姿态估计。

技术实现要素：

为了实时获取更为精准的姿态估计信息，本发明提出了一种基于扩展卡尔曼滤波的云台姿态估计方法，用于估计出非线性状态模型下的云台姿态信息。

为达到上述目的，本发明提供的基于扩展卡尔曼滤波的云台姿态估计方法,具体包括如下步骤：

1)采用航向姿态参考系统(ahrs)来给云台控制器提供较为精确的姿态信息，姿态信息用欧拉角进行表示，欧拉角包括俯仰角、横滚角和偏航角，采用四元数q表示欧拉角；

2)初始化系统状态，其中系统状态量如下式所示：

式(1)中，表示系统状态量，q表示四元数；q＝[q0q1q2q3]^t；δθb表示角度增量的偏移误差；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

3)读取陀螺仪数据，对旋转四元数进行预测；

式(2)中，δθm表示陀螺仪实际测得的角度增量；δθb表示角度增量的偏移误差；δθn表示角增量噪声；qk表示k时刻的四元数；qk+1表示k+1时刻的四元数；k表示时刻；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

4)更新先验协方差矩阵pk+1/k；

5)使用加速度计对系统状态量进行修正；更新加速度计修改后的后验协方差矩阵pk+1/k+1_accel；

6)使用磁力计对系统状态量进行修正；更新磁力计修正后的后验协方差矩阵pk+1/k+1_mag。

所述导航坐标系定为东-北-天。

其中，所述步骤4)具体为：

4-1)计算k时刻至k+1时刻的一步转移阵f；

式(3)中由式(2)qk+1对qk求导得到，由式(2)qk+1对求导得到；

k表示时刻；

4-2)计算系统噪声驱动阵γ；

式(4)中由式(2)qk+1对求导得到；k表示时刻；

4-3)计算系统噪声方差阵q；

式(5)中δθn表示角增量噪声；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

4-4)计算过程噪声矩阵；

式(6)中为陀螺仪偏差的过程噪声协方差；

4-5)得到先验协方差矩阵pk+1/k；

pk+1/k＝f·pk/k·f^t+γ·q·γ^t+nprocess(7)

式(7)中，f^t表示f矩阵的转置矩阵；γ^t表示γ矩阵的转置矩阵；k表示时刻；

进一步，所述步骤5)具体包括：

5-1)计算加速度计预测值；

式(8)中，为方向余弦矩阵，表示从导航坐标系旋转到机体坐标系，如式(9)所示；

n表示导航坐标系；b表示机体坐标系；g表示重力加速度；

5-2)计算量测矩阵；

式(10)中，haccel表示加速度计修正过程中的量测矩阵，是由加速度计预测值apred对四元数q求导得到；

5-3)计算加速度计可信度accconfidence；

5-4)计算卡尔曼滤波增益kaccel；

式(11)中raccel为加速度计的协方差矩阵；表示haccel矩阵的转置矩阵；()^-1表示逆矩阵；

5-5)计算加速度计修正量qε1；

qε1＝kaccel·(zaccel-apred)(12)

qε1＝qε1,0+qε1,1+qε1,2+0·qε1,3(13)

式(12)中，qε1为加速度计修正量；zaccel为加速度计实际测量值；apred为加速度计的预测值；由于加速度计只能修正横滚角和俯仰角，不能修正偏航角，为了确保偏航角不受加速度计修正所影响，故将式(12)中的四元数第三矢量置为零，如式(13)所示；

5-6)计算加速度计修正后的后验系统状态量xk+1/k+1_accel；

xk+1/k+1_accel＝xk+1/k+qε1(14)

式(14)中，xk+1/k为陀螺仪更新后的先验系统状态量；k表示时刻；

5-7)更新加速度计修正后的后验协方差矩阵pk+1/k+1_accel，

pk+1/k+1_accel＝[i-kaccel·haccel]·pk+1/k(15)

式(15)中，i为7*7大小的单位矩阵；kaccel为加速度计修正的卡尔曼增益；pk+1/k为先验协方差矩阵；k表示时刻；

进一步，所述步骤6)具体包括：

6-1)根据磁力计测量值，计算导航坐标系磁场值mn；

式(16)中，为采用四元数形式描述的方向余弦矩阵，表示从机体坐标系旋转到导航坐标系，如式(17)所示，表示从导航坐标系旋转到机体坐标系的方向余弦矩阵，如式(9)所示，t表示矩阵的转置；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

6-2)计算忽略地球东西方向磁场后根据坐标系得到的mn的修正值

式(18)中，t表示矩阵的转置；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

6-3)计算机体坐标系下的磁场预测值mpred；

6-4)计算量测矩阵hmag；

式(20)中，hmag表示磁力计修正过程中的量测矩阵，是由磁场预测值mpred对四元数q求导得到；

6-5)计算卡尔曼增益kmag；

式(21)中，rmag为磁力计的协方差矩阵；()^-1表示该矩阵的逆矩阵；t表示矩阵的转置；

k表示时刻；

6-6)计算磁力计修正量qε2；

qε2＝kmag·(zmag-mpred)(22)

qε2＝qε2,0+0·qε2,1+0·qε2,2+qε2,3(23)

式(22)中，qε2为磁力计修正量；zmag为磁力计实际测量值；mpred为磁力计的预测值；由于磁力计只能修正偏航角，不能修正横滚角和俯仰角，为了确保横滚角和俯仰角不受磁力计修正所影响，故将式(22)中的第一矢量和第二矢量置为零，如式(23)所示；

6-7)计算磁力计修正后的后验系统状态量xk+1/k+1_mag；

xk+1/k+1_mag＝xk+1/k+1_accel+qε2(24)

式(24)中，xk+1/k+1_accel为加速度计修正后的后验系统状态量；k表示时刻；

6-8)更新磁力计修正后的后验协方差矩阵pk+1/k+1_mag；

pk+1/k+1_mag＝[i-kmag·hmag]·pk+1/k+1_accel(25)

式(25)中，i为7*7大小的单位矩阵；kmag为磁力计修正的卡尔曼增益；pk+1/k+1_accel为加速度计修正后的后验协方差矩阵；k表示时刻；

本发明具有如下有益效果：

首先本发明采用四元数来表示物体当前的姿态，先计算四元数，再转化为欧拉角，计算量较小；其次，算法的系统状态量包含四元数与角度增量的偏移误差，使用加速度计和磁力计修正角度增量的偏移误差，使得姿态估计更加精确；第三，将加速计修正与磁力计修正分为两阶段实行，使得加速度计修正与磁力计修正互不干扰，提高姿态估计精确度；第四，为了避免加速度计修正与磁力计修正互相干扰，在加速度计修正中，为了避免偏航角受加速度计修正的影响，把修正量中的四元数第三矢量置为零，在磁力计修正中，为了避免横滚角和俯仰角受磁力计修正的影响，把修正量中的四元数第一矢量和第二矢量置为零，以此提高加速度计和磁力计修正的精确度；第五，为了避免云台运动时加速度计数据异常而影响修正，引入加速度计可信度，当数据异常时可信度降低，减少对修正过程的影响。

附图说明

图1为本发明基于扩展卡尔曼滤波的云台姿态估计方法流程图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

如图1所示，本发明基于扩展卡尔曼滤波的云台姿态估计方法，包括：

一、采用航向姿态参考系统(ahrs)来给云台控制器提供较为精确的姿态信息，姿态信息用欧拉角进行表示，欧拉角包括俯仰角、横滚角和偏航角。俯仰角即云台绕y轴旋转角度φ；横滚角即云台绕x轴旋转角度θ；偏航角即云台绕z轴旋转角度ψ。ahrs提供了导航坐标系和机体坐标系，方向余弦矩阵(dcm)表示了两个坐标系的关系，且包含了云台的相关姿态信息，由于欧拉角表示的旋转矩阵具有缺陷，会产生死锁现象，而用四元数q表示的旋转矩阵则能弥补上述缺陷，且计算量较小。四元数q由实数加上三个虚数单位组成，用q＝[q0q1q2q3]^t进行表示。本实施例导航坐标系定为东-北-天(east-north-up)。

二、采用的扩展卡尔曼系统状态量如下所示：

式(1)中，表示系统状态量，q表示四元数；q＝[q0q1q2q3]^t；δθb表示角度增量的偏移误差；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

三、采用陀螺仪进行状态更新，即计算出先验系统状态量xk+1/k，预测方程如下式所示，由于角度增量的偏移误差不随着陀螺仪的变化而变化，需对旋转四元数进行预测；

式(2)中，δθm表示陀螺仪实际测得的角度增量；δθb表示角度增量的偏移误差；δθn表示角增量噪声；qk表示k时刻的四元数；qk+1表示k+1时刻的四元数；k表示时刻；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

四、更新先验协方差矩阵pk+1/k，具体计算步骤如下所示：

1.计算k时刻至k+1时刻的一步转移阵f；

式(3)中由式(2)qk+1对qk求导得到，由式(2)qk+1对求导得到；

k表示时刻；

2.计算系统噪声驱动阵γ；

式(4)中由式(2)qk+1对求导得到；k表示时刻；

3.计算系统噪声方差阵q；

式(5)中δθn表示角增量噪声；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

4.计算过程噪声矩阵；

式(6)中为陀螺仪偏差的过程噪声协方差；

5.得到先验协方差矩阵pk+1/k；

pk+1/k＝f·pk/k·f^t+γ·q·γ^t+nprocess(7)

式(7)中，f^t表示f矩阵的转置矩阵；γ^t表示γ矩阵的转置矩阵；k表示时刻；

五、使用加速度计对系统状态量进行修正，即得到加速度计修正后的后验系统状态量。具体修正步骤如下所示：

1.计算加速度计预测值；

式(8)中，为方向余弦矩阵，表示从导航坐标系旋转到机体坐标系，如式(9)所示；

n表示导航坐标系；b表示机体坐标系；g表示重力加速度；

2.计算量测矩阵；

式(10)中，haccel表示加速度计修正过程中的量测矩阵，是由加速度计预测值apred对四元数q求导得到；

3.计算加速度计可信度accconfidence；

由于云台处于运动状态，加速度计获取的数据可能会异常，故引入accconfigence变量。若加速度计数据正确，那么加速度值归一化后值为1，加速度计可信度accconfigence将为1；若加速度计值异常，加速度计可信度将小于1，降低由于数据异常导致的修正错误，具体由以下步骤获得：

1)设定accnormp初始值为1：

accnormp＝1(11)

2)计算加速度归一化值accnorm：

式(12)中g为重力加速度；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

3)更新accnorm：

accnorm＝α·accnormp+β·accnorm(13)

式(13)中α和β为两个参数，可依据实际效果进行调整，在本实例中α＝0.9，β＝0.1；

4)将accnorm的值赋给accnormp，更新accnormp，用于下一组数据的可信度计算：

5)计算可信度：

式(14)中，||表示取绝对值；

6)对可信度accconfidence进行限位：

4.计算卡尔曼滤波增益kaccel；

式(16)中raccel为加速度计的协方差矩阵；表示haccel矩阵的转置矩阵；()^-1表示逆矩阵；k表示时刻；

5.计算加速度计修正量qε1；

qε1＝kaccel·(zaccel-apred)(17)

qε1＝qε1,0+qε1,1+qε1,2+0·qε1,3(18)

式(17)中，qε1为加速度计修正量；zaccel为加速度计实际测量值；apred为加速度计的预测值；由于加速度计只能修正横滚角和俯仰角，不能修正偏航角，为了确保偏航角不受加速度计修正所影响，故将式(17)中的四元数第三矢量置为零，如式(18)所示；

6.计算加速度计修正后的后验系统状态量xk+1/k+1_accel；

xk+1/k+1_accel＝xk+1/k+qε1(19)

式(19)中，xk+1/k为陀螺仪更新后的先验系统状态量；k表示时刻；

7.更新加速度计修正后的后验协方差矩阵pk+1/k+1_accel；

pk+1/k+1_accel＝[i-kaccel·haccel]·pk+1/k(20)

式(20)中，i为7*7大小的单位矩阵；kaccel为加速度计修正的卡尔曼增益；pk+1/k为先验协方差矩阵；k表示时刻；

六、使用磁力计对系统状态量进行修正，即得到磁力计修正后的后验系统状态量。具体修正步骤如下所示：

1.根据磁力计测量值，计算导航坐标系磁场值mn；

式(21)中，为采用四元数形式描述的方向余弦矩阵，表示从机体坐标系坐标系旋转到导航坐标系，如式(22)所示，表示从导航坐标系旋转到机体坐标系的方向余弦矩阵，如式(9)所示，t表示矩阵的转置；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

2.计算忽略地球东西方向磁场后根据坐标系得到的mn的修正值

式(23)中，t表示矩阵的转置；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

3.计算机体坐标系下的磁场预测值mpred；

4.计算量测矩阵hmag；

式(25)中，hmag表示磁力计修正过程中的量测矩阵，是由磁场预测值mpred对四元数q求导得到；

5.计算卡尔曼增益kmag；

式(26)中，rmag为磁力计的协方差矩阵；()^-1表示该矩阵的逆矩阵；t表示矩阵的转置；

k表示时刻；

6.计算磁力计修正量qε2；

qε2＝kmag·(zmag-mpred)(27)

qε2＝qε2,0+0·qε2,1+0·qε2,2+qε2,3(28)

式(27)中，qε2为磁力计修正量；zmag为磁力计实际测量值；mpred为磁力计的预测值；由于磁力计只能修正偏航角，不能修正横滚角和俯仰角，为了确保横滚角和俯仰角不受磁力计修正所影响，故将式(27)中的第一矢量和第二矢量置为零，如式(28)所示；

7.计算磁力计修正后的后验系统状态量xk+1/k+1_mag；

xk+1/k+1_mag＝xk+1/k+1_accel+qε2(29)

式(29)中，xk+1/k+1_accel为加速度计修正后的后验系统状态量；k表示时刻；

8.更新磁力计修正后的后验协方差矩阵pk+1/k+1_mag；

pk+1/k+1_mag＝[i-kmag·hmag]·pk+1/k+1_accel(30)

式(30)中，i为7*7大小的单位矩阵；kmag为磁力计修正的卡尔曼增益；pk+1/k+1_accel为加速度计修正后的后验协方差矩阵；k表示时刻。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。