(19)【発行国】日本国特許庁(JP)
(12)【公報種別】特許公報(B2)
(11)【特許番号】
(24)【登録日】2022-05-13
(45)【発行日】2022-05-23
(54)【発明の名称】マルチセンサによる複合キャリブレーション装置及び方法
(51)【国際特許分類】
G01S 17/86 20200101AFI20220516BHJP
G06T 7/80 20170101ALI20220516BHJP
G01S 7/497 20060101ALI20220516BHJP
B25J 11/00 20060101ALI20220516BHJP
【ＦＩ】
G01S17/86
G06T7/80
G01S7/497
B25J11/00 Z
【請求項の数】
9
(21)【出願番号】P 2021003139
(22)【出願日】2021-01-12
(65)【公開番号】P2022039906
(43)【公開日】2022-03-10
【審査請求日】2021-01-13
(31)【優先権主張番号】202010881818.4
(32)【優先日】2020-08-28
(33)【優先権主張国・地域又は機関】CN
(73)【特許権者】
【識別番号】520022540
【氏名又は名称】中国計量大学
(74)【代理人】
【識別番号】100178434
【弁理士】
【氏名又は名称】李 じゅん
(72)【発明者】
【氏名】▲羅▼ 哉
(72)【発明者】
【氏名】江 文松
(72)【発明者】
【氏名】朱 志▲遠▼
(72)【発明者】
【氏名】▲趙▼ 洪楠
(72)【発明者】
【氏名】▲陳▼ ▲げい▼文
(72)【発明者】
【氏名】黄 杰伊
【審査官】松浦 陽
(56)【参考文献】
【文献】中国特許出願公開第１０９８２８２６２（ＣＮ，Ａ）
【文献】中国特許出願公開第１０３９８３９６１（ＣＮ，Ａ）
【文献】中国特許出願公開第１１０３２２５１９（ＣＮ，Ａ）
【文献】特表２０２０－５１６８８３（ＪＰ，Ａ）
【文献】米国特許第１０７２６５７９（ＵＳ，Ｂ１）
【文献】特表２０１８－５２６６４１（ＪＰ，Ａ）
【文献】中国特許出願公開第１１０５９９５４１（ＣＮ，Ａ）
【文献】中国特許出願公開第１１１６２７０７２（ＣＮ，Ａ）
【文献】特表２０２２－５１４９１２（ＪＰ，Ａ）
(58)【調査した分野】(Int.Cl.，ＤＢ名)
Ｇ０１Ｓ １７／８６
Ｇ０１Ｓ ７／４９７
Ｇ０６Ｔ ７／８０
Ｂ２５Ｊ １１／００
Ｂ２５Ｊ １３／０８
Ｂ２５Ｊ １９／０４
(57)【特許請求の範囲】
【請求項1】
ロボットアームを含み、前記ロボットアーム上にはセンサ融合フレームが設置され、センサ融合フレーム上にはレーザーレーダ、単眼カメラ、及びデータ処理のためのコンピュータが設置されるマルチセンサによる複合キャリブレーション装置であって、レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲットをさらに含み、前記レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲットは、一号キャリブレーションプレート、二号キャリブレーションプレート、三号キャリブレーションプレート、四号キャリブレーションプレートを含み、
一号キャリブレーションプレート、二号キャリブレーションプレート、三号キャリブレーションプレートの中心位置はドットでマークされ、外部パラメータキャリブレーションの特徴点を提供するために使用され、
四号キャリブレーションプレート上には、カメラの内部パラメータキャリブレーションのために黒白の碁盤目模様が配列され、
４つのキャリブレーションプレートは、「田」字の形状に配置され、一号キャリブレーションプレートと二号キャリブレーションプレートは並列に配置され、三号キャリブレーションプレートと四号キャリブレーションプレートは一号キャリブレーションと二号キャリブレーションプレートの前方に並列に配置され、三号キャリブレーションプレートと四号キャリブレーションプレートは一号キャリブレーションプレートと二号キャリブレーションプレートより低く、
三号キャリブレーションプレート表面の法線ベクトルｎ３と四号キャリブレーションプレート表面の法線ベクトルｎ４との間の角度差は３０°より大きく、
一号キャリブレーションプレート表面の法線ベクトルｎ１と二号キャリブレーションプレート表面の法線ベクトルｎ２との間の角度差は３０°より大きい、
マルチセンサによる複合キャリブレーション装置。
【請求項2】
請求項１に記載のマルチセンサによる複合キャリブレーション装置の複合キャリブレーション方法であって、カメラの内部パラメータキャリブレーションと、レーザーレーダ－カメラの外部パラメータキャリブレーションとを含み、
カメラの内部パラメータキャリブレーションの場合、異なる姿勢下でカメラが碁盤目キャリブレーションプレートに対して撮影を行うように、コンピュータユニットがロボットアームとカメラを制御し、張正友キャリブレーション方法を採用してカメラの内部パラメータＫを計算し、
レーザーレーダ－カメラの外部パラメータキャリブレーションの場合、以下のステップを含む、
ａ）キャリブレーションプレートモジュールがレーザーレーダ及びカメラの視野範囲に現れるように、コンピュータユニットがロボットアームの姿勢を調整するとともに、
ロボットアームを制御して静止状態を維持し、レーザーレーダ及びカメラはデータ採集を行う、
ｂ）レーザーレーダデータに対して分析及び処理を行い、点群特徴点を抽出し、
各点群の座標情報を記録し、異常点をフィルタリングし、点群分割法を採用して点群データを分割し、４つのキャリブレーションプレートの点群データを４つの異なるグループ｛Ｌ１｝、｛Ｌ２｝、｛Ｌ３｝，｛Ｌ４｝へ分け、Ｋ－ｍｅａｎｓ方法を採用して点群のクラスタリング中心点を抽出し、
ｃ）カメラデータに対して分析及び処理を行い、視覚特徴点を抽出し、
各ピクセルのグレー値を記録し、ＦＡＳＴキーポイント抽出アルゴリズムを採用し、キャリブレーションプレートのローカルピクセルのグレー値が明らかに変化された箇所を検出することにより、各キャリブレーションプレートの中心点位置を抽出し、
一号キャリブレーションプレート、二号キャリブレーションプレート、三号キャリブレーションプレートのドット中心点位置を抽出し、その座標値Ｃ１、Ｃ２、Ｃ３を記録し、
碁盤目間の関係を分析することにより、四号キャリブレーションプレートの中心点座標値Ｃ４を求め、
Ｃｉは、第ｉ番目のキャリブレーションプレートの中心点位置のカメラ座標系Ｃにおける二次元座標値であり、Ｃｉをマッチングのための視覚特徴点として選定する、
ｄ）レーザーレーダの特徴点
特徴点のマッチング関係によって最小再投影誤差を確立し、誤差方程式を構築し、誤差方程式によって最小二乗法による求解を行い、最適な外部パラメータ行列ＴＬＣを取得する、
ことを特徴とするマルチセンサによる複合キャリブレーション方法。
【請求項3】
キャリブレーションプレートの中心位置特徴点に基づいて、キャリブレーションプレートのエッジ位置を採集して特徴点とする、ことを特徴とする請求項２に記載のマルチセンサによる複合キャリブレーション方法。
【請求項4】
キャリブレーションプレートの角度を調整し、異なる角度の状態下で複数グループのキャリブレーションプレートのセンサデータを採集し、グループ毎のデータのキャリブレーションプレートの中心点座標値
ことを特徴とする請求項２に記載のマルチセンサによる複合キャリブレーション方法。
【請求項5】
前記カメラの内部パラメータキャリブレーションは、碁盤目キャリブレーション方法を採用しており、前記碁盤目キャリブレーション方法は、異なる姿勢下でカメラが碁盤目キャリブレーションプレートに対して撮影を行うように、コンピュータユニットがロボットアーム及びカメラを制御し、２０～３０枚の画像のエッジ情報を抽出し、張正友キャリブレーション方法を採用してカメラの内部パラメータＫを計算することである、
ことを特徴とする請求項２に記載のマルチセンサによる複合キャリブレーション方法。
【請求項6】
請求項１に記載のマルチセンサによる複合キャリブレーション装置の複合キャリブレーション方法であって、センサ融合フレーム上に配置された慣性ナビゲーションシステムをさらに含み、
複合キャリブレーション方法には、レーザーレーダ及び慣性ナビゲーションシステムによる複合キャリブレーション方法を含んでおり、
ロボットアームの初期運動時刻をｔ０として定義し、ロボットアームの運動終了時刻をｔｎとして定義し、ｔｎ時刻に走査されたレーザー点群をＰｎとして定義し、ｔ０時刻でのレーザーレーダの座標系をＬ０として定義し、ｔ０時刻での慣性ナビゲーションシステムの座標系をＩ０として定義し、ｔｎ時刻でのレーザーレーダの座標系をＬｎとして定義し、ｔｎ時刻での慣性ナビゲーションシステムの座標系をＩｎとして定義し、レーザーレーダのｔ０時刻からｔｎ時刻までの間の姿勢変換行列をＴＬとして定義し、慣性ナビゲーションシステムのｔ０時刻からｔｎ時刻までの間の姿勢変換行列をＴＩとして定義し、レーザーレーダと慣性ナビゲーションシステムとの間の外部パラメータ行列をＴＬＩとして定義すると、
前記複合キャリブレーション方法は、以下のステップを含む、
ａ）ロボットアームは指定された軌跡に従って運動を行い、ロボットアームの運動過程中に、レーザーレーダ及び慣性ナビゲーションシステムがデータ採集を行う、
ｂ）ロボットアームを制御して運動を停止し、センサによって採集されたデータに対して処理を行い、
レーザーレーダの場合、均一運動モデルに従ってフレーム毎のレーザー点群に対して歪み除去処理を行い、
点群フィルタリングアルゴリズムを採用し、点群データ中の外れ値を除去し、
計算の複雑さを軽減するために、ボクセルグリッド方法を使用してフレーム毎のレーザー点群データに対してダウンサンプリング処理を行う、
ｃ）レーザーレーダの座標系
姿勢変換行列ＴＬの計算方法は、以下の通りである、
反復最近傍法を採用して第ｉフレーム点群と第ｉ＋１フレーム点群をマッチングし、第ｉフレーム点群
第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換は、回転行列Ｒｉと並進ベクトルｔｉから構成されており、２フレーム点群の姿勢変換関係を示し、さらに、誤差方程式を構築し、誤差方程式を最小二乗問題に変換し、ＳＶＤ法を採用してＲｉ、ｔｉを計算し、Ｒｉ、ｔｉによって第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換行列を
ｄ）慣性ナビゲーションシステムの座標系
姿勢変換行列ＴＩの計算方法は、以下の通りである、
慣性ナビゲーションシステムの加速度測定データ及び角速度測定データに対して積分を行い、変位データｔＩ及び回転データＲＩを取得すると、慣性ナビゲーションシステムのｔ０時刻からｔｎ時刻までの姿勢変換行列
ｅ）ＴＬによってｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影し、点群ＰｎＬを取得する、
ｆ）ＴＩ及びキャリブレーション待ちパラメータＴＬＩによってｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影し、ＰｎＩを取得する、
ｇ）２グループの点群ＰｎＬ及びＰｎＩをマッチングし、２グループの点群を整列し、外部パラメータ行列ＴＬＩに対して最適化を行い、反復最近傍法を採用してＰｎＬ及びＰｎＩが記述された同一領域の点群領域をレジストレーションし、最近傍誤差Ｔｅｒｒｏｒを構築及び最適化し、Ｔｅｒｒｏｒ、ＴＬ、ＴＩによって外部パラメータ行列ＴＬＩを求解する、
ことを特徴とするマルチセンサによる複合キャリブレーション方法。
【請求項7】
視覚条件が十分である場合、単眼カメラの観測データを記録し、視覚－ＩＭＵキャリブレーションツールを採用し、単眼カメラと慣性ナビゲーションシステムとの間の外部パラメータ行列ＴＣＩを計算し、決定された３つの外部パラメータ行列ＴＬＣ、ＴＬＩ、ＴＣＩ間の姿勢一貫性に基づいて複合検証を行い、外部パラメータ行列中の変換パラメータを調整し、マルチセンサによる複合キャリブレーションの精度を向上し、
前記パラメータ最適化は、オンライン調整方法を採用し、レーザーレーダ、単眼カメラ、慣性ナビゲーションシステムのデータを融合し、ＴＬＩ、ＴＣＩに対して調整を行う、
ことを特徴とする請求項６に記載のマルチセンサによる複合キャリブレーション方法。
【請求項8】
視覚条件が十分である場合、キャリブレーションターゲットがカメラの視野範囲内に現れるように、ロボットアームを制御し、カメラが常にキャリブレーションターゲットを観測できる条件下で、ロボットアームを制御して空間内で六自由度の運動をし、単眼カメラの観測データを記録し、キャリブレーションツールを採用して視覚－慣性ナビゲーションシステムの外部パラメータ行列ＴＣＩを次のようにキャリブレーションする、
【請求項9】
レーザーレーダ、単眼カメラ、慣性測定ユニット間の姿勢変換行列に対して複合最適化を行い、決定された３つの外部パラメータ行列ＴＬＣ、ＴＬＩ、ＴＣＩ間の姿勢一貫性に基づいて複合検証を行う、
具体的なステップは、以下の通りである、
各姿勢変換誤差を計算する、
【数1】
その後、各姿勢変換行列誤差がすべて誤差閾値より小さいという制約を満たす条件下で、総誤差ｅｒｒｏｒＴが最小になるように、各姿勢変換行列に対して調整を行う、
【数2】
並進ベクトル及び回転行列中の各ベクトル成分を分析し、並進ベクトルに対して、次の調整を行う、
【数3】
回転行列に対して、次のような調整を行う、
【数4】
ここで、Δｒの大きさと正負は、
ことを特徴とする請求項７に記載のマルチセンサによる複合キャリブレーション方法。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、センサ技術分野に関するものである。特に、マルチセンサによるキャリブレーション技術に関するものである。
【背景技術】
【0002】
リアルタイム位置特定と地図構築（Ｓｉｍｕｌｔａｎｅｏｕｓ Ｌｏｃａｌｉｚａｔｉｏｎ ａｎｄ Ｍａｐｐｉｎｇ、ＳＬＡＭと称する）技術は、無人運転のための環境認識情報を提供しており、従来のＳＬＡＭ技術は、レーザーＳＬＡＭと視覚ＳＬＡＭに分けられる。レーザーレーダは測距精度が高く、光の影響を受けない等の利点があり、カメラはコストが低く、画像情報が豊富である等の利点がある。しかしながら、単一センサＳＬＡＭには、大きな局限性がある。例えば、レーザーレーダの場合、更新頻度が遅く、モーション歪みがあって、雨や雪等の過酷な環境下では正確な測定値を提供できない。カメラの場合、正確な三次元情報を取得できなく、周囲光による制限が比較的に大きい。
【0003】
慣性ナビゲーションシステムは、姿勢推定補助ツールとして、正確な角速度と加速度を提供できる。そのため、レーザーレーダ、視覚センサ、慣性ナビゲーションシステム等のマルチセンサデータを融合することにより、ＳＬＡＭの環境知覚能力を向上させることができる。
【0004】
ＳＬＡＭシステム中のセンサのキャリブレーションは、内部パラメータキャリブレーションと外部パラメータキャリブレーションに分けられる。センサの内部パラメータキャリブレーションは、主に、センサ測定データの正確さを確保するためのカメラの内部パラメータ行列の計算と慣性ナビゲーションシステムの誤差係数の計算を指す。センサ間の外部パラメータキャリブレーションは、マルチセンサ情報の融合を正確に行うための前提条件であり、センサ間の外部パラメータキャリブレーションは、各センサの座標系間の姿勢変換関係を決定することである。
【0005】
従来の外部パラメータキャリブレーション方法は、車体座標系を基準として、レーザーレーダの座標系、カメラ座標系、慣性ナビゲーションシステムの座標系から車体座標系への姿勢変換関係を求める。ただし、従来の外部パラメータキャリブレーションでは、すべてのセンサが車体内に固定され、車体は運動次元の制限を受けるため、キャリブレーション過程は複雑であり、ヨー角とロール角の正確なキャリブレーションを実現することは困難である。
【0006】
レーザーレーダは測距精度が高く、キャリブレーション参照物に対して特別な要件はないが、カメラは二次元画像特性に基づくため、キャリブレーションするためには特定のキャリブレーション参照物が必要である。現在、既存のレーザーレーダとカメラの外部パラメータキャリブレーション方法には、単一の碁盤目キャリブレーションプレートに基づくキャリブレーション方法、「Ｌ」字の形状のキャリブレーションプレートに基づくキャリブレーション方法、及び三次元碁盤目ターゲットに基づくキャリブレーション方法が含まれる。これらの方法は類似しており、すべてレーザーの三次元特徴点とカメラの二次元特徴点をマッチングすることにより、外部パラメータ行列の求解を行うことである。
【0007】
レーザーレーダと慣性ナビゲーションシステムのキャリブレーションは、運動条件下で行う必要があり、慣性ナビゲーションシステムは、正確な加速度測定値と角速度測定値を提供することができる。従来のキャリブレーション方法は、ハンドアイキャリブレーション方法であるが、そのキャリブレーションの精度を保証することは困難である。中国百度（バイドゥ）のＡｐｏｌｌｏ（アポロ）キャリブレーションツールは、「８」字の形状の回りを運動するように車両を制御することにより、センサのデータ採集と外部パラメータキャリブレーションを実現する。
【0008】
マルチセンサによる複合キャリブレーションは、現在無人運転の分野で最もホットなトピックの一つである。既存のキャリブレーション技術は、自動化の程度が低く、操作が複雑であり、精度が低い等のデメリットがある。
【発明の概要】
【0009】
本発明は従来技術に存在する上記の課題を解決するためになされたものであり、操作が簡単であり、精度が高いマルチセンサによる複合キャリブレーション装置及び方法を提供することを目的とする。
【0010】
本発明の目的は、以下の技術的解決策を通じて達成することができる。
【0011】
ロボットアームを含み、前記ロボットアーム上にはセンサ融合フレームが設置され、センサ融合フレーム上にはレーザーレーダ、単眼カメラ、及びデータ処理のためのコンピュータが設置され、レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲットをさらに含み、前記レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲットは、一号キャリブレーションプレート、二号キャリブレーションプレート、三号キャリブレーションプレート、四号キャリブレーションプレートを含み、一号キャリブレーションプレート、二号キャリブレーションプレート、三号キャリブレーションプレートの中心位置はドットでマークされ、外部パラメータキャリブレーションの特徴点を提供するために使用され、四号キャリブレーションプレート上には、カメラの内部パラメータキャリブレーションのために黒白の碁盤目模様が配列され、４つのキャリブレーションプレートは、「田」字の形状に配置され、一号キャリブレーションプレートと二号キャリブレーションプレートは並列に配置され、三号キャリブレーションプレートと四号キャリブレーションプレートは一号キャリブレーションと二号キャリブレーションプレートの前方に並列に配置され、三号キャリブレーションプレートと四号キャリブレーションプレートは一号キャリブレーションプレートと二号キャリブレーションプレートより低く、三号キャリブレーションプレート表面の法線ベクトルｎ３と四号キャリブレーションプレート表面の法線ベクトルｎ４との間の角度差は３０°より大きく、一号キャリブレーションプレート表面の法線ベクトルｎ１と二号キャリブレーションプレート表面の法線ベクトルｎ２との間の角度差は３０°より大きい、マルチセンサによる複合キャリブレーション装置を提供する。
【0012】
カメラの内部パラメータキャリブレーションと、レーザーレーダ－カメラの外部パラメータキャリブレーションとを含むマルチセンサによる複合キャリブレーション方法を提供する。カメラの内部パラメータキャリブレーションの場合、異なる姿勢下でカメラが碁盤目キャリブレーションプレートに対して撮影を行うように、コンピュータユニットがロボットアーム及びカメラを制御し、張正友（ＺｈａｎｇＺｈｅｎｇｙｏｕ）キャリブレーション方法を採用してカメラの内部パラメータＫを計算する。レーザーレーダ－カメラの外部パラメータキャリブレーションの場合、以下のステップを含む。
【0013】
ステップ１、キャリブレーションプレートモジュールがレーザーレーダ及びカメラの視野範囲に現れるように、コンピュータユニットがロボットアームの姿勢を調整するとともに、ロボットアームを制御して静止状態を維持し、レーザーレーダ及びカメラはデータ採集を行う。
【0014】
ステップ２、レーザーレーダデータに対して分析及び処理を行い、点群特徴点を抽出し、各点群の座標情報を記録し、異常点をフィルタリングし、点群分割法を採用して点群データを分割し、４つのキャリブレーションプレートの点群データを４つの異なるグループ｛Ｌ１｝、｛Ｌ２｝、｛Ｌ３｝，｛Ｌ４｝へ分け、Ｋ－ｍｅａｎｓ方法を利用して点群のクラスタリング中心点を抽出し、
は、第ｉ番目のキャリブレーションプレートの中心点位置のレーザーレーダの座標系における三次元座標値であり、
をマッチングのためのレーザー特徴点として選定する。
【0015】
ステップ３、カメラデータに対して分析及び処理を行い、視覚特徴点を抽出し、各ピクセルのグレー値を記録し、ＦＡＳＴキーポイント抽出アルゴリズムを採用し、キャリブレーションプレートのローカルピクセルのグレー値が明らかに変化された箇所を検出することにより、各キャリブレーションプレートの中心点位置を抽出し、一号キャリブレーションプレート、二号キャリブレーションプレート、三号キャリブレーションプレートのドット中心点位置を抽出し、その座標値Ｃ１、Ｃ２、Ｃ３を記録し、碁盤目間の関係を分析することにより、四号キャリブレーションプレートの中心点座標値Ｃ４を求め、Ｃｉは、第ｉ番目のキャリブレーションプレートの中心点位置のカメラ座標系Ｃにおける二次元座標値であり、Ｃｉをマッチングのための視覚特徴点として選定する。
【0016】
ステップ４、レーザーレーダの特徴点
及びカメラの特徴点
によってマッチング関係
を確立し、特徴点のマッチング関係によって最小再投影誤差を確立し、誤差方程式を構築し、誤差方程式によって最小二乗法による求解を行い、最適な外部パラメータ行列ＴＬＣを取得する。
【0017】
いくつかの実施形態では、キャリブレーションプレートの中心位置特徴点に基づいて、キャリブレーションプレートのエッジ位置を採集して特徴点とする。
【0018】
いくつかの実施形態では、前記カメラの内部パラメータキャリブレーションは、碁盤目キャリブレーション方法を採用しており、前記碁盤目キャリブレーション方法は、異なる姿勢下でカメラが碁盤目キャリブレーションプレートに対して撮影を行うように、コンピュータユニットがロボットアーム及びカメラを制御し、２０～３０枚の画像のエッジ情報を抽出し、張正友（ＺｈａｎｇＺｈｅｎｇｙｏｕ）キャリブレーション方法を採用してカメラの内部パラメータＫを計算することである。
【0019】
いくつかの実施形態では、センサ融合フレーム上に配置された慣性ナビゲーションシステムをさらに含む。複合キャリブレーション方法には、レーザーレーダ及び慣性ナビゲーションシステムによる複合キャリブレーション方法を含む。ロボットアームの初期運動時刻をｔ０として定義し、ロボットアームの運動終了時刻をｔｎとして定義し、ｔｎ時刻に走査されたレーザー点群をＰｎとして定義し、ｔ０時刻でのレーザーレーダの座標系をＬ０として定義し、ｔ０時刻での慣性ナビゲーションシステムの座標系をＩ０として定義し、ｔｎ時刻でのレーザーレーダの座標系をＬｎとして定義し、ｔｎ時刻での慣性ナビゲーションシステムの座標系をＩｎとして定義し、レーザーレーダのｔ０時刻からｔｎ時刻までの間の姿勢変換行列をＴＬとして定義し、慣性ナビゲーションシステムのｔ０時刻からｔｎ時刻までの間の姿勢変換行列をＴＩとして定義し、レーザーレーダと慣性ナビゲーションシステムとの間の外部パラメータ行列をＴＬＩとして定義する。
【0020】
前記複合キャリブレーション方法は、以下のステップを含む。
ステップ１、ロボットアームは指定された軌跡に従って運動を行い、ロボットアームの運動過程中に、レーザーレーダ及び慣性ナビゲーションシステムがデータ採集を行う。
ステップ２、ロボットアームを制御して運動を停止し、センサによって採集されたデータに対して処理を行う。レーザーレーダの場合、均一運動モデルに従ってフレーム毎のレーザー点群に対して歪み除去処理を行う。点群フィルタリングアルゴリズムを採用し、点群データ中の外れ値を除去する。計算の複雑さを軽減するために、ボクセルグリッド方法を使用してフレーム毎のレーザー点群データに対してダウンサンプリング処理を行う。
ステップ３、レーザーレーダの座標系
の初期時刻ｔ０での座標系Ｌ０と運動終了時刻での座標系Ｌｎとの間の姿勢変換行列ＴＬを計算する。姿勢変換行列ＴＬの計算方法は、下記の通りである。反復最近傍法を採用して第ｉフレーム点群と第ｉ＋１フレーム点群をマッチングし、第ｉフレーム点群
と第ｉ＋１フレーム点群
のマッチング関係を取得する。第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換は、回転行列Ｒｉと並進ベクトルｔｉから構成されており、２フレーム点群の姿勢変換関係を示す。さらに、誤差方程式を構築し、誤差方程式を最小二乗問題に変換し、ＳＶＤ法を採用してＲｉ、ｔｉを計算する。Ｒｉ、ｔｉによって第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換行列を
として取得する。さらに、ｎフレームのレーザーレーダの姿勢変換行列Ｔｉを累積的に乗算し、レーザーレーダのｔ０時刻からｔｎ時刻までの姿勢変換行列ＴＬを取得する。
ステップ４、慣性ナビゲーションシステムの座標系
の初期時刻ｔ０での座標系Ｉ０と運動終了時刻ｔｎでの座標系Ｉｎとの間の姿勢変換行列ＴＩを計算する。姿勢変換行列ＴＩの計算方法は、下記の通りである。慣性ナビゲーションシステムの加速度測定データ及び角速度測定データに対して積分を行い、変位データｔＩ及び回転データＲＩを取得すると、慣性ナビゲーションシステムのｔ０時刻からｔｎ時刻までの姿勢変換行列
に表される。
ステップ５、ＴＬによってｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影し、点群ＰｎＬを取得する。
ステップ６、ＴＩ及びキャリブレーション待ちパラメータＴＬＩによってｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影し、ＰｎＩを取得する。
ステップ７、２グループの点群ＰｎＬ及びＰｎＩをマッチングする。２グループの点群を整列し、外部パラメータ行列ＴＬＩに対して最適化を行う。反復最近傍法を採用してＰｎＬ及びＰｎＩが記述された同一点群領域をレジストレーションし、最近傍誤差Ｔｅｒｒｏｒを構築及び最適化し、Ｔｅｒｒｏｒ、ＴＬ、ＴＩによって外部パラメータ行列ＴＬＩを求解する。
ステップ１、ロボットアームは指定された軌跡に従って運動を行い、ロボットアームの運動過程中に、レーザーレーダ及び慣性ナビゲーションシステムがデータ採集を行う。
ステップ２、ロボットアームを制御して運動を停止し、センサによって採集されたデータに対して処理を行う。レーザーレーダの場合、均一運動モデルに従ってフレーム毎のレーザー点群に対して歪み除去処理を行う。点群フィルタリングアルゴリズムを採用し、点群データ中の外れ値を除去する。計算の複雑さを軽減するために、ボクセルグリッド方法を使用してフレーム毎のレーザー点群データに対してダウンサンプリング処理を行う。
ステップ３、レーザーレーダの座標系
ステップ４、慣性ナビゲーションシステムの座標系
ステップ５、ＴＬによってｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影し、点群ＰｎＬを取得する。
ステップ６、ＴＩ及びキャリブレーション待ちパラメータＴＬＩによってｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影し、ＰｎＩを取得する。
ステップ７、２グループの点群ＰｎＬ及びＰｎＩをマッチングする。２グループの点群を整列し、外部パラメータ行列ＴＬＩに対して最適化を行う。反復最近傍法を採用してＰｎＬ及びＰｎＩが記述された同一点群領域をレジストレーションし、最近傍誤差Ｔｅｒｒｏｒを構築及び最適化し、Ｔｅｒｒｏｒ、ＴＬ、ＴＩによって外部パラメータ行列ＴＬＩを求解する。
【0021】
いくつかの実施形態では、視覚条件が十分である場合、単眼カメラの観測データを記録し、視覚－ＩＭＵキャリブレーションツールを採用し、単眼カメラと慣性ナビゲーションシステムとの間の外部パラメータ行列ＴＣＩを計算する。決定された３つの外部パラメータ行列ＴＬＣ、ＴＬＩ、ＴＣＩ間の姿勢一貫性に基づいて複合検証を行い、外部パラメータ行列中の変換パラメータに対して調整を行い、マルチセンサによる複合キャリブレーションの精度を向上する。前記パラメータ最適化は、オンライン調整方法を採用し、レーザーレーダ、単眼カメラ、慣性ナビゲーションシステムのデータを融合し、ＴＬＩ、ＴＣＩに対して調整を行う。
【0022】
従来技術と比較して、本発明のマルチセンサによる複合キャリブレーション装置及び方法には、以下の利点がある。
【0023】
本発明は、１６ライン、３２ライン、６４ライン等のマルチラインレーザーレーダ、単眼カメラ、双眼カメラ、ＲＧＢＤカメラ等の視覚センサに適用される。本発明の実施形態は、キャリブレーション及び二次開発に便利な携帯型マルチセンサ融合フレーを構築する。本発明の実施形態は、ロボットアームによる補助キャリブレーション方法を使用することにより、インテリジェントキャリブレーション及びバッチキャリブレーションを実現することができる。
【図面の簡単な説明】
【0024】
図面（必ずしも一定の縮尺で描かれているわけではない）で、同様の参照番号は異なる図で同様の構成要素を記載する場合がある。異なる文字接尾辞を持つ同様の参照番号は、同様な構成要素の異なる例を表すことができる。図面は、限定ではなく例として、本明細書で論じられる各実施形態を示している。
【図1】センサ融合フレームが設置されているロボットアームの概略図である。
【図2】マルチセンサ融合フレームの概略図である。
【図3】レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲットの位置関係の概略図である。
【図4】キャリブレーションターゲットの空間配置の概略図である。
【図5】レーザーレーダ－カメラの複合キャリブレーションの原理概略図である。
【図6】特徴点の分布概略図である。
【図7】レーザー－視覚特徴マッチングの概略図である。
【図8】レーザーレーダの姿勢変換の概略図である。
【図9】慣性ナビゲーションシステムの姿勢変換の概略図である。
【図10】キャリブレーションのフローチャートである。
【発明を実施するための形態】
【0025】
以下は、本発明の具体的な実施例であり、図面を結合しながら本発明の技術的解決策をさらに説明するが、本発明は、これらの実施例によって限定されず、以下の実施形態は、請求の範囲に係る本発明を限定するものではない。また、実施形態に記載されている特徴のすべての組み合わせは、本発明の解決に必ずしも必須ではない。
【0026】
実施例
マルチセンサ融合フレーム１０１、ロボットアーム１０２、連結機構１０３、コンピュータユニット１０４、及び操作プラットフォームを含むマルチセンサによる複合キャリブレーション装置であって、前記マルチセンサ融合フレーム１０１は、マルチセンサによる複合構築法によってレーザーレーダ２０１、単眼カメラ２０２、及び慣性ナビゲーションシステム２０３を自由的に運動可能な金属フレーム２０４に固定され、このフレームは治具２０６を介して無人車両、無人航空機等の環境感知の場合に搭載され、二次開発に適用されるとともに、外部パラメータキャリブレーションの実施に便利である。前記金属フレームは、長さ１８ｃｍ、幅６ｃｍ、高さ８ｃｍである。マルチセンサによる複合構築法を採用して各センサは同じ座標系指向に従って設置される。そのうち、レーザーレーダ２０１は、金属フレーム２０４の上部の中央位置に設置され、固定装置２０５は金属フレームの内部に設置される。単眼カメラ２０２は、固定装置２０５の左側５ｃｍの位置に設置され、慣性ナビゲーションシステム２０３は固定装置２０５の右側５ｃｍの位置に設置される。所述ロボットアーム１０２は、操作プラットフォーム１０５の上方に設置され、三軸方向の並進及び回転運動を提供する。ロボットアームの末端は連結機構１０３を介して固定装置２０５に連接され、初期状態下に、マルチセンサ融合フレーム１０１の地面からの高さは１４０ｃｍである。前記コンピュータユニット１０３は、ボットアーム１０２の運動の制御、センサによるデータ採集の制御、センサデータの処理、外部パラメータ行列の計算を行う機能を有する。
マルチセンサ融合フレーム１０１、ロボットアーム１０２、連結機構１０３、コンピュータユニット１０４、及び操作プラットフォームを含むマルチセンサによる複合キャリブレーション装置であって、前記マルチセンサ融合フレーム１０１は、マルチセンサによる複合構築法によってレーザーレーダ２０１、単眼カメラ２０２、及び慣性ナビゲーションシステム２０３を自由的に運動可能な金属フレーム２０４に固定され、このフレームは治具２０６を介して無人車両、無人航空機等の環境感知の場合に搭載され、二次開発に適用されるとともに、外部パラメータキャリブレーションの実施に便利である。前記金属フレームは、長さ１８ｃｍ、幅６ｃｍ、高さ８ｃｍである。マルチセンサによる複合構築法を採用して各センサは同じ座標系指向に従って設置される。そのうち、レーザーレーダ２０１は、金属フレーム２０４の上部の中央位置に設置され、固定装置２０５は金属フレームの内部に設置される。単眼カメラ２０２は、固定装置２０５の左側５ｃｍの位置に設置され、慣性ナビゲーションシステム２０３は固定装置２０５の右側５ｃｍの位置に設置される。所述ロボットアーム１０２は、操作プラットフォーム１０５の上方に設置され、三軸方向の並進及び回転運動を提供する。ロボットアームの末端は連結機構１０３を介して固定装置２０５に連接され、初期状態下に、マルチセンサ融合フレーム１０１の地面からの高さは１４０ｃｍである。前記コンピュータユニット１０３は、ボットアーム１０２の運動の制御、センサによるデータ採集の制御、センサデータの処理、外部パラメータ行列の計算を行う機能を有する。
【0027】
レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲットをさらに含み、レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲットは、４つの同じサイズのキャリブレーションプレートから構成されている。前記レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲット１０４は、４つの３０ｃｍ×３０ｃｍのキャリブレーションプレートから構成されている。図３に示すように、一～三号キャリブレーションプレートの中央位置はドットでマークされ、外部パラメータキャリブレーションのための特徴点を提供する。四号キャリブレーションプレート上には、カメラの内部パラメータキャリブレーションのために黒白の碁盤目模様が配列される。４つのキャリブレーションプレートは、「田」字の形状で空中に配置される。４つのキャリブレーションプレートをより正確に分割するために、それらは異なる距離及び角度で環境中に配置される。図４に示すように、三号キャリブレーションプレート及び四号キャリブレーションプレートは高さ１２０ｃｍのベースを採用して固定し、ベースは水平線一上に配置され、マルチセンサ融合フレームとの距離はＤ１＝１３０ｃｍであり、三号キャリブレーションプレートの法線ベクトルｎ
３
と四号キャリブレーションプレートの法線ベクトルｎ
４
との間の角度差は３０°以上である。一号キャリブレーションプレート及び二号キャリブレーションプレートがカバーされないように確保するために、一号キャリブレーションプレート及び二号キャリブレーションプレートは高さ１６０ｃｍのベースを採用して固定し、ベースは水平線二上に配置され、マルチセンサ融合フレームとの距離はＤ２＝１８０ｃｍである。一号キャリブレーションプレートの法線ベクトルｎ１と二号キャリブレーションプレートの法線ベクトルｎ２との間の角度差は３０°以上である。
【0028】
前記カメラの内部パラメータキャリブレーションは、異なる姿勢下でカメラが碁盤目キャリブレーションプレートに対して撮影を行うように、コンピュータユニットがロボットアーム及びカメラを制御し、２０～３０枚の画像のエッジ情報を抽出し、張正友（ＺｈａｎｇＺｈｅｎｇｙｏｕ）キャリブレーション方法を採用してカメラの内部パラメータＫを計算する。
【0029】
前記レーザーレーダ－カメラによる四キャリブレーションプレート複合キャリブレーションターゲットは、単眼カメラの内部パラメータキャリブレーション及びレーザーレーダ－カメラの外部パラメータキャリブレーションに採用される。前記レーザーレーダと単眼カメラによる複合キャリブレーション方法は、以下のステップを含む。
【0030】
まず、図５に示すように、複合キャリブレーションターゲットがレーザーレーダ及びカメラの視野範囲に現れるように、コンピュータユニットがロボットアームの姿勢を調整する。ロボットアームを制御して静止状態を維持し、レーザーレーダとカメラを採用してデータ採集を行った後、コンピュータユニットが測定データを処理する。最後に、最小二乗問題を構築し、外部パラメータ行列ＴＬＣを求解する。
【0031】
前記データ採集によってレーザーレーダデータ及びカメラデータに対してそれぞれ処理を行う。
【0032】
レーザーレーダデータの場合、各点群の座標情報を記録し、異常点をフィルタリングし、点群分割法を採用して点群データを分割し、４つのキャリブレーションプレートの点群データを４つの異なるグループ｛Ｌ１｝、｛Ｌ２｝、｛Ｌ３｝，｛Ｌ４｝に分ける。Ｋ－ｍｅａｎｓ法を利用して点群のクラスタリング中心点を抽出する。
は、第ｉ番目のキャリブレーションプレートの中心点位置のレーザーレーダの座標系Ｌにおける三次元座標値である。
をマッチングのためのレーザー特徴点として選定する。
【0033】
カメラデータの場合、各ピクセルのグレー値を記録し、ＦＡＳＴキーポイント抽出アルゴリズムを採用し、キャリブレーションプレートのローカルピクセルのグレー値が明らかに変化された箇所を検出することにより、各キャリブレーションプレートの中心点位置を抽出する。一～三号キャリブレーションプレートのドットの中心位置を抽出し、その座標値Ｃ１、Ｃ２、Ｃ３を記録する。四号キャリブレーションプレートの場合、碁盤目間の関係を分析することにより、キャリブレーションプレートの中心点座標値Ｃ４を求める。
は、第ｉ番目のキャリブレーションプレートの中心点位置のカメラ座標系Ｃにおける二次元座標値である。Ｃｉをマッチングのための視覚特徴点として選定する。
レーザーレーダの三次元特徴点
及び単眼カメラの二次元特徴点
によってマッチング関係
を確立する。
図７に示すように、レーザーレーダ座標
及び単眼カメラ座標
の特徴点Ｐｉに基づくマッチング関係を定義することにより、最小再投影誤差を確立し、最小二乗問題を構築する。レーザーレーダ及びカメラの外部パラメータ行列を次のように定義する。
レーザーレーダの三次元特徴点
図７に示すように、レーザーレーダ座標
【数1】
【0034】
ここで、Ｒが回転行列であり、ｔが並進ベクトルである。マッチング関係及び外部パラメータ行列を結合すると、誤差方程式は次のように表すことができる。
【数2】
【0035】
ここで、ｓｉは視覚特徴点の深さ値であり、誤差が最小化になるように、誤差方程式に従って最小二乗最適化関数を構築し、最適な外部パラメータ行列を取得する。
【数3】
【0036】
さらに、必要に応じて、より多くの特徴点を採集してマッチングを行い、キャリブレーションプレートの中心位置特徴点に基づいて、キャリブレーションプレートのエッジ位置を採集して特徴点とする。図６に示すように、キャリブレーションプレート毎の５つの位置を選択して特徴点とし、上記方法に従って最小二乗最適化関数を構築し、外部パラメータ行列を求解する。実験によって特性点の数が多いほど、外部パラメータ行列の計算がより正確であることが証明されている。
【0037】
さらに、キャリブレーションプレートの角度を調整し、異なる角度の状態下で複数グループのキャリブレーションプレートのセンサデータを採集し、グループ毎のデータのキャリブレーションプレートの中心点座標値
をそれぞれ計算し、偏差の大きい中心点座標値を削除し、平均値策略を採用して次のように残りの中心点座標を計算する。
【数4】
【0038】
既存のキャリブレーション技術では、キャリブレーションプレートの位置及び角度は固定である。レーザーレーダの縦方向点群のスパース性によってレーザーレーダによる走査がキャリブレーションプレートのすべての領域を完全にカバーすることを保証することは困難である。レーザーレーダによる走査ラインがキャリブレーションプレートの完全な輪郭を完全に走査できなかった場合、点群特徴点（キャリブレーションプレートの中心点）の座標は、実際の位置と差異が大きくなってしまう。
【0039】
本方法は、レーザーレーダ及びカメラが複数グループのキャリブレーションプレートデータを採集するように、キャリブレーションプレートの角度を調整する。キャリブレーションプレートの中心点を角度調整の回転中心として採用するため、キャリブレーションプレートの調整に関係なく、その中心点座標は常に変わらない。本方法は、キャリブレーションプレートの中心点位置の不変性を利用して、異なる角度下で複数グループの点群データによって複数グループの中心点座標をフィッティングし、誤差が大きい中心点座標を除去し、残りの中心点座標の平均値をキャリブレーションプレートの点群特徴点とする。既存技術と比較して、本方法で抽出された特徴点の精度は大幅に向上されている。
【0040】
レーザーレーダ及び慣性ナビゲーションシステムの複合キャリブレーション方法は、次のステップで構成される。
【0041】
ロボットアームの支援下で、点群マッチングに基づくレーザーレーダ－慣性ナビゲーションシステムによる外部パラメータキャリブレーションを採用する。具体的なステップは下記の通りである。
【0042】
まず、ロボットアームを制御して運動し、センサが空間内で運動しながらデータを採集する。
【0043】
次に、ロボットアームの運動が停止され、センサによって採集されたデータに対して処理を行う。
【0044】
その後、レーザーレーダの初期時刻ｔ０での座標系Ｌ０と運動終了時刻ｔｎでの座標系Ｌｎとの間の姿勢変換行列ＴＬを計算する。慣性ナビゲーションシステムの初期時刻ｔ０での座標系Ｉ０と運動終了時刻ｔｎでの座標系Ｉｎとの間の姿勢変換行列ＴＩを計算する。
【0045】
再び、ＴＬによってｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影し、点群ＰｎＬを取得する。ＴＩ及びキャリブレーション待ちパラメータＴＬＩによってｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影し、ＰｎＩを取得する。最後に、２グループの点群ＰｎＬとＰｎＩをマッチングし、２グループの点群を整列することにより、外部パラメータ行列ＴＬＩを計算する。
【0046】
前記ロボットアームの運動は、指定された軌跡内を運動するようにロボットアームを制御することである。ロボットアームの座標軸を基準として、Ｘ軸の正方向に沿って１００ｃｍ移動し、Ｘ軸の負方向に沿って１００ｃｍ移動し、Ｙ軸の正方向に沿って１００ｃｍ移動し、Ｙ軸の負方向に沿って１００ｃｍ移動し、Ｚ軸の正方向に沿って１００ｃｍ移動し、Ｚ軸の負方向に沿って１００ｃｍ移動する。Ｘ軸を中心に時計回りに１８０°回転し、Ｘ軸を中心に反時計回りに１８０°回転し、Ｙ軸を中心に時計回りに１８０°回転し、Ｙ軸を中心に反時計回りに１８０°回転し、Ｚ軸を中心に時計回りに１８０°回転し、Ｚ軸を中心に反時計回りに１８０°回転する。ロボットアームの運動過程中に、レーザーレーダ及び慣性ナビゲーションシステムがデータ採集を行う。
【0047】
前記センサのデータ処理は、レーザーレーダの場合、均一運動モデルに従ってフレーム毎のレーザー点群に対して歪み除去処理を行い、点群フィルタリングアルゴリズムを採用し、点群データの外れ値を除去する。計算の複雑さを軽減するために、フレーム毎のレーザー点群データに対して、ボクセルグリッド方法を使用してダウンサンプリング処理を行うことである。
【0048】
前記姿勢変換行列ＴＬの計算方法は、次の通りである。反復最近傍法を採用して第ｉフレーム点群と第ｉ＋１フレーム点群をマッチングし、第ｉフレーム点群
と第ｉ＋１フレーム点群
とのマッチング関係を取得する。第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換が回転行列Ｒｉと並進ベクトルｔｉから構成されると定義すると、２フレーム間の対応する関係は次のように表される。
【数5】
【0049】
誤差方程式は、次のように表される。
【数6】
【0050】
最小二乗問題を構築することにより、ＳＶＤ法を採用してＲｉ、ｔｉを計算する。
【数7】
【0051】
第ｉ時刻から第ｉ＋１時刻までのレーザーレーダの姿勢変換行列は次の通りである。
【数8】
【0052】
ＴＬはレーザーレーダのｔ０時刻からｔｎ時刻までの姿勢変換行列であり、次のように表される。
【数9】
【0053】
前記姿勢変換行列ＴＩの計算方法は、下記の通りである。慣性ナビゲーションシステムの加速度測定データ及び角速度測定データに対して積分を行い、変位データｔＩ及び回転データＲＩを取得する。ｔ０時刻からｔｎ時刻までの慣性ナビゲーションシステムの姿勢変換行列は、次のように表される。
【数10】
【0054】
前記ｔｎ時刻でのレーザー点群Ｐｎをｔ０時刻でのＬ０座標系に投影するとは、レーザーレーダの場合、図８に示すように、直接に姿勢変換行列ＴＬによってＰｎのＬ０座標系における座標表示ＰｎＬを取得することができる。
【数11】
【0055】
【0056】
前記外部パラメータ行列ＴＬＩの計算とは、点群ＰｎＬと点群ＰｎＩを整列することである。理論的に、ＰｎＬ及び点群ＰｎＩは同一領域の点群データを記述しており、それらは空間上でオーバーラップする。即ち、次の通りである。
【数13】
【0057】
外部パラメータ誤差が存在するため、ＰｎＬ及びＰｎＩは完全にオーバーラップしているわけではない。反復最近傍法を採用して、ＰｎＬ及びＰｎＩが記載している同一領域の点群領域をレジストレーションし、最近傍誤差を構築して最適化する。
【数14】
【0058】
Ｔｅｒｒｏｒ、ＴＬ，ＴＩによって外部パラメータ行列ＴＬＩを求解する。
【数15】
【0059】
ここまで、上記キャリブレーション方法によって外部パラメータ行列ＴＬＣ及びＴＬＩを取得する。
【0060】
本発明は、点群レジストレーションに基づくレーザーレーダ－慣性ナビゲーションキャリブレーションであって、既存技術（点群マッチングに基づくレーザーレーダ／ＩＭＵ複合キャリブレーション方法）は小車にレーザーレーダ及びＩＭＵを搭載する方法を採用しているが、小車は平面内でしか運動できないため、完全な六自由度の運動を提供することはできない。本方法では、キャリブレーションシステムの運動装置としてロボットアームを採用しているため、豊富な空間運動を提供できる。従来技術と比較して、キャリブレーションの精度が向上されている。
【0061】
さらに、必要に応じて、視覚条件が十分である場合、単眼カメラの観測データを記録し、視覚－ＩＭＵキャリブレーションツールを採用し、単眼カメラと慣性ナビゲーションシステムとの間の外部パラメータ行列ＴＣＩを計算する。決定された３つの外部パラメータ行列ＴＬＣ、ＴＬＩ、ＴＣＩ間の姿勢一貫性に基づいて複合検証を行い、検証が失敗された場合、外部パラメータ行列中の変換パラメータを検証に成功するまで調整する。そのため、検証が失敗された場合、本発明の実施形態は、ＴＬＣ、ＴＬＩ、ＴＣＩに対してパラメータの最適化を行い、マルチセンサによる複合キャリブレーションの精度を向上する。前記パラメータ最適化は、オンライン調整方法を採用し、レーザーレーダ、単眼カメラ、慣性ナビゲーションシステムのデータを融合し、ＴＬＩ、ＴＣＩに対して調整を行う。
【0062】
さらに、必要に応じて、視覚条件が十分である場合、キャリブレーションターゲットがカメラの視野範囲内に現れるように、ロボットアームを制御する。カメラが常にキャリブレーションターゲットを観測できる条件下で、ロボットアームを制御して空間内で六自由度の運動をする。単眼カメラの観測データを記録し、キャリブレーションツールを採用して視覚－慣性ナビゲーションシステムの外部パラメータ行列ＴＣＩをキャリブレーションする。
【数16】
【0063】
さらに、レーザーレーダ、単眼カメラ、慣性測定ユニット間の姿勢変換行列に対して複合最適化を行う。決定された３つの外部パラメータ行列ＴＬＣ、ＴＬＩ、ＴＣＩ間の姿勢一貫性に基づいて複合検証を行う。原則的に、３つの外部パラメータ行列間には、次の数学的関係が存在する。
【数17】
【0064】
ただし、キャリブレーションの過程に誤差が存在するため、実際に姿勢一貫性を完全に達成することはできない。そのため、外部パラメータ行列中の変換パラメータに対して最適化を行い、マルチセンサによる複合キャリブレーションの精度を向上する必要がある。姿勢変換総誤差を最小化することにより、ＴＬＣ、ＴＬＩ、ＴＣＩを調整する。
【0065】
前記パラメータの最適化の具体的なステップは、次の通りである。
【0066】
各姿勢変換誤差を計算する。
【数18】
【0067】
【0068】
その後、各姿勢変換行列誤差がすべて誤差閾値より小さいという制約を満たす条件下で、総誤差ｅｒｒｏｒＴが最小になるように、各姿勢変換行列に対して調整を行う。
【数19】
【0069】
【0070】
並進ベクトル及び回転行列中の各ベクトル成分を分析し、並進ベクトルに対して次の調整を行う。
【数20】
【0071】
ここで、Δｘ、Δｙ、及びΔｚの大きさ及び正負は、
の具体的なベクトル成分を分析することによって取得する。回転行列に対して、次のような調整を行う。
【数21】
【0072】
ここで、Δｒの大きさと正負は、
の具体的なベクトル成分を分析することによって取得する。
【0073】
例を挙げて説明する。
【0074】
まず、主な誤差由来を特定する。誤差の大きさの順序が
である場合、ＴＬＣが最大の誤差由来と見なされる。ＴＬＣの誤差項目
及び
を分析する。
【0075】
しかし、誤差由來はＴＬＣだけではない。ＴＬＩ及びＴＣＩの場合、上記と同様なステップを採用して調整を行う。相違点は、ＴＬＣ、ＴＬＩ、及びＴＣＩに対して、それぞれα、β、及びγのステップサイズの調整（α＞β＞γ）を行うとともに、最大調整範囲を制限することである。
【数22】
【0076】
最適化過程中で、ＴＬＣ、ＴＬＩ、ＴＣＩを順番に調整し、ｅｒｒｏｒＴの変化を観察することにより、α、β、γのステップサイズを適切に変更する。調整過程中で、ｅｒｒｏｒＴが明らかに低下されることが観察されると、ステップサイズを適切に増やし、調整過程中で、ｅｒｒｏｒＴの緩やかな減少又は増加傾向が見られると、ステップサイズを適切に減らすか又はステップサイズの正負を変更する。
【0077】
ｅｒｒｏｒＴが最小になるまで、上記のステップに従って外部パラメータ行列を調整する。
【0078】
調整過程中で、計算結果が局所的な最適化になることを防ぐために、ＴＬＣ、ＴＬＩ、ＴＣＩの調整順番を変更するとともに、最小のｅｒｒｏｒＴが見つかるまで最適化結果を上記方法と比較する。
【0079】
既存技術と比較して、本発明のマルチセンサによる姿勢変換行列の複合最適化方法は、オフライン最適化方法を採用しており、ＴＬＣ、ＴＬＩ、及びＴＣＩ間の姿勢一貫性に基づいて姿勢変換行列総誤差の最適化関数を構築し、各外部パラメータに対してパラメータの調整を行う。この最適化方法は、各外部パラメータ行列の誤差を最小化し、精度を最大化する効果を達成する。
【0080】
本明細書では、いくつかの用語が多く使用されているが、他の用語を使用する可能性を排除することではない。これらの用語は、本発明の本質をより便利に記載及び説明するためにのみ使用される。それらをあらゆる追加の制限として解釈することは、本発明の精神に反することである。明細書及び図面に示される装置及び方法における動作、ステップ等の実行順序は、特に明記されていない限り、前の処理の出力が後の処理で使用されていない限り、任意の順序で実施することができる。説明の便宜上、「まず」、「次に」等を採用した説明は、必ずこの順序で実行すべきであることを意味するわけではない。
【0081】
本明細書に記載された特定の実施形態は、単に本発明の精神を例示するものである。当業者は、記載された特定の実施形態に対して様々な変更又は補足又は類似な方法への置き換えを行うことができるが、それは本発明の精神から逸脱したり、添付の特許請求の範囲によって定義された範囲を超えたりすることはない。
【符号の説明】
【0082】
１０１、センサ融合フレーム；１０２、ロボットアーム；１０３、連結機構；１０４、コンピュータユニット；１０５、操作プラットフォーム；２０１、レーザーレーダ；２０２、単眼カメラ；２０３、慣性ナビゲーションシステム；２０４、金属フレーム；２０５、固定装置；２０６、治具。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】
【図10】
