[發明專利]一種車輛組合定位導航系統及方法

無效

• 申請號: 201110215437.3
• 申請日: 2011-07-29
• 公開（公告）號: CN102410837A
• 公開（公告）日: 2012-04-11
• 發明（設計）人: 陸文昌;陳龍;汪若塵;袁朝春;張迎
• 申請（專利權）人: 江蘇大學
• 主分類號: G01C21/12
• 分類號: G01C21/12;G01S19/42
• 代理公司: 南京知識律師事務所 32207
• 代理人: 汪旭東

1.一種車輛組合定位導航系統，其特征在于，包括角速率陀螺儀(1)、GPS(5)、微處理器(6)、數字處理器(8)、低通濾波器(2)、A/D轉換器(3)、里程表(4)、電平轉換(7)、CAN控制器(9)、發送接收器(10)和顯示器(11)；所述GPS(5)與微處理器(6)連接后重構成數字傳感器A（22）；角速度陀螺儀（1）的輸出與低通濾波器（2）的輸入相連，低通濾波器（2）的輸出與A/D轉換器(3)的輸入相連，A/D轉換器(3)的輸出與微處理器(6)連接，?A/D轉換器(3)將轉換后的數據以串行方式輸入到微處理器(6)；里程表(4)的輸出與微處理器(6)連接；將角速度陀螺儀（1）、低通濾波器（2）、A/D轉換器(3)和里程表(4)重構成數字傳感器B（21）；微處理器(6)通過電平轉換(7)與數字處理器(8)連接，將微處理器(6)、電平轉換(7)和數字處理器(8)重構成導航計算機（28）；在導航計算機（28）中，設置航位推算器（23）、濾波器A（24）、濾波器B（25）和全局濾波器（26）；將數字傳感器B（21）和航位推算器（23）重構成智能航位推算傳感器（27）。

2.一種利用權利要求1所述的車輛組合定位導航系統的導航方法，其特征在于，將兩個數字傳感器信號用濾波器進行濾波處理，所得狀態估計值和估計誤差方差作為全局濾波器的輸入信號，全局濾波器融合兩個數字傳感器對應的濾波器估計信息，在得到全局最優估計值的同時，把相關信息分別反饋給兩個濾波器，提高定位精度和完整性。

3.根據權利要求2所述的車輛組合定位導航方法，其特征在于，具體步驟為：

???第一步，選???????????????????????????????????????????????、、分別為車輛在東向的位置分量、速度分量和加速度分量的狀態變量，、、分別為車輛在北向的位置分量、速度分量和加速度分量的狀態變量，選、為車輛在東向和北向的誤差，將GPS和DR系統定位時的誤差源等效為一階馬爾柯夫過程，取為組合定位系統的狀態向量；

????第二步，取組合定位系統的連續狀態方程為

?????????????=

??????式中，

，，

式中：、分別為、的高斯白噪聲；、分別為車輛東向和北向機動加速度變化率的相關時間常數；、分別對應馬爾柯夫的相關時間常數；、分別為車輛東向和北向機動加速度分量的當前均值；

????第三步，取T為采樣周期，車輛的連續運動模型為機動目標的“當前”統計模型的離散化模型為

??????????????????????????????????（1）

式中，

??????????????????（2）

??????取，，則，，分別為

?????，

?????，

?????????????????????????（3）

式中

，

，

???????，，==0

?????第四步，取數字傳感器A(22)、濾波器A(24)?子系統的狀態變量，狀態方程與總體狀態方程相同，選子系統的東向位置信息和北向的位置信息(單位均化為m)為觀測量，，離散化觀測方程為：

??????????????????????????????????????????????????（4）

?????式中，

????????????????????????????????????????????????（5）

?

?????和是GPS接收機位置觀測噪聲，近似為、的高斯白噪聲，量測噪聲協方差矩陣為：

?????????

第五步，取智能航位推算傳感器(27)、濾波器B(25)?子系統的狀態變量，狀態方程與總體狀態方程相同，選角速率陀螺的輸出以及里程計在一個采樣周期內輸出的距離作為觀測量，里程計的刻度系數取為K=1；

取，得連續的觀測方程為：

????

為陀螺的漂移，近似為的高斯白噪聲；為里程計的觀測噪聲，近似為的高斯白噪聲；

觀測噪聲協方差矩陣為：

將觀測方程離散化，得到系統離散的觀測方程為

??????

?式中

????????

????????，

????將上述非線性的觀測方程采用擴展Kalman濾波線性化，對在預測值處泰勒級數展開，并忽略二階及以上項，得

????

????化簡得DR系統線性離散的觀測方程

??????????????????????????????????（6）

???其中，

????????????????????????????????（7）

????

第六步，由前面式(2)和式(3)，得系統狀態轉移矩陣、系統控制向量矩陣為：

?????

?????

由前面式(5)、(7)?得系統的觀測矩陣和為：

????

????

根據系統噪聲的統計特性，得；

根據系統噪聲的統計特性，求出如下：

，

，

其中：

??

??

??

??

??

??

式中：和都是對稱矩陣，中的元素表達式和中的元素表達式相似，將中的各元素表達式中的用來代替，即可相應地得到中的元素表達式；

第七步，根據所建立的濾波器A的狀態方程(1)和觀測方程（4）得GPS濾波器A的離散模型為：

??

??

根據所建立的濾波器B的狀態方程(1)和觀測方程（6），得DR濾波器B的離散模型為：

根據擴展Kalman濾波遞推方程，GPS濾波器A的離散模型和DR濾波器B的離散模型的濾波方程如下：

??(1)、系統狀態一步預測估計方程:

?????

??(2)、系統狀態估計值方程：

?????

??(3)、濾波增益方程：

?????

??(4)、一步預測估計誤差方差方程：

?????

??(5)、濾波誤差方差方程：

????

式中，；

其中，=

???輸出濾波器A的狀態估計值、估計誤差方差、噪聲均方值和濾波器B的狀態估計值、估計誤差方差、噪聲均方值；

第七步，按下列方法對濾波器A和濾波器B的估計信息進行融合，得到全局最優估計值，全局估計誤差方差、全局噪聲均方值和全局狀態估計值：

????????????

????????????

????????????

????第八步，全局濾波器將信息反饋給濾波器A和濾波器B

?????????????

?????????????

?????????????

?????????????

??????????????

??????????????

???????????????

在GPS能正常工作、定位精度較高時，取==0.5；當有遮擋等原因GPS定位系統不能正常定位或定位精度較差時，取=0，=1。