▶ 株式会社デンソーの特許一覧 ▶ トヨタ自動車株式会社の特許一覧 ▶ 株式会社ミライズテクノロジーズの特許一覧
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2023183773
(43)【公開日】2023-12-28
(54)【発明の名称】状態推定装置、状態推定方法
(51)【国際特許分類】
G01C 21/16 20060101AFI20231221BHJP
G01C 21/20 20060101ALI20231221BHJP
G06T 7/20 20170101ALI20231221BHJP
G06T 7/00 20170101ALI20231221BHJP
【ＦＩ】
G01C21/16
G01C21/20
G06T7/20 100
G06T7/00 650B
【審査請求】未請求
【請求項の数】10
【出願形態】ＯＬ
(21)【出願番号】P 2022097471
(22)【出願日】2022-06-16
(71)【出願人】
【識別番号】000004260
【氏名又は名称】株式会社デンソー
(71)【出願人】
【識別番号】000003207
【氏名又は名称】トヨタ自動車株式会社
(71)【出願人】
【識別番号】520124752
【氏名又は名称】株式会社ミライズテクノロジーズ
(74)【代理人】
【識別番号】110001128
【氏名又は名称】弁理士法人ゆうあい特許事務所
(72)【発明者】
【氏名】松平 宣明
(72)【発明者】
【氏名】徐 慧
【テーマコード（参考）】
2F129
5L096
【Ｆターム（参考）】
2F129AA03
2F129BB20
2F129BB22
2F129BB26
2F129BB33
2F129BB38
2F129BB39
2F129BB40
2F129BB41
2F129BB42
2F129BB66
2F129CC33
2F129GG17
5L096BA04
5L096CA04
5L096FA09
5L096FA67
5L096FA69
5L096HA05
(57)【要約】 （修正有）
【課題】移動体の位置、速度、姿勢の少なくとも１つを含む状態の推定の精度向上を図る。
【解決手段】状態推定装置１０は、車両の位置、速度、姿勢の少なくとも１つを含む状態を推定する。状態推定装置１０は、撮像部２が出力する画像データおよび慣性計測装置３が出力する慣性データを読み込む入力部２２と、画像データ中の特徴点の抽出およびトラッキングを行うとともに、慣性データから車両の状態を算出する前処理部２４と、を備える。状態推定装置１０は、画像データの特徴点、慣性データに基づく車両の位置、速度、姿勢に対してバンドル調整を実施して慣性計測装置３のバイアス誤差を求める演算部２６と、慣性データからバイアス誤差を除去した補正データを求める補正部２８と、を備える。状態推定装置１０は、補正データに基づいて車両の位置、速度、姿勢の少なくとも１つを含む状態を推定する推定部３０を備える。
【選択図】図１
【解決手段】状態推定装置１０は、車両の位置、速度、姿勢の少なくとも１つを含む状態を推定する。状態推定装置１０は、撮像部２が出力する画像データおよび慣性計測装置３が出力する慣性データを読み込む入力部２２と、画像データ中の特徴点の抽出およびトラッキングを行うとともに、慣性データから車両の状態を算出する前処理部２４と、を備える。状態推定装置１０は、画像データの特徴点、慣性データに基づく車両の位置、速度、姿勢に対してバンドル調整を実施して慣性計測装置３のバイアス誤差を求める演算部２６と、慣性データからバイアス誤差を除去した補正データを求める補正部２８と、を備える。状態推定装置１０は、補正データに基づいて車両の位置、速度、姿勢の少なくとも１つを含む状態を推定する推定部３０を備える。
【選択図】図１
【特許請求の範囲】
【請求項1】
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定装置であって、
前記移動体の周囲を撮像する撮像部（２）が出力する画像データおよび前記移動体に設置された慣性計測装置（３）が出力する前記移動体の慣性データを読み込む入力部（２２）と、
前記画像データに含まれる特徴点の抽出および前記特徴点のトラッキングを行うとともに、前記慣性データに基づいて前記移動体の位置、速度、姿勢を算出する前処理部（２４）と、
前記画像データの特徴点、前記慣性データに基づく前記移動体の位置、速度、姿勢に対してバンドル調整を実施して前記慣性計測装置のバイアス誤差を求める演算部（２６）と、
前記慣性データから前記バイアス誤差を除去した補正データを求める補正部（２８）と、
前記補正データに基づいて前記移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定する推定部（３０）と、
を備える状態推定装置。
前記移動体の周囲を撮像する撮像部（２）が出力する画像データおよび前記移動体に設置された慣性計測装置（３）が出力する前記移動体の慣性データを読み込む入力部（２２）と、
前記画像データに含まれる特徴点の抽出および前記特徴点のトラッキングを行うとともに、前記慣性データに基づいて前記移動体の位置、速度、姿勢を算出する前処理部（２４）と、
前記画像データの特徴点、前記慣性データに基づく前記移動体の位置、速度、姿勢に対してバンドル調整を実施して前記慣性計測装置のバイアス誤差を求める演算部（２６）と、
前記慣性データから前記バイアス誤差を除去した補正データを求める補正部（２８）と、
前記補正データに基づいて前記移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定する推定部（３０）と、
を備える状態推定装置。
【請求項2】
前記推定部は、前記補正データおよび前記移動体に設置された車輪速センサ（４）のセンサ出力に基づいて、前記移動体の速度を求め、求めた前記移動体の速度に基づいて前記移動体の位置を推定する、請求項１に記載の状態推定装置。
【請求項3】
前記慣性計測装置は、前記移動体の角速度を検出するジャイロセンサ（３ａ）および前記移動体の加速度を検出する加速度センサ（３ｂ）を含み、
前記推定部は、
前記ジャイロセンサのセンサ出力を前記補正部で補正したものに基づいて前記移動体の姿勢を示す第１姿勢角を求め、
前記加速度センサのセンサ出力を前記補正部で補正したものおよび前記移動体の車輪速センサのセンサ出力から求めた重力加速度に基づいて前記移動体の姿勢を示す第２姿勢角を求め、
前記第１姿勢角を相補フィルタのハイパスフィルタを通し、前記第２姿勢角を前記相補フィルタのローパスフィルタを通した後に両者を合成することで、前記移動体の姿勢を推定する、請求項２に記載の状態推定装置。
前記推定部は、
前記ジャイロセンサのセンサ出力を前記補正部で補正したものに基づいて前記移動体の姿勢を示す第１姿勢角を求め、
前記加速度センサのセンサ出力を前記補正部で補正したものおよび前記移動体の車輪速センサのセンサ出力から求めた重力加速度に基づいて前記移動体の姿勢を示す第２姿勢角を求め、
前記第１姿勢角を相補フィルタのハイパスフィルタを通し、前記第２姿勢角を前記相補フィルタのローパスフィルタを通した後に両者を合成することで、前記移動体の姿勢を推定する、請求項２に記載の状態推定装置。
【請求項4】
前記撮像部が所期の機能を発揮することが困難な異常な状況であるか否かを判定する異常判定部（３１）を備え、
前記補正部は、
前記異常な状況でない場合は前記慣性データから前記バイアス誤差を除去することで前記補正データを算出し、
前記異常な状況になると、前記演算部で求めた前記バイアス誤差に代えて、事前にメモリに記憶していたバイアス推定値を前記慣性データから除去することで前記補正データを算出する、請求項１ないし３のいずれか１つに記載の状態推定装置。
前記補正部は、
前記異常な状況でない場合は前記慣性データから前記バイアス誤差を除去することで前記補正データを算出し、
前記異常な状況になると、前記演算部で求めた前記バイアス誤差に代えて、事前にメモリに記憶していたバイアス推定値を前記慣性データから除去することで前記補正データを算出する、請求項１ないし３のいずれか１つに記載の状態推定装置。
【請求項5】
前記異常判定部は、前記画像データに基づいて前記異常な状況であるか否かを判定する、請求項４に記載の状態推定装置。
【請求項6】
前記異常判定部は、前記移動体に設置された操舵角センサ（５）のセンサ出力および前記演算部で求めた前記移動体の姿勢に基づいて前記異常な状況であるか否かを判定する、請求項４に記載の状態推定装置。
【請求項7】
前記異常判定部は、前記移動体に設置された車輪速センサ（４）のセンサ出力および前記演算部で求めた前記移動体の速度に基づいて前記異常な状況であるか否かを判定する、請求項４に記載の状態推定装置。
【請求項8】
前記異常判定部は、前記演算部で求めた前記移動体の位置、速度、姿勢および前記推定部で推定した前記移動体の位置、速度、姿勢に基づいて前記異常な状況であるか否かを判定する、請求項４に記載の状態推定装置。
【請求項9】
前記補正部は、前記慣性計測装置の温度に応じて前記バイアス推定値を補正し、補正後の前記バイアス推定値を前記慣性データから除去することで前記補正データを算出する、請求項４に記載の状態推定装置。
【請求項10】
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定方法であって、
前記移動体の周囲を撮像する撮像部（２）が出力する画像データおよび前記移動体に設置された慣性計測装置（３）が出力する前記移動体の慣性データを読み込むことと、
前記画像データに含まれる特徴点の抽出および前記特徴点のトラッキングを行うとともに、前記慣性データに基づいて前記移動体の位置、速度、姿勢を算出することと、
前記画像データの特徴点、前記慣性データに基づく前記移動体の位置、速度、姿勢に対してバンドル調整を実施して前記慣性計測装置のバイアス誤差を求めることと、
前記慣性データから前記バイアス誤差を除去した補正データを求めることと、
前記補正データに基づいて前記移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定することと、
を含む状態推定方法。
前記移動体の周囲を撮像する撮像部（２）が出力する画像データおよび前記移動体に設置された慣性計測装置（３）が出力する前記移動体の慣性データを読み込むことと、
前記画像データに含まれる特徴点の抽出および前記特徴点のトラッキングを行うとともに、前記慣性データに基づいて前記移動体の位置、速度、姿勢を算出することと、
前記画像データの特徴点、前記慣性データに基づく前記移動体の位置、速度、姿勢に対してバンドル調整を実施して前記慣性計測装置のバイアス誤差を求めることと、
前記慣性データから前記バイアス誤差を除去した補正データを求めることと、
前記補正データに基づいて前記移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定することと、
を含む状態推定方法。
【発明の詳細な説明】
【技術分野】
【0001】
本開示は、移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定装置および状態推定方法に関する。
【背景技術】
【0002】
従来、カメラおよび慣性計測装置（所謂、ＩＭＵ）を用いて、バンドル調整という非線形な最小二乗法によって、複数のパラメータを精度よく推定する視覚慣性オドメトリＶＩＯといった技術が知られている。例えば、特許文献１には、視覚慣性オドメトリＶＩＯによって、移動体の位置、姿勢、速度、慣性計測装置のバイアス誤差を推定する技術が開示されている（例えば、特許文献１参照）。なお、“ＶＩＯ”は、Visual Inertial Odometryの略称である。また、“ＩＭＵ”は、Inertial Measurement Unitの略称である。
【先行技術文献】
【特許文献】
【0003】
【特許文献1】米国特許第９４２４６４７号明細書
【発明の概要】
【発明が解決しようとする課題】
【0004】
本発明者らは、視覚慣性オドメトリＶＩＯによって、車両等の移動体の位置、姿勢、速度、慣性計測装置のバイアス誤差を推定することを検討しているが、これらの推定を開始してから所定時間以内での姿勢変化の推定誤差が大きいことが判った。この理由としては、カメラが出力する画像は、モーションブラーや周囲の物体の動きの影響を受け易く、画像データに基づいて求める姿勢の短時間での誤差が大きくなってしまうことが考えられる。なお、これらは、本発明者の鋭意検討の末に見い出された。
【0005】
本開示は、移動体の位置、速度、姿勢の少なくとも１つを含む状態の推定の精度向上を図ることが可能な状態推定装置および状態推定方法を提供することを目的とする。
【課題を解決するための手段】
【0006】
請求項１に記載の発明は、
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定装置であって、
移動体の周囲を撮像する撮像部（２）が出力する画像データおよび移動体に設置された慣性計測装置（３）が出力する移動体の慣性データを読み込む入力部（２２）と、
画像データに含まれる特徴点の抽出および特徴点のトラッキングを行うとともに、慣性データに基づいて移動体の位置、速度、姿勢を算出する前処理部（２４）と、
画像データの特徴点、慣性データに基づく移動体の位置、速度、姿勢に対してバンドル調整を実施して慣性計測装置のバイアス誤差を求める演算部（２６）と、
慣性データからバイアス誤差を除去した補正データを求める補正部（２８）と、
補正データに基づいて移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定する推定部（３０）と、を備える。
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定装置であって、
移動体の周囲を撮像する撮像部（２）が出力する画像データおよび移動体に設置された慣性計測装置（３）が出力する移動体の慣性データを読み込む入力部（２２）と、
画像データに含まれる特徴点の抽出および特徴点のトラッキングを行うとともに、慣性データに基づいて移動体の位置、速度、姿勢を算出する前処理部（２４）と、
画像データの特徴点、慣性データに基づく移動体の位置、速度、姿勢に対してバンドル調整を実施して慣性計測装置のバイアス誤差を求める演算部（２６）と、
慣性データからバイアス誤差を除去した補正データを求める補正部（２８）と、
補正データに基づいて移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定する推定部（３０）と、を備える。
【0007】
本発明者らの検討によると、視覚慣性オドメトリＶＩＯによる姿勢変化の推定を開始してから或る程度時間が経過すると、姿勢変化の推定誤差が小さくなることが判っている。この理由としては、画像データの解析から得られる姿勢推定結果には、慣性計測装置のバイアス誤差が含まれないことが挙げられる。
【0008】
これらを考慮し、本開示の状態推定装置は、視覚慣性オドメトリＶＩＯでバイアス誤差を求め、当該バイアス誤差を慣性データから除去した補正データに基づいて、移動体の状態を推定する。
【0009】
このように、視覚慣性オドメトリＶＩＯによってバイアス誤差を推定し、当該バイアス誤差および慣性データに基づいて移動体の位置、速度、姿勢を推定すれば、モーションブラーや周囲の移動物体等に起因する誤差の影響を抑えることができる。したがって、本開示の状態推定装置によれば、移動体の位置、速度、姿勢の少なくとも１つを含む状態の精度向上を図ることができる。
【0010】
請求項１０に記載の発明は、
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定方法であって、
移動体の周囲を撮像する撮像部（２）が出力する画像データおよび移動体に設置された慣性計測装置（３）が出力する移動体の慣性データを読み込むことと、
画像データに含まれる特徴点の抽出および特徴点のトラッキングを行うとともに、慣性データに基づいて移動体の位置、速度、姿勢を算出することと、
画像データの特徴点、慣性データに基づく移動体の位置、速度、姿勢に対してバンドル調整を実施して慣性計測装置のバイアス誤差を求めることと、
慣性データからバイアス誤差を除去した補正データを求めることと、
補正データに基づいて移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定することと、を含む。
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定方法であって、
移動体の周囲を撮像する撮像部（２）が出力する画像データおよび移動体に設置された慣性計測装置（３）が出力する移動体の慣性データを読み込むことと、
画像データに含まれる特徴点の抽出および特徴点のトラッキングを行うとともに、慣性データに基づいて移動体の位置、速度、姿勢を算出することと、
画像データの特徴点、慣性データに基づく移動体の位置、速度、姿勢に対してバンドル調整を実施して慣性計測装置のバイアス誤差を求めることと、
慣性データからバイアス誤差を除去した補正データを求めることと、
補正データに基づいて移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定することと、を含む。
【0011】
このように、視覚慣性オドメトリＶＩＯによってバイアス誤差を推定し、当該バイアス誤差および慣性データに基づいて移動体の位置、速度、姿勢を推定すれば、モーションブラーや周囲の移動物体等に起因する誤差の影響を抑えることができる。したがって、本開示の状態推定状態によれば、移動体の位置、速度、姿勢の少なくとも１つを含む状態の精度向上を図ることができる。
【0012】
なお、各構成要素等に付された括弧付きの参照符号は、その構成要素等と後述する実施形態に記載の具体的な構成要素等との対応関係の一例を示すものである。
【図面の簡単な説明】
【0013】
【図1】第１実施形態に係る状態推定装置の概略構成図である。
【図2】状態推定装置に用いられる撮像部を説明するための説明図である。
【図3】状態推定装置に用いられる慣性計測装置を説明するための説明図である。
【図4】視覚慣性オドメトリの概要を説明するための説明図である。
【図5】画像に関する残差を説明するための説明図である。
【図6】慣性計測装置に関する残差を説明するための説明図である。
【図7】事前情報に関する残差を説明するための説明図である。
【図8】ジャイロセンサで測定される角速度および視覚慣性オドメトリで求めた姿勢から推定される角速度を説明するための説明図である。
【図9】慣性データに含まれるバイアス誤差の時間変化を説明するための説明図である。
【図10】第１実施形態に係る状態推定装置を説明するための説明図である。
【図11】視覚慣性オドメトリによって求めたバイアス誤差および慣性データから算出した補正データに基づく車両の姿勢の推定結果を説明するための説明図である。
【図12】第２実施形態に係る状態推定装置を説明するための説明図である。
【図13】第３実施形態に係る状態推定装置を説明するための説明図である。
【図14】第３実施形態に係る状態推定装置における車両の姿勢の推定方法を説明するための説明図である。
【図15】加速度センサのセンサ出力から車両の姿勢角を演算して求める方法を説明するための説明図である。
【図16】異常な状況時の課題を説明するための説明図である。
【図17】第４実施形態に係る状態推定装置を説明するための説明図である。
【図18】異常な状況下での挙動を説明するための説明図である。
【図19】第５実施形態に係る状態推定装置を説明するための説明図である。
【図20】第６実施形態に係る状態推定装置を説明するための説明図である。
【図21】第７実施形態に係る状態推定装置を説明するための説明図である。
【図22】第８実施形態に係る状態推定装置を説明するための説明図である。
【図23】第９実施形態に係る状態推定装置を説明するための説明図である。
【発明を実施するための形態】
【0014】
以下、本開示の実施形態について図面を参照して説明する。なお、以下の実施形態において、先行する実施形態で説明した事項と同一もしくは均等である部分には、同一の参照符号を付し、その説明を省略する場合がある。また、実施形態において、構成要素の一部だけを説明している場合、構成要素の他の部分に関しては、先行する実施形態において説明した構成要素を適用することができる。以下の実施形態は、特に組み合わせに支障が生じない範囲であれば、特に明示していない場合であっても、各実施形態同士を部分的に組み合わせることができる。
【0015】
（第１実施形態）
本実施形態について、図１～図１１を参照して説明する。本実施形態では、図１に示す本開示の状態推定装置１０を、車両１に適用し、車両１の位置ｐ、速度ｖ、姿勢φを推定して、外部へ出力する例について説明する。本実施形態では、車両１が“移動体”に対応している。
本実施形態について、図１～図１１を参照して説明する。本実施形態では、図１に示す本開示の状態推定装置１０を、車両１に適用し、車両１の位置ｐ、速度ｖ、姿勢φを推定して、外部へ出力する例について説明する。本実施形態では、車両１が“移動体”に対応している。
【0016】
車両１には、状態推定装置１０が搭載されている。車両１には、状態推定装置１０の他に、撮像部２および慣性計測装置３が搭載されている。なお、車両１には、状態推定装置１０は、その一部が車外に設置されていてもよい。
【0017】
撮像部２は、図２に示すように、車両１の周囲を周期的に撮像する。撮像部２は、車両１の周辺領域を撮像した画像データを出力する。撮像部２は、例えば、ＣＣＤ、ＣＭＯＳ等の光電変換素子を備えるカメラ等で構成される。なお、ＣＣＤは、Charge Coupled Deviceの略称である。ＣＭＯＳは、Complementary Metal Oxide Semiconductorの略称である。本実施形態の撮像部２は、単眼カメラで構成されている。なお、撮像部２は、複眼カメラで構成されていてもよい。
【0018】
慣性計測装置３は、車両１における３次元の慣性運動を検出する装置である。慣性計測装置３は、車両１における直交三軸方向の並進運動および回転運動を慣性データとして出力する。慣性計測装置３は、車両１の回転運動として車両１の角速度ωｘ、ωｙ、ωｚを検出するジャイロセンサ３ａおよび車両１の並進運動として車両１の加速度ｆｘ、ｆｙ、ｆｚを検出する加速度センサ３ｂを含んでいる。本実施形態の慣性計測装置３は、ＭＥＭＳベースの小型なＩＭＵとして構成されている。ＭＥＭＳは、Micro Electro Mechanical Systemsの略称である。
【0019】
状態推定装置１０は、図１に示すように、撮像部２が出力する画像データおよび慣性計測装置３が出力する慣性データに基づいて、車両１の位置ｐ、速度ｖ、姿勢φといった車両１の状態を推定し、その推定結果を外部へ出力する。
【0020】
状態推定装置１０は、プロセッサを含む制御部２０およびメモリ５０等を有するコンピュータである。メモリ５０は、各種の制御処理を実行するためのプログラム、データ等が格納されている。制御部２０は、メモリ５０に記憶された各種プログラムを実行する。
【0021】
状態推定装置１０は、制御部２０による各種プログラムの実行等によって各種の機能部として機能する。状態推定装置１０は、入力部２２、前処理部２４、演算部２６、補正部２８、および推定部３０を備える。
【0022】
入力部２２には、撮像部２および慣性計測装置３それぞれが接続されている。入力部２２は、撮像部２が出力する画像データおよび慣性計測装置３が出力する慣性データを読み込む。
【0023】
前処理部２４は、入力部２２で読み込んだ画像データの特徴点ＦＰの抽出および当該特徴点ＦＰのトラッキングを行うとともに、入力部２２で読み込んだ慣性データに基づいて車両１の位置ｐ、速度ｖ、姿勢φを算出する。
【0024】
前処理部２４は、画像データの特徴点ＦＰの抽出および当該特徴点ＦＰのトラッキングを行う画像処理部２４１を有する。画像処理部２４１は、例えば、ＳＩＦＴやＳＵＲＦ等によって局所的な特徴量に基づいて特徴点ＦＰを抽出し、最近傍探索等によって現在の画像フレームで抽出された特徴点ＦＰと前回の画像フレームで抽出された特徴点ＦＰとの対応付けを行う。なお、画像処理部２４１における特徴点ＦＰの抽出や特徴点ＦＰの対応付けは、上記したものとは異なる手段によって実現されていてもよい。
【0025】
また、前処理部２４は、慣性データに基づいて車両１の位置ｐ、速度ｖ、姿勢φを算出する慣性処理部２４２を有する。慣性処理部２４２は、例えば、ジャイロセンサ３ａのセンサ出力である角速度ωを積分して車両１の姿勢φおよび回転行列Ｃｂを求める。また、慣性処理部２４２は、加速度センサ３ｂのセンサ出力である加速度ｆと回転行列Ｃｂとを乗じたものを積分して車両１の速度ｖを求めるとともに、求めた車両１の速度ｖを積分して車両１の位置ｐを求める。慣性処理部２４２では、姿勢φとしてロール角、ピッチ角、ヨー角といった３つの姿勢角を計算によって求める。
【0026】
演算部２６は、視覚慣性オドメトリＶＩＯによって慣性計測装置３のバイアス誤差を含む各種パラメータを推定する。本実施形態の演算部２６は、画像データの特徴点ＦＰ、慣性データに基づく車両１の位置ｐ、速度ｖ、姿勢φに対して、バンドル調整という非線形な最小二乗法を実施して車両１の位置ｐ、姿勢φ、速度ｖ、慣性計測装置３のバイアス誤差を推定する。演算部２６は、ジャイロセンサ３ａのバイアス誤差および加速度センサ３ｂのバイアス誤差それぞれを慣性計測装置３のバイアス誤差として推定する。
【0027】
具体的には、演算部２６は、図４に示すように、画像に関する残差、ＩＭＵに関する残差、事前情報に関する残差をバンドル調整によって最適化することによって、車両１の位置ｐ、姿勢φ、速度ｖ、慣性計測装置３のバイアス誤差を推定する。
【0028】
演算部２６は、画像座標系と世界座標系との間における再投影誤差を画像に関する残差としてバンドル調整によって最適化する。例えば、演算部２６は、図５に示すように、ｉ番目の画像フレームでの特徴点ＦＰの位置を世界座標系に変換した後、ｊ番目の画像フレームの画像座標系に再投影した位置とｊ番目の画像フレームでの特徴点ＦＰの位置との差を画像に関する残差として最適化する。
【0029】
また、演算部２６は、例えば、慣性計測装置３での位置ｐ、姿勢φの測定結果と画像データから予測される位置ｐ、姿勢φの予測結果との差をＩＭＵに関する残差としてバンドル調整によって最適化する。慣性計測装置３における慣性データのサンプリング時間が撮像部２での画像データのサンプリング時間よりも短い。このため、例えば、演算部２６は、図６に示すように、ｉ番目とｊ番目の画像フレーム間での車両１の位置ｐ、姿勢φの変化の予測と、各画像フレーム間で取得される慣性データを積分した位置ｐ、姿勢φの測定結果との差をＩＭＵに関する残差として最適化する。
【0030】
また、演算部２６は、図７に示すように、画像データおよび慣性データの直近の情報だけでなく、事前の情報を含めてバンドル調整を実施する。演算部２６は、例えば、直近の情報から推定される車両１の位置ｐ、姿勢φと事前の情報から推定される車両１の位置ｐ、姿勢φとの差を事前情報に関する残差として最適化する。
【0031】
演算部２６では、新たな情報が追加されると、事前情報の一部を削除したり、周辺化処理したりすることで、演算処理等の負荷を抑えるようになっている。なお、視覚慣性オドメトリＶＩＯでの位置、速度ｖ、姿勢φの求め方は、例えば、以下の文献１等にも開示されている。
【0032】
［文献１］
T. Qin, P. Li and S. Shen, "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator," in IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004-1020, Aug. 2018, doi: 10.1109/TRO.2018.2853729。
T. Qin, P. Li and S. Shen, "VINS-Mono: A Robust and Versatile Monocular Visual-Inertial State Estimator," in IEEE Transactions on Robotics, vol. 34, no. 4, pp. 1004-1020, Aug. 2018, doi: 10.1109/TRO.2018.2853729。
【0033】
ここで、図８は、ジャイロセンサ３ａで測定される角速度ωおよび視覚慣性オドメトリＶＩＯで求めた姿勢φから推定される角速度ωをアラン分散で解析したものである。なお、図８では、ジャイロセンサ３ａで測定される角速度ωの解析結果を二点鎖線で示し、視覚慣性オドメトリＶＩＯで求めた姿勢φから推定される角速度ωの解析結果を一点鎖線で示している。
【0034】
図８に示すように、視覚慣性オドメトリＶＩＯによる角速度ωの推定は、ジャイロセンサ３ａでの測定結果に比べて、推定開始してから所定時間以内における誤差が大きいことが判った。この理由としては、撮像部２が出力する画像データは、モーションブラーや周囲の移動物体の影響を受け易く、画像データの解析から得られる姿勢φの短時間での誤差が大きくなってしまうことが考えられる。
【0035】
一方、視覚慣性オドメトリＶＩＯによる角速度ωの推定は、時間経過とともに誤差が小さくなる。これは、視覚慣性オドメトリＶＩＯによる角速度ωの推定には、ジャイロセンサ３ａのバイアス誤差の影響がないことが要因と考えられる。
【0036】
これらに対して、ジャイロセンサ３ａでの測定結果は、図８および図９に示すように、測定開始から或る程度時間が経過するまでは誤差が小さいが、時間経過とともに次第に誤差が大きくなる。これは、時間経過とともにバイアス誤差が累積することが要因と考えられる。
【0037】
これらの特徴を考慮し、状態推定装置１０では、視覚慣性オドメトリＶＩＯによってバイアス誤差を求め、当該バイアス誤差を慣性データから除去した補正データに基づいて、車両１の状態を推定する。本実施形態の状態推定装置１０は、慣性データからバイアス誤差を除去した補正データを求める補正部２８と、当該補正データに基づいて車両１の位置ｐ、速度ｖ、姿勢φの少なくとも１つを推定する推定部３０とを備える。
【0038】
補正部２８は、例えば、図１０に示すように、慣性計測装置３が出力する慣性データから視覚慣性オドメトリＶＩＯで得られた慣性計測装置３のバイアス誤差を減算したものを補正データとして出力する。
【0039】
推定部３０は、補正データに基づいて車両１の位置ｐ、速度ｖ、姿勢φを計算し、その計算結果を出力する。推定部３０は、補正部２８で補正した角速度ωを積分して車両１の姿勢φおよび回転行列Ｃｂを求める。また、推定部３０は、補正部２８で補正した加速度ｆと回転行列Ｃｂとを乗じたものを積分して車両１の速度ｖを求めるとともに、求めた車両１の速度ｖを積分して車両１の位置ｐを求める。
【0040】
ここで、図１１は、ジャイロセンサ３ａで測定される角速度ω、視覚慣性オドメトリＶＩＯで求めた姿勢φから推定される角速度ω、ジャイロセンサ３ａで測定される角速度ωからバイアス誤差を除去して求めた角速度ωをアラン分散で解析したものである。なお、図１１では、ジャイロセンサ３ａで測定される角速度ωの解析結果を二点鎖線で示し、視覚慣性オドメトリＶＩＯで求めた姿勢φから推定される角速度ωの解析結果を一点鎖線で示している。また、図１１では、ジャイロセンサ３ａで測定される角速度ωからバイアス誤差を除去して求めた角速度ωの解析結果を実線で示している。
【0041】
図１１に示すように、ジャイロセンサ３ａで測定される角速度ωからバイアス誤差を除去して求めた角速度ωは、視覚慣性オドメトリＶＩＯで求めた姿勢φから推定される角速度ωと異なり、推定開始直後から誤差が小さい結果となった。また、ジャイロセンサ３ａで測定される角速度ωからバイアス誤差を除去して求めた角速度ωは、ジャイロセンサ３ａの測定結果と異なり、或る程度時間が経過した後も誤差が小さい結果となった。
【0042】
以上説明した状態推定装置１０および状態推定方法は、視覚慣性オドメトリＶＩＯでバイアス誤差を求め、当該バイアス誤差を慣性データから除去した補正データに基づいて、車両１の位置ｐ、速度ｖ、姿勢φを推定する。このように、視覚慣性オドメトリＶＩＯによってバイアス誤差を推定し、当該バイアス誤差および慣性データに基づいて車両１の位置ｐ、速度ｖ、姿勢φを推定すれば、モーションブラーや周囲の移動物体等に起因する誤差の影響を抑えることができる。したがって、本開示の状態推定装置１０および状態推定方法によれば、車両１の位置ｐ、速度ｖ、姿勢φの少なくとも１つを含む状態の推定の精度向上を図ることができる。
【0043】
ここで、視覚慣性オドメトリＶＩＯは、カルマンフィルタのように過去の測定結果と現在の測定結果から逐次的に現在の状態を推定するものではなく、バンドル調整といった非線形な最小二乗法によって誤差の最小化を行うものである。バンドル調整は計算負荷が大きいものの、過去から現在に至る複数のデータを用いて、誤差が最小となる推定値を反復計算で求めるため、精度が高いことが特徴である。特に、外乱ノイズに対する耐性や非線形な関数を用いた状態推定において、バンドル調整は、カルマンフィルタより優れた性能を有する。本開示の状態推定装置１０および状態推定方法は、視覚慣性オドメトリＶＩＯによって高精度に推定されたバイアス誤差を用いているので、慣性データを適切に補正することができる。このことは、車両１の位置ｐ、速度ｖ、姿勢φの少なくとも１つを含む状態の推定の精度向上を図る上で有効である。
【0044】
（第２実施形態）
次に、第２実施形態について、図１２を参照して説明する。本実施形態では、第１実施形態と異なる部分について主に説明する。
次に、第２実施形態について、図１２を参照して説明する。本実施形態では、第１実施形態と異なる部分について主に説明する。
【0045】
第１実施形態の如く、撮像部２が単眼カメラで構成される場合は、複眼カメラを用いる場合に比較して、スケール推定の誤差が大きくなる。この誤差が大きいと、慣性データに含まれる加速度ｆのバイアス誤差の推定精度が低下することがあり、バイアス誤差を慣性データに含まれる加速度ｆから除去したものを積分して車両１の速度ｖや位置ｐを推定しても、充分な精度向上の効果が得られない虞がある。
【0046】
このことを考慮し、本実施形態の状態推定装置１０は、図１２に示すように、バイアス誤差を慣性データから除去したものだけでなく、車輪速センサ４のセンサ出力を用いて、車両１の位置ｐおよび速度ｖを求める構成になっている。車輪速センサ４は、例えば、ロータリエンコーダで構成される。車輪速センサ４は、車両１の車輪に回転数に応じた信号をセンサ出力として外部に出力する。
【0047】
推定部３０は、車輪速センサ４のセンサ出力を読み込むことができるように、車輪速センサ４の直接的または間接的に接続されている。推定部３０は、補正部２８で求められた補正データおよび車輪速センサ４のセンサ出力にも基づいて、車両１の速度ｖおよび位置ｐを推定する。
【0048】
具体的には、推定部３０は、補正部２８で補正した角速度ωを積分して車両１の姿勢φおよび回転行列Ｃｂを求める。そして、推定部３０は、補正部２８で補正した加速度ｆではなく、車輪速センサ４の出力から推定した車両１の加速度ｆと回転行列Ｃｂとを乗じたものを積分して車両１の速度ｖを求めるとともに、求めた車両１の速度ｖを積分して車両１の位置ｐを求める。
【0049】
その他については、第１実施形態と同様である。本実施形態の状態推定装置１０および状態推定方法は、第１実施形態と共通の構成または均等な構成から奏される効果を第１実施形態と同様に得ることができる。
【0050】
また、本実施形態の状態推定装置１０は、以下の特徴を備える。
【0051】
（１）状態推定装置１０の推定部３０は、補正データおよび車両１に設置された車輪速センサ４のセンサ出力に基づいて、車両１の速度ｖを求め、求めた車両１の速度ｖに基づいて車両１の位置ｐを推定する。これによれば、撮像部２が単眼カメラで構成される場合であっても、車両１の速度ｖおよび位置ｐを充分な精度で推定することができる。本案の構成は、撮像部２に単眼カメラを用いる構成やカメラのスケール推定の誤差が小さくすること難しい構成に好適である。
【0052】
【0053】
慣性計測装置３では、ＭＥＭＳ等のセンサ構造が、ジャイロセンサ３ａより加速度センサ３ｂの方が簡易な構造になっており、ジャイロセンサ３ａのバイアス変化に比べると、加速度センサ３ｂのバイアス変化が小さくなる傾向がある。このような背景もあり、視覚慣性オドメトリＶＩＯで推定するバイアス誤差は、ジャイロセンサ３ａの方が加速度センサ３ｂよりも精度が低い傾向がある。
【0054】
これらを踏まえ、本実施形態の状態推定装置１０では、図１３に示すように、視覚慣性オドメトリＶＩＯで求めたバイアス誤差を用いて慣性データを補正した補正データに加えて、車輪速センサ４のセンサ出力を用いて車両１の姿勢φを推定する。
【0055】
図１４に示すように、推定部３０は、視覚慣性オドメトリＶＩＯでジャイロセンサ３ａのバイアス誤差を求めるとともに、当該バイアス誤差を用いてジャイロセンサ３ａのセンサ出力を補正したものに基づいて車両１の姿勢φを示す第１姿勢角φ１を求める。
【0056】
また、推定部３０は、視覚慣性オドメトリＶＩＯで加速度センサ３ｂのバイアス誤差を求める。そして、推定部３０は、加速度センサ３ｂのバイアス誤差を用いて加速度センサ３ｂのセンサ出力を補正したものおよび車輪速センサ４のセンサ出力から求めた重力加速度に基づいて車両１の姿勢φを示す第２姿勢角φ２を求める。
【0057】
具体的には、推定部３０は、車輪速センサ４のセンサ出力の微分値を用いて加速度センサ３ｂのセンサ出力から並進加速度を除去し、重力加速度のみを抽出してから図１５に示す姿勢角の演算を行って、車両１の姿勢φを示す第２姿勢角φ２を求める。
【0058】
ここで、車輪速センサ４は量子化雑音が大きいため、ローパスフィルタで帯域を制限して用いることが好ましい。例えば、推定部３０で求めた第２姿勢角φ２は、移動平均フィルタで平滑化して低周波成分のみを使用することが好ましい。この場合、高周波成分が不足することになるが、不足する高周波成分は、第１姿勢角φ１を用いればよい。
【0059】
これらを加味して、推定部３０は、第１姿勢角φ１を相補フィルタのハイパスフィルタを通し、第２姿勢角φ２を相補フィルタのローパスフィルタを通した後に両者を合成することで、車両１の姿勢φを推定する。相補フィルタは、ローパスフィルタとハイパスフィルタのカットオフ周波数の次数が一致していることが望ましい。
【0060】
その他については、これまで説明した実施形態と同様である。本実施形態の状態推定装置１０および状態推定方法は、これまで実施形態と共通の構成または均等な構成から奏される効果をこれまでと同様に得ることができる。
【0061】
また、本実施形態の状態推定装置１０は、以下の特徴を備える。
【0062】
（１）推定部３０は、第１姿勢角φ１を相補フィルタのハイパスフィルタを通し、第２姿勢角φ２を相補フィルタのローパスフィルタを通した後に両者を合成することで、車両１の姿勢φを推定する。このように、ジャイロセンサ３ａのセンサ出力から推定した第１姿勢角φ１に加えて、加速度センサ３ｂのセンサ出力から推定した第２姿勢角φ２を用いて車両１の姿勢φを推定する構成になっていれば、車両１の姿勢φの推定の充分な精度向上を図ることができる。
【0063】
【0064】
車両１の周囲の環境変化（例えば、逆光、トンネル等）がある場合、撮像部２が所期の機能を発揮することが困難となる。このような異常な状況が生ずると、例えば、図１６に示すように、視覚慣性オドメトリＶＩＯによるバイアス誤差の推定が安定せず、車両１の位置ｐ、速度ｖ、姿勢φを高精度に推定することが難しくなる。
【0065】
このことを鑑み、状態推定装置１０には、図１７に示すように、撮像部２が所期の機能を発揮することが困難な異常な状況であるか否かを判定する異常判定部３１が備えられている。
【0066】
異常判定部３１は、例えば、撮像部２が出力する画像データに基づいて、撮像部２が特徴点ＦＰの抽出等の所期の機能を発揮できるか否かを判定する。異常判定部３１は、撮像部２が所期の機能を発揮できる場合は、異常な状況でないと判定する。異常判定部３１は、撮像部２が所期の機能を発揮できない場合は、異常な状況と判定する。
【0067】
本実施形態の補正部２８は、異常判定部３１の判定結果を考慮して、慣性データの補正データを算出する。具体的には、補正部２８は、撮像部２が所期の機能を発揮できる正常な状況である場合には、視覚慣性オドメトリＶＩＯで求めたバイアス誤差を慣性データから除去することで補正データを算出する。
【0068】
一方、補正部２８は、撮像部２が所期の機能を発揮できない異常な状況でない場合には、視覚慣性オドメトリＶＩＯで求めたバイアス誤差に代えて、事前にメモリ５０に記憶されていたバイアス推定値を慣性データから除去することで補正データを算出する。
【0069】
ここで、バイアス誤差は、応力、温度等に依存して変化するが、これらは短時間（例えば、１０秒程度）であまり変化しない。このことから、例えば、補正部２８は、撮像部２が所期の機能を発揮できない異常な状況になる直前に視覚慣性オドメトリＶＩＯで求めたバイアス誤差を、バイアス推定値としてメモリ５０に記憶するようになっていることが望ましい。
【0070】
その他については、これまで説明した実施形態と同様である。本実施形態の状態推定装置１０および状態推定方法は、これまで実施形態と共通の構成または均等な構成から奏される効果をこれまでと同様に得ることができる。
【0071】
また、本実施形態の状態推定装置１０は、以下の特徴を備える。
【0072】
（１）状態推定装置１０は、撮像部２が所期の機能を発揮することが困難な異常な状況であるか否かを判定する異常判定部３１を備える。補正部２８は、異常判定部３１の判定が異常な状況でないことを示す場合は慣性データからバイアス誤差を除去することで補正データを算出する。また、補正部２８は、異常判定部３１の判定が異常な状況を示す場合は、演算部２６で求めたバイアス誤差に代えて、事前にメモリ５０に記憶していたバイアス推定値を慣性データから除去することで補正データを算出する。これによれば、撮像部２が所期の機能を発揮することができない異常な状況になったとしても、例えば、図１８に示すように、車両１の状態の推定を適切に継続することができる。
【0073】
（２）異常判定部３１は、撮像部２が出力する画像データに基づいて異常な状況であるか否かを判定する。これによると、撮像部２の異常判定専用のセンサ機器を追加する必要がないので、簡易な形態で車両１の状態の推定を継続することができる。
【0074】
（第５実施形態）
次に、第５実施形態について、図１９を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
次に、第５実施形態について、図１９を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
【0075】
撮像部２が所期の機能を発揮することが困難な異常な状況では、前処理部２４での画像データの特徴点ＦＰの抽出ができなくなったり、当該特徴点ＦＰのトラッキングができなくなったりする可能性が高い。
【0076】
このことを鑑み、本実施形態の異常判定部３１は、撮像部２から画像データを取得するのではなく、図１９に示すように、前処理部２４から画像データの解析結果を取得し、当該解析結果に基づいて異常な状況であるか否かを判定する。例えば、異常判定部３１は、前処理部２４での画像データの特徴点ＦＰの抽出が全くできなくなったり、当該特徴点ＦＰのトラッキングが突然できなくなったりした場合に、異常な状況と判定する。
【0077】
その他については、第４実施形態と同様である。本実施形態の状態推定装置１０および状態推定方法は、第４実施形態と共通の構成または均等な構成から奏される効果をこれまでと同様に得ることができる。
【0078】
（第６実施形態）
次に、第６実施形態について、図２０を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
次に、第６実施形態について、図２０を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
【0079】
撮像部２が所期の機能を発揮することが困難な異常な状況では、視覚慣性オドメトリＶＩＯによる車両１の姿勢φの推定精度が低下し易い。逆に考えると、視覚慣性オドメトリＶＩＯによる車両１の姿勢φの推定精度が低下している場合、撮像部２が所期の機能を発揮することが困難な異常な状況になっている可能性がある。
【0080】
このこと鑑み、異常判定部３１は、図２０に示すように、車両１に設置された操舵角センサ５のセンサ出力および演算部２６で求めた車両１の姿勢φに基づいて、撮像部２が所期の機能を発揮することが困難な異常な状況であるか否かを判定する。例えば、異常判定部３１は、操舵角センサ５のセンサ出力から求めた車両１の姿勢φと演算部２６で求めた車両１の姿勢φとが基準値を超えて乖離している場合に、異常な状況であると判定する。
【0081】
その他については、第４実施形態と同様である。本実施形態の状態推定装置１０および状態推定方法は、第４実施形態と共通の構成または均等な構成から奏される効果をこれまでと同様に得ることができる。
【0082】
また、本実施形態の状態推定装置１０は、以下の特徴を備える。
【0083】
（１）異常判定部３１は、操舵角センサ５のセンサ出力および演算部２６で求めた車両１の姿勢φに基づいて、撮像部２が所期の機能を発揮することが困難な異常な状況であるか否かを判定する。このように、車両１に既設の操舵角センサ５のセンサ出力を利用して異常な状況であるか否かを判定する構成とすれば、撮像部２の異常判定専用のセンサ機器を追加する必要がないので、簡易な形態で車両１の状態の推定を継続することができる。
【0084】
（第７実施形態）
次に、第７実施形態について、図２１を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
次に、第７実施形態について、図２１を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
【0085】
撮像部２が所期の機能を発揮することが困難な異常な状況では、視覚慣性オドメトリＶＩＯによる速度ｖの推定精度が低下し易い。逆に考えると、視覚慣性オドメトリＶＩＯによる車両１の速度ｖの推定精度が低下している場合、撮像部２が所期の機能を発揮することが困難な異常な状況になっている可能性がある。
【0086】
このこと鑑み、異常判定部３１は、図２１に示すように、車両１に設置された車輪速センサ４のセンサ出力および演算部２６にて求めた車両１の速度ｖに基づいて、撮像部２が所期の機能を発揮することが困難な異常な状況であるか否かを判定する。例えば、異常判定部３１は、車輪速センサ４のセンサ出力から求めた車両１の速度ｖと演算部２６で求めた車両１の速度ｖとが基準値を超えて乖離している場合に、異常な状況であると判定する。
【0087】
その他については、第４実施形態と同様である。本実施形態の状態推定装置１０および状態推定方法は、第４実施形態と共通の構成または均等な構成から奏される効果をこれまでと同様に得ることができる。
【0088】
また、本実施形態の状態推定装置１０は、以下の特徴を備える。
【0089】
（１）異常判定部３１は、車輪速センサ４のセンサ出力および演算部２６で求めた車両１の速度ｖに基づいて、撮像部２が所期の機能を発揮することが困難な異常な状況であるか否かを判定する。このように、車両１に既設の車輪速センサ４のセンサ出力を利用して異常な状況であるか否かを判定する構成とすれば、撮像部２の異常判定専用のセンサ機器を追加する必要がないので、簡易な形態で車両１の状態の推定を継続することができる。
【0090】
（第８実施形態）
次に、第８実施形態について、図２２を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
次に、第８実施形態について、図２２を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
【0091】
撮像部２が所期の機能を発揮することが困難な異常な状況では、視覚慣性オドメトリＶＩＯによる位置ｐ、速度ｖ、姿勢φの推定精度が低下し易い。このため、撮像部２が所期の機能を発揮することが困難な異常な状況では、演算部２６で求める車両１の位置ｐ、速度ｖ、姿勢φと推定部３０で推定する車両１の位置ｐ、速度ｖ、姿勢φとが乖離し易い。
【0092】
このこと鑑み、異常判定部３１は、図２２に示すように、演算部２６で求めた車両１の位置ｐ、速度ｖ、姿勢φおよび推定部３０で推定した位置ｐ、速度ｖ、姿勢φに基づいて、撮像部２が所期の機能を発揮することが困難な異常な状況であるか否かを判定する。例えば、異常判定部３１は、演算部２６にて求めた車両１の位置ｐ、速度ｖ、姿勢φの少なくとも１つが推定部３０で推定した車両１の位置ｐ、速度ｖ、姿勢φに対して基準値を超えて乖離している場合に、異常な状況であると判定する。
【0093】
その他については、第４実施形態と同様である。本実施形態の状態推定装置１０および状態推定方法は、第４実施形態と共通の構成または均等な構成から奏される効果をこれまでと同様に得ることができる。
【0094】
また、本実施形態の状態推定装置１０は、以下の特徴を備える。
【0095】
（１）異常判定部３１は、演算部２６で求めた車両１の位置ｐ、速度ｖ、姿勢φおよび推定部３０で推定した車両１の位置ｐ、速度ｖ、姿勢φに基づいて、撮像部２が所期の機能を発揮することが困難な異常な状況であるか否かを判定する。これによれば、撮像部２の異常判定専用のセンサ機器を追加する必要がないので、簡易な形態で車両１の状態の推定を継続することができる。
【0096】
（第９実施形態）
次に、第９実施形態について、図２３を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
次に、第９実施形態について、図２３を参照して説明する。本実施形態では、第４実施形態と異なる部分について主に説明する。
【0097】
慣性計測装置３のバイアス誤差は、慣性計測装置３の温度に応じて変化する特性を有する。このため、補正部２８にて補正データを算出する際に用いるバイアス推定値は、固定値ではなく、慣性計測装置３の温度に応じて変化する可変値であることが望ましい。
【0098】
このことを考慮し、補正部２８は、図２３に示すように、慣性計測装置３の温度の計測結果に応じてバイアス推定値を補正し、補正後のバイアス推定値を慣性データから除去することで補正データを算出する。例えば、慣性計測装置３の温度が上昇するとバイアス誤差が大きくなり易い場合は、補正部２８は、慣性計測装置３の温度の上昇時に、メモリ５０に記憶されたバイアス推定値に所定値を加算して補正する。
【0099】
ここで、慣性計測装置３の温度を計測する手段は、慣性計測装置３に付加した温度センサ６でもよいし、外気温および慣性計測装置３の使用状況からの推定でもよい。また、バイアス推定値の補正は、上記したものに限定されず、他の方法で実施されていてもよい。
【0100】
その他については、第４実施形態と同様である。本実施形態の状態推定装置１０および状態推定方法は、第４実施形態と共通の構成または均等な構成から奏される効果をこれまでと同様に得ることができる。
【0101】
また、本実施形態の状態推定装置１０は、以下の特徴を備える。
【0102】
（１）補正部２８は、慣性計測装置３の温度に応じてバイアス推定値を補正し、補正後のバイアス推定値を慣性データから除去することで補正データを算出する。これによると、撮像部２が所期の機能を発揮することができない異常な状況になったとしても、車両１の状態の推定を適切な形態で継続することができる。
【0103】
（他の実施形態）
以上、本開示の代表的な実施形態について説明したが、本開示は、上述の実施形態に限定されることなく、例えば、以下のように種々変形可能である。
以上、本開示の代表的な実施形態について説明したが、本開示は、上述の実施形態に限定されることなく、例えば、以下のように種々変形可能である。
【0104】
上述の実施形態の状態推定装置１０は、車両１に適用した例について説明したが、これに限定されない。状態推定装置１０は、車両１以外の移動体に適用可能である。
【0105】
上述の実施形態の状態推定装置１０は、車両１の位置ｐ、速度ｖ、姿勢φそれぞれを推定するものを例示したが、これに限定されない。状態推定装置１０は、車両１の位置ｐ、速度ｖ、姿勢φの一部を含む状態を推定するようになっていてもよい。
【0106】
上述の実施形態において、実施形態を構成する要素は、特に必須であると明示した場合および原理的に明らかに必須であると考えられる場合等を除き、必ずしも必須のものではないことは言うまでもない。
【0107】
上述の実施形態において、実施形態の構成要素の個数、数値、量、範囲等の数値が言及されている場合、特に必須であると明示した場合および原理的に明らかに特定の数に限定される場合等を除き、その特定の数に限定されない。
【0108】
上述の実施形態において、構成要素等の形状、位置関係等に言及するときは、特に明示した場合および原理的に特定の形状、位置関係等に限定される場合等を除き、その形状、位置関係等に限定されない。
【0109】
本開示の制御部及びその手法は、コンピュータプログラムにより具体化された一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリを構成することによって提供された専用コンピュータで、実現されてもよい。本開示の制御部及びその手法は、一つ以上の専用ハードウエア論理回路によってプロセッサを構成することによって提供された専用コンピュータで、実現されてもよい。本開示の制御部及びその手法は、一つ乃至は複数の機能を実行するようにプログラムされたプロセッサ及びメモリと一つ以上のハードウエア論理回路によって構成されたプロセッサとの組み合わせで構成された一つ以上の専用コンピュータで、実現されてもよい。また、コンピュータプログラムは、コンピュータにより実行されるインストラクションとして、コンピュータ読み取り可能な非遷移有形記録媒体に記憶されていてもよい。
【0110】
（本開示の特徴）
［開示１］
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定装置であって、
前記移動体の周囲を撮像する撮像部（２）が出力する画像データおよび前記移動体に設置された慣性計測装置（３）が出力する前記移動体の慣性データを読み込む入力部（２２）と、
前記画像データに含まれる特徴点の抽出および前記特徴点のトラッキングを行うとともに、前記慣性データに基づいて前記移動体の位置、速度、姿勢を算出する前処理部（２４）と、
前記画像データの特徴点、前記慣性データに基づく前記移動体の位置、速度、姿勢に対してバンドル調整を実施して前記慣性計測装置のバイアス誤差を求める演算部（２６）と、
前記慣性データから前記バイアス誤差を除去した補正データを求める補正部（２８）と、
前記補正データに基づいて前記移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定する推定部（３０）と、
を備える状態推定装置。
［開示１］
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定装置であって、
前記移動体の周囲を撮像する撮像部（２）が出力する画像データおよび前記移動体に設置された慣性計測装置（３）が出力する前記移動体の慣性データを読み込む入力部（２２）と、
前記画像データに含まれる特徴点の抽出および前記特徴点のトラッキングを行うとともに、前記慣性データに基づいて前記移動体の位置、速度、姿勢を算出する前処理部（２４）と、
前記画像データの特徴点、前記慣性データに基づく前記移動体の位置、速度、姿勢に対してバンドル調整を実施して前記慣性計測装置のバイアス誤差を求める演算部（２６）と、
前記慣性データから前記バイアス誤差を除去した補正データを求める補正部（２８）と、
前記補正データに基づいて前記移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定する推定部（３０）と、
を備える状態推定装置。
【0111】
［開示２］
前記推定部は、前記補正データおよび前記移動体に設置された車輪速センサ（４）のセンサ出力に基づいて、前記移動体の速度を求め、求めた前記移動体の速度に基づいて前記移動体の位置を推定する、開示１に記載の状態推定装置。
前記推定部は、前記補正データおよび前記移動体に設置された車輪速センサ（４）のセンサ出力に基づいて、前記移動体の速度を求め、求めた前記移動体の速度に基づいて前記移動体の位置を推定する、開示１に記載の状態推定装置。
【0112】
［開示３］
前記慣性計測装置は、前記移動体の角速度を検出するジャイロセンサ（３ａ）および前記移動体の加速度を検出する加速度センサ（３ｂ）を含み、
前記推定部は、
前記ジャイロセンサのセンサ出力を前記補正部で補正したものに基づいて前記移動体の姿勢を示す第１姿勢角を求め、
前記加速度センサのセンサ出力を前記補正部で補正したものおよび前記移動体の車輪速センサのセンサ出力から求めた重力加速度に基づいて前記移動体の姿勢を示す第２姿勢角を求め、
前記第１姿勢角を相補フィルタのハイパスフィルタを通し、前記第２姿勢角を前記相補フィルタのローパスフィルタを通した後に両者を合成することで、前記移動体の姿勢を推定する、開示１または２に記載の状態推定装置。
前記慣性計測装置は、前記移動体の角速度を検出するジャイロセンサ（３ａ）および前記移動体の加速度を検出する加速度センサ（３ｂ）を含み、
前記推定部は、
前記ジャイロセンサのセンサ出力を前記補正部で補正したものに基づいて前記移動体の姿勢を示す第１姿勢角を求め、
前記加速度センサのセンサ出力を前記補正部で補正したものおよび前記移動体の車輪速センサのセンサ出力から求めた重力加速度に基づいて前記移動体の姿勢を示す第２姿勢角を求め、
前記第１姿勢角を相補フィルタのハイパスフィルタを通し、前記第２姿勢角を前記相補フィルタのローパスフィルタを通した後に両者を合成することで、前記移動体の姿勢を推定する、開示１または２に記載の状態推定装置。
【0113】
［開示４］
前記撮像部が所期の機能を発揮することが困難な異常な状況であるか否かを判定する異常判定部（３１）を備え、
前記補正部は、
前記異常な状況でない場合は前記慣性データから前記バイアス誤差を除去することで前記補正データを算出し、
前記異常な状況になると、前記演算部で求めた前記バイアス誤差に代えて、事前にメモリに記憶していたバイアス推定値を前記慣性データから除去することで前記補正データを算出する、開示１ないし３のいずれか１つに記載の状態推定装置。
前記撮像部が所期の機能を発揮することが困難な異常な状況であるか否かを判定する異常判定部（３１）を備え、
前記補正部は、
前記異常な状況でない場合は前記慣性データから前記バイアス誤差を除去することで前記補正データを算出し、
前記異常な状況になると、前記演算部で求めた前記バイアス誤差に代えて、事前にメモリに記憶していたバイアス推定値を前記慣性データから除去することで前記補正データを算出する、開示１ないし３のいずれか１つに記載の状態推定装置。
【0114】
［開示５］
前記異常判定部は、前記画像データに基づいて前記異常な状況であるか否かを判定する、開示４に記載の状態推定装置。
前記異常判定部は、前記画像データに基づいて前記異常な状況であるか否かを判定する、開示４に記載の状態推定装置。
【0115】
［開示６］
前記異常判定部は、前記移動体に設置された操舵角センサ（５）のセンサ出力および前記演算部で求めた前記移動体の姿勢に基づいて前記異常な状況であるか否かを判定する、開示４または５に記載の状態推定装置。
前記異常判定部は、前記移動体に設置された操舵角センサ（５）のセンサ出力および前記演算部で求めた前記移動体の姿勢に基づいて前記異常な状況であるか否かを判定する、開示４または５に記載の状態推定装置。
【0116】
［開示７］
前記異常判定部は、前記移動体に設置された車輪速センサ（４）のセンサ出力および前記演算部で求めた前記移動体の速度に基づいて前記異常な状況であるか否かを判定する、開示４ないし６のいずれか１つに記載の状態推定装置。
前記異常判定部は、前記移動体に設置された車輪速センサ（４）のセンサ出力および前記演算部で求めた前記移動体の速度に基づいて前記異常な状況であるか否かを判定する、開示４ないし６のいずれか１つに記載の状態推定装置。
【0117】
［開示８］
前記異常判定部は、前記演算部で求めた前記移動体の位置、速度、姿勢および前記推定部で推定した前記移動体の位置、速度、姿勢に基づいて前記異常な状況であるか否かを判定する、開示４ないし７のいずれか１つに記載の状態推定装置。
前記異常判定部は、前記演算部で求めた前記移動体の位置、速度、姿勢および前記推定部で推定した前記移動体の位置、速度、姿勢に基づいて前記異常な状況であるか否かを判定する、開示４ないし７のいずれか１つに記載の状態推定装置。
【0118】
［開示９］
前記補正部は、前記慣性計測装置の温度に応じて前記バイアス推定値を補正し、補正後の前記バイアス推定値を前記慣性データから除去することで前記補正データを算出する、開示４ないし８のいずれか１つに記載の状態推定装置。
前記補正部は、前記慣性計測装置の温度に応じて前記バイアス推定値を補正し、補正後の前記バイアス推定値を前記慣性データから除去することで前記補正データを算出する、開示４ないし８のいずれか１つに記載の状態推定装置。
【0119】
［開示１０］
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定方法であって、
前記移動体の周囲を撮像する撮像部（２）が出力する画像データおよび前記移動体に設置された慣性計測装置（３）が出力する前記移動体の慣性データを読み込むことと、
前記画像データに含まれる特徴点の抽出および前記特徴点のトラッキングを行うとともに、前記慣性データに基づいて前記移動体の位置、速度、姿勢を算出することと、
前記画像データの特徴点、前記慣性データに基づく前記移動体の位置、速度、姿勢に対してバンドル調整を実施して前記慣性計測装置のバイアス誤差を求めることと、
前記慣性データから前記バイアス誤差を除去した補正データを求めることと、
前記補正データに基づいて前記移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定することと、
を含む状態推定方法。
移動体（１）の位置、速度、姿勢の少なくとも１つを含む状態を推定する状態推定方法であって、
前記移動体の周囲を撮像する撮像部（２）が出力する画像データおよび前記移動体に設置された慣性計測装置（３）が出力する前記移動体の慣性データを読み込むことと、
前記画像データに含まれる特徴点の抽出および前記特徴点のトラッキングを行うとともに、前記慣性データに基づいて前記移動体の位置、速度、姿勢を算出することと、
前記画像データの特徴点、前記慣性データに基づく前記移動体の位置、速度、姿勢に対してバンドル調整を実施して前記慣性計測装置のバイアス誤差を求めることと、
前記慣性データから前記バイアス誤差を除去した補正データを求めることと、
前記補正データに基づいて前記移動体の位置、速度、姿勢の少なくとも１つを含む状態を推定することと、
を含む状態推定方法。
【符号の説明】
【0120】
１ 車両（移動体）
２ 撮像部
３ 慣性計測装置
１０ 状態推定装置
２２ 入力部
２４ 前処理部
２６ 演算部
２８ 補正部
３０ 推定部
２ 撮像部
３ 慣性計測装置
１０ 状態推定装置
２２ 入力部
２４ 前処理部
２６ 演算部
２８ 補正部
３０ 推定部
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
【図11】
【図12】
【図13】
【図14】
【図15】
【図16】
【図17】
【図18】
【図19】
【図20】
【図21】
【図22】
【図23】