專利名稱：定位裝置及導(dǎo)航系統(tǒng)的制作方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種用于檢測(cè)移動(dòng)物體位置的定位裝置及導(dǎo)航系統(tǒng)， 尤其涉及一種通過(guò)在無(wú)線電導(dǎo)航獲得的定位位置上累積自主傳感器測(cè) 得的信息，同時(shí)對(duì)地圖數(shù)據(jù)進(jìn)行規(guī)范從而估計(jì)位置的定位裝置及導(dǎo)航 系統(tǒng)。
背景技術(shù)：
在導(dǎo)航系統(tǒng)中，依據(jù)從GPS (Global Positioning System)衛(wèi)星 發(fā)出的無(wú)線電波檢測(cè)出車輛的位置，用車速傳感器及陀螺傳感器等累 積行駛距離及行駛方向，高精度地估計(jì)車輛的當(dāng)前位置。
但是，在無(wú)法收到GPS衛(wèi)星發(fā)出的無(wú)線電波的狀態(tài)下，包含在自 主導(dǎo)航定位之中的誤差將隨時(shí)間的推移而被放大，因而位置的精度會(huì) 逐漸下降。因此，提出了各種方法來(lái)對(duì)通過(guò)自主導(dǎo)^i全測(cè)出的車輛位 置進(jìn)行補(bǔ)正。
例如，有利用導(dǎo)航系統(tǒng)的地圖數(shù)據(jù)，對(duì)通過(guò)自主導(dǎo)航所定位的位 置進(jìn)行補(bǔ)正的地圖匹配方法。由于地圖數(shù)據(jù)是將節(jié)點(diǎn)(例如，交叉路 口 )間用連接線(例如，道路)進(jìn)行結(jié)合而成，因此通過(guò)將自主導(dǎo)航 所檢測(cè)的位置與地圖數(shù)據(jù)相對(duì)應(yīng)，從而對(duì)車輛的位置進(jìn)行補(bǔ)正以使車 輛能夠在道路上行駛。
但是，地圖數(shù)據(jù)的精度并不太高，通過(guò)地圖匹配進(jìn)行的補(bǔ)正通常 不能獲得足夠的精度。因此，現(xiàn)公開(kāi)的技術(shù)有，例如從地圖數(shù)據(jù)中選
擇與通過(guò)自主導(dǎo)航所定位的位置、方向最為符合的位置及方位的道路， 并根據(jù)該道路而對(duì)位置、方向進(jìn)行補(bǔ)正的方法。(例如，參照專利文獻(xiàn)
專利文獻(xiàn)l:特開(kāi)2002-213979號(hào)公報(bào)

發(fā)明內(nèi)容
但是，如上文所述， 一般市售的導(dǎo)航系統(tǒng)的地圖數(shù)據(jù)精度不是很 高，此外，由于將道路網(wǎng)用連接交叉路口 (節(jié)點(diǎn))的直線狀的連接線 來(lái)表現(xiàn)，因此有時(shí)會(huì)與實(shí)際道路的形狀不一致。
圖1 (a)表示了地圖數(shù)據(jù)的連接線信息與實(shí)際道路形狀偏離的一 個(gè)示例。如圖1 (a)中所示，實(shí)際的道路有緩慢的彎曲，但是在地圖 數(shù)據(jù)中，將其用兩條連接線來(lái)表現(xiàn)，因此出現(xiàn)了實(shí)際道路與連接線信 息不相符的位置。例如，對(duì)于"車輛的實(shí)際位置"，當(dāng)車輛正在行駛， 并且能夠捕捉到GPS衛(wèi)星信號(hào)的狀態(tài)下，可將"車輛的實(shí)際位置"作 為車輛的位置進(jìn)行識(shí)別；但是在不能捕捉到GPS衛(wèi)星信號(hào)的狀態(tài)下用 直線狀的連接線進(jìn)行了地圖匹配時(shí)，車輛的實(shí)際位置與定位的位置之 間就會(huì)有誤差L。
圖1 (b)表示了通過(guò)測(cè)量對(duì)誤差L的程度進(jìn)行了調(diào)查的地圖數(shù)據(jù) 精度的一個(gè)示例。在圖1 (b)中，橫軸表示測(cè)量點(diǎn)(定位的識(shí)別序號(hào))， 縱軸表示實(shí)際的道路形狀與地圖數(shù)據(jù)間的比較差值。如圖1 (b)所示， 地圖數(shù)據(jù)在交叉路口 (節(jié)點(diǎn))附近與實(shí)際道路間的誤差較小，根據(jù)地 點(diǎn)不同，有時(shí)與實(shí)際道路有6m左右的差距。因此，在不能捕捉到GPS 衛(wèi)星信號(hào)的狀態(tài)下，對(duì)地圖數(shù)據(jù)進(jìn)行地圖匹配從而對(duì)位置進(jìn)行補(bǔ)正時(shí)， 存在高精度定位比較困難的問(wèn)題。
另一方面，為了獲得高精度的地圖數(shù)據(jù)，需要道路的正確測(cè)量及 連接線的高精細(xì)化，因而4艮難對(duì)所有道路實(shí)現(xiàn)上述要求。
因此，本發(fā)明是鑒于上述問(wèn)題點(diǎn)而進(jìn)行的。鑒于上述課題，其目 的在于，提供一種即使在GPS衛(wèi)星信號(hào)捕捉困難的狀態(tài)下，也能夠使 用現(xiàn)有的地圖數(shù)據(jù)實(shí)現(xiàn)高精度定位的定位裝置、導(dǎo)航系統(tǒng)。
根據(jù)本發(fā)明的一種觀點(diǎn)，能夠提供一種定位裝置，其通過(guò)無(wú)線電
導(dǎo)航定位單元檢測(cè)移動(dòng)物體的位置，其特征在于，具有第1自主傳 感器，用于檢測(cè)所述移動(dòng)物體的動(dòng)作信息；位置檢測(cè)單元，用于將第1 自主傳感器檢測(cè)的檢測(cè)信息累積至所述無(wú)線電導(dǎo)航定位單元檢測(cè)的定 位結(jié)果，檢測(cè)所述移動(dòng)物體的估計(jì)位置；地圖數(shù)據(jù)定位單元，用于根 據(jù)所述位置檢測(cè)單元;^測(cè)的所述估計(jì)位置并參考地圖數(shù)據(jù)存儲(chǔ)單元， 檢測(cè)從對(duì)應(yīng)于所述估計(jì)位置的連接線開(kāi)始的預(yù)定距離的地圖數(shù)據(jù)估計(jì) 位置；最優(yōu)位置估計(jì)單元，用于將所述估計(jì)位置及所述地圖數(shù)據(jù)估計(jì) 位置應(yīng)用于卡爾曼濾波器，估計(jì)所述移動(dòng)物體的所述位置。
根據(jù)本發(fā)明的其他觀點(diǎn)，能夠提供一種使用了下述定位裝置的導(dǎo) 航系統(tǒng)。 一種定位裝置，其通過(guò)無(wú)線電導(dǎo)航定位單元檢測(cè)移動(dòng)物體的 位置，其特征在于，具有第1自主傳感器，用于檢測(cè)所述移動(dòng)物體 的動(dòng)作信息；位置檢測(cè)單元，用于將第1自主傳感器檢測(cè)的檢測(cè)信息 累積至所述無(wú)線電導(dǎo)航定位單元檢測(cè)的定位結(jié)果，檢測(cè)所述移動(dòng)物體 的估計(jì)位置；地圖數(shù)據(jù)定位單元，用于根據(jù)所述位置檢測(cè)單元檢測(cè)的 所述估計(jì)位置并參考地圖數(shù)據(jù)存儲(chǔ)單元，檢測(cè)從對(duì)應(yīng)于所述估計(jì)位置 的連接線開(kāi)始的預(yù)定距離的地圖數(shù)據(jù)估計(jì)位置；最優(yōu)位置估計(jì)單元， 用于將所述估計(jì)位置及所述地圖數(shù)據(jù)估計(jì)位置應(yīng)用于卡爾曼濾波器， 估計(jì)所述移動(dòng)物體的所述位置。通過(guò)本發(fā)明，能夠提供一種即使在GPS衛(wèi)星信號(hào)捕捉困難的狀態(tài) 下，也能夠使用現(xiàn)有的地圖數(shù)據(jù)實(shí)現(xiàn)高精度定位的定位裝置及導(dǎo)航系統(tǒng)。


圖1為表示地圖數(shù)據(jù)的連接線信息與實(shí)際道路形狀之間的偏離的 一個(gè)示例的示意圖。
圖2為使用了定位裝置的導(dǎo)航系統(tǒng)的概要構(gòu)成圖。
圖3為，定位裝置的功能框圖及卡爾曼濾波器的耦合圖像的示意圖。
圖4為，為了與卡爾曼濾波器耦合，通過(guò)兩個(gè)傳感器系統(tǒng)檢測(cè)出 的各個(gè)位置的示意圖(其1)。
圖5為，為了與卡爾曼濾波器耦合，通過(guò)兩個(gè)傳感器系統(tǒng)檢測(cè)出 的各個(gè)位置的示意圖(其2)。
圖6為，用于說(shuō)明卡爾曼濾波器中使用的觀測(cè)方程式的示意圖。
圖7為，定位裝置依據(jù)GPS無(wú)線電波的捕捉狀況對(duì)車輛位置進(jìn)行 定位的順序的流程圖。
圖8為，通過(guò)定位裝置進(jìn)行實(shí)際定位獲得的實(shí)驗(yàn)結(jié)果的示意圖。
符號(hào)說(shuō)明
1, GPS4妻收裝置 2， 車速傳感器3, 橫擺率傳感器
4, 轉(zhuǎn)向角傳感器 5， 地圖數(shù)據(jù)庫(kù)
6, 輸入裝置
7, 顯示裝置 8， 導(dǎo)航ECU 9， 定位裝置 10, 導(dǎo)航系統(tǒng)
具體實(shí)施例方式
以下參照?qǐng)D紙對(duì)本發(fā)明的最優(yōu)實(shí)施形態(tài)進(jìn)行說(shuō)明。
圖2中圖示了使用了本實(shí)施形態(tài)中的定位裝置9的導(dǎo)航系統(tǒng)10的 概要構(gòu)成圖。
導(dǎo)4元系統(tǒng)10受控制導(dǎo)4元系統(tǒng)的導(dǎo)4元ECU (Electrical Control Unit) 8控制。導(dǎo)航ECU8的構(gòu)成為，將運(yùn)行程序的CPU、存儲(chǔ)程序的 存儲(chǔ)裝置(硬盤(pán)、R0M)、臨時(shí)存儲(chǔ)數(shù)據(jù)及程序的RAM、數(shù)據(jù)輸入及輸出 的輸入輸出裝置、NV (Non Volatile:非易失)-RAM等通過(guò)總線連接 在一起的計(jì)算機(jī)。
導(dǎo)航ECU8中連接有下列部件接收來(lái)自GPS (Global Positioning System)衛(wèi)星的無(wú)線電波的GPS接收裝置1;檢測(cè)車輛速度的車速傳感 器2;對(duì)圍繞經(jīng)過(guò)車的重心并沿車輛前后方向的軸線(側(cè)傾軸)的轉(zhuǎn)動(dòng) 角速度進(jìn)行檢測(cè)的橫擺率傳感器3;檢測(cè)方向盤(pán)轉(zhuǎn)向角的轉(zhuǎn)向角傳感器 4;存儲(chǔ)了地圖數(shù)據(jù)的地圖數(shù)據(jù)庫(kù)(以下簡(jiǎn)稱地圖DB) 5;用于操作導(dǎo) 航系統(tǒng)10的輸入裝置6;顯示地圖中的當(dāng)前位置的液晶或HUD( Head Up Display:抬頭顯示器)等顯示裝置7。
地圖DB5中，將實(shí)際的道^^網(wǎng)與節(jié)點(diǎn)(例如，道^各與道^各的交叉 點(diǎn)、從交叉路口每按規(guī)定間隔分段的點(diǎn)等)以及連接線(連接節(jié)點(diǎn)與 節(jié)點(diǎn)的道路)相對(duì)應(yīng)，構(gòu)成圖表狀的數(shù)據(jù)庫(kù)。
導(dǎo)航ECU8，將檢測(cè)出的當(dāng)前位置周邊的地圖從地圖DB5中提取， 并按指定的縮小比例顯示在設(shè)置于車廂內(nèi)的顯示裝置7上。導(dǎo)航ECU8， 根據(jù)需要在地圖中重疊顯示車輛的當(dāng)前位置。
此外，從按鍵式的鍵盤(pán)及遙控器等輸入裝置6輸入目的地時(shí)，從 檢測(cè)出的當(dāng)前位置到目的地，例如用迪杰斯特拉算法(dijkstra)等 已知的路線檢索法進(jìn)行路線檢索，將路線在地圖中重疊顯示的同時(shí)， 在左右轉(zhuǎn)向的交叉路口前向駕駛員提示路線。
導(dǎo)航ECU5的CPU,通過(guò)運(yùn)行存儲(chǔ)于存儲(chǔ)裝置中的程序，實(shí)現(xiàn)本實(shí) 施形態(tài)中所說(shuō)明的定位。在圖3 (a)中圖示了定位裝置9的功能框圖。
本實(shí)施形態(tài)中的定位裝置9,能在GPS衛(wèi)星信號(hào)捕捉困難及GPS定 位的可靠性降低的狀況下，對(duì)車輛位置進(jìn)行高精度定位。下面對(duì)其概 要進(jìn)行說(shuō)明，定位裝置9，在通過(guò)自主導(dǎo)航進(jìn)行的定位中，附加基于地
圖DB5的定位信息，然后將通過(guò)卡爾曼濾波器進(jìn)行了最優(yōu)估計(jì)的位置 輸出。與在自主導(dǎo)航獲得的定位結(jié)果上進(jìn)行地圖匹配的情況相比，能 夠提高定位精度。通過(guò)后文詳述的實(shí)際定位結(jié)果可以看出，本實(shí)施形 態(tài)的位置補(bǔ)正，特別是對(duì)于與行進(jìn)方向(道路長(zhǎng)度方向)垂直的方向 上的位置偏離的補(bǔ)正，具有4艮好的效果。
此外，對(duì)于卡爾曼濾波器的應(yīng)用，需要使耦合的定位信息相互獨(dú) 立，因此，在本實(shí)施形態(tài)中，
A. 車速傳感器2及轉(zhuǎn)向角傳感器4
B. 地圖DB5及^f黃擺率傳感器3
劃分為上述兩個(gè)傳感器系統(tǒng)。另外，也可以在A傳感器系統(tǒng)使用橫擺 率傳感器3、在B傳感器系統(tǒng)使用轉(zhuǎn)向角傳感器4，還可以在檢測(cè)車輛 方向的傳感器中使用陀螺傳感器。
此外，應(yīng)用卡爾曼濾波器時(shí)，將由兩個(gè)傳感器系統(tǒng)獲得的輸出結(jié) 果通過(guò)觀測(cè)方程式進(jìn)行耦合，但是相對(duì)于在A傳感器系統(tǒng)中檢測(cè)出位 置及方向，在B傳感器系統(tǒng)中檢測(cè)出的是與地圖DB5中地圖數(shù)據(jù)的相 對(duì)距離及相對(duì)方向，二者不能直接耦合。因此，本實(shí)施形態(tài)的定位裝 置9，導(dǎo)出了用于將從兩個(gè)傳感器系統(tǒng)獲得的檢測(cè)值進(jìn)行耦合的觀測(cè)方 程式。以下進(jìn)行詳細(xì)說(shuō)明。
首先，GPS定位單元81是用已知的方法、依據(jù)來(lái)自GPS衛(wèi)星的無(wú) 線電波對(duì)車輛的位置進(jìn)行定位。GPS定位單元81，從圍繞規(guī)定軌道轉(zhuǎn)
動(dòng)的多個(gè)GPS衛(wèi)星中，選擇4顆進(jìn)入當(dāng)前車輛位置的規(guī)定仰角內(nèi)的GPS 衛(wèi)星，并接收從這些GPS衛(wèi)星發(fā)出的無(wú)線電波。GPS定位單元81計(jì)算 電波的到達(dá)時(shí)間，由到達(dá)時(shí)間與光速c算出距捕捉到的GPS衛(wèi)星的距 離。并且，將3個(gè)GPS衛(wèi)星與車輛的距離交叉的一點(diǎn)作為車輛的位置 進(jìn)行定位。
定位裝置9在接收GPS無(wú)線電波過(guò)程中，每隔規(guī)定時(shí)間對(duì)車輛位 置進(jìn)行定位。并且，如果GPS無(wú)線電波被屏蔽，則進(jìn)入將最后定位的 位置作為初始位置，將當(dāng)時(shí)的行駛方向作為初始方向，將這些行駛距 離及方向進(jìn)行累積的自主導(dǎo)航定位。
圖4 (a)為通過(guò)自主導(dǎo)航(A傳感器系統(tǒng))檢測(cè)出的位置的示意 圖。圖4(a)中的道路21,是用從地圖DB5中提取的兩個(gè)連接線22、 23構(gòu)成的地圖數(shù)據(jù)來(lái)表示的。設(shè)定車輛正在從連接線22向連接線23 的方向行駛，GPS無(wú)線電波在初始位置24處被屏蔽。
從GPS衛(wèi)星發(fā)出的無(wú)線電波被屏蔽時(shí)，INS ( Inertial Navigation Sensor )定位單元82，通過(guò)車速傳感器2檢測(cè)車速，通過(guò)轉(zhuǎn)向角傳感 器4檢測(cè)轉(zhuǎn)向角，然后在初始位置24及初期方向上累積行駛距離及方 向，從而通過(guò)自主導(dǎo)航對(duì)位置及方向進(jìn)行估計(jì)(以下稱為估計(jì)位置、 估計(jì)方向)。在圖4(a)中，圖示了相對(duì)于車輛的實(shí)際位置25的、通 過(guò)自主導(dǎo)航估計(jì)得到的估計(jì)位置26。
此外，為了使用卡爾曼濾波器，需要每個(gè)傳感器系統(tǒng)的誤差分散， 而車速傳感器2及轉(zhuǎn)向角傳感器4的誤差根據(jù)速度及GPS無(wú)線電波屏 蔽時(shí)間等為已知。因此，依據(jù)上述這些條件，累積得到的估計(jì)位置26
的誤差分散也為已知。在圖4 (a)中，估計(jì)位置26的誤差分散是用虛 線圍成的橢圓來(lái)表示的。
圖4 (b)為，依據(jù)INS定位單元82估計(jì)的估計(jì)位置26,使其與 地圖DB (B傳感器系統(tǒng))5的連接線對(duì)應(yīng)從而檢測(cè)出的位置的示意圖。 INS定位單元82估計(jì)的估計(jì)位置26被輸入到地圖數(shù)據(jù)定位單元83, 地圖數(shù)據(jù)定位單元83將與估計(jì)位置26對(duì)應(yīng)的連接線23從地圖DB5中 提取。
由于車輛被估計(jì)為存在于估計(jì)位置26與連接線23的周邊，因此， 地圖數(shù)據(jù)定位單元83將車輛的存在位置(以下稱為地圖數(shù)據(jù)估計(jì)位置 27 )估計(jì)為，存在于從連接線23向估計(jì)位置26的方向隔著預(yù)定距離r (以下稱為偏移量r)的線上的任意點(diǎn)上。
此偏移量r為，GPS屏蔽前的初始位置24與連接線22之間的距離。 即，估計(jì)為GPS被屏蔽前的初始位置24與連接線22的偏移持續(xù)存在。
此外，估計(jì)位置26和偏移量r都只是估計(jì)值，因此，地圖數(shù)據(jù)估 計(jì)位置27為"隔著偏移量r的線上"的任意點(diǎn)。即，地圖數(shù)據(jù)估計(jì)位 置27并不限定于一點(diǎn)。
圖5 (c)為，通過(guò)橫擺率傳感器(B傳感器系統(tǒng))的檢測(cè)值估計(jì) 的車輛方向的示意圖。通過(guò)GPS定位單元81獲得的初始方向被輸入到 方向檢測(cè)單元84,方向檢測(cè)單元84在初始方向上累積通過(guò)橫擺率傳感 器3檢測(cè)出的橫擺率的積分值(表示方向)，從而估計(jì)車輛的方向(以
下稱為累積估計(jì)方向28)。因此，與A傳感器系統(tǒng)分別進(jìn)^f亍車輛行駛方 向的估計(jì)。
此外，如上文所述，地圖數(shù)據(jù)估計(jì)位置27為本身包含誤差分散的 值，累積估計(jì)方向28也包含橫擺率傳感器3的檢測(cè)誤差等誤差。地圖 數(shù)據(jù)估計(jì)單元83依據(jù)偏移量r 、速度及GPS無(wú)線電波被屏蔽時(shí)間等， 算出地圖數(shù)據(jù)估計(jì)位置27、累積估計(jì)方向28的誤差分散。
用上文所述方法，通過(guò)A、 B傳感器系統(tǒng)分別對(duì)車輛位置及方向進(jìn) 行估計(jì)時(shí)，卡爾曼濾波器運(yùn)算單元85輸出車輛位置及方向的最優(yōu)位置。 圖5 (d)為，依據(jù)通過(guò)A傳感器系統(tǒng)獲得的估計(jì)位置26及估計(jì)方向、 和通過(guò)B傳感器系統(tǒng)獲得的地圖數(shù)據(jù)估計(jì)位置27及累積估計(jì)方向28, 運(yùn)算得到的最終估計(jì)位置29的示意圖。最終估計(jì)位置29中包含表示 方向的信息。
圖5 (d)中，將兩個(gè)傳感器系統(tǒng)的誤差分散，用估計(jì)位置26與地 圖數(shù)據(jù)估計(jì)位置27中有凸部的分散來(lái)表示，但是分散實(shí)際上有3維的 擴(kuò)散。圖3 (b)中圖示了此分散及通過(guò)卡爾曼濾波器進(jìn)行最優(yōu)位置估 計(jì)的圖像?？柭鼮V波器在一個(gè)系統(tǒng)的狀態(tài)被各自獨(dú)立的進(jìn)行估計(jì)時(shí)， 依據(jù)狀態(tài)的概率密度分布，估計(jì)概率最高的狀態(tài)(分布的積為最大的 狀態(tài))。因此，通過(guò)將兩個(gè)系統(tǒng)的定位信息用卡爾曼濾波器進(jìn)行耦合， 能夠估計(jì)出存在概率最高的位置。
接下來(lái)，對(duì)卡爾曼濾波器中使用的觀測(cè)方程式進(jìn)行說(shuō)明。
圖6為用于說(shuō)明卡爾曼濾波器中使用的觀測(cè)方程式的示意圖。如 果設(shè)定適當(dāng)?shù)淖鴺?biāo)系(例如，Y方向朝北)，并用Xt表示車輛的實(shí)際位
置、行駛方向，則X可以用2維平面位置(x， y)及行駛方向6的函 數(shù)來(lái)表示。
Xt=(x,y，0) …(1)
此外，粗實(shí)線表示的是連接線23,連接線23相對(duì)于x軸的傾斜角 設(shè)為cc。另外，連接線23的交叉路口節(jié)點(diǎn)位置設(shè)為與y軸的交點(diǎn)(0， c),這樣獲取連接線23時(shí)，連接線23通過(guò)如下面所示的車道的公式 來(lái)表示。
—x.sin + y-cosa + c = 0 ... (2)
Xt與連接線23的斜率設(shè)為4) , Xt與連接線23的距離設(shè)為r。斜 率(J)為，依據(jù)橫擺率傳感器3的檢測(cè)值，由方向檢測(cè)單元84檢測(cè)出的 值，距離r為GPS無(wú)線電波被屏蔽前的偏移量r。此外，在圖6中，a 以逆時(shí)針旋轉(zhuǎn)為正，4)以順時(shí)針旋轉(zhuǎn)為正。
因此，可以首先設(shè)立使距連接線23的距離r成為零的觀測(cè)方程式。 此外，也可以設(shè)立使相對(duì)于連接線23的行駛方向的斜率6成為零的觀 測(cè)方程式。
因此，將觀測(cè)量S設(shè)為
S= ( cx , c, r, (J))
時(shí)，觀測(cè)方程式可以用式(3)表示。此外，如式3所示的觀測(cè)方程式
為2行1列的行列式。
<formula>formula see original document page 15</formula>(3)
上側(cè)的要素表示從車道開(kāi)始的距離的關(guān)系式，下側(cè)的要素表示從 車道開(kāi)始的斜率的關(guān)系式。此外，當(dāng)車輛在行駛直線下側(cè)時(shí)，上側(cè)的 要素成為 "xsina -ycosa _c_r ，， o
關(guān)于式3的觀測(cè)方程式向卡爾曼濾波器的應(yīng)用，在后文進(jìn)行詳細(xì)
說(shuō)明,
圖7為表示定位裝置9依據(jù)GPS無(wú)線電波的捕捉狀況對(duì)車輛位置 進(jìn)行定位的順序的流程圖。
定位裝置9在GPS定位單元81的每個(gè)定位間隔，對(duì)GPS無(wú)線電波 是否被屏蔽進(jìn)行判斷(Sl )。如果GPS無(wú)線電波沒(méi)有被屏蔽(Sl為否)， 則利用GPS無(wú)線電波對(duì)車輛位置進(jìn)行定位(S2)。
當(dāng)GPS無(wú)線電波被屏蔽時(shí)(Sl為是)，INS定位單元82依據(jù)車速 及轉(zhuǎn)向角，在初始位置24及初始方向上累積行駛距離及行駛方向，從 而通過(guò)自主導(dǎo)航對(duì)估計(jì)位置及估計(jì)方向進(jìn)行估計(jì)(S3)。此外，INS定 位單元82，根據(jù)車速傳感器2及轉(zhuǎn)向角傳感器4的誤差等，算出基于 這些而累積的估計(jì)位置26以及估計(jì)方向的分散。此外，方向檢測(cè)單元84在GPS無(wú)線電波被屏蔽前的初始方向上累 積橫擺率的積分值，從而檢測(cè)出車輛的累積估計(jì)方向(S4)。此外，方 向檢測(cè)單元84依據(jù)橫擺率傳感器3的檢測(cè)誤差等，算出累積估計(jì)方向 的分散。
此外，地圖數(shù)據(jù)定位單元83依據(jù)INS定位單元82所估計(jì)的估計(jì) 位置，參照地圖DB5，在與對(duì)應(yīng)的連接線23相距偏移量r的線上的任 意位置，檢測(cè)地圖數(shù)據(jù)估計(jì)位置(S5)。
其次，卡爾曼濾波器運(yùn)算單元85，將來(lái)自A傳感器系統(tǒng)的估計(jì)位 置及估計(jì)方向、來(lái)自B傳感器系統(tǒng)的地圖數(shù)據(jù)估計(jì)位置及累積估計(jì)方 向通過(guò)卡爾曼濾波器進(jìn)行耦合(S5)。
用上述方法，能夠依據(jù)由兩個(gè)傳感器系統(tǒng)獨(dú)立檢測(cè)出的位置及方 向，輸出概率最高的最終估計(jì)位置。此外，步驟S4與S5的順序不同。
通過(guò)自主導(dǎo)航進(jìn)行的位置估計(jì)，具有能反映車輛的行駛過(guò)程的優(yōu) 點(diǎn)和隨著時(shí)間的推移誤差會(huì)增大的缺點(diǎn)；基于地圖DB的位置估計(jì)，具 有能通過(guò)地圖數(shù)據(jù)限定車輛位置的優(yōu)點(diǎn)和其含有因地圖數(shù)據(jù)的精度不 足而產(chǎn)生的誤差的缺點(diǎn)。
本實(shí)施形態(tài)的定位裝置9，考慮到由這兩個(gè)系統(tǒng)估計(jì)的位置的誤 差，用卡爾曼濾波器對(duì)最優(yōu)位置進(jìn)行估計(jì)，因而能夠?qū)﹄p方取長(zhǎng)補(bǔ)短。 因此，能夠使用地圖數(shù)據(jù)，對(duì)通過(guò)自主導(dǎo)航;險(xiǎn)測(cè)出的位置進(jìn)行高精度 的才卜正。
由于即使GPS無(wú)線電波被屏蔽也能夠進(jìn)行高精度定位，因此能夠 在適當(dāng)?shù)臅r(shí)機(jī)向駕駛員提供在交叉路口等處的駕駛支持(車輛控制、 喚起注意等)。
下面，對(duì)為了將觀測(cè)方程式(3)應(yīng)用于卡爾曼濾波器而對(duì)觀測(cè)方 程式(3)實(shí)施的操作進(jìn)行說(shuō)明。
首先，將觀測(cè)方程式(3)線形化。為此，觀測(cè)方程式(3)在時(shí) 刻t的估計(jì)位置
估計(jì)4立置&
的周圍進(jìn)行泰勒(Taylor)展開(kāi)。 [數(shù)3]
h(Xt，§t) + J'x(Xt-Xt) + J;AS = 02xl…(4)
其中，Xt為真實(shí)位置
^為觀測(cè)到的測(cè)定值 AS為測(cè)定誤差 真實(shí)觀'J定值S" + AS
此外，J'X,J;為觀測(cè)方程式的偏微分。 [數(shù)4]
<formula>formula see original document page 18</formula>
將式(4)變形可以得到下式。
<formula>formula see original document page 18</formula>... (5)
進(jìn)行標(biāo)準(zhǔn)化
<formula>formula see original document page 18</formula>... (6)
<formula>formula see original document page 18</formula>... (7)
在此，Jx，-Ws,Js分別可用下面的公式表示,
<formula>formula see original document page 18</formula>
<formula>formula see original document page 19</formula>此外，在卡爾曼濾波器中，由于狀態(tài)方程式及觀測(cè)方程式均以線 形模型為前提，因此將x,定義如下。<formula>formula see original document page 19</formula>
并且，如果用乘以J,的m維空間考慮，可以通過(guò)線形模型來(lái)表述。 由式(7)可得，
-WS=XS+JSAS... ( 8 ) 式(8)為線形模型。
由于觀測(cè)量的測(cè)定誤差A(yù)S被^假定為白色高斯分散，因此，其平均 ^f直E ( AS) =0。因此，
E(JSAS) = 0
E(JSASASTJST) = JSE(ASAST)JST=JSQSJST=Q'S... (9)
將通過(guò)自主導(dǎo)航算出的估計(jì)位置的平均分散，用乘以J,的m維空 間考慮時(shí)，可以表示如下。
<formula>formula see original document page 20</formula>…(10) 但是，假定平均E(AXt)二O
<formula>formula see original document page 20</formula> (11) 根據(jù)上述內(nèi)容，最優(yōu)估計(jì)值可用如下方法求得。 [數(shù)9]<formula>formula see original document page 20</formula>) ... (12)
但是，<formula>formula see original document page 20</formula>
為使式(12)返回X- (x， y， 6 )的空間，對(duì)其進(jìn)行逆映射。 [數(shù)IO]<formula>formula see original document page 20</formula>[通過(guò)定位裝置9獲得的測(cè)定結(jié)果]
圖8所示為，在一般道路上GPS無(wú)線電波被屏蔽的狀況下，定位
裝置9的定位結(jié)果。定位時(shí)間與GPS無(wú)線電波被屏蔽的時(shí)間基本相等， 約為950秒。
偏移量r相當(dāng)于標(biāo)準(zhǔn)偏差10[m]、觀測(cè)量S= ( a , c， r, c)))的 誤差共分散行列式Qs設(shè)定如下。此外，誤差共分散行列式Qs為a 、 c、 r、小的順序。
0 0 、
0 0 100 0 0 0週y
圖8 (a)所示為，相對(duì)于用其他系統(tǒng)進(jìn)行定位的實(shí)際車道(作為 真實(shí)值)，通過(guò)自主導(dǎo)航檢測(cè)出的估計(jì)位置及本實(shí)施形態(tài)的定位裝置9 定位而得的位置的偏離大小的試驗(yàn)結(jié)果。在圖8 (a)中，以定位時(shí)間 (行駛時(shí)間)作為橫軸，累積誤差平均的距離[m]用縱軸表示。
如圖8 (a)所示，通過(guò)自主導(dǎo)航檢測(cè)出的估計(jì)位置中，有相對(duì)于 真實(shí)值偏離30m以上的情況，而用本實(shí)施形態(tài)的定位裝置，即使行駛 了 950秒，相對(duì)于真實(shí)值也僅有10m左右的偏離。
此外，在圖8 (b)中分別圖示了 I.用其他系統(tǒng)定位的實(shí)際車道、 II.通過(guò)自主導(dǎo)航檢測(cè)出的估計(jì)位置、III.用本實(shí)施形態(tài)的定位裝置9
00
00
00
00
定位的位置。黑圓點(diǎn)為交叉路口。此外，在圖8 (b)中，I.用其他系
統(tǒng)定位的實(shí)際車道，與地圖DB5中提取的車道(連接線細(xì)實(shí)線)基 本重疊。
由于在通過(guò)自主導(dǎo)航檢測(cè)出的估計(jì)位置中，在沿連接線的車道上 有大幅度偏離的情況，因此，即使進(jìn)行地圖匹配，在匹配時(shí)也不可能 不包含誤差。但是，本實(shí)施形態(tài)的定位裝置9定位的位置，與實(shí)際車 道高精度的一致。此外，如圖8 (b)所示，本實(shí)施形態(tài)的位置補(bǔ)正， 特別是對(duì)于與行駛方向垂直的方向上的補(bǔ)正具有^f艮大效果。
以上通過(guò)實(shí)施例對(duì)本發(fā)明進(jìn)行了說(shuō)明，但是本發(fā)明并不僅限于所 述實(shí)施例，在本發(fā)明的范圍內(nèi)，進(jìn)行各種變形及改良均可。
本國(guó)際申請(qǐng)為，主張2006年7月25日提交的日本專利申請(qǐng) 2006-202475號(hào)的優(yōu)先權(quán)的申請(qǐng)，日本專利申請(qǐng)2006-202475號(hào)的全部 內(nèi)容都援引至本國(guó)際申請(qǐng)。
工業(yè)利用性
本發(fā)明能夠適用于檢測(cè)移動(dòng)物體位置的定位裝置及導(dǎo)航系統(tǒng)，特 別是，能夠適用于將通過(guò)無(wú)線電導(dǎo)航獲得的定位位置與通過(guò)自主傳感 器測(cè)得的信息進(jìn)行累積，同時(shí)基于地圖數(shù)據(jù)，從而估計(jì)位置的定位裝 置及導(dǎo)航系統(tǒng)。
權(quán)利要求
1、一種定位裝置，其通過(guò)無(wú)線電導(dǎo)航定位單元檢測(cè)移動(dòng)物體的位置，其特征在于，具有第1自主傳感器，用于檢測(cè)所述移動(dòng)物體的動(dòng)作信息；位置檢測(cè)單元，用于將第1自主傳感器檢測(cè)的檢測(cè)信息累積至所述無(wú)線電導(dǎo)航定位單元檢測(cè)的定位結(jié)果，檢測(cè)所述移動(dòng)物體的估計(jì)位置；地圖數(shù)據(jù)定位單元，用于根據(jù)所述位置檢測(cè)單元檢測(cè)的所述估計(jì)位置并參考地圖數(shù)據(jù)存儲(chǔ)單元，檢測(cè)從對(duì)應(yīng)于所述估計(jì)位置的連接線開(kāi)始的預(yù)定距離的地圖數(shù)據(jù)估計(jì)位置；最優(yōu)位置估計(jì)單元，用于將所述估計(jì)位置及所述地圖數(shù)據(jù)估計(jì)位置應(yīng)用于卡爾曼濾波器，估計(jì)所述移動(dòng)物體的所述位置。
2、 如權(quán)利要求1所述的定位裝置，其特征在于，具有第2自主傳感器，用于檢測(cè)所述移動(dòng)物體的動(dòng)作信息；方向檢測(cè)單元，用于將第2自主傳感器檢測(cè)的檢測(cè)信息累積至所 述無(wú)線電導(dǎo)航定位單元檢測(cè)的定位結(jié)果，檢測(cè)累積估計(jì)方向，所述最優(yōu)位置估計(jì)單元除了將所述估計(jì)位置以及所述地圖數(shù)據(jù)估 計(jì)位置應(yīng)用于卡爾曼濾波器外，還將所述位置檢測(cè)單元檢測(cè)的估計(jì)方 向以及所述累積估計(jì)方向也應(yīng)用于卡爾曼濾波器，估計(jì)所述移動(dòng)物體 的所述位置。
3、 如權(quán)利要求1或權(quán)利要求2所述的定位裝置，其特征在于所述第1自主傳感器為轉(zhuǎn)向角傳感器及車速傳感器，所述第2自 主傳感器為橫擺率傳感器，所述卡爾曼濾波器的觀測(cè)方程式為 [數(shù)12]<formula>formula see original document page 3</formula>":車道的斜率c:車道的截距r:從車道開(kāi)始的距離 4):從車道開(kāi)始的斜率。
4、 一種導(dǎo)航系統(tǒng)，其使用了權(quán)利要求1至權(quán)利要求3中任意一項(xiàng)所 述的定位裝置。
全文摘要
一種使用無(wú)線電導(dǎo)航定位單元(81)檢測(cè)移動(dòng)物體位置的定位裝置(9)，其特征在于，具有檢測(cè)移動(dòng)物體動(dòng)作信息的第1及第2自主傳感器(2)、(4)、(3)；將定位結(jié)果與通過(guò)第1自主傳感器測(cè)得的信息進(jìn)行累積，從而測(cè)出估計(jì)位置及估計(jì)方向的位置檢測(cè)單元(82)；依據(jù)估計(jì)位置并參考地圖數(shù)據(jù)存儲(chǔ)單元(5)，測(cè)得距對(duì)應(yīng)的連接線預(yù)定距離的地圖數(shù)據(jù)估計(jì)位置的地圖數(shù)據(jù)定位單元(83)；將定位結(jié)果與通過(guò)第2自主傳感器測(cè)得的信息進(jìn)行累積，從而測(cè)出累積估計(jì)方向的方向檢測(cè)單元(84)；將估計(jì)位置及地圖數(shù)據(jù)估計(jì)位置、與估計(jì)方向及累積估計(jì)方向適用于卡爾曼濾波器，從而估計(jì)出移動(dòng)物體位置的最優(yōu)位置估計(jì)單元(85)。
文檔編號(hào)G09B29/00GK101360971SQ200780001589
公開(kāi)日2009年2月4日 申請(qǐng)日期2007年6月8日 優(yōu)先權(quán)日2006年7月25日
發(fā)明者小堀訓(xùn)成, 香川和則 申請(qǐng)人:豐田自動(dòng)車株式會(huì)社