專利名稱：位置推測裝置及位置推測方法
技術(shù)領(lǐng)域：
本發(fā)明涉及一種汽車和電車等移動體的車載裝置，特別是涉及進(jìn)行位置推測的設(shè)備。
背景技術(shù)：
在現(xiàn)有的汽車導(dǎo)航系統(tǒng)中，根據(jù)測位結(jié)果進(jìn)行地圖匹配，從而求出本車位置。在從日本專利特開2008-287669號公報(bào)中示出的這樣的汽車導(dǎo)航系統(tǒng)向車輛運(yùn)動控制設(shè)備輸出車輛位置的車輛運(yùn)動控制系統(tǒng)中，要求更準(zhǔn)確的車輛位置的測定。為了進(jìn)行正確的測位，提出了準(zhǔn)備在本車位置的計(jì)算中使用的多個(gè)公式模型， 根據(jù)事前決定的固定閾值變更在計(jì)算中使用公式模式的技術(shù)。例如，在日本專利特開平 8-14923號公報(bào)中公開了使用全球定位系統(tǒng)GlobalPositioning System(GPS)的信號強(qiáng)度和衛(wèi)星位置等信息，如果GPS輸出的位置誤差在某一固定值以上，則切換成根據(jù)陀螺儀和加速度傳感器等相對傳感器的位置計(jì)算的裝置。在使用卡爾曼濾波器等概率系統(tǒng)推測本車位置的系統(tǒng)中，如日本專利特開平 5-333132號公報(bào)所示，提出了事前準(zhǔn)備多個(gè)與可接收的GPS衛(wèi)星的數(shù)量對應(yīng)的卡爾曼濾波器，并切換它們來測量本車位置的裝置。作為使用采用卡爾曼濾波器或粒子濾波器等概率模型的位置推測邏輯來推測本車位置的技術(shù)，在日本專利特開2007-322391號公報(bào)中公開了使用擴(kuò)展卡爾曼濾波器算法的發(fā)明。在基于測位誤差切換位置計(jì)算方法的情況下，作為搭載舵角傳感器和陀螺儀傳感器的汽車導(dǎo)航系統(tǒng)中的測位誤差模型，公知有基于速度的阿卡曼模型和推算定位模型 (dead reckoning model)。已知搭載GPS和陀螺儀傳感器的汽車導(dǎo)航系統(tǒng)中的、采用了 GPS 時(shí)的傳感器融合模型(sensorfusion model)、和非GPS接收或未采用GPS時(shí)的推算定位模型。阿卡曼模型被記載在“安部正人，「自動車O運(yùn)動i制御」，東京電機(jī)大學(xué)出版局， 2008年3月刊，ISBN:9784501417000”中。傳感器融合及內(nèi)界傳感器模型記載在“青野俊宏，他，「GPS i內(nèi)界七 > 寸 用…t起伏地(二 fe It ^移動機(jī)Q位置計(jì)測」，計(jì)測自動制御學(xué)會論文集，Vol. 35，No. 8，1999 年”中。作為在數(shù)字地圖上匹配本車位置時(shí)使用處于數(shù)字地圖信息中的線路信息時(shí)的傳感器融合模型(以后稱為線路融合模型)，已知有日本專利特開2008-26282號公報(bào)中所示的、將數(shù)字地圖數(shù)據(jù)的線路作為卡爾曼濾波器的觀測方程式來使用的公式模型。車輛的測位誤差受到所謂車輛行駛中的路面狀態(tài)、車輛的外部氣溫、車輛的各部件的經(jīng)過年限、駕駛車輛的駕駛員的特性這樣的各種狀況影響?？紤]到這些影響，事先決定用于根據(jù)測位誤差切換各種公式模型的閾值很難。即使這樣，如果事前決定不變的閾值，則會因上述各種狀況，使得模型化誤差變大，有可能給本車位置推測的計(jì)算精度造成很大的惡劣影響。

