特許 7099832 測距装置および較正パラメータの推定方法

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

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

(11)【特許番号】

(24)【登録日】2022-07-04

(45)【発行日】2022-07-12

(54)【発明の名称】測距装置および較正パラメータの推定方法

(51)【国際特許分類】

   G01C 3/00 20060101AFI20220705BHJP

   G01C 3/06 20060101ALI20220705BHJP

【ＦＩ】

G01C3/00 120

G01C3/06 110V

G01C3/06 140

【請求項の数】 6

(21)【出願番号】P 2018034510

(22)【出願日】2018-02-28

(65)【公開番号】P2019148548

(43)【公開日】2019-09-05

【審査請求日】2020-12-14

(73)【特許権者】

【識別番号】000004695

【氏名又は名称】株式会社ＳＯＫＥＮ

(73)【特許権者】

【識別番号】000004260

【氏名又は名称】株式会社デンソー

(73)【特許権者】

【識別番号】592032636

【氏名又は名称】学校法人トヨタ学園

(74)【代理人】

【識別番号】110000028

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

(72)【発明者】

【氏名】石丸 和寿

(72)【発明者】

【氏名】大石 智之

(72)【発明者】

【氏名】テヘラニニキネジャド ホセイン

(72)【発明者】

【氏名】西 隆史

(72)【発明者】

【氏名】シュー ユーチュエン

(72)【発明者】

【氏名】三田 誠一

【審査官】眞岩 久恵

(56)【参考文献】

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

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

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

Ｇ０１Ｃ ３／００－３／３２

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

【請求項1】

測距装置（１００）であって、
第１撮像装置（１０）によって対象物が撮像された第１画像と、第２撮像装置（２０）によって前記対象物が撮像された第２画像とを取得する画像取得部（３０）と、
前記第１画像および前記第２画像を較正するための較正パラメータを、前記第１画像および前記第２画像に共通する特徴の対応点の位置に基づき推定する較正パラメータ推定部（４０）と、
推定された前記較正パラメータに基づき前記第１画像および前記第２画像を較正する画像較正部（６０）と、
較正された前記第１画像および前記第２画像を用いて前記対象物までの距離を算出する距離算出部（７０）と、
を備え、
前記較正パラメータ推定部（４０）は、
前記較正パラメータおよび前記対応点の位置を更新しながら、前記較正パラメータの更新前後の変化量を表す項と、前記対応点の位置の更新前後の変化量を表す項と、を含む評価関数から算出される評価値を、繰り返し算出して最適化することにより、前記較正パラメータの推定を行い、
前記較正パラメータ推定部は、前記較正パラメータとして、少なくとも回転パラメータと並進パラメータとを推定する、
測距装置（１００）。

【請求項2】

請求項１に記載の測距装置であって、
前記評価関数は、前記第１撮像装置および前記第２撮像装置の位置の一致度を表す項を含む、測距装置。

【請求項3】

請求項１または請求項２に記載の測距装置であって、
前記較正パラメータ推定部は、前記較正パラメータの初期値として、前記測距装置の工場出荷時の値、前記較正パラメータの設計値、または、前記測距装置が搭載された車両の走行中に推定された較正パラメータの値、を用いる、測距装置。

【請求項4】

請求項１から請求項３までのいずれか一項に記載の測距装置であって、
前記較正パラメータ推定部は、前記測距装置が搭載された車両が静止しているとき、または、前記測距装置が搭載された車両において前記距離算出部によって算出された前記距離が用いられる運転制御が実行されていない走行中に、前記較正パラメータの推定を実行する、測距装置。

【請求項5】

請求項１から請求項４までのいずれか一項に記載の測距装置であって、
前記較正パラメータ推定部は、今回推定された較正パラメータと、前回推定された較正パラメータとの差が、予め定めた許容範囲外の場合に、今回推定された較正パラメータを破棄する、測距装置。

【請求項6】

