基于GPS的路線測量與擬合

  前言

　　GPS（全球定位系統(tǒng)）由24顆衛(wèi)星支持，具備全球性，全天候，連續(xù)的導(dǎo)航與定位。由于其高精度，高速度，低成本，使用方便的特點(diǎn)，不僅在軍事上獲得廣泛的關(guān)注，在民用部門的應(yīng)用也越來越多。

　　目前，國內(nèi)對(duì)GPS的應(yīng)用主要集中在車輛信息服務(wù)系統(tǒng)和鐵路、公路施工勘測上。對(duì)鐵、公路路線的勘測可分兩種情況，一是建設(shè)路線的需要，利用 GPS進(jìn)行初測導(dǎo)線點(diǎn)與水準(zhǔn)點(diǎn)的定位，一是對(duì)現(xiàn)有路線通過GPS采集路線數(shù)據(jù)，還原出實(shí)際路線圖。對(duì)后一種情況，由于路線采集點(diǎn)密度和測量誤差的因素，在實(shí)際應(yīng)用中，需要利用所得數(shù)據(jù)進(jìn)行一定的擬合。

　　2  課題背景

　　在國內(nèi) ，現(xiàn)有機(jī)車頭燈都是固定的，當(dāng)機(jī)車進(jìn)入彎道時(shí)，光線并不能實(shí)時(shí)照射在前方鐵軌上。設(shè)計(jì)一種活動(dòng)頭燈，以機(jī)車路線數(shù)據(jù)為基礎(chǔ)，通過GPS獲得機(jī)車當(dāng)前位置，并實(shí)時(shí)調(diào)整頭燈轉(zhuǎn)角，可以大大提高機(jī)車行車安全。該系統(tǒng)框圖如下：

圖1 機(jī)車頭燈控制系統(tǒng)框圖

　　在車載控制器中，GPS接收機(jī)獲得的位置、速度數(shù)據(jù)與預(yù)先存儲(chǔ)的路線數(shù)據(jù)表進(jìn)行比較計(jì)算，最終得出當(dāng)前機(jī)車所在位置需要發(fā)出的控制轉(zhuǎn)角信號(hào)量。預(yù)先存儲(chǔ)的路線數(shù)據(jù)表需要在個(gè)人計(jì)算機(jī)機(jī)中提前進(jìn)行處理，即前言提到的擬合。無論是在車輛信息服務(wù)系統(tǒng)還是其他與地理路線相關(guān)的應(yīng)用系統(tǒng)中，路線數(shù)據(jù)的采集與擬合都是極為重要的環(huán)節(jié)。

　　3  GPS數(shù)據(jù)的采集

　　GPS接收機(jī)接收到的數(shù)據(jù)是按一定報(bào)文格式的數(shù)據(jù)流以串口方式輸出的。其格式為：

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

　　以逗號(hào)為分隔符，各數(shù)據(jù)項(xiàng)依次表示新數(shù)據(jù)幀的開始標(biāo)志、格林威治標(biāo)準(zhǔn)時(shí)間、數(shù)據(jù)有效標(biāo)志、緯度 、南北緯標(biāo)志 、精度 、東西經(jīng)標(biāo)志、移動(dòng)速度、日期 、磁變、東西磁變標(biāo)志與校驗(yàn)和。對(duì)于路線的擬合，實(shí)際需要的只是每個(gè)點(diǎn)的經(jīng)緯度，為此需要進(jìn)行提取處理，具體可通過便攜計(jì)算機(jī)隨車采集，計(jì)算機(jī)與GPS接收機(jī)之間通過串口通信， 通信控制可利用微軟公司的MSCcomm串行通信控件進(jìn)行簡單靈活的編程，另外也可以直接調(diào)用Windows API函數(shù)或動(dòng)態(tài)鏈接庫進(jìn)行更底層豐富的設(shè)計(jì)。這些方法有很多文章進(jìn)行專門的論述，本文就該系統(tǒng)實(shí)際采用的PC104微型主板通過串口通信的方法做個(gè)簡單介紹。PC104微型主板體積小，加上GPS接收機(jī)仍然非常小巧，便于攜帶，采集路線數(shù)據(jù)時(shí)置于機(jī)車上，采集的數(shù)據(jù)存儲(chǔ)在自身FLASH上，采集完畢可以通過IDE接口拷貝到個(gè)人計(jì)算機(jī)硬盤上。PC104裝載的是DOS6.0系統(tǒng)， 對(duì)串口操作分軟中斷和硬中斷兩種方式，硬中斷相對(duì)效率要高些。采用C語言在DOS下進(jìn)行硬中斷方式的串口通信編程步驟及注意事項(xiàng)如下：

