JP2020064029A

JP2020064029A - 移動体制御装置

Info

Publication number: JP2020064029A
Application number: JP2018197593A
Authority: JP
Inventors: 滝沢　圭; Kei Takizawa; 圭滝沢
Original assignee: Toshiba Corp; Toshiba Electronic Devices and Storage Corp
Current assignee: Toshiba Corp; Toshiba Electronic Devices and Storage Corp
Priority date: 2018-10-19
Filing date: 2018-10-19
Publication date: 2020-04-23
Anticipated expiration: 2038-10-19
Also published as: US20200125111A1; JP7278740B2

Abstract

【課題】信頼性の高い推定結果を高速に算出することができる、移動体制御装置を提供する。【解決手段】移動体１００に設けられたカメラ１００Ｂにより得られる撮影画像に基づき、カメラ姿勢を推定し、推定結果として出力する画像処理装置１０と、推定結果に基づいて移動体１００に設けられた動力部１００Ｈの制御を行う動力制御部１００Ｇと、を備えた移動体制御装置であって、画像処理装置１０は、推定結果の信頼度特徴量を算出する信頼度特徴量計算部１１９と、信頼度特徴量に基づき推定結果の信頼度を判定する信頼度判定部１２０を備えており、動力制御部１００Ｇは、信頼度の判定結果に基づき、動力部１００Ｈの自動制御を行うか否かを決定する。【選択図】図４

Description

本実施形態は、移動体制御装置に関する。

近年、例えば、ステレオカメラを車両などの移動体に搭載し、このカメラから一定間隔で出力される画像に基づいて障害物を検出し、当該障害物との接触を回避するように車両の走行に係る自動制御を行って運転の支援をするようなシステム（安全運転システム）が実用化されつつある。

このような安全運転システムを用いて移動体の制御を行う場合、ステレオカメラの姿勢を推定する際に、信頼性の高い推定結果を高速に算出することが重要となる。

特開２０１３−２３９９０５号公報

本実施形態は、信頼性の高い推定結果を高速に算出することができる、移動体制御装置を提供することを目的とする。

本実施形態の移動体制御装置は、移動体に設けられた撮像装置により得られる撮影画像に基づき、前記撮像装置の姿勢を推定し、推定結果として出力する画像処理装置と、前記推定結果に基づいて前記移動体に設けられた動力部の制御を行う動力制御部と、を備えた移動体制御装置であって、前記画像処理装置は、前記推定結果の信頼度特徴量を算出する信頼度特徴量計算部と、前記信頼度特徴量に基づき前記推定結果の信頼度を判定する信頼度判定部を備えている。また、前記動力制御部は、前記信頼度の判定結果に基づき、前記動力部の自動制御を行うか否かを決定する。

本実施形態に係わる移動体１００の一例を示す図。本実施形態に係わる移動体制御装置を含む、移動体１００の構成の一例を示すブロック図。移動体１００に搭載された画像処理装置１０の一例を示すブロック図。画像処理装置１０における、移動体制御装置に係わる構成の一例を示す概略ブロック図。本実施形態に係わる移動体自動制御継続可否の決定手順の一例を説明するフローチャート。画像処理装置における、カメラ姿勢・点３次元座標の推定の一連の手順を説明するフローチャート。第１の実施形態に係わる運動推定２の一連の手順を説明するフローチャート。推定結果の信頼性評価に用いる画像のブロックマッチングを説明する図。推定結果の信頼性評価方法を説明する図。推定結果の信頼性評価方法を説明する図。第１の実施形態に係わる運動推定２の別の手順を説明するフローチャート。第１の実施形態の変形例に係わる運動推定２の一連の手順を説明するフローチャート。第１の実施形態の変形例に係わる運動推定２の別の手順を説明するフローチャート。第２の実施形態に係わる運動推定２の一連の手順を説明するフローチャート。第２の実施形態に係わる運動推定２の別の手順を説明するフローチャート。第３の実施形態に係わる運動推定２の一連の手順を説明するフローチャート。第３の実施形態に係わる運動推定２の別の手順を説明するフローチャート。

以下、図面を参照して実施形態を説明する。

（第１の実施形態）
図１は、本実施形態に係わる移動体制御装置を搭載する移動体１００の一例を示す図である。

移動体１００は、画像処理装置１０と、出力部１００Ａと、センサ１００Ｂと、入力装置１００Ｃと、動力制御部１００Ｇと、動力部１００Ｈと、を備える。

移動体１００は、移動可能な物体である。移動体１００は、例えば、車両（自動二輪車、自動四輪車、自転車）、台車、ロボット、船舶、飛翔体（飛行機、無人航空機（UAV：Unmanned Aerial Vehicle）など）である。移動体１００は、例えば、人による運転操作を介して走行する移動体や、人による運転操作を介さずに自動的に走行（自律走行）可能な移動体である。自律走行可能な移動体は、例えば、自動運転車両である。本実施形態の移動体１００は、自律走行可能であり、かつ、人による運転操作も可能な車両を一例として説明する。

出力部１００Ａは、各種情報を出力する。例えば出力部１００Ａは、各種処理による出力情報を出力する。

出力部１００Ａは、例えば、出力情報を送信する通信機能、出力情報を表示する表示機能、出力情報を示す音を出力する音出力機能、などを備える。例えば、出力部１００Ａは、通信部１００Ｄと、ディスプレイ１００Ｅと、スピーカ１００Ｆと、を含む。

通信部１００Ｄは、外部装置と通信する。通信部１００Ｄは、VICS（登録商標）通信回路やダイナミックマップ通信回路である。通信部１００Ｄは、出力情報を外部装置へ送信する。また、通信部１００Ｄは、道路情報などを外部装置から受信する。道路情報は、信号、標識、周囲の建物、各車線の道幅、レーン中心線などである。道路情報は、画像処理装置内に設けたRAM、ROMといったメモリ１０ｂ内に記憶されても、移動体内に別に設けたメモリ内に記憶してもよい。

ディスプレイ１００Ｅは、出力情報を表示する。ディスプレイ１００Ｅは、例えば、公知のLCD（Liquid Crystal Display）や投影装置やライトなどである。スピーカ１００Ｆは、出力情報を示す音を出力する。

センサ１００Ｂは、移動体１００の走行環境を取得するセンサである。走行環境は、例えば、移動体１００の観測情報や、移動体１００の周辺情報である。センサ１００Ｂは、例えば、外界センサや内界センサである。

内界センサは、観測情報を観測するセンサである。観測情報は、例えば、移動体１００の加速度、移動体１００の速度、移動体１００の角速度（ヨー軸角速度）を含む。

内界センサは、例えば、慣性計測装置（IMU：Inertial Measurement Unit）、加速度センサ、速度センサ、ロータリエンコーダ、ヨーレートセンサ、などである。IMUは、移動体１００の三軸加速度および三軸角速度を含む観測情報を観測する。

外界センサは、移動体１００の周辺情報を観測する。外界センサは、移動体１００に搭載されていてもよいし、該移動体１００の外部（例えば、他の移動体や外部装置など）に搭載されていてもよい。

周辺情報は、移動体１００の周辺の状況を示す情報である。移動体１００の周辺とは、該移動体１００から予め定めた範囲内の領域である。この範囲は、外界センサの観測可能な範囲である。この範囲は、予め設定すればよい。

周辺情報は、例えば、移動体１００の周辺の撮影画像および距離情報などである。なお、周辺情報は、移動体１００の位置情報を含んでいてもよい。撮影画像は、撮影によって得られる撮影画像データである（以下、単に、撮影画像と称する場合がある）。距離情報は、移動体１００から対象までの距離を示す情報である。対象は、外界における、外界センサによって観測可能な箇所である。位置情報は、相対位置であってもよいし、絶対位置であってもよい。

外界センサは、例えば、撮影によって撮影画像を得る撮影装置（カメラ）、距離センサ（ミリ波レーダ、レーザセンサ、距離画像センサ）、位置センサ（GNSS（Global Navigation Satellite System）、GPS（Global Positioning System）、無線通信装置）などである。

撮影画像は、画素ごとに画素値を規定したデジタル画像データや、画素毎にセンサ１００Ｂからの距離を規定したデプスマップなどである。レーザセンサは、例えば、水平面に対して平行に設置された二次元LIDAR（Laser Imaging Detection and Ranging）センサや、三次元LIDARセンサである。

入力装置１００Ｃは、ユーザからの各種指示や情報入力を受け付ける。入力装置１００Ｃは、例えば、マウスやトラックボール等のポインティングデバイス、または、キーボード等の入力デバイスである。また、入力装置１００Ｃは、ディスプレイ１００Ｅと一体的に設けられたタッチパネルにおける入力機能であってもよい。

動力制御部１００Ｇは、動力部１００Ｈを制御する。動力部１００Ｈは、移動体１００に搭載された、駆動するデバイスである。動力部１００Ｈは、例えば、エンジン、モータ、車輪、などである。

