ホーム > 特許ランキング > 北京図森智途科技有限公司
特開2022-140374姿勢推定方法及び装置、並びに関連デバイス及び記憶媒体
(19)【発行国】日本国特許庁(JP)
(12)【公報種別】公開特許公報(A)
(11)【公開番号】P2022140374
(43)【公開日】2022-09-26
(54)【発明の名称】姿勢推定方法及び装置、並びに関連デバイス及び記憶媒体
(51)【国際特許分類】
G06T 7/70 20170101AFI20220915BHJP
G06T 7/00 20170101ALI20220915BHJP
H04N 5/232 20060101ALI20220915BHJP
【ＦＩ】
G06T7/70 B
G06T7/00 650Z
H04N5/232 290
【審査請求】未請求
【請求項の数】15
【出願形態】ＯＬ
(21)【出願番号】P 2022035891
(22)【出願日】2022-03-09
(31)【優先権主張番号】202110258991.3
(32)【優先日】2021-03-10
(33)【優先権主張国・地域又は機関】CN
(71)【出願人】
【識別番号】521254764
【氏名又は名称】北京図森智途科技有限公司
(74)【代理人】
【識別番号】110002217
【氏名又は名称】弁理士法人矢野内外国特許事務所
(72)【発明者】
【氏名】潘亮亮
(72)【発明者】
【氏名】趙季
【テーマコード（参考）】
5C122
5L096
【Ｆターム（参考）】
5C122DA03
5C122DA09
5C122DA13
5C122DA14
5C122DA27
5C122EA06
5C122EA59
5C122FH10
5C122FH11
5C122FH12
5C122FH14
5C122GA01
5C122HA13
5C122HA35
5C122HA46
5C122HA76
5C122HA88
5C122HB01
5C122HB10
5L096AA02
5L096AA06
5L096BA04
5L096CA04
5L096DA02
5L096FA66
5L096FA67
5L096FA69
5L096GA30
(57)【要約】 （修正有）
【課題】視覚観測モデル制約と運動モデル制約の融合モデルを用いて複雑なシーンでの姿勢推定の精度、安定性及びロバスト性を向上させる姿勢推定方法及び装置並びにモバイルデバイス、コンピューティングデバイス及びコンピュータ読み取り可能な記憶媒体を提供する。
【解決手段】姿勢推定方法は、車両の移動中にカメラが収集した複数のフレームの画像及び慣性測定ユニットが収集した慣性データを受信し、画像に基づいて視覚観測モデル制約を構築し、慣性データに基づいて運動モデル制約を構築し、且つ、視覚観測モデル制約及び運動モデル制約に基づいて、各画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出する。
【選択図】図３
【解決手段】姿勢推定方法は、車両の移動中にカメラが収集した複数のフレームの画像及び慣性測定ユニットが収集した慣性データを受信し、画像に基づいて視覚観測モデル制約を構築し、慣性データに基づいて運動モデル制約を構築し、且つ、視覚観測モデル制約及び運動モデル制約に基づいて、各画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出する。
【選択図】図３
【特許請求の範囲】
【請求項1】
対象の移動中にカメラが収集した複数の画像及び慣性測定ユニットが収集した慣性データを受信するステップと、
前記画像に基づいて視覚観測モデル制約を構築するステップと、
前記慣性データに基づいて運動モデル制約を構築するステップと、
前記視覚観測モデル制約及び運動モデル制約に基づいて各画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出するステップと、を含み、
前記カメラ及び慣性測定ユニットは前記対象に固定的に取り付けられる、姿勢推定方法。
前記画像に基づいて視覚観測モデル制約を構築するステップと、
前記慣性データに基づいて運動モデル制約を構築するステップと、
前記視覚観測モデル制約及び運動モデル制約に基づいて各画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出するステップと、を含み、
前記カメラ及び慣性測定ユニットは前記対象に固定的に取り付けられる、姿勢推定方法。
【請求項2】
前記画像に基づいて視覚観測モデル制約を構築するステップは、
前記複数の画像の間における特徴のレジストレーション誤差に基づいてローカル視覚観測モデル制約を構築するステップ、又は
前記地図における特徴と前記画像における特徴との間のレジストレーション誤差に基づいてグローバル視覚観測モデル制約を構築するステップの少なくとも１つを含む、請求項１に記載の方法。
前記複数の画像の間における特徴のレジストレーション誤差に基づいてローカル視覚観測モデル制約を構築するステップ、又は
前記地図における特徴と前記画像における特徴との間のレジストレーション誤差に基づいてグローバル視覚観測モデル制約を構築するステップの少なくとも１つを含む、請求項１に記載の方法。
【請求項3】
前記慣性データに基づいて運動モデル制約を構築するステップは、
前記複数の画像における所定数の画像及び前記所定数の画像の収集期間内に収集された慣性データを含むスライディング時間ウィンドウを構築するステップと、
前記スライディング時間ウィンドウ内の慣性データに基づいて運動モデル制約を構築するステップと、を含む請求項１に記載の方法。
前記複数の画像における所定数の画像及び前記所定数の画像の収集期間内に収集された慣性データを含むスライディング時間ウィンドウを構築するステップと、
前記スライディング時間ウィンドウ内の慣性データに基づいて運動モデル制約を構築するステップと、を含む請求項１に記載の方法。
【請求項4】
前記画像に基づいて視覚観測モデル制約を構築するステップは、
前記スライディング時間ウィンドウ内の所定数の画像の間の特徴のレジストレーション誤差に基づいてローカル視覚観測モデル制約を構築するステップ、又は、
前記スライディング時間ウィンドウ内の所定数の画像と地図との間における特徴のレジストレーション誤差に基づいてグローバル視覚観測モデル制約を構築するステップの少なくとも１つを含む、請求項３に記載の方法。
前記スライディング時間ウィンドウ内の所定数の画像の間の特徴のレジストレーション誤差に基づいてローカル視覚観測モデル制約を構築するステップ、又は、
前記スライディング時間ウィンドウ内の所定数の画像と地図との間における特徴のレジストレーション誤差に基づいてグローバル視覚観測モデル制約を構築するステップの少なくとも１つを含む、請求項３に記載の方法。
【請求項5】
前記画像における特徴は投影特徴及び対応する実特徴を含み、前記画像と地図との間のレジストレーション誤差は、
前記地図における特徴の前記画像での投影特徴及び対応する実特徴を決定し、
前記投影特徴及び実特徴に基づいて前記レジストレーション誤差を算出するように算出され、
前記地図における特徴は車線及び交通標識のうちの少なくとも１つを含む、請求項１に記載の方法。
前記地図における特徴の前記画像での投影特徴及び対応する実特徴を決定し、
前記投影特徴及び実特徴に基づいて前記レジストレーション誤差を算出するように算出され、
前記地図における特徴は車線及び交通標識のうちの少なくとも１つを含む、請求項１に記載の方法。
【請求項6】
前記運動モデル制約は、前記慣性データに関連する事前積分因子制約、前記慣性データに関連する入力ノイズ制約及び前記慣性データに関連する測定値偏差制約の少なくとも１つを含み、
前記方法は、
前記慣性測定ユニットの現在の画像時刻における状態変数の事前積分量を取得するために、前記慣性データを処理するステップと、
前記事前積分量及び前記慣性測定ユニットの前の画像時刻における状態変数に基づき、前記状態変数の残差を算出し、前記事前積分因子とするステップと、
を更に含む請求項１に記載の方法。
前記方法は、
前記慣性測定ユニットの現在の画像時刻における状態変数の事前積分量を取得するために、前記慣性データを処理するステップと、
前記事前積分量及び前記慣性測定ユニットの前の画像時刻における状態変数に基づき、前記状態変数の残差を算出し、前記事前積分因子とするステップと、
を更に含む請求項１に記載の方法。
【請求項7】
前記視覚観測モデル制約及び運動モデル制約に基づいて各画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出するステップは、
前記視覚観測モデル制約及び運動モデル制約に基づいて前記慣性測定ユニットの状態変数を取得するステップと、
前記慣性測定ユニットの状態変数に基づいて各画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出するステップと、を含む、請求項１～６のいずれか一項に記載の方法。
前記視覚観測モデル制約及び運動モデル制約に基づいて前記慣性測定ユニットの状態変数を取得するステップと、
前記慣性測定ユニットの状態変数に基づいて各画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出するステップと、を含む、請求項１～６のいずれか一項に記載の方法。
【請求項8】
前記状態変数は、前記慣性測定ユニットの変位、速度、方向、ジャイロのバイアス及びジッタ因子の少なくとも１つを含む、請求項７に記載の方法。
【請求項9】
前記視覚観測モデル制約及び運動モデル制約に基づいて前記慣性測定ユニットの状態変数を取得するステップは、
前記視覚観測モデル制約及び運動モデル制約に基づき、前記慣性測定ユニットの前記スライディング時間ウィンドウ内の現在の画像時刻における状態変数を算出し、且つ前記慣性測定ユニットの前記スライディング時間ウィンドウ内の少なくとも１つの前の画像時刻における状態変数を更新するステップを含む、請求項７に記載の方法。
前記視覚観測モデル制約及び運動モデル制約に基づき、前記慣性測定ユニットの前記スライディング時間ウィンドウ内の現在の画像時刻における状態変数を算出し、且つ前記慣性測定ユニットの前記スライディング時間ウィンドウ内の少なくとも１つの前の画像時刻における状態変数を更新するステップを含む、請求項７に記載の方法。
【請求項10】
前記視覚観測モデル制約、運動モデル制約と前記状態変数との間の関係に基づいて以下の式を構築するステップであって、
そのうち、ｘ＊は状態変数を表し、ａｒｇｍｉｎは事後確率が最小値を取る変数集合を表し、ｒｐ（Ｘ）は前の時刻に周辺化された事前項を表し、
は慣性測定ユニットの事前積分因子、即ち慣性測定ユニットの状態変数の予測値の残差を表し、
はグローバル観測の車線実線の投影因子、即ちスライディング時間ウィンドウ内の第ｉ時刻における視覚車線実線制約の残差を表し、前記実線投影因子のうちのＺｉ はｉ番目の車線サンプリング点の視覚観測を表し、
はグローバル観測の車線破線の投影因子、即ちスライディング時間ウィンドウ内の第ｋ時刻における視覚車線破線の端点制約の幾何学的残差を表し、当該破線投影因子のうちのＺｉはｉ番目の車線破線点の視覚観測を表し、
及び
はそれぞれ車線実線点及び破線点の観測共分散を表し、
は事前積分の共分散を表すステップを更に含む、請求項７に記載の方法。
【請求項11】
各画像時刻のカメラ姿勢を得るために、収集された画像が所定数より小さいことに応答し、当該連続フレームの画像に対して純視覚的なカメラ姿勢予測を行うステップと、
前記慣性測定ユニットと前記カメラに基づいて融合するカメラ姿勢予測を行うために、収集された画像が所定数に等しいことに応答し、前記慣性測定ユニットの状態変数の初期化値を算出するステップと、を更に含み、
前記状態変数の初期化値は前記所定数の画像のカメラ姿勢に基づいて算出される、請求項７に記載の方法。
前記慣性測定ユニットと前記カメラに基づいて融合するカメラ姿勢予測を行うために、収集された画像が所定数に等しいことに応答し、前記慣性測定ユニットの状態変数の初期化値を算出するステップと、を更に含み、
前記状態変数の初期化値は前記所定数の画像のカメラ姿勢に基づいて算出される、請求項７に記載の方法。
【請求項12】
前記純視覚的なカメラ姿勢予測は、
全地球測位システムの位置情報に基づいて前記地図を決定するステップと、
前記地図における特徴及び前記画像における特徴のレジストレーション制約に基づき、各画像時刻に対応する姿勢を算出するステップと、
を含む請求項１１に記載の方法。
全地球測位システムの位置情報に基づいて前記地図を決定するステップと、
前記地図における特徴及び前記画像における特徴のレジストレーション制約に基づき、各画像時刻に対応する姿勢を算出するステップと、
を含む請求項１１に記載の方法。
【請求項13】
前記慣性測定ユニットの状態変数の初期化値を算出するステップは、
前記スライディング時間ウィンドウ内の隣接する画像間の相対姿勢を算出するステップと、
前記相対姿勢の回転変化量及び前記慣性データの隣接する画像間における角速度増分に基づき、前記慣性測定ユニットの初期ジャイロバイアスを算出するステップと、
前記スライディング時間ウィンドウ内の複数の画像のカメラ姿勢軌跡及び前記慣性データの隣接する画像間における変位増分に基づき、前記慣性測定ユニットの初期加速度を算出するステップと、
前記初期ジャイロバイアス及び初期加速度に基づいて前記慣性測定ユニットの初期速度を算出するステップと、
を含む請求項１１に記載の方法。
前記スライディング時間ウィンドウ内の隣接する画像間の相対姿勢を算出するステップと、
前記相対姿勢の回転変化量及び前記慣性データの隣接する画像間における角速度増分に基づき、前記慣性測定ユニットの初期ジャイロバイアスを算出するステップと、
前記スライディング時間ウィンドウ内の複数の画像のカメラ姿勢軌跡及び前記慣性データの隣接する画像間における変位増分に基づき、前記慣性測定ユニットの初期加速度を算出するステップと、
前記初期ジャイロバイアス及び初期加速度に基づいて前記慣性測定ユニットの初期速度を算出するステップと、
を含む請求項１１に記載の方法。
【請求項14】
メモリと、前記メモリに通信可能に接続された１つ又は複数のプロセッサとを含むコンピューティングデバイスであって、
前記メモリには、前記１つ又は複数のプロセッサにより実行可能な命令が記憶されており、前記命令が前記１つ又は複数のプロセッサにより実行される時に、前記１つ又は複数のプロセッサに請求項１～１３のいずれか一項に記載の方法を実現させる、コンピューティングデバイス。
前記メモリには、前記１つ又は複数のプロセッサにより実行可能な命令が記憶されており、前記命令が前記１つ又は複数のプロセッサにより実行される時に、前記１つ又は複数のプロセッサに請求項１～１３のいずれか一項に記載の方法を実現させる、コンピューティングデバイス。
【請求項15】
コンピュータプログラムが記憶されているコンピュータ読み取り可能な記憶媒体であって、前記コンピュータプログラムがプロセッサにより実行される時に、前記プロセッサに請求項１～１３のいずれか一項に記載の方法を実現させる、コンピュータ読み取り可能な記憶媒体。
【発明の詳細な説明】
【技術分野】
【0001】
本発明は、自動運転分野に関し、特に姿勢推定方法及び装置、モバイルデバイス、コンピューティングデバイス及びコンピュータ読み取り可能な記憶媒体に関する。
【背景技術】
【0002】
視覚位置決め技術は、画像撮影時のカメラの位置及び姿勢（又は回転角度）を取得するために用いられ、自動運転、ロボット、ドローン、ウェアラブルデバイス及びＶＲメガネ等の分野において幅広く応用されている。自動運転のシーンでは、車両にカメラが取り付けられ、カメラが撮影した画像に基づいて人工知能アルゴリズムを組み合わせて車両の位置決めとナビゲーションを実現する。車両走行中の揺れ、バンプ、急ブレーキ、急カーブ等によりカメラに非常に深刻なジッタ、回転等の問題が発生するため、どのように正確な姿勢を得ることができるかは早急に解決すべき技術的問題となる。
【0003】
従来の姿勢推定方法は、例えば、カメラが収集した画像と全地球測位データに基づく融合最適化方法を採用する。ただし、当該融合最適化方法の精度及び安定性は全地球測位システム（ＧＰＳ）の状態によって制限され、トンネル及びビルに遮蔽されたシーンで信号のジャンプが発生し、カメラ姿勢推定結果のドリフトを直接引き起こす。更に、連続フレームの画像のカメラ姿勢推定はいずれも前のフレームの推定結果に基づくため、姿勢誤差がシステムで伝達されて累積され、更にカメラ姿勢推定結果のドリフトを増大させる。
【発明の概要】
【発明が解決しようとする課題】
【0004】
上記問題に鑑みて、本発明の目的は、視覚観測モデル制約と運動モデル制約の融合モデルを用いて複雑なシーンでの姿勢推定の精度、安定性及びロバスト性を向上させる姿勢推定方法及び装置、並びに関連デバイス及び記憶媒体を提供することである。
【課題を解決するための手段】
【0005】
本発明の第１の態様によれば、対象の移動中にカメラが収集した複数のフレームの画像及び慣性測定ユニットが収集した慣性データを受信するステップと、画像に基づいて視覚観測モデル制約を構築するステップと、慣性データに基づいて運動モデル制約を構築するステップと、視覚観測モデル制約及び運動モデル制約に基づいて各フレームの画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出するステップと、を含み、カメラ及び慣性測定ユニットは対象に固定的に取り付けられる、姿勢推定方法を提供する。
【0006】
本発明の第２の態様によれば、対象の移動中にカメラが収集した複数のフレームの画像及び慣性測定ユニットが収集した慣性データを受信するためのデータ受信モジュールと、画像に基づいて視覚観測モデル制約を構築するための第１の構築モジュールと、前記慣性データに基づいて運動モデル制約を構築するための第２の構築モジュールと、視覚観測モデル制約及び運動モデル制約に基づいて各フレームの画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を得る姿勢算出モジュールと、を含み、カメラ及び慣性測定ユニットは前記対象に固定的に取り付けられる、姿勢推定装置を提供する。
【0007】
本発明の第３の態様によれば、モバイルデバイスであって、上記姿勢推定装置と、モバイルデバイスに固定的に取り付けられ、それぞれ前記モバイルデバイスの移動中に複数のフレームの画像を収集すること及び慣性データを収集することに用いられるカメラ及び慣性測定ユニットと、を含むモバイルデバイスを提供する。
【0008】
本発明の第４の態様によれば、メモリと、前記メモリに通信可能に接続された１つ又は複数のプロセッサとを含むコンピューティングデバイスであって、前記メモリには、前記１つ又は複数のプロセッサにより実行可能な命令が記憶されており、前記命令が前記１つ又は複数のプロセッサにより実行されることで、前記１つ又は複数のプロセッサに上記方法を実現させる、コンピューティングデバイスを提供する。
【0009】
本発明の第５の態様によれば、コンピュータプログラムが記憶されているコンピュータ読み取り可能な記憶媒体であって、前記コンピュータプログラムがプロセッサにより実行される時に、上記方法を実現する、コンピュータ読み取り可能な記憶媒体を提供する。
【0010】
本発明の実施例の提供する姿勢推定方法は、視覚観測モデル制約と運動モデル制約の融合モデルを用いて姿勢を推定する。慣性測定ユニットが収集した慣性データは環境の変化による影響をほとんど受けないため、当該姿勢推定方法は環境が変化する時にも連続した姿勢推定結果を提供することができる。慣性測定ユニットの収集フレームレートがカメラの収集フレームレートより高いため、当該姿勢推定方法は姿勢推定のリアルタイム性を向上させることができる。対象の移動過程の複雑なシーンでは、トンネル及びビルの遮蔽により、全地球測位システムの位置情報が一時的に欠落する可能性があり、又は、障害物の遮蔽又は悪天候により、意味特徴が一時的に欠落する可能性がある。当該姿勢推定方法は、視覚観測モデル制約と運動モデル制約の融合モデルに基づき、複雑なシーンで連続的で確実な姿勢推定結果を取得することができるため、複雑なシーンでの姿勢推定の精度、リアルタイム性、安定性及びロバスト性を向上させることができる。
【0011】
好ましい実施例では、グローバル視覚観測モデル制約は、正規化されたカメラ座標系を用い、そのうち、世界座標系がＩＭＵ座標系に変換され、ＩＭＵ座標系がカメラ座標系に変換される。運動モデル制約とともに融合最適化を行う場合、姿勢推定精度を更に向上させることができる。
【0012】
好ましい実施例では、純視覚的なカメラ姿勢予測から慣性測定ユニットの状態変数を初期化し、続いて視覚観測モデル制約と運動モデル制約の融合モデルを用いて姿勢を推定する。慣性測定ユニットの状態変数に対する初期化は、高速デカップリングを実現することができ、融合最適化の収束を更に加速することができる。
【0013】
好ましい実施例では、視覚観測モデル制約は、ローカル視覚観測モデル制約及びグローバル視覚観測モデル制約の少なくとも１つを含む。そのうち、ローカル視覚観測モデル制約は、複数のフレームの画像間における特徴のレジストレーション誤差に基づいて構築され、グローバル視覚観測モデル制約は、画像と地図の特徴のレジストレーション誤差に基づいて構築される。そのうち、特徴は、例えば、意味特徴、幾何学的特徴、空間関係特徴、形状特徴のうちの少なくとも１つを含むことができる。複数のフレームの画像の間において、特徴は、更に画像の色特徴、テクスチャ特徴のうちの少なくとも１つを含むことができる。これに限定されないことが当然であり、当業者は、必要に応じて他の画像特徴を追加することができ、本願はこれを制限しない。自動運転の応用シーンでは、車線は道路の連続的な特徴であり、車線制約に基づいて連続的な姿勢推定結果が得られるため、複雑なシーンでの姿勢推定の精度、安定性及びロバスト性を更に向上させることができる。
【発明の効果】
【0014】
本発明の実施例の提供する姿勢推定装置、コンピューティングデバイス及びコンピュータ読み取り可能な記憶媒体は、上記位置推定方法を実行するために用いられる。本発明の実施例の提供するモバイルデバイスは、当該姿勢推定方法を用いてカメラ姿勢を取得し、且つカメラ姿勢に基づいてデバイスの位置決めを行うため、デバイスの位置決め精度、安定性及びロバスト性を向上させることができる。
【図面の簡単な説明】
【0015】
以下の図面を参照した本発明の実施例の説明により、本発明の上記及び他の目的、特徴及び利点はより明らかになるであろう。
【図1】センサホルダにセンサが取り付けられた車両の正面図を示す。
【図2】世界座標系、画像座標系と画像平面座標系との関係概略図を示す。
【図3】本発明の第１の実施例による姿勢推定方法のフローチャートを示す。
【図4】本発明の第２の実施例による姿勢推定方法のフローチャートを示す。
【図5】本発明の第３の実施例による姿勢推定方法のフローチャートを示す。
【図7】姿勢推定方法に用いられるスライディング時間ウィンドウアルゴリズムの原理概略図を示す。
【図8】本発明の第４の実施例による姿勢推定装置の概略ブロック図を示す。
【図9】本発明の第５の実施例によるコンピューティングデバイスの概略ブロック図を示す。
【発明を実施するための形態】
【0016】
当業者が本発明の技術的解決手段を詳しく理解できるように、以下、本発明の実施例の図面を参照して、本発明の実施例の技術的解決手段を明らか且つ全面的に説明するが、説明される実施例は、本発明の一部の実施例に過ぎず、全部の実施例ではないことが明らかである。当業者が本発明の実施例に基づいて創造的な工夫をせずに得られた他の全ての実施例は、本発明により保護される範囲に属する。
【0017】
本願で使用される用語は以下のように定義される。
ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ，慣性測定ユニット，その内部に三軸ジャイロ（三軸偏向角度を測定するために用いられる）及び三軸加速度計（三軸加速度を測定するために用いられる）が設けられている。
ＧＰＳ：Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ，全地球測位システム。
ＧＮＳＳ：Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ，全地球航法衛星システム。
ＩＭＵ：Ｉｎｅｒｔｉａｌ Ｍｅａｓｕｒｅｍｅｎｔ Ｕｎｉｔ，慣性測定ユニット，その内部に三軸ジャイロ（三軸偏向角度を測定するために用いられる）及び三軸加速度計（三軸加速度を測定するために用いられる）が設けられている。
ＧＰＳ：Ｇｌｏｂａｌ Ｐｏｓｉｔｉｏｎｉｎｇ Ｓｙｓｔｅｍ，全地球測位システム。
ＧＮＳＳ：Ｇｌｏｂａｌ Ｎａｖｉｇａｔｉｏｎ Ｓａｔｅｌｌｉｔｅ Ｓｙｓｔｅｍ，全地球航法衛星システム。
【0018】
姿勢は、任意の対象の位置及び姿勢を含み、３つの位置自由度及び３つの向き自由度が含まれる６つの自由度を含み、３つの向き自由度は、一般的には、ピッチ角（Ｐｉｔｃｈ）、ロール角（Ｒｏｌｌ）、ヨー角（Ｙａｗ）で表され、例えば、カメラ、慣性測定ユニット及び車両はそれぞれの姿勢を有する。
【0019】
状態変数は、変位、速度、方向、ジャイロのバイアス、ジッタ因子などが含まれる、姿勢推定に関連する任意の対象の運動パラメータを含む。
【0020】
視覚観測は、システムが取得した視覚データ、例えばカメラが収集した画像を含む。
【0021】
運動は、システムが取得した運動データ、例えば、慣性測定ユニットが収集した慣性データを含む。
【0022】
慣性データは、慣性測定ユニットが出力した加速度及び回転角データを含む。
【0023】
意味特徴は、地図及びカメラが収集した画像内で検出されて識別された目標特徴、例えば、画像内でマークされた車線及び標識を含む。
【0024】
意味地図は、意味特徴がマークされた高精度地図を含み、例えば、地図に車線実線と破線、及び標識（例えば交通標識）等をマークする。
【0025】
残差は、数理統計において実際の観測値と推定値との間の誤差を表すものを含み、カメラ姿勢推定式の残差を最小二乗法で求めて最適化されたカメラ姿勢パラメータを得ることができる。
【0026】
スライディング時間ウィンドウは、動的更新のための固定時間幅のデータ構造を含み、協同最適化のための各時点の状態変数及び観測データを記憶するために用いられる。１つのスライディング時間ウィンドウは、一般的には、当該時間幅内の複数の画像フレームを含むか、又は、複数の画像フレームごとに１つのスライディング時間ウィンドウを構成することができる。例えば、スライドウィンドウは、画像シーケンスの第１フレームからスライドし始め、ｍフレームごとに１つのスライドウィンドウを構成することができる。また、各画像フレームは対応する画像時刻（即ちタイムスタンプ）を有する。当該画像時刻／タイムスタンプは、カメラがトリガされる時のトリガ時間、ピクチャが露光する時の露光時間、カメラが画像を収集する時の収集時間、カメラがコンピューティングデバイスに画像を送信する時の送信時間、コンピューティングデバイスが画像を受信する時の受信時間のいずれか１つであってもよい。理解すべきものとして、本開示における画像時間は同一タイプの時間を採用し、即ち、あるフレーム画像は収集時間を基準とすると、他のフレーム画像も収集時間を基準とする。スライディング時間ウィンドウは、更に対応する時間幅にわたって収集された慣性データを含む。一般的に、ＩＭＵ収集頻度は画像収集フレームレートより高いため、１つのスライディング時間ウィンドウ内に一定数の画像フレームがあり、隣接する画像時刻の間に複数の慣性データを有する。
【0027】
本発明の実施例では、係るカメラ座標系と車両座標系との間の変換関係、カメラ座標系と世界座標系との間の変換関係、カメラの画像座標系とイメージング平面座標系との間の変換関係、カメラのイメージング平面座標系とカメラ座標系との間の変換関係は、全ての従来技術を含む。
【0028】
本発明の実施例では、車両に適用されるカメラ姿勢推定方法及び装置を例に説明する。しかしながら、本発明の姿勢推定方法及び装置は、これに限定されず、ロボット、ドローン、ウェアラブルデバイス、ＶＲメガネ等の任意の対象に更に適用することができる。本願は、技術的解決手段の応用シーンを限定しない。
【0029】
以下、図面と実施例に合わせて本発明の具体的な実施例をより詳細に説明する。
【0030】
図１は、センサホルダにセンサが取り付けられた車両の正面図を示す。車両１００は、セダン型自動車、トラック、自動二輪車、バス、船、飛行機、ヘリコプター、芝刈り機、ショベル、スノーモービル、航空機、レクリエーション用車両、遊園地車両、農場装置、建築装置、路面電車、ゴルフカート、列車、トロリーバス、又は他の車両であってもよい。
【0031】
車両１００は、完全又は部分的に自動運転モードで動作することができる。車両１００は、自動運転モードでそれ自体を制御することができ、例えば、車両１００は、車両の現在状態及び車両が位置する環境の現在状態を決定し、当該環境における少なくとも１つの他の車両の予測挙動を決定し、当該少なくとも１つの他の車両がこの予測挙動を実行する可能性に対応する信頼レベルを決定し、決定された情報に基づいて車両１００自体を制御することができる。自動運転モードにある場合、車両１００は、人と対話することなく動作することができる。
【0032】
車両１００は、駆動システム、センサシステム、制御システム、ユーザインタフェースシステム、コンピューティングシステム及び通信システムなどの各種の車両システムを含むことができる。車両は、より多いか又はより少ないシステムを含んでもよく、各システムは、複数のユニットを含むことができる。更に、車両の各システムとユニットとは、互いに接続されてもよい。例えば、コンピューティングシステムは、駆動システム、センサシステム、制御システム、ユーザインタフェースシステム及び通信システムのうちの１つ又は複数とデータ通信を行うことができる。それにより、車両１００の１つ又は複数の説明される機能は、付加的な機能性部材又は実体部材として分けられるか、又は、数がより少ない機能性部材又は実体部材として組み合わせられることが可能である。
【0033】
車両１００の駆動システムは、車両１００に運動エネルギーを提供する複数の操作可能な部材（又はユニット）を含むことができる。一実施例では、駆動システムは、エンジン又は電動機、車輪、変速機、電子システム及び動力（又は動力源）を含むことができる。エンジン又は電動機は、内燃機関、モータ、蒸気機関、燃料電池エンジン、プロパンエンジン又は他の形態のエンジン又は電動機の任意の組み合わせであってもよい。いくつかの実施例では、エンジンは、動力源を機械的エネルギーに変換することができる。いくつかの実施例では、駆動システムは、様々なエンジン又は電動機を含むことができる。例えば、ハイブリッド車両は、ガソリンエンジン及び電動機を含んでもよく、それ以外の場合を含んでもよい。
【0034】
車両１００のセンサシステムは、車両１００の環境及び条件の情報を感知するための複数のセンサを含むことができる。例えば、センサシステムは、慣性測定ユニット（ＩＭＵ）、ＧＮＳＳ（全地球航法衛星システム）、トランシーバ（例えば、全地球測位システム（ＧＰＳ）トランシーバ）、レーダ（ＲＡＤＡＲ）、レーザ距離計／ＬＩＤＡＲ（又は他の距離測定装置）、音響センサ、超音波センサ及びカメラ又は画像キャプチャ装置を含むことができる。センサシステムは、車両１００を監視するための複数のインダクタ（例えば、酸素（Ｏ２）モニタ、燃料計センサ、エンジン油圧センサ及び温度・湿度・圧力センサ等）を更に含んでもよい。他のセンサを更に配置してもよい。センサシステムに含まれる１つ又は複数のセンサは、１つ又は複数のセンサの位置、方向、又はその両方を更新するために、単独で駆動されるか又は一括で駆動されることができる。
【0035】
ＩＭＵは、慣性加速に基づいて車両１００の位置変化及び方向変化を感知するために、センサの組み合わせ（例えば、加速器及びジャイロ）を含むことができる。ＧＰＳトランシーバは、車両１００の地理的位置を推定するための任意のセンサであってもよい。この目的のために、ＧＰＳトランシーバは、地球に対する車両１００の位置情報を提供するために、受信機／送信機を含むことができる。ＧＰＳは、全地球航法衛星システムの一例であるため、いくつかの実施例では、ＧＰＳトランシーバは、北斗衛星ナビゲーションシステムトランシーバ又はガリレオ衛星ナビゲーションシステムトランシーバに置き換えられてもよいことを説明しておく。レーダユニットは、無線信号を用いて車両１００が位置する環境における対象を感知することができる。いくつかの実施例では、対象を感知するほか、レーダユニットは、車両１００に接近する物体の速度及び前進方向を感知するために用いることもできる。レーザ距離計又はＬＩＤＡＲユニット（又は他の距離測定装置）は、レーザを用いて車両１００が位置する環境における物体を感知する任意のセンサであってもよい。一実施例では、レーザ距離計／ＬＩＤＡＲユニットは、レーザ光源、レーザスキャナー及び感知器を含むことができる。レーザ距離計／ＬＩＤＡＲユニットは、連続（例えば、ヘテロダイン検出を使用する）又は不連続の検出モードで動作するために用いられる。カメラは、車両１００が位置する環境の複数の画像をキャプチャするための装置を含むことができる。カメラは、静止画像カメラ又は動的ビデオカメラであってもよい。
【0036】
車両１００のコンピューティングシステムは、少なくとも１つのプロセッサ（それが少なくとも１つのマイクロプロセッサを含むことができる）を含むことができ、プロセッサは、不揮発性コンピュータ読み取り可能な媒体（例えば、データ記憶装置又はメモリ）に記憶される処理命令（即ち、機械実行可能命令）を実行する。コンピューティングシステムは、車両の部材又はシステムを分散的に制御する複数のコンピューティング装置であってもよい。いくつかの実施例では、メモリには、プロセッサによって車両の様々な機能を実現するように実行される処理命令（例えば、プログラム論理）を含むことができる。一実施例では、コンピューティングシステムは、駆動システム、センサシステム、制御システム、ユーザインタフェースシステム、及び／又は通信システムとデータ通信を行うことができる。コンピューティングシステムにおけるインタフェースは、コンピューティングシステムと駆動システム、センサシステム、制御システム、ユーザインタフェースシステム、及び通信システムとの間のデータ通信を促進するために用いられる。
【0037】
メモリは、データ送信用の命令、データ受信用の命令、インタラクション用の命令、又は駆動システム、センサシステム、又は制御システム又はユーザインタフェースシステムを制御するための命令を含む、他の命令を含んでもよい。処理命令を記憶するほか、メモリは、画像処理パラメータ、道路地図及び経路情報などの様々な情報又はデータを記憶することができる。車両１００が自動方式、半自動方式及び／又は手動モードで動作する間、これらの情報は、車両１００及びコンピューティングシステム１５０によって使用されることができる。
【0038】
いくつかの実施例では、車両１００のコンピューティングシステムは、自動運転制御機能の一部又は全部を実現することができ、例えば、１つ又は複数のメモリ（又はデータ記憶装置）に常駐するプログラムコード命令を利用して実現し、且つ１つ又は複数のプロセッサによって実行されることができる。コンピューティングシステムは、様々な車両システム（例えば、駆動システム、センサシステム及び制御システム）から受信した入力、又はユーザインタフェースシステムから受信した入力に基づき、車両１００の機能を制御することができる。例えば、コンピューティングシステムは、制御システムからの入力を用いて、センサシステムにより検出された障害物を回避するようにステアリングユニットを制御することができる。一実施例では、コンピューティングシステムは、車両１００及びそのシステムの複数の面を制御するために用いることができる。
【0039】
いくつかの実施例では、車両１００は独立した自動運転制御ユニットを含み、例えば、様々な特定用途向け回路論理、様々なプロセッサ、様々なフィールドプログラマブルゲートアレイ（「ＦＰＧＡ」）、様々な特定用途向け集積回路（「ＡＳＩＣ」）、様々なリアルタイムコントローラ及びハードウェアを少なくとも部分的に使用して実現することができる。
【0040】
車両１００の運転室の上方には、センサホルダ１１０が固定的に取り付けられる。センサホルダ１１０の取り付け位置は運転室の上方に限定されず、運転室の両側、エンジンカバーの上方、フロントバンパー等のデータを収集しやすい任意の位置に取り付けられてもよい。センサホルダ１１０の数は１つに限られず、車両１００の異なる位置に複数のセンサホルダを固定的に取り付けてもよい。センサホルダには、センサシステムの複数のセンサが設けられ、例えばカメラ１１１及び慣性測定ユニット１１２が固定的に取り付けられる。カメラ１１１及び慣性測定ユニット１１２は、センサホルダの同じ位置に取り付けられてもよいし、又は異なる位置に取り付けられてもよい。カメラ１１１及び慣性測定ユニット１１２は、例えば、カメラ１１１の姿勢と慣性測定ユニット１１２の姿勢がほぼ一致するように、センサ１１０上にスタッキングされることが好ましく、それにより、融合された姿勢最適化において、慣性測定ユニット１１２の状態変数に基づいてカメラ１１１の姿勢パラメータを取得することができる。運転室内には、車両１１０の位置情報を推定するとともに位置情報に基づいてカメラの姿勢を大まかに予測するためのＧＰＳトランシーバが設けられる。
【0041】
上記実施例では、車両１００が様々な部材（又はユニット）を含むことが説明され、これらの部材（又はユニット）のうちの１つ又は複数は、車両１００に搭載されるか又は車両１００に単独で関連付けられてもよい。例えば、コンピューティングシステムは、部分的又は全体的に車両１００から独立して存在してもよい。従って、車両１００は、分離又は集積されたデバイスユニットの形態で存在することができる。車両１００を構成するデバイスユニットの間は、有線通信又は無線通信の方式で相互通信を実現することができる。いくつかの実施例では、付加的な部材又はユニットを各システムに追加するか、又はシステムから１つ以上の部材又はユニットを取り外すことができる。
【0042】
図２は、世界座標系、ＩＭＵ座標系、カメラ座標系と画像平面座標系との関係概略図を示す。以下に説明される姿勢推定方法では、複数の座標系の座標変換に及んでいる。
【0043】
自動運転の応用シーンでは、図１に示すように、カメラは車両のセンサホルダに取り付けられ、車両の移動中に、カメラの位置はそれに伴って変化する。世界座標系は絶対座標系であり、カメラ座標系はカメラ位置を原点とする座標系であり、ＩＭＵ座標系は慣性測定ユニットの位置を原点とする座標系である。環境内に位置する物体の３Ｄ座標は、全て世界座標系、カメラ座標系及びＩＭＵ座標系を用いて表すことができる。
【0044】
世界座標系における物体の３Ｄ座標ｐｗは、式（１）に示すように、回転行列
及び並進ベクトル
を用いてＩＭＵ座標系の３Ｄ座標ｐｂに変換することができる。
【0045】
【数1】
【0046】
ＩＭＵ座標系における物体の３Ｄ座標ｐｂは、式（２）に示すように、回転行列
及び並進ベクトル
を用いてカメラ座標系の３Ｄ座標ｐｃに変換することができる。
【0047】
【数2】
【0048】
画像平面座標系は、カメラによって撮影された画像平面の二次元座標系である。環境内に位置する任意の物体は、カメラを介して画像平面へイメージングされ、３Ｄ座標を２Ｄ座標に投影する。カメラ座標系の原点Ｏとイメージング平面座標系の原点Ｏ’は同一の軸（Ｚ軸）にあり、ＯＯ’はカメラの焦点距離である。式（３）に示すように、カメラ座標系における任意の物体の３Ｄ座標ｐｃは、カメラ内部パラメータＫを用いて画像平面座標系の２Ｄ座標ｐｕｖに変換することができる。
【0049】
【数3】
【0050】
そのうち、カメラ内部パラメータＫはカメラの特性パラメータであり、カメラの出荷時に既に決定された。
【0051】
本発明の実施例では、意味地図の意味特徴の特徴点は、世界座標系における３Ｄ座標として表され、２回の座標変換を経てから、カメラ画像の画像平面へ投影されることで、レジストレーション誤差を算出する。１回目の座標変換は世界座標系からＩＭＵ座標系への変換であり、２回目の座標変換はＩＭＵ座標系からカメラ座標系への変換である。従って、当該座標変換方法によれば、カメラの姿勢変化を含むだけでなく慣性測定ユニットの姿勢変化をも含む正規化されたカメラ座標系が得られ、視覚観測モデルと運動モデルとの融合最適化が可能となる。
【0052】
図３は、本発明の第１の実施例による姿勢推定方法のフローチャートを示す。図１を参照し、対象（例えば車両）のセンサホルダにカメラ及び慣性測定ユニットが固定的に取り付けられ、当該姿勢推定方法は、対象（例えば車両）の走行中に各フレームの画像時刻に対応するカメラ姿勢を推定するために用いられる。以下、車両を例とするが、実際には、当該車両は任意の対象であってもよく、例えば任意のモバイルデバイスであってもよく、セダン型自動車、トラック、自動二輪車、バス、船、飛行機、ヘリコプター、芝刈り機、ショベル、スノーモービル、航空機、レクリエーション用車両、遊園地車両、農場装置、建築装置、路面電車、ゴルフカート、列車、トロリーバス、又は他の車両を含むが、これらに限定されないことを理解されたい。
【0053】
ステップＳ０１において、車両の移動中にカメラが収集した複数のフレームの画像及び慣性測定ユニットが収集した慣性データを受信する。
【0054】
車両の移動中に、カメラ及び慣性測定ユニットは、それぞれ各々のフレームレートで画像及び慣性データを収集する。慣性データは、例えば、慣性測定ユニットが出力する加速度及び回転角である。慣性測定ユニットの収集フレームレートはカメラの収集フレームレートより高く、例えば、同じ時間帯にｍフレームの画像とｎ組の慣性データを収集し、そのうち、ｎ＞ｍである。従って、隣接する画像フレームの間で複数組の慣性データを取得することができる。
【0055】
本発明の実施例の姿勢推定方法において、スライディング時間ウィンドウを構築し、連続フレームのセンサデータを処理する。スライディング時間ウィンドウは、例えば、時間幅が固定されており、複数のフレームの画像における所定数のフレームの画像と、隣接する画像時刻の間の慣性データを含む（図７に示す）。各スライディング時間ウィンドウにおいて各画像フレームに対応するカメラ姿勢推定結果を出力する。スライディング時間ウィンドウは、状態変数の全確率密度の推定及び更新を再帰的に維持することができるように、時系列に沿ってスライドする。
【0056】
ステップＳ０２において、画像に基づいてグローバル視覚観測モデル制約を構築する。グローバル視覚観測モデル制約は、画像と地図との間の特徴のレジストレーション誤差を特徴付けるために用いられる。
【0057】
ここで、地図は、例えば点群地図、意味地図等の点の空間位置情報を提供できるだけでよく、本発明は地図のタイプを特に限定しない。一般的には、意味特徴を提供する意味地図を予め構築することができる。自動運転について、意味情報とは、無人車が走行ルールをよりよく理解し、路面交通状況を感知し、走行経路を計画することを可能にし、且つ高精度地図に包含される、異なるレベル、異なる次元の情報のことである。意味特徴とは、地図において検出されて識別された目標特徴のことである。高精度地図に車線、標識、街灯柱、信号機、自動車、分離帯、街路樹、青空等の多くの異なる対象及び概念をマークすることにより、意味地図を形成する。
【0058】
当分野で知られているように、意味地図の作成は、例えば、データ収集及びデータ処理を含む。高精度の全地球測位システム、慣性測定ユニット及びレーザレーダが搭載された収集車を利用してシーンのデータ収集を行う。収集されたデータに対して前処理を行い、慣性測定ユニットの慣性データとレーザレーダの点群フレーム間のレジストレーション結果とを融合し、姿勢図のグローバル最適化を行い、グローバルに一致する点群地図を生成する。点群地図に対して意味特徴の抽出を行い、点群地図から対応する車線（実線及び破線を含む）、交通信号機及び灯柱等の意味元素を抽出する。最後に、これらの意味元素の点群に対して意味マーキングを行い、その３Ｄ座標に合わせて、対応する意味地図にパラメータ化する。意味地図は、東－北－上（Ｅａｓｔ－Ｎｏｒｔｈ－Ｕｐ，ＥＮＵ）座標系を基準系とする。意味地図の作成については、本願において単に例示的に説明しただけであり、これに対して何らの制限を加えるわけではない。
【0059】
更に、カメラが収集した画像に対して前処理を行い、画像における意味特徴及び動的目標を抽出し、且つ画像における意味特徴に対してベクトル化を行う。
【0060】
自動運転の応用シーンでは、車線は道路の連続的な特徴であり、車線制約に基づいて連続的な姿勢推定結果を得ることに寄与する。この実施例では、車線実線及び破線は、好ましい意味特徴とされる。しかしながら、本発明はこれに限定されず、グローバル視覚観測モデル制約において、任意の画像特徴を採用することができ、意味特徴を含むことができるだけでなく非意味特徴（これらの非意味特徴は対応する記述符号を提供することができる）を含むこともできる。意味特徴は、車線及び標識（例えば交通標識）のうちの少なくとも１つを含み、そのうち、車線は、破線車線及び実線車線のうちの少なくとも１つを含み、標識は、街灯柱、信号機、ゴミ箱、広告掲示板、電柱等を含むが、これらに限定されない。非意味特徴は、幾何学的特徴、空間関係特徴、形状特徴等を含むが、これらに限定されない。
【0061】
動的目標のマスクは、後続の意味地図のデータとの関連付け及び異常点／外点の除去に用いられる。例えば、意味地図で意味特徴点を選択した後、ある点の画像での投影点が動的目標に遮蔽された場合、対応する意味特徴点を除去する。
【0062】
更に、スライディング時間ウィンドウ内のカメラ画像と意味地図との間の意味特徴のレジストレーション誤差を算出する。例えば、このステップでは、車線実線のレジストレーション誤差と車線破線のレジストレーション誤差をそれぞれ算出する。具体的には、地図における特徴の画像での投影特徴及び当該画像での対応する実特徴を決定し、且つ投影特徴及び実特徴に基づいてレジストレーション誤差を算出することができる。例えば、地図における特徴点Ａについて、この点Ａを画像に投影して対応する投影点Ｂを得ることができ、画像においてこの点Ａに対応する実点は点Ｃであり、このようにして投影点Ｂと実点Ｃとの間のレジストレーション誤差を算出することができる。当該投影特徴は、例えば、車線破線点／実線点の投影点であり、対応する実特徴は、車線破線点又は車線実線を識別することができる。この時、地図と画像における各車線破線の各コーナーを決定し、続いて地図におけるコーナーを画像に投影し、投影点と画像において決定されたコーナーの実値をマッチングし、対応するレジストレーション誤差を算出することができる。なお、地図における特徴を画像に投影して投影点を得てもよいし、画像における特徴を地図に投影してもよく、本発明はこれを限定しない。
【0063】
これに基づき、スライディング時間ウィンドウ内の所定数のフレームの画像と地図との間の特徴のレジストレーション誤差に基づいてグローバル視覚観測モデル制約を構築することができ、当該所定数のフレームはスライディング時間ウィンドウの全てのフレーム又は一部のフレームであってもよく、本発明はこれを限定しない。
【0064】
意味地図において、車線実線は連続線であり、車線実線からサンプリングされた複数の画素点ｐを取得するように車線実線を切断し、画素点ｐに対して座標変換を行って世界座標系からカメラ画像に投影し、且つ画素点とカメラ画像における最も近い車線実線との距離を算出してレジストレーション誤差とし、それにより車線実線の投影因子
を取得し、式（４）に示す通りである。
【0065】
【数4】
【0066】
そのうち、
は地図からサンプリングされた車線実線の特徴点のうちｉ番目のサンプルの世界座標系における座標を表し、
及び
はそれぞれ第ｋ時刻における世界座標系からＩＭＵ座標系までの回転行列及び並進ベクトルを表し、
及び
はそれぞれＩＭＵ座標系からカメラ座標系までの回転行列及び並進ベクトルを表し、ＩＭＵからカメラへの外部パラメータとも記され、
及び
はそれぞれセンサホルダのジッタの回転行列及び並進ベクトルを表し、関数ｐｒｏｊｅｃｔ（・）はピンホールカメラの投影モデルであり、特徴点をカメラ座標系から画像の画像平面に投影するために用いられ、関数ｄｉｓｔ＿ｉｍｇは車線実線の距離関数を表し、関数ｄｉｓｔ（ｐ，ｄｉｓｔ＿ｉｍｇ）は画素点ｐから最も近い車線実線までの距離を表す。第ｋ時刻は、画像収集時刻、即ち第ｋフレームの画像の収集時刻であってもよいと理解されるべきである。
【0067】
上記関数ｄｉｓｔ＿ｉｍｇは、カメラ画像変換に基づいて生成されたデジタル画像を表す（Ｇ． Ｊ． Ｃ． Ｖ． Ｇ． Ｂｏｒｇｅｆｏｒｓ ａｎｄ Ｉ． Ｐｒｏｃｅｓｓｉｎｇ， 『Ｄｉｓｔａｎｃｅ ｔｒａｎｓｆｏｒｍａｔｉｏｎｓ ｉｎ ｄｉｇｉｔａｌ ｉｍａｇｅｓ』，ｖｏｌ． ３４，１９８６を参照する）。以下の方式で関数ｄｉｓｔ＿ｉｍｇを得る：カメラ画像において、車線実線が位置する画素値を０とし、他の位置の画素値を１とし、車線実線に関する意味画像ｓｅｇ＿ｍｇを得て、意味画像ｓｅｇ＿ｍｇに対して距離変換を行って関数ｄｉｓｔ＿ｉｍｇを得て、そのうち、各画素値はｓｅｇ＿ｍｇにおける対応する位置の画素から現在の画像（ｓｅｇ＿ｍｇ）の最も近いゼロ画素までの画素距離を表す。
【0068】
意味地図において、車線破線を複数の画素点ｐとしてサンプリングし（例えば４つのコーナーとしてサンプリングする）、画素点ｐに対して座標変換を行って世界座標系からカメラ画像に投影し、且つ画素点とカメラ画像における対応する画素点との距離を算出してレジストレーション誤差とし、それにより車線破線の投影因子
を得て、式（５）に示す通りである。
【0069】
【数5】
【0070】
そのうち、ｐｉ＿ｕｖはカメラ画素からサンプリングされた車線破線の特徴点におけるｉ番目のサンプルの画素座標を表し、
は地図からサンプリングされた車線破線の特徴点のうちｉ番目のサンプルの世界座標系における座標を表し、
及び
はそれぞれ第ｋ時刻における世界座標系からＩＭＵ座標系までの回転行列及び並進ベクトルを表し、
及び
はそれぞれＩＭＵ座標系からカメラ座標系までの回転行列及び並進ベクトルを表し、
及び
はそれぞれセンサホルダのジッタの回転行列及び並進ベクトルを表し、関数ｐｒｏｊｅｃｔ（・）はピンホールカメラの投影モデルであり、特徴点をカメラ座標系から画像の画像平面に投影するために用いられる。
【0071】
上記グローバル視覚観測モデル制約において、レジストレーション誤差を算出するように、車線実線からも車線破線からもサンプリングされた特徴点は２回の座標変換を経てから、カメラ画像の画像平面に投影される。１回目の座標変換は世界座標系からＩＭＵ座標系への変換であり、２回目の座標変換はＩＭＵ座標系からカメラ座標系への変換である。従って、上記グローバル視覚観測モデル制約は、カメラの姿勢変化を含むだけでなく慣性測定ユニットの姿勢変化をも含む正規化されたカメラ座標系を採用する。グローバル視覚観測モデル制約は正規化されたカメラ座標系を採用するため、グローバル視覚観測モデル制約の車線実線の投影因子及び車線破線の投影因子は、以下の運動モデル制約におけるＩＭＵ事前積分因子とともに融合最適化を行うことができる。
【0072】
ステップＳ０３において、慣性データに基づいて運動モデル制約を構築する。運動モデル制約は、慣性測定ユニットの事前積分量の残差を特徴付けるために用いられる。
【0073】
スライディング時間ウィンドウ内に複数のフレームの画像における所定数のフレームの画像と隣接する画像時刻の間の慣性データを含むことが知られており、ステップＳ０３は、具体的には、スライディング時間ウィンドウ内の慣性データに基づいて運動モデル制約を構築することを含むことができ、当該慣性データは、所定数のフレームの画像収集時間帯内に収集された慣性データである。ここで、運動モデル制約は、慣性データが合致すべき運動制約を含み、具体的には、慣性データの連続画像フレームの間における事前積分因子制約、慣性データの入力ノイズ制約及び慣性データの測定値偏差制約の少なくとも１つを含む。例えば、元のＩＭＵの入力ノイズはガウスホワイトノイズ分布に合致し、元のＩＭＵの測定値偏差はランダムウォークに合致する。当然のことながら、当業者は、必要に応じて他の制約条件を設定してもよく、本発明はこれを制限しない。
【0074】
このステップでは、慣性測定ユニットが収集した慣性データに対して前処理を行い、慣性データは、例えば、バイアス（ｂｉａｓ）及びノイズ（ｎｏｉｓｅ）を含む加速度及び角速度である。隣接する画像フレーム間の１組の慣性データを数値積分（中央値積分又はオイラー積分）方法により伝播し、且つ事前積分方法によってこの組の慣性データを処理し、絶対姿勢に依存しない事前積分量（それぞれ変位の変化量
、速度の変化量
及び回転の変化量
を表す）を得る。
【0075】
事前積分因子は、慣性測定ユニットの現在の画像時刻での状態変数の事前積分量を取得するために慣性データを処理し、且つ事前積分量及び慣性測定ユニットの前の画像時刻における状態変数に基づき、状態変数の残差を算出し、事前積分因子とする方法によって算出されることができる。
【0076】
更に、スライディング時間ウィンドウ内の慣性測定ユニットの事前積分量の実際の観測値と推定値との間の誤差（即ち、残差）を算出することにより、事前積分因子
を得て、式（６）に示す通りである。
【0077】
【数6】
【0078】
そのうち、
は変位の変化量を表し、
は速度の変化量を表し、
は回転の変化量を表し、
は第ｋ時刻における世界座標系からＩＭＵ座標系までの回転行列を表し、
は第ｋ時刻におけるカメラ座標系から世界座標系までの並進ベクトルを表し、
は第ｋ＋１時刻におけるカメラ座標系から世界座標系までの並進ベクトルを表し、
は第ｋ時刻におけるカメラ座標系から世界座標系までの速度ベクトルを表し、
は第ｋ＋１時刻におけるカメラ座標系から世界座標系までの速度ベクトルを表し、
は第ｋ時刻におけるカメラ座標系から世界座標系までの回転角を表し、
は第ｋ＋１時刻におけるカメラ座標系から世界座標系までの回転角を表し、
は前の画像フレームから現在の慣性収集フレームまでの間の時間を表し、ｇは重力ベクトルを表し、
は第ｋ時刻における加速度計のバイアスを表し、
は第ｋ＋１時刻の加速度計のバイアスを表し、
は第ｋ時刻におけるジャイロのバイアスを表し、
は第ｋ＋１時刻におけるジャイロのバイアスを表す。
【0079】
ステップＳ０４において、視覚観測モデル制約及び運動モデル制約に基づいて各画像時刻に対応するカメラ、慣性測定ユニット又は車両の姿勢を算出する。
【0080】
本発明は、複数の制約に基づき、当該複数の制約条件を満たす時の最適化状態を算出し、当業者は必要に応じて制約条件に基づいて状態変数を求める算出方式を自ら選定することができ、本発明はこれを制限しない。例えば、目的最適化方法を採用し、好ましくは最小二乗法を採用して求めることができ、当該複数の制約条件を直接加算し、又は重み付け加算し、且つ最小二乗法で求めることができる。
【0081】
このステップでは、視覚観測モデル制約及び運動モデル制約に基づいて慣性測定ユニットの状態変数の式を構築し、式（７）に示す。
【0082】
【数7】
【0083】
そのうち、Ｘ＊は状態変数を表し、ａｒｇｍｉｎは事後確率が最小値を取る変数集合を表し、ｒｐ（Ｘ）は前の時刻に周辺化された事前項を表し、
は慣性測定ユニットの事前積分因子、即ち慣性測定ユニットの状態変数予測値の残差を表す。
は車線実線の投影因子、即ちスライディング時間ウィンドウ内の第ｋ時刻における視覚車線実線制約の残差を表し、当該実線投影因子のうちのＺｉはｉ番目の車線のサンプリング点（即ちサンプリングされた車線実線点におけるｉ番目のサンプル）の視覚観測を表し、当該Ｚｉは
である。
は、車線破線の投影因子、即ちスライディング時間ウィンドウ内の第ｋ時刻における視覚車線破線の端点制約の幾何学的残差を表し、当該破線投影因子のうちのＺｉはｉ番目の車線破線点（即ちサンプリングされた車線破線点におけるｉ番目のサンプル）の視覚観測を表し、当該Ｚｉは
と記されてもよい。
及び
はそれぞれ車線実線点及び破線点の観測共分散を表し、
は事前積分の共分散を表す。
【0084】
更に、慣性測定ユニットの状態変数を得るために、式を最小二乗法で解く。慣性測定ユニットの状態変数は、慣性測定ユニットの変位、速度、方向、ジャイロのバイアス及びジッタ因子の少なくとも１つを含む。
【0085】
当該状態変数は、更にカメラのジッタ因子、例えばカメラホルダのジッタ因子を含むことができる。第ｋ時刻について、状態変数は
を含み、それぞれＩＭＵの変位、速度、方向、加速度計及びジャイロのバイアス、ＩＭＵのジッタ、カメラホルダのジッタを表す。具体的には、ｋ時刻におけるＩＭＵの姿勢は
であり、
はカメラホルダとＩＭＵとの間のジッタを表し、
，
，
はそれぞれＩＭＵのバイアス及び速度を表す。カメラの姿勢は
であり、そのうち、
はＩＭＵからＥＮＵまでの姿勢を表し、
はカメラからＩＭＵまでの外部パラメータを表す。従って、ＩＭＵの状態変数及び所定の座標変換関係が既知であれば、ＩＭＵの姿勢、カメラの姿勢及び車両の姿勢を決定することができる。
【0086】
本発明において算出により求めた状態変数は、慣性測定ユニットのスライディング時間ウィンドウ内の各画像時刻（即ち各画像フレーム）における状態変数を含む。スライディング時間ウィンドウ内において、視覚観測モデル制約及び運動モデル制約に基づき、現在の画像時刻（即ち現在のフレームの画像）の状態変数を取得することができるだけでなく、スライディング時間ウィンドウ内の前の画像時刻（即ち前のフレームの画像）における状態変数を更新することもできる。
【0087】
一実施例によれば、図７を参照し、破線枠はスライドウィンドウを表し、１つのスライディング時間ウィンドウ内に複数の最適化される状態変数があり、各状態変数は１フレームの画像に対応し、状態変数の事前値が既知であり、各フレームの画像には最適化される状態変数があり、隣接する画像の間に運動モデル制約を構築するためのＩＭＵの事前積分量を有する。また、視覚観測モデル制約を構築するための、意味地図と画像との間の視覚特徴因子、及びセンサホルダのジッタ因子がある。一連のセンサ入力、履歴観測Ｚ及び状態Ｘｋが与えられ、上記の２つの観測モデル制約に基づき、ベイジアンネットワークモデルを構築することにより、状態変数の事後確率
を算出することができる。そのうち、ｋは第ｋフレームの画像を表し、ｌはｌ番目のセンサ（例えばカメラ）を表し、Ｌはセンサの総数を表し、
はそれぞれ状態変数の事前確率、時系列の運動モデルの条件確率、グローバル観測モデルの事後確率を表す。
【0088】
一般的には、各現在のフレームの画像の状態変数を算出した後、事前の知識によって取得された隣接するフレームの画像間の状態変数の事前積分量に基づき、次のフレームの画像の予測状態変数を算出することができる。例えば、現在のフレームの画像の状態変数と隣接するフレームの画像の状態変数の事前積分量を加算して、当該予測状態変数を得る。スライディング時間ウィンドウ内に合計でｍフレームの画像があり、当該次のフレームの画像が第ｍフレームの画像であると、前のｍ－１フレームの画像における既存の状態変数、及び当該第ｍフレームの画像の予測状態変数、並びに当該ｍフレームの画像に関連する視覚観測モデル制約及び運動モデル制約に基づき、協同最適化により、当該次のフレームの画像の最適化状態変数を得て、再び前のｍ－１フレームの画像の状態変数を更新することができる。
【0089】
更に、ステップＳ０４は、視覚観測モデル制約及び運動モデル制約に基づいて慣性測定ユニットの状態変数を得るステップと、慣性測定ユニットの状態変数に基づいて各フレームの画像時刻に対応するカメラ、慣性測定ユニット又は対象の姿勢を算出するステップと、を含んでもよい。具体的には、慣性測定ユニットの状態変数に基づいて慣性測定ユニットの姿勢を算出し、且つ慣性測定ユニットとカメラとの座標変換関係に基づいて位相姿勢を算出する。本願では、対象の状態変数に基づく姿勢の算出に対して一切制限しない。
【0090】
本発明の第１の実施例の姿勢推定方法では、グローバル視覚観測モデル制約と運動モデル制約の融合モデルを用いて姿勢を推定する。慣性測定ユニットが収集した慣性データは環境の変化による影響をほとんど受けないため、当該姿勢推定方法は環境が変化する時にも連続した姿勢推定結果を提供することができる。慣性測定ユニットの収集フレームレートがカメラの収集フレームレートより高いため、当該姿勢推定方法は姿勢推定のリアルタイム性を向上させることができる。対象の移動過程の複雑なシーンでは、トンネル及びビルの遮蔽により、全地球測位システムの位置情報が一時的に欠落する可能性があり、又は、障害物の遮蔽又は悪天候により、意味特徴が一時的に欠落する可能性がある。当該姿勢推定方法は、視覚観測モデル制約と運動モデル制約の融合モデルに基づき、複雑なシーンで連続的で確実な姿勢推定結果を取得することができるため、複雑なシーンでの姿勢推定の精度、リアルタイム性、安定性及びロバスト性を向上させることができる。
【0091】
図４は、本発明の第２の実施例による姿勢推定方法のフローチャートを示す。第１の実施例に比べ、第２の実施例による姿勢推定方法は、追加のローカル視覚観測モデル制約を更に含み、融合最適化の収束を更に加速することができる。
【0092】
ステップＳ１１において、車両の移動中にカメラが収集した複数のフレームの画像及び慣性測定ユニットが収集した慣性データを受信する。このステップは、図３に示すステップＳ０１と同様であるため、ここでは詳述しない。
【0093】
ステップＳ１２において、画像に基づいてグローバル視覚観測モデル制約を構築する。このステップは、図３に示すステップＳ０２と同様であるため、ここでは詳述しない。
【0094】
ステップＳ１３において、画像に基づいてローカル視覚観測モデル制約を構築する。
【0095】
このステップでは、カメラが収集した画像に対して前処理を行い、画像における意味特徴及び動的目標を抽出し、且つ画像における意味特徴に対してベクトル化を行う。
【0096】
ローカル視覚観測モデル制約において、意味特徴、幾何学的特徴、色特徴、テクスチャ特徴、空間関係特徴、形状特徴等を含むが、これらに限定されない任意の画像特徴を採用することができ、意味特徴は、車線及び標識のうちの少なくとも１つを含み、そのうち、車線は、破線車線及び実線車線のうちの少なくとも１つを含み、標識は、街灯柱、信号機、ゴミ箱、広告掲示板、電柱などを含むが、これらに限定されない。
【0097】
動的目標のマスクは、後続の意味地図のデータとの関連付け及び異常点／外点の除去に用いられる。例えば、意味地図で意味特徴点を選択した後、ある点の画像での投影点が動的目標に遮蔽された場合、対応する意味特徴点を除去する。
【0098】
更に、スライディング時間ウィンドウ内の所定数のフレームの画像間の特徴のレジストレーション誤差に基づいてローカル視覚観測モデル制約を構築することができ、当該所定数のフレームは、スライディング時間ウィンドウ内の各隣接する画像フレーム又は一部の間隔が隔てられた画像フレームを含んでもよく、本発明はこれを制限しない。例えば、スライディング時間ウィンドウ内の隣接するフレームのカメラ画像間の特徴のレジストレーション誤差を算出する。例えば、このステップでは、車線意味特徴のレジストレーション誤差を算出する。
【0099】
カメラ画像において、画像特徴をサンプリングして複数の画素点ｐを取得し、続いて、画素点ｐに対して座標変換を行い、第ｋ－１時刻のカメラ座標系から第ｋ時刻のカメラ座標系に変換した後にカメラ画像に投影（即ち、再投影）し、且つ画素点とカメラ画像における対応する画素点との距離を算出してレジストレーション誤差とすることで、ローカル観測の特徴投影因子
を得て、式（８）に示す通りである。
【0100】
【数8】
【0101】
そのうち、ｐｋｐｉはｉ番目の特徴点の第ｋ－１時刻のカメラ座標系における３Ｄ座標を表し、
及び
はそれぞれ第ｋ－１時刻のカメラ姿勢からｋ時刻のカメラ姿勢までの相対回転行列及び相対並進ベクトルを表し、ｐｉ＿ｕｖはカメラ画素におけるｉ番目の特徴点の画素座標を表し、関数ｐｒｏｊｅｃｔ（・）は特徴点をカメラ座標系から画像の画像平面に投影するためのピンホールカメラ投影モデルである。
【0102】
ステップＳ１４において、慣性データに基づいて運動モデル制約を構築する。このステップは、図３に示すステップＳ０３と同様であるため、ここでは詳述しない。
【0103】
ステップＳ１５において、視覚観測モデル制約及び運動モデル制約に基づいて各画像時刻に対応するカメラ、慣性測定ユニット又は車両の姿勢を算出する。
【0104】
このステップでは、視覚観測モデル制約及び運動モデル制約に基づいて慣性測定ユニットの状態変数の式を構築し、式（９）に示す通りである。
【0105】
【数9】
【0106】
そのうち、Ｘ＊は状態変数を表し、ａｒｇｍｉｎは事後確率が最小値を取る変数集合を表し、ｒｐ（Ｘ）は前の時刻に周辺化された事前項を表し、
は慣性測定ユニットの事前積分因子、即ち慣性測定ユニットの状態変数予測値の残差を表す。
はローカル観測の特徴投影因子、即ちスライディング時間ウィンドウ内の第ｋ時刻におけるローカル視覚観測モデル制約の残差を表し、当該特徴投影因子のうちのＺｉはｉ番目の特徴サンプリング点の視覚観測を表し、当該Ｚｉは
と記されてもよい。
はグローバル観測の車線実線の投影因子、即ちスライディング時間ウィンドウ内の第ｋ時刻における視覚車線実線制約の残差を表し、当該実線投影因子のうちのＺｉはｉ番目の車線のサンプリング点の視覚観測を表す。
はグローバル観測の車線破線の投影因子、即ちスライディング時間ウィンドウ内のｋ時刻の視覚車線破線の端点制約の幾何学的残差を表し、当該破線の投影因子のうちのＺｉはｉ番目の車線破線点の視覚観測を表し、
は連続フレームの特徴点制約の観測共分散を表す。
及び
はそれぞれ車線実線点及び破線点の観測共分散を表し、
は事前積分の共分散を表す。
【0107】
更に、慣性測定ユニットの状態変数を得るために、式を最小二乗法で解く。慣性測定ユニットの状態変数は、慣性測定ユニットの変位、速度、方向、ジャイロのバイアス、ジッタ因子及びカメラのジッタ因子（例えば、カメラホルダのジッタ因子）の少なくとも１つを含む。スライディング時間ウィンドウ内で、現在の画像フレームの状態変数を取得することができるだけでなく、前の画像フレームの状態変数を更新することもできる。
【0108】
更に、慣性測定ユニットの状態変数に基づいて慣性測定ユニットの姿勢を算出し、且つ慣性測定ユニットとカメラとの座標変換関係に基づいて位相姿勢を算出する。本願では、対象の状態変数に基づく姿勢の算出に対して一切制限しない。
【0109】
本発明の第２の実施例の姿勢推定方法では、グローバル視覚観測モデル制約とローカル視覚観測モデルと運動モデル制約の融合モデルを用いて姿勢を推定する。当該姿勢推定方法は、視覚観測モデル制約と運動モデル制約の融合モデルに基づき、複雑なシーンで連続的で確実な姿勢推定結果を取得することができ、追加のローカル視覚観測モデル制約を用いて融合最適化の収束を更に加速することができるため、複雑なシーンでの姿勢推定の精度、リアルタイム性、安定性及びロバスト性を向上させることができる。
【0110】
図５は、本発明の第３の実施例による姿勢推定方法のフローチャートを示す。第１の実施例に比べ、第３の実施例による姿勢推定方法は、追加の状態変数初期化ステップを更に含み、融合最適化の収束を更に加速することができる。
【0111】
ステップＳ２１において、車両の移動中にカメラが収集した複数のフレームの画像及び慣性測定ユニットが収集した慣性データを受信する。このステップは、図３に示すステップＳ０１と同様であるため、ここでは詳述しない。
【0112】
ステップＳ２２において、純視覚的なカメラ姿勢予測から慣性測定ユニットの状態変数を初期化する。
【0113】
このステップは、図６に示すように、複数のサブステップＳ２２１～Ｓ２２４を含む。
【0114】
ステップＳ２２１において、全地球測位システムの位置情報に基づいて現在の地図を決定する。
【0115】
ステップＳ２２２において、地図及び画像における特徴のレジストレーション制約に基づき、画像フレームに対応するカメラ姿勢を算出する。このステップは、例えば、グローバル視覚観測モデル制約の式を構築し、且つ式を最小二乗法で解いてカメラ姿勢の予測値を得るステップを含む。
【0116】
ステップＳ２２３において、連続フレームの累積カメラ姿勢の数が所定数より大きいか否かを判定する。累積カメラ姿勢の数が所定数より大きいと、ステップＳ２２４を引き続き実行する。累積カメラ姿勢の数が所定数以下であると、ステップＳ２２２に戻って次の画像フレームのカメラ姿勢を取得する。当該所定数は、スライディング時間ウィンドウのフレーム数であってもよく、例えば４フレームであり、これに限定されないことが当然である。即ち、１つのスライドウィンドウ分のカメラ姿勢を累積した場合、状態変数の初期化を行うことができる。
【0117】
ステップＳ２２４において、取得された画像フレームが所定数以上であることに応答し、カメラ姿勢に基づいて慣性測定ユニットの状態変数を算出する。例えば、このステップは、慣性測定ユニットの状態変数の初期化値を算出するステップを含み、具体的には、複数の画像フレームのカメラ姿勢に基づいて慣性測定ユニットの初期ジャイロバイアス、初期加速度、初期速度のうちの少なくとも１つを算出するステップを含む。以上から分かるように、本発明は、各画像時刻のカメラ姿勢を得るために、収集された画像が所定数より小さくなるまでに、当該連続フレームの画像に対して純視覚的なカメラ姿勢予測を行う。現在のフレーム及び後続のフレームに対して慣性測定ユニットとカメラに基づいて融合するカメラ姿勢予測を行うために、収集された画像が所定数に達する場合、慣性測定ユニットの状態変数の初期化値を算出する。慣性測定ユニットの初期ジャイロバイアスを算出する時、スライディング時間ウィンドウ内の隣接する画像フレーム間の相対姿勢を算出し、相対姿勢の回転変化量及び慣性データの隣接する画像フレーム間における角速度増分に基づいて初期ジャイロバイアスを得る。具体的には、スライディングウィンドウ内に累積されたカメラの履歴姿勢の軌跡
，ｉ＝ｋ－ｍ＋１，ｋ－ｍ＋２，．．．， ｋ－１，ｋが既知であると、隣接するカメラフレーム間の相対姿勢
を算出することができる。
の回転変化量
及び隣接するカメラフレーム間のＩＭＵ事前積分の角速度増分
の一致性制約を利用して、連続する複数のフレームの制約を合算し、最適化問題を構築し、ジャイロのバイアスｂｇを求める。
【0118】
慣性測定ユニットの初期加速度を算出する時、スライディング時間ウィンドウ内の複数のフレームの画像のカメラ姿勢軌跡及び慣性データの隣接する画像フレーム間における変位増分に基づいて初期加速度を得る。具体的には、スライドウィンドウに累積されたカメラ姿勢の履歴軌跡と事前積分の変位増分
のスケール一致性を利用して制約を構築し、重力加速度ｇを求める。
【0119】
慣性測定ユニットの初期速度を算出する時、初期ジャイロバイアス及び初期加速度に基づいて初期速度を得る。具体的には、ジャイロのバイアス及び初期重力を得た後、下記の式（１０）により初期速度を算出することができ、ｉ＝ｋはスライドウィンドウの最後のフレーム（即ち現在のフレーム）を表す。
【0120】
【数10】
【0121】
そのうち、ｖｉ－１及びｖｉはそれぞれ第ｉ－１フレーム及び第ｉフレームの速度を表し、Ｒｉ及びＲｉ－１はそれぞれ第ｉフレームの回転行列であり、
及び
はそれぞれ第ｉフレームと第ｉ－１フレームの画像のジャイロの元の観測量である。当業者であれば理解されるように、本発明において、ｉは観測サンプルの順序を表し、観測サンプルが画像である場合、ｉは第ｉフレームを表し、観測サンプルが画像における特徴点である場合、ｉはｉ番目の特徴点を表し、例えば画像の実線のｉ番目のサンプリング点、破線のｉ番目のサンプリング点などである。
【0122】
ステップＳ２３において、画像に基づいてグローバル視覚観測モデル制約を構築する。このステップは、図３に示すステップＳ０２と同様であるため、ここでは詳述しない。
【0123】
ステップＳ２４において、慣性データに基づいて運動モデル制約を構築する。このステップは、図３に示すステップＳ０３と同様であるため、ここでは詳述しない。
【0124】
ステップＳ２５において、視覚観測モデル制約及び運動モデル制約に基づいて各フレームの画像時刻に対応するカメラ、慣性測定ユニット又は車両の姿勢を算出する。このステップは、図３に示すステップＳ０４と同様であるため、ここでは詳述しない。
【0125】
本発明の第３の実施例による姿勢推定方法では、純視覚的なカメラ姿勢予測に基づいて慣性測定ユニットの状態変数を初期化し、続いてグローバル視覚観測モデル制約と運動モデル制約の融合モデルを用いて姿勢を推定する。慣性測定ユニットの特性により、慣性測定ユニットの複数の状態変数が結合される。この実施例では、初期化段階で慣性測定ユニットが収集した慣性データを処理する必要がなく、複数のフレームの画像のカメラ姿勢を累積してから、カメラ姿勢に基づいて慣性測定ユニットの状態変数を算出する。慣性測定ユニットの状態変数に対する初期化は、高速デカップリングを実現することができ、融合最適化の収束を更に加速することができる。初期化段階は純視覚的なカメラ姿勢予測に依存し、複雑なシーンで環境変化の影響を受けやすいため、複数のフレームの画像のカメラ姿勢を累積してから初期化する必要がある。初期化後の融合最適化段階では、複雑なシーンで連続的で確実な姿勢推定結果を取得することができるため、複雑なシーンでの姿勢推定の精度、リアルタイム性、安定性及びロバスト性を向上させることができる。
【0126】
【0127】
姿勢推定装置１０は、例えば、モバイルデバイスの内部システムとして、上記実施例の姿勢推定方法を実現するために用いられる。モバイルデバイスには、更に、カメラ１１１、慣性測定ユニット１１２及び全地球測位システム１１３のような複数のセンサが固定的に取り付けられている。
【0128】
姿勢推定装置１０は、データ受信モジュール１１と、第１の構築モジュール１２と、第２の構築モジュール１３と、姿勢算出モジュール１４とを含む。データ受信モジュール１１は、複数のセンサに接続され、移動中にカメラ１１１が収集した複数のフレームの画像及び慣性測定ユニット１１２が収集した慣性データを受信するために用いられる。第１の構築モジュール１２は、データ受信モジュール１１に接続され、前記画像に基づいて視覚観測モデル制約を構築するために用いられる。例えば、視覚観測モデル制約は、グローバル観測の車線実線の投影因子、グローバル観測の車線破線の投影因子、及びローカル観測の特徴投影因子のうちの少なくとも１つを含む。第２の構築モジュール１３は、データ受信モジュール１１に接続され、慣性データに基づいて運動モデル制約を構築するために用いられる。例えば、運動モデル制約は、慣性測定ユニットの事前積分因子、即ち慣性測定ユニットの状態変数予測値の残差を含む。姿勢算出モジュール１４は、第１の構築モジュール１２及び第２の構築モジュール１３に接続され、視覚観測モデル制約及び運動モデル制約に基づいて慣性測定ユニットの状態変数の式を構築し、且つ式を最小二乗法で解いて慣性測定ユニットの状態変数を得て、更に、慣性測定ユニットの状態変数に基づいて各フレームの画像時刻に対応するカメラ、慣性測定ユニット又はモバイルデバイスの姿勢を取得するために用いられる。
【0129】
【0130】
コンピューティングデバイス２０は、バスによって接続されたプロセッサ２１と、メモリ２２と、入出力デバイス２３とを含む。メモリ２２は、リードオンリーメモリ（ＲＯＭ）及びランダムアクセスメモリ（ＲＡＭ）を含み、メモリ２２内にシステム機能を実行するために必要な各種のコンピュータ命令及びデータが記憶され、プロセッサ２１は、メモリ２２から各種のコンピュータ命令を読み出して各種の適切な動作及び処理を実行する。入出力コンピューティングデバイス２３は、キーボード、マウスなどの入力部と、陰極線管（ＣＲＴ）、液晶ディスプレイ（ＬＣＤ）、スピーカ等を含む出力部と、ハードディスク等を含む記憶部と、ＬＡＮカード、モデム等のネットワークインタフェースカードを含む通信部とを備える。メモリ２２には、本開示の実施例における姿勢推定方法に係る動作を実現するためのコンピュータ命令も記憶されている。
【0131】
それに応じて、本開示の実施例は、メモリと、前記メモリに通信可能に接続された１つ又は複数のプロセッサとを含むコンピューティングデバイスであって、前記メモリには、前記１つ又は複数のプロセッサにより実行可能な命令が記憶されており、前記命令が前記１つ又は複数のプロセッサにより実行されることで、前記１つ又は複数のプロセッサに上記姿勢推定方法を実現させるコンピューティングデバイスを提供する。
【0132】
それに応じて、本開示の実施例は、コンピュータプログラムが記憶されているコンピュータ読み取り可能な記憶媒体であって、前記コンピュータプログラムがプロセッサにより実行される時に上記姿勢推定方法を実現するコンピュータ読み取り可能な記憶媒体を提供する。
【0133】
以上、具体的な実施例を参照して本発明の基本的な原理を説明したが、指摘すべきものとして、当業者であれば、本発明の方法及び装置の全て又は任意のステップ又は部材は、任意のコンピューティングデバイス（プロセッサ、記憶媒体等を含む）又はコンピューティングデバイスのネットワークにおいて、ハードウェア、ファームウエア、ソフトウェア又はそれらの組み合わせで実現することができると理解することができ、これは、当業者が本発明の説明を読んだ上でそれらの基本的なプログラミング技術を用いて実現できるものである。
【0134】
当業者であれば、上記実施例の方法に伴われる全て又は一部のステップは、プログラムによって関連するハードウェアを命令することで達成可能であると理解することができ、前記プログラムは、コンピュータ読み取り可能な記憶媒体に記憶されてもよく、当該プログラムを実行する時、方法の実施例のステップの１つ又はその組み合わせを含む。
【0135】
なお、本願の各実施例における各機能ユニットは、１つの処理モジュールに集積されてもよいし、各ユニットが単独で物理的に存在してもよいし、２つ又は２つ以上のユニットが１つのモジュールに集積されてもよい。上記集積されたモジュールは、ハードウェアの形態で実現されてもよいし、ソフトウェア機能モジュールの形態で実現されてもよい。前記集積されたモジュールは、ソフトウェア機能モジュールの形態で実現され、且つ独立した製品として販売又は使用される場合、コンピュータ読み取り可能な記憶媒体に記憶されてもよい。
【0136】
当業者は、本発明の実施例が方法、システム、又はコンピュータプログラム製品として提供可能であると理解すべきである。従って、本発明は、完全なハードウェアの実施例、完全なソフトウェアの実施例、又はソフトウェアとハードウェアとを組み合わせた実施例の形を用いることができる。そして、本発明は、コンピュータ利用可能なプログラムコードが含まれる１つ又は複数のコンピュータ利用可能な記憶媒体（ディスクメモリ、光メモリ等を含むが、これらに限定されない）で実施されるコンピュータプログラム製品の形を用いることができる。
【0137】
本発明は、本発明の実施例による方法、デバイス（システム）、及びコンピュータプログラム製品のフローチャート及び／又はブロック図を参照しながら、説明された。コンピュータプログラム命令により、フローチャート及び／又はブロック図におけるそれぞれのフロー及び／又はブロック、及び、フローチャート及び／又はブロック図におけるフロー及び／又はブロックの組み合わせを実現することができると理解すべきである。１つの機械が生成されるように、汎用コンピュータ、専用コンピュータ、組み込みプロセッサ又はその他のプログラマブルデータ処理機器のプロセッサにこれらのコンピュータプログラム命令を提供することができ、それで、コンピュータ又はその他のプログラマブルデータ処理機器のプロセッサにより実行される命令によって、フローチャートの１つのフロー又は複数のフロー及び／又はブロック図の１つのブロック又は複数のブロックで指定された機能を実現するための装置が生成される。
【0138】
これらのコンピュータプログラム命令は、コンピュータ又はその他のプログラマブルデータ処理機器が特定の方式で作動するようにガイドできるコンピュータ読み取り可能なメモリに記憶されてもよく、それで、当該コンピュータ読み取り可能なメモリに記憶された命令によって、フローチャートの１つのフロー又は複数のフロー及び／又はブロック図の１つのブロック又は複数のブロックで指定された機能を実現する命令装置を含む製品が生成される。
【0139】
これらのコンピュータプログラム命令は、コンピュータにより実現される処理を発生させるために、コンピュータ又はその他のプログラマブル機器で一連の操作ステップが実行されるように、コンピュータ又はその他のプログラマブルデータ処理機器にロードされてもよく、それで、コンピュータ又はその他のプログラマブル機器で実行される命令によって、フローチャートの１つのフロー又は複数のフロー及び／又はブロック図の１つのブロック又は複数のブロックで指定された機能を実現するためのステップが提供される。
【0140】
本発明の上記実施例について説明したが、当業者が創造的な基本コンセプトを理解した上で、これらの実施例に対してその他の変更や修正を実施することができる。従って、添付される請求項が上記実施例及び本発明の範囲内である全ての変更及び修正を含むと理解すべきである。
【0141】
勿論、当業者は、本発明の精神及び範囲から逸脱することなく、本発明に対して様々な修正や変更を行うことができる。このように、本発明のこれらの修正や変更が本発明の請求項及びその同等な技術の範囲内にある場合、本発明は、これらの修正や変更を含むことも意図している。
【図1】
【図2】
【図3】
【図4】
【図5】
【図6】
【図7】
【図8】
【図9】