CN107918139B - 一种角速度辅助的Kalman滤波定位方法 - Google Patents

Abstract

一种角速度辅助的Kalman滤波定位方法，包括如下步骤：步骤1、捕获卫星信号，解调导航电文，剔除故障卫星；步骤2、解算运动载体的初始状态变量、方位角、角速度、方差；步骤3、确定采用的Kalman滤波方程模型，根据不同载体类型和不同的接收机类型确定各个参数的值：步骤4、检测接收卫星信息是否良好，若是，直接利用Kalman方程解算出当前时刻的状态变量‑位置、速度和加速度；当接收卫星信息不好，不能利用卫星信息进行卫星定位时，确定现时刻的载体的所在方位，联合Kalman滤波的预测方程，两者进行等权计算现时刻状态解算载体的位置、速度等信息。本发明在卫星定位失败时，能够显著提高没有惯性传感器等辅助手段情况下的动态定位精度。

Description

一种角速度辅助的Kalman滤波定位方法

技术领域

本发明涉及卫星导航定位的技术领域，特别涉及一种角速度辅助的Kalman滤波定位方法。

背景技术

随着中国北斗卫星系统的发展，目前国内已经打破了GPS独大的局面，相应的卫星导航定位相关产品也多以双模或多模的形式出现。其中应用较为广泛便是北斗+GPS双模动态定位。

卫星动态定位的精度主要依靠于接收的卫星信息、定位的算法和辅助信息，由于动态运动的环境复杂性，我们很难改变接收的卫星信息。传统的动态定位一般采用组合导航的模式，即卫星导航定位+惯性传感器，惯性传感器一般可以测得载体的角速度且在卫星导航定位失败的情况下能提供短时间的定位。而对于一般的运动载体而言，也不能苛求它拥有完善的惯性传感器等可以辅助定位的装置，这时候改进定位算法提高定位精度就成了有效的手段。

在没有其他融合的惯性传感器等的辅助下，卫星动态定位会因地理位置不利导致卫星接收不到进而导致定位失败。消除车辆卫星定位随机误差的方法有多种，其中比较常用的方法之一是GPS动态滤波，即利用滤波器消除各种随机误差，从而提高定位精度。一般情况下，多用Kalman滤波算法来提高动态定位的精度，Kalman预测方程实际上是根据上一时刻的位置和速度预测出的一条运动轨迹，又由于动态运动载体的运动复杂性，因此单靠Kalman状态方程预测定位结果精度很难得到保障。但是当卫星信号被严重遮挡或者没有卫星信号时，普通的基于位置、速度、加速度的自适应Kalman滤波的定位精度不高，有待改善。

发明内容

有鉴于此，本发明的目的在于，提供一种角速度辅助的Kalman滤波定位方法，在卫星定位失败的情况下，添加基于角速度的辅助方程，再依据前一时刻的状态值和预测方程一起预测现时刻的状态量(位置、速度)，结果表明这种方法行之有效，可以在一定程度上提高卫星定位失败情况下的实际导航定位精度。

本发明的目的是以下述方式实现的：

一种角速度辅助的Kalman滤波定位方法，包括如下步骤：

步骤1、运动载体的接收机捕获并跟踪卫星信号，解调卫星导航电文，剔除故障卫星和异常卫星；

步骤2、利用接收到的前几个历元的卫星信息解算运动载体的初始状态变量X₀、方位角S₀、角速度Vr₀、方差P₀；

步骤3、确定采用的Kalman滤波方程模型，假设没有系统控制输入，根据不同载体类型和不同的接收机类型确定以下参数：过程激励噪声协方差矩阵为Q，观测噪声协方差矩阵为R，转换矩阵φ，测量矩阵H，马尔可夫过程在三个坐标轴方向的相关时间常数τ_x、τ_y、τ_z；

取状态变量为X＝[x v_x a_x ε_x y v_y a_y ε_y z v_z a_z ε_z]^T，观测变量Z＝[L_x L_vx L_yL_vy L_z L_vz]^T；

