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

Abstract

本发明提供一种基于扩展卡尔曼滤波的云台姿态估计方法，该方法采用四元数来表示物体当前的姿态，系统状态量包含四元数与角度增量的偏移误差，使用加速度计和磁力计修正角度增量的偏移误差，使得姿态估计更加精确，且将加速计修正与磁力计修正分为两阶段实行，使得加速度计修正与磁力计修正互不干扰，提高姿态估计精确度；在加速度计修正中，把修正量中的四元数第三矢量置为零，在磁力计修正中，把修正量中的四元数第一矢量和第二矢量置为零。采用本发明能够获得更为精准的姿态估计信息。

Description

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

技术领域

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

背景技术

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

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

发明内容

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

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

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

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

式(1)中，

表示系统状态量，q表示四元数；q＝[q₀ q₁ q₂ q₃]^T；Δθ_b表示角度增量的偏移误差；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

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

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

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

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

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

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

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

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

式(3)中

由式(2)q_k+1对q_k求导得到，

由式(2)q_k+1对

求导得到；

k表示时刻；

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

式(4)中

由式(2)q_k+1对

求导得到；k表示时刻；

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

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

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

式(6)中

为陀螺仪偏差的过程噪声协方差；

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

P_k+1/k＝F·P_k/k·F^T+Γ·Q·Γ^T+N_process (7)

式(7)中，F^T表示F矩阵的转置矩阵；Γ^T表示Γ矩阵的转置矩阵；k表示时刻；

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

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

式(8)中，

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

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

5-2)计算量测矩阵；

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

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

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

式(11)中R_accel为加速度计的协方差矩阵；

表示H_accel矩阵的转置矩阵；()^-1表示逆矩阵；

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

q_ε1＝K_accel·(z_accel-a_pred) (12)

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

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

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

x_{k+1/k+1_accel}＝x_k+1/k+q_ε1 (14)

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

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

P_{k+1/k+1_accel}＝[I-K_accel·H_accel]·P_k+1/k (15)

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

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

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

式(16)中，

为采用四元数形式描述的方向余弦矩阵，表示从机体坐标系旋转到导航坐标系，如式(17)所示，

表示从导航坐标系旋转到机体坐标系的方向余弦矩阵，如式(9)所示，T表示矩阵的转置；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

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

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

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

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

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

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

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

k表示时刻；

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

q_ε2＝K_mag·(z_mag-m_pred) (22)

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

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

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

x_{k+1/k+1_mag}＝x_{k+1/k+1_accel}+q_ε2 (24)

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

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

P_{k+1/k+1_mag}＝[I-K_mag·H_mag]·P_{k+1/k+1_accel} (25)

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

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

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

附图说明

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

具体实施方式

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

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

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

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

式(1)中，

表示系统状态量，q表示四元数；q＝[q₀ q₁ q₂ q₃]^T；Δθ_b表示角度增量的偏移误差；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

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

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

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

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

式(3)中

由式(2)q_k+1对q_k求导得到，

由式(2)q_k+1对

求导得到；

k表示时刻；

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

式(4)中

由式(2)q_k+1对

求导得到；k表示时刻；

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

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

4.计算过程噪声矩阵；

式(6)中

为陀螺仪偏差的过程噪声协方差；

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

P_k+1/k＝F·P_k/k·F^T+Γ·Q·Γ^T+N_process (7)

式(7)中，F^T表示F矩阵的转置矩阵；Γ^T表示Γ矩阵的转置矩阵；k表示时刻；

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

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

式(8)中，

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

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

2.计算量测矩阵；

式(10)中，H_accel表示加速度计修正过程中的量测矩阵，是由加速度计预测值a_pred对四元数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.计算卡尔曼滤波增益K_accel；

式(16)中R_accel为加速度计的协方差矩阵；

表示H_accel矩阵的转置矩阵；()^-1表示逆矩阵；k表示时刻；

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

q_ε1＝K_accel·(z_accel-a_pred) (17)

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

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

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

x_{k+1/k+1_accel}＝x_k+1/k+q_ε1 (19)

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

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

P_{k+1/k+1_accel}＝[I-K_accel·H_accel]·P_k+1/k (20)

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

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

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

