JP7358108B2 - 情報処理装置、情報処理方法及びプログラム - Google Patents

Description

本発明は、移動体の自己位置姿勢推定に関する。

工場や物流倉庫で利用される無人搬送車（移動体（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｅｈｉｃｌｅ））などの車輪ロボットは、床や道路などの平面上を走行することが一般的である。このような移動体や車輪ロボット（これらを総称して移動体と呼ぶ）の制御に必要な位置姿勢の計測には２Ｄのレーザスキャナ（２Ｄ ＬｉＤＡＲ（Ｌｉｇｈｔ Ｄｅｔｅｃｔｉｏｎ ａｎｄ Ｒａｎｇｉｎｇ））が利用されることが多い。２Ｄ ＬｉＤＡＲは走行時に床面に平行となるように移動体上に設置され、移動体が走行する平面（床面）上の位置（２自由度）と姿勢（１自由度）の計３自由度の位置姿勢が計測される。また、３自由度の地図情報に移動体の目標位置を設定することによって、移動体を目標位置に向かって移動させることができる。

特開２０１５－１４１５８０号公報

特許文献１では、所定の面における移動体と環境に存在する物体との距離計測を行う距離センサによる計測結果を使って移動体の位置推定を行う。更に、環境内に設置されたランドマークを撮影可能なエリアに移動体がいる場合は、ステレオカメラで撮像した画像からランドマークを認識して移動体の位置推定も行う。しかしながら、特許文献１では、坂道等で移動体が傾く場合に、距離センサによる位置推定に移動体の傾きが考慮されないため、移動体の位置及び姿勢を間違って推定する可能性がある。本発明は上記課題に鑑みてなされたものであり、移動体の位置及び姿勢を安定的に得ることを目的とする。

上記課題を解決する本発明にかかる情報処理装置は、撮像装置を搭載した移動体の位置及び姿勢を推定する情報処理装置であって、前記撮像装置の６自由度の位置及び姿勢を示す第１情報を推定する６自由度推定手段と、前記移動体が走行する位置での高さ方向の値から得られる、前記移動体と基準となる面との成す傾きが所定の値より大きい場合は、前記第１情報に基づいて前記移動体の３自由度の位置及び姿勢を決定し、前記移動体と基準となる面との成す傾きが所定の値より小さい場合は、前記移動体の３自由度の位置及び姿勢を示す第２情報に基づいて前記移動体の３自由度の位置及び姿勢を決定する決定手段と、有することを特徴とする。

本発明により、移動体の位置及び姿勢を安定的に取得できる。

移動体の傾きを説明する概念図 移動体の移動制御の様子を説明する図 情報処理装置のハードウェア構成例を示す図 情報処理装置の機能構成例を示すブロック図 情報処理装置の機能構成例を示すブロック図 情報処理装置が実行する処理を説明するフローチャート 情報処理装置が実行する処理を説明するフローチャート 情報処理装置の機能構成例を示すブロック図 情報処理装置が実行する処理を説明するフローチャート 情報処理装置の機能構成例を示すブロック図 情報処理装置が実行する処理を説明するフローチャート

以下、図面を参照しながら実施形態を説明する。なお、以下の実施形態において示す構成は一例に過ぎず、本発明は図示された構成に限定されるものではない。

＜実施形態１＞
本実施形態では、移動体の移動制御に本発明を適用した場合について説明する。工場や倉庫で荷物を運搬するロボット等の一般的な移動体は、画像センサや距離センサを用いて自己位置姿勢推定をしながら自動運転する。一般的な移動体は水平な面を走行することを想定されているが、走行環境に存在する坂道や段差、また自身に搭載された荷物によって、予め地図情報等で設定された基準となる水平面に対して移動体自身が傾いてしまうことがある。このような場合、平面を走査する２次元の距離センサの計測値から推定された自己位置姿勢推定の結果は、地図情報における絶対位置とズレて推定される可能性がある。

本実施形態では、移動体は、部分的に坂道が存在する平面（床面）を走行するものとし、移動体に搭載された画像センサ（カメラ）が撮影する画像情報をもとに推定される６自由度の位置姿勢をもとに移動体の走行距離の算出を行う。図１は、移動体１が基準となる面Ｓ１に対して角度Θだけ傾いている面Ｓ２を走行する状況を模した図である。このとき、移動体が点Ｐ１から特定の時間ｔに進んだ正しい走行距離を点Ｐ１と点Ｐ２を結んだＤ１であるとする。点Ｐ１と水平面上の点Ｐ２’を結んだ距離Ｄ２は３自由度の（２次元を走査する距離センサによる）位置姿勢計測結果に基づいて算出される走行距離である。３自由度の位置及び姿勢の推定においては、移動体１は基準となる面Ｓ１を走行するという前提で位置姿勢が推定されるためＤ１とＤ２は一致しない。Ｄ１とＤ２が一致しないと、例えば、ＡＧＶのバッテリー充電やメンテナンス、部品交換のタイミングを走行距離に基づいて管理する際に、最適なタイミングでメンテナンスを行えない。

本実施形態では、Ｄ１とＤ２の差が大きくならないように、移動体が斜面を走行する場合は、６自由度の位置（Ｘ、Ｙ、Ｚ）及び姿勢（Ｒｏｌｌ、Ｐｉｔｃｈ、Ｙａｗ）の計測結果を変換することによって移動体の移動制御に必要な３自由度の移動体の位置及び姿勢を推定する。これによって、移動体の位置及び姿勢を取得できる。

（構成説明）図２は、本実施形態における移動体の移動制御の様子を説明する図である。本実施形態においては、工場や倉庫内の物流を担うＡＧＶ（Ａｕｔｏｍａｔｅｄ Ｇｕｉｄｅｄ Ｖｉｈｅｃｌｅ）の移動制御を具体例として説明する。移動体は基本的に人間の操作を介することなく一定の指示を与えることで自己位置を自律的に移動可能な可動式のロボットであればよい。

移動体１に搭載された情報処理装置１１は、例えばステレオカメラで移動体の周囲を撮像した画像に基づいて移動体の位置及び姿勢を推定し、その推定結果に基づいて移動体を移動制御する。情報処理装置１１は、画像センサ１２、距離センサ１３、接触センサ１４、アクチュエータ１５、オドメーター１７、記憶部１８に接続されている。また移動体１は上位管理システム１６との無線通信を行う。

図３は、情報処理装置のハードウェア構成例を示す図である。中央処理ユニット（ＣＰＵ）１０１は、ＲＡＭ１０３をワークメモリとして、ＲＯＭ１０２や記憶装置１０４に格納されたＯＳやその他プログラムを読みだして実行し、システムバス１０９に接続された各構成を制御して、各種処理の演算や論理判断などを行う。ＣＰＵ１０１が実行する処理には、実施形態の情報処理が含まれる。記憶装置１０４は、ハードディスクドライブや外部記憶装置などであり、実施形態の情報処理にかかるプログラムや各種データを記憶する。入力部１０５は、カメラなどの撮像装置、ユーザ指示を入力するためのボタン、キーボード、タッチパネルなどの入力デバイスである。なお、記憶装置１０４は例えばＳＡＴＡなどのインタフェイスを介して、入力部１０５は例えばＵＳＢなどのシリアルバスを介して、それぞれシステムバス１０９に接続されるが、それらの詳細は省略する。通信Ｉ／Ｆ１０６は無線通信で外部の機器と通信を行う。表示部１０７はディスプレイである。センサ１０８は画像センサや距離センサである。

画像センサ１２は具体的にはカメラである。画像センサ１２は、移動体の周囲を撮像する撮像装置である。画像センサによって移動体の周囲の環境を撮像し、撮像された計測結果を画像情報として画像情報入力部１１１に出力する。本実施形態における画像センサは、水平・垂直方向の画角を有するモノクロカメラ２個から構成されるステレオカメラであるとする。ステレオカメラは、光軸が水平面よりやや上方を向くように移動体に取り付ける。ステレオカメラを構成する２台のカメラの内部パラメータ（焦点距離、画像中心、歪みパラメータ）及びカメラ間の外部パラメータ（相対的な位置姿勢）は公知の方法により予め校正されているものとする。本実施形態では、カメラの光学中心を原点、光軸方向をＺ軸、画像の水平方向をＸ軸、画像の垂直方向をＹ軸とする三次元の座標系をカメラ座標系と規定する。ステレオカメラのカメラ座標系は、左側カメラ（または右側カメラ）のカメラ座標系であるとする。画像センサ１２の移動体における取付け位置は設計値により既知であるとする。なお、画像センサの代わりに６自由度の距離計測が可能な距離センサを用いてもよい。具体的には、３Ｄ－ＬｉＤＡＲ等でもよい。

距離センサ１３はセンサからのシーンの距離情報を計測する装置である。本実施形態における距離センサは２Ｄ ＬｉＤＡＲである。２Ｄ ＬｉＤＡＲは、レーザ光を平面上で走査させることによって距離情報を取得する距離センサである。距離センサ１３は、レーザ光を走査させる平面が略水平面となるように移動体に取り付ける。距離センサ１３の移動体における取付け位置は設計値により既知であるとする。計測された距離情報は情報処理装置１１の距離情報入力部１１８に出力される。なお、距離センサの代わりに、画像センサで撮像された画像から、２次元の絶対位置を示すマーカを認識する構成でもよい。

