特許 7332403 位置推定装置、移動体制御システム、位置推定方法およびプログラム

(19)【発行国】日本国特許庁(JP)

(12)【公報種別】特許公報(B2)

(11)【特許番号】

(24)【登録日】2023-08-15

(45)【発行日】2023-08-23

(54)【発明の名称】位置推定装置、移動体制御システム、位置推定方法およびプログラム

(51)【国際特許分類】

   G01C 21/28 20060101AFI20230816BHJP

   G05D 1/02 20200101ALI20230816BHJP

   G08G 1/00 20060101ALI20230816BHJP

【ＦＩ】

G01C21/28

G05D1/02 J

G08G1/00 X

【請求項の数】 20

(21)【出願番号】P 2019165633

(22)【出願日】2019-09-11

(65)【公開番号】P2021043082

(43)【公開日】2021-03-18

【審査請求日】2022-03-14

(73)【特許権者】

【識別番号】000003078

【氏名又は名称】株式会社東芝

(73)【特許権者】

【識別番号】317011920

【氏名又は名称】東芝デバイス＆ストレージ株式会社

(74)【代理人】

【識別番号】110002147

【氏名又は名称】弁理士法人酒井国際特許事務所

(72)【発明者】

【氏名】中島 諒

(72)【発明者】

【氏名】関 晃仁

【審査官】白石 剛史

(56)【参考文献】

【文献】特開２０１７－２１５１９３（ＪＰ，Ａ）

【文献】特開２０１１－００８３８５（ＪＰ，Ａ）

(58)【調査した分野】(Int.Cl.，ＤＢ名)

Ｇ０１Ｃ ２１／２８

Ｇ０５Ｄ １／０２

Ｇ０８Ｇ １／００

(57)【特許請求の範囲】

【請求項1】

移動に関する情報を観測する１または複数のセンサおよび周囲を撮像する撮像装置が設けられた移動体装置の自己位置を推定する位置推定装置であって、
前記１または複数のセンサのそれぞれの観測値に基づき、前記１または複数のセンサのそれぞれの状態を表す状態量を推定する状態量推定部と、
前記１または複数のセンサのそれぞれの前記状態量の信頼の度合いを表す信頼度を算出する信頼度算出部と、
前記１または複数のセンサのうち前記信頼度が予め定められた閾値以上の前記１または複数のセンサを、１または複数の対象センサとして選択し、対象時刻において前記撮像装置により撮像された撮像画像と、前記対象時刻より前の過去時刻における前記撮像画像と、前記過去時刻における前記自己位置と、選択した前記１または複数の対象センサのそれぞれの前記観測値および前記状態量とに基づき、前記対象時刻における前記自己位置を推定する自己位置推定部と、
を備え、
前記１または複数のセンサのそれぞれについて、前記状態量推定部は、所定の関係式に対して、１または複数の前記過去時刻のそれぞれにおける前記自己位置を代入した場合における残差を最小とするような、前記状態量を推定し、
前記所定の関係式は、前記過去時刻における前記自己位置、対応するセンサの前記観測値および前記対応するセンサの前記状態量、および、前記過去時刻と前記対象時刻との時間差に基づき、前記対象時刻における前記自己位置を算出する運動方程式である
位置推定装置。

【請求項2】

前記自己位置は、回転成分を含み、
前記１または複数のセンサは、角速度センサを含み、
前記角速度センサは、前記対象時刻における前記移動体装置の角速度を測定し、
前記状態量推定部は、前記角速度センサの前記状態量として、前記運動方程式における回転成分を表す回転成分運動方程式に対して、前記過去時刻における前記自己位置の回転成分を代入した場合における第１残差を最小とするような、角速度バイアスを推定し、
前記回転成分運動方程式は、下記の式（１）により表され、
Ｒ（ｔ＋Δｔ）＝Ｒ（ｔ）×Ｅｘｐ（（ω（ｔ）－ｂ _ｇ ）×Δｔ）…（１）
Ｒ（ｔ＋Δｔ）は、前記対象時刻における前記自己位置の回転成分を表し、
Ｒ（ｔ）は、前記過去時刻における前記自己位置の回転成分を表し、
ω（ｔ）は、測定された前記角速度を表し、
ｂ _ｇ は、前記角速度バイアスを表し、
Δｔは、前記過去時刻と前記対象時刻との時間差を表す
請求項１に記載の位置推定装置。

【請求項3】