動力部１００Ｈは、動力制御部１００Ｇの制御によって駆動する。本実施形態の移動体１００は、自律走行可能であるので、動力制御部１００Ｇは、画像処理装置１で生成された出力情報や、センサ１００Ｂから得られた情報などに基づいて、周辺の状況を判断し、アクセル量、ブレーキ量、操舵角などの制御を行う。すなわち、移動体１００の前方に検出された障害物が移動体１００に衝突する可能性がある場合には、動力部１００Ｈを制御して、移動体１００と障害物との接触を回避させる。

次に、移動体１００の電気的構成について詳細に説明する。図２は、移動体１００の構成の一例を示すブロック図である。

移動体１００は、画像処理装置１０と、出力部１００Ａと、センサ１００Ｂと、入力装置１００Ｃと、動力制御部１００Ｇと、動力部１００Ｈと、を備える。出力部１００Ａは、上述したように、通信部１００Ｄと、ディスプレイ１００Ｅと、スピーカ１００Ｆと、を含む。本実施形態の移動体制御装置１は、画像処理装置１０と、動力制御部１００Ｇと、を含んで構成される。

画像処理装置１０、出力部１００Ａ、センサ１００Ｂ、入力装置１００Ｃ、および動力制御部１００Ｇは、バス１００Ｉを介して接続されている。動力部１００Ｈは、動力制御部１００Ｇに接続されている。

なお、出力部１００Ａ（通信部１００Ｄ、ディスプレイ１００Ｅ、スピーカ１００Ｆ）、センサ１００Ｂ、入力装置１００Ｃ、および動力制御部１００Ｇ、の少なくとも１つは、有線または無線で画像処理装置１０に接続すればよい。また、出力部１００Ａ（通信部１００Ｄ、ディスプレイ１００Ｅ、スピーカ１００Ｆ）、センサ１００Ｂ、入力装置１００Ｃ、および動力制御部１００Ｇの少なくとも１つと、画像処理装置１０とを、ネットワークを介して接続してもよい。

図３は、移動体１００に搭載された画像処理装置１０の一例を示すブロック図である。画像処理装置１０は、Ｉ／Ｆ１０ｃ、メモリ１０ｂ、プロセッサ１０ａを含む。

Ｉ／Ｆ１０ｃは、他のシステムとのネットワーク（N／W）等に接続されている。また、Ｉ／Ｆ１０ｃは、通信部１００Ｄとの間の情報の送受信を司る。Ｉ／Ｆ１０ｃを介して、人などの認識された対象の情報、認識された対象までの距離の情報が出力される。

メモリ１０ｂは、各種データを記憶する。メモリ１０ｂは、例えば、RAM（Random Access Memory）、フラッシュメモリ等の半導体メモリ素子、ハードディスク、光ディスク等である。なお、メモリ１０ｂは、画像処理装置１の外部に設けてもよい。ROMは、プロセッサ１０ａによって実行されるプログラムや必要なデータを保持する。RAMは、プロセッサ１０ａの作業領域として機能する。また、メモリ１０ｂは、移動体１００の外部に設けてもよい。例えば、メモリ１０ｂを、クラウド上に設置されたサーバ装置に配置してもよい。

また、メモリ１０ｂは、記憶媒体であってもよい。具体的には、記憶媒体は、プログラムや各種情報を、LAN（Local Area Network）やインターネットなどを介してダウンロードして記憶または一時記憶したものであってもよい。また、メモリ１０ｂを、複数の記憶媒体から構成してもよい。

プロセッサ１０ａにおける各処理機能は、コンピュータによって実行可能なプログラムの形態でメモリ１０ｂへ記憶されている。プロセッサ１０ａは、プログラムをメモリ１０ｂから読出、実行することで、各プログラムに対応する機能部を実現するプロセッサである。

なお、各機能の各々を実現するための独立した複数のプロセッサを組み合わせて処理回路１０ｅを構成してもよい。この場合、各プロセッサがプログラムを実行することにより各機能を実現する。また、各処理機能がプログラムとして構成され、１つの処理回路１０ｅが各プログラムを実行する場合であってもよいし、専用回路として画像処理アクセラレータ１０ｄを設け、特定の機能が独立したプログラム実行回路に実装される場合であってもよい。

プロセッサ１０ａは、メモリ１０ｂに保存されたプログラムを読み出し実行することで機能を実現する。なお、メモリ１０ｂにプログラムを保存する代わりに、プロセッサの回路内にプログラムを直接組み込むよう構成しても構わない。この場合、プロセッサは回路内に組み込まれたプログラムを読み出し実行することで機能を実現する。

図４は、画像処理装置１０の構成の一例を示す概略ブロック図である。図４には、移動体制御装置に係わる構成のみを図示している。画像処理装置１０は、画像処理部１１と、障害物検出部１２とから構成されている。

画像処理部１１は、センサ１００Ｂに含まれているカメラに関し、３次元空間での姿勢（以下、カメラ姿勢と示す）を推定する。また、カメラ前方の障害物を３次元点群として抽出する。画像処理部１１は、画像処理制御部１１１と、画像入力部１１２と、特徴点対応付け部１１４と、第１運動推定部１１５と、運動予測部１１６と、第２運動推定部１１７と、点３次元座標推定部１１８と、信頼度特徴量計算部１１９と、信頼度判定部１２０とを有して構成される。

障害物検出部１２は、画像処理部１１によって推定されたカメラ姿勢と、抽出された３次元点群に基づき、カメラ前方にある障害物を検出する。

画像処理制御部１１１は、カメラ姿勢を推定して３次元点群を抽出するよう、画像処理部１１の他の構成要素の動作を制御する。

画像入力部１１２は、センサ１００Ｂに含まれるカメラから、時系列で撮影画像を取得する。時系列の撮影画像は一定時間間隔で入力される。以下の説明において、撮影画像を入力した時刻を、入力した順に０、１、２、…、ｔ−１、ｔ、ｔ＋１、…のように番号で表す。また、時刻ｔの入力画像をフレームｔと呼ぶ。

なお、画像処理部１１から出力は、各時刻の３次元空間でのカメラ姿勢と３次元点群である。時刻ｔにおけるカメラ姿勢は、（１）式に示す６次元のベクトルで表される。

これは、３次元空間の並進運動と回転運動を表すパラメータであり、リー代数se(3)として知られているものである。ξ^(t)は、３次元での回転行列と並進ベクトルの組と相互に変換可能である。

特徴点抽出部１１３は、画像入力部１１２が取得した各フレームから、エッジの強い点等を、例えばGFTT(Good Features To Track)などのアルゴリズムを用いて、特徴点として抽出する。

特徴点対応付け部１１４は、フレームｔ−１とフレームｔの特徴点同士の対応付けを行う。対応付けには、例えば、ORB特徴量(Oriented FAST and Rotated BRIEF)を用いる。

第１運動推定部１１５は、フレームｔ−１とフレームｔとの対応関係から、カメラの相対的な姿勢を推定する。例えば、D. Nister, “An efficient solution to the five-point relative pose problem,” PAMI, 2004.に開示されているアルゴリズム（２フレーム間の５点以上の対応関係から、２フレームのカメラの相対的な姿勢（３次元空間での回転運動と並進運動）を推定するアルゴリズム）を用いて推定する。

運動予測部１１６は、過去に推定したカメラ姿勢から、フレームｔのカメラ姿勢である、バー付きξ^(t)を予測する。バー付きξ^(t)は、（２）式に示す６次元ベクトルで表される。

予測値であるバー付きξ^(t)は、第２運動推定の予測の初期値として用いられる。バー付きξ^(t)の予測は、例えば、フレームｔ−１のカメラ姿勢の推定値であるハット付きξ^(t-1)と、フレームｔ−２のカメラ姿勢の推定値であるハット付きξ^(t-2)から、次の（３）式を用いて行われる。

点３次元座標推定部１１８は、フレームｔ−１とフレームｔのカメラ姿勢、フレーム
ｔ−１とフレームｔとの間の特徴点の対応関係、フレームｔ−１とフレームｔにおける特徴点の入力画像上での座標、の各情報に基づき、特徴点の３次元座標を推定する。座標の推定には、三角測量など一般的な３次元座標推定手法を用いる。

第２運動推定部１１７は、（１）３次元点座標、（２）３次元点に対応したフレームｔの特徴点の座標、（３）フレームｔのカメラ姿勢の初期値、を入力とし、フレームｔの運動推定結果（時刻ｔのカメラ姿勢の推定値、ハット付きξ^(t)）を出力する。カメラ姿勢の推定値の具体的な算出方法については、後に詳述する。

信頼度特徴量計算部１１９は、第２運動推定部１１７で算出された運動推定結果の信頼性を算出する。

信頼度判定部１２０は、予め設定された閾値と、信頼度特徴量計算部１１９で算出された信頼性とを比較し、運動推定結果が信頼できるか否かを判定する。