測距装置における較正パラメータの推定方法であって、
（ａ）第１撮像装置によって対象物が撮像された第１画像と、第２撮像装置によって前記対象物が撮像された第２画像とを取得する工程と、
（ｂ）前記第１画像および前記第２画像を較正するための較正パラメータを、前記第１画像および前記第２画像に共通する特徴の対応点の位置に基づき推定する工程と、を備え、
前記工程（ｂ）では、
前記較正パラメータおよび前記対応点の位置を更新しながら、前記較正パラメータの更新前後の変化量を表す項と、前記対応点の位置の更新前後の変化量を表す項と、を含む評価関数から算出される評価値を、繰り返し算出して最適化することにより、前記較正パラメータの推定を行い、
前記較正パラメータとして、少なくとも回転パラメータと並進パラメータとを推定する、
推定方法。

【発明の詳細な説明】

【技術分野】

【0001】

本開示は、測距装置および較正パラメータの推定方法に関する。

【背景技術】

【0002】

近年、車両の自動運転制御において、複数の撮像装置を用いて対象物までの距離を測定する測距装置が用いられている。例えば、特許文献１には、２つの撮像装置によって撮像された２つの画像に基づき、カメラパラメータ（内部パラメータ、外部パラメータ、歪み係数）を較正（キャリブレーション）する技術が開示されている。

【先行技術文献】

【特許文献】

【0003】

【文献】特開２０１２－７５０６０号公報

【発明の概要】

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

【0004】

車両の自動運転制御では、測距された距離は、車両の緊急ブレーキや車間距離の調整等に利用されるため、測定誤差は小さい方が好ましい。そのため、測距装置に備えられた撮像装置を精度良く較正することのできる技術が求められている。このような課題は、自動運転制御用の測距装置に限らず、他の用途に用いられる測距装置にも共通した課題である。

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

【0005】

本開示は、上述の課題の少なくとも一部を解決するためになされたものであり、以下の形態として実現することが可能である。

【0006】

本開示の一形態によれば、測距装置（１００）が提供される。この測距装置（１００）は、第１撮像装置（１０）によって対象物が撮像された第１画像と、第２撮像装置（２０）によって前記対象物が撮像された第２画像とを取得する画像取得部（３０）と；前記第１画像および第２画像を較正するための較正パラメータを、前記第１画像および前記第２画像に共通する特徴の対応点の位置に基づき推定する較正パラメータ推定部（４０）と；推定された前記較正パラメータに基づき前記第１画像および前記第２画像を較正する画像較正部（６０）と；較正された前記第１画像および前記第２画像を用いて前記対象物までの距離を算出する距離算出部（７０）と；を備える。そして、前記較正パラメータ推定部（４０）は、前記較正パラメータおよび前記対応点の位置を更新しながら、前記較正パラメータの更新前後の変化量を表す項と、前記対応点の位置の更新前後の変化量を表す項と、を含む評価関数から算出される評価値を、繰り返し算出して最適化することにより、前記較正パラメータの推定を行い、前記較正パラメータ推定部は、前記較正パラメータとして、少なくとも回転パラメータと並進パラメータとを推定することを特徴とする。

【0007】

この形態の制御装置によれば、較正パラメータだけではなく、第１画像および第２画像に共通する対応点の位置を更新しながら評価値を最適化して較正パラメータを推定するので、第１撮像装置および第２撮像装置によって撮像された画像を精度よく較正することができる。そのため、対象物までの距離を正確に測定することができる。

【0008】

本開示は、測距装置以外の種々の形態で実現することも可能である。例えば、較正パラメータの推定方法や測距方法、それらの方法を実現するためのコンピュータプログラム、コンピュータプログラムを記録した一時的でない有形の記録媒体等の形態で実現できる。

【図面の簡単な説明】

【0009】

【図1】測距装置のブロック図。

【図2】較正パラメータ推定処理のフローチャート。

【図3】比較例における回転パラメータ（ピッチ）の推定結果を示すグラフ。

【図4】比較例における回転パラメータ（ロール）の推定結果を示すグラフ。

【図5】比較例における回転パラメータ（ヨー）の推定結果を示すグラフ。

【図6】実施形態における回転パラメータ（ピッチ）の推定結果を示すグラフ。

【図7】実施形態における回転パラメータ（ロール）の推定結果を示すグラフ。

【図8】実施形態における回転パラメータ（ヨー）の推定結果を示すグラフ。

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