接触センサ１４は安全センサである。接触センサが移動体と他の物体との接触を検知すると移動体は緊急停止するよう制御部１１５に情報を出力する。

アクチュエータ１５は、情報処理装置１１からの指示や障害物センサ、安全センサの出力をもとにモータを制御して移動体を移動または静止させる。

上位管理システム１６は、複数の移動体の運行管理を行うシステムである。上位管理システム１６は、各移動体の情報処理装置１１に目標位置姿勢を送信する。本実施形態では、移動体の目標位置姿勢は平面上における３自由度の位置姿勢であるとする。上位管理システム１６と情報処理装置１１の間の通信は、ＷｉｆｉやＢｌｕｅＴｏｏｔｔｈ、５Ｇなどの無線通信により行う。

オドメーター１７は、例えば、移動体の車輪の回転量から走行距離を計測する。

記憶部１８は、移動体が走行する環境についての地図情報を記憶する。他、移動体の走行距離や移動制御に必要な情報を保持する。なお、図４のように記憶部１８は移動体上に搭載されている形以外でもよく、上位管理システム１６の内部にあってもよい。

（情報処理装置１１の詳細）
図４に、本実施形態における情報処理装置の機能構成例を示す図を示す。情報処理装置１１は、画像情報入力部１１１、６自由度推定部１１２、変換部１１３、制御部１１５、地図情報取得部１１６、平面情報取得部１１７、から構成される。情報処理装置１１は、さらに距離情報入力部１１８、３自由度推定部１１９、決定部１２０、第２地図情報取得部１２１、計測部１２２から構成される。また、移動体１には、計測部１２２、制御部１１５、距離情報入力部１１８、３自由度推定部１１９、決定部１２０、第２地図情報取得部１２１を有する。情報処理装置１１は、例えばＰＣ、組み込みシステム、ＰＬＣ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｃｏｎｔｒｏｌｌｅｒ）である。

画像情報入力部１１１は、画像センサ１２から画像情報を入力し、入力された画像情報を６自由度推定部１１２に送る。本実施形態における画像センサ１２はステレオカメラであるため、画像情報はステレオ画像（左右のカメラが撮影する２枚の画像）である。画像センサ１２は、移動体が走行する工場や物流倉庫などの画像情報を撮像する。よって、本実施形態における画像情報とは、移動体に搭載された撮像装置によって移動体の走行する環境を撮像したステレオ画像を指す。

６自由度推定部１１２は、画像情報入力部１１１から入力された画像情報（ステレオ画像）をシーンの地図情報と照合することにより３次元空間中におけるカメラの６自由度の位置姿勢（位置３自由度、姿勢３自由度）を推定する。６自由度の位置姿勢は、前述の移動体が走行する環境を示す地図情報において規定される３次元の座標系（以下、世界座標系）を基準とした位置姿勢（第１情報）である。本実施形態では、Ｍｕｒ－Ａｒｔａｌらの手法によりシーンの地図情報の生成及びカメラの６自由度の位置姿勢推定を行う。（Ｒ．Ｍｕｒ－Ａｒｔａｌ 他，“ＯＲＢ－ＳＬＡＭ２：Ａｎ Ｏｐｅｎ－Ｓｏｕｒｃｅ ＳＬＡＭ Ｓｙｓｔｅｍ ｆｏｒ Ｍｏｎｏｃｕｌａｒ，Ｓｔｅｒｅｏ，ａｎｄ ＲＧＢ－Ｄ Ｃａｍｅｒａｓ，２０１７．）。Ｍｕｒ－Ａｒｔａｌらの手法は画像上で検出される特徴点を利用する手法である。シーンの地図情報として、移動体が走行する工場や物流倉庫といった移動体が走行するシーンにおける３次元点群の情報を保持する。具体的には、シーンにおける３次元点群の情報は特徴点の世界座標系における３次元座標のことである。本実施形態では、移動体を走行させながら撮影した映像をもとに、前述のＭｕｒ－Ａｒｔａｌらの手法によりあらかじめシーンの地図情報を生成しておく。シーンの地図情報は、記憶部１８に保存されているものとする。Ｍｕｒ－Ａｒｔａｌらの手法は、画像上の特徴点の位置と、推定する位置姿勢を用いて地図上の特徴点を画像上に投影した時の位置の差（再投影誤差）を最小化することによりカメラの位置姿勢を推定する。６自由度推定部は、撮像装置によって撮像された移動体の周囲を写した画像を入力し、移動体の３自由度の位置及び姿勢を出力する学習済みモデルに基づいて移動体の３自由度の位置及び姿勢を推定してもよい。

変換部１１３は、６自由度推定部１１２が推定した６自由度のカメラの位置姿勢を、移動体の制御が可能なように３自由度の位置姿勢に変換する。すなわち、変換部１１３は、地図情報において規定される３次元の座標系（以下、世界座標系）を基準とした位置姿勢である第１情報を用いて、３自由度の位置及び姿勢を示す第３情報を出力する。移動体の傾きは、例えば、６自由度の位置及び姿勢の推定結果の高さ方向の値（Ｚ）で判断してもよい。例えば、Ｚ＝０の場合は、移動体は基準となる面を走行していると考え、３自由度で計測する距離センサの計測結果を用いて移動体を制御する。一方で、Ｚ＞０（またはＺ＜０）の場合は、斜面を登っている（或いは下っている）と考えられるため、６自由度の位置及び姿勢を３自由度に変換した結果を用いて制御を行うようにする。前述の通り、３自由度の位置姿勢は移動体が走行する平面における位置姿勢である。３自由度のうち２自由度は平面上の２次元位置、残りの１自由度は平面の法線まわりの姿勢（平面上の向き）を表わす。移動体が走行する平面の推定方法については後述する。移動体が走行する平面の情報は平面情報取得部１１７に保存されているものとする。

制御部１１５は、決定部１２０が決定した移動体の位置及び姿勢に基づいて移動体を制御する。第２情報または変換部１１３が変換した３自由度の位置及び姿勢を示す第３情報をもとに移動体の制御を行う。具体的には、制御部１１５は、あらかじめ設定した移動体の目標位置姿勢（３自由度）と、変換部１１２が出力する３自由度の位置姿勢の差をもとに経路計算を行い、計算された経路に従うようにアクチュエータ１５にモータの回転速度などの指示を送る。移動体の位置姿勢が目標位置姿勢と略一致した場合には移動体を停止し、上位管理システム１６から次の目標位置姿勢が送られてくるのを待つ。なお、経路計算はＡ＊アルゴリズムやＲＲＴ（Ｒａｐｉｄｌｙ ｅｘｐｌｏｒｉｎｇ ｒａｎｄｏｍ ｔｒｅｅ）といったアルゴリズムを用いてもよいし、Ｑ学習やＤＱＮなどの機械学習による方法を用いてもよい。

地図情報取得部１１６は、記憶部１８より移動体が走行する環境の３自由度の地図情報を取得する。また、予め設定された移動体の３自由度の目標位置姿勢を取得する。

平面情報取得部１１７は、移動体を運用時と同じ基準となる平面上を走行させることにより平面推定用のデータの取得を行う。平面推定用のデータは、移動体が走行した時の複数（Ｎ点）の位置における位置姿勢データである。位置姿勢データは、６自由度位置姿勢推定によって推定される画像センサ１１２の６自由度位置姿勢である。基準面よりΘだけ傾いた傾斜面を走行する場合でも、３自由度の位置及び姿勢を正しく推定できるように、６自由度の位置及び姿勢を撮像装置によって撮像された画像を基に推定する。

計測部１２２は、オドメーター１７によって計測された移動体の車輪の回転量に基づいて、移動体の走行距離を計測する。計測部１２２の計測結果に基づいて、決定部１２０におけて移動体の位置姿勢を決定する。

距離情報入力部１１８は、距離センサ１３から移動体とその周囲にある物体との距離を計測した距離情報を入力し、入力された距離情報を３自由度推定部２１９に送る。前述したように、本実施形態における距離センサは２Ｄ ＬｉＤＡＲである。２Ｄ ＬｉＤＡＲは、レーザ光を走査させる平面が略水平面となるように移動体に取り付けられる。これにより距離センサ１３は、距離センサ１３が取り付けられた高さの水平面における距離情報を取得する。

