基于GPS的路线测量与拟合

  前言

　　GPS（全球定位系统）由24颗卫星支持，具备全球性，全天候，连续的导航与定位。由于其高精度，高速度，低成本，使用方便的特点，不仅在军事上获得广泛的关注，在民用部门的应用也越来越多。

　　目前，国内对GPS的应用主要集中在车辆信息服务系统和铁路、公路施工勘测上。对铁、公路路线的勘测可分两种情况，一是建设路线的需要，利用 GPS进行初测导线点与水准点的定位，一是对现有路线通过GPS采集路线数据，还原出实际路线图。对后一种情况，由于路线采集点密度和测量误差的因素，在实际应用中，需要利用所得数据进行一定的拟合。

　　2  课题背景

　　在国内 ，现有机车头灯都是固定的，当机车进入弯道时，光线并不能实时照射在前方铁轨上。设计一种活动头灯，以机车路线数据为基础，通过GPS获得机车当前位置，并实时调整头灯转角，可以大大提高机车行车安全。该系统框图如下：

图1 机车头灯控制系统框图

　　在车载控制器中，GPS接收机获得的位置、速度数据与预先存储的路线数据表进行比较计算，最终得出当前机车所在位置需要发出的控制转角信号量。预先存储的路线数据表需要在个人计算机机中提前进行处理，即前言提到的拟合。无论是在车辆信息服务系统还是其他与地理路线相关的应用系统中，路线数据的采集与拟合都是极为重要的环节。

　　3  GPS数据的采集

　　GPS接收机接收到的数据是按一定报文格式的数据流以串口方式输出的。其格式为：

　　$GP RMC,081546，A,105.7038,N,30.3624,E,0.000,0.0,220406,1.1,W*78

　　以逗号为分隔符，各数据项依次表示新数据帧的开始标志、格林威治标准时间、数据有效标志、纬度 、南北纬标志 、精度 、东西经标志、移动速度、日期 、磁变、东西磁变标志与校验和。对于路线的拟合，实际需要的只是每个点的经纬度，为此需要进行提取处理，具体可通过便携计算机随车采集，计算机与GPS接收机之间通过串口通信， 通信控制可利用微软公司的MSCcomm串行通信控件进行简单灵活的编程，另外也可以直接调用Windows API函数或动态链接库进行更底层丰富的设计。这些方法有很多文章进行专门的论述，本文就该系统实际采用的PC104微型主板通过串口通信的方法做个简单介绍。PC104微型主板体积小，加上GPS接收机仍然非常小巧，便于携带，采集路线数据时置于机车上，采集的数据存储在自身FLASH上，采集完毕可以通过IDE接口拷贝到个人计算机硬盘上。PC104装载的是DOS6.0系统， 对串口操作分软中断和硬中断两种方式，硬中断相对效率要高些。采用C语言在DOS下进行硬中断方式的串口通信编程步骤及注意事项如下：

　　1．  串行通信以通用异步发送/接收器8250进行控制，8250有10个可编程的单字节寄存器，占用7个端口地址，复用地址通过读/写操作和线路控制寄存器的第7位来区分。COM1、COM2对应的7个端口地址分别为3F8H~3FEH、2F8H~2FEH。初始化串口主要是写波特率因子寄存器以设定通信速率，其次是读取接收寄存器和中断标志寄存器以清除现有的接收或发送中断标志。

　　2．  硬中断通道IRQ4（COM1）和IRQ3（COM2）分别对应中断向量0BH和0CH，在载入新的中断服务程序之前必须获取并保存原中断服务程序的入口地址，相应函数为getvect()和setvect()。

　　3．  中断控制器8259有两个口线用作COM1和COM2硬中断通道，可通过设置其中断屏蔽寄存器位（bit4对应IRQ4，bit3对应IRQ3）来开启或屏蔽，中断屏蔽寄存器的端口地址为21H。每次中断服务程序返回必须往中断命令寄存器(地址20H)写入20H以使8259清除相关寄存器位。

　　4．  在中断服务程序中，通过读取中断标志寄存器判别中断类型。按优先级高低依次为：接收出错中断、接收到数据中断、发送寄存器空中断、MODEM状态寄存器改变中断。响应中断后，8250自动复位中断标志。