式(21)中，

为采用四元数形式描述的方向余弦矩阵，表示从机体坐标系坐标系旋转到导航坐标系，如式(22)所示，

表示从导航坐标系旋转到机体坐标系的方向余弦矩阵，如式(9)所示，T表示矩阵的转置；x,y,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

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

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

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

4.计算量测矩阵H_mag；

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

5.计算卡尔曼增益K_mag；

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

k表示时刻；

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

q_ε2＝K_mag·(z_mag-m_pred) (27)

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

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

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

x_{k+1/k+1_mag}＝x_{k+1/k+1_accel}+q_ε2 (29)

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

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

P_{k+1/k+1_mag}＝[I-K_mag·H_mag]·P_{k+1/k+1_accel} (30)

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

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

Claims (5)

1.一种基于扩展卡尔曼滤波的云台姿态估计方法,其特征在于，包括如下步骤：

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

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

式(1)中，

表示系统状态量，q表示四元数；q＝[q₀ q₁ q₂ q₃]^T；Δθ_b表示角度增量的偏移误差；x、y、,z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

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

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

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

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

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

2.如权利要求1所述基于扩展卡尔曼滤波的云台姿态估计方法,其特征在于，导航坐标系定为东-北-天。.

3.如权利要求1所述基于扩展卡尔曼滤波的云台姿态估计方法,其特征在于，步骤4)具体为：

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

式(3)中

由式(2)q_k+1对q_k求导得到，

由式(2)q_k+1对

求导得到；k表示时刻；

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

式(4)中

由式(2)q_k+1对

求导得到；k表示时刻；

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

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

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

式(6)中

为陀螺仪偏差的过程噪声协方差；

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

P_k+1/k＝F·P_k/k·F^T+Γ·Q·Γ^T+N_process (7)

式(7)中，F^T表示F矩阵的转置矩阵；Γ^T表示Γ矩阵的转置矩阵；k表示时刻。

4.如权利要求1所述基于扩展卡尔曼滤波的云台姿态估计方法,其特征在于，步骤5)具体包括：

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

式(8)中，

为方向余弦矩阵，表示从导航坐标系旋转到机体坐标系，如式(9)所示；n表示导航坐标系；b表示机体坐标系；g表示重力加速度；

5-2)计算量测矩阵；

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

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

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

式(11)中R_accel为加速度计的协方差矩阵；

表示H_accel矩阵的转置矩阵；()^-1表示逆矩阵；k表示时刻；

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

q_ε1＝K_accel·(z_accel-a_pred) (12)

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

式(12)中，q_ε1为加速度计修正量；z_accel为加速度计实际测量值；a_pred为加速度计的预测值；

式(12)中的四元数第三矢量置为零，为式(13)所示；

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

x_{k+1/k+1_accel}＝x_k+1/k+q_ε1 (14)

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

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

P_{k+1/k+1_accel}＝[I-K_accel·H_accel]·P_k+1/k (15)

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

5.如权利要求1所述基于扩展卡尔曼滤波的云台姿态估计方法,其特征在于，步骤6)具体包括：

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

式(16)中，

为采用四元数形式描述的方向余弦矩阵，表示从机体坐标系旋转到导航坐标系，为式(17)所示，

表示从导航坐标系旋转到机体坐标系的方向余弦矩阵，如式(9)所示，T表示矩阵的转置；x、y、z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

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

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

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

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

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

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

式(21)中，R_mag为磁力计的协方差矩阵；()^-1表示该矩阵的逆矩阵；T表示矩阵的转置；x、y、z分别表示空间直角坐标系中的横轴、纵轴和竖轴；

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

q_ε2＝K_mag·(z_mag-m_pred) (22)

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

式(22)中，q_ε2为磁力计修正量；z_mag为磁力计实际测量值；m_pred为磁力计的预测值；式(22)中的第一矢量和第二矢量置为零，为式(23)所示；

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

x_{k+1/k+1_mag}＝x_{k+1/k+1_accel}+q_ε2 (24)

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

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

P_{k+1/k+1_mag}＝[I-K_mag·H_mag]·P_{k+1/k+1_accel} (25)

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

Images