前記自己位置は、並進成分を含み、
前記１または複数のセンサは、加速度センサを含み、
前記加速度センサは、前記対象時刻における前記移動体装置の加速度を測定し、
前記状態量推定部は、前記加速度センサの前記状態量として、前記運動方程式における並進成分を表す並進成分運動方程式に対して、前記過去時刻における前記自己位置の並進成分を代入した場合における第２残差を最小とするような、前記移動体装置の速度、重力加速度、および、前記移動体装置の加速度バイアスを推定し、
前記並進成分運動方程式は、下記の式（２）および式（３）により表され、
ｐ（ｔ＋Δｔ）＝ｐ（ｔ）＋ｖ（ｔ）×Δｔ＋（１／２）×ｇ×Δｔ ^２ ＋（１／２）×Ｒ（ｔ）×（ａ（ｔ）－ｂ _ａ ）×Δｔ ^２ …（２）
ｖ（ｔ＋Δｔ）＝ｖ（ｔ）＋ｇ×Δｔ＋Ｒ（ｔ）×（ａ（ｔ）－ｂ _ａ ）×Δｔ…（３）
ｐ（ｔ＋Δｔ）は、前記対象時刻における前記自己位置の並進成分を表し、
ｐ（ｔ）は、前記過去時刻における前記自己位置の並進成分を表し、
ｖ（ｔ）は、前記速度を表し、
Δｔは、前記過去時刻と前記対象時刻との時間差を表し、
ｇは、前記重力加速度を表し、
ａ（ｔ）は、測定された前記加速度を表し、
ｂ _ａ は、前記加速度バイアスを表し、
ｖ（ｔ＋Δｔ）は、前記対象時刻における前記速度を表す
請求項１または２に記載の位置推定装置。

【請求項4】

前記自己位置推定部は、
前記１または複数のセンサの中に、前記信頼度が前記閾値以上の前記１または複数の対象センサが存在しない場合、前記対象時刻における前記撮像画像と、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する
請求項１に記載の位置推定装置。

【請求項5】

前記状態量推定部は、非線形最小二乗法により前記状態量を推定し、
前記信頼度算出部は、
推定された前記状態量における異なる複数の成分のうちの２つの成分の組毎の共分散を含む共分散行列を取得し、
取得した前記共分散行列の対角成分の和の逆数を前記信頼度として算出する
請求項１から４の何れか１項に記載の位置推定装置。

【請求項6】

前記移動体装置は、前記状態量に相当する観測値を観測する第２センサをさらに備え、
前記信頼度算出部は、前記第２センサの観測値と、推定した前記状態量とを比較して、前記信頼度を算出する
請求項１から４の何れか１項に記載の位置推定装置。

【請求項7】

移動に関する情報を観測する１または複数のセンサおよび周囲を撮像する撮像装置が設けられた移動体装置の自己位置を推定する位置推定装置であって、
前記１または複数のセンサのそれぞれの観測値に基づき、前記１または複数のセンサのそれぞれの状態を表す状態量を推定する状態量推定部と、
前記１または複数のセンサのそれぞれの前記状態量の信頼の度合いを表す信頼度を算出する信頼度算出部と、
前記１または複数のセンサのうち前記信頼度が予め定められた閾値以上の前記１または複数のセンサを、１または複数の対象センサとして選択し、対象時刻において前記撮像装置により撮像された撮像画像と、前記対象時刻より前の過去時刻における前記撮像画像と、前記過去時刻における前記自己位置と、選択した前記１または複数の対象センサのそれぞれの前記観測値および前記状態量に基づき、前記対象時刻における前記自己位置を推定する自己位置推定部と、
を備え、
前記自己位置は、並進成分および回転成分を含み、
前記１または複数のセンサは、角速度センサと、加速度センサとを含み、
前記角速度センサは、前記対象時刻における前記移動体装置の角速度を測定し、
前記加速度センサは、前記対象時刻における前記移動体装置の加速度を測定し、
前記状態量推定部は、
前記角速度センサの前記状態量として、前記移動体装置の角速度バイアスを推定し、
前記加速度センサの前記状態量として、前記移動体装置の速度、重力加速度、および、前記移動体装置の加速度バイアスを推定し、
前記自己位置推定部は、
前記対象時刻における前記撮像画像と、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第１方式、
前記対象時刻における前記撮像画像と、測定した前記角速度と、推定した前記角速度バイアスと、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第２方式、または、
前記対象時刻における前記撮像画像と、測定した前記角速度と、推定した前記角速度バイアスと、測定した前記加速度と、推定した前記速度と、推定した前記重力加速度と、推定した前記加速度バイアスと、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第３方式
に基づき前記対象時刻における前記自己位置を推定し、
前記自己位置推定部は、
前記角速度バイアスの前記信頼度が前記閾値以上ではない場合、前記第１方式を選択し、
前記角速度バイアスの前記信頼度が前記閾値以上であり、前記加速度バイアスの前記信頼度が前記閾値以上ではない場合、前記第２方式を選択し、
前記角速度バイアスの前記信頼度および前記加速度バイアスの前記信頼度の何れも前記閾値以上の場合、前記第３方式を選択する
位置推定装置。

【請求項8】

前記第１方式において、前記自己位置推定部は、
前記過去時刻における前記自己位置に基づき、前記対象時刻における前記自己位置を推定し、
推定した前記対象時刻における前記自己位置に基づき、前記対象時刻における前記撮像画像に含まれる１または複数の特徴点のそれぞれと、前記過去時刻における前記撮像画像に含まれる対応する特徴点とを対応付け、
再投影誤差を最小化するように、推定した前記対象時刻における前記自己位置を調整し、
前記再投影誤差は、前記対象時刻における前記撮像画像に含まれる前記１または複数の特徴点のそれぞれの画素位置と、前記過去時刻における前記撮像画像に含まれる前記対応する特徴点を、推定した前記対象時刻における前記自己位置に基づき前記対象時刻における前記撮像画像に再投影した画素位置との誤差の評価値である
請求項７に記載の位置推定装置。