　　5．  采用硬中断方式的串口通信必须设置合适的接收和发送缓冲区，缓冲区的读取和写入可以通过缓冲区首尾索引变量来操作。

　　4   三次样条插值的原理

　　随车采集的路线数据是一个个离散的点，对这些点数据进行平滑过渡，形成一条平滑曲线便得到机车行车路线。一般一条机车路线的数据采集点是有限的，对相邻点进行简单连线过渡是比较粗糙的。在工程上，通常采取插值的方法来构造曲线，插值也叫拟合。插值有线性插值，立方插值和三次样条插值等方法。线性插值应用比较广泛，但它只适用于离散点变化缓和，波动不大的情况，对于波动较大的情况，三次样条函数具有优良的性能。目前GPS在民用定位上还有5米左右的误差，考虑到该因素以及采集点相对稀疏，采用三次样条插值是比较合适的选择。

       样条也就是弹性细木条或金属条，早期工程技术人员利用样条的弹性弯曲使之通过一系列离散点来得到平滑曲线。在数学上，利用样条函数进行插值的方法即为样条插值。

　　假设在区间[a ，b]上，存在n+1个点满足：a=x0<x1<x2<x3<…<xn-1<xn=b，且存在函数f（x），如果此时另有一函数S(x)符合如下条件：

　　1.   S(x)在区间[xi-1，xi]（i=1,2,…,n）上均为三次多项式

　　2.    S(x)通过上述所有节点，即S(xi)=f(xi)=yi(i=0,1,…,n)

　　3.    S(x)存在一阶二阶导数在区间[a，b]上连续

　　则S(x)即称为三次样条函数，三次样条函数以多项式的方式表示如下：

　　        S(x)=aix3+bix2+cix+di     xi-1<x<xi(i=1,2,…,n)

　　根据条件2和3，可以得出以下两个方程组：

　　Si(xi)=aixi3+bixi2+cixi+di=yi

　　Si(xi-1)=aix i-13+bix i-12+cix i-1+di=y i-1   (i=1,2,3,…,n)

　　    3ai-1x i2+2bi-1x i+ci-1=3aixi2+2bixi+ci

　　    6ai-1x i+2bi-1=6aixi+2bi                        (i=2,3,4,…,n)

　　这两个方程组共有4n个未知系数，确定的方程只有4n-2个，必须附加两个条件才能求解，为此可根据三次自然样条插值法,增加两个自然边界条件：

　　S〞(x0)=y0〞=0

　　S″(xn)=yn″=0

　　       若令Mi=S″(xi)，hi=xi-xi-1则

　　       S″(x)=(xi-x)Mi-1/hi+(x-xi-1)Mi/hi   (i=1,2,3,…,n)

　　对上式进行两次积分得到：

　　S(x)=(xi-x)Mi-1/hi/6+(x-xi-1)Mi/hi/6+c1(xi-x)+c2(x-xi-1)  xi-1<x<xi (i=1,2,3,…,n)

　　根据条件2，S(x)=yi，可以确定c1，c2，最终得到S(x)表达式：

　　       S(x)=(xi-x)Mi-1/hi/6+(x-xi-1)Mi/hi/6+(xi-x)(yi-1/hi-hiMi-1/6)+(x-xi-1)(yi/hi-hiMi/6)

　　根据条件3，对上式求左右导数并令其相等，可得到n-1个方程：

　　hiMi/2-(Mi-Mi-1)hi/6+(yi-yi-1)/hi=-hi+1Mi/2-hi+1(Mi+1-Mi)/6+(yi+1-yi)/hi+1   (i=1,2,3…,n-1)

　　令ui=hi+1/(hi+hi+1),vi=[(yi+1-yi)/hi+1-(yi-yi-1)/hi]/hi+hi+1,上述方程可简化为：

　　(1-ui)Mi-1+2Mi+uiMi+1=vi   (i=1,2,3,…,n-1)

　　再结合考虑两端边界条件：2M0+u1M1=v1和unMn-1+2Mn=vn,最后方程矩阵如下：