図５は、本実施形態に係わる移動体自動制御継続可否の決定手順の一例を説明するフローチャートである。まず、画像処理部１１において、カメラ姿勢を推定する（Ｓ１）。また、Ｓ１では、推定したカメラ姿勢の信頼度特徴量を算出し、信頼度判定部１２０において推定したカメラ姿勢の信頼度を判定する。推定されたカメラ姿勢の信頼度が低い（運動推定結果が信頼できない）と判定された場合（Ｓ２、Ｙｅｓ）、動力制御部１００Ｇは、車両自動制御を中止する（Ｓ３）。また、画像処理装置１０は、ディスプレイ１００Ｅに、車両自動制御が中止された旨の表示を行う（Ｓ４）。

一方、信頼度判定部１２０において、推定したカメラ姿勢の信頼度が低くない（運動推定結果が信頼できる）と判定された場合（Ｓ２、Ｎｏ）、動力制御部１００Ｇは、車両自動制御を継続する。

図６は、画像処理装置における、カメラ姿勢・点３次元座標の推定の一連の手順を説明するフローチャートである。まず、画像処理制御部１１１は、運動推定モードを「初期モード」にセットする（Ｓ１１）。画像処理制御部１１１には、カメラ姿勢の推定を行う際の状態に応じて２つの運動推定モード（「初期モード」と「通常モード」）が登録されている。それぞれのモードは異なる運動推定方法を用いる。

「初期モード」とは、カメラ姿勢の基準となる３次元座標系が決定しておらす、カメラ姿勢の推定も行われていない状態に対応するモードである。「初期モード」に設定された場合、第１運動推定部１１５においてカメラ姿勢の推定を行う。「通常モード」とは、カメラ姿勢の基準となる３次元座標系が決定しており、時刻ｔ−１のカメラ姿勢の推定が成功している状態に対応するモードである。「通常モード」に設定された場合、第２運動推定部１１７においてカメラ姿勢の推定を行う。

運動推定モードのセットが完了すると、画像入力部１１２は、センサ１００Ｂのカメラから、時刻ｔの撮影画像（フレームｔ）を取得する（Ｓ１２）。続いて、特徴点抽出部１１３は、フレームｔから、エッジの強い点等を特徴点として抽出する（Ｓ１３）。

次に、特徴点対応付け部１１４は、フレームｔから抽出された特徴点と、一つ前のフレーム（フレームｔ−１）において抽出された特徴点とを対応付ける（Ｓ１４）。続いて、画像処理制御部１１１は、運動推定モードが「初期モード」であるか否かを確認する（Ｓ１５）。

運動推定モードが「初期モード」である場合（Ｓ１５、Ｙｅｓ）、第１運動推定部１１５において、カメラ姿勢を推定する（Ｓ１６、運動推定１の実行）。運動推定１では、フレームｔ−１とフレームｔとの対応関係から、カメラの相対的な姿勢を推定する。運動推定１によりカメラの相対的な姿勢が推定できた場合（Ｓ１７、Ｙｅｓ）、画像処理制御部１１１は、運動推定モードを「通常モード」にセットする（Ｓ２１）。続いて、点３次元座標推定部１１８において、特徴点の３次元座標を推定し（Ｓ２２）、ステップＳ１２に戻り、次のフレームを取得して、カメラ姿勢と点３次元座標の推定を行う。

一方、運動推定１によりカメラの相対的な姿勢が推定できなかった場合（Ｓ１７、Ｎｏ）、ステップＳ１２に戻り、次のフレームを取得して、運動推定１を再度試みる。

Ｓ１５において、運動推定モードが「通常モード」である場合（Ｓ１５、Ｎｏ）、運動予測部１１６において、過去に推定したカメラ姿勢から、フレームｔのカメラ姿勢である、バー付きξ^(t)を予測する（Ｓ１８）。

次に、第２運動推定部１１７において、カメラ姿勢を推定する（Ｓ１９、運動推定２の実行）。運動推定２を実行するにあたり、第２運動推定部１１７には、（１）３次元点座標、（２）３次元点に対応したフレームｔの特徴点の座標、（３）フレームｔのカメラ姿勢の初期値、が入力される。以下、個々の入力項目について説明する。

（１）３次元点座標：点３次元座標推定部１１８で推定された３次元座標点であり、かつ、フレームｔの特徴点との対応付けがなされている点の座標である。３次元点座標は、次の（４）式のように表される。

なお、Ｍは、３次元座標の数とする。また、上付きTは、行列の転置を表す。

（２）３次元点に対応したフレームｔの特徴点の座標：個々の３次元点に対応するフレームｔの特徴点の座標である。特徴点の座標は、次の（５）式のように表される。

（３）フレームｔのカメラ姿勢の初期値：運動予測部１１６において予測されたフレームｔのカメラ姿勢（バー付きξ^(t)、（２）式参照）である。

第２運動推定部１１７は、これらの入力を用いて運動推定２を実行し、時刻ｔのカメラ姿勢の推定値（ハット付きξ^(t)）を算出して出力する。なお、以下の説明においては、必要に応じて、時刻ｔのカメラ姿勢ξ^(t)、時刻ｔのカメラ姿勢の初期値（バー付きξ^(t)）、時刻ｔのカメラ姿勢の推定値（ハット付きξ^(t)）、における（ｔ）を省略し、それぞれξ、バー付きξ、ハット付きξと記す。

運動推定２では、ガウスニュートン法や、レーベンバーグ・マーカート法など、非線形最小二乗問題を解くアルゴリズムを用いて、ξを推定する。以下、ガウスニュートン法を用いる場合のパラメータ更新式について説明する。

運動推定２では、次の（６）式に示す投影誤差Ｅ（ξ）を目的関数とし、ξを推定する。

ここで、３次元点ｉのフレームｔにおける投影座標は、次の（７）式のように表される。

３次元点ｉのフレームｔにおける投影座標は、３次元点座標を（８）〜（１０）式を用いて変換して算出する。

なお、（８）式において、Ｒ_ξとＴ_ξはそれぞれ回転ベクトルと並進ベクトルであり、ξから変換したものとする。また、（９）式におけるｆ_ｘは、ｘ方向の焦点距離とし、（１０）式におけるｆ_ｙは、ｙ軸方向の焦点距離とする。

ガウスニュートン法では、繰り返し計算でのパラメータ更新は、以下の（１１）式を用いて更新量Δξを計算する。

（１１）式において、ｇは投影誤差Ｅの勾配ベクトルであり、以下の（１２）式で表される。

また、（１１）式において、Ｈは投影誤差Ｅのヘッセ行列であり、以下の（１３）式で表される。

次に、運動推定２（図６のＳ１９）の具体的な手順について、図７を用いて説明する。図７は、第１の実施形態に係わる運動推定２の一連の手順を説明するフローチャートである。

まず、バー付きξをξに代入し、ξに初期値を設定する（Ｓ１０１）。次に、ｇとＨを、それぞれ（１２）式、（１３）式を用いて算出する（Ｓ１０２）。続いて、Ｓ１０２で算出したｇとＨとを（１１）式に代入し、カメラ姿勢の更新量Δξを計算する（Ｓ１０３）。次に、Ｓ１０３で算出した更新量Δξを用い、カメラ姿勢ξを更新する（Ｓ１０４）。

続いて、ξの収束判定を行なう（Ｓ１０５）。ξの更新回数が予め設定された回数に達した場合、または、設定された収束条件を満たした場合（Ｓ１０５、Ｙｅｓ）、パラメータ更新処理を終了し、その時点でのξを推定結果（ハット付きξ）とする（Ｓ１０６）。

Ｓ１０５における収束条件としては、例えば、次の（１４）式、または（１５）式を用いる。

（１４）（１５）式において、Ｔ_Ａ、Ｔ_Ｂは、予め設定された固定の閾値である。（１４）式は、３次元点１点あたりの投影誤差の平均が予め設定された閾値未満であるか否かを判定する場合の判定式を示している。また、（１５）式は、推定対象であるパラメータの更新量を、推定パラメータのノルムで割って正規化した値が予め設定された閾値未満であるか否かを判定する場合の判定式を示している。

ξの更新回数が予め設定された回数に達していない場合、かつ、設定された収束条件を満していない場合（Ｓ１０５、Ｎｏ）、Ｓ１０２に戻り、パラメータ更新処理を継続する。

Ｓ１０６において、カメラ姿勢の推定結果（ハット付きξ）を決定したら、当該推定結果の信頼性を評価する。本実施形態における信頼性評価には、画像のブロックマッチングにおける推定結果の信頼性の評価方法を応用する。