其中，

分别为X、Y、Z轴上的加速度方差矩阵，

分别为X、Y、Z轴上的定位误差方差矩阵；其中(x v_x α_x)、(y v_y α_y)、(z v_z α_z)3组状态变量分别为xyz三个坐标轴上的位置、速度、加速度分量，ε_x、ε_y、ε_z分别为各种误差源在三个坐标轴方向造成的总位置误差，观测变量(L_x L_vx)、(L_y L_vy)、(L_z L_vz)分别为接收机解算出的xyz三个坐标轴上的位置、速度分量；R＝diag[R_x R_y R_z]，其中R_x、R_y、R_z分别为X、Y、Z轴上的观测噪声协方差分量；

步骤4、根据现时刻接收卫星信息初步解算位置、速度、载体运行方位角，然后根据前一时刻的位置、速度、加速度，利用卡尔曼滤波方程解算现时刻的位置、速度和加速度；

具体包括如下步骤：

步骤401、检测接收卫星信息是否良好以解算当前载体运行方位角，若是，利用如下卡尔曼滤波方程解算当前时刻的状态变量

返回

退出；若否，转到步骤402；

K_k＝P_(k，k-1)H^T(HP_(k，k-1)H^T+R)^-1

P_(k，k-1)＝φP_k-1φ^T+Q

P_k＝(I-K_kH)P_(k，k-1)

式中，

是利用K-1时刻状态变量预测的K时刻状态变量预测结果，

是K-1时刻的状态变量最优结果，

是K时刻的状态变量最优估算值，K_k为卡尔曼增益；P_k为后验估计误差的协方差矩阵，P_(k，k-1)为先验估计误差的协方差矩阵；

步骤402、将k-1时刻状态变量

由XYZ坐标系转换为NEU坐标系，设NEU坐标系下的速度变量为Vn0、Ve0、Vh0：

步骤403、计算添加角速度辅助后的速度变量：

Vn＝(Vn0*Vn0+Ve0*Ve0)^1/2*cos(S_k-1)

Ve＝(Vn0*Vn0+Ve0*Ve0)^1/2*sin(S_k-1)

Vh＝Vh0

步骤404、；令Vr_k-1＝Vr_k-2，Vr_k-1、Vr_k-2分别是K-1、K-2时刻的角速度；根据辅助方程角速度：Vr_k-1＝(S_k-S_k-1)/T计算S_k＝S_k-1+Vr_k-1*T，S_k是K时刻的载体运行方位角；

步骤405、将NEU坐标系下的速度变量Vn、Ve、Vh转换为XYZ坐标系，设此时XYZ坐标系下的速度变量相应为v_x1、v_y1、v_z1，令

步骤406、联合Kalman滤波方程，两者进行等权计算现时刻状态变量预测结果：

返回

退出。

优选地，所述步骤1具体包括如下步骤：

步骤101)接收机天线捕获并跟踪来自卫星的信号，解调出卫星的导航电文，得到卫星位置、钟差以及其他修正参数；

步骤102)根据导航电文的内容和格式来判断校验是否存在误码，剔除掉含有误码的导航电文对应的故障卫星；

步骤103)对跟踪到的可用的卫星的位置和运动轨迹的连续性进行判断卫星导航电文的正确性，剔除掉存在跳变异常的卫星以及载噪比和仰角不满足门限值要求的卫星。

优选地，所述步骤3中，在车载GPS接收机情况下，

R_x＝R_y＝R_z＝diag(12²，0.2²)，τ_x＝τ_y＝τ_z＝0.5。

优选地，所述步骤3中，

T为观测时间间隔，

其中Φ_x、Φ_y、Φ_z分别为XYZ三个坐标轴上的转换矩阵。

