导航技术方案

一、惯性导航系统

航位推算法(Dead reckoning，简称DR)是一种比较经典的导航方法，广泛使用在航海、航空和车辆自动定位系统中。航位推算法是利用初始位置，结合移动速度与方向来计算出位置的过程，即通过前一个计算历元的己知位置与时间间隔，以及从前一个历元到当前解算历元的平均速度及方向来进行计算完成。通过方向角将速度分解成东向速度与北向速度两个速度分量，各分量乘以时间间隔，得出位置的变化量，通过对初始位置矢量与位置的变化量求和，获得当前位置。

平台式惯性导航系统是INS惯性导航系统(Inertial Navigation System，简称INS)的其中一种，工作原理为：在稳定平台上安装陀螺仪和加速度计，通过加速度计测量载体沿平台坐标系各轴的加速度分量计算出载体速度、位置和对陀螺仪的加矩信息，而陀螺仪输出的信号则通过平台稳定回路，使平台跟踪导航坐标系。平台式惯性导航系统具有较高的精度，但系统设备的体积比较大，成本也比较高，故只适用于对精度要求比较高的导航定位系统。

二、GPS/INS组合导航定位系统

1.松组合

        GPS和INS都是独立的系统，导航信息也均由各自独立解算，采用卡尔曼滤波的方法对导航信息进行融合，并估计出组合导航系统的误差状态，然后再以误差状态的最优估计值来校正组合导航系统，最后解算出载体的位置、速度和姿态信息。组合导航系统一般采用反馈控制，以减小低精度IMU的输出误差。在反馈控制结构中，将滤波器估计出的传感器误差反馈给INS解算模块以补偿IMU输出误差。松组合方式提高了组合导航系统的可靠性，低精度INS与GPS多采用松组合。

2.紧组合

        GPS和INS也是作为两个独立的传感器进行信息融合。在到达4颗可见GPS卫星的情况下，紧组合和松组合导航精度基本相同，如果少于4颗星，紧组合依然可以跟踪航迹。紧组合算法的估计量为GPS与INS测量计算出的伪距与伪距率。上述两种组合模式下，均通过GPS测量值实现对INS误差漂移的控制，因此松组合与紧组合统称为GPS辅助INS的组合方式。

3.松组合结构图

        以位置、速度、姿态以及传感器误差作为卡尔曼滤波器的状态量进行估计，以INS与GPS解算的位置与速度信息作为滤波器的观测量，滤波器估计出的误差状态量用以修正INS的导航解算，从而输出车载组合系统的导航信息。 

4.嵌入式组合导航系统硬件平台

       嵌入式组合导航系统硬件平台包括处理器模块、A／D采样模块与智能多串口通讯模块，计算机系统。

 5.嵌入式组合导航系统的软件系统

• 初始化模块：初始化PC主机以及其它功能模块
• 自检模块：检测系统各关键故障点，并依照故障级别对系统工作状况进行处理或切换；
• 数据采集中断任务模块：定时的采集智能通讯模块输出数据；
• 命令发送模块：定时的发送各种命令报文；
• 时钟更新中断任务模块：更新系统内部时钟；
• 其它通讯中断任务模块：通讯接口的通讯服务；
• 数据采集专用监控模块：采集各传感器模块输出数据，预处理后通过双口RAM与主计算机进行通讯。
• 时问同步任务模块：对外部传感器数据采集的时间同步处理；
• 应用软件模块：初始对准模块、INS导航解算模块、GPS数据处理模块、组合导航信息融合模块以及人机交互模块等。

初始对准模块

静基座对准流程图

 动基座对准流程图

 组合导航信息融合模块

        在组合导航系统基于车载的应用背景下，采用松组合的方式进行信息融合并设计卡尔曼滤波器作为组合导航滤波算法，以位置误差、速度误差、失准角及惯性仪表随机误差作为状态量，通过卡尔曼滤波进行估计。

        卡尔曼滤波器得出估计值后，利用估计值进行校正，校正方法分两种：开环法和闭环法。开环法估计值仅校正系统的输出量，不对系统进行校正，输出校正的滤波器采用IMU原始测量值；而闭环法则将估计值反馈到系统中以校正系统状态，反馈校正的滤波器采用修正后的IMU测量值。IMU原始测量值数值大，修正后的IMU测量值数值小，如果系统状态方程为经过一阶近似的线性方程，那么状态量的数值越小近似的准确性就越高，因此两者相比，反馈校正的系统状态方程更为接近真实的系统状态动态变化过程。

        信息融合算法是组合导航系统设计的核心工作之一。通过对卡尔曼滤波技术的设计改进可以有效提升低动态与窄带宽的GPS与高动态与宽频带的INS结合的互补优势。