【0010】

Ａ．第１実施形態：
Ａ１．装置構成：
図１に示すように、本開示の第１実施形態における測距装置１００は、画像取得部３０と、較正パラメータ推定部４０と、較正パラメータ記憶部５０と、画像較正部６０と、距離算出部７０とを備える。測距装置１００には、第１撮像装置１０および第２撮像装置２０が接続されている。測距装置１００は、車両に搭載され、緊急ブレーキや車間距離の調整といった自動運転制御に用いられる。

【0011】

測距装置１００は、ＣＰＵやＲＡＭ、不揮発性メモリ等を備えるコンピュータによって構成されている。ＣＰＵは、不揮発性メモリに記憶された制御プログラムをＲＡＭにロードして実行することによって、画像取得部３０、較正パラメータ推定部４０、画像較正部６０、距離算出部７０、として機能する。これらの機能部は、各種の回路によってハードウェア的に実現されてもよい。較正パラメータ記憶部５０は、不揮発性メモリ内に確保された記憶領域である。

【0012】

画像取得部３０は、対象物２００を異なる位置から撮像して得られた第１画像および第２画像を取得する。本実施形態では、画像取得部３０は、第１撮像装置１０から第１画像を取得し、第２撮像装置２０から第２画像を取得する。第１撮像装置１０は、第２撮像装置２０の左側に配置されており、左側の画像（第１画像）を撮像する。第２撮像装置は、第１撮像装置１０の右側に配置されており、右側の画像（第２画像）を撮像する。第１撮像装置１０のことを左カメラともいい、第２撮像装置２０のことを右カメラともいう。第１撮像装置１０と第２撮像装置２０とをあわせてステレオカメラともいう。

【0013】

較正パラメータ推定部４０は、第１画像および第２画像を較正するための較正パラメータを、第１画像および第２画像に共通する特徴の対応点の位置に基づき推定する。較正パラメータとは、第１画像および第２画像の視差に基づき対象物２００までの距離を測定可能とするために、これらの画像を補正（較正）するためのパラメータである。本実施形態では、第１撮像装置１０および第２撮像装置２０の内部パラメータ（焦点距離、画像中心、アスペクト比、スキュー）および歪み係数は既知であるものとし、較正パラメータとして、外部パラメータを推定する。外部パラメータとは、撮像装置の回転パラメータおよび並進パラメータのことをいう。外部パラメータは、回転方向に３自由度、並進方向に３自由度の計６自由度を有するパラメータである。

【0014】

本実施形態では、較正パラメータ推定部４０は、較正パラメータおよび対応点の位置を更新しながら、較正パラメータの更新前後の変化量を表す項と、対応点の位置の更新前後の変化量を表す項と、を含む評価関数から算出される評価値を、繰り返し算出して最適化することにより、較正パラメータを推定する。評価関数や最適化の詳細については後で詳しく説明する。推定された較正パラメータは、較正パラメータ記憶部５０に記憶される。

【0015】

画像較正部６０は、較正パラメータ記憶部５０に記憶された較正パラメータに基づき第１画像および第２画像を較正する。本実施形態において、「第１画像および第２画像を較正する」とは、後述する距離算出部７０が、画像中に撮像された対象物２００までの距離を算出可能なように、較正パラメータを用いて第１画像および第２画像の少なくとも一方を補正することをいい、第１画像のみを補正することと、第２画像のみを補正することと、第１画像および第２画像の両方を補正することと、のいずれかを含む。

【0016】

距離算出部７０は、画像較正部６０によって較正された第１画像および第２画像を用いて、これらの画像に共通して撮像された対象物２００までの距離を算出する。距離の算出は、三角測量法によって行われる。三角測量法は周知の技術であるため、説明は省略する。距離算出部７０によって算出された距離は、車両のＥＣＵ（Electronic Control Unit）に出力され、緊急ブレーキや車間距離の調整といった自動運転制御に用いられる。

【0017】

Ａ２．処理内容：
図２は、測距装置１００によって実行される較正パラメータ推定処理のフローチャートである。この処理は、測距装置１００の動作中に所定の周期で繰り返し実行される処理である。