本申请在卫星定位失败的情况下，添加基于角速度的辅助方程，再依据前一时刻的状态值和预测方程一起预测现时刻的状态量(位置、速度)，利用辅助方程代替惯性传感器的功能，在没有融合辅助工具时，在卫星定位失败的情况下，添加的基于角速度的辅助方程的Kalman滤波动态定位一定程度上提高了定位精度，改善了滤波器的动态性能，使其具有一定的跟踪能力,显著提高了在动态定位中没有惯性传感器等辅助手段情况下的动态定位精度。

附图说明

图1为添加角速度辅助方程的Kalman滤波算法的原理示意图。

具体实施方式

如图1所示，本发明提供的角速度辅助的Kalman滤波定位方法，具体包括如下步骤：

(1)首先接收机天线捕获并跟踪来自卫星的信号，解调出卫星的导航电文，得到卫星位置、钟差以及其他修正参数。其次根据导航电文的内容和格式来判断校验是否存在误码，剔除掉含有误码的导航电文对应的故障卫星。最后对跟踪到的可用的卫星的位置和运动轨迹的连续性进行判断卫星导航电文的正确性，剔除掉存在跳变等异常的卫星以及载噪比和仰角不满足门限值要求的卫星。

(2)利用接收到的前几个历元的卫星信息解算运动载体的初始状态变量、方位角S₀、角速度Vr₀、方差P₀。

(3)确定采用的Kalman滤波方程模型，假设没有系统控制输入，根据不同载体类型和不同的接收机类型确定各已知为常数的参数：过程激励噪声协方差矩阵为Q，观测噪声协方差矩阵为R，转换矩阵Φ，测量矩阵H，马尔可夫过程在三个坐标轴方向的相关时间常数τ_x、τ_y、τ_z。取状态变量为X＝[x v_x a_x ε_x y v_y a_y ε_y z v_z a_z ε_z]^T，观测变量Z＝[L_x L_vx L_yL_vy L_z L_vz]^T，其中(x v_x a_x)、(y v_y a_y)、(z v_z a_z)3组状态变量分别为xyz三个坐标轴上的位置、速度、加速度分量，ε_x、ε_y、ε_z分别为各种误差源在三个坐标轴方向造成的总位置误差，观测变量(L_x L_vx)、(L_y L_vy)、(L_z L_vz)分别为接收机解算出的xyz三个坐标轴上的位置、速度分量。

Q阵为过程激励噪声协方差矩阵：

其中，

分别为XYZ轴上的加速度方差矩阵，

分别为XYZ轴上的定位误差方差。

R阵为观测噪声协方差矩阵：R＝diag[R_x R_y R_z]，R_x、R_y、R_z分别为X、Y、Z轴上的观测噪声协方差分量。

实际中，过程激励噪声协方差矩阵Q和观测噪声协方差矩阵R根据不同的载体不同的接收机类型而变化，但同一接收机同一载体运行过程中视为常量。因此不同的载体运行状态和不同的接收机针对不同的各参数取值，一般都是通过大量实验得来的经验值，比如，本专利采用的车载GPS接收机仿真实验中，取

R_x＝R_y＝R_z＝diag(12²，0.2²)，τ_x＝τ_y＝τ_z＝0.5，

(T为观测时间间隔)

其中Φ_x、Φ_y、Φ_z分别为XYZ三个坐标轴上的转换矩阵，

(4)根据现时刻接收卫星信息初步解算位置、速度、载体运行方位角，然后根据前一时刻的位置、速度、加速度，利用卡尔曼滤波方程解算现时刻的位置、速度和加速度(扩展卡尔曼滤波算法可参见相关文档，这里不再累述)。具体实现步骤如下：

a)设状态方程：X_k＝ΦX_k-1+W_k-1

观测方程：Z_k＝HX_k+V_k

式中随机信号W_k和V_k分别表示过程激励噪声和观测噪声，假设它们为相互独立、正态分布的白色噪声：W_k～N(0，Q)，V_k～N(0，R)。

卡尔曼滤波模型：

K_k＝P_(k，k-1)H^T(HP_(k，k-1)HT+R)^-1

P_(k，k-1)＝ΦP_k-1Φ^T+Q