ここで、画像のブロックマッチングにおける推定結果の信頼性の一般的な評価方法について説明する。図８は、推定結果の信頼性評価に用いる画像のブロックマッチングを説明する図である。図８（ａ）は、時刻ｔ−１の画像ｌ_ｔ−１（ｘ、ｙ）を示しており、図８（ｂ）は、時刻ｔの画像ｌ_ｔ（ｘ、ｙ）を示している。画像のブロックマッチングとは、時刻ｔ−１の画像ｌ_ｔ−１（ｘ、ｙ）中のブロックＴ（ｘ、ｙ）に対応するブロックを、時刻ｔの画像ｌ_ｔ（ｘ、ｙ）から探索することである。

ここで、ブロックＴ（ｘ、ｙ）の幅と高さを共にＮピクセルとし、（ｕ_０、ｖ_０）をｌ_ｔ−１（ｘ、ｙ）におけるＴ（ｘ、ｙ）の左上の座標とする。ｌ_ｔ（ｘ、ｙ）において、図８（ｂ）に示す直線Ｌ上を探索する場合、Ｔ（ｘ、ｙ）と、ｌ_ｔ（ｘ、ｙ）の直線Ｌ上に位置するブロックＴ´（ｘ、ｙ）との正規化相関を計算する。正規化相関は、次の（１６）式により算出される。

（１６）式において、（ｕ（η）、ｖ（η））は、ｌ_ｔ（ｘ、ｙ）中のブロックＴ´（ｘ、ｙ）の左上の座標とする。また、ηは、直線Ｌ上でＴ（ｘ、ｙ）の左上の点からＴ´（ｘ、ｙ）の左上の点までの長さとし、図８の位置関係の場合にηが正であるとする。η＝０の場合、Ｔ´（ｘ、ｙ）の左上の座標は（ｕ_０、ｖ_０）となり、Ｔ（ｘ、ｙ）の左上の座標と一致する。

ブロックマッチングでは、Ｓ（η）が最大となるηを推定結果ハット付きηとする。このとき、Ｔ（ｘ、ｙ）に対応するＴ´（ｘ、ｙ）の左上の座標は、（ｕ（ハット付きη）、ｖ（ハット付きη））となる。

推定結果の信頼性の評価方法としては、推定結果ハット付きη周辺でのＳ（η）の急峻性を信頼指標として用いる方法がある。つまり、Ｓ（ハット付きη）が、周囲に比較して突出して大きければ信頼性が高いと判定する。図９は、推定結果の信頼性評価方法を説明する図である。図９（ｂ）に示すＳ（η）のほうが、図９（ａ）に示すＳ（η）に比べ、推定結果ハット付きηの近傍での急峻性が高い。従って、図９（ｂ）に示すＳ（ハット付きη）のほうが図９（ａ）に示すＳ（ハット付きη）よりも信頼性が高いとみなすことができる。

信頼性を数値として算出する場合、信頼度特徴量ｃｆ（ハット付きη）は、例えば、以下に示す（１７）式、または、（１８）式を用いることができる。

（１７）（１８）式において、Ｕ（ハット付きη）は、ハット付きηから一定距離離れたハット付きηの近傍点の集合とする。（１７）（１８）式は、推定結果ハット付きηに対応するＳ（ハット付きη）が、ハット付きηの近傍に比較してどの程度大きいかを表している。（１７）式は、比率を用いて比較しており、（１８）式は、差を用いて比較している。

例えば、Ｕ（ハット付きη）を（１９）式に示す集合とした場合、（１７）式は（２０）式のようになり、（１８）式は（２１）式のように表すことができる。

（２０）式、または（２１）式を用いた具体的な信頼性の算出方法について、図１０を用いて説明する。図１０は、推定結果の信頼性評価方法を説明する図である。図１０において、Ｕ（ハット付きη）の下限、上限についてのＳ（η）の値を、それぞれｂ、ｃ（ｂ＞ｃ）とし、Ｓ（ハット付きη）の値をａとする。すなわち、各Ｓ（η）を、以下の（２２）〜（２４）式に示す値とする。

この場合、（２０）式は、以下に示す（２５）式となる。

また、（２１）式は、以下に示す（２６）式となる。

上述した画像のブロックマッチングにおける推定結果の信頼性の一般的な評価方法を、本実施形態の運動推定２の推定結果の信頼性評価に用いる場合、以下の相違点を考慮して（１７）式、または、（１８）式を修正する。

１点目は、画像のブロックマッチングの場合、目的関数Ｓ（η）が最大となるパラメータを推定するのに対し、運動推定２では目的関数Ｅ（ξ）が最小となるパラメータを推定する点である。従って、画像のブロックマッチングの場合は近傍点の最大値を計算するが、運動推定２では近傍点の最小値を計算する。

２点目は、画像のブロックマッチングの場合、推定値における目的関数Ｓ（ハット付きη）が近傍点に対して大きいほど信頼性が高いとみなすのに対し、運動推定２では、推定値における目的関数Ｅ（ハット付きξ）が近傍点に対して小さいほど信頼性が高いとみなす点である。

従って、比率を用いて信頼度特徴量を算出する場合、画像のブロックマッチングでは近傍点の最大値を目的関数Ｓ（ハット付きη）で除して算出するが、運動推定２では、近傍点の最小値を目的関数Ｅ（ハット付きξ）で除して算出する。また、差を用いて信頼度特徴量を算出する場合、画像のブロックマッチングでは目的関数Ｓ（ハット付きη）から近傍点の最大値を差し引いて算出するが、運動推定２では近傍点の最小値から目的関数Ｅ（ハット付きξ）を差し引いて算出する。

３点目は、画像のブロックマッチングの場合、ブロックＴ（ｘ、ｙ）の幅と高さ（＝Ｎ）は固定値であるのに対し、運動推定２では投影誤差計算に用いられる３次元点の数（＝Ｍ）が時刻で変動する点である。従って、差分を用いて信頼度特徴量を算出する場合、運動推定２では、投影誤差の差が時刻で変動するＭに依存するのを防ぐために、近傍点の最小値と目的関数Ｅ（ハット付きξ）との差分をＭで除して算出する。

以上の３点を考慮して（１７）（１８）式を応用した、運動推定２における信頼度特徴量を算出する式は、以下の（２７）（２８）式が考えられる。

（２７）（２８）式において、Ｕ（ハット付きξ）は、ハット付きξから一定距離離れた近傍の点の集合とする。例えば、以下に示す（２９）式を満たすξをサンプリングした集合が考えられる。

なお、（２９）式において、ｄは定数とする。（２７）（２８）式で算出する信頼度特徴量ｃｆ（ハット付きξ）は、Ｅ（ハット付きξ）がハット付きξの近傍に比較してどの程度小さいかを表している。（２７）式は（１７）式を応用したものであり、比較に比率を用いている。（２８）式は（１８）式を応用したものであり、比較に差を用いている。（２７）（２８）式共に、Ｅ（ハット付きξ）がハット付きξの近傍に対して小さいほど、信頼度特徴量ｃｆ（ハット付きξ）は大きくなる。

信頼度特徴量ｃｆ（ハット付きξ）の算出にあたっては、推定結果ハット付きξの周辺で目的関数Ｅ（ξ）を算出する必要があり、（６）（８）（９）（１０）式を何度も計算しなくてはならない。また、目的関数Ｅ（ξ）の計算は、３次元点の数Ｍが大きくなるほど計算量が増大する。しかし、信頼度特徴量の算出に時間を要すると、移動体自動制御継続可否の判断が遅れてしまい、タイムリーに自動制御の中止を行えなくなってしまう可能性がある。そこで、本実施形態においては、信頼度特徴量ｃｆ（ハット付きξ）を高速に計算するため、（３０）式に示すように、テイラー展開を用いて推定結果ハット付きξの周辺で目的関数Ｅ（ξ）を算出する。

図７のフローチャートに戻り、Ｓ１０６において決定された、カメラ姿勢の推定結果（ハット付きξ）の信頼性を評価する。信頼性評価に用いる信頼度特徴量は、（２７）式、または、（２８）式を用いて算出する。いずれの式においても、推定結果ハット付きξの周辺で目的関数Ｅ（ξ）を算出する際には（３０）式を用いる。

そこで、信頼度特徴量の計算に先立ち、Ｅ（ハット付きξ）、ｇ、Ｈを算出する（Ｓ１０７）。なお、Ｅ（ハット付きξ）は（６）式を、ｇは（１２）式を、Ｈは（１３）式を、それぞれ用いて算出する。

続いて、Ｓ１０７での算出結果に基づき、（２７）式、または、（２８）式を用いて信頼度特徴量ｃｆ（ハット付きξ）を算出する（Ｓ１０８）。Ｓ１０８において、推定結果ハット付きξの周辺で目的関数Ｅ（ξ）を算出する際には（３０）式を用いることにより、高速に信頼度特徴量ｃｆ（ハット付きξ）を算出することができる。