３自由度推定部１１９は、移動体が走行する環境を示す２次元の地図情報と、移動体が走行する環境における物体と該移動体との距離を計測した距離情報とに基づいて移動体に搭載された距離センサの３自由度の位置及び姿勢を示す第２情報を推定する。すなわち、距離情報入力部１１８から入力された距離情報をシーンの地図情報と照合することにより距離センサ１３の位置姿勢を推定する。本実施形態では、３自由度推定部１１９は、距離センサ１３の平面上における３自由度の位置姿勢（位置２自由度、姿勢１自由度）を推定する。移動体の移動制御に必要な２次元の地図情報は事前に生成され第２地図情報取得部１２１によって予め取得されているものとする。３自由度の位置姿勢は、前述の地図情報において規定される平面（２次元）の世界座標系（以下、２次元世界座標系）を基準とした位置姿勢である。距離情報に基づく地図情報の生成や位置姿勢の推定は、パーティクルフィルタを利用するＧｒｉｓｅｔｔｉらの手法により行う。（Ｇ．Ｇｒｉｓｅｔｔｉ，他，“Ｉｍｐｒｏｖｅｄ Ｔｅｃｈｎｉｑｕｅｓ ｆｏｒ Ｇｒｉｄ Ｍａｐｐｉｎｇ ｗｉｔｈ Ｒａｏ－Ｂｌａｃｋｗｅｌｌｉｚｅｄ Ｐａｒｔｉｃｌｅ Ｆｉｌｔｅｒｓ，”２００７．）。距離センサの位置姿勢は、移動体に対する距離センサの既知の取付け位置をもとに３自由度推定部または決定部において移動体の位置姿勢に変換する。３自由度の位置姿勢の推定は、６自由度から３自由度に変換する処理が不要なため、６自由度の位置姿勢推定よりも処理負荷が小さいといったメリットがある。そのため、移動体が平面を走行する際は３自由度の位置姿勢を行い、なるべく省電力で移動体を制御できる。なお、ここでは距離情報から位置及び姿勢を推定する方法を述べるが３自由度の位置及び姿勢を推定できればどのような情報を用いても良い。例えば、地図における２次元座標を示すマーカを環境内に設置し、マーカを画像認識することで移動体の位置を特定する方法でも良い。

決定部１２０は、移動体の傾きに応じて、第２情報および第３情報の少なくとも１つを選択的に用いることにより、移動体の位置及び姿勢を決定する。つまり、移動体と基準となる面との成す傾きが所定の値より大きい場合は、第１情報に基づいた第３の情報を移動体の３自由度の位置及び姿勢として決定する。決定部１２０は、移動体と基準となる面との成す傾きが所定の値より小さい、すなわちほぼ水平な面を走行する場合は、第２情報に基づいて移動体の３自由度の位置及び姿勢を決定する。移動体が傾いている場合、例えば斜面を走行する場合は、３自由度の距離の計測結果では重力方向の変位を計測できない。一方で、６自由度で位置姿勢計測が可能な第１情報を用いることで重力方向の変位を含んだ走行距離を推定できる。このように移動体の重力方向の傾きによって計測結果を使い分けることで、安定的に３自由度の位置姿勢推定結果を得ることが出来る。なお、移動体と基準となる面との成す傾きが所定の値より小さい場合は、第２情報に加えて第１情報に基づいて位置姿勢を決定してもよい。その場合は例えば第１情報と第２情報の結果から平均値や中央値を移動体の位置姿勢として決定する。

図６に本発明を移動体の移動制御に適用した場合の処理手順のフローチャートを示す。以下の説明では、各工程（ステップ）について先頭にＳを付けて表記することで、工程（ステップ）の表記を省略する。ただし、情報処理装置１１はこのフローチャートで説明するすべてのステップを必ずしも行わなくても良い。

Ｓ１０１０では、平面情報取得部１１７が、事前準備として移動体が走行する平面の推定を行う。推定される平面を用いることで、６自由度推定部１１２によって推定される６自由度位置姿勢を平面上の３自由度位置姿勢に変換することが可能になる。移動体が走行する平面の推定は、移動体を平面上に走行させたときに推定される位置に平面当てはめを行うことにより推定する。移動体が走行する平面の推定方法の詳細については後述する。

Ｓ１０２０では、地図情報取得部１１６は、上位管理システム１６から移動体の目標位置姿勢を取得する。本実施形態では、移動体の目標位置姿勢は平面上における３自由度の位置姿勢であるとする。

Ｓ１０３０では、６自由度推定部１１２は、撮像装置が少なくとも移動体の周囲を撮像した画像に基づいて、移動体の６自由度の位置及び姿勢を示す第１情報を推定する。具体的には、６自由度推定部１１２は、ステレオカメラによって撮影された画像情報をもとにカメラの６自由度の位置姿勢推定を行う。位置姿勢推定の際には地図情報取得部１１６によって予め取得された地図情報を利用する。カメラの６自由度の位置姿勢推定には、前述のＭｕｒ－Ａｒｔａｌらの手法を用いる。地図は生成済みであるため、地図情報は更新せずカメラの位置姿勢のみを画像情報から推定する。なお、ＳＬＡＭ技術を用いて地図情報を更新しながら位置姿勢を行っても良い。

Ｓ１０３５では、３自由度推定部１１９が、移動体が走行する環境における物体と該移動体との距離を計測した距離情報に基づいて移動体の３自由度の位置及び姿勢を示す第２情報を推定する。すなわち、ＡＧＶの３自由度の位置姿勢推定を行う。３自由度推定部１１９は、距離情報入力部１１７から入力された距離情報を第２地図情報取得部１２１に保存されているシーンの地図情報と照合することにより距離センサ１３の位置姿勢を推定する。推定される距離センサの位置姿勢を（ｘＬ，ｙＬ，θＬ）とする。

Ｓ１０４０では、決定部１２０は、第１情報に基づいて、移動体と基準となる面とのなす傾きが所定の値より大きいか否かを判断する。この傾きは、例えば、移動体の位置のＺ方向の大きさで判断する。他の値を基準にしてもよい。移動体と基準となる面とが成す傾きが所定の閾値以下である場合、Ｓ１０４５に進み、６自由度推定部１１２が推定した６自由度のカメラの位置姿勢を、該傾きに基づいて、移動体の制御が可能なように６自由度の位置姿勢を３自由度の位置姿勢に変換する。決定部１２０が、移動体と基準となる面とが成す傾きが所定の閾値以上であると判断した場合は、３自由度の位置姿勢Ｓ１０５０に進む。変換部１１３は、撮像装置の位置姿勢推定結果である第１情報に基づいて移動体の６自由度の位置及び姿勢を決定する。

Ｓ１０５０では、決定部１２０が、移動体の傾きに応じて第２情報および第３情報の少なくとも１つを選択的に用いることにより、移動体の位置及び姿勢を決定する。Ｓ１０３０で推定した位置姿勢と、Ｓ１０３５で推定した位置姿勢から、最終的に制御部１５に出力する位置姿勢を決定する。または、変換部１３０が、Ｓ１０３０で推定された６自由度の位置姿勢を、Ｓ１０１０で推定された移動体が走行する平面パラメータの推定結果に基づいて変換することで、移動体の３自由度の位置姿勢を決定する。また前述の画像センサ１２の移動体における取付け位置に基づいて、カメラの位置姿勢を移動体の位置姿勢に変換する。最終的に制御部１１５に出力する３自由度の位置姿勢を決定する。

Ｓ１０６０では、制御部１１５が、Ｓ１０５０で取得した移動体の位置姿勢と、Ｓ１０２０で取得した移動体の目標位置姿勢を比較し、移動体が目標位置姿勢に到達したか判定する。本実施形態では、現在の位置と目標位置との差、現在の姿勢と目標姿勢との差のそれぞれに閾値（例えば１ｍ）を設け、位置の差及び姿勢の差の双方が該当する閾値を下回った場合に目標位置に到達したと判定する。本実施形態では、現在の位置と目標位置との差、現在の姿勢と目標姿勢との差のそれぞれに閾値を設け、位置の差及び姿勢の差の双方が該当する閾値を下回った場合に目標位置に到達したと判定する。目標位置に到達した場合は、Ｓ１０２０に戻り、新たな目標位置姿勢を上位管理システムから受け取る。

Ｓ１０７０では、制御部１１５は、決定部１２０によって決定された移動体の３自由度の位置及び姿勢と、移動体が走行する環境を３自由度で表した地図情報とに基づいて、前移動体を地図情報において示される目標位置に移動させる。つまり、Ｓ１０５０で決定された移動体の３自由度の位置姿勢推定結果と、上位管理システム１６から送信された移動体の目標位置姿勢との差をもとに経路計算を行う。計算された経路に従うように移動体のアクチュエータ１５を制御する。

Ｓ１０８０では、制御部１１５は、移動制御の終了判定を行う。上位管理システム１６から終了命令が送られた場合には移動制御を終了する。さもなければＳ１０３０に戻り、移動制御を継続する。