P_k＝(I-K_kH)P_(k，k-1)

式中，

是利用K-1时刻状态变量预测的K时刻状态变量预测结果，

是K-1时刻的状态变量最优结果，

是K时刻的状态变量最优估算值，K_k为卡尔曼增益；P_k为后验估计误差的协方差矩阵，P_(k，k-1)为先验估计误差的协方差矩阵，Φ为转换矩阵，Q为过程激励噪声协方差矩阵，R为系统的观测噪声方差阵。

辅助方程角速度：Vr_k-1＝(S_k-S_k-1)/T，(3)

当接收卫星信息不好，不能解算载体运行方位角时，Vr_k-1近似等于Vr_k-2，因此估算Vr_k-1＝Vr_k-2，Vr_k-1、Vr_k-2分别是K-1、K-2时刻的角速度；计算K时刻的载体运行方位角S_k＝S_k-1+Vr_k-1*T，S_k用于K+1时刻接收卫星的速度变量Vn、Ve、Vh计算使用。

将k-1时刻状态变量

由XYZ坐标系转换为NEU坐标系，设NEU坐标系下的速度变量为Vn0、Ve0、Vh0：

假设运动存在均角速度运动，则添加角速度辅助后的速度变量变为：

Vn＝(Vn0*Vn0+Ve0*Ve0)^1/2*cos(S_k-1)

Ve(Vn0*Vn0+Ve0*Ve0)^1/2*sin(S_k-1)

Vh＝Vh0

再将NEU坐标系下的速度变量Vn、Ve、Vh转换为XYZ坐标系，设此时XYZ坐标系下的速度变量相应为v_x1、v_y1、v_z1，令

b)当接收卫星信息良好，直接利用方程(1)和(2)解算出当前时刻的状态变量-位置、速度和加速度；当接收卫星信息不好，不能利用卫星信息进行卫星定位时，利用辅助方程(4)和(1)联合解算现时刻的状态变量-位置、速度和加速度。

具体联合思想为：利用辅助方程确定现时刻的载体的所在方位，联合Kalman滤波 的预测方程，两者进行等权计算现时刻状态解算载体的位置、速度等信息预测结果：

经过仿真测试与实际车载动态试验，定位结果有改善。其中，添加辅助方程的运动轨迹和实际道路更匹配；通过车载动态试验表明，添加了角速度辅助方程后，在载体接收不到卫星信号解算不出当前位置速度观测值时，联合自适应扩展Kalman滤波方程解算的位置比没有添加加速度辅助方程的解算结果精度上有明显的提升。

以上所述的仅是本发明的优选实施方式，应当指出，对于本领域的技术人员来说，在不脱离本发明整体构思前提下，还可以作出若干改变和改进，这些也应该视为本发明的保护范围。

Claims (4)

1.一种角速度辅助的Kalman滤波定位方法，其特征在于：包括如下步骤：

步骤1、运动载体的接收机捕获并跟踪卫星信号，解调卫星导航电文，剔除故障卫星和异常卫星；

步骤2、利用接收到的前几个历元的卫星信息解算运动载体的初始状态变量X₀、方位角S₀、角速度Vr₀、方差P₀；

步骤3、确定采用的Kalman滤波方程模型，假设没有系统控制输入，根据不同载体类型和不同的接收机类型确定以下参数：过程激励噪声协方差矩阵为Q，观测噪声协方差矩阵为R，转换矩阵Φ，测量矩阵H，马尔可夫过程在三个坐标轴方向的相关时间常数τ_x、τ_y、τ_z；

取状态变量为X＝[x v_x a_x ε_x y v_y a_y ε_y z v_z a_z ε_z]^T，观测变量Z＝[L_x L_vx L_y L_vy L_zL_vz]^T；

其中，

分别为X、Y、Z轴上的加速度方差矩阵，