【0018】

較正パラメータ推定処理が開始されると、まず、測距装置１００は、現在が、較正パラメータの更新タイミングであるか否かを判断する（ステップＳ１０）。本実施形態において、測距装置１００は、１０分経過するごとに更新タイミングであると判断する。その他、例えば、測距装置１００は、測距装置１００が搭載された車両が静止したタイミングや、測距装置１００が搭載された車両において距離が用いられる運転制御が実行されていない走行中のタイミングに、較正パラメータの更新タイミングであると判断してもよい。

【0019】

測距装置１００は、現在が、較正パラメータの更新タイミングではないと判断した場合には（ステップＳ１０：Ｎｏ）、当該較正パラメータ推定処理を終了する。一方、現在が、較正パラメータの更新タイミングであると判断した場合には（ステップＳ１０：Ｙｅｓ）、測距装置１００は、較正パラメータの初期値を較正パラメータ記憶部５０から読み込む（ステップＳ２０）。較正パラメータの初期値とは、当該較正パラメータ推定処理が過去に実行済みであれば、前回の較正パラメータ推定処理において推定された値である。これに対して、当該較正パラメータ推定処理が過去に一度も実行されたことがなければ、当該測距装置１００の工場出荷時に較正パラメータ記憶部５０に記憶された値である。なお、当該較正パラメータ推定処理が過去に一度も実行されたことがない場合、工場出荷時に記憶された値ではなく、較正パラメータの設計値や、測距装置１００が搭載された車両の走行中に当該較正パラメータ推定処理と同様の処理を実行して推定した仮の較正パラメータの値を初期値として用いてもよい。

【0020】

較正パラメータの初期値を読み込むと、測距装置１００の画像取得部３０が、第１撮像装置１０から第１画像を取得するとともに、第２撮像装置２０から第２画像を取得する（ステップＳ３０）。第１画像と第２画像とは、同一のタイミングで第１撮像装置１０および第２撮像装置２０により撮像された画像である。

【0021】

これら２つの画像を取得すると、測距装置１００の較正パラメータ推定部４０が、２つの画像において同じ特徴を有する対応点を特定する（ステップＳ４０）。本実施形態では、較正パラメータとして、６つの自由度を有する外部パラメータの推定を行うため、少なくとも５つの対応点を特定する。較正パラメータ推定部４０は、対応点の特定の際に、ＲＡＮＳＡＣ(Random Sampling Consensus)等のロバスト推定によって外れ値を除去してもよい。特徴点の抽出および対応点の特定は、周知の種々のアルゴリズムを用いて行うことが可能であり、手法は特に限定されない。

【0022】

較正パラメータ推定部４０は、２つの画像から対応点を特定すると、較正パラメータおよび対応点の位置を更新しながら以下の式（１）によって表される評価関数（コスト関数ともいう）を用いて評価値ＥＶを算出する（ステップＳ５０）。較正パラメータ推定部４０は、本実施形態では勾配法を用いて、この評価値ＥＶが小さくなるように、較正パラメータおよび対応点を次々に更新することによって評価値を最適化して較正パラメータを求める。つまり、本実施形態では、較正パラメータの推定にあたり、較正パラメータだけではなく、対応点の位置も更新しながら、較正パラメータの推定を行う。なお、他の実施形態では、勾配法ではなく、レーベンバーグ・マーカート法等の最適化手法を用いてもよい。

【0023】

ＥＶ＝｜（更新後の対応点の位置）－（特定された対応点の位置）｜^２
＋｜（更新後の較正パラメータ）－（較正パラメータの初期値）｜^２
＋｜（第１撮像装置および第２撮像装置の位置の一致度）｜^２ ・・・（１）

【0024】

上記式（１）において、右辺の第１項は、対応点の位置の更新前後の変化量を表し、第２項は、較正パラメータの更新前後の変化量を表す。右辺の第３項は、左右カメラの幾何拘束ともいう。式（１）の詳細、および、これを用いた最適化の手法については後で詳しく説明する。

【0025】