發(fā)明內(nèi)容
根據(jù)本發(fā)明的第一方式，是一種位置推測裝置，包括公式模型處理部，其從傳感器獲取移動體的姿勢信息及位置測定信息，并基于獲取到的上述姿勢信息及上述位置測定信息、和預(yù)先具有的表示上述移動體的動作的多個(gè)公式模型中的特定的公式模型，通過使用了概率模型的濾波處理，計(jì)算上述移動體的位置、表示上述移動體的移動狀態(tài)的狀態(tài)量、 和上述狀態(tài)量的誤差；閾值計(jì)算部，其使用由上述公式模型處理部計(jì)算出的上述誤差，計(jì)算用于選擇上述特定的公式模型的閾值的候補(bǔ)值；閾值決定部，其基于上述候補(bǔ)值決定上述閾值；以及公式模型選擇部，其基于由上述閾值決定部決定的上述閾值，選擇上述特定的公式模型。根據(jù)本發(fā)明的第二方式，在第一方式的位置推測裝置中，基于能夠從外部觀測的信息或由上述公式模型處理部計(jì)算出的上述位置、上述狀態(tài)量、和上述誤差，對表示上述動作的上述多個(gè)公式模型的每一個(gè)決定相對于上述動作的適用范圍的限制；上述閾值決定部基于按照上述多個(gè)公式模型的每一個(gè)計(jì)算出的上述誤差的大小，決定上述閾值；上述公式模型選擇部根據(jù)上述限制，選擇上述特定的公式模型。根據(jù)本發(fā)明的第三方式，在第二方式的位置推測裝置中，還包括存儲上述限制和上述閾值的數(shù)據(jù)庫。根據(jù)本發(fā)明的第四方式，是一種位置推測方法，包括從傳感器獲取移動體的姿勢信息及位置測定信息的步驟；基于獲取到的上述姿勢信息及上述位置測定信息、和預(yù)先具有的表示上述移動體的動作的多個(gè)公式模型中的特定的公式模型，通過使用了概率模型的濾波處理，計(jì)算上述移動體的位置、表示上述移動體的移動狀態(tài)的狀態(tài)量、和上述狀態(tài)量的誤差的步驟；使用計(jì)算出的上述誤差，計(jì)算用于選擇上述特定的公式模型的閾值的候補(bǔ)值的步驟；基于上述候補(bǔ)值決定上述閾值的步驟；以及基于決定的上述閾值選擇上述特定的公式模型的步驟。根據(jù)本發(fā)明的第五方式，在第四方式的位置方法中，基于能夠從外部觀測的信息或計(jì)算出的上述位置、上述狀態(tài)量、和上述誤差，對表示上述動作的上述多個(gè)公式模型的每一個(gè)決定相對于上述動作的適用范圍的限制；基于按照上述多個(gè)公式模型的每一個(gè)計(jì)算出的上述誤差的大小，決定上述閾值；根據(jù)上述限制，選擇上述特定的公式模型。