図７はＳ１０１０における移動体が走行する平面の推定方法の処理手順を示すフローチャートである。Ｓ１１００では、平面情報取得部１１７が、移動体を運用時と同じ平面上を走行させることにより平面推定用のデータの取得を行う。平面推定用のデータは、移動体が走行した時の複数（Ｎ点）の位置における位置姿勢データである。位置姿勢データは、６自由度位置姿勢推定によって推定される画像センサ１１２の６自由度位置姿勢である。ここでｉ番目（１≦ｉ≦Ｎ）の位置姿勢データにおけるカメラの６自由度位置姿勢を（Ｘｖｉ，Ｙｖｉ，Ｚｖｉ，Ｗｘｖｉ，Ｗｙｖｉ，Ｗｚｖｉ）と表す。Ｘｖｉ，Ｙｖｉ，Ｚｖｉは３次元空間におけるカメラの３次元位置、Ｗｘｖｉ，Ｗｙｖｉ，Ｗｚｖｉは３次元の姿勢を表わす。本実施形態では、３次元ベクトル（Ｗｘｖｉ，Ｗｙｖｉ，Ｗｚｖｉ）のノルムを回転角、方向を回転軸とする姿勢表現を用いる。また、Ｗｘｖｉ，Ｗｙｖｉ，Ｗｚｖｉで表される姿勢の３×３回転行列をＲｖｉ、３次元ベクトル（Ｘｖｉ，Ｙｖｉ，Ｚｖｉ）ｔをｔｖｉと表す。平面推定用データを取得する際は、移動体の移動軌跡から平面パラメータを推定できるように、直線的な軌跡ではなく、例えば移動体の軌跡が閉曲線を描くように移動体を移動させる。なお、部分的な坂道や移動体の振動や床面の凹凸などによる平面からのずれは平面当てはめにより吸収できるものとする。

Ｓ１１１０では、Ｎ個の３自由度の位置データ（Ｘｖｉ，Ｙｖｉ，Ｚｖｉ）ｔに対して平面当てはめを行う。平面推定用データ取得の際に移動体が走行した平面のパラメータ（法線ベクトルｎｖ＝（ｎｖｘ，ｎｖｙ，ｎｖｚ）ｔ、通過位置ｃｖ＝（ｃｖｘ，ｃｖｙ，ｃｖｚ）ｔ）を推定する。法線ベクトルｎｖは単位ベクトルであるとする。通過位置ｃｖは（Ｘｖｉ，Ｙｖｉ，Ｚｖｉ）ｔの重心を算出することにより求める。法線ベクトルｎｖは、Ｎ個の３次元ベクトル（Ｘｖｉ－ｃｖｘ，Ｙｖｉ－ｃｖｙ，Ｚｖｉ－ｃｖｚ）ｔの共分散行列を固有値分解し、最小固有値に対応する固有ベクトルとして求める。推定した平面パラメータは、平面情報取得部１１７に保存しておく。

次にＳ１０４５における６自由度の位置姿勢を３自由度の位置姿勢への変換方法の処理手順について説明する。

Ｓ１０００で推定された平面上に６自由度位置姿勢（Ｘｖ，Ｙｖ，Ｚｖ，Ｗｘｖ，Ｗｙｖ，Ｗｚｖ）を射影し、３自由度の位置姿勢（ｘｖ，ｙｖ，θｖ）ｔに変換する（なお、Ｗｘｖ，Ｗｙｖ，Ｗｚｖで表される姿勢の３×３回転行列をＲｖとする）。具体的には、平面の通過位置ｃｖを原点、平面の法線ベクトルｎｖをＺ軸とする新しい３次元の座標系（以下、平面座標系）を定義し、３次元世界座標系における６自由度位置姿勢を平面座標系に変換する。定義から、３次元世界座標系において、平面座標系の原点（０，０，０）ｔがｃｖｔ、Ｚ軸（０，０，１）ｔがｎｖとなる。このことから、平面座標系における３次元座標Ｘｐ（＝（Ｘｐ，Ｙｐ，Ｚｐ）は次式のように３次元世界座標系における３次元座標Ｘｖ（＝（Ｘｖ，Ｙｖ，Ｚｖ）に変換される。

Ｘｖ＝ＲｐｖＸｐ＋ｃｖ
ここで、ＲｐｖはＺ軸（０，０，１）ｔを平面の法線ベクトルｎｖに変換する３×３回転行列である。Ｒｐｖは、回転軸を（０，０，１）ｔとｎｖの外積、回転角を（０，０，１）ｔとｎｖの内積の逆余弦（ａｒｃｃｏｓ）として計算する。よって、３次元世界座標系における３次元座標Ｘｖは次式のように平面座標系における３次元座標Ｘｐに変換される。

Ｘｐ＝Ｒｐｖ－１（Ｘｖ－ｃｖ）
Ｘｖとしてｔｖを入力したときのＸｐを算出する。３自由度の位置姿勢のうちｘｖ，ｙｖは、３次元座標ＸｐのＸｐ成分、Ｙｐ成分に該当する。θｖは画像センサの光軸を平面に射影したベクトルの方向である。本実施形態ではステレオカメラの左側のカメラの光軸を画像センサの光軸とする。画像センサに規定されるカメラ座標系における画像センサの光軸を表わす３次元ベクトルをｖｃとすると、平面座標系における画像センサの光軸ｖｏは次式のように表わされる。

ｖｏ＝Ｒｐｖ－１Ｒｖｖｃ
前述したように、本実施形態ではステレオカメラの光軸が水平面よりやや上方を向くように無人搬送車に取り付けるため、光軸ｖｏを平面座標系のＸＹ平面に射影してθｖを算出する。平面座標系のＸＹ平面上に射影された光軸ｖｏ’は、次式のように算出される。

ｖｏ’＝ｖｏ－（ｖｏ・（０，０，１）ｔ）ｖｏ
本実施形態では、平面座標系のＸ軸に対するｖｏ’の角度をθｖとする。

情報処理装置１１の構成は図４で述べた例に限定しない。例えば、図５に示すような情報処理装置の機能構成例をでもよい。情報処理装置１１は、画像情報入力部１１１、６自由度推定部１１２、変換部１１３、地図情報取得部１１６、平面情報取得部１１７、計測部１２２、決定部１２０を有する。移動体１は、距離情報入力部１１８、３自由度推定部１１９、制御部１１５、第２地図情報取得部２２１を有する。

Ｓ１０４０において、決定部１２０が、移動体に傾きが発生しているか否かを判断するが、毎回必ずしも判断しなくてもよい。例えば、移動体の傾きが頻繁に変わるような凸凹道を走行する際、水平になる度に６自由度から３自由度の推定に変更すると、かえって移動体の位置姿勢推定が不安定になる場合がある。頻繁に推定方法を変更することがないように、移動体に所定の値より大きい傾きが検出された場合は、決定部１２０は、移動体の傾きが解消されても所定時間（例えば、１分間）が経過するまでは６自由度の計測結果に基づいて、位置姿勢を決定する。つまり、決定部１２０は、移動体の傾きが所定の値より大きい状態から所定の値より小さくなった場合に、移動体の傾きが所定の値より小さくなった時点から一定時間後に３自由度の位置姿勢計測結果に基づいて移動体の位置姿勢を決定する。または、小さな段差を通過する際は移動体の傾きは一瞬で解消される。そのような場合は６自由度で位置姿勢計測をしなくてもそれほど誤差は発生しないため、６自由度の位置姿勢推定のみに切り替えることをせず、３自由度による位置姿勢推定を継続する。つまり、所定時間より長い時間だけ移動体の傾きが所定の値より大きい状態を保持した場合に、決定部１２０が６自由度の位置姿勢計測結果に基づいて、移動体の３自由度の位置姿勢を決定する。このように、位置姿勢推定の方法を切り替えた時間または移動体の傾きが保持された時間に応じて決定部１２０の処理を変更することで、より安定して移動体の位置姿勢を決定できる。

以上述べたように、本実施形態では、移動体の傾きが所定の値より大きい場合、画像情報によって推定される６自由度の位置姿勢を３自由度の位置姿勢に変換した情報を用いて移動体の位置姿勢を決定する。これによって、移動体が走行する床面上に坂道があっても安定的に位置姿勢を決定できる。

＜実施形態２＞
前述の実施形態では、移動体の傾きが所定の値より大きい場合、画像情報によって推定される６自由度の位置姿勢を３自由度の位置姿勢に変換した情報を用いて移動体の位置姿勢を決定することを可能にした。実施形態２では、移動体は略平面（基準となる面）を走行するものとして、走行時に本来傾くはずのない移動体の傾きを検出する。移動体の傾きは、例えば移動体上の荷物が荷台にバランスよく搭載されていない場合に生じる。この場合、荷崩れが起こる可能性があるため、傾きを検出したらユーザに警告を通知する必要がある。

図８に、本実施形態における情報処理装置２１の機能構成例のブロック図を示す。移動体２に搭載された情報処理装置２１は、情報処理装置１１と同様に画像センサ１２、距離センサ１３、接触センサ１４、アクチュエータ１５、オドメーター１７、記憶部１８を備え、さらに重力センサ２９に接続されている。重力センサ２９は、移動体の重力方向の傾きを検知し、３自由度の姿勢を計測する。

情報処理装置２１は、実施形態１の構成に加え、傾き検出部２１４、出力部２２１を有する。情報処理装置１１と同様に情報処理装置２１は、例えばＰＣ、組み込みシステム、ＰＬＣ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｃｏｎｔｒｏｌｌｅｒ）である。