　　如果边界条件假定为如前所述的自然边界，则u0=0,v0=0,un=0,vn=0,也即M0=Mn=0。

　　求解方程组得到Mi（i=0,1,2,… ,n），代入S(x)即为相临两点间的三次样条函数，n段三次样条曲线以已知数据点为临界衔接点进行连接便可得出一条通过所有数据点的平滑曲线。

       5   MATLAB实现样条插值

　　在本系统中，路线数据的采集点以经纬度描述，整条路线类似于二维曲线，以实验测试路段为例，其采集点如下：

　　表一  路线采集点数据

       三次样条插值过程实质上就是求解三次样条函数的过程，在实际应用中，首先必须根据具体样本点情况构造出合适的三次样条函数，但这需要工程技术人员具备良好的数学功底和数据分析能力。此外，对于离散样本点较多的情况，编制计算机程序求解大方程组也不简单。

　　MATLAB将高性能的数值计算和可视化集成在一起，并提供了大量专业领域的内置函数，从而被广泛地应用于科学计算、控制系统、信息处理等领域的分析、仿真和设计工作。就数值插值而言，MATLAB提供有一维、二维、三维插值方法，对于一维插值可通过函数interp1(x,y,xi,'method')实现，其中x,y是已知数据点的值，xi是要内插的数据点,method是内插方法，可以指定为一次（linear）、三 次(cubic)方程式或spline函数，其预设方法是linear。如果数据的变化较大，以 spline函数内插所形成的曲线最平滑 ，所以效果最好。而三次方程式所得到的内插曲线平滑度，则介于线性与spline函数之间。

　　对表一数据进行插值，步骤如下：

　　1.   分析经纬两组数据，以相对单调性较好的一组为自变量确定插值精度。表一数据比较简单，可以经度为自变量，0.1’为步长确定需要内插数据点矩阵（GPS接收机输出的经纬度数据由度数和分数组合在一起，如10352.0329表示103度52.0329分，在应用中需要统一转换成以分为单位。本文的实验测试路线较短，不涉及度数部分的变化，为了直观，暂不做转换）。

　　2.    将已知数据点列成１２*２矩阵形式。

　　3.    调用interp1(x,y,xi,'method')函数，选择内插方法为spline,所得结果与内插数据点构成2*27矩阵，该矩阵每列上的两个数据即为拟合路线上位置点经纬度。

　　对应程序如下：

　　x=[10352.0329 10352.3988 10353.0197 10353.2919 10353.246 10353.3482 10353.5829 10353.7319 10354.1310 10354.2028 10354.5745 10354.6678;　3018.2936 3018.6298 3018.7247 3019.1021 3019.2701 3019.5516 3019.844 3020.0859 3020.9478 3021.167 3022.5694 3022.7908]';
  1.03544329000000    1.03545329000000     1.03546329000000 ;   0.30182936000000   0.30186004069566   0.30187182180522   0.30186964693605   0.30185845969551   0.30184320369097   0.30182882252981   0.30182025981940   0.30182245916710   0.30184036418029    0.30187889352712    0.30192733665639    0.30193266863021   0.30193987411039   0.30199701768689   0.30198992627645   0.30198730620686   0.30200882997022   0.30203060327638   0.30205012896416   0.30207062461427   0.30209530780615    0.30212717111085    0.30216587747376    0.30220651085041   0.30224372965360    0.30227219229616]

　　 由于步长足够小，产生的数据相对原数据丰富的多，拟合出的路线曲线平滑，这可通过函数plot()进行效果显示。在具体路线的拟合过程中，可根据ＧＰＳ数据采集点的密度改变插值步长，以达到实际控制要求。

　　6   小结

　　ＧＰＳ在应用过程中的工作主要集中在定位数据的采集和后续处理，特别是在与ＧＩＳ（地理信息系统）的结合应用中，为了描绘实际地理形状或路线，必须采取一定的算法尽量见减小结果误差。三次样条插值以其优良的数学特征正好满足这方面的需求，结合功能强大的ＭＡＴＬＡＢ工具，其实现简单方便，大大提高了相关设计目标的质量。