【請求項9】

前記第２方式において、前記自己位置推定部は、
前記過去時刻における前記自己位置の並進成分に基づき、前記対象時刻における前記自己位置の並進成分を推定し、
前記過去時刻における前記自己位置の回転成分、測定した前記角速度、および、推定した前記角速度バイアスに基づき、前記対象時刻における前記自己位置の回転成分を推定し、
推定した前記対象時刻における前記自己位置に基づき、前記対象時刻における前記撮像画像に含まれる１または複数の特徴点のそれぞれと、前記過去時刻における前記撮像画像に含まれる対応する特徴点とを対応付け、
前記再投影誤差と、回転成分運動方程式における第１残差との合計を最小化するように、推定した前記対象時刻における前記自己位置を調整し、
前記回転成分運動方程式における前記第１残差は、前記回転成分運動方程式に対して、前記過去時刻における前記自己位置の回転成分、測定された前記角速度、および、推定した前記角速度バイアスを代入した場合における残差の評価値である
請求項８に記載の位置推定装置。

【請求項10】

前記第３方式において、前記自己位置推定部は、
前記過去時刻における前記自己位置の並進成分、測定した前記加速度、推定した前記速度、推定した前記重力加速度、および、推定した前記加速度バイアスに基づき、前記対象時刻における前記自己位置の並進成分を推定し、
前記過去時刻における前記自己位置の回転成分、測定した前記角速度、および、推定した前記角速度バイアスに基づき、前記対象時刻における前記自己位置の回転成分を推定し、
推定した前記対象時刻における前記自己位置に基づき、前記対象時刻における前記撮像画像に含まれる１または複数の特徴点のそれぞれと、前記過去時刻における前記撮像画像に含まれる対応する特徴点とを対応付け、
前記再投影誤差と、前記回転成分運動方程式における前記第１残差と、並進成分運動方程式における第２残差と、の合計を最小化するように、推定した前記対象時刻における前記自己位置を調整し、
前記並進成分運動方程式における前記第２残差は、前記並進成分運動方程式に対して、前記過去時刻における前記自己位置の並進成分、測定された前記加速度、推定した前記速度、推定した前記重力加速度、および、推定した前記加速度バイアスを代入した場合における残差の評価値である
請求項９に記載の位置推定装置。

【請求項11】

前記状態量推定部は、前記回転成分運動方程式に対して、前記過去時刻における前記自己位置の回転成分を代入した場合における前記第１残差を最小とするような、前記角速度バイアスを推定し、
前記回転成分運動方程式は、下記の式（１）により表され、
Ｒ（ｔ＋Δｔ）＝Ｒ（ｔ）×Ｅｘｐ（（ω（ｔ）－ｂ_ｇ）×Δｔ）…（１）
Ｒ（ｔ＋Δｔ）は、前記対象時刻における前記自己位置の回転成分を表し、
Ｒ（ｔ）は、前記過去時刻における前記自己位置の回転成分を表し、
ω（ｔ）は、測定された前記角速度を表し、
ｂ_ｇは、前記角速度バイアスを表し、
Δｔは、前記過去時刻と前記対象時刻との時間差を表す
請求項１０に記載の位置推定装置。

【請求項12】

前記状態量推定部は、複数の前記過去時刻における前記自己位置の回転成分を前記回転成分運動方程式に代入して、非線形最小二乗法により前記角速度バイアスを推定し、
前記信頼度算出部は、
前記角速度バイアスにおける、互いに直交するＸ方向成分、Ｙ方向成分およびＺ方向成分のうちの２つ成分の組毎の共分散を含む共分散行列を取得し、
取得した前記共分散行列の対角成分の和の逆数を前記角速度バイアスの前記信頼度として算出する
請求項１１に記載の位置推定装置。

【請求項13】

前記状態量推定部は、前記並進成分運動方程式に対して、前記過去時刻における前記自己位置の並進成分を代入した場合における前記第２残差を最小とするような、前記速度、前記重力加速度および前記加速度バイアスを推定し、
前記並進成分運動方程式は、下記の式（２）および式（３）により表され、
ｐ（ｔ＋Δｔ）＝ｐ（ｔ）＋ｖ（ｔ）×Δｔ＋（１／２）×ｇ×Δｔ^２＋（１／２）×Ｒ（ｔ）×（ａ（ｔ）－ｂ_ａ）×Δｔ^２…（２）
ｖ（ｔ＋Δｔ）＝ｖ（ｔ）＋ｇ×Δｔ＋Ｒ（ｔ）×（ａ（ｔ）－ｂ_ａ）×Δｔ…（３）
ｐ（ｔ＋Δｔ）は、前記対象時刻における前記自己位置の並進成分を表し、
ｐ（ｔ）は、前記過去時刻における前記自己位置の並進成分を表し、
ｖ（ｔ）は、前記速度を表し、
Δｔは、前記過去時刻と前記対象時刻との時間差を表し、
ｇは、前記重力加速度を表し、
ａ（ｔ）は、測定された前記加速度を表し、
ｂ_ａは、前記加速度バイアスを表し、
ｖ（ｔ＋Δｔ）は、前記対象時刻における前記速度を表す
請求項１１または１２に記載の位置推定装置。