最後に、信頼度特徴量ｃｆ（ハット付きξ）を予め設定した閾値Ｔと比較する（Ｓ１０９）。信頼度特徴量ｃｆ（ハット付きξ）が閾値Ｔより大きい場合、運動推定結果であるハット付きξが信頼できる（信頼性が高い）と判定する。一方、信頼度特徴量ｃｆ（ハット付きξ）が閾値Ｔ以下の場合、運動推定結果であるハット付きξは信頼性が低いと判定する。

運動推定結果であるハット付きξと、信頼度特徴量ｃｆ（ハット付きξ）による信頼性判定結果を出力し、図７に示す運動推定２の一連の手順を終了する。

図６のフローチャートに戻り、Ｓ１９において運動推定２が成功した場合（Ｓ２０、Ｙｅｓ）、Ｓ２２に進み、点３次元座標推定部１１８において、特徴点の３次元座標を推定する。フレームが継続されている場合は、ステップＳ１２に戻り、次のフレームを取得して、カメラ姿勢と点３次元座標の推定を行う。一方、Ｓ１９において運動推定２が失敗した場合（Ｓ２０、Ｎｏ）、Ｓ１１に戻り、運動推定モードを「初期モード」にセットして、カメラ姿勢・点３次元座標の推定の一連の手順を最初からやり直す。

このように、本実施形態によれば、運動推定２におけるカメラ姿勢の推定結果が信頼できるか否かを、推定結果の信頼度特徴量を算出して判定する。信頼度特徴量を求める際に、テイラー展開を用いてハット付きξ近傍のＥ（ξ）を算出しているので、信頼性の高い推定結果を高速に算出することができる。また、信頼度が高いと判定された場合のみ移動体の自動制御を継続し、信頼度が低いと判定された場合には移動体の自動制御を中止し、かつ、運転者に自動制御の中止を通知する。従って、運転者が自身で移動体を運転しなければならないことを容易に認識できるので、より安全に移動体の制御を行なうことができる。

なお、上述では、運動推定２においてガウスニュートン法を用いてξを推定した一例を説明したが、ξの推定に用いるアルゴリズムは、非線形最小二乗問題を解くアルゴリズムであればよく、ガウスニュートン法に限定されるものではない。例えば、レーベンバーグ・マーカート法を用いてξの推定を行なってもよい。

レーベンバーグ・マーカート法では、繰り返し計算でのパラメータ更新は、以下の（３１）式を用いて更新量Δξを計算する。

（３１）式において、Ｈ^＊は、Ｈの対角成分にダンピングファクタλを足した行列であり、以下の（３２）式で表される。

（３２）式において、Ｉは単位行列を表す。また、ダンピングファクタλは、処理開始後は固定の値を設定し、繰り返し計算の中で更新される。

以下、レーベンバーグ・マーカート法を用いた場合の運動推定２の一連の手順について、図１１を用いて説明する。図１１は、第１の実施形態に係わる運動推定２の別の手順を説明するフローチャートである。まず、バー付きξをξに代入し、ξに初期値を設定する（Ｓ１１１）。次に、λに初期値を設定する（Ｓ１１２）。続いて、ｇとＨとＨ^＊を、それぞれ（１２）式、（１３）式、（３２）式を用いて算出する（Ｓ１１３）。

続いて、Ｓ１１３で算出したｇ、Ｈ、Ｈ^＊を（３１）式に代入し、カメラ姿勢の更新量Δξを計算する（Ｓ１１４）。ξ＋Δξとξについて、それぞれ投影誤差Ｅ（ξ＋Δξ）、Ｅ（ξ）を算出して比較する（Ｓ１１５）。Ｅ（ξ＋Δξ）がＥ（ξ）以上である場合（Ｓ１１５、Ｙｅｓ）、カメラ姿勢ξの更新はせず、λを１０倍する（Ｓ１１８）一方、Ｅ（ξ＋Δξ）がＥ（ξ）未満である場合（Ｓ１１５、Ｎｏ）、Ｓ１１４で算出した更新量Δξを用いてカメラ姿勢ξを更新し（Ｓ１１６）、λを０．１倍する（Ｓ１１７）。

Ｓ１１８、またはＳ１１７でλを更新後、続いて、ξの収束判定を行なう（Ｓ１１９）。ξの更新回数が予め設定された回数に達した場合、または、設定された収束条件を満たした場合（Ｓ１１９、Ｙｅｓ）、パラメータ更新処理を終了し、その時点でのξを推定結果（ハット付きξ）とする（Ｓ１２０）。Ｓ１２０における収束判定は、図７のＳ１０５と同様、（１４）式、または（１５）式を用いて行なう。一方、ξの更新回数が予め設定された回数に達していない場合、かつ、設定された収束条件を満していない場合（Ｓ１１９、Ｎｏ）、Ｓ１１３に戻り、パラメータ更新処理を継続する。

Ｓ１２０において、カメラ姿勢の推定結果（ハット付きξ）を決定したら、当該推定結果の信頼性を評価する。信頼性の評価（Ｓ１２１〜Ｓ１２３）は、図７のＳ１０７〜Ｓ１０９と同様の手順で行なう。運動推定結果であるハット付きξと、信頼度特徴量ｃｆ（ハット付きξ）による信頼性判定結果を出力し、図１１に示す運動推定２の一連の手順を終了する。

このように、レーベンバーグ・マーカート法を用いて運動推定２を行なう場合にも、運動推定２におけるカメラ姿勢の推定結果が信頼できるか否かを、推定結果の信頼度特徴量を算出して判定する。信頼度特徴量を求める際に、テイラー展開を用いてハット付きξ近傍のＥ（ξ）を算出しているので、信頼性の高い推定結果を高速に算出することができる。

なお、上述では、信頼度特徴量の計算量を削減するために、テイラー展開を用いてハット付きξ近傍のＥ（ξ）を算出しているが、６変数であるＥ（ξ）を、２変数の関数Ｅ（ξ_ｉ、ξ_ｊ）とみなして計算することで、信頼度特徴量の計算量を更に削減することができる。なお、ξ_ｉ、ξ_ｊ以外のξの成分は、ハット付きξの対応する成分に等しいとする。つまり、ξ_ｋ＝ハット付きξ_ｋ（ｋ≠ｉかつｋ≠ｊ）とする。

信頼度特徴量は、定義上、ｃｆ（ξ_ｉ、ξ_ｊ）＝ｃｆ（ξ_ｊ、ξ_ｉ）が成立するので、ｃｆ（ξ_ｉ、ξ_ｊ）とｃｆ（ξ_ｊ、ξ_ｉ）はどちらか一方のみを計算すればよい。例えば、ｉ≠ｊかつｉ＜ｊについて、ｃｆ（ξ_ｉ、ξ_ｊ）を計算する。具体的には、ｃｆ（ξ_０、ξ_１）、ｃｆ（ξ_０、ξ_２）、ｃｆ（ξ_０、ξ_３）、ｃｆ（ξ_０、ξ_４）、ｃｆ（ξ_０、ξ_５）、ｃｆ（ξ_１、ξ_２）、ｃｆ（ξ_１、ξ_３）、ｃｆ（ξ_１、ξ_４）、ｃｆ（ξ_１、ξ_５）、ｃｆ（ξ_２、ξ_３）、ｃｆ（ξ_２、ξ_４）、ｃｆ（ξ_２、ξ_５）、ｃｆ（ξ_３、ξ_４）、ｃｆ（ξ_３、ξ_５）、ｃｆ（ξ_４、ξ_５）の、１５通りの信頼度特徴量を計算する。

また、近傍点Ｕ（ハット付きξ）についてのＥ（ξ_ｉ、ξ_ｊ）の計算は、例えば、固定の値Δξ_ｉとΔξ_ｊとを予め決めておき、Ｅ（ハット付きξ_ｉ＋Δξ_ｉ、ハット付きξ_ｊ＋Δξ_ｊ）、Ｅ（ハット付きξ_ｉ−Δξ_ｉ、ハット付きξ_ｊ＋Δξ_ｊ）、Ｅ（ハット付きξ_ｉ＋Δξ_ｉ、ハット付きξ_ｊ−Δξ_ｊ）、Ｅ（ハット付きξ_ｉ−Δξ_ｉ、ハット付きξ_ｊ−Δξ_ｊ）の４通りについて行なう。

比率を用いて信頼度特徴量を算出する場合、ｃｆ（ξ_ｉ、ξ_ｊ）は次に示す（３３）式で計算する。

ここで、（３３）式の分子のＥ（ハット付きξ_ｉ＋Δξ_ｉ、ハット付きξ_ｊ＋Δξ_ｊ）、Ｅ（ハット付きξ_ｉ−Δξ_ｉ、ハット付きξ_ｊ＋Δξ_ｊ）、Ｅ（ハット付きξ_ｉ＋Δξ_ｉ、ハット付きξ_ｊ−Δξ_ｊ）、Ｅ（ハット付きξ_ｉ−Δξ_ｉ、ハット付きξ_ｊ−Δξ_ｊ）は、テイラー展開した（３４）式により近似計算する。また、Δξ_ｉとΔξ_ｊとは、予め決められた固定の値とする。

