CN101413799B - 一种用于室外移动目标的组合导航系统 - Google Patents

Abstract

本发明一种用于室外移动目标的组合导航系统，属于组合导航系统领域，特别涉及用于室外移动目标采用多个传感器组合导航领域。组合导航系统主要由偏振光传感器、GPS、里程计、数据采集和信息处理电路和信息处理算法组成。导航处理机由核心处理器、ROM、RAM、DSP组成，导航处理机子系统将偏振光传感器子系统，GPS子系统和里程计子系统输出的数据进行处理和信息融合，之后输出室外移动目标的位置、航向信息。本发明中偏振光传感器仅仅依赖于外部稳定的偏振光分布模式，导航信息误差不随时间增加而增加，为室外移动目标的组合导航提供了新的选择。

Description

一种用于室外移动目标的组合导航系统

技术领域

本发明属于组合导航系统领域，特别涉及用于室外移动目标采用多个传感器组合导航领域。

背景技术

现有的移动目标定位方法主要分为相对定位和绝对定位两类。相对定位方法短时间内精度和稳定性较好，但是测量误差随时间不断累积，最终导致测量失效；绝对定位方法测量误差具有时间和空间的独立性，不随时间累积，但是容易受到干扰，短时间波动较大。现在广泛采用将绝对定位和相对定位组合使用的方法，对移动目标进行精确的定位。然而，任何单一的导航系统都不可能完全满足这种需要，并且，对任何单一的导航系统的改进都只有一定的限度，进一步的改进则更加困难。在这种情况下，发展组合导航技术则成为一种必然趋势。

室外移动目标目前最为成熟的组合导航系统是惯性导航系统(简称为INS)和全球定位系统(简称为GPS)。其中陀螺仪为INS的核心部件，因为其可以提供载体的姿态信息，一直备受导航系统设计者青睐，但是陀螺仪的价格却是非常昂贵的，高精度的陀螺仪更是令人望而却步；另外，陀螺仪误差随时间累积，必须根据需要在一定的时间之后对陀螺仪进行校正。

发明内容

本发明要解决的技术难题是克服上述现有技术的缺陷，提供一种新型的组合导航系统，将来自GSP、偏振光传感器、里程计的信息处理后输出，为室外移动目标提供必要的导航信息。用偏振光传感器，里程计和GPS组成一个导航系统，里程计的瞬时精度高，但是误差累计，而偏振光传感器和GPS的误差都不随着时间累计，三种传感器的这些特点是互补的。本发明为室外移动目标提供全方位的导航信息，包括移动目标的位置，速度，角速度，海拔。偏振光传感器为系统的测角传感器；GPS为系统提供位置信息；里程计为系统提供速度、角速度和位置信息。该发明提高了室外移动目标导航的精度和可靠性，并且较传统的室外移动目标的组合导航系统成本较低；适用于室外移动目标的导航领域。

本发明采用的技术方案是：一种用于室外移动目标的组合导航系统，由偏振光传感器子系统，GPS子系统，里程计子系统，导航处理机子系统，共四部分组成；导航处理机子系统将偏振光传感器子系统，GPS子系统和里程计子系统输出的数据进行处理和信息融合，之后输出室外移动目标的位置、速度信息；导航处理机子系统由核心处理器、ROM、RAM、DSP组成，核心处理器用于控制和采集传感器数据，DSP用于导航计算；核心处理器通过UART串口采集GPS子系统、偏振光传感器子系统和里程计子系统的数据；核心处理器将采集到的数据通过DSP的HPI口传送给DSP进行导航计算；核心处理器通过JTAG接口进行调试；LCD为显示控制界面；USB用于存储传感器数据。

导航处理机子系统在对信息进行处理时采用了卡尔曼滤波，将GPS子系统与里程计子系统的位置信息进行卡尔曼滤波得到处理后的位置信息；将偏振光传感器子系统与里程计子系统的角度信息进行卡尔曼滤波得到处理后的位置信息的角度；利用卡尔曼滤波融合里程计子系统和偏振光传感器子系统数据，状态向量取为：