【請求項14】

前記状態量推定部は、複数の前記過去時刻における前記自己位置の並進成分を前記並進成分運動方程式に代入して、非線形最小二乗法により前記加速度バイアスを推定し、
前記信頼度算出部は、
前記加速度バイアスにおける、互いに直交するＸ方向成分、Ｙ方向成分およびＺ方向成分のうちの２つ成分の組毎の共分散を含む共分散行列を取得し、
取得した前記共分散行列の対角成分の和の逆数を前記加速度バイアスの前記信頼度として算出する
請求項１３に記載の位置推定装置。

【請求項15】

推定された前記自己位置の軌跡に基づき前記移動体装置の運動種別を特定する運動種別特定部と、
運動種別毎に期待される前記１または複数の対象センサの組み合わせのパターンを予め記憶し、選択した前記１または複数の対象センサの組み合わせが、特定した前記運動種別に対応して記憶しているパターンに一致するか否かを判断し、対応しない場合に、異常が発生したと判定する異常判定部と、
をさらに備える請求項１から１４の何れか１項に記載の位置推定装置。

【請求項16】

前記移動体装置の自己位置を推定する、請求項１から１５の何れか１項に記載の位置推定装置と、
推定された前記自己位置に基づき、前記移動体装置の移動を制御する移動体制御装置と、
を備える移動体制御システム。

【請求項17】

情報処理装置により、移動に関する情報を観測する１または複数のセンサおよび周囲を撮像する撮像装置が設けられた移動体装置の自己位置を推定する位置推定方法であって、
前記情報処理装置が、
前記１または複数のセンサのそれぞれの観測値に基づき、前記１または複数のセンサのそれぞれの状態を表す状態量を推定し、
前記１または複数のセンサのそれぞれの前記状態量の信頼の度合いを表す信頼度を算出し、
前記１または複数のセンサのうち前記信頼度が予め定められた閾値以上の前記１または複数のセンサを、１または複数の対象センサとして選択し、対象時刻において前記撮像装置により撮像された撮像画像と、前記対象時刻より前の過去時刻における前記撮像画像と、前記過去時刻における前記自己位置と、選択した前記１または複数の対象センサのそれぞれの前記観測値および前記状態量とに基づき、前記自己位置を推定し、
前記情報処理装置が、前記１または複数のセンサのそれぞれについて、所定の関係式に対して、１または複数の前記過去時刻のそれぞれにおける前記自己位置を代入した場合における残差を最小とするような、前記状態量を推定し、
前記所定の関係式は、前記過去時刻における前記自己位置、対応するセンサの前記観測値および前記対応するセンサの前記状態量、および、前記過去時刻と前記対象時刻との時間差に基づき、前記対象時刻における前記自己位置を算出する運動方程式である
位置推定方法。

【請求項18】

情報処理装置により、移動に関する情報を観測する１または複数のセンサおよび周囲を撮像する撮像装置が設けられた移動体装置の自己位置を推定する位置推定装置であって、
前記情報処理装置が、
前記１または複数のセンサのそれぞれの観測値に基づき、前記１または複数のセンサのそれぞれの状態を表す状態量を推定し、
前記１または複数のセンサのそれぞれの前記状態量の信頼の度合いを表す信頼度を算出し、
前記１または複数のセンサのうち前記信頼度が予め定められた閾値以上の前記１または複数のセンサを、１または複数の対象センサとして選択し、対象時刻において前記撮像装置により撮像された撮像画像と、前記対象時刻より前の過去時刻における前記撮像画像と、前記過去時刻における前記自己位置と、選択した前記１または複数の対象センサのそれぞれの前記観測値および前記状態量に基づき、前記対象時刻における前記自己位置を推定し、
前記自己位置は、並進成分および回転成分を含み、
前記１または複数のセンサは、角速度センサと、加速度センサとを含み、
前記角速度センサは、前記対象時刻における前記移動体装置の角速度を測定し、
前記加速度センサは、前記対象時刻における前記移動体装置の加速度を測定し、
前記情報処理装置は、
前記角速度センサの前記状態量として、前記移動体装置の角速度バイアスを推定し、
前記加速度センサの前記状態量として、前記移動体装置の速度、重力加速度、および、前記移動体装置の加速度バイアスを推定し、
前記情報処理装置は、
前記対象時刻における前記撮像画像と、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第１方式、
前記対象時刻における前記撮像画像と、測定した前記角速度と、推定した前記角速度バイアスと、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第２方式、または、
前記対象時刻における前記撮像画像と、測定した前記角速度と、推定した前記角速度バイアスと、測定した前記加速度と、推定した前記速度と、推定した前記重力加速度と、推定した前記加速度バイアスと、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第３方式
に基づき前記対象時刻における前記自己位置を推定し、
前記情報処理装置は、
前記角速度バイアスの前記信頼度が前記閾値以上ではない場合、前記第１方式を選択し、
前記角速度バイアスの前記信頼度が前記閾値以上であり、前記加速度バイアスの前記信頼度が前記閾値以上ではない場合、前記第２方式を選択し、
前記角速度バイアスの前記信頼度および前記加速度バイアスの前記信頼度の何れも前記閾値以上の場合、前記第３方式を選択する
位置推定方法。