較正パラメータ推定部４０は、評価値ＥＶを算出すると、最適化が完了したか否かを判断する（ステップＳ６０）。較正パラメータ推定部４０は、評価値ＥＶの最小値が探索された場合、評価値ＥＶが所定の閾値よりも小さくなった場合、または、上記ステップＳ５０およびステップＳ６０を繰り返し実行している時間が予め定められた時間を経過した場合に、最適化が完了したと判断する。較正パラメータ推定部４０は、最適化が完了していないと判断した場合には（ステップＳ６０：Ｎｏ）、処理をステップＳ５０に戻して、評価値ＥＶが小さくなるように、較正パラメータおよび対応点を更新する。

【0026】

較正パラメータ推定部４０は、最適化が完了したと判断した場合には（ステップＳ６０：Ｙｅｓ）、最適化が完了した時点での較正パラメータ、つまり、今回推定された較正パラメータと、ステップＳ２０で読み込んだ初期値（前回推定された較正パラメータ）とを比較して、その差が予め定めた許容範囲内であるか否かを判断する（ステップＳ７０）。

【0027】

較正パラメータ推定部４０は、今回と前回の較正パラメータの差が許容範囲外であると判断した場合には（ステップＳ７０：Ｎｏ）、今回推定された較正パラメータを破棄する（ステップＳ８０）。従って、この場合、較正パラメータ記憶部５０には、過去に推定された較正パラメータがそのまま保存された状態になる。これに対して、較正パラメータ推定部４０は、較正パラメータの変化が許容範囲内であると判断すれば（ステップＳ７０：Ｙｅｓ）、較正パラメータ記憶部５０に記憶されている過去の較正パラメータを消去して、最適化が完了した時点における較正パラメータを較正パラメータ記憶部５０に保存する（ステップＳ９０）。以上で説明した一連の処理によって較正パラメータ推定処理は終了する。

【0028】

Ａ３．最適化の詳細：
以下では、上述した評価関数を用いた最適化について説明する。第１撮像装置１０（左カメラ）のカメラ座標系の位置（ｕ_ｌ，ｖ_ｌ）と、実空間上の位置（Ｘ，Ｙ，Ｚ）との対応関係は、次の式（２）によって表される。Ｋは、第１撮像装置１０の内部パラメータであり、本実施形態では定数である。また、Ｉは単位行列である。

【0029】

【数1】

【0030】

第２撮像装置２０（右カメラ）のカメラ座標系の位置（ｕ_ｒ，ｕ_ｒ）と、実空間上の位置（Ｘ，Ｙ，Ｚ）との対応関係は、次の式（３）によって表される。Ｋは、第２撮像装置２０の内部パラメータであり、本実施形態では定数である。このＫは、第１撮像装置１０のＫと同じ値でもよいし、異なる値でもよい。［Ｒ，Ｔ］は外部パラメータであり、Ｒは、回転パラメータ（ｒｘ，ｒｙ，ｒｚ）を表し、Ｔは並進パラメータ（ｔｘ，ｔｙ，ｔｚ）を表す。なお、ｒｘをピッチ、ｒｙをヨー、ｒｚをロールともいう。

【0031】

【数2】

【0032】

上記式（２）および上記（３）より、次の式（４）が求められ、式（４）は、式（５）のように表すことができる。

【0033】

【数3】

【0034】

【数4】

【0035】

そして、式（５）は、次の式（６）（７）として表される。

【0036】

【数5】

【0037】

ここで、左画像（第１画像）の座標と、右画像（第２画像）の座標との対応関係は、次の式（８）によって表される。本実施形態ではＫは既知であるため、外部パラメータ［Ｒ，ｔ］が決定されれば、距離算出部７０は、対象物２００の右画像における座標と左画像における座標から、その奥行き方向の位置Ｚを算出できる。

【0038】

（右画像の座標）＝Ｚ＊Ｋ＊［Ｒ，ｔ］＊（左画像の座標） ・・・（８）

【0039】

上記式（１）によって表される評価関数の右辺は、上記式（６）および式（７）により、以下の式（９）のように表すことができる。

【0040】

【数6】

【0041】