　　1．  串行通信以通用異步發(fā)送/接收器8250進(jìn)行控制，8250有10個(gè)可編程的單字節(jié)寄存器，占用7個(gè)端口地址，復(fù)用地址通過讀/寫操作和線路控制寄存器的第7位來區(qū)分。COM1、COM2對(duì)應(yīng)的7個(gè)端口地址分別為3F8H~3FEH、2F8H~2FEH。初始化串口主要是寫波特率因子寄存器以設(shè)定通信速率，其次是讀取接收寄存器和中斷標(biāo)志寄存器以清除現(xiàn)有的接收或發(fā)送中斷標(biāo)志。

　　2．  硬中斷通道IRQ4（COM1）和IRQ3（COM2）分別對(duì)應(yīng)中斷向量0BH和0CH，在載入新的中斷服務(wù)程序之前必須獲取并保存原中斷服務(wù)程序的入口地址，相應(yīng)函數(shù)為getvect()和setvect()。

　　3．  中斷控制器8259有兩個(gè)口線用作COM1和COM2硬中斷通道，可通過設(shè)置其中斷屏蔽寄存器位（bit4對(duì)應(yīng)IRQ4，bit3對(duì)應(yīng)IRQ3）來開啟或屏蔽，中斷屏蔽寄存器的端口地址為21H。每次中斷服務(wù)程序返回必須往中斷命令寄存器(地址20H)寫入20H以使8259清除相關(guān)寄存器位。

　　4．  在中斷服務(wù)程序中，通過讀取中斷標(biāo)志寄存器判別中斷類型。按優(yōu)先級(jí)高低依次為：接收出錯(cuò)中斷、接收到數(shù)據(jù)中斷、發(fā)送寄存器空中斷、MODEM狀態(tài)寄存器改變中斷。響應(yīng)中斷后，8250自動(dòng)復(fù)位中斷標(biāo)志。

　　5．  采用硬中斷方式的串口通信必須設(shè)置合適的接收和發(fā)送緩沖區(qū)，緩沖區(qū)的讀取和寫入可以通過緩沖區(qū)首尾索引變量來操作。

　　4   三次樣條插值的原理

　　隨車采集的路線數(shù)據(jù)是一個(gè)個(gè)離散的點(diǎn)，對(duì)這些點(diǎn)數(shù)據(jù)進(jìn)行平滑過渡，形成一條平滑曲線便得到機(jī)車行車路線。一般一條機(jī)車路線的數(shù)據(jù)采集點(diǎn)是有限的，對(duì)相鄰點(diǎn)進(jìn)行簡單連線過渡是比較粗糙的。在工程上，通常采取插值的方法來構(gòu)造曲線，插值也叫擬合。插值有線性插值，立方插值和三次樣條插值等方法。線性插值應(yīng)用比較廣泛，但它只適用于離散點(diǎn)變化緩和，波動(dòng)不大的情況，對(duì)于波動(dòng)較大的情況，三次樣條函數(shù)具有優(yōu)良的性能。目前GPS在民用定位上還有5米左右的誤差，考慮到該因素以及采集點(diǎn)相對(duì)稀疏，采用三次樣條插值是比較合適的選擇。

       樣條也就是彈性細(xì)木條或金屬條，早期工程技術(shù)人員利用樣條的彈性彎曲使之通過一系列離散點(diǎn)來得到平滑曲線。在數(shù)學(xué)上，利用樣條函數(shù)進(jìn)行插值的方法即為樣條插值。

　　假設(shè)在區(qū)間[a ，b]上，存在n+1個(gè)點(diǎn)滿足：a=x0<x1<x2<x3<…<xn-1<xn=b，且存在函數(shù)f（x），如果此時(shí)另有一函數(shù)S(x)符合如下條件：

　　1.   S(x)在區(qū)間[xi-1，xi]（i=1,2,…,n）上均為三次多項(xiàng)式

　　2.    S(x)通過上述所有節(jié)點(diǎn)，即S(xi)=f(xi)=yi(i=0,1,…,n)

　　3.    S(x)存在一階二階導(dǎo)數(shù)在區(qū)間[a，b]上連續(xù)

　　則S(x)即稱為三次樣條函數(shù)，三次樣條函數(shù)以多項(xiàng)式的方式表示如下：

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

　　根據(jù)條件2和3，可以得出以下兩個(gè)方程組：

　　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)

　　這兩個(gè)方程組共有4n個(gè)未知系數(shù)，確定的方程只有4n-2個(gè)，必須附加兩個(gè)條件才能求解，為此可根據(jù)三次自然樣條插值法,增加兩個(gè)自然邊界條件：

　　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)

　　對(duì)上式進(jì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)

　　根據(jù)條件2，S(x)=yi，可以確定c1，c2，最終得到S(x)表達(dá)式：

　　       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)

　　根據(jù)條件3，對(duì)上式求左右導(dǎo)數(shù)并令其相等，可得到n-1個(gè)方程：

　　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)