【請求項19】

情報処理装置を、移動に関する情報を観測する１または複数のセンサおよび周囲を撮像する撮像装置が設けられた移動体装置の自己位置を推定する位置推定装置として機能させるためのプログラムであって、
前記情報処理装置を、
前記１または複数のセンサのそれぞれの観測値に基づき、前記１または複数のセンサのそれぞれの状態を表す状態量を推定する状態量推定部と、
前記１または複数のセンサのそれぞれの前記状態量の信頼の度合いを表す信頼度を算出する信頼度算出部と、
前記１または複数のセンサのうち前記信頼度が予め定められた閾値以上の前記１または複数のセンサを、１または複数の対象センサとして選択し、対象時刻において前記撮像装置により撮像された撮像画像と、前記対象時刻より前の過去時刻における前記撮像画像と、前記過去時刻における前記自己位置と、選択した前記１または複数の対象センサのそれぞれの前記観測値および前記状態量とに基づき、前記自己位置を推定する自己位置推定部と、
して機能させ、
前記１または複数のセンサのそれぞれについて、前記状態量推定部は、所定の関係式に対して、１または複数の前記過去時刻のそれぞれにおける前記自己位置を代入した場合における残差を最小とするような、前記状態量を推定し、
前記所定の関係式は、前記過去時刻における前記自己位置、対応するセンサの前記観測値および前記対応するセンサの前記状態量、および、前記過去時刻と前記対象時刻との時間差に基づき、前記対象時刻における前記自己位置を算出する運動方程式である
プログラム。

【請求項20】

情報処理装置を、移動に関する情報を観測する１または複数のセンサおよび周囲を撮像する撮像装置が設けられた移動体装置の自己位置を推定する位置推定装置として機能させるためのプログラムであって、
前記情報処理装置を、
前記１または複数のセンサのそれぞれの観測値に基づき、前記１または複数のセンサのそれぞれの状態を表す状態量を推定する状態量推定部と、
前記１または複数のセンサのそれぞれの前記状態量の信頼の度合いを表す信頼度を算出する信頼度算出部と、
前記１または複数のセンサのうち前記信頼度が予め定められた閾値以上の前記１または複数のセンサを、１または複数の対象センサとして選択し、対象時刻において前記撮像装置により撮像された撮像画像と、前記対象時刻より前の過去時刻における前記撮像画像と、前記過去時刻における前記自己位置と、選択した前記１または複数の対象センサのそれぞれの前記観測値および前記状態量に基づき、前記対象時刻における前記自己位置を推定する自己位置推定部と、
して機能させ、
前記自己位置は、並進成分および回転成分を含み、
前記１または複数のセンサは、角速度センサと、加速度センサとを含み、
前記角速度センサは、前記対象時刻における前記移動体装置の角速度を測定し、
前記加速度センサは、前記対象時刻における前記移動体装置の加速度を測定し、
前記状態量推定部は、
前記角速度センサの前記状態量として、前記移動体装置の角速度バイアスを推定し、
前記加速度センサの前記状態量として、前記移動体装置の速度、重力加速度、および、前記移動体装置の加速度バイアスを推定し、
前記自己位置推定部は、
前記対象時刻における前記撮像画像と、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第１方式、
前記対象時刻における前記撮像画像と、測定した前記角速度と、推定した前記角速度バイアスと、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第２方式、または、
前記対象時刻における前記撮像画像と、測定した前記角速度と、推定した前記角速度バイアスと、測定した前記加速度と、推定した前記速度と、推定した前記重力加速度と、推定した前記加速度バイアスと、前記過去時刻における前記撮像画像と、前記過去時刻における前記自己位置とに基づき、前記対象時刻における前記自己位置を推定する第３方式
に基づき前記対象時刻における前記自己位置を推定し、
前記自己位置推定部は、
前記角速度バイアスの前記信頼度が前記閾値以上ではない場合、前記第１方式を選択し、
前記角速度バイアスの前記信頼度が前記閾値以上であり、前記加速度バイアスの前記信頼度が前記閾値以上ではない場合、前記第２方式を選択し、
前記角速度バイアスの前記信頼度および前記加速度バイアスの前記信頼度の何れも前記閾値以上の場合、前記第３方式を選択する
プログラム。

【発明の詳細な説明】

【技術分野】

【0001】

本発明の実施形態は、位置推定装置、移動体制御システム、位置推定方法およびプログラムに関する。

【背景技術】

【0002】

