專利名稱：角速度推測裝置以及角速度推測方法
技術(shù)領(lǐng)域：
本發(fā)明涉及推測移動體的角速度的角速度推測裝置。
背景技術(shù)：
在陀螺傳感器等對移動體的角速度進(jìn)行觀測的角速度觀測裝置中，有偏移誤差 (bias error)等誤差。有如下方式使用全球定位系統(tǒng)(GPQ等位置觀測裝置，觀測移動體的位置，并根據(jù)觀測結(jié)果來推測移動體的角速度。另外，有如下方式使用根據(jù)位置觀測裝置的觀測結(jié)果推測出的角速度，來校正角速度觀測裝置觀測到的角速度。專利文獻(xiàn)1日本特開平6-148319號公報專利文獻(xiàn)2日本特開平9-49875號公報專利文獻(xiàn)3日本特開2007-333385號公報

發(fā)明內(nèi)容
如GPS那樣依靠外部的系統(tǒng)來對移動體的位置進(jìn)行觀測的位置觀測裝置有時根據(jù)電波狀況等而無法使用。本發(fā)明是為了解決例如所述那樣的課題而完成的，目的在于不用依靠GPS那樣的位置觀測裝置，而高精度地推測移動體的角速度。本發(fā)明的角速度推測裝置的特征在于，具有相對位置取得部、靜止物體判定部、物體相關(guān)部、以及角速度推測部，所述相對位置取得部針對存在于移動體的周邊的物體，取得周邊物體觀測裝置觀測到的觀測結(jié)果，其中，所述周邊物體觀測裝置反復(fù)觀測以所述移動體為基準(zhǔn)的所述物體的相對位置，所述靜止物體判定部根據(jù)所述相對位置取得部所取得的觀測結(jié)果，判定所述周邊物體觀測裝置觀測到相對位置的物體是否靜止，所述物體相關(guān)部根據(jù)所述相對位置取得部所取得的觀測結(jié)果，從所述周邊物體觀測裝置觀測到的多個相對位置中，判定針對同一物體由所述周邊物體觀測裝置觀測到的多個相對位置，所述角速度推測部根據(jù)所述相對位置取得部所取得的觀測結(jié)果、所述靜止物體判定部所判定的判定結(jié)果、以及所述物體相關(guān)部所判定的判定結(jié)果，推測所述移動體的角速度。根據(jù)本發(fā)明的角速度推測裝置，即使沒有GPS那樣的位置觀測裝置，也可以高精度地推測移動體的角速度。


圖1是示出實施方式1中的偏航率(yaw rate)偏移校正裝置800的結(jié)構(gòu)的框圖。圖2是示出直至?xí)r刻、 、在時間方向上連續(xù)地取得了相關(guān)性的靜止物體的情形的圖。圖3是示出實施方式1中的相關(guān)靜止物體選擇部141的處理的處理流程圖。圖4是示出向靜止物體基準(zhǔn)坐標(biāo)系的坐標(biāo)變換的圖。圖5是示出靜止物體基準(zhǔn)坐標(biāo)系中的本車軌道的圖。圖6是示出實施方式1中的偏移校正部840的處理的處理流程圖。圖7是示出時刻對準(zhǔn)的圖。圖8是示出實施方式2中的偏航率偏移校正裝置800的結(jié)構(gòu)的框圖。圖9是示出多個靜止物體的相對位置的一個例子的圖。圖10是時序的說明圖。圖11是示出實施方式3中的偏航率偏移校正裝置800的結(jié)構(gòu)的框圖。圖12是示出實施方式3中的軌道綜合部150的動作的處理流程圖。圖13是示出實施方式4中的靜止物體用追蹤部120的結(jié)構(gòu)的框圖。圖14是示出實施方式5中的角速度推測裝置200的硬件結(jié)構(gòu)的一個例子的硬件結(jié)構(gòu)圖。圖15是示出實施方式5中的角速度推測裝置200的功能塊的結(jié)構(gòu)的一個例子的塊結(jié)構(gòu)圖。圖16是用于說明移動體801的移動速度、與物體701、702相對移動體801的相對速度的關(guān)系的圖。圖17是用于說明物體相對移動體的相對速度的觀測時刻781 783、與移動體的移動速度的觀測時刻791 799的關(guān)系的圖。圖18是用于說明移動體801的移動軌跡751、與物體703 708相對移動體801 的相對位置的軌跡761 766的關(guān)系的圖。圖19是用于說明實施方式5中的物體相關(guān)部230的動作的圖。圖20是用于說明以移動體801為基準(zhǔn)的相對坐標(biāo)系、與以靜止點為基準(zhǔn)的絕對坐標(biāo)系之間的關(guān)系的圖。圖21是示出實施方式5中的角速度推測處理S500的流程的一個例子的流程圖。圖22是示出實施方式5中的觀測結(jié)果取得工序S510的流程的一個例子的流程圖。圖23是示出實施方式5中的靜止判定工序S520的流程的一個例子的流程圖。圖M是示出實施方式5中的物體相關(guān)工序S530的流程的一個例子的流程圖。圖25是示出實施方式5中的狀態(tài)推測工序S550的流程的一個例子的流程圖。圖沈是用于說明物體相關(guān)部230在相關(guān)處理中使用的平行移動量以及旋轉(zhuǎn)角度、 與移動體801的移動量的關(guān)系的圖。圖27是示出實施方式7中的角速度推測裝置200的功能塊的結(jié)構(gòu)的一個例子的塊結(jié)構(gòu)圖。圖觀是示出實施方式8中的角速度推測裝置200的功能塊的結(jié)構(gòu)的一個例子的塊結(jié)構(gòu)圖。附圖標(biāo)記說明110 靜止物體識別部；120 靜止物體用追蹤部；121 相關(guān)部；122 平滑部；123 預(yù)測部；1 延遲部；130 靜止物體存儲部；140 本車軌道計算部；141 相關(guān)靜止物體選擇部；142 靜止物體基準(zhǔn)坐標(biāo)變換部；143 軌道推測部；150 軌道綜合部；200 角速度推測裝置；211 相對位置取得部；212 速度觀測值取得部；213 角速度觀測值取得部；220 靜止物體判定部；230 物體相關(guān)部；240 狀態(tài)推測部；251 角速度誤差存儲部；252 角速度校正部；253 角速度檢驗部；2M 角速度誤差計算部；800 偏航率偏移校正裝置；810 雷達(dá)；811 周邊物體觀測裝置；820 車速傳感器；821 移動速度觀測裝置；830 偏航率傳感器；831 角速度觀測裝置；840 偏移校正部；901 輸出裝置；902 輸入裝置；911 處理裝置；914:存儲裝置。
具體實施例方式說明在使用車輛中搭載的雷達(dá)裝置來監(jiān)視車輛前方時對車輛前方的物體進(jìn)行檢測的裝置。有如下的車輛用前方監(jiān)視裝置，即，該車輛用前方監(jiān)視裝置搭載于車輛，向車輛前方的規(guī)定角度照射光波、毫米波等發(fā)送波，并接收其反射波，從而識別在本車輛的前方行駛的車輛、本車輛的前方的障礙物。在車輛用前方監(jiān)視裝置中，需要從所檢測出的物體中，不依賴于直線路徑行駛時、轉(zhuǎn)彎路徑行駛時而將本車輛的行進(jìn)方向上存在的車輛識別為先行車輛。此時，作為對在本車行進(jìn)方向上存在的先行車輛進(jìn)行識別的方式，使用車速和偏航率 (角速度)傳感器來計算道路曲率，識別先行車輛。但是，在為了計算道路曲率而使用的偏航率傳感器中產(chǎn)生與電壓偏置量相當(dāng)量的零點偏移誤差，而且伴隨著溫度、時間變化，零點偏移誤差會漂移，所以如果原樣地使用，則弄錯先行車判定的可能性高。因此，需要進(jìn)行傳感器的零點偏移校正。有如下方式為了計算本車的偏航率(以下，稱為偏航角速度)，取得由本車輛的雷達(dá)檢測出的多個靜止物體的前一時刻下的角度與當(dāng)前時刻下的角度的相關(guān)性。作為相關(guān)方式有如下方式使在前一時刻檢測出的靜止物體的角度偏移△ θ，將與在當(dāng)前時刻檢測出的靜止物體的角度最具有相關(guān)性的△ θ視為角度變化量，計算偏航角速度。但是，在實際的道路行駛環(huán)境中，在時間方向上未必能夠連續(xù)地檢測出靜止物體。例如，由于在途中發(fā)生檢測故障、再出現(xiàn)、或者多徑波(multipattwave)所致的無用信號的混入等，所以在所述方法中無法正確地推測偏航角速度。另外，有如下方式為了檢測本車輛的停止?fàn)顟B(tài)(速度為0的情況)，使用與檢測物體的相對速度成為0的條件，將低速時的偏航角速度視為偏航角速度零點偏移值而進(jìn)行校正。但是，如果本車并非低速時，則無法進(jìn)行偏移校正。另外，有如下方式根據(jù)利用GPS得到的本車的絕對位置以及利用雷達(dá)檢測出的與靜止物體的相對位置，計算包括偏航角速度的本車輛的運動。在該方式中，還包括與本車的運動無關(guān)的靜止物體而計算本車軌道，因此如果沒有利用GPS得到的絕對位置，就無法正確地計算出偏航角速度。這樣，在偏航率傳感器中產(chǎn)生電壓偏置量的零點偏移誤差，而且伴隨著溫度、時間變化，零點偏移誤差會漂移，所以如果原樣地使用，則弄錯先行車判定的可能性高。因此，利用追蹤濾波器從多個靜止物體中抽取在時間方向上具有相關(guān)性的靜止物體，根據(jù)該靜止物體與本車輛的相對位置的時間序列數(shù)據(jù)，來計算基于本車的位置和運動(在本文中稱為軌道)的偏航角速度推測值。而且，根據(jù)偏航角速度觀測值與推測值之差來計算偏移誤差，校正偏航角速度。實施方式1.使用圖1 圖7，說明實施方式1。圖1是示出本實施方式中的偏航率偏移校正裝置800的結(jié)構(gòu)的框圖。偏航率偏移校正裝置800具備雷達(dá)810、車速傳感器820、偏航率傳感器830、靜止物體識別部110、靜止物體用追蹤部120、靜止物體存儲部130、本車軌道計算部140、以及偏移校正部840。雷達(dá)810 (周邊物體觀測裝置)向車輛前方的規(guī)定角度范圍內(nèi)照射雷達(dá)波，并接收物體反射的雷達(dá)波，檢測該物體的位置以及相對速度。車速傳感器820 (移動速度觀測裝置)檢測車輛的速度。偏航率傳感器830 (角速度觀測裝置)檢測偏航角速度。靜止物體識別部110 (相對位置取得部、速度觀測值取得部、靜止物體判定部)根據(jù)由所述雷達(dá)810檢測出的物體的相對速度和由所述車速傳感器820檢測出的車輛的速度，識別該物體是否為靜止物體。靜止物體用追蹤部120 (物體相關(guān)部)利用追蹤濾波器，從由所述靜止物體識別部 110識別為靜止物體的多個靜止物體中抽取在時間方向上具有相關(guān)性的靜止物體。靜止物體用追蹤部120例如具有相關(guān)部121、平滑部122、預(yù)測部123、以及延遲部124。靜止物體存儲部130存儲由所述靜止物體用追蹤部120抽取的已相關(guān)的靜止物體。本車軌道計算部140(角速度推測部)將由所述靜止物體存儲部130得到的靜止物體位置作為輸入，將所述靜止物體位置變換到靜止物體基準(zhǔn)的坐標(biāo)系從而得到本車位置，根據(jù)所述本車位置來推測包含偏航角速度的本車軌道。本車軌道計算部140例如具有相關(guān)靜止物體選擇部141、靜止物體基準(zhǔn)坐標(biāo)變換部142、以及軌道推測部143。偏移校正部840使用由偏航率傳感器830檢測出的偏航角速度和從所述本車軌道計算部140得到的偏航角速度推測值，計算偏航角速度的偏移誤差并進(jìn)行校正。圖2是示出直至?xí)r刻、 tN在時間方向上連續(xù)地取得了相關(guān)性的靜止物體的情形的圖。當(dāng)追蹤靜止物體時，可知根據(jù)本車輛的運動而移動。在靜止物體用追蹤部120中，如該圖所示利用追蹤濾波器(卡爾曼濾波器、 α-β (-Υ)濾波器等公知的濾波器)，從多個靜止物體中抽取在時間方向上取得相關(guān)性的有效的靜止物體。該圖的白圓意味著在時間方向上取得了相關(guān)性的同一靜止物體，帶斜線的圓意味著所述靜止物體(白圓)的最新時刻的位置，黑圓意味著在時間方向上沒有連續(xù)性而在靜止物體用追蹤部120中沒有取得相關(guān)性的靜止物體。從靜止物體用追蹤部120輸出取得了相關(guān)性的靜止物體的位置(如果有多普勒速度則還包括多普勒速度)和該靜止物體編號(表示同一靜止物體的編號)。在靜止物體存儲部130中，記錄靜止物體編號和其已相關(guān)靜止物體的時間序列數(shù)據(jù)，并將所記錄的相關(guān)靜止物體的時間序列數(shù)據(jù)和該靜止物體編號輸出到相關(guān)靜止物體選擇部141。
在相關(guān)靜止物體選擇部141中，將每個同一靜止物體的相關(guān)靜止物體時間序列數(shù)據(jù)作為輸入，從所述相關(guān)靜止物體時間序列數(shù)據(jù)中，僅抽取進(jìn)入預(yù)先設(shè)定的距離Rmin(例如 30m)以內(nèi)的相關(guān)靜止物體，并對相關(guān)次數(shù)Ncot進(jìn)行計數(shù)。而且，在所述相關(guān)次數(shù)N。 是相關(guān)次數(shù)閾值Nmin以上的情況下，輸出所述相關(guān)靜止物體的位置。圖3是示出本實施方式中的相關(guān)靜止物體選擇部141的處理的處理流程圖。在步驟1中，相關(guān)靜止物體選擇部141抽取進(jìn)入距離閾值以內(nèi)的相關(guān)靜止物體。在步驟2中，相關(guān)靜止物體選擇部141判定相關(guān)靜止物體數(shù)是否為閾值以上。在相關(guān)靜止物體數(shù)是閾值以上的情況下，在步驟3中，相關(guān)靜止物體選擇部141輸出所述相關(guān)靜止物體。此處，對于相關(guān)次數(shù)閾值Nmin，使用本車速VM、雷達(dá)觀測速率T、距離Rmin如下式那樣決定上限。式11
權(quán)利要求
1.一種角速度推測裝置，其特征在于，具有相對位置取得部，取得周邊物體觀測裝置觀測到的觀測結(jié)果，其中，所述周邊物體觀測裝置反復(fù)觀測存在于移動體的周邊的物體，對以所述移動體為基準(zhǔn)的所述物體的相對位置進(jìn)行測定；靜止物體判定部，根據(jù)所述相對位置取得部所取得的觀測結(jié)果，判定所述周邊物體觀測裝置觀測到的物體是否靜止；物體相關(guān)部，根據(jù)所述相對位置取得部所取得的多個觀測結(jié)果，從所述周邊物體觀測裝置所測定的多個相對位置中，判定所述周邊物體觀測裝置觀測同一物體而測定的相對位置的組；以及角速度推測部，根據(jù)所述相對位置取得部所取得的觀測結(jié)果、所述靜止物體判定部所判定的判定結(jié)果、以及所述物體相關(guān)部所判定的判定結(jié)果，推測所述移動體的角速度。
2.根據(jù)權(quán)利要求1所述的角速度推測裝置，其特征在于，所述角速度推測裝置還具有相對位置計算部，該相對位置計算部根據(jù)所述相對位置取得部所取得的多個觀測結(jié)果、所述靜止物體判定部所判定的判定結(jié)果、以及所述物體相關(guān)部所判定的判定結(jié)果，計算以靜止的物體為基準(zhǔn)的所述移動體的相對位置，所述角速度推測部根據(jù)所述相對位置計算部計算出的相對位置，推測所述移動體的角速度。
3.根據(jù)權(quán)利要求1所述的角速度推測裝置，其特征在于，所述角速度推測裝置還具有角速度觀測值取得部，取得角速度觀測裝置觀測到的觀測結(jié)果，其中，所述角速度觀測裝置對所述移動體的角速度進(jìn)行觀測；以及角速度觀測誤差計算部，根據(jù)所述角速度推測部推測出的推測結(jié)果和所述角速度觀測值取得部取得的觀測結(jié)果，計算所述角速度觀測裝置觀測到的角速度的誤差。
4.根據(jù)權(quán)利要求1所述的角速度推測裝置，其特征在于，所述角速度推測裝置還具有速度觀測值取得部，該速度觀測值取得部取得移動速度觀測裝置觀測到的觀測結(jié)果，其中，所述移動速度觀測裝置對所述移動體的移動速度進(jìn)行觀測，所述靜止物體判定部根據(jù)所述相對位置取得部所取得的觀測結(jié)果以及所述速度觀測值取得部所取得的觀測結(jié)果，判定所述物體是否靜止。
5.根據(jù)權(quán)利要求4所述的角速度推測裝置，其特征在于，所述周邊物體觀測裝置還測定以所述移動體為基準(zhǔn)的所述物體的相對速度，所述靜止物體判定部根據(jù)所述移動速度觀測裝置觀測到的所述移動體的移動速度以及所述周邊物體觀測裝置所測定的所述物體的相對速度，判定所述物體是否靜止。
6.根據(jù)權(quán)利要求4所述的角速度推測裝置，其特征在于，所述角速度推測裝置還具有相對速度推測部，該相對速度推測部根據(jù)所述物體相關(guān)部所判定的判定結(jié)果，對于由所述周邊物體觀測裝置多次觀測到的物體，推測所述物體相對所述移動體的相對速度，所述靜止物體判定部根據(jù)所述移動速度觀測裝置觀測到的所述移動體的移動速度以及所述相對速度推測部所推測的所述物體的相對速度，判定所述物體是否靜止。
7.根據(jù)權(quán)利要求1所述的角速度推測裝置，其特征在于，通過由計算機執(zhí)行計算機程序，來實現(xiàn)所述相對位置取得部、所述靜止物體判定部、所述物體相關(guān)部以及所述角速度推測部。
8.一種角速度推測方法，推測移動體的角速度，其特征在于，取得周邊物體觀測裝置觀測到的觀測結(jié)果，其中，所述周邊物體觀測裝置反復(fù)觀測存在于所述移動體的周邊的物體，對以所述移動體為基準(zhǔn)的所述物體的相對位置進(jìn)行測定， 根據(jù)所取得的觀測結(jié)果，判定所述周邊物體觀測裝置觀測到的物體是否靜止， 根據(jù)所取得的多個觀測結(jié)果，從所述周邊物體觀測裝置所測定的多個相對位置中，判定所述周邊物體觀測裝置觀測同一物體而測定的相對位置的組，根據(jù)所取得的觀測結(jié)果、與所述物體的靜止性相關(guān)的判定結(jié)果、以及與所述相對位置的組相關(guān)的判定結(jié)果，推測所述移動體的角速度。
9.根據(jù)權(quán)利要求8所述的角速度推測方法，其特征在于， 由計算機執(zhí)行計算機程序，其中，該計算機程序具有如下步驟取得周邊物體觀測裝置觀測到的觀測結(jié)果的步驟，其中，所述周邊物體觀測裝置反復(fù)觀測存在于移動體的周邊的物體，對以所述移動體為基準(zhǔn)的所述物體的相對位置進(jìn)行測定；根據(jù)所取得的觀測結(jié)果，判定所述周邊物體觀測裝置觀測到的物體是否靜止的步驟； 根據(jù)所取得的多個觀測結(jié)果，從所述周邊物體觀測裝置所測定的多個相對位置中，判定所述周邊物體觀測裝置觀測同一物體而測定的相對位置的組的步驟；以及根據(jù)所取得的觀測結(jié)果、與所述物體的靜止性相關(guān)的判定結(jié)果、以及與所述相對位置的組相關(guān)的判定結(jié)果，推測所述移動體的角速度的步驟。
全文摘要
提供一種角速度推測裝置以及角速度推測方法，高精度地推測移動體的角速度。周邊物體觀測裝置(811；雷達(dá))針對移動體的周邊存在的物體，反復(fù)觀測以移動體為基準(zhǔn)的物體的相對位置。相對位置取得部(211)取得周邊物體觀測裝置(811)觀測到的觀測結(jié)果。靜止物體判定部(220；靜止物體識別部)判定周邊物體觀測裝置(811)觀測到相對位置的物體是否靜止。物體相關(guān)部(230；靜止物體用追蹤部)從由周邊物體觀測裝置(811)觀測到的多個相對位置中，判定針對同一物體由周邊物體觀測裝置(811)觀測到的多個相對位置。角速度推測部(狀態(tài)推測部240、軌道推測部)推測移動體的角速度。
文檔編號G01S13/60GK102288957SQ201110126438
公開日2011年12月21日 申請日期2011年5月16日 優(yōu)先權(quán)日2010年5月26日
發(fā)明者亀田洋志, 松崎貴史, 高林佑樹 申請人:三菱電機株式會社