差を用いて信頼度特徴量を算出する場合、ｃｆ（ξ_ｉ、ξ_ｊ）は次に示す（３５）式で計算する。

ここで、（３５）式の分子のＥ（ハット付きξ_ｉ＋Δξ_ｉ、ハット付きξ_ｊ＋Δξ_ｊ）、Ｅ（ハット付きξ_ｉ−Δξ_ｉ、ハット付きξ_ｊ＋Δξ_ｊ）、Ｅ（ハット付きξ_ｉ＋Δξ_ｉ、ハット付きξ_ｊ−Δξ_ｊ）、Ｅ（ハット付きξ_ｉ−Δξ_ｉ、ハット付きξ_ｊ−Δξ_ｊ）も、テイラー展開した上述の（３４）式により近似計算する。

信頼度判定は、ｃｆ（ξ_ｉ、ξ_ｊ）が、全てのｉとｊの組み合わせについて、予め決められた閾値Ｔ_ｉｊより大きければ、推定結果ハット付きξが信頼できると判定する。このように、６変数であるＥ（ξ）を、２変数の関数Ｅ（ξ_ｉ、ξ_ｊ）とみなして計算することで、信頼度特徴量の計算量を更に削減することができる。

なお、６変数であるＥ（ξ）を、１変数ξ_ｉの関数Ｅ（ξ_ｉ）とみなして計算することで、信頼度特徴量の計算量を更に削減することができる。このとき、ξ_ｉ以外のξの成分は、ハット付きξの対応する成分に等しいとする。つまり、ξ_ｋ＝ハット付きξ_ｋ（ｋ≠ｉ）とする。

近傍点Ｕ（ハット付きξ）についてのＥ（ξ_ｉ）の計算は、例えば、固定の値Δξ_ｉを予め決めておき、Ｅ（ハット付きξ_ｉ＋Δξ_ｉ）、Ｅ（ハット付きξ_ｉ−Δξ_ｉ）の２通りについて行なう。

比率を用いて信頼度特徴量を算出する場合、ｃｆ（ξ_ｉ）は次に示す（３６）式で計算する。

ここで、（３６）式の分子のＥ（ハット付きξ_ｉ＋Δξ_ｉ）と、Ｅ（ハット付きξ_ｉ−Δξ_ｉ）は、テイラー展開した（３４）式により近似計算する。また、Δξ_ｉは、予め決められた固定の値とする。

差を用いて信頼度特徴量を算出する場合、ｃｆ（ξ_ｉ）は次に示す（３７）式で計算する。

ここで、（３７）式の分子のＥ（ハット付きξ_ｉ＋Δξ_ｉ）と、Ｅ（ハット付きξ_ｉ−Δξ_ｉ）も、テイラー展開した上述の（３４）式により近似計算する。

信頼度判定は、ｃｆ（ξ_ｉ）が、全てのｉ（ｉ＝０、１、…、５）について、予め決められた閾値Ｔ_ｉより大きければ、推定結果ハット付きξが信頼できると判定する。このように、６変数であるＥ（ξ）を、１変数の関数Ｅ（ξ_ｉ）とみなして計算することで、信頼度特徴量の計算量を更に削減することができる。

次に、本実施形態における変形例について、図１２を用いて説明する。図１２は、第１の実施形態の変形例に係わる運動推定２の一連の手順を説明するフローチャートである。図７を用いて説明した運動推定２におけるカメラ姿勢の推定手順では、パラメータの繰り返し計算が終了して推定結果ハット付きξが決定した後に、信頼度特徴量を算出するためにＥ（ハット付きξ）、ｇ、Ｈの計算を行っている（図７、Ｓ１０７）。ｇ、Ｈの計算は、繰り返し計算内で実行しているので（図７、Ｓ１０２）、Ｅ（ξ）も繰り返し計算内で計算することにより、推定結果ハット付きξが決定した後にＥ（ハット付きξ）を算出する手順を省略することが可能となる。

本変形例における運動推定２の具体的な手順について、図１２を用いて説明する。まず、図７の手順と同様に、バー付きξをξに代入し、ξに初期値を設定する（Ｓ１０１）。次に、ｇとＨを、それぞれ（１２）式、（１３）式を用いて算出する（Ｓ１０２）。続いて、（６）式を用いてＥ（ξ）を算出する（Ｓ１０７´）。

次に、図７の手順と同様に、Ｓ１０２で算出したｇとＨとを（１１）式に代入し、カメラ姿勢の更新量Δξを計算する（Ｓ１０３）。次に、Ｓ１０３で算出した更新量Δξを用い、カメラ姿勢ξを更新し（Ｓ１０４）、ξの収束判定を行なう（Ｓ１０５）。

ξの更新回数が予め設定された回数に達した場合、または、設定された収束条件を満たした場合（Ｓ１０５、Ｙｅｓ）、パラメータ更新処理を終了し、その時点でのξを推定結果（ハット付きξ）とする（Ｓ１０６）。一方、ξの更新回数が予め設定された回数に達していない場合、かつ、設定された収束条件を満していない場合（Ｓ１０５、Ｎｏ）、Ｓ１０２に戻り、パラメータ更新処理を継続する。

最後に、Ｓ１０２〜Ｓ１０５の繰り返し計算内において、最後に算出したＥ（ξ）、ｇ、Ｈを用いて、推定結果（ハット付きξ）の信頼度特徴量を算出する（Ｓ１０８）。このとき、最後に算出したＥ（ξ）を、Ｅ（ハット付きξ）とする。Ｓ１０８で算出した信頼度特徴量に基づき信頼度を判定し（Ｓ１０９）、運動推定結果であるハット付きξと、信頼度特徴量ｃｆ（ハット付きξ）による信頼性判定結果を出力して、図１２に示す運動推定２の変形例の一連の手順を終了する。

このように、繰り返し手順内でＥ（ξ）を算出し、最後に算出したＥ（ξ）、ｇ、Ｈを用いて信頼度特徴量を算出することで、信頼度特徴量算出のために別途Ｅ（ξ）を算出する手順を省略することができ、計算量を削減することができる。

なお、上述では、ξの推定に用いるアルゴリズムとしてガウスニュートン法を用いた運動推定２の変形例の手順を、図１２を用いて説明したが、レーベンバーグ・マーカート法を用いる場合にも、同様にして信頼度特徴量算出のために別途Ｅ（ξ）を算出する手順を省略することができる。

以下、レーベンバーグ・マーカート法を用いた場合の、変形例における運動推定２の一連の手順について、図１３を用いて説明する。図１３は、第１の実施形態の変形例に係わる運動推定２の別の手順を説明するフローチャートである。まず、図１１に示す手順と同様に、バー付きξをξに代入し、ξに初期値を設定する（Ｓ１１１）。次に、λに初期値を設定する（Ｓ１１２）。続いて、ｇとＨとＨ^＊を、それぞれ（１２）式、（１３）式、（３２）式を用いて算出する（Ｓ１１３）。続いて、（６）式を用いてＥ（ξ）を算出する（Ｓ１２１´）。

次に、図１１の手順と同様に、Ｓ１１３で算出したｇ、Ｈ、Ｈ^＊を（３１）式に代入し、カメラ姿勢の更新量Δξを計算する（Ｓ１１４）。ξ＋Δξとξについて、それぞれ投影誤差Ｅ（ξ＋Δξ）、Ｅ（ξ）を算出して比較し、Ｅ（ξ＋Δξ）がＥ（ξ）以上である場合（Ｓ１１５、Ｙｅｓ）、カメラ姿勢ξの更新はせず、λを１０倍する（Ｓ１１８）一方、Ｅ（ξ＋Δξ）がＥ（ξ）未満である場合（Ｓ１１５、Ｎｏ）、Ｓ１１４で算出した更新量Δξを用いてカメラ姿勢ξを更新し（Ｓ１１６）、λを０．１倍する（Ｓ１１７）。

Ｓ１１８、またはＳ１１７でλを更新後、続いて、ξの収束判定を行なう（Ｓ１１９）。ξの更新回数が予め設定された回数に達した場合、または、設定された収束条件を満たした場合（Ｓ１１９、Ｙｅｓ）、パラメータ更新処理を終了し、その時点でのξを推定結果（ハット付きξ）とする（Ｓ１２０）。一方、ξの更新回数が予め設定された回数に達していない場合、かつ、設定された収束条件を満していない場合（Ｓ１１９、Ｎｏ）、Ｓ１１３に戻り、パラメータ更新処理を継続する。

最後に、Ｓ１１３〜Ｓ１１９の繰り返し計算内において、最後に算出したＥ（ξ）、ｇ、Ｈを用いて、推定結果（ハット付きξ）の信頼度特徴量を算出する（Ｓ１２２）。このとき、最後に算出したＥ（ξ）を、Ｅ（ハット付きξ）とする。Ｓ１２２で算出した信頼度特徴量に基づき信頼度を判定し（Ｓ１２３）、運動推定結果であるハット付きξと、信頼度特徴量ｃｆ（ハット付きξ）による信頼性判定結果を出力して、図１３に示す運動推定２の変形例の一連の手順を終了する。