傾き検出部２１４は、６自由度推定部２１２によって推定されるカメラの姿勢に基づいて走行する平面に対する移動体の傾きを検出する。あるいは、加速度センサやジャイロセンサ等の重力センサ２９を用いて、移動体の重力方向の傾きを検出してもよい。

出力部２２１は、傾き検出部２１４が所定の値より大きい移動体の傾きを検出した場合にユーザに警告を発して通知する。出力部２２１は例えばスピーカーである。または、警告表示を行うディスプレイでもよい。移動体が傾いていることをユーザに知らせることで、移動体の移動制御が失敗する前に安全に対処することができる。

図９に本実施形態における移動体の移動制御に適用した場合の処理手順のフローチャートを示す。同一のステップ番号の処理は実施形態１または２の処理と同一であるので説明を省略する。

Ｓ２０４０では、傾き検出部３１４において、走行する平面に対する移動体の傾きθを検出する。まず移動体の傾き角度を算出する。傾きのリファレンスを、Ｓ３０１０における平面推定用のデータ取得時のカメラ座標系における走行平面の法線ベクトルｎｒｅｆとする。ｎｒｅｆ＝（Ｒｖｉ）－１ｎｖである。ｎｒｅｆはＮ個のＲｖｉのうち１個のＲｖｉを決定して算出してもよいし、それぞれのＲｖｉからｎｒｅｆを算出し、平均をとってもよい。６自由度推定部３１２によって推定される姿勢を表わす３×３回転行列がＲｖのとき、カメラ座標系における走行平面の法線ベクトルは、ｎｃｕｒ＝（Ｒｖ）－１ｎｖでのように算出される。走行する平面に対する移動体の傾きθは、ｎｒｅｆとｎｃｕｒのなす角である。

Ｓ２０４５では、出力部３２１が、走行する平面に対する移動体の傾きθが所定の閾値以上であれば移動体が傾いていると判定し、ユーザに警告を通知する。ユーザへの警告は、例えば出力部３２１のスピーカーから警告音を発するなどして行う。

＜実施形態３＞
本実施形態では、移動体の移動制御に本発明を適用した場合について説明する。本実施形態では、移動体は、部分的に坂道が存在する平面（床面）を走行するものとし、移動体に搭載された画像センサ（カメラ）が撮影する画像情報をもとに推定される６自由度の位置姿勢をもとに移動体の走行距離の算出を行う。６自由度の位置及び姿勢の計測結果を用いて移動体の位置及び姿勢を推定する。これによって、移動体の位置及び姿勢を安定的に取得できる。

図１０に示す移動体３に搭載された情報処理装置３１は、移動体の周囲を撮像した画像に基づいて移動体の位置及び姿勢を推定し、その推定結果に基づいて移動体を移動制御する。情報処理装置３１は、画像センサ３２、距離センサ３３、接触センサ３４、アクチュエータ３５、オドメーター３７、記憶部３８に接続されている。また移動体３は図１の上位管理システム１６との無線通信を行う。

画像センサ３２はカメラである。画像センサ３２は、移動体の周囲を撮像する撮像装置である。本実施形態における画像センサは、水平・垂直方向の画角を有するモノクロカメラ２個から構成されるステレオカメラであるとする。ステレオカメラは、光軸が水平面よりやや上方を向くように移動体に取り付ける。ステレオカメラを構成する２台のカメラの内部パラメータ（焦点距離、画像中心、歪みパラメータ）及びカメラ間の外部パラメータ（相対的な位置姿勢）は公知の方法により予め校正されているものとする。本実施形態では、カメラの光学中心を原点、光軸方向をＺ軸、画像の水平方向をＸ軸、画像の垂直方向をＹ軸とする三次元の座標系をカメラ座標系と規定する。ステレオカメラのカメラ座標系は、左側カメラ（または右側カメラ）のカメラ座標系であるとする。画像センサ３２の移動体における取付け位置は設計値により既知であるとする。

距離センサ３３はセンサからのシーンの距離情報を計測する装置である。本実施形態における距離センサは２Ｄ ＬｉＤＡＲである。２Ｄ ＬｉＤＡＲは、レーザ光を平面上で走査させることによって距離情報を取得する距離センサである。距離センサ３３は、レーザ光を走査させる平面が略水平面となるように移動体に取り付ける。距離センサ３３の移動体における取付け位置は設計値により既知であるとする。実施形態３では、距離センサ３３を障害物センサとして利用する。

接触センサ３４は安全センサである。接触センサが移動体と他の物体との接触を検知すると移動体は緊急停止する。

アクチュエータ３５は、情報処理装置３１からの指示や障害物センサ、安全センサの出力をもとにモータを制御して移動体を移動または静止させる。

上位管理システム１６は、複数の移動体の運行管理を行うシステムである。上位管理システム１６は、各移動体の情報処理装置３１に目標位置姿勢を送信する。本実施形態では、移動体の目標位置姿勢は平面上における３自由度の位置姿勢であるとする。上位管理システム１６と情報処理装置３１の間の通信は、ＷｉｆｉやＢｌｕｅＴｏｏｔｔｈ、５Ｇなどの無線通信により行う。

オドメーター３７は、移動体の車輪の回転量から走行距離を計測する。

記憶部３８は、移動体が走行する環境についての地図情報を記憶する。他、移動体の走行距離や移動制御に必要な情報を保持する。なお、図１０のように記憶部３８は移動体上に搭載されている形以外でもよく、上位管理システム１６の内部にあってもよい。

（情報処理装置３１の詳細）
図１０に、本実施形態における情報処理装置の機能構成例を示す図を示す。情報処理装置３１は、画像情報入力部３１１、６自由度推定部３１２、変換部３１３、走行距離算出部３１４、制御部３１５、地図情報取得部３１６、平面情報取得部３１７、から構成される。情報処理装置３１は、例えばＰＣ、組み込みシステム、ＰＬＣ（Ｐｒｏｇｒａｍｍａｂｌｅ Ｌｏｇｉｃ Ｃｏｎｔｒｏｌｌｅｒ）である。

画像情報入力部３１１は、画像センサ３２から画像情報を入力し、入力された画像情報を６自由度推定部３１２に送る。本実施形態における画像センサ３２はステレオカメラであるため、画像情報はステレオ画像（左右のカメラが撮影する２枚の画像）である。画像センサ３２は、移動体が走行する工場や物流倉庫などの画像情報を撮像する。よって、本実施形態における画像情報とは、移動体に搭載された撮像装置によって移動体の走行する環境を撮像したステレオ画像を指す。

６自由度推定部３１２は、画像情報入力部３１１から入力された画像情報（ステレオ画像）をシーンの地図情報と照合することにより３次元空間中におけるカメラの６自由度の位置姿勢（位置３自由度、姿勢３自由度）を推定する。６自由度の位置姿勢は、前述のシーンの地図情報において規定される３次元の座標系（以下、世界座標系）を基準とした位置姿勢である。本実施形態では、Ｍｕｒ－Ａｒｔａｌらの手法によりシーンの地図情報の生成及びカメラの６自由度の位置姿勢推定を行う。（Ｒ．Ｍｕｒ－Ａｒｔａｌ 他，“ＯＲＢ－ＳＬＡＭ２：Ａｎ Ｏｐｅｎ－Ｓｏｕｒｃｅ ＳＬＡＭ Ｓｙｓｔｅｍ ｆｏｒ Ｍｏｎｏｃｕｌａｒ，Ｓｔｅｒｅｏ，ａｎｄ ＲＧＢ－Ｄ Ｃａｍｅｒａｓ，”，２０１７．）。Ｍｕｒ－Ａｒｔａｌらの手法は画像上で検出される特徴点を利用する手法である。シーンの地図情報は、移動体が走行する工場や物流倉庫といった移動体が走行するシーンにおける３次元点群の情報を保持する。具体的には、シーンにおける３次元点群の情報は特徴点の世界座標系における３次元座標のことである。本実施形態では、移動体を走行させながら撮影した映像をもとに、前述のＭｕｒ－Ａｒｔａｌらの手法によりあらかじめシーンの地図情報を生成しておく。シーンの地図情報は、記憶部３８に保存されているものとする。Ｍｕｒ－Ａｒｔａｌらの手法は、画像上の特徴点の位置と、推定する位置姿勢を用いて地図上の特徴点を画像上に投影した時の位置の差（再投影誤差）を最小化することによりカメラの位置姿勢を推定する。