圖1是表示測位裝置的示意性結(jié)構(gòu)的圖。圖2是表示狀態(tài)量與公式模型的誤差之間的相關(guān)的圖。圖3是閾值候補(bǔ)產(chǎn)生處理的流程圖。圖4是閾值決定處理的流程圖。圖5是表示公式模型變量表的圖。圖6是公式模型選擇處理的流程圖。3/14 頁圖7是表示相對于阿卡曼模型及推算定位模型的公式模型的車速的誤差變化的圖。圖8是表示閾值Vth的時(shí)間變化的圖。圖9是表示阿卡曼模型及推算定位模型的公式模型表的圖。圖10是表示傳感器融合模型及內(nèi)界傳感器模型的公式模型的誤差變化的圖。圖11是表示傳感器融合模型及內(nèi)界傳感器模型的公式模型表的圖。圖12是表示線路融合模型及INS模型的公式模型的誤差變化的圖。圖13是表示線路融合模型及INS模型的公式模型表的圖。
具體實(shí)施例方式在用于根據(jù)測位誤差切換各種公式模型的閾值是不變的情況下，存在對本車位置推測的計(jì)算精度造成很大的惡劣影響的可能性。在本車位置的計(jì)算精度與車輛運(yùn)動控制的有關(guān)系的情況下，有可能給車輛控制的精度造成惡劣影響。在本發(fā)明中，為了在任何情況下都高精度地保證本車位置的推測精度，并不是將用于切換在預(yù)先準(zhǔn)備的測位中所使用的多個(gè)公式模型的閾值設(shè)為事前準(zhǔn)備的不變的值，而是將其設(shè)為使用本車的狀態(tài)、特別是為了用在本車位置計(jì)算而進(jìn)行推測或?qū)W習(xí)的參數(shù)、變量或系數(shù)等計(jì)算出的值。通過將以上方式計(jì)算出的值作為閾值，利用基于該閾值使得用于測位的公式模型變化(決定)的本車位置推測裝置，能防止給本車位置推測的計(jì)算精度帶來的惡劣影響。在汽車導(dǎo)航系統(tǒng)和自動巡航系統(tǒng)等必須高精度地推測自身的絕對位置的系統(tǒng)中，通過切換用于基于上述的閾值推測自身的絕對位置的模型，從而在任何狀態(tài)、狀況下都始終能夠推測高精度的位置。基于用于高精度地推測位置所需的自身的誤差，能輸出本車位置的可靠性。下面，說明使用本發(fā)明的測位裝置的詳情情況。(第一實(shí)施方式)圖1表示使用了本發(fā)明的測位裝置的示意性結(jié)構(gòu)。測位裝置由汽車導(dǎo)航系統(tǒng)或?qū)Ｓ媒K端實(shí)現(xiàn)，由獲取搭載了測位裝置的車輛的當(dāng)前的姿勢信息及位置測定信息的傳感器 101、和本車位置推測裝置102構(gòu)成。本車位置推測裝置102具有概率模型濾波處理部 103，使用基于卡爾曼濾波器等概率模型的公式模型來周期性地計(jì)算車輛位置；閾值計(jì)算部 104，基于由該概率模型濾波處理部103計(jì)算出的速度等表示車輛的移動狀態(tài)的狀態(tài)量和該計(jì)算出的狀態(tài)量的誤差，計(jì)算可成為用于變更在概率模型濾波處理部103中用于車輛位置的計(jì)算中的公式模型的閾值的閾值候補(bǔ)的值；以及閾值決定部105，根據(jù)由閾值計(jì)算部 104求出的閾值候補(bǔ)的值及在概率模型濾波處理部103使用的公式模型的特性，決定變更在概率模型濾波處理部103中用于計(jì)算的公式模型的閾值。概率模型濾波處理部103具有公式模型處理部106，由為了計(jì)算本車位置而使用的表示車輛的動作的多個(gè)公式模型構(gòu)成；以及公式模型選擇部107，基于在閾值決定部105 中決定的閾值，選擇在公式模型處理部106中用于轉(zhuǎn)移矩陣的各要素值的計(jì)算的公式模型。在公式模型處理部106中計(jì)算由卡爾曼濾波器等概率模型的狀態(tài)方程式等求出的轉(zhuǎn)移矩陣的各要素的值。公式模型是構(gòu)成表示測位算法的模型的狀態(tài)方程式或觀測方程式的方程式的一個(gè)個(gè)式子，用識別碼來管理。在公式模型處理部106中，通過組合與多個(gè)公式模型對應(yīng)的各個(gè)狀態(tài)方程式或觀測方程式，表示狀態(tài)方程式和觀測方程式，并根據(jù)該狀態(tài)方程式和觀測方程式計(jì)算卡爾曼濾波器等的概率模型。在閾值決定部105中保存由閾值計(jì)算部104計(jì)算出的閾值候補(bǔ)的值，相對于用于在公式模型選擇部107中選擇、變更在公式模型處理部106的計(jì)算中所使用的公式模型的公式模型表108，設(shè)定閾值。在公式模型表108中，保存有公式模型變量表111，公式模型變量表111以從傳感器101輸入的狀態(tài)量為變量，保存了表示各公式模型的狀態(tài)方程式或觀測方程式時(shí)的系數(shù)。自獲取車輛的姿勢信息及位置測定信息的傳感器101向本車位置推測裝置102 輸入本車的姿勢和移動方向、移動速度、或總體的位置的數(shù)據(jù)。在概率模型濾波處理部103 中，基于這些輸入值，通過公式模型選擇部107選擇在公式模型處理部106中用于轉(zhuǎn)移矩陣的計(jì)算的公式模型，輸出通過該公式模型的計(jì)算求出的位置。向閾值計(jì)算部104輸入在公式模型處理部106中由公式模型計(jì)算出的位置以外的狀態(tài)量。在閾值計(jì)算部104中，根據(jù)為公式模型處理部106準(zhǔn)備的多個(gè)公式模型的特性，計(jì)算各公式模型中的狀態(tài)量和誤差的關(guān)系，決定有關(guān)該公式模型的閾值的候補(bǔ)。向閾值決定部105輸入計(jì)算出的閾值的候補(bǔ)。在閾值決定部105中，關(guān)于多個(gè)公式模型，基于應(yīng)用時(shí)的誤差的大小及此時(shí)的狀態(tài)量決定閾值，為了能從公式模型選擇部107參照該閾值，將該閾值保存在公式模型表108中。從概率模型濾波處理部103內(nèi)的公式模型選擇部107參照由閾值決定部105決定的各狀態(tài)量每一個(gè)的閾值。在下一個(gè)位置計(jì)算周期時(shí)，在從獲取車輛的姿勢信息及位置測定信息的傳感器101向本車位置推測裝置102輸入數(shù)據(jù)時(shí)，為了從公式模型處理部106的多個(gè)公式模型中選擇最佳的公式模型而使用該閾值。通過周期地循環(huán)處理此流程，相對于各狀態(tài)量依次決定最佳的閾值，更新公式模型表108。從獲取車輛的姿勢信息及位置測定信息的傳感器101中輸出本車進(jìn)行移動的方位、本車的斜度等車輛的姿勢、本車的絕對位置、本車進(jìn)行移動的速度、本車和其它物體的相對位置及方位/姿勢的關(guān)系等信息。通過如GPS那樣可計(jì)算直接位置的傳感器、測量絕對位置和相對于形狀已知的物體的相對位置的照相機(jī)或雷達(dá)傳感器等傳感器，計(jì)算出車輛的絕對位置。根據(jù)陀螺儀傳感器這樣的方位傳感器的信號、或在不同的時(shí)間接收到的GPS信號，計(jì)算車輛進(jìn)行移動的方位，其中，陀螺儀傳感器對在賦予了可測量直接絕對方位的地磁傳感器的信號和初始方位的基礎(chǔ)上測量出的角速度進(jìn)行積分來計(jì)算方位。根據(jù)可計(jì)算直接速度的車速脈沖的傳感器的信號、或通過檢測車輛的加速度并加以積分來計(jì)算速度的加速度傳感器的信號、或者在不同的時(shí)間接收到的GPS信號，計(jì)算車輛進(jìn)行移動的速度。根據(jù)如加速度傳感器等那樣通過感知重力的方向來檢測車輛的斜度的姿勢檢測傳感器的信號、可測量相對于形狀已知的物體的相對姿勢的雷達(dá)傳感器的信號、或可測量對于作為形狀已知的物體的本車的斜度這樣的照相機(jī)的圖像，計(jì)算車輛的姿勢。在概率模型濾波處理部103中，公式模型處理部106通過卡爾曼濾波器或粒子濾波器等使用了基于概率模型的位置推測邏輯的多個(gè)公式模型，來推測位置。作為使用了位置推測邏輯的位置推測的代表例，有根據(jù)如上述那樣的擴(kuò)展卡爾曼濾波器算法的本車位置推測。在基于概率模型的位置推測邏輯中，將來自傳感器101的輸出或由公式模型處理部 106計(jì)算出的本車的方位/位置/姿勢/速度的信息表示為考慮了其誤差的觀測方程式，將從某一時(shí)間向下一個(gè)時(shí)間轉(zhuǎn)換的本車的方位/位置/姿勢/速度表示為狀態(tài)方程式。根據(jù)公式模型處理部106所具有的各公式模型，計(jì)算此觀測方程式及狀態(tài)方程式之后進(jìn)行濾波處理。在公式模型處理部106中，與本車的位置一起還推測關(guān)于方位/姿勢/速度的信息(狀態(tài)量)。計(jì)算出的狀態(tài)量被輸出到閾值計(jì)算部104。在公式模型處理部106中，在使用計(jì)算誤差和誤差的分散的公式模型進(jìn)行計(jì)算的情況下，也輸出此誤差。在閾值計(jì)算部104中，估計(jì)相對于從概率模型濾波處理部103輸入的狀態(tài)量的、公式模型處理部106所具有的多個(gè)公式模型的誤差。圖2表示公式模型中的狀態(tài)量及其誤差的相互關(guān)系例的示意圖。圖2的橫軸是從概率模型濾波處理部103輸入的狀態(tài)量，縱軸是與該狀態(tài)量一起從概率模型濾波處理部103輸入的誤差的值，圖2表示相對于每一公式模型的狀態(tài)量的誤差的值。曲線201是表示了某一公式模型1中的相對于狀態(tài)量的誤差變化的曲線，曲線202是表示公式模型2中的相對于狀態(tài)量的誤差變化的曲線。曲線201和曲線202的交點(diǎn)A203是公式模型1的誤差和公式模型2的誤差翻轉(zhuǎn)的點(diǎn)。在狀態(tài)量為從交點(diǎn)A203向右的狀態(tài)量的情況下，即，狀態(tài)量比交點(diǎn)A203的狀態(tài)量的值B204大的情況下，使用公式模型1計(jì)算時(shí)誤差小。在狀態(tài)量為從交點(diǎn)A203向左的狀態(tài)量的情況下，即，狀態(tài)量比交點(diǎn)A203的狀態(tài)量的值B204小的情況下，使用公式模型2計(jì)算時(shí)誤差小。交點(diǎn)203的狀態(tài)量的值B204成為進(jìn)行切換濾波處理中所使用的公式模型的判斷時(shí)的閾值候補(bǔ)，向閾值決定部105輸出此閾值候補(bǔ)。圖3表示閾值計(jì)算部104中的閾值候補(bǔ)的生成處理。此處理是對公式模型處理部 106所管理的公式模型內(nèi)的兩個(gè)公式模型比較其誤差，從而分別求有關(guān)兩個(gè)公式模型間的閾值候補(bǔ)的處理。首先，從概率模型濾波處理部103獲取由公式模型處理部106計(jì)算出的狀態(tài)量(步驟S301)。關(guān)于獲取的狀態(tài)量，按分別用在計(jì)算中的每一個(gè)公式模型從最新的值最多存儲到規(guī)定的數(shù)量。接著，從公式模型處理部106所具有的多個(gè)公式模型中，搜索出使用在步驟S301中獲取的狀態(tài)量的多個(gè)公式模型，并生成清單。關(guān)于生成的清單中的各公式模型，參照公式模型變量表111獲取對應(yīng)于狀態(tài)量的系數(shù)，來構(gòu)成狀態(tài)方程式或觀測方程式。 下面，將獲取對應(yīng)于狀態(tài)量的公式模型的系數(shù)的情形稱為獲取公式模型?；讷@取的狀態(tài)量進(jìn)行計(jì)算，針對各公式模型獲取其誤差(步驟S302)。假設(shè)在公式模型變量表111中僅預(yù)先設(shè)定了用于決定閾值候補(bǔ)所需的公式模型。假設(shè)獲取的公式模型的誤差是按分別在計(jì)算中使用的每一公式模型，由閾值計(jì)算部104從最新的值最多存儲到規(guī)定的數(shù)量。關(guān)于在由步驟S301獲取的狀態(tài)量中包含用于公式模型的誤差比較中的狀態(tài)量、 即成為可支配的變量的狀態(tài)量的公式模型，調(diào)查是否殘留了存在于由步驟S302生成的清單中的未處理的狀態(tài)量(步驟S30;3)。假設(shè)在保存到公式模型表108中的公式模型變量表 111中預(yù)先定義成為可支配的變量的狀態(tài)量。如圖5所示，關(guān)于公式模型處理部106所具有的各公式模型，公式模型變量表存儲與各個(gè)公式模型所處理的狀態(tài)量對應(yīng)的變量的變量名稱、和表示此變量是否為可支配的標(biāo)志。在公式模型變量表111中僅設(shè)定預(yù)先決定閾值候補(bǔ)所需的公式模型。按公式模型表的序號順序檢索是否存在含有成為選擇對象的狀態(tài)量的公式模型。如果有未處理的狀態(tài)量，則選擇此狀態(tài)量(步驟S304)。接著，搜索以選擇出的狀態(tài)量為可支配的變量的公式模型。從頭開始按順序搜索在步驟S302中生成的清單，從最初發(fā)現(xiàn)的未處理的公式模型的下一個(gè)開始，通過搜索以相同的狀態(tài)量為可支配的變量的公式模型，從而選擇以相同的狀態(tài)量為可支配的變量的公式模型對(步驟S305)。在即便是一個(gè)也發(fā)現(xiàn)了成為對象的公式模型對的情況下，也暫時(shí)保存公式模型變量表的序號和公式模型表的序號，進(jìn)入步驟S306及S307。如果至少存在一個(gè)成為對象的公式模型對，則在步驟S306及S307中，決定進(jìn)行誤差比較的一對、即關(guān)于兩個(gè)公式模型的圖2中的曲線201及曲線202。通過設(shè)定在步驟 S302獲取的誤差作為相對于由步驟S304選擇出的狀態(tài)量的變化的誤差，從而決定曲線201 及曲線202。計(jì)算基于由步驟S306及S307計(jì)算出或推測出的誤差決定的曲線彼此的交點(diǎn) A203，計(jì)算此交點(diǎn)A203的狀態(tài)量的值(步驟S308)。計(jì)算出的狀態(tài)量或作為閾值候補(bǔ)而輸出到閾值決定部105，或作為輸出待機(jī)而加以緩沖。(步驟S309)。在步驟S310中，在由步驟S302獲得的各公式模型的清單中，自開頭方向按順序搜索以選擇出的狀態(tài)量為可支配的變量的公式模型。自發(fā)現(xiàn)的未處理的公式模型的下一個(gè)開始，反復(fù)搜索還是以相同的狀態(tài)量為可支配的變量的公式模型，對于可支配的狀態(tài)量重復(fù)的公式模型的所有的組合，判斷是否完成了從步驟S306到S309的處理。在完成了全部計(jì)算的情況下(步驟S310 是)，即在結(jié)束以由步驟S304選擇出的狀態(tài)量為可支配的變量的公式模型對的所有處理的情況下，返回步驟S303。在步驟S309中存在作為輸出待機(jī)而緩沖的數(shù)據(jù)的情況下，也可以在該時(shí)刻向閾值決定部105輸出。另一方面，在對于所有的公式模型對的未完成計(jì)算的情況下，由于具有還存在成為其它的選擇對象的公式模型的可能性， 所以返回步驟S305，重復(fù)上述的計(jì)算(步驟S310 否)。在步驟S303的判定中，在以由步驟S302生成的清單的公式模型為可支配的變量的狀態(tài)量內(nèi)、沒有殘留未處理的狀態(tài)量的情況下(步驟S303 不能判定)，終止閾值計(jì)算部 104的閾值候補(bǔ)的生成處理。在閾值決定部105中，使用從閾值計(jì)算部104輸入的閾值候補(bǔ)決定各狀態(tài)量的閾值。決定的閾值被輸出到公式模型表108。閾值決定部105具有存儲來自閾值計(jì)算部104 的閾值候補(bǔ)的存儲區(qū)域(未圖示)。圖4的流程圖表示根據(jù)從閾值計(jì)算部104輸入的閾值候補(bǔ)決定各狀態(tài)量的閾值的閾值決定處理。首先，從公式模型表108的公式模型變量表111中獲取公式模型處理部106 所具有的多個(gè)公式模型(步驟S401)。接著，獲取從閾值計(jì)算部104輸出的閾值候補(bǔ)(步驟
5402)。在保存了所有到此為止的閾值候補(bǔ)的區(qū)域中進(jìn)一步保存本次閾值候補(bǔ)的值(步驟
5403)。在設(shè)定有存儲量的上限的情況下，也可以將到此為止保存的閾值候補(bǔ)替換為如平均值等這樣的代表值。接著，在步驟S404中，選出某一閾值候補(bǔ)的狀態(tài)量，判斷是否存在選出的閾值候補(bǔ)的狀態(tài)量影響誤差的公式模型。在存在閾值候補(bǔ)的狀態(tài)量變動時(shí)誤差變動的公式模型的情況下，在步驟S405中按照閾值候補(bǔ)的狀態(tài)量的特性隨時(shí)間變化的閾值候補(bǔ)(時(shí)變的狀態(tài)量)和不隨時(shí)間變化的閾值候補(bǔ)(時(shí)不變的狀態(tài)量)分開處理。由于狀態(tài)量隨時(shí)間變化還是不隨時(shí)間變化是在后述的公式模型表108中進(jìn)行分類設(shè)定，所以參照它。在閾值候補(bǔ)為時(shí)變的狀態(tài)量的情況下，移向步驟S406，設(shè)定依據(jù)到此為止的閾值候補(bǔ)的值的逐次學(xué)習(xí)法， 例如使用卡爾曼濾波器或最小二乘法等可追蹤時(shí)變的閾值的變動的逐次學(xué)習(xí)法的增益。在步驟S407中，決定來自步驟S406的根據(jù)逐次學(xué)習(xí)法的追蹤的最終輸出值作為閾值。接著， 在步驟S408中，在比由步驟S407決定的閾值還大的情況下選擇誤差變小的公式模型，在比由步驟S407決定的閾值還小的情況下選擇誤差變小的公式模型，將表示選擇出的公式模型的序號設(shè)定為公式模型表108的對象行或?qū)ο罅兄械某蔀閷ο蟮囊?。以圖2為例，在成為閾值的對象的狀態(tài)量的值比值B小的情況下，由于公式模型2的誤差小，所以將[2]設(shè)定為公式模型表的要素，在成為閾值的對象的狀態(tài)量的值比值B大的情況下，由于公式模型1 的誤差小，所以將[1]設(shè)定為公式模型表的要素。在閾值候補(bǔ)是時(shí)不變的狀態(tài)量的情況下，移向步驟S409，使用取閾值候補(bǔ)的平均值的方法、和將到此為止的閾值候補(bǔ)的值分類為兩個(gè)的分組方法，例如使用“Christopher Μ. Bishop著「"夕一 >認(rèn)識i機(jī)械學(xué)習(xí)下」y) W —· ” ^ “ > 2008年7月1日刊 ISBN:9784431100317”中記載的支持向量機(jī)等，進(jìn)行閾值的決定。在步驟S410中，決定來自步驟S409的最終的輸出值為閾值。接著，在步驟S411中，在比由步驟S410決定的閾值還大的情況下選擇誤差變小的公式模型，在比由步驟S410決定的閾值還小的情況下選擇誤差變小的公式模型，將表示選擇出的公式模型的識別序號設(shè)定為成為公式模型表108的對象行或?qū)ο罅兄械膶ο蟮囊亍＝又?，在步驟S412中，確認(rèn)對所有的公式模型決定了由步驟S407或步驟S410決定的閾值。這是因?yàn)椋捎谠试S在公式模型中存在多個(gè)閾值(狀態(tài)量)，所以僅決定唯一的閾值(狀態(tài)量)是不夠的。假如在未對所有的公式模型決定閾值的情況下，即在存在沒有決定閾值的公式模型的情況下，返回步驟S404，進(jìn)一步進(jìn)行其它閾值的決定。在對所有的公式模型決定了閾值的情況下，進(jìn)入步驟S413，刪除作為到此為止的閾值而決定的值，重新更新由步驟S407或步驟S410計(jì)算出的值作為閾值。最后，在步驟S414中，將新決定的閾值設(shè)定在公式模型表108中。通過以上的處理，能相對于已計(jì)算閾值候補(bǔ)的狀態(tài)量決定一個(gè)閾值，對于處于公式模型處理部106中的所有公式模型至少決定一個(gè)閾值。圖9表示公式模型表108的結(jié)構(gòu)。在此表的縱方向上排列決定誤差的大小所需的公式模型的狀態(tài)量。此狀態(tài)量是由概率模型濾波處理部103計(jì)算出的狀態(tài)量，是時(shí)變的狀態(tài)量。例如在概率模型濾波處理部103中使用了卡爾曼濾波器的情況下，在觀測方程式中使用決定此誤差的大小所需的公式模型。在決定誤差的大小所需的公式模型內(nèi)，低速度狀態(tài)下存在模型化誤差大這樣的公式模型的情況下，根據(jù)誤差的大小，會存在低速度行駛時(shí)和高速度行駛時(shí)的兩個(gè)狀態(tài)。無論用哪個(gè)狀態(tài)都能區(qū)分模型的誤差的大小。在橫方向上設(shè)置表示概率模型的內(nèi)部狀態(tài)的狀態(tài)量。這主要設(shè)為根據(jù)由概率模型濾波處理部103計(jì)算出的狀態(tài)量計(jì)算的狀態(tài)量，且時(shí)不變的狀態(tài)量。即使是設(shè)置在縱方向的公式模型分類用的狀態(tài)量，如果是表示概率模型的內(nèi)部狀態(tài)的狀態(tài)量，則認(rèn)為時(shí)間變動小，可以設(shè)置為橫方向的狀態(tài)量。作為時(shí)不變的狀態(tài)量，例如在由概率模型濾波處理部103計(jì)算出的狀態(tài)量中存在傳感器的安裝角的誤差的情況下，此狀態(tài)量被包含在排列于橫方向上的狀態(tài)量的一個(gè)中， 會存在完成學(xué)習(xí)及未完成學(xué)習(xí)的兩個(gè)狀態(tài)。這種學(xué)習(xí)的完成/未完成的判斷，在概率模型濾波處理部103中會區(qū)別于濾波處理而另外進(jìn)行計(jì)算。在上述的例子中，通過預(yù)先決定的分散值除以某一固定時(shí)間內(nèi)的傳感器安裝角的誤差的計(jì)算結(jié)果的分散值，能夠?qū)鞲衅鞯陌惭b角的誤差波動了多少、即學(xué)習(xí)是否正在進(jìn)展進(jìn)行數(shù)值化。將此數(shù)值稱為學(xué)習(xí)度，以某一值為閾值來判斷學(xué)習(xí)的完成/未完成。作為表示這樣的內(nèi)部狀態(tài)的狀態(tài)量，除此之外還有陀螺儀傳感器的偏差的學(xué)習(xí)、標(biāo)度因子(scale factor)的學(xué)習(xí)、舵角特性的學(xué)習(xí)等。設(shè)學(xué)習(xí)度為1以上的值的狀態(tài)為理論上學(xué)習(xí)完全結(jié)束的狀態(tài)。在公式模型表108的內(nèi)部，通過在縱軸及橫軸選出的狀態(tài)量的狀況的組合來設(shè)定最佳的模型。此最佳的模型是由步驟S408或步驟S411設(shè)定的公式模型。在公式模型處理部106中也可以存在互不排他的多個(gè)公式模型。因此，相對于在公式模型表的內(nèi)部的縱軸及橫軸選出的狀況，可設(shè)定多個(gè)公式模型。在步驟S414中，將在步驟S413中保存的閾值設(shè)定為縱軸或橫軸的閾值。若在步驟S414中設(shè)定的閾值為速度10km/h、以及傳感器安裝角學(xué)習(xí)度為0. 1，則縱軸內(nèi)的低速度行駛表示不到10km/h，高速度行駛表示10km/h以上。此外，同樣地，在橫軸，將傳感器安裝角學(xué)習(xí)度為0.1以下的情況設(shè)定為未完成學(xué)習(xí)，將0.1以上設(shè)為完成學(xué)習(xí)。在公式模型選擇部107中，按照由閾值決定部105生成的公式模型表108，選擇多個(gè)最適合于當(dāng)前的車輛狀態(tài)的公式模型。圖6表示公式模型選擇部107的從多個(gè)公式模型處理部106選擇多個(gè)最佳的模型的處理的流程圖。首先，在步驟S601，讀入公式模型表 108。接著，讀入由公式模型處理部106在上一次計(jì)算出的表示車輛的動作和姿勢/狀態(tài)的狀態(tài)量(步驟S602)。接著，判定讀入的各狀態(tài)量屬于設(shè)定在公式模型表108中的閾值的縱軸側(cè)及橫軸側(cè)的哪一側(cè)，從公式模型表108選擇針對縱軸和橫軸計(jì)算出的車輛的狀況(狀態(tài)量)的值與條件相符的行/列(步驟S603)。在這里選擇的行及列可以是多個(gè)。在步驟 S604中，讀入最適于在選擇出的行/列內(nèi)設(shè)定的對應(yīng)的狀況的公式模型。接著，在步驟S605中，針對由步驟S604選擇出的所有公式模型確認(rèn)有無重復(fù)，在存在重復(fù)的模型的情況下，移向步驟S606，在沒有重復(fù)的公式模型之后，設(shè)定為公式模型處理部106的計(jì)算對象的公式模型。這是為了避免在公式模型處理部106中重復(fù)使用相同的方程式。在步驟S605中，在選擇出的公式模型中不存在重復(fù)的公式模型的情況下，移向步驟S607，在公式模型處理部106中設(shè)定選擇出的所有公式模型。在公式模型處理部106中，僅使用由公式模型選擇部107選擇出的公式模型來計(jì)算位置及其它的狀態(tài)量。如果公式模型處理部106整體管理N個(gè)公式模型，則在公式模型選擇部107中就會選擇其中的η個(gè)。其中，η是N以下的數(shù)。在公式模型處理部106中，構(gòu)成與由公式模型選擇部107選擇出的η個(gè)公式模型對應(yīng)的概率模型的狀態(tài)方程式和觀測方程式，通過卡爾曼濾波器等概率模型濾波器計(jì)算位置及其它的狀態(tài)量。位置及其它的狀態(tài)量，計(jì)算出預(yù)先決定的數(shù)，不依賴于由公式模型選擇部107選擇出的公式模型的數(shù)。通過選擇誤差少的公式模型來計(jì)算概率模型濾波器，能夠進(jìn)行精度比使用含誤差大的信息的所有的公式模型來推測狀態(tài)量時(shí)更高的推測。在公式模型處理部106中，公式模型的設(shè)定例如可像以下那樣實(shí)現(xiàn)。設(shè)利用公式模型選擇部，在公式模型處理部106所具有的N個(gè)公式模型內(nèi)，選擇了公式模型1、公式模型2、公式模型3。公式模型1如(式1)所示，公式模型2如(式2)所示，公式模型3如(式3)所示。Ci是公式模型i的觀測值，(x、y、z)是從概率模型濾波處理部103輸出的所有狀態(tài)量，Ail Ai3是公式模型i中的各自的狀態(tài)量的系數(shù)。C1 = Anx+A12y+A13z ...(式 1)C2 = A21x+A22y+A23z ...(式 2)C3 = A31x+A32y+A”z (式 3)在將這些公式模型作為卡爾曼濾波器的觀測方程式來使用的情況下，如果在步驟S607中所有的公式模型沒有重復(fù)，則觀測方程式如(式4)所示。
權(quán)利要求
1.一種位置推測裝置，包括公式模型處理部，其從傳感器獲取移動體的姿勢信息及位置測定信息，并基于獲取到的上述姿勢信息及上述位置測定信息、和預(yù)先具有的表示上述移動體的動作的多個(gè)公式模型中的特定的公式模型，通過使用了概率模型的濾波處理，計(jì)算上述移動體的位置、表示上述移動體的移動狀態(tài)的狀態(tài)量、和上述狀態(tài)量的誤差；閾值計(jì)算部，其使用由上述公式模型處理部計(jì)算出的上述誤差，計(jì)算用于選擇上述特定的公式模型的閾值的候補(bǔ)值；閾值決定部，其基于上述候補(bǔ)值決定上述閾值；以及公式模型選擇部，其基于由上述閾值決定部決定的上述閾值，選擇上述特定的公式模型。
2.根據(jù)權(quán)利要求1所述的位置推測裝置，其中，基于能夠從外部觀測的信息或由上述公式模型處理部計(jì)算出的上述位置、上述狀態(tài)量、和上述誤差，對表示上述動作的上述多個(gè)公式模型的每一個(gè)決定相對于上述動作的適用范圍的限制；上述閾值決定部基于按照上述多個(gè)公式模型的每一個(gè)計(jì)算出的上述誤差的大小，決定上述閾值；上述公式模型選擇部根據(jù)上述限制，選擇上述特定的公式模型。
3.根據(jù)權(quán)利要求2所述的位置推測裝置，其中，所述位置推測裝置還包括存儲上述限制和上述閾值的數(shù)據(jù)庫。
4.一種位置推測方法，包括從傳感器獲取移動體的姿勢信息及位置測定信息的步驟；基于獲取到的上述姿勢信息及上述位置測定信息、和預(yù)先具有的表示上述移動體的動作的多個(gè)公式模型中的特定的公式模型，通過使用了概率模型的濾波處理，計(jì)算上述移動體的位置、表示上述移動體的移動狀態(tài)的狀態(tài)量、和上述狀態(tài)量的誤差的步驟；使用計(jì)算出的上述誤差，計(jì)算用于選擇上述特定的公式模型的閾值的候補(bǔ)值的步驟；基于上述候補(bǔ)值決定上述閾值的步驟；以及基于決定的上述閾值選擇上述特定的公式模型的步驟。
5.根據(jù)權(quán)利要求4所述的位置推測方法，其中，基于能夠從外部觀測的信息或計(jì)算出的上述位置、上述狀態(tài)量、和上述誤差，對表示上述動作的上述多個(gè)公式模型的每一個(gè)決定相對于上述動作的適用范圍的限制；基于按照上述多個(gè)公式模型的每一個(gè)計(jì)算出的上述誤差的大小，決定上述閾值； 根據(jù)上述限制，選擇上述特定的公式模型。
全文摘要
本發(fā)明提供一種位置推測裝置及位置推測方法，位置推測裝置包括公式模型處理部，其從傳感器獲取移動體的姿勢信息及位置測定信息，并基于獲取到的上述姿勢信息及上述位置測定信息、和預(yù)先具有的表示上述移動體的動作的多個(gè)公式模型中的特定的公式模型，通過使用了概率模型的濾波處理，計(jì)算上述移動體的位置和上述姿勢信息及上述位置測定信息的誤差；閾值計(jì)算部，其使用由上述公式模型處理部計(jì)算出的上述誤差，計(jì)算用于選擇上述特定的公式模型的閾值的候補(bǔ)值；閾值決定部，其基于上述候補(bǔ)值決定上述閾值；以及公式模型選擇部，其基于由上述閾值決定部決定的上述閾值，選擇上述特定的公式模型。
文檔編號G01C21/26GK102192745SQ20111004209
公開日2011年9月21日 申請日期2011年2月17日 優(yōu)先權(quán)日2010年2月24日
發(fā)明者岡田和弘, 劉智星, 小比田啟之, 板東干雄, 橫田孝義, 蛭田智昭, 青木利幸 申請人:歌樂株式會社