组合导航系统组成

组合导航系统组成框图

 多串口通讯模块软件设计

主CPU请求外部中断0服务模块流程图

请求中断的外部中断1服务模块流程图

多传感器数据采集时间同步的软件设计

时间同步的软件设计流程框图

三、GPS/DR组合导航定位系统

1.DR定位原理

        DR 定位利用载体航向和里程信息，以相对定位的方式推算载体的当前方位。随着多种定位技术的交叉融合，现代车辆 DR 定位技术是传统意义上的 DR（罗盘、里程计定位）和惯性导航（陀螺、加速度传感器定位）的组合，典型方式有惯性陀螺与轮速传感器构成 DR 子系统。惯性导航的基本原理是根据牛顿提出的相对惯性空间的力学定律，利用陀螺、加速度传感器等惯性元件测量载体的角速度和加速度，对观测量进行积分，在已知初始方位的基础上推算载体的当前方位。现代陆地车辆都装备有精确的轮速脉冲传感器，利用轮速脉冲测量里程信息比用加速度传感器信号进行两次积分计算里程更加方便和准确，因而现代车辆导航系统广泛采用轮速传感器测量行驶里程。

2.DR定位传感器

        DR 定位传感器主要包括角速率陀螺、轮速脉冲传感器和加速度传感器等。

3.DR定位误差

        数字计算误差和传感器观测误差。

4.GPS/DR组合定位

紧耦合的融合定位

        所有传感器信息集中输入到一个滤波器方程，估计系统状态，并用估计的误差校正传感器参数和定位输出。在 GPS 失效时，采用DR 定位。这种方式典型代表是集中卡尔曼滤波，优点是实现统计最优估计，精度高，缺点是计算量大影响到实时性，系统容错性差，一个传感器故障可能对整个系统产生“污染”，容易导致系统发散。

松耦合的融合定位

        用局部滤波方程对 GPS、DR 子系统滤波，由主滤波器融合局部滤波器的输出。这种方式有分散滤波、联合滤波、加权融合、模糊融合等。分散的信息融合技术降低了系统维数，计算量小，便于并行处理，且各子系统的耦合程度降低，单个传感器故障不会对其它子系统滤波方程产生严重影响，提高系统的容错性。Calson 提出的联合滤波器以其设计灵活、计算量小和容错性好而受到广泛重视。

GPS/DR组合定位基本结构

下机位数据采集程序

        GPS/DR组合定位算法

        利用导航计算机的处理资源，在导航主机上实现组合定位算法，实现原理如图所示。由 MCU 采集 GPS 和 DR 传感器的信息，通过串口发送到导航 CPU。处理串口接收的信息有两种实现方法，第一种方法是不对串口驱动做任何修改，组合定位算法为一个单独的进程或线程，独占串口资源，采用标准的串口操作函数对串口进行读写，获取到 GPS 和 DR 传感器信息后进行滤波融合，通过适当的机制（如事件机制、消息机制或动态内存交换技术）将融合后的定位信息传送给导航软件。因为通常导航软件是通过对串口（或其他通信设备）的读写获取 GPS 定位信息，因此这种方法要对导航软件的定位信息获取方式进行修改。

组合定位算法在导航主机上的实现

        重写串口驱动 PDD（Platform Dependent Driver）层，组合定位算法在串口驱动中完成，实现原理如图所示。CPU 接收到串口数据后，产生中断信号，中断首先由 MDD（Model Dependent Driver）层处理，调用串口驱动硬件相关层 PDD。PDD 层的一般处理方法是读取中断类型（接收事件、发送事件、错误事件等），然后返回给 MDD 层，MDD 层再调用 PDD 层的相应处理函数，如读取硬件接收缓冲区的数据等。这里修改了这一实现机制，直接在 PDD 层处理接收的数据，并传送到组合定位算法线程，在该线程处理接收的定位信息，若算法线程已经完成了一次融合运算，则将融合输出传送到接收缓存区，并向 MDD 层返回一个接收事件，由 MDD 层通知应用程序有接收事件发生，否则返回一个空事件，使 MDD 不做任何处理。这种实现方法的优点是串口的处理效率提高，并且不需要对应用软件做任何修改。为了方便调试与升级维护，算法的核心部分为一个单独的动态连接库，在串口驱动初始化时加载。