X＝[θ  ω_d]^T                        (1)

系统的状态方程为：

X_k+1＝Φ_k+1/kX_k+Γ_kW_k                 (2)

系统的量测方程为：

Z_k＝H_kX_k+V_k                        (3)

融合卡尔曼滤波方程为：

X_k+1/k＝Φ_k+1/kX_k/k

$P_{k+1/k}={\mathrm{\Φ}}_{k+1/k}{P}_{k/k}{\mathrm{\Φ}}_{k+1/k}^T+Q$

$K=P_{k+1/k}{H}_k^T{(H_k{P}_{k+1/k}{H}_k^T+R)}^{-1}$

X_k+1/k+1＝X_k+1/k+K(Z_k-H_kX_k+1/k)        (4)

GPS与里程计进行组合时，系统状态变量选取为：

X＝[x y v_x v_y]^T                     (5)

系统的状态方程为：

X_k+1＝Φ_k+1/kX_k+Γ_kW_k                   (6)

式中ω_g为偏振光传感器的角速度，Φ_k+1/k是t_k时刻到t_k+1时刻的状态转移矩阵，表达式见式(4)；β是里程计校正系数；W_k是协方差为Q的系统高斯白噪声矩阵；Γ_k是系统噪声矩阵的驱动矩阵，H_k为系统量测矩阵，本系统中为I；V_k是协方差为R的系统量测噪声矩阵，P为误差协方差矩阵；K为卡尔曼增益矩阵；

通过公式(4)，计算出移动目标的当前航向角θ和位置坐标(x，y)。

本发明的显著效果如下：偏振光传感器仅仅依赖于外部稳定的偏振光分布模式，导航信息误差不随着时间增加而增加；抗干扰能力较好；实现成本低。

附图说明

附图1为偏振光传感器、里程计和GPS组合导航原理图。1—GPS子系统，2—偏振光传感器子系统，3—里程计子系统，15卡尔曼滤波，16—剔除GPS野值，30—位置、航向。

附图2为系统硬件结构图。1—GPS子系统，2—偏振光传感器子系统，3—里程计子系统，4—导航处理机，5—DSP，6—HPI口，7—核心处理器，8—JTAG接口，9—网络接口，10—LCD，11—USB，12—ROM，13—RAM。

附图3为系统软件结构图。17—应用程序，18—内核共享缓冲区，19—设备驱动程序，20—实时任务，21—Linux内核模块，22—实时调度模块，23—非中断控制硬件，24—中断控制硬件。

具体实施方式

结合附图和技术方案详细说明本发明的具体实施方式，偏振光传感器作为一种新型测角传感器，其测角原理是利用天空中稳定的偏振光分布模式，而有些生物具有对太阳光偏振方向极其敏感的视觉神经系统，这些生物可以利用稳定的偏振光分布模式进行导航。偏振光传感器则模仿沙漠蚂蚁利用偏振光导航机理设计的一种传感器。

地面轮式移动目标通常采用在轮上安装里程计(光电编码器)测量角位移，进而结合轮子的直径来推算移动目标的速度、角度和位置等信息，但是由于移动目标轮子半径及移动目标结构尺寸的误差、运动中的滑动及地面的起伏等原因，里程计的误差将逐渐增大，无法进行长时间的定位。本文采用偏振光传感器、GPS与里程计进行数据融合。偏振光传感器在移动目标速度较低的情况下可以很好的工作，没有误差累积，这恰恰是里程计所不具备的。偏振光传感器缺点是在周围有遮挡物体的短时间内输出误差较大，而里程计虽然累积误差逐渐增大，但是短时间内里程计的精度却可以很高。Kalman滤波器将里程计输出的位置信息和GPS的数据进行融合；将里程计输出的航向角信息与偏振光传感器进行融合。根据各传感器的特点设计一种滤波器，传感器组合原理如图1所示。

本发明对多传感器信息融合选用较为常用的松散组合，即基于速度和位置的组合。用里程计与偏振光传感器、GPS输出的位置和速度信息作为量测值，经卡尔曼滤波，估计里程计的误差，然后对里程计进行校正。