式（９）の第１項、第２項、第３項は、式（１）の右辺の第１項、第２項、第３項に対応する。つまり、第１項は、対応点の位置の更新前後の変化量を表し、第２項は、較正パラメータの更新前後の変化量を表し、第３項は、第１撮像装置および第２撮像装置の位置の一致度を表す。ただし、式（９）では、式（１）の評価関数における第３項の影響度を任意に設定可能とするため、任意の定数λを導入している。

【0042】

上記式（９）の第３項の括弧内をｘ座標とｙ座標とに分離して考える。ｘ座標については、以下の式（１０）によって、コスト関数cost1として表され、ｙ座標については、以下の式（１１）によって、コスト関数cost2として表される。

【0043】

cost1 = ty - v2*(tz + Z*v1*sin(rx) + Z*cos(rx)*cos(ry) - Z*u1*cos(rx)*sin(ry)) + Z*(sin(ry)*sin(rz) - cos(ry)*cos(rz)*sin(rx)) + Z*u1*(cos(ry)*sin(rz) + cos(rz)*sin(rx)*sin(ry)) + Z*v1*cos(rx)*cos(rz) ・・・（１０）

【0044】

cost2 = Z*(cos(rz)*sin(ry) + cos(ry)*sin(rx)*sin(rz)) - u2*(tz + Z*v1*sin(rx) + Z*cos(rx)*cos(ry) - Z*u1*cos(rx)*sin(ry)) - tx + Z*u1*(cos(ry)*cos(rz) - sin(rx)*sin(ry)*sin(rz)) - Z*v1*cos(rx)*sin(rz) ・・・（１１）

【0045】

ｘ座標についての回転パラメータの傾き（grx1,gry1,grz1）および並進パラメータの傾き（gtx1,gty1,gtz1）は、以下のように表される。

【0046】

grx1 = Z*u1*cos(rx)*cos(rz)*sin(ry) - Z*cos(rx)*cos(ry)*cos(rz) - Z*v1*cos(rz)*sin(rx) - v2*(Z*v1*cos(rx) - Z*cos(ry)*sin(rx) + Z*u1*sin(rx)*sin(ry));
gry1 = Z*(cos(ry)*sin(rz) + cos(rz)*sin(rx)*sin(ry)) + v2*(Z*cos(rx)*sin(ry) + Z*u1*cos(rx)*cos(ry)) - Z*u1*(sin(ry)*sin(rz) - cos(ry)*cos(rz)*sin(rx));
grz1 = Z*(cos(rz)*sin(ry) + cos(ry)*sin(rx)*sin(rz)) + Z*u1*(cos(ry)*cos(rz) - sin(rx)*sin(ry)*sin(rz)) - Z*v1*cos(rx)*sin(rz);
gtx1 = 0;
gty1 =1;
gtz1 =-v2;

【0047】

また、ｙ座標についての回転パラメータの傾き（grx2,gry2,grz2）および並進パラメータの傾き（gtx2,gty2,gtz2）は、以下のように表される。

【0048】

grx2 = Z*cos(rx)*cos(ry)*sin(rz) - u2*(Z*v1*cos(rx) - Z*cos(ry)*sin(rx) + Z*u1*sin(rx)*sin(ry)) + Z*v1*sin(rx)*sin(rz) - Z*u1*cos(rx)*sin(ry)*sin(rz);
gry2 = Z*(cos(ry)*cos(rz) - sin(rx)*sin(ry)*sin(rz)) + u2*(Z*cos(rx)*sin(ry) + Z*u1*cos(rx)*cos(ry)) - Z*u1*(cos(rz)*sin(ry) + cos(ry)*sin(rx)*sin(rz));
grz2 = - Z*(sin(ry)*sin(rz) - cos(ry)*cos(rz)*sin(rx)) - Z*u1*(cos(ry)*sin(rz) + cos(rz)*sin(rx)*sin(ry)) - Z*v1*cos(rx)*cos(rz);
gtx2 = -1;
gty2 = 0;
gtz2 = -u2;

【0049】

ｘ座標についての対応点の傾きは、以下のように表される。

【0050】