変換部３１３は、６自由度推定部３１２が推定した６自由度のカメラの位置姿勢を、移動体の制御が可能なように移動体の３自由度の位置姿勢に変換する。なお、３自由度の位置姿勢も推定あるいは計測可能な移動体である場合は、移動体の傾きに応じて３自由度の位置姿勢推定結果と６自由度の位置姿勢変換結果を選択的に用いてもよい。移動体の傾きは、例えば、６自由度の位置及び姿勢の推定結果の高さ方向の値（Ｚ）で判断してもよい。例えば、Ｚ＝０の場合は、移動体は基準となる面を走行していると考え、３自由度に次元を減らした位置及び姿勢に変換する。一方で、Ｚ＞０（またはＺ＜０）の場合は、斜面を登っている（或いは下っている）と考えられるため、６自由度の位置及び姿勢を用いて制御を行うようにする。前述の通り、３自由度の位置姿勢は移動体が走行する平面における位置姿勢である。３自由度のうち２自由度は平面上の２次元位置、残りの１自由度は平面の法線まわりの姿勢（平面上の向き）を表わす。移動体が走行する平面の推定方法については後述する。移動体が走行する平面の情報は平面情報取得部３１７に保存されているものとする。

走行距離算出部３１４は、６自由度推定部３１２が推定するカメラの位置と前回計測された位置との差を使って移動体の走行距離を算出する。

制御部３１５は、変換部３１３が変換した位置姿勢をもとに移動体の制御を行う。具体的には、制御部３１５は、あらかじめ設定した移動体の目標位置姿勢（３自由度）と、変換部３１２が出力する３自由度の位置姿勢の差をもとに経路計算を行い、計算された経路に従うようにアクチュエータ３５にモータの回転速度などの指示を送る。移動体の位置姿勢が目標位置姿勢と略一致した場合には移動体を停止し、上位管理システム１６から次の目標位置姿勢が送られてくるのを待つ。なお、経路計算はＡ＊アルゴリズムやＲＲＴ（Ｒａｐｉｄｌｙ ｅｘｐｌｏｒｉｎｇ ｒａｎｄｏｍ ｔｒｅｅ）といったアルゴリズムを用いてもよいし、Ｑ学習やＤＱＮなどの機械学習による方法を用いてもよい。

地図情報取得部３１６は、記憶部３８より移動体が走行する環境の３自由度の地図情報を取得する。また、予め設定された移動体の３自由度の目標位置姿勢を取得する。

平面情報取得部３１７は、移動体を運用時と同じ基準となる平面上を走行させることにより平面推定用のデータの取得を行う。平面推定用のデータは、移動体が走行した時の複数（Ｎ点）の位置における位置姿勢データである。位置姿勢データは、６自由度位置姿勢推定によって推定される画像センサ３１２の６自由度位置姿勢である。基準面よりΘだけ傾いた傾斜面を走行する場合でも、３自由度の位置及び姿勢を正しく推定できるように、６自由度の位置及び姿勢を撮像装置によって撮像された画像を基に推定する。

図１１に本発明を移動体の移動制御に適用した場合の処理手順のフローチャートを示す。以下の説明では、各工程（ステップ）について先頭にＳを付けて表記することで、工程（ステップ）の表記を省略する。ただし、情報処理装置３１はこのフローチャートで説明するすべてのステップを必ずしも行わなくても良い。

Ｓ３０１０では、平面情報取得部３１７が、事前準備として移動体が走行する平面の推定を行う。推定される平面を用いることで、６自由度推定部３１２によって推定される６自由度位置姿勢を平面上の３自由度位置姿勢に変換することが可能になる。移動体が走行する平面の推定は、移動体を平面上に走行させたときに推定される位置に平面当てはめを行うことにより推定する。移動体が走行する平面の推定方法の詳細については後述する。

Ｓ３０２０では、地図情報取得部３１６は、上位管理システム１６から移動体の目標位置姿勢を取得する。本実施形態では、移動体の目標位置姿勢は平面上における３自由度の位置姿勢であるとする。

Ｓ３０３０では、６自由度推定部３１２は、少なくとも移動体の周囲を撮像した画像に基づいて、撮像装置の６自由度の位置及び姿勢を示す第１情報を推定する。具体的には、６自由度推定部３１２は、ステレオカメラによって撮影された画像情報をもとにカメラの６自由度の位置姿勢推定を行う。位置姿勢推定の際には地図情報取得部３１６によって予め取得された地図情報を利用する。カメラの６自由度の位置姿勢推定には、前述のＭｕｒ－Ａｒｔａｌらの手法を用いる。地図は生成済みであるため、地図情報は更新せずカメラの位置姿勢のみを画像情報から推定する。

Ｓ３０４５では、変換部３１３が、Ｓ３０３０で推定された６自由度の位置姿勢を、Ｓ３０１０で推定された移動体が走行する平面パラメータの推定結果に基づいて変換することで、移動体の３自由度の位置姿勢を決定する。また前述の画像センサ３２の移動体における取付け位置に基づいて、カメラの位置姿勢を移動体の位置姿勢に変換する。

Ｓ３０６０では、Ｓ３０４５で変換した移動体の位置姿勢と、Ｓ３０２０で取得した移動体の目標位置姿勢を比較し、移動体が目標位置姿勢に到達したか判定する。本実施形態では、現在の位置と目標位置との差、現在の姿勢と目標姿勢との差のそれぞれに閾値を設け、位置の差及び姿勢の差の双方が該当する閾値を下回った場合に目標位置に到達したと判定する。本実施形態では、現在の位置と目標位置との差、現在の姿勢と目標姿勢との差のそれぞれに閾値を設け、位置の差及び姿勢の差の双方が該当する閾値を下回った場合に目標位置に到達したと判定する。目標位置に到達した場合は、Ｓ３０２０に戻り、新たな目標位置姿勢を上位管理システムから受け取る。

Ｓ３０７０では、制御部３１５は、Ｓ３０４５で変換された移動体の位置姿勢と上位管理システム１６から送信された移動体の目標位置姿勢の差をもとに経路計算を行い、計算された経路に従うように移動体のアクチュエータ３５を制御する。また、走行距離算出部３１４は、６自由度推定部３１３が推定するカメラの位置をもとに移動体の走行距離Ｄを算出する。具体的には、移動体の運行を始める際に走行距離Ｄをゼロに設定しておき、本ステップにおいて前回推定時の位置との今回推定された位置の差ｄをもとに、走行距離ＤをＤ←Ｄ＋ｄのように更新する。

Ｓ３０８０では、制御部３１５は、移動制御の終了判定を行う。上位管理システム１６から終了命令が送られた場合には移動制御を終了する。さもなければＳ３０３０に戻り、移動制御を継続する。

図５はＳ３０１０における移動体が走行する平面の推定方法の処理手順を示すフローチャートである。Ｓ１１００では、平面情報取得部３１７が、移動体を運用時と同じ平面上を走行させることにより平面推定用のデータの取得を行う。平面推定用のデータは、移動体が走行した時の複数（Ｎ点）の位置における位置姿勢データである。位置姿勢データは、６自由度位置姿勢推定によって推定される画像センサ３２の６自由度位置姿勢である。ここでｉ番目（１≦ｉ≦Ｎ）の位置姿勢データにおけるカメラの６自由度位置姿勢を（Ｘｖｉ，Ｙｖｉ，Ｚｖｉ，Ｗｘｖｉ，Ｗｙｖｉ，Ｗｚｖｉ）と表す。Ｘｖｉ，Ｙｖｉ，Ｚｖｉは３次元空間におけるカメラの３次元位置、Ｗｘｖｉ，Ｗｙｖｉ，Ｗｚｖｉは３次元の姿勢を表わす。本実施形態では、３次元ベクトル（Ｗｘｖｉ，Ｗｙｖｉ，Ｗｚｖｉ）のノルムを回転角、方向を回転軸とする姿勢表現を用いる。また、Ｗｘｖｉ，Ｗｙｖｉ，Ｗｚｖｉで表される姿勢の３×３回転行列をＲｖｉ、３次元ベクトル（Ｘｖｉ，Ｙｖｉ，Ｚｖｉ）ｔをｔｖｉと表す。平面推定用データを取得する際は、移動体の移動軌跡から平面パラメータを推定できるように、直線的な軌跡ではなく、例えば移動体の軌跡が閉曲線を描くように移動体を移動させる。なお、部分的な坂道や移動体の振動や床面の凹凸などによる平面からのずれは平面当てはめにより吸収できるものとする。

Ｓ１１１０では、Ｎ個の３自由度の位置データ（Ｘｖｉ，Ｙｖｉ，Ｚｖｉ）ｔに対して平面当てはめを行う。平面推定用データ取得の際に移動体が走行した平面のパラメータ（法線ベクトルｎｖ＝（ｎｖｘ，ｎｖｙ，ｎｖｚ）ｔ、通過位置ｃｖ＝（ｃｖｘ，ｃｖｙ，ｃｖｚ）ｔ）を推定する。法線ベクトルｎｖは単位ベクトルであるとする。通過位置ｃｖは（Ｘｖｉ，Ｙｖｉ，Ｚｖｉ）ｔの重心を算出することにより求める。法線ベクトルｎｖは、Ｎ個の３次元ベクトル（Ｘｖｉ－ｃｖｘ，Ｙｖｉ－ｃｖｙ，Ｚｖｉ－ｃｖｚ）ｔの共分散行列を固有値分解し、最小固有値に対応する固有ベクトルとして求める。推定した平面パラメータは、平面情報取得部３１７に保存しておく。