组合定位算法在串口驱动中的实现原理

        卡尔曼滤波在 GPS/DR 组合定位中的应用

        卡尔曼滤波理论是 Kalman 在 1960 年提出来的一种线性自适应滤波算法，具有两个显著的特点：

        （1）用状态空间方程描述对象的数学模型；

        （2）求解过程是递归计算，可以不加修改的应用于平稳和非平稳对象过程。

        状态的更新都由前一次估计和新的输入观测值计算得到，使用的存储空间小。卡尔曼滤波器的基本结构是预测器—修正器，如下图所示，这一结构包括两步：

        （1）利用观测值计算称为新息（innovation）的前向预测误差；

        （2）利用新息更新与随机变量的观测值线性相关的最小均方估计。

卡尔曼滤波器的求解过程

        基于模糊逻辑的 GPS/DR 组合定位方法

        基于卡尔曼滤波的多传感器信息融合方法可以得到线性随机系统的统计最优（或次优）估计，但实际应用中存在下述问题：

        1）滤波器性能取决于所建模型的准确度，GPS 噪声是一种非平稳随机噪声，建模难度大，对状态方程和观测方程线性化也引入建模误差；

        2）矩阵运算的计算量大，为保证实时性，要求采用较高性能的处理器，成本高，并占用较多的内存资源；

        3）算法本身存在数值稳定性问题。

        模糊逻辑等计算智能技术对复杂非线性系统具有很强的建模和学习能力，有研究者将其应用到 GPS/DR 组合导航中，用模糊逻辑对卡尔曼滤波增益、观测噪声和过程噪声进行调整。通过引入模糊逻辑，一定程度上改善卡尔曼滤波算法的精度和稳定性，但没有根本解决计算量大（实时性）的问题。Carlson 提出的联合卡尔曼滤波实质是按协方差阵加权的信息融合算法，与其相似，模糊多传感器信息融合以各子系统的估计误差为输入，用模糊逻辑计算子系统的置信度（或加权因子），算法具有计算量小、容错性好和便于调整的优点。

        GPS 和 DR 子系统独立工作，产生各自的定位输出，两个误差估计器分别估计 GPS 和 DR 子系统的输出误差。主模糊融合器有三个输入变量：DR 子系统输出误差估计、GPS 子系统输出误差估计、DR 与 GPS 子系统输出的偏差，融合器根据三个输入量计算DR 误差补偿，与 DR 输出结果相加，得到模糊融合结果。DR 误差随时间累积，需要经常重置或校正 DR，模糊推理器根据各子系统的误差估计，以及偏差的大小，控制 DR 系统的重新初始化和实时校正。

GPS/DR 模糊融合定位方案

四、RTX实时操作系统

1.RTX相关概念

        IntervalZero 公司研发的 RTX 系统是 Windows 环境下纯软件实现的硬实时操作系统，其拥有采用抢占式优先级的高效任务管理器，同时可以支持大部分C/C++语言函数库和 Win32 API。 RTX 通过对 Windows硬件抽象层(HAL)进行扩展来实现独立的内核驱动模型，并增加与 Win32 子系统并存的实时子系统(RTSS)来保证系统的实时性，RTX的体系架构如图所示。在RTX 环境下进行实时系统的开发，有以下几点优势：

        1)能够保证程序的实时性；

        2)实时接口 RtApi 的使用与Win32 环境类似, 便于程序的开发；

        3)RTX 支持使用C/C++的集成开发环境进行程序开发。

RTX 和 Windows 运行的架构图

2.事件标志

• 事件标志是实现进程同步的主要方法，每个进程有16个事件标识可供使用，所以最多能等待16个不同的事件。也可以同时等待多个事件标志，这种情况下，如果这些事件标志是与的关系，那么这些事件标志必须都被置位后该进程才能继续运行；如果这些事件标志是或的关系，那么这些事件标志中的一个或几个被置位后该进程就可以继续运行。
• 事件标志也可被ARM中断功能置位。在这种机制下，通过使用ARM中断函数设置任务等待的标志，可以使异步的外部事件和RTX核的任务同步。