g1u1 = Z*(cos(ry)*sin(rz) + cos(rz)*sin(rx)*sin(ry)) + Z*v2*cos(rx)*sin(ry);
g1v1 = Z*cos(rx)*cos(rz) - Z*v2*sin(rx);
g1u2 = 0;
g1v2 = Z*u1*cos(rx)*sin(ry) - Z*v1*sin(rx) - Z*cos(rx)*cos(ry) - tz;

【0051】

また、ｙ座標についての対応点の傾きは、以下のように表される。

【0052】

g2u1 = Z*(cos(ry)*cos(rz) - sin(rx)*sin(ry)*sin(rz)) + Z*u2*cos(rx)*sin(ry);
g2v1 = - Z*u2*sin(rx) - Z*cos(rx)*sin(rz);
g2u2 = Z*u1*cos(rx)*sin(ry) - Z*v1*sin(rx) - Z*cos(rx)*cos(ry) - tz;
g2v2 = 0;

【0053】

対応点ｉ毎の回転・並進パラメータの傾きは以下のように表される。ただし、lambda1は定数であり、rx0,ry0,rz0,tx0,ty0,tz0は、回転・並進パラメータの初期値（前回値）である。

【0054】

grx(i) = (grx1*cost1+grx2*cost2)/Z/Z+lambda1*(rx-rx0);
gry(i) = (gry1*cost1+gry2*cost2)/Z/Z+lambda1*(ry-ry0);
grz(i) = (grz1*cost1+grz2*cost2)/Z/Z+lambda1*(rz-rz0);
gtx(i) = (gtx1*cost1+gtx2*cost2)/abs(Z)+lambda1*(tx-tx0);
gty(i) = (gty1*cost1+gty2*cost2)/abs(Z)+lambda1*(ty-ty0);
gtz(i) = (gtz1*cost1+gtz2*cost2)/abs(Z)+lambda1*(tz-tz0);

【0055】

また、対応点ｉ毎の位置の傾きは以下のように算出される。ただし、lambda2は定数であり、u10(i),v10(i),u20(i),v20(i)は、対応点ｉごとの位置の初期値（前回値）である。

【0056】

gu1(i) = (g1u1*cost1+g2u1*cost2)/abs(Z)+lambda2*(u1-u10(i));
gv1(i) = (g1v1*cost1+g2v1*cost2)/abs(Z)+lambda2*(v1-v10(i));
gu2(i) = (g1u2*cost1+g2u2*cost2)/abs(Z)+lambda2*(u2-u20(i));
gv2(i) = (g1v2*cost1+g2v2*cost2)/abs(Z)+lambda2*(v2-v20(i));

【0057】

こうして対応点毎に求めた傾きの平均値（mean()）に定数lr1,lr2を乗じて、回転・並進パラメータと、対応点の位置とを更新する。ただし、ｆは焦点距離である。

【0058】

rx = rx-lr1*mean(grx);
ry = ry-lr1*mean(gry);
rz = rz-lr1*mean(grz);
tx = tx-lr1*mean(gtx)/10;
ty = ty-lr1*mean(gty)/10;
tz = tz-lr1*mean(gtz)/1000;
pt1(i,1) = pt1(i,1)-lr2./300*f.*gu1(i);
pt1(i,2) = pt1(i,2)-lr2./300*f.*gv1(i);
pt2(i,1) = pt2(i,1)-lr2./300*f.*gu2(i);
pt2(i,2) = pt2(i,2)-lr2./300*f.*gv2(i);

【0059】

なお、画像の左上が原点の座標であるpt1,pt2と、カメラの焦点中心が原点の座標であるu1,v1,u2,v2の関係は以下のとおりである。ただし、cx,cyは焦点中心の座標，fは焦点距離である。

【0060】

u1(i) = (pt1(i,1)-cx)/f;
v1(i) = (pt1(i,2)-cy)/f;
u2(i) = (pt2(i,1)-cx)/f;
v2(i) = (pt2(i,2)-cy)/f;

【0061】

本実施形態では、上記各計算式を用いて、回転・並進パラメータおよび対応点の位置を更新しつつ、上記式（９）を用いて評価値ＥＶを最小化するように最適化していくことで、回転・並進パラメータおよび対応点の位置を真値に近づけるよう推定する。なお、上述した各式における定数（lambda1,lambda2,lr1,lr2）は、各パラメータの更新前後において値を大きく変動させないための定数であり、値は、実験に基づき任意に設定可能である。