次にＳ３０４５における６自由度の位置姿勢を３自由度の位置姿勢への変換方法の処理手順について説明する。

Ｓ３０１０で推定された平面上に６自由度位置姿勢（Ｘｖ，Ｙｖ，Ｚｖ，Ｗｘｖ，Ｗｙｖ，Ｗｚｖ）を射影し、３自由度の位置姿勢（ｘｖ，ｙｖ，θｖ）ｔに変換する（なお、Ｗｘｖ，Ｗｙｖ，Ｗｚｖで表される姿勢の３×３回転行列をＲｖとする）。具体的には、平面の通過位置ｃｖを原点、平面の法線ベクトルｎｖをＺ軸とする新しい３次元の座標系（以下、平面座標系）を定義し、３次元世界座標系における６自由度位置姿勢を平面座標系に変換する。定義から、３次元世界座標系において、平面座標系の原点（０，０，０）ｔがｃｖｔ、Ｚ軸（０，０，１）ｔがｎｖとなる。このことから、平面座標系における３次元座標Ｘｐ（＝（Ｘｐ，Ｙｐ，Ｚｐ）は次式のように３次元世界座標系における３次元座標Ｘｖ（＝（Ｘｖ，Ｙｖ，Ｚｖ）に変換される。

Ｘｖ＝ＲｐｖＸｐ＋ｃｖ
ここで、ＲｐｖはＺ軸（０，０，１）ｔを平面の法線ベクトルｎｖに変換する３×３回転行列である。Ｒｐｖは、回転軸を（０，０，１）ｔとｎｖの外積、回転角を（０，０，１）ｔとｎｖの内積の逆余弦（ａｒｃｃｏｓ）として計算する。よって、３次元世界座標系における３次元座標Ｘｖは次式のように平面座標系における３次元座標Ｘｐに変換される。

Ｘｐ＝Ｒｐｖ－１（Ｘｖ－ｃｖ）
Ｘｖとしてｔｖを入力したときのＸｐを算出する。３自由度の位置姿勢のうちｘｖ，ｙｖは、３次元座標ＸｐのＸｐ成分、Ｙｐ成分に該当する。θｖは画像センサの光軸を平面に射影したベクトルの方向である。本実施形態ではステレオカメラの左側のカメラの光軸を画像センサの光軸とする。画像センサに規定されるカメラ座標系における画像センサの光軸を表わす３次元ベクトルをｖｃとすると、平面座標系における画像センサの光軸ｖｏは次式のように表わされる。

ｖｏ＝Ｒｐｖ－１Ｒｖｖｃ
前述したように、本実施形態ではステレオカメラの光軸が水平面よりやや上方を向くように無人搬送車に取り付けるため、光軸ｖｏを平面座標系のＸＹ平面に射影してθｖを算出する。平面座標系のＸＹ平面上に射影された光軸ｖｏ’は、次式のように算出される。

ｖｏ’＝ｖｏ－（ｖｏ・（０，０，１）ｔ）ｖｏ
本実施形態では、平面座標系のＸ軸に対するｖｏ’の角度をθｖとする。

以上述べたように、本実施形態では、画像情報によって推定される６自由度の位置姿勢をもとに移動体の走行距離を算出する。これによって、移動体が走行する床面上に坂道があっても安定的に走行距離の算出ができる。

（変形例）
前述の実施形態では、画像センサとしてステレオカメラを利用していた。しかしながら画像センサはこれに限るものではなく、位置姿勢を推定するための画像情報が得られるカメラであればいかなるカメラであってもよい。例えば、単眼カメラであってもよいし、ＲＧＢ画像と距離画像が取得できるＲＧＢＤカメラであってもよい。また、画像はモノクロである必要はなく、ＲＧＢ画像などのカラー画像であってもよい。

本実施形態では、移動体として工場や倉庫の物流を行うＡＧＶを想定したいたが、それ以外のロボットでもよい。例えば、部屋を清掃する掃除機ロボットにも適用可能である。

前述の実施形態では、入力部はカメラからの画像情報を入力していた。しかしながら、入力部を介して入力するのは画像情報に限るものではない。例えばカメラに据え付けられた慣性センサ（加速度センサ、角速度センサ）や姿勢センサからのセンサ計測値を入力してもよい。

前述の実施形態では、画像上で検出される特徴点の再投影誤差を最小化する方法を用いてカメラの位置姿勢を推定していた。しかしながらカメラの位置姿勢推定方法はこれに限るものではない。例えば、Ｅｎｇｅｌらが開示している方法のように、輝度値の差を最小化する方法であってもよい。（Ｊ．Ｅｎｇｅｌ，他，“ＬＳＤ－ＳＬＡＭ：Ｌａｒｇｅ－Ｓｃａｌｅ Ｄｉｒｅｃｔ Ｍｏｎｏｃｕｌａｒ ＳＬＡＭ，”２０１４．）。また、画像だけでなく、加速度センサや角速度センサといった慣性センサとのハイブリッド手法であってもよい。さらには、ニューラルネットワーク（学習済みモデル）を使った位置姿勢推定方法であってもよい。撮像装置によって撮像された前記移動体の周囲を写した画像から移動体の３自由度の位置及び姿勢を出力する学習済みモデルを予め準備してもよい。学習済みモデルは画像に対して正しい距離の計測値を正解として学習させる。他には例えば、Ｔａｔｅｎｏらが開示している方法のように、ＣＮＮ（Ｃｏｎｖｏｌｕｔｉｏｎａｌ Ｎｅｕｒａｌ Ｎｅｔｗｏｒｋ）により推定した距離情報をもとに位置姿勢推定を行ってもよい。（Ｋ．Ｔａｔｅｎｏ，他，“ＣＮＮ－ＳＬＡＭ：Ｒｅａｌ－ｔｉｍｅ ｄｅｎｓｅ ｍｏｎｏｃｕｌａｒ ＳＬＡＭ ｗｉｔｈ ｌｅａｒｎｅｄ ｄｅｐｔｈ ｐｒｅｄｉｃｔｉｏｎ，”，２０１７．）。

前述の実施形態では、移動体が走行する平面を走行軌跡に対する平面当てはめにより求めていた。しかしながら、移動体が走行する平面を求める方法はこれに限るものではない。例えば、シーン中に配置した２次元マーカを利用してもよい。（例えば、加藤、Ｍ．Ｂｉｌｌｉｎｇｈｕｒｓｔ、浅野、橘、“マーカ追跡に基づく拡張現実感システムとそのキャリブレーション”，１９９９．に開示されている２次元マーカ）。具体的には、２次元マーカを移動体の走行する平面上に配置してマーカの座標系を平面と一致させ、同一地点におけるマーカに対するカメラの位置姿勢と６自由度推定部によるカメラの位置姿勢の関係からマーカの座標系を基準とした位置姿勢に変換してもよい。また２次元マーカを移動体の走行する平面に対して垂直に配置してもよい。さらには、平面が水平面である場合には、移動体に加速度センサを設置し、加速度センサによって計測される重力軸方向を平面の法線ベクトルとしてもよい。

実施形態では、距離情報に基づく位置姿勢推定方法としてパーティクルフィルタを利用するＧｒｉｓｅｔｔｉらの手法を利用していた。しかしながら距離情報に基づく位置姿勢推定方法はこれに限るものではなく、例えばカルマンフィルタを利用する手法であってもよい。またパーティクルフィルタやカルマンフィルタなどの時系列フィルタを使う必要は必ずしもなく、ＩＣＰアルゴリズムによってフレームごとに位置姿勢を推定してもよい。（Ｙ．Ｃｈｅｎ，Ｇ．Ｍｅｄｉｏｎｉ，“Ｏｂｊｅｃｔ ｍｏｄｅｌｌｉｎｇ ｂｙ ｒｅｇｉｓｔｒａｔｉｏｎ ｏｆ ｍｕｌｔｉｐｌｅ ｒａｎｇｅ ｉｍａｇｅｓ，”Ｉｍａｇｅ ａｎｄ Ｖｉｓｉｏｎ Ｃｏｍｐｕｔｉｎｇ，１９９２．）。

実施形態では、画像情報を使って推定した位置姿勢と距離情報を使って推定した位置姿勢から制御部１５に出力する位置姿勢を決定する際、画像情報を使った位置姿勢推定の失敗したときに距離情報を使って推定した位置姿勢を決定していた。しかしながら、位置姿勢の決定方法はこれに限るものではない。例えば、距離情報を使った位置姿勢推定が失敗したら画像情報を使って推定した位置姿勢を決定してもよい。また、画像情報／距離情報を使って推定される位置姿勢を共通の尺度で評価し、その尺度に基づいて位置姿勢を決定してもよい。例えば、立野らの方法に開示されるように、誤差の確率分布がわかれば推定される位置姿勢の尤度を計算することができる。（立野，小竹，内山、“ビンピッキングのための距離・濃淡画像を最ゆうに統合する高精度高安定なモデルフィッティング手法”，電子情報通信学会論文誌，ｖｏｌ．Ｊ９４－Ｄ，ｎｏ．８，ｐｐ．１４１０－１４２２，２０１１．）。そこで、画像上の特徴点の検出誤差の分布や距離の計測誤差の分布を予め求めておき、画像情報／距離情報を使って推定されるそれぞれの位置姿勢の尤度を算出・比較することで位置姿勢を決定することも可能である。