撮像装置の自己位置を推定する位置推定装置が知られている。位置推定装置は、撮像装置により撮像された画像、および、撮像装置に取り付けられたセンサにより観測された観測値を用いて自己位置を推定する。さらに、位置推定装置は、センサにより観測された観測値に加えて、センサの状態を表す状態量を用いて、自己位置を推定する場合もある。例えば、角速度センサの状態量は、角速度に含まれるバイアスである。加速度センサの状態量は、加速度に含まれるバイアス、重力加速度、および、速度である。

【0003】

位置推定装置は、センサの状態量を、センサにより観測された観測値に基づき推定する。しかし、推定された状態量の精度は、センサの移動軌跡等によって変化する。例えば、等速直線運動をしている場合、加速度センサの観測値は、ほぼ一定となる。従って、このような場合、加速度バイアス、重力加速度、および速度の精度は、低くなる。位置推定装置は、このような精度の低い状態量を用いた場合、自己位置の推定精度が悪くなってしまう。

【先行技術文献】

【特許文献】

【0004】

【文献】特許第６０６１７７０号公報

【発明の概要】

【発明が解決しようとする課題】

【0005】

本発明が解決しようとする課題は、移動体装置の自己位置を精度良く推定することにある。

【課題を解決するための手段】

【0006】

実施形態に係る位置推定装置は、移動に関する情報を観測する１または複数のセンサおよび周囲を撮像する撮像装置が設けられた移動体装置の自己位置を推定する。前記位置推定装置は、状態量推定部と、信頼度算出部と、自己位置推定部とを備える。前記状態量推定部は、前記１または複数のセンサのそれぞれの観測値に基づき、前記１または複数のセンサのそれぞれの状態を表す状態量を推定する。前記信頼度算出部は、前記１または複数のセンサのそれぞれの前記状態量の信頼の度合いを表す信頼度を算出する。前記自己位置推定部は、前記１または複数のセンサのうち前記信頼度が予め定められた閾値以上の前記１または複数のセンサを、１または複数の対象センサとして選択し、対象時刻において前記撮像装置により撮像された撮像画像と、前記対象時刻より前の過去時刻における前記撮像画像と、前記過去時刻における前記自己位置と、選択した前記１または複数の対象センサのそれぞれの前記観測値および前記状態量とに基づき、前記自己位置を推定する。前記１または複数のセンサのそれぞれについて、前記状態量推定部は、所定の関係式に対して、１または複数の前記過去時刻のそれぞれにおける前記自己位置を代入した場合における残差を最小とするような、前記状態量を推定する。前記所定の関係式は、前記過去時刻における前記自己位置、対応するセンサの前記観測値および前記対応するセンサの前記状態量、および、前記過去時刻と前記対象時刻との時間差に基づき、前記対象時刻における前記自己位置を算出する運動方程式である。

【図面の簡単な説明】

【0007】

【図1】実施形態に係る移動体を示す図。

【図2】移動体制御システムの構成図。

【図3】位置推定装置の構成図。

【図4】位置推定装置の処理の流れを示すフローチャート。

【図5】自己位置推定部の流れを示すフローチャート。

【図6】第１方式による推定処理の流れを示すフローチャート。

【図7】第２方式による推定処理の流れを示すフローチャート。

【図8】第３方式による推定処理の流れを示すフローチャート。

【図9】変形例に係る位置推定装置の構成図。

【発明を実施するための形態】

【0008】

以下、図面を参照しながら、本発明の実施形態を説明する。

【0009】

図１は、実施形態に係る移動体装置１０を示す図である。本実施形態において、移動体装置１０は、自動車またはモータバイク等の車両である。しかし、移動体装置１０は、車両に限られず、自身が移動したり、人またはロボット等により持ち運びされたりすることが可能であれば、どのような装置であってもよい。例えば、移動体装置１０は、ロボットであってもよいし、船舶であってもよいし、ドローン等の飛行物体であってもよい。

【0010】

移動体装置１０は、移動体制御システム１２を搭載する。移動体制御システム１２は、例えば専用または汎用コンピュータを含む装置である。移動体制御システム１２は、情報処理機能が移動体装置１０に搭載されず、移動体装置１０とネットワークを介して接続されるクラウド等の装置に搭載されてもよい。

【0011】

図２は、実施形態に係る移動体制御システム１２の構成の一例を示す図である。移動体制御システム１２は、撮像装置２１と、１または複数のセンサ２２と、記憶装置２３と、入力装置２４と、表示装置２５と、通信装置２６と、移動体制御装置２７と、情報処理装置４０とを備える。

【0012】

撮像装置２１は、移動体装置１０に設けられる。撮像装置２１は、移動体装置１０における固定された位置から、固定された姿勢で、移動体装置１０の周囲を撮像する。撮像装置２１は、移動体装置１０との相対的な姿勢が測定できれば、固定された姿勢でなくてもよい。本実施形態において、撮像装置２１は、単眼カメラである。撮像装置２１は、ステレオカメラであってもよいし、デプスカメラであってもよい。また、撮像装置２１は、所定の画角のカメラであってもよいし、全方位カメラであってもよい。撮像装置２１は、所定の時間間隔毎に、移動体装置１０の周囲を撮像して撮像画像を生成する。