このように、レーベンバーグ・マーカート法を用いる場合においても、繰り返し手順内でＥ（ξ）を算出し、最後に算出したＥ（ξ）、ｇ、Ｈを用いて信頼度特徴量を算出することで、信頼度特徴量算出のために別途Ｅ（ξ）を算出する手順を省略することができ、計算量を削減することができる。

（第２の実施形態）
上述の第１の実施形態では、設定された収束条件（３次元点１点あたりの投影誤差の平均が予め設定された閾値未満であるか否かを、または、パラメータの更新量を、推定パラメータのノルムで割って正規化した値が予め設定された閾値未満であるか否か）を満たす場合に、運動推定２におけるカメラ姿勢ξの繰り返し計算が収束したと判定している。これに対し、本実施形態においては、繰り返し計算内でカメラ姿勢ξの推定値の信頼性を評価し、信頼性が高い場合に運動推定２におけるカメラ姿勢ξの繰り返し計算が収束したと判定する点が異なっている。本実施形態における移動体制御装置の構成は、図１〜図４に示す第１の移動体制御装置と同様であるので、説明を省略する。

図１４は、第２の実施形態に係わる運動推定２の一連の手順を説明するフローチャートである。図１４は、ガウスニュートン法を用いて運動推定２を行う場合の手順を示している。

まず、バー付きξをξに代入し、ξに初期値を設定する（Ｓ１０１）。次に、ｇとＨを、それぞれ（１２）式、（１３）式を用いて算出する（Ｓ１０２）。続いて、Ｓ１０２で算出したｇとＨとを（１１）式に代入し、カメラ姿勢の更新量Δξを計算する（Ｓ１０３）。次に、Ｓ１０３で算出した更新量Δξを用い、カメラ姿勢ξを更新する（Ｓ１０４）。ここまでの手順は、図７を用いて説明した第１の実施形態における手順と全く同様である。

次に、（６）式を用いてＥ（ξ）を算出し、Ｓ１０２で算出したｇとＨとあわせて、カメラ姿勢ξの信頼度特徴量を算出する（Ｓ２０１）。続いて、算出した信頼度特徴量に基づき信頼度を判定する（Ｓ２０２）。

ξの更新回数が予め設定された回数に達した場合、または、Ｓ２０２において信頼度が高いと判定された場合（Ｓ２０３、Ｙｅｓ）、パラメータ更新処理を終了し、その時点でのξを推定結果（ハット付きξ）とする（Ｓ１０６）。一方、ξの更新回数が予め設定された回数に達していない場合、かつ、Ｓ２０２において信頼度が低いと判定された場合（Ｓ２０３、Ｎｏ）、Ｓ１０２に戻り、パラメータ更新処理を継続する。

最後に、Ｓ１０６において決定された、カメラ姿勢の推定結果（ハット付きξ）の信頼性を評価する（Ｓ１０７〜Ｓ１０９）。Ｓ１０７〜Ｓ１０９の手順は、図７を用いて説明した第１の実施形態における手順と全く同様であるので、説明を省略する。なお、Ｓ２０３における収束判定において、カメラ姿勢ξの信頼度が高いと判定された場合、Ｓ１０７〜Ｓ１０９における信頼性評価の一連の手順は省略可能である。

このように、本実施形態によれば、カメラ姿勢の推定を行うための繰り返し計算における収束判定を、カメラ姿勢の信頼度に基づき行っている。従って、信頼度の高い推定結果を得ることができるので、移動体自動制御の中止頻度を低減することができ、運転者の利便性を向上させることができる。また、第１の実施形態と同様に、推定結果の信頼度特徴量を求める際に、テイラー展開を用いてハット付きξ近傍のＥ（ξ）を算出しているので、信頼性の高い推定結果を高速に算出することができる。更に、信頼度が高いと判定された場合のみ移動体の自動制御を継続し、信頼度が低いと判定された場合には移動体の自動制御を中止し、かつ、運転者に自動制御の中止を通知する。従って、運転者が自身で移動体を運転しなければならないことを容易に認識できるので、より安全に移動体の制御を行なうことができる。

なお、上述では、ξの推定に用いるアルゴリズムとしてガウスニュートン法を用いた手順を説明したが、レーベンバーグ・マーカート法を用いる場合にも、同様にして本実施形態における運動推定２を実施することができる。

以下、レーベンバーグ・マーカート法を用いた場合の、第２の実施形態における運動推定２の一連の手順について、図１５を用いて説明する。図１５は、第２の実施形態に係わる運動推定２の別の手順を説明するフローチャートである。

まず、バー付きξをξに代入し、ξに初期値を設定する（Ｓ１１１）。次に、λに初期値を設定する（Ｓ１１２）。続いて、ｇとＨとＨ^＊を、それぞれ（１２）式、（１３）式、（３２）式を用いて算出する（Ｓ１１３）。続いて、Ｓ１１３で算出したｇ、Ｈ、Ｈ^＊を（３１）式に代入し、カメラ姿勢の更新量Δξを計算する（Ｓ１１４）。ξ＋Δξとξについて、それぞれ投影誤差Ｅ（ξ＋Δξ）、Ｅ（ξ）を算出して比較し、Ｅ（ξ＋Δξ）がＥ（ξ）以上である場合（Ｓ１１５、Ｙｅｓ）、カメラ姿勢ξの更新はせず、λを１０倍する（Ｓ１１８）一方、Ｅ（ξ＋Δξ）がＥ（ξ）未満である場合（Ｓ１１５、Ｎｏ）、Ｓ１１４で算出した更新量Δξを用いてカメラ姿勢ξを更新し（Ｓ１１６）、λを０．１倍する（Ｓ１１７）。ここまでの手順は、図１１を用いて説明した第１の実施形態における手順と全く同様である。

Ｓ１１８、またはＳ１１７でλを更新後、（６）式を用いてＥ（ξ）を算出し、Ｓ１１３で算出したｇとＨとあわせて、カメラ姿勢ξの信頼度特徴量を算出する（Ｓ２１１）。続いて、算出した信頼度特徴量に基づき信頼度を判定する（Ｓ２１２）。

ξの更新回数が予め設定された回数に達した場合、または、Ｓ２１２において信頼度が高いと判定された場合（Ｓ２１３、Ｙｅｓ）、パラメータ更新処理を終了し、その時点でのξを推定結果（ハット付きξ）とする（Ｓ１２０）。一方、ξの更新回数が予め設定された回数に達していない場合、かつ、Ｓ２１２において信頼度が低いと判定された場合（Ｓ２１３、Ｎｏ）、Ｓ１１３に戻り、パラメータ更新処理を継続する。

最後に、Ｓ１２０において決定された、カメラ姿勢の推定結果（ハット付きξ）の信頼性を評価する（Ｓ１２１〜Ｓ１２３）。Ｓ１２１〜Ｓ１２３の手順は、図１１を用いて説明した第１の実施形態における手順と全く同様であるので、説明を省略する。なお、Ｓ２１３における収束判定において、カメラ姿勢ξの信頼度が高いと判定された場合、Ｓ１２１〜Ｓ１２３における信頼性評価の一連の手順は省略可能である。

このように、レーベンバーグ・マーカート法を用いる場合においても、カメラ姿勢の推定を行うための繰り返し計算における収束判定を、カメラ姿勢の信頼度に基づき行うことにより信頼度の高い推定結果を得ることができる。従って、移動体自動制御の中止頻度を低減することができ、運転者の利便性を向上させることができる。

なお、本実施形態においても、第１の実施形態の変形例と同様に、パラメータ推定の繰り返し手順内でＥ（ξ）を算出し、最後に算出したＥ（ξ）、ｇ、Ｈを用いて信頼度特徴量を算出することで、カメラ姿勢の推定結果（ハット付きξ）が算出された後に、信頼度特徴量算出のために別途Ｅ（ξ）を算出する手順を省略することができる。

また、第１の実施形態と同様に、信頼度特徴量の計算において、６変数であるＥ（ξ）を２変数の関数Ｅ（ξ_ｉ、ξ_ｊ）や、１変数ξ_ｉの関数Ｅ（ξ_ｉ）とみなして計算することができる。

（第３の実施形態）
第１、第２の実施形態では、運動推定２におけるカメラ姿勢ξの繰り返し計算の収束判定を、外界センサであるカメラから取得した情報（画像情報）に基づいて行っていたが、第３実施形態では、内界センサである車両センサから取得する車両情報に基づき行う点が異なっている。