前述の実施形態では、６自由度の位置姿勢を用いて走行距離の算出や移動体の傾き検出を行っていた。しかしながら、６自由度の位置姿勢の利用方法はこれに限るものではない。例えば、移動体が略平面上を走行するという事前知識を用いて、カメラの位置姿勢推定の失敗を判定してもよい。例えば、推定されるカメラ位置が平面上になければカメラ位置姿勢推定に失敗していると判定してもよい。また、失敗判定の対象はカメラの位置姿勢推定に限るものではない。例えば、前述のＭｕｒ－Ａｒｔａｌらの手法は、カメラの位置姿勢推定とシーンの地図情報の生成を同時に行う手法である。そのため、推定されるカメラの位置姿勢を移動体が略平面上を走行するという事前知識を利用して、地図情報の生成の失敗判定を行ってもよい。

前述の実施形態では、画像情報を用いて６自由度の位置姿勢の推定を行っていた。しかしながら、６自由度の位置姿勢推定の際に利用するのは画像情報に限るものではなく、６自由度の位置姿勢を推定できる情報であれば他の情報であってもよい。例えば、デプスカメラや３Ｄ ＬｉＤＡＲといった距離センサから得られる距離情報を用いて６自由度の位置姿勢の推定を行ってもよい。また、画像情報と距離情報を併用して６自由度の位置姿勢の推定を行ってもよい。さらには、加速度センサや角速度センサなどの慣性センサからの情報を併用してもよい。

本発明は、以下の処理を実行することによっても実現される。即ち、上述した実施形態の機能を実現するソフトウェア（プログラム）を、データ通信用のネットワーク又は各種記憶媒体を介してシステム或いは装置に供給する。そして、そのシステム或いは装置のコンピュータ（またはＣＰＵやＭＰＵ等）がプログラムを読み出して実行する処理である。また、そのプログラムをコンピュータが読み取り可能な記録媒体に記録して提供してもよい。

１ 移動体
１１ 情報処理装置
１２ 画像センサ
１３ 距離センサ
１４ 接触センサ
１５ アクチュエータ
１７ オドメーター
１１１ 入力部
１１２ ６自由度推定部
１１３ 変換部
１１５ 制御部
１１６ 地図情報取得部
１１７ 平面情報取得部

Claims (18)

1. 複数の位置及び姿勢を計測する機能を搭載した移動体の位置及び姿勢を決定する情報処理装置であって、
   前記移動体の６自由度の位置及び姿勢を示す第１情報を取得する第１取得手段と、
   前記移動体の３自由度の位置及び姿勢を示す第２情報を取得する第２取得手段と、
   前記第１情報を用いて、３自由度の位置及び姿勢を示す第３情報を出力する出力手段と、
   前記移動体が走行する位置での高さ方向の値から得られる、前記移動体の傾きが所定の値より小さい場合は前記第２情報を、前記移動体の傾きが所定の値より大きい場合は前記第３情報を、それぞれ用いて前記移動体の位置及び姿勢を決定する決定手段と、を有することを特徴とする情報処理装置。
2. 前記第１取得手段は、少なくとも前記移動体の周辺を撮像装置が撮像した画像に基づいて、前記第１情報を取得することを特徴とする請求項１に記載の情報処理装置。
3. 前記第２取得手段は、前記移動体が走行する環境を示す２次元の地図情報と、前記移動体が走行する環境における物体と該移動体との距離を計測した距離情報とに基づいて、前記第２情報を取得することを特徴とする請求項１または２に記載の情報処理装置。
4. 前記決定手段によって決定された前記移動体の３自由度の位置及び姿勢と、前記移動体が走行する環境を表した地図情報とに基づいて、前記地図情報において示される目標位置に前記移動体を移動させる制御手段と、を更に有することを特徴とする請求項１乃至３のいずれか１項に記載の情報処理装置。
5. 前記決定手段は、前記移動体の傾きが前記所定の値より小さい場合は、前記第２情報に基づいて前記移動体の３自由度の位置及び姿勢を決定し、前記移動体の傾きが前記所定の値より大きい場合、前記第３情報に基づいて前記移動体の３自由度の位置及び姿勢を決定することを特徴とする請求項１乃至４のいずれか１項に記載の情報処理装置。
6. 前記移動体の傾きは前記第１情報に基づいて得られることを特徴とする請求項５に記載の情報処理装置。
7. 前記移動体に搭載された重力センサに基づいて、前記移動体の傾きが得られることを特徴とする請求項５に記載の情報処理装置。
8. 前記第１取得手段は、前記移動体に搭載された撮像装置によって撮像された前記移動体の周辺を写した画像を入力し、前記移動体の３自由度の位置及び姿勢を出力する学習済みモデルに基づいて前記移動体の３自由度の位置及び姿勢を取得することを特徴とする請求項１乃至７のいずれか１項に記載の情報処理装置。
9. 前記第１情報に基づいて、前記移動体の傾きが検出された場合にユーザに通知を出力する出力手段を更に有することを特徴とする請求項１乃至８のいずれか１項に記載の情報処理装置。
10. 前記決定手段は、前記移動体の傾きが所定の値より小さい場合は、前記第２情報と第３情報とに基づいて前記移動体の３自由度の位置及び姿勢を決定することを特徴とする請求項１乃至９のいずれか１項に記載の情報処理装置。
11. 前記移動体の３自由度の位置及び姿勢は、２自由度の位置と１自由度の姿勢であることを特徴とする請求項１乃至１０のいずれか１項に記載の情報処理装置。
12. 前記移動体の６自由度の位置及び姿勢は、３自由度の位置と３自由度の姿勢であることを特徴とする請求項１乃至１１のいずれか１項に記載の情報処理装置。
13. 前記決定手段は、前記移動体の傾きが所定の値より大きい状態が所定の時間より長く継続した場合は、前記第３情報を用いて前記移動体の３自由度の位置及び姿勢を決定することを特徴とする請求項１乃至１２のいずれか１項に記載の情報処理装置。
14. 前記決定手段は、前記移動体の傾きが所定の値より大きい状態から小さくなり、かつ該移動体の傾きが小さい状態が所定の時間より長く継続した場合は、前記第３情報から前記第２情報を用いた前記移動体の位置及び姿勢を決定するように変更することを特徴とする請求項１乃至１３のいずれか１項に記載の情報処理装置。
15. 撮像装置を搭載した移動体の位置及び姿勢を推定する情報処理装置であって、
    少なくとも前記移動体の周辺を撮像した画像に基づいて、前記撮像装置の６自由度の位置及び姿勢を示す第１情報を推定する６自由度推定手段と、
    前記移動体が走行する位置での高さ方向の値から得られる、前記移動体と基準となる面との成す傾きが所定の値より大きい場合は、前記第１情報に基づいて前記移動体の３自由度の位置及び姿勢を決定し、前記移動体と基準となる面との成す傾きが所定の値より小さい場合は、前記移動体が走行する環境における物体と該移動体との距離を計測した距離情報に基づいて前記移動体の３自由度の位置及び姿勢を示す第２情報に基づいて前記移動体の３自由度の位置及び姿勢を決定する決定手段と、を有することを特徴とする情報処理装置。
16. コンピュータを、請求項１乃至１５のいずれか１項に記載の情報処理装置が有する各手段として機能させるためのプログラム。
17. 撮像装置を搭載した移動体の位置及び姿勢を推定する情報処理方法であって、
    前記撮像装置の６自由度の位置及び姿勢を示す第１情報を推定する６自由度推定ステップと、
    前記移動体が走行する位置での高さ方向の値から得られる、前記移動体と基準となる面との成す傾きが所定の値より大きい場合は、前記第１情報に基づいて前記移動体の３自由度の位置及び姿勢を決定し、前記移動体と基準となる面との成す傾きが所定の値より小さい場合は、前記移動体の３自由度の位置及び姿勢を示す第２情報に基づいて前記移動体の３自由度の位置及び姿勢を決定する決定ステップと、有することを特徴とする情報処理方法。
18. 複数の位置及び姿勢を計測する機能を搭載した移動体の位置及び姿勢を決定する情報処理方法であって、
    前記移動体の６自由度の位置及び姿勢を示す第１情報を取得する第１取得ステップと、
    前記移動体の３自由度の位置及び姿勢を示す第２情報を取得する第２取得ステップと、
    前記第１情報を用いて、３自由度の位置及び姿勢を示す第３情報を出力する出力ステップと、
    前記移動体が走行する位置での高さ方向の値から得られる、前記移動体の傾きが所定の値より小さい場合は前記第２情報を、前記移動体の傾きが所定の値より大きい場合は前記第３情報を、それぞれ用いて前記移動体の位置及び姿勢を決定する決定ステップと、を有することを特徴とする情報処理方法。

Images