【0013】

１または複数のセンサ２２は、移動体装置１０に設けられる。１または複数のセンサ２２のそれぞれは、例えば、移動体装置１０における固定された位置に、固定された姿勢で取り付けられる。１または複数のセンサ２２のそれぞれは、移動体装置１０の移動に関する情報を観測する。

【0014】

本実施形態において、１または複数のセンサ２２は、角速度センサ３１および加速度センサ３２を含む。

【0015】

角速度センサ３１は、移動体装置１０の角速度を観測値として検出する。角速度センサ３１は、角速度のＸ方向成分、角速度のＹ方向成分およびＺ方向成分を検出する。Ｘ方向成分、Ｙ方向成分およびＺ方向成分は、互いに直交する。

【0016】

加速度センサ３２は、移動体装置１０の並進方向の加速度を観測値として検出する。加速度センサ３２は、加速度のＸ方向成分、加速度のＹ方向成分および加速度のＺ方向成分を検出する。

【0017】

１または複数のセンサ２２は、角速度センサ３１および加速度センサ３２以外のものを含んでもよい。例えば、１または複数のセンサ２２は、移動体装置１０の速度を検出する速度センサ、衛星からの信号等に基づき移動体装置１０の位置を検出する位置センサを含んでもよい。また、１または複数のセンサ２２は、例えばＬｉＤＡＲ等の移動体装置１０の周囲の物体との距離を測定する距離センサ、移動体装置１０の周囲の物体を音波によって探知するソナー、および、地磁気センサ等を含んでもよい。

【0018】

記憶装置２３は、ハードディスクドライブ、光ディスクドライブまたはフラッシュメモリ等の半導体メモリ素子等である。記憶装置２３は、移動体制御システム１２により実行されるプログラムおよび移動体制御システム１２が用いるデータを記憶する。

【0019】

入力装置２４は、ユーザからの指示および情報入力を受け付ける。入力装置２４は、例えば、操作パネル、マウスまたはトラックボール等のポインティングデバイス、または、キーボード等の入力デバイスである。

【0020】

表示装置２５は、ユーザに対して各種の情報を表示する。表示装置２５は、例えば、液晶表示装置等の表示デバイスである。通信装置２６は、無線通信により外部装置と情報を送受信する。

【0021】

移動体制御装置２７は、移動体装置１０を移動させるための駆動機構を制御する。例えば、移動体装置１０が自動運転車両である場合には、情報処理装置４０から得られる情報およびその他の情報に基づいて周辺の状況を判断して、アクセル量、ブレーキ量および操舵角等を制御する。

【0022】

情報処理装置４０は、例えば、専用または汎用の１または複数のコンピュータである。情報処理装置４０は、撮像装置２１と、１または複数のセンサ２２と、記憶装置２３と、入力装置２４と、表示装置２５と、通信装置２６と、移動体制御装置２７とを管理および制御する。情報処理装置４０は、メモリ４１と、１または複数のハードウェアプロセッサ４２とを有する。

【0023】

メモリ４１は、例えば、ＲＯＭ４３（Read Only Memory）と、ＲＡＭ４４（Random Access Memory）とを含む。ＲＯＭ４３は、情報処理装置４０の制御に用いられるプログラムおよび各種設定情報等を書き換え不可能に記憶する。ＲＡＭ４４は、ＳＤＲＡＭ（Synchronous Dynamic Random Access Memory）等の揮発性の記憶媒体である。ＲＡＭ４４は、１または複数のハードウェアプロセッサ４２の作業領域として機能する。

【0024】

１または複数のハードウェアプロセッサ４２は、メモリ４１（ＲＯＭ４３およびＲＡＭ４４）にバスを介して接続される。１または複数のハードウェアプロセッサ４２は、例えば、１または複数のＣＰＵ（Central Processing Unit）を含んでもよいし、１または複数のＧＰＵ（Graphics Processing Unit）を含んでもよい。また、１または複数のハードウェアプロセッサ４２は、ニューラルネットワークを実現するための専用の処理回路を含む半導体装置等を含んでもよい。

【0025】

１または複数のハードウェアプロセッサ４２は、ＲＡＭ４４を作業領域としてＲＯＭ４３または記憶装置２３に予め記憶された各種プログラムとの協働により各種処理を実行することにより、位置推定装置５０として機能する。位置推定装置５０については、以下詳細に説明する。

【0026】

図３は、位置推定装置５０の構成を示す図である。位置推定装置５０は、移動体装置１０の自己位置を推定する。より具体的には、移動体装置１０は、自己位置の並進成分および自己位置の回転成分を推定する。

【0027】

自己位置の並進成分は、移動体装置１０の三次元空間内での座標（Ｘ方向の位置、Ｙ方向の位置およびＺ方向の位置）を表す。例えば、自己位置の並進成分は、任意の基準点からの距離（Ｘ方向の距離、Ｙ方向の距離およびＺ方向の距離）を表す。任意の基準点は、例えば、開始時刻等の所定時刻における移動体装置１０の自己位置の並進成分であってよい。