组合导航系统分为偏振光传感器子系统、里程计子系统、GPS子系统、导航处理机，如附图2。其中偏振光传感器子系统输出为数字量，接口为RS232；里程计子系统包括光电编码器和单片机，输出接口为RS232，移动目标的左右轮各安装一个光电编码器；GPS子系统同样为串口输出。由于GPS数据更新速率较低，对数据采集要求不高，导航数据采集选用三星公司的一款ARM9处理器S3C2440。该处理器架构采用RISC体系结构，时钟频率可达500MHz，并且其功耗低，内核工作电压1.2V，可在16/32位模式下工作。另外该处理器还具有MMU，因此适合运行多任务操作系统。系统RAM为2X32MByte，RAM采用NandFlash，大小为64M，用于存放系统代码。导航数据处理部分采用DSP处理器，导航核心处理将接收到的数据通过DSP的HPI口发送给DSP，如图2，DSP对导航数据进行卡尔曼滤波，并将滤波后的导航信息通过HPI口再发送给导航核心处理器，以完成对移动目标的导航。DSP采用TI公司的数字信号处理芯片TMS320VC5416。

系统软件结构如图3，主要分为两部分，软件主要包括内核部分和用户部分两个层次，而内核部分主要又包括内核共享缓冲区，设备驱动程序，实时任务，Linux内核模块，实时调度模块，非中断控制硬件，中断控制硬件，这些部分构成了操作系统。应用程序部分包括数据的采集、处理和显示。驱动程序包括串口驱动程序和ARM与DSP通讯的驱动程序。引入操作系统，不仅可以使程序的编写变得简单，更重要的是增加了系统的稳定性。Linux是一个多用户、多任务、开源的操作系统。用户可以根据自己的需要在遵守开源精神的前提下，对操作系统内核进行配置和修改。这里需要编写数据采集的驱动程序、ARM与DSP通讯驱动程序等，并将其编译进内核。然后将编译好的内核烧写到flash中。应用程序主要包括数据采集和运算，首先在PC机上交叉编译通过，编译通过后，移植到导航系统硬件上运行测试。

数据处理部分，从附图1组合导航处理原理中可以看出。首先用里程计的瞬时值剔除GPS的野值，然后Kalman滤波器将里程计输出的位置信息和GPS的数据进行融合；同时将里程计输出的航向角信息与偏振光传感器进行融合。利用卡尔曼滤波器融合里程计和偏振光传感器数据，状态向量取为：

X＝[θ ω_d a_w]^T                            (7)

式中ω_g为偏振光传感器的角速度，考虑到移动目标以比较低的速度旋转，并且驱动电机的加速度较大，因此移动目标旋转可以视为匀角速度运动，则状态向量可以写为：

X＝[θ ω_d]^T                               (8)

系统的状态方程为：

X_k+1＝Φ_k+1/kX_k+Γ_kW_k                        (9)

式中，Φ_k+1/k是t_k时刻到t_k+1时刻的状态转移矩阵，；β是里程计校正系数；W_k是协方差为Q的系统高斯白噪声矩阵；Γ_k是系统噪声矩阵的驱动矩阵，本系统中为I。

${\mathrm{\Φ}}_{k+1/k}=\left[\begin{array}{cc}1& T\\ 0& 1\end{array}\right]---\left(10\right)$

系统的量测方程为：

z_k＝H_kX_k+V_k                                   (11)

其中，H_k为系统量测矩阵，本系统中为I；V_k是协方差为R的系统量测噪声矩阵，则融合卡尔曼滤波方程为：

X_k+1/k＝Φ_k+1/kX_k/k

$P_{k+1/k}={\mathrm{\Φ}}_{k+1/k}{P}_{k/k}{\mathrm{\Φ}}_{k+1/k}^T+Q$

$K=P_{k+1/k}{H}_k^T{(H_k{P}_{k+1/k}{H}_k^T+R)}^{-1}$

X_k+1/k+1＝X_k+1/k+K(Z_k-H_kX_k+1/k)                   (12)

其中，P为误差协方差矩阵；K为卡尔曼增益矩阵。