　　再結(jié)合考慮兩端邊界條件：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)即為相臨兩點(diǎn)間的三次樣條函數(shù)，n段三次樣條曲線以已知數(shù)據(jù)點(diǎn)為臨界銜接點(diǎn)進(jìn)行連接便可得出一條通過所有數(shù)據(jù)點(diǎn)的平滑曲線。

       5   MATLAB實(shí)現(xiàn)樣條插值

　　在本系統(tǒng)中，路線數(shù)據(jù)的采集點(diǎn)以經(jīng)緯度描述，整條路線類似于二維曲線，以實(shí)驗(yàn)測試路段為例，其采集點(diǎn)如下：

　　表一  路線采集點(diǎn)數(shù)據(jù)

       三次樣條插值過程實(shí)質(zhì)上就是求解三次樣條函數(shù)的過程，在實(shí)際應(yīng)用中，首先必須根據(jù)具體樣本點(diǎn)情況構(gòu)造出合適的三次樣條函數(shù)，但這需要工程技術(shù)人員具備良好的數(shù)學(xué)功底和數(shù)據(jù)分析能力。此外，對(duì)于離散樣本點(diǎn)較多的情況，編制計(jì)算機(jī)程序求解大方程組也不簡單。

　　MATLAB將高性能的數(shù)值計(jì)算和可視化集成在一起，并提供了大量專業(yè)領(lǐng)域的內(nèi)置函數(shù)，從而被廣泛地應(yīng)用于科學(xué)計(jì)算、控制系統(tǒng)、信息處理等領(lǐng)域的分析、仿真和設(shè)計(jì)工作。就數(shù)值插值而言，MATLAB提供有一維、二維、三維插值方法，對(duì)于一維插值可通過函數(shù)interp1(x,y,xi,'method')實(shí)現(xiàn)，其中x,y是已知數(shù)據(jù)點(diǎn)的值，xi是要內(nèi)插的數(shù)據(jù)點(diǎn),method是內(nèi)插方法，可以指定為一次（linear）、三 次(cubic)方程式或spline函數(shù)，其預(yù)設(shè)方法是linear。如果數(shù)據(jù)的變化較大，以 spline函數(shù)內(nèi)插所形成的曲線最平滑 ，所以效果最好。而三次方程式所得到的內(nèi)插曲線平滑度，則介于線性與spline函數(shù)之間。

　　對(duì)表一數(shù)據(jù)進(jìn)行插值，步驟如下：

　　1.   分析經(jīng)緯兩組數(shù)據(jù)，以相對(duì)單調(diào)性較好的一組為自變量確定插值精度。表一數(shù)據(jù)比較簡單，可以經(jīng)度為自變量，0.1’為步長確定需要內(nèi)插數(shù)據(jù)點(diǎn)矩陣（GPS接收機(jī)輸出的經(jīng)緯度數(shù)據(jù)由度數(shù)和分?jǐn)?shù)組合在一起，如10352.0329表示103度52.0329分，在應(yīng)用中需要統(tǒng)一轉(zhuǎn)換成以分為單位。本文的實(shí)驗(yàn)測試路線較短，不涉及度數(shù)部分的變化，為了直觀，暫不做轉(zhuǎn)換）。

　　2.    將已知數(shù)據(jù)點(diǎn)列成１２*２矩陣形式。

　　3.    調(diào)用interp1(x,y,xi,'method')函數(shù)，選擇內(nèi)插方法為spline,所得結(jié)果與內(nèi)插數(shù)據(jù)點(diǎn)構(gòu)成2*27矩陣，該矩陣每列上的兩個(gè)數(shù)據(jù)即為擬合路線上位置點(diǎn)經(jīng)緯度。

　　對(duì)應(yīng)程序如下：

　　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]

　　 由于步長足夠小，產(chǎn)生的數(shù)據(jù)相對(duì)原數(shù)據(jù)豐富的多，擬合出的路線曲線平滑，這可通過函數(shù)plot()進(jìn)行效果顯示。在具體路線的擬合過程中，可根據(jù)ＧＰＳ數(shù)據(jù)采集點(diǎn)的密度改變插值步長，以達(dá)到實(shí)際控制要求。

　　6   小結(jié)

　?。牵校釉趹?yīng)用過程中的工作主要集中在定位數(shù)據(jù)的采集和后續(xù)處理，特別是在與ＧＩＳ（地理信息系統(tǒng)）的結(jié)合應(yīng)用中，為了描繪實(shí)際地理形狀或路線，必須采取一定的算法盡量見減小結(jié)果誤差。三次樣條插值以其優(yōu)良的數(shù)學(xué)特征正好滿足這方面的需求，結(jié)合功能強(qiáng)大的ＭＡＴＬＡＢ工具，其實(shí)現(xiàn)簡單方便，大大提高了相關(guān)設(shè)計(jì)目標(biāo)的質(zhì)量。