【0028】

自己位置の回転成分は、移動体装置１０の姿勢（Ｘ軸周りの回転角度、Ｙ軸周りの回転角度およびＺ軸周りの回転角度）を表す。なお、回転成分は、任意の基準回転角からの角度差であってよい。任意の基準回転角は、例えば、開始時刻等の所定時刻における移動体装置１０の自己位置の回転成分であってよい。また、例えば、回転成分は、回転行列、回転軸と回転方向を表すベクトル、または、四元数等によって表現されていてもよい。

【0029】

位置推定装置５０は、所定の時間間隔毎に、自己位置を推定する。自己位置を推定する対象となる時刻を、対象時刻という。また、対象時刻より前であり、既に自己位置を推定済みの時刻を過去時刻という。

【0030】

位置推定装置５０は、自己位置記憶部５２と、画像取得部５４と、画像記憶部５６と、観測値取得部５８と、状態量推定部６０と、信頼度算出部６２と、自己位置推定部６４とを備える。

【0031】

自己位置記憶部５２は、１または複数の過去時刻のそれぞれにおける自己位置を記憶する。画像取得部５４は、対象時刻において撮像装置２１により撮像された撮像画像を取得する。画像記憶部５６は、１または複数の過去時刻において撮像装置２１により撮像された撮像画像を記憶する。

【0032】

観測値取得部５８は、１または複数のセンサ２２のそれぞれにより観測された観測値を取得する。観測値取得部５８は、例えば、時系列で観測値を取得する。本実施形態においては、観測値取得部５８は、角速度センサ３１により観測された角速度、および、加速度センサ３２により観測された加速度を観測値として取得する。

【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】

例えば、自己位置推定部６４は、過去時刻における撮像画像に含まれる対応する特徴点の３次元位置を、Ｓｔｒｕｃｔｕｒｅ－ｆｒｏｍ－ＭｏｔｉｏｎまたはＳＬＡＭ（Simultaneous Localization and Mapping）等により算出する。自己位置推定部６４は、算出した３次元位置を、Ｓ３１およびＳ３２で推定した対象時刻における自己位置から対象時刻における撮像画像に投影し、対象時刻における撮像画像上の候補位置を特定する。自己位置推定部６４は、対象時刻における撮像画像の候補位置の周囲における類似テクスチャを探索し、過去時刻における撮像画像に含まれる対応する特徴点と、対象時刻における撮像画像の特徴点とを対応付ける。

【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】

このような変形例に係る位置推定装置５０は、選択された自己位置の推定方式と、推定された自己位置との関係性を評価することとができる。そして、位置推定装置５０は、推定方式と推定された自己位置との関係性の評価結果に基づき、位置推定装置５０の推定動作が異常であるか否かを自己診断することができる。

【0121】

そして、変形例に係る位置推定装置５０は、自己診断の結果に基づき、新たな自己位置を推定することができる。これにより、変形例に係る位置推定装置５０によれば、より精度良く自己位置を推定することができる。

【0122】

以上のように、本実施形態に係る位置推定装置５０は、１または複数のセンサ２２のそれぞれの状態を表す状態量を推定し、１または複数のセンサ２２のそれぞれの状態量の信頼度を算出する。そして、位置推定装置５０は、１または複数のセンサ２２のそれぞれの状態量の信頼度に基づき１または複数のセンサ２２のうちの１または複数の対象センサを選択し、選択した１または複数の対象センサのそれぞれの観測値および状態量に基づき、自己位置を推定する。

【0123】

これにより、位置推定装置５０は、信頼度の低い状態量を利用したことによる自己位置の推定精度の悪化を回避することができる。従って、位置推定装置５０によれば、状態量の信頼度の高いセンサ２２の観測値および状態量を用いて、移動体装置１０の自己位置を精度良く推定することができる。

【0124】

以上、本発明の実施形態を説明したが、上述の実施形態は例として提示したものであり、発明の範囲を限定することは意図していない。これら新規な実施形態は、その他の様々な形態で実施されることが可能であり、発明の要旨を逸脱しない範囲で、種々の省略、置き換え、変更を行うことができる。これら新規な実施形態やその変形は、発明の範囲や要旨に含まれるとともに、特許請求の範囲に記載された発明とその均等の範囲に含まれる。

【符号の説明】

【0125】

１０ 移動体装置
１２ 移動体制御システム
２１ 撮像装置
２２ センサ
２３ 記憶装置
２４ 入力装置
２５ 表示装置
２６ 通信装置
２７ 移動体制御装置
４０ 情報処理装置
４１ メモリ
４２ ハードウェアプロセッサ
４３ ＲＯＭ
４４ ＲＡＭ
５０ 位置推定装置
５２ 自己位置記憶部
５４ 画像取得部
５６ 画像記憶部
５８ 観測値取得部
６０ 状態量推定部
６２ 信頼度算出部
６４ 自己位置推定部
７４ 運動種別特定部
７６ 異常判定部
７８ リセット部

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】

【図9】