3.信号量

• 在多任务实时操作系统中，需要特别的方法访问共享资源。否则，这些任务对共享资源的同时访问可能会导致数据的不一致或外设的错误操作。
• 解决访问临界资源问题的主要方法是信号量。信号量是包含了虚拟标志的软件对象。内核将标志给第一个请求的任务。在任务将其返回给信号量之前，没有其他的任务可以获取这个标志。只有拥有标志的任务才能访问公共资源，这就阻止了其他的任务访问和扰乱公共资源。
• 当信号量的标志不可用时，访问它的进程将被挂起，一旦标志被返回，这个进程就会被唤醒。为了解决错误的等待条件，必须引入超时机制。

4.互斥量

• 互斥量是解决进程同步问题的另一种方法。它们用作对临界区的访问控制，只有拥有互斥量的进程才能访问临界区，其他试图访问临界区的进程将被阻塞。

5.信箱

• 有时进程之间需要交换消息，这在网络中是很常见的，例如TCP-IP、UDP、ISDN等。
• 消息就是包含协议消息或帧的内存块的指针，这样的内存块可以动态的分配和提供给用户。为了防止内存泄漏，用户有责任正确地分配和回收内存块。
• 如果接收进程访问信箱中的消息不存在，它将被挂起，直到该消息被发送进程发送到信箱中，该被挂起的接收进程才会被唤醒。

6.RTX工作原理 

1）时间片中断

• 使用一个标准的ARM定时器产生周期性的中断，这个中断即RTX核的时间片中断。RTX核是通过定时器0来产生系统时钟片。任何一个定时器都可以作为系统时钟片产生器，在系统配置文件RTX Config.c可进行设置。
• RTX核时间片中断每隔150000个机器周期产生一次，因此速度运行的标准ARM设备来说，其时钟片是0.01秒(10ms)或者说是100Hz(60MHz/4/150000)。

2）系统时钟任务

• 节拍时钟任务是系统任务，在每个时钟节拍中断中执行，具有最高的优先级，而且不会被抢占，其主要是用于任务间的切换。
• 最高优先级原理：os_clock_demon 是系统节拍时钟脉冲任务，是系统任务，也是由系统创建，管理其他的所有任务，处理任务延时超时和将等待任务休眠。当必需条件满足，又将休眠任务唤醒。

3）任务管理

• RTX的所有任务都处在一个确定的状态，主要状态类别有：
• 运行态RUNNING、就绪态READY、阻塞等待态WAIT_*、不活跃态INACTIVE

4）空闲任务

• 空闲任务os_idle_demon，是一个简单的死循环。
• ARM设备提供了空闲模式以减少能耗，它是通过停止执行程序来实现的，可以通过中断来唤醒。在这种模式下，所有的外设包括中断系统仍然在运行。
• RTX核是在os_idle_demon()空闲任务（当没有其他任务处于就绪态时）中初始化空闲模式的。当RTX核时间片中断（或其他中断）发生时，微控制器将重新开始执行程序。
• 可在RTX核os_idle_demon()空闲任务中执行用户自己扩展的代码，在 RTX_Config.c系统配置文件中可以设置这些代码。
• 注意：在使用JTAG接口调试时不能使用空闲模式。

5）系统资源

• TCB，RTX内核任务由任务控制块。相当于linux下的PCB。

6）多任务协作

• 如果禁止了多任务轮转，必须设计和实现多任务的协作。那么在每一个任务的某些位置都必须调用系统等待函数如os_dly_wait() 和os_tsk_pass()，这些函数可通知RTX 核去执行另一个任务。
• 注意：如果下一个就绪的任务拥有比当前运行任务低的优先级，则调用os_tsk_pass 不会产生任务切换。

7）轮转多任务

• 即并发，实际上，这些任务并非真正并发执行，而只是基于时间片的轮转，由于时间片很短，仅有几个毫秒，所以感觉到这些任务好像是在同时执行。

8）多任务抢占

• RTX核是一个实时的多任务抢占系统，如果有一个比当前运行任务优先级高且就绪的任务，那么当前的任务将会被挂起。

9）栈的管理

• RTX核系统需要一个当前运行的任务的栈空间
• 局部栈: 存储参数，动态变量和函数的返回地址，该栈可在ARM芯片中的任何地方，但基于性能的考虑，推荐使用片上RAM。

10）中断函数