本実施形態における移動体制御装置の構成は、図１〜図４に示す第１の移動体制御装置と同様であるが、センサ１００Ｂには、内界センサとして車両センサが含まれている。車両センサから画像処理装置１０に対し、車両情報が入力される。車両情報とは、車両センサから取得するデータであり、例えば、ヨーレートセンサから取得する自車の旋回運動の回転量である。回転量としては、具体的には、時刻ｔとｔ−１間のヨー角の変化Δα_ｙаｗ（ｔ、ｔ−１）を取得する。

図１６は、第３の実施形態に係わる運動推定２の一連の手順を説明するフローチャートである。図１６は、ガウスニュートン法を用いて運動推定２を行う場合の手順を示している。

次に、フレームｔのカメラ姿勢の推定値ξと、フレームｔ−１のカメラ姿勢の推定結果ハット付きξ^(t-1)の間でのヨー角の変化であるΔθ_ｙаｗ（ξ、ξ^(t-1)）を算出する（Ｓ３０１）。ξの更新回数が予め設定された回数に達した場合、または、ヨーレートセンサから取得したヨー角の変化Δα_ｙаｗ（ｔ、ｔ−１）とＳ３０１で算出されたヨー角の変化であるΔθ_ｙаｗ（ξ、ξ^(t-1)）の差が、予め設定された閾値（Ｔ_ｙаｗ）未満である場合（Ｓ３０２、Ｙｅｓ）、パラメータ更新処理を終了し、その時点でのξを推定結果（ハット付きξ）とする（Ｓ１０６）。

一方、ξの更新回数が予め設定された回数に達していない場合、かつ、Δα_ｙаｗ（ｔ、ｔ−１）とΔθ_ｙаｗ（ξ、ξ^(t-1)）の差が、予め設定された閾値（Ｔ_ｙаｗ）以上である場合（Ｓ３０２、Ｎｏ）、Ｓ１０２に戻り、パラメータ更新処理を継続する。

最後に、Ｓ１０６において決定された、カメラ姿勢の推定結果（ハット付きξ）の信頼性を評価する（Ｓ１０７〜Ｓ１０９）。Ｓ１０７〜Ｓ１０９の手順は、図７を用いて説明した第１の実施形態における手順と全く同様であるので、説明を省略する。

このように、本実施形態によれば、カメラ姿勢の推定を行うための繰り返し計算における収束判定を、車両センサから取得した車両情報、例えば、ヨーレートセンサから取得したヨー角の変化を用いて行っている。異なるセンサの情報に基づきカメラ姿勢の推定をチェックすることで、信頼度の高い推定結果を得ることができる。また、第１の実施形態と同様に、推定結果の信頼度特徴量を求める際に、テイラー展開を用いてハット付きξ近傍のＥ（ξ）を算出しているので、信頼性の高い推定結果を高速に算出することができる。更に、信頼度が高いと判定された場合のみ移動体の自動制御を継続し、信頼度が低いと判定された場合には移動体の自動制御を中止し、かつ、運転者に自動制御の中止を通知する。従って、運転者が自身で移動体を運転しなければならないことを容易に認識できるので、より安全に移動体の制御を行なうことができる。

以下、レーベンバーグ・マーカート法を用いた場合の、第２の実施形態における運動推定２の一連の手順について、図１７を用いて説明する。図１７は、第３の実施形態に係わる運動推定２の別の手順を説明するフローチャートである。

Ｓ１１８、またはＳ１１７でλを更新後、フレームｔのカメラ姿勢の推定値ξと、フレームｔ−１のカメラ姿勢の推定結果ハット付きξ^(t-1)の間でのヨー角の変化であるΔθ_ｙаｗ（ξ、ξ^(t-1)）を算出する（Ｓ３１１）。ξの更新回数が予め設定された回数に達した場合、または、ヨーレートセンサから取得したヨー角の変化Δα_ｙаｗ（ｔ、ｔ−１）とＳ３１１で算出されたヨー角の変化であるΔθ_ｙаｗ（ξ、ξ^(t-1)）の差が、予め設定された閾値（Ｔ_ｙаｗ）未満である場合（Ｓ３１２、Ｙｅｓ）、パラメータ更新処理を終了し、その時点でのξを推定結果（ハット付きξ）とする（Ｓ１２０）。

一方、ξの更新回数が予め設定された回数に達していない場合、かつ、Δα_ｙаｗ（ｔ、ｔ−１）とΔθ_ｙаｗ（ξ、ξ^(t-1)）の差が、予め設定された閾値（Ｔ_ｙаｗ）以上である場合（Ｓ３１２、Ｎｏ）、Ｓ１１３に戻り、パラメータ更新処理を継続する。

最後に、Ｓ１２０において決定された、カメラ姿勢の推定結果（ハット付きξ）の信頼性を評価する（Ｓ１２１〜Ｓ１２３）。Ｓ１２１〜Ｓ１２３の手順は、図１１を用いて説明した第１の実施形態における手順と全く同様であるので、説明を省略する。

このように、レーベンバーグ・マーカート法を用いる場合においても、カメラ姿勢の推定を行うための繰り返し計算における収束判定を、車両センサから取得した車両情報、例えば、ヨーレートセンサから取得したヨー角の変化を用いて行っている。異なるセンサの情報に基づきカメラ姿勢の推定をチェックすることで、信頼度の高い推定結果を得ることができる。従って、移動体自動制御の中止頻度を低減することができ、運転者の利便性を向上させることができる。

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

１…移動体制御装置
１０…画像処理装置
１０ａ…プロセッサ
１０ｂ…メモリ
１０ｄ…画像処理アクセラレータ
１０ｅ…処理回路
１１…画像処理部
１２…障害物検出部
１００…移動体
１００Ａ…出力部
１００Ｂ…センサ
１００Ｃ…入力装置
１００Ｄ…通信部
１００Ｅ…ディスプレイ
１００Ｆ…スピーカ
１００Ｇ…動力制御部
１００Ｈ…動力部
１００Ｉ…バス
１１１…画像処理制御部
１１２…画像入力部
１１３…特徴点抽出部
１１４…特徴点対応付け部
１１５…第１運動推定部
１１６…運動予測部
１１７…第２運動推定部
１１８…点３次元座標推定部
１１９…信頼度特徴量計算部
１２０…信頼度判定部

Claims

移動体に設けられた撮像装置により得られる撮影画像に基づき、前記撮像装置の姿勢を推定し、推定結果として出力する画像処理装置と、
前記推定結果に基づいて前記移動体に設けられた動力部の制御を行う動力制御部と、
を備えた移動体制御装置であって、
前記画像処理装置は、前記推定結果の信頼度特徴量を算出する信頼度特徴量計算部と、前記信頼度特徴量に基づき前記推定結果の信頼度を判定する信頼度判定部を備えており、
前記動力制御部は、前記信頼度の判定結果に基づき、前記動力部の自動制御を行うか否かを決定することを特徴とする、移動体制御装置。
前記信頼度の判定結果が低い場合、前記動力制御部は、前記動力部の自動制御を中止し、また、前記画像処理装置は、前記動力制御部が前記動力部の自動制御を行わない旨を、前記移動体に設けられたディスプレイに表示させることを特徴とする、請求項１に記載の移動体制御装置。
前記画像処理装置は、投影誤差を前記撮像装置の姿勢を推定するための目的関数とし、前記推定結果近傍の前記目的関数の値を、テイラー展開を用いて算出することを特徴とする、請求項１又は請求項２に記載の移動体制御装置。
前記画像処理装置は、前記目的関数の値の算出を、ガウスニュートン法を用いて行うことを特徴とする、請求項３に記載の移動体制御装置。
前記画像処理装置は、前記目的関数の値の算出を、レーベンバーグ・マーカート法を用いて行うことを特徴とする、請求項３に記載の移動体制御装置。
前記画像処理装置は、前記目的関数の複数の変数のうち一部の前記変数を固定値とみなして、前記目的関数の値を算出することを特徴とする、請求項３乃至請求項５のいずれか一項に記載の移動体制御装置。
前記画像処理装置は、前記撮像装置の姿勢の推定において最後に算出した、前記目的関数の値と、前記目的関数の勾配ベクトルと、前記目的関数のヘッセ行列とを用いて、前記信頼度特徴量を算出することを特徴とする、請求項３乃至請求項６のいずれか一項に記載の移動体制御装置。
前記画像処理装置は、前記推定結果の信頼度特徴量が設定された値より高くなるまで、前記撮像装置の姿勢を繰り返し推定することを特徴とする、請求項１乃至請求項７のいずれか一項に記載の移動体制御装置。
前記画像処理装置は、前記移動体に設けられた車両センサから取得する車両情報と、前記撮像装置の姿勢の推定値との差が、設定された値より小さくなるまで、前記撮像装置の姿勢を繰り返し推定することを特徴とする、請求項１乃至請求項７のいずれか一項に記載の移動体制御装置。
前記車両センサはヨーレートセンサであり、前記車両情報はヨー角であることを特徴とする、請求項９に記載の移動体制御装置。