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