里程计对移动目标路程的推算误差随着时间累积，为了实现移动目标较高精度的定位，采用GPS对里程计进行校正。系统状态变量选取为：

X＝[x y v_x v_y]^T                              (13)

其中，(x，y)为移动目标当前位置坐标；(V_x，V_y)为移动目标移动速度在x轴和y轴方向的分量值；

系统的状态方程为：

X_k+1＝Φ_k+1/kX_k+Γ_kW_k                            (14)

其中，

${\mathrm{\Φ}}_{k+1/k}=\left[\begin{array}{cccc}1& 0& T& 0\\ 0& 1& 0& T\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]---\left(15\right)$

其中T为系统状态时间间隔。系统的量测方程为：

Z_k＝H_kX_k+V_k                                    (16)

通过公式(12)，就可以计算出移动目标的当前(x，y)坐标，然后再根据计算出的(x，y)坐标对里程计的累积误差进行补偿。

Claims (1)

1.一种用于室外移动目标的组合导航系统，其特征在于：由导航处理机子系统(4)将偏振光传感器子系统(1)，GPS子系统(2)和里程计子系统(3)输出的数据进行处理和信息融合，之后输出室外移动目标的位置、速度信息；采用速度和位置的组合方式，用里程计与偏振光传感器、GPS输出的位置和速度信息作为量测值，经卡尔曼滤波，估计里程计的误差，然后对里程计进行校正；

里程计子系统(3)包括光电编码器和单片机，输出接口为RS232，导航处理机子系统(4)由用于控制和采集传感器数据的核心处理器(7)、ROM(12)、RAM(13)和用于导航计算的DSP(5)组成；核心处理器(7)通过UART串口采集GPS子系统(1)、偏振光传感器子系统(2)和里程计子系统(3)的数据；核心处理器(7)将采集到的数据通过DSP(5)的HPI口(6)传送给DSP(5)进行导航计算；核心处理器(7)通过JTAG接口(8)进行调试；LCD(10)为显示控制界面；USB(11)用于存储传感器数据；导航处理机子系统(4)在对信息进行处理时采用了卡尔曼滤波(15)，将GPS子系统(1)与里程计子系统(3)的位置信息进行卡尔曼滤波(15)得到处理后的位置信息(30)；将偏振光传感器子系统(2)与里程计子系统(3)的角度信息进行卡尔曼滤波得到处理后的位置信息(30)的角度；利用卡尔曼滤波(15)融合里程计子系统(3)和偏振光传感器子系统(2)数据，状态向量取为：

X＝[θω_d]^T    (1)

系统的状态方程为：

X_k+1＝Φ_k+1/kX_k+Г_kW_k    (2)

系统的量测方程为：

Z_k＝H_kX_k+V_k    (3)

融合卡尔曼滤波方程为：

X_k+1/k＝Φ_k+1/kX_k/k

$P_{k+1/k}={\mathrm{\Φ}}_{k+1/k}{P}_{k/k}{\mathrm{\Φ}}_{k+1/k}^T+Q$

$K=P_{k+1/k}{H}_k^T{(H_k{P}_{k+1/k}{H}_k^T+R)}^{-1}$

X_k+1/k+1＝X_k+1/k+K(Z_k-H_kX_k+1/k)    (4)

GPS与里程计进行组合时，系统状态变量选取为：

X＝[x y v_x v_y]^T    (5)

系统的状态方程为：

X_k+1＝Φ_k+1/kX_K+Г_kW_k    (6)

式中ω_g为偏振光传感器的角速度，Φ_k+1/k是t_k时刻到t_k+1时刻的状态转移矩阵，表达式见式(4)；β是里程计校正系数；W_k是协方差为Q的系统高斯白噪声矩阵；Г_k是系统噪声矩阵的驱动矩阵，H_k为系统量测矩阵，本系统中为I；V_k是协方差为R的系统量测噪声矩阵，P为误差协方差矩阵；K为卡尔曼增益矩阵；通过公式(4)，计算出移动目标的当前航向角θ和位置坐标(x y v_x v_y)。

Images