分别为X、Y、Z轴上的定位误差方差矩阵；其中(x v_x a_x)、(y v_y a_y)、(z v_z a_z)3组状态变量分别为xyz三个坐标轴上的位置、速度、加速度分量，ε_x、ε_y、ε_z分别为各种误差源在三个坐标轴方向造成的总位置误差，观测变量(L_x L_vx)、(L_y L_vy)、(L_z L_vz)分别为接收机解算出的xyz三个坐标轴上的位置、速度分量；

R＝diag[R_x R_y R_z]，其中R_x、R_y、R_z分别为X、Y、Z轴上的观测噪声协方差分量；

步骤4、根据现时刻接收卫星信息初步解算位置、速度、载体运行方位角，然后根据前一时刻的位置、速度、加速度，利用卡尔曼滤波方程解算现时刻的位置、速度和加速度；

具体包括如下步骤：

步骤401、检测接收卫星信息是否良好以解算当前载体运行方位角，若是，利用如下卡尔曼滤波方程解算当前时刻的状态变量

返回

退出；若否，转到步骤402；

K_k＝P_(k，k-1)H^T(HP_(k，k-1)H^T+R)^-1

P_(k，k-1)＝ΦP_k-1Φ^T+Q

P_k＝(I-K_kH)P_(k，k-1)

式中，

是利用K-1时刻状态变量预测的K时刻状态变量预测结果，

是K-1时刻的状态变量最优结果，

是K时刻的状态变量最优估算值，K_k为卡尔曼增益；P_k为后验估计误差的协方差矩阵，P_(k，k-1)为先验估计误差的协方差矩阵，Z_k是K时刻观测变量；

步骤402、将k-1时刻状态变量X_k-1＝[x，v_x，a_x，ε_x，y，v_y，a_y，ε_y，z，v_z，a_z，ε_z]^T由XYZ坐标系转换为NEU坐标系，设NEU坐标系下的速度变量为Vn0、Ve0、Vh0：

步骤403、计算添加角速度辅助后的速度变量：

Vn＝(Vn0*Vn0+Ve0*Ve0)^1/2*cos(S_k-1)

Ve＝(Vn0*Vn0+Ve0*Ve0)^1/2*sin(S_k-1)

Vh＝Vh0

步骤404、令Vr_k-1＝Vr_k-2，Vr_k-1、Vr_k-2分别是K-1、K-2时刻的角速度；根据辅助方程角速度：Vr_k-1＝(S_k-S_k-1)/T计算S_k＝S_k-1+Vr_k-1*T，S_k是K时刻的载体运行方位角，T为观测时间间隔；步骤405、将NEU坐标系下的速度变量Vn、Ve、Vh转换为XYZ坐标系，设此时XYZ坐标系下的速度变量相应为v_x1、v_y1、v_z1，令

步骤406、联合Kalman滤波方程，两者进行等权计算现时刻状态变量预测结果：

返回

退出。

2.根据权利要求1所述的角速度辅助的Kalman滤波定位方法，其特征在于：所述步骤1具体包括如下步骤：

步骤101)接收机天线捕获并跟踪来自卫星的信号，解调出卫星的导航电文，得到卫星位置、钟差以及其他修正参数；

步骤102)根据导航电文的内容和格式来判断校验是否存在误码，剔除掉含有误码的导航电文对应的故障卫星；

步骤103)对跟踪到的可用的卫星的位置和运动轨迹的连续性进行判断卫星导航电文的正确性，剔除掉存在跳变异常的卫星以及载噪比和仰角不满足门限值要求的卫星。

3.根据权利要求1所述的角速度辅助的Kalman滤波定位方法，其特征在于：步骤3中，在车载GPS接收机情况下，

R_x＝R_y＝R_z＝diag(12²，0.2²)，τ_x＝τ_y＝ε_z＝0.5。

4.根据权利要求1所述的角速度辅助的Kalman滤波定位方法，其特征在于：步骤3中，

T为观测时间间隔，

其中Φ_x、Φ_y、Φ_z分别为XYZ三个坐标轴上的转换矩阵。

Images