• RTX允许中断函数并发操作，但还是推荐避免使用IRQ中的嵌套。

五、实时动态定位技术RTK

        RTK(Real-Time-Kinematic)技术是GPS实时载波相位差分的简称。这是一种将GPS与数传技术相结合，实时解算并进行数据处理，在1~2秒时间内得到高精度位置信息的技术。与动态定位方法相比，定位模式相同，仅要在基准站和流动站间增加一套数据链，实现各点坐标的实时计算、实时输出。

1.工作原理

        基准站建在已知或未知点上；基准站接收到的卫星信号通过无线通信网实时发给用户；用户接收机将接收到的卫星信号和收到基准站信号实时联合解算，求得基准站和流动站间坐标增量（基线向量）。站间距30公里，平面精度1-2厘米。

        高精度的GPS测量必须采用载波相位观测值，RTK定位技术就是基于载波相位观测值的实时动态定位技术，它能够实时地提供测站点在指定坐标系中的三维定位结果，并达到厘米级精度。

        （1）在RTK作业模式下，基准站通过数据链将其观测值和测站坐标信息一起传送给流动站。

        （2）流动站不仅通过数据链接收来自基准站的数据，还要采集GPS观测数据，并在系统内组成差分观测值进行实时处理，同时给出厘米级定位结果，历时不足一秒钟。

        （3）流动站可处于静止状态，也可处于运动状态；可在固定点上先进行初始化后再进入动态作业，也可在动态条件下直接开机，并在动态环境下完成整周模糊度的搜索求解。

        （4）在整周未知数解固定后，即可进行每个历元的实时处理，只要能保持四颗以上卫星相位观测值的跟踪和必要的几何图形，则流动站可随时给出厘米级定位结果。

        总结：RTK就是实时动态差分测量，其原理简单来说就是，基准站用一个固定坐标来做参考，以后基站每通过接收卫星算得一个坐标，就跟固定坐标作比对，得到差值，然后将这个差值发送给移动站，移动站用卫星接收到的坐标再减去基站发过来的差值就得到了改正后的坐标。

2.RTK技术应用中的问题

1）GPS实时差分定位RTK技术的缺点

• 用户需要架设本地参考站
• 定位误差随距离的增长而增大
• 误差增长使流动站和基准站的作业距离受到限制
• 在RTK作业过程中，由于受作业距离的限制，GPS技术固有条件的限制(参考站和流动站之间必须没有山体、楼群之类的遮挡，作业区域不能存在强烈的电磁波干扰)，需要不断移动基准站，架设中继站，作业效率低，工作繁琐，且由于数据通讯的原因影响了单基准站RTK的定位精度和质量，可靠性和可行性也随距离而降低
• GPS接收机的性能要高，确保长基线进行正确整周未知数求解
• 数据链的性能要好，传送距离要远，能正确无误的将参考站数据发送到流动站

2）实际应用中受到的限制

• 由于RTK技术局限性的存在，使得其在实际应用中受到了限制，常规RTK作业时，流动站GPS接收机接收到基准站的差分信号后，首先出现的是浮动解结果，经过一段时间的观测和计算后得到固定解结果，这段时间被称为初始化时间，其与作业期间的观测卫星数、卫星分布状况、与基准站的距离和观测环境等都有关系。

3.移动站的解算状态

1）固定解

• 当使用载波相位观测值定位的时候，会产生模糊度，模糊度理论上是整数。通过算法解出整数的模糊度之后，会大幅度提高定位精度。对固定解而言,其定位精度在1厘米以内,可满足除首级控制测量及其他高精度要求的测绘工作以外的各种比例尺的地形图测绘、施工放样及图根点测量等。

2）浮点解

• 有的时候解不出整数所得到的解。如果浮点解的定位精度小于0.5米,根据经验其平面和高程误差一般在0.1-0.3米,可满足精度要求不大于0.5米的测绘工作;定位精度若大于0.5米,平面和高程误差一般在1-3米。

3）单点解

• 接收机未使用任何差分改正信息计算的3D坐标。差分解和单点解的定位精度一般在5-10米之间,有时还会更大,一般不采用。

4）区别

• 对于不接收基准站信号的情况下的单点解,若使用离测区最近区域的校正参数作为转换参数,一般平面误差在1-5米,可用来寻找控制点,但高程相差很大,不能利用。
• 精度上固定解>浮动解>单点解