【0062】

Ａ４．実施形態の効果：
図３～図５には、上記式（１）の評価関数から第１項を省略して回転パラメータをフレーム毎に推定した結果を比較例として示し、図６～図８には、上記式（１）の評価関数を用いて回転パラメータをフレーム毎に推定した結果を示している。図３，図６には、第１撮像装置１０と第２撮像装置２０とでピッチが一定値ずれている場合の推定結果を示し、図４，７には、第１撮像装置１０と第２撮像装置２０とでロールが一定値ずれている場合の推定結果を示している。また、図５，８には、第１撮像装置１０と第２撮像装置２０とでヨーが一定値ずれている場合の推定結果を示している。

【0063】

図３～図５と図６～図８とを対比して明らかなように、式（１）の第１項、すなわち、対応点の位置の更新前後の変化量を評価関数に含めて回転パラメータを推定したところ、対応点の位置の更新前後の変化量を含めずに回転パラメータを推定した比較例よりも、推定値の変動が格段に小さくなった。これは、対応点の位置には特徴点の抽出過程や座標上の点とするための端数処理において誤差あるいはノイズが含まれることがあるが、本実施形態では、このような誤差やノイズを含む可能性のある対応点の位置についても真値になるように評価値が最適化されるため、較正パラメータを精度よく推定できるからである。

【0064】

以上で説明したように、本実施形態の測距装置１００によれば、較正パラメータだけではなく、対応点の位置を更新しながら評価値を最適化して較正パラメータを推定するので、較正パラメータを精度よく求めることができる。そのため、第１撮像装置１０および第２撮像装置２０によって撮像された画像を精度よく較正することができ、対象物までの距離を正確に測定することができる。また、本実施形態では、第１撮像装置１０および第２撮像装置２０の位置の一致度をも用いて最適化を行うため、より精度よく較正パラメータを推定することができる。さらに、本実施形態では、推定された較正パラメータが許容範囲外である場合に、その値を破棄するため、より精度よく距離を測定することができる。

【0065】

Ｂ．他の実施形態：
本開示は、上述の実施形態に限られるものではなく、その趣旨を逸脱しない範囲において種々の形態で実現することができる。例えば、以下のような形態が可能である。

【0066】

（Ｂ－１）上記実施形態では、測距装置１００は車両に搭載されるものとして説明したが、測距装置１００は、車両に限らず、他の移動体や他の測定装置に搭載されてもよい。

【0067】

（Ｂ－２）上記実施形態では、較正パラメータとして外部パラメータの推定を行った。これに対して、較正パラメータ推定部４０は、外部パラメータにとともに、内部パラメータや歪み係数を推定してもよく、画像較正部６０はそれらのパラメータを用いて画像を較正してもよい。

【0068】

（Ｂ－３）上記実施形態において式（１）によって表される評価関数は、第１項～第３項以外にも他の項を含んでもよい。また、第１項～第３項のうち、例えば、第３項を省略してもよい。

【0069】

（Ｂ－４）上記実施形態では、較正パラメータ推定部４０は、図２に示したステップＳ７０～Ｓ８０において、今回と前回の較正パラメータの差が許容範囲内でなければ、較正パラメータを破棄するものとした。しかし、この処理は省略可能である。また、例えば、較正パラメータ推定部４０は、ステップＳ７０～Ｓ８０の処理を、初回の較正パラメータ推定処理のみスキップしてもよい。こうすることにより、初回の較正パラメータ推定処理では、工場出荷時の初期値から離れた値が推定された場合でも、その値により初期値を強制的に更新することができる。

【符号の説明】

【0070】

１０ 第１撮像装置、２０ 第２撮像装置、３０ 画像取得部、４０ 較正パラメータ推定部、５０ 較正パラメータ記憶部、６０ 画像較正部、７０ 距離算出部、１００ 測距装置、２００ 対象物

【図1】

【図2】

【図3】

【図4】

【図5】

【図6】

【図7】

【図8】