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

Abstract

【課題】簡単な計算で、通路の境界物に沿ってリアルタイムに安定して移動体を走行制御する。【解決手段】検出部２５により検出された移動体の移動路の三次元環境から、抽出部５２が、各物体の存在位置及び姿勢を示す当て嵌め対象領域を抽出する。認識部５３は、当て嵌め対象領域に対応する物体の外形となる三次元モデルを、位置及び姿勢を調整して当て嵌め対象領域に当て嵌めることで、移動体を原点とした座標系における各物体の位置及び姿勢を認識する。そして、生成部５５が、認識された各物体の位置及び姿勢に応じて、移動体を移動制御するための制御信号を生成する。【選択図】図８

Description

本発明は、情報処理装置、情報処理方法及び情報処理プログラムに関する。

屋外環境において、カメラ装置又はレーザーレンジファインダ等の外界センサを用いて、通路を形成している壁又は境界に沿って移動体を自動走行するための技術が知られている。

例えば、特許文献１（特開２０１０−０６６９３４号公報）には、三次元モデルを利用して移動体の通路を形成する自己位置認識システムが開示されている。また、特許文献２（特開２００９−２６５９４１号公報）には、走行領域をもれなく走行させる目的で、境界の縁をレーザー等のセンサを用いて検出し、沿うように移動制御する自律移動装置が開示されている。さらに、特許文献３（特開２００６−０３１５０３号公報）には、移動体が走行空間内の壁際や隅を探索走行する目的で、接触センサと赤外線センサにより壁との距離を保ちながら壁に沿って移動する自律走行移動体が開示されている。

しかし、従来の走行技術は、予め走行する環境について地図等を作成する必要があるため、手間がかかる問題があった。また、安定して自己位置を計算するためには、計算コストが高くなる問題があった。すなわち、屋外等の開かれた環境では、経時変化が容易に起こるため、定期的に地図を更新する必要がある。また、屋外の不整地環境における外界センサの振動は、自己位置推定に悪影響を与える。このため、安定して自動走行させるためには、様々なセンサ出力に基づく計算を行う必要があり、計算コストが高くなる。

特許文献１に開示されている自己位置認識システムの場合、環境全体のデータを作る必要があり、移動体の振動等による画像とモデルのズレを考慮していないため、手間がかかり、かつ、不整地環境で安定した自動走行ができないという問題がある。特許文献２に開示されている自律移動装置の場合、境界の縁の認識が困難となる問題がある。さらに、特許文献３に開示されている自律走行移動体の場合、例えば壁等の一様な境界面しか対応しておらず、屋外環境で安定して移動困難となる問題がある。

本発明は、上述の課題に鑑みてなされたものであり、簡単な計算で、通路の境界物に沿ってリアルタイムに安定して移動体を走行制御できるような情報処理装置、情報処理方法及び情報処理プログラムの提供を目的とする。

上述した課題を解決し、目的を達成するために、本発明は、検出部により検出された移動体の移動路の三次元環境から、各物体の存在位置及び姿勢を示す当て嵌め対象領域を抽出する抽出部と、当て嵌め対象領域に対応する物体の外形となる三次元モデルを、位置及び姿勢を調整して当て嵌め対象領域に当て嵌めることで、移動体を原点とした座標系における各物体の位置及び姿勢を認識する認識部と、認識された各物体の位置及び姿勢に応じて、移動体を移動制御するための制御信号を生成する生成部とを有する。

本発明によれば、簡単な計算で、通路の境界物に沿ってリアルタイムに安定して移動体を走行制御できるという効果を奏する。

図１は、実施の形態のクローラの外観を示す斜視図である。 図２は、実施の形態のクローラの上面図である。 図３は、アーム部を移動させるアクチュエータが設けられたクローラを示す図である。 図４は、アーム部を移動させる複数のアクチュエータが設けられたクローラを示す図である。 図５は、アーム部を移動させる複数のアクチュエータ及びレールが設けられたクローラを示す図である。 図６は、実施の形態のクローラの電気構成を説明するためのハードウェア構成図である。 図７は、実施の形態のクローラのソフトウェア構成を説明するための図である。 図８は、実施の形態のクローラの機能ブロック図である。 図９は、実施の形態のクローラの領域抽出部における領域抽出動作を説明するための図である。 図１０は、実施の形態のクローラの領域抽出部の領域抽出動作の流れを示すフローチャートである。 図１１は、実施の形態のクローラの姿勢認識部における姿勢認識処理を説明するための図である。 図１２は、実施の形態のクローラの姿勢認識部による、当て嵌め対象領域に対する３Ｄモデルの当て嵌め処理を説明するための図である。 図１３は、実施の形態のクローラの姿勢認識部による、全長が長い当て嵌め対象領域に対する３Ｄモデルの当て嵌め処理を説明するための図である。 図１４は、実施の形態のクローラの姿勢認識部における姿勢認識処理の流れ示すフローチャートである。 図１５は、対向二輪モデルである実施の形態のクローラの左右の車輪の速度を示すパラメータを説明するための図である。 図１６は、境界物が一つである場合における、実施の形態のクローラの移動方向の演算動作を説明するための図である。 図１７は、境界物が二つである場合における、実施の形態のクローラの移動方向の演算動作を説明するための図である。

以下、図面に基づいて実施の形態の移動体となるクローラの説明をする。この実施の形態のクローラは、三次元計測センサにより取得したデータに対して、通路を形成している物体及び領域の三次元モデル（３Ｄモデル）をリアルタイムで当て嵌めることで、通路境界を認識し、３Ｄモデルに対する相対自己位置を用いて移動制御を行う。なお、以下、移動体は、地面を走行するクローラであることとして説明を進めるが、移動体は、遠隔操作又は自動的に空中を移動する移動体でもよいし、水上又は水中を移動する移動体でもよいものと理解されたい。

（クローラの機械構成）
図１は、実施の形態のクローラ１の外観を示す斜視図である。また、図２は、クローラ１の上面図である。この図１及び図２は、クローラ１の前側に境界を形成する境界物３が存在する例を示している。境界物３は、例えば車道と歩道の区切りを示す縁石等が該当する。クローラ１は、キャタピラ４を回転駆動して地面を移動する。本体５上には、屈曲動作及び回転動作等が可能なアーム部６が設けられている。また、アーム部６の先端部（反本体５側）には、三次元計測センサ２が設けられている。

一例ではあるが、三次元計測センサ２としては、ステレオカメラ装置が設けられている。また、アーム部６には、三次元計測センサ２を移動させるアクチュエータが設けられている。図３の例は、アーム部６の先端に三次元計測センサ２を上下左右方向及び斜め方向に移動させるアクチュエータ７が設けられている例である。

また、図４（ａ）の例は、アーム部６の本体５側に、アーム部６を中心軸回りに回転動作させるアクチュエータ８が設けられている。また、アーム部６の全長に対して約真ん中の位置に、アーム部６を上下に屈曲させるアクチュエータ９が設けられている。さらに、アーム部６の反本体５側に、三次元計測センサ２を上下左右方向及び斜め方向に移動させるアクチュエータ７が設けられている。図４（ｂ）は、図４（ａ）に示したクローラ１を前方から見た状態を示す正面図である。この図４（ｂ）からわかるように、図４（ａ）に示したクローラ１の場合、アーム部６を上下に屈曲移動させることができ、また、アーム部６を、３６０度、回転駆動することができる。

また、図５（ａ）の例は、クローラ１が前進する方向に対して２次元的に直交する方向に沿ってアーム部６を移動させるレール１０を、本体５に設けた例である。図５（ｂ）は、アーム部６を取り除いた状態で、レール１０を本体５の上面側から見た図である。また、図５（ｃ）は、クローラ１を正面から見た状態の正面図であり、アーム部６がレール１０の右端まで移動した状態を示している。なお、レールは、クローラ１が前進する方向及び斜め方向に沿って設けてもよい。

このようにアクチュエータ７〜９及びレール１０を設けることで、三次元計測センサ２を自在に移動可能とすることができ、センシング領域を広げることができる。

（クローラのハードウェア構成）
次に、図６に、クローラ１の電気回路のハードウェア構成を示す。この図６に示すように、クローラ１は、ＣＰＵ２１、ＲＯＭ２２、ＲＡＭ２３、ＨＤＤ２４、三次元計測センサ２５、モーションセンサ２６、アクチュエータ２７、移動モータ２８及び通信部２９を有している。ＣＰＵ２１〜通信部２９は、バスライン３０を介して相互に接続されている。

ＣＰＵ２１は、後述する情報処理プログラムに基づいて、クローラ１の動作を制御する。ＲＯＭ２２には、オペレーションシステム（ＯＳ）等のプログラムが記憶されている。ＲＡＭ２３は、ＣＰＵ２１により情報処理プログラムが展開され、また、ワークエリア等として用いられる。ＨＤＤ２４には、所定のプログラム及び各センサから取得された情報等が記憶される。

三次元計測センサ２５は、三次元情報を取得するためのセンサであり、図１〜図５を用いて説明した三次元計測センサ２に相当する。三次元計測センサ２５は、対象となる領域を認識するために、ステレオカメラ装置、又は、ＬＩＤＡＲ(Light Detection and Ranging、又は、Laser Imaging Detection and Ranging)等を用いる。

モーションセンサ２６は、クローラ１の姿勢とステレオカメラ装置の姿勢を取得するためのセンサであり、加速度、角加速度、地磁気等の情報を用いてクローラ１の姿勢を示す情報取得する。アクチュエータ２７は、図３〜図５を用いて説明したアクチュエータ７〜９等に相当し、様々な方向の情報を得るために三次元計測センサ２５移動させる。

移動モータ２８は、キャタピラ４を回転駆動させてクローラ１を地面に沿って移動させる。通信部２９は、オペレータからの制御情報等を受信し、また、各センサの情報をオペレータ側の制御装置等に送信する無線通信（有線通信でもよい）を行う。

（クローラのソフトウェア構成）
次に、図７は、クローラ１に記憶されているプログラム及び各種情報を示している。この図７に示しように、クローラ１には、情報処理プログラム、三次元情報、認識結果、ＯＳ及び３Ｄモデル等が記憶されている。これらのプログラム及び各種情報は、クローラ１の内外を問わず、どこに記憶されていてもよいが、一例として、この実施の形態の場合、情報処理プログラム、三次元情報、認識結果、及び、３Ｄモデルは、ＨＤＤ２４に記憶されている。また、ＯＳは、ＲＯＭ２２に記憶されている。

なお、上述したプログラム、情報、３Ｄモデル等は、一例として、ＲＯＭ２２又はＨＤＤ２４等に記憶されることとしたが、この他、ＲＡＭ２２又はフラッシュメモリ等の他の記憶部に記憶してもよい。さらには、ネットワークを介して接続可能な記憶装置等からダウンロードして用いてもよい。

（クローラの機能構成）
次に、クローラ１のＣＰＵ２１は、ＨＤＤ２４に記憶されている情報処理プログラムを実行することで、図８に示す各種機能を実現する。具体的には、ＣＰＵ２１は、情報処理プログラムを実行することで、演算部４１及び作成部４２を実現する。演算部４１は、各種機能として、点群取得部５１、領域抽出部５２、姿勢認識部５３、三次元計測センサ制御部５４及び移動制御部５５を有している。また、作成部４２は、各種機能として、３Ｄモデル作成部６１、地図作成部６２及び記憶制御部６３を有している。

点群取得部５１は、三次元計測センサ２５によって得られたデータを点群として処理する。領域抽出部５２は、抽出部の一例であり、点群取得部５１で生成された点群に基づいて領域を抽出する。姿勢認識部５３は、認識部の一例であり、領域抽出部５２で抽出された領域から、境界を形成している物体がどのような姿勢で存在しているかを認識する。三次元計測センサ制御部５４は、３Ｄモデルをセンシングするために、アクチュエータ２７を制御する。

移動制御部５５は、生成部の一例である。移動制御部５５は、姿勢認識部５３からの姿勢認識出力、モーションセンサ２６で検出されるクローラ１の姿勢（及び三次元計測センサ２５の姿勢）、及び、以下に説明する地図作成部６２で検出されるクローラ１の位置に基づいて、クローラ１を走行制御するための制御信号を生成する。移動制御部５５は、制御信号を移動モータ２８に供給して回転駆動し、クローラ１を走行制御する。また、移動制御部５５は、アクチュエータ２７を駆動制御するためのアクチュエータ制御信号を生成する。アクチュエータ制御信号は、三次元計測センサ制御部５４を介してアクチュエータ２７に供給される。これにより、アクチュエータ２７を介して三次元計測センサ２５の姿勢が制御される。

３Ｄモデル作成部６１は、例えばＣＡＤ(Computer Aided Design)システム等を用いることができ、後述する３Ｄモデルを作成する。地図作成部６２は、キャタピラ４を回転させる移動モータ２８の回転角等からのオドメトリ情報、又は、ＧＰＳ（Global positioning system）による座標値等に基づいて、クローラ１の走行位置を検出して走行履歴を作成する。記憶制御部６３は、三次元情報、認識結果、３Ｄモデル及び走行履歴等をＨＤＤ２４等の記憶部に記憶する。

なお、この例では、点群取得部５１〜移動制御部５５、及び、３Ｄモデル作成部６１〜記憶制御部６３は、情報処理プログラムにより、ソフトウェア的に実現されることとして説明を進める。しかし、点群取得部５１〜移動制御部５５、及び、３Ｄモデル作成部６１〜記憶制御部６３のうち、一部又は全部を、ＩＣ（Integrated Circuit）等のハードウェアで実現してもよい。

また、情報処理プログラムは、インストール可能な形式又は実行可能な形式のファイルでＣＤ−ＲＯＭ、フレキシブルディスク（ＦＤ）等のコンピュータ装置で読み取り可能な記録媒体に記録して提供してもよい。また、情報処理プログラムは、ＣＤ−Ｒ、ＤＶＤ（Digital Versatile Disk）、ブルーレイディスク（登録商標）、半導体メモリ等のコンピュータ装置で読み取り可能な記録媒体に記録して提供してもよい。また、情報処理プログラムは、インターネット等のネットワーク経由でインストールするかたちで提供してもよい。さらに、情報処理プログラムは、機器内のＲＯＭ等に予め組み込んで提供してもよい。

（領域抽出動作の概要）
次に、図９を用いて、領域抽出部５２における領域抽出動作を説明する。図９（ａ）は、三次元計測センサ２５が地面と水平に前方を向いている状態を示す図である。三次元計測センサ２５には、モーションセンサ２６が取り付けられている。モーションセンサ２６で、三次元計測センサ２５の姿勢を検出することで、三次元計測センサ２５から取得した点群情報の地面からの高さ位置を検出することができる。例えば、図９（ａ）に示すように、地面に対して垂直方向を三次元計測センサ２５のＺ軸とし、右側（移動体の前方）方向を三次元計測センサ２５のＹ軸とする。三次元計測センサ２５の中心を原点（０，０，０）としたとき、重力加速度の基底ベクトルｇは（０，０，−１）で表すことができる。

次に、この状態で前方方向にθだけ三次元計測センサ２５を傾けたとする。重力加速度ベクトルの方向は常に地面の方向を向いているが、三次元計測センサ２５自体は傾いているので、センサ座標系で考えると、ｇは（０，ｓｉｎθ，ｃｏｓθ）で表すことができる。この時、三次元計測センサ２５から得られた点群上のある点のベクトルをｐ（ａ，ｂ，ｃ）とすると、センサ座標系における点ｐの地面に垂直方向のベクトルｈｖはｇとｐのベクトルの内積「ｇ×ｐ」で表すことができる。すなわち、三次元計測センサ２５の地面からの高さＨから、ベクトルｈｖの大きさを引くことで（Ｈ−｜ｈｖ｜）、点ｐの地面からの高さを得ることができる。これにより、三次元計測センサ２５の姿勢にかかわらず、得られた点群の地面からの高さを求めることができる。

（領域抽出動作の流れ）
次に、図１０のフローチャートを用いて、領域抽出部５２における、３Ｄモデルをあてはめる領域を抽出する動作の流れを説明する。まず、クローラ１に取り付けられている三次元計測センサ２５の地面からの高さ位置を取得する（ステップＳ１）。この三次元計測センサ２５の高さ位置は、あらかじめ人間が入力しても良いし、超音波センサからの超音波を地面に出射して戻ってくる時間を検出して測定しても良い。

次に、点群取得部５１からＮ個の点群点ｐ＿ｎ（ｎ＝１，２，〜Ｎ）を取得し、初期値としてｎ＝１をセットする（ステップＳ２）。次に、三次元計測センサ２５に設けられたモーションセンサ２６から姿勢情報を取得し、図９（ａ）を用いて説明した重力加速ベクトルｇを計算する（ステップＳ３）。次に、図９（ｂ）を用いて説明した点ｐにおける地面に垂直方向のベクトルｈｖを計算する（ステップＳ４）。

次に、地面から三次元計測センサ２５までの高さＨと、ベクトルｈｖの大きさ｜ｈｖ｜を比較する。そして、地面から三次元計測センサ２５までの高さＨは、ベクトルｈｖの大きさ｜ｈｖ｜よりも大きいか否かを判別する（ステップＳ５）。高さＨの方が大きい、つまり点ｐ＿ｎの高さが、地面よりも高い位置にある場合は（ステップＳ５：Ｙｅｓ）、「ラベル１」をｐ＿ｎに付与する（ステップＳ６）。高さＨの方が小さい、つまり点ｐ＿ｎの高さが、地面よりも低い位置にある場合は（ステップＳ５：Ｎｏ）、「ラベル０」をｐ＿ｎに付与する（ステップＳ７）。

次に、ｎの数が、点群の総数であるＮ個となったか否かを判別する（ｎ＝Ｎ？：ステップＳ８）。ｎの数が、点群の総数であるＮ個となっていない場合は（ステップＳ８：Ｎｏ）、ｎの数を一つインクリメントし、ステップＳ３に処理を戻す（ステップＳ１０）。ｎの数が、点群の総数であるＮ個となった場合は（ステップＳ８：Ｙｅｓ）、ステップＳ３からの処理が全ての点群に対して行われたことを意味するため、点群においてラベル１が付与されている点を「当て嵌め対象領域」の点として、記憶制御部６３が、ＨＤＤ２４等の記憶部に保存して（ステップＳ９）、図１０のフローチャートの処理を終了する。

（姿勢認識処理）
次に、図１１を用いて姿勢認識部５３による姿勢認識処理を説明する。姿勢認識部５３は、境界を形成する物体に対し、３Ｄモデルを当て嵌めることで、物体がどのような姿勢で存在しているのかを認識する。例えば、図１１（ａ）に示すように四角柱形状の縁石７０が車道と歩道の境界に沿って設けられていたとする。これに対して、図１１（ｂ）に示す四角柱形状の３Ｄモデルである縁石モデル７０Ｄが、縁石７０に対して当て嵌めを行う３Ｄモデルである。図１１（ｂ）に示すｘｙｚの座標系は、縁石モデル７０Ｄの中心を原点としたモデル座標系を示している。

例えば、クローラ１と縁石７０が、図１１（ｃ）に示すように、クローラ１の左斜め前方に、縁石７０が存在する位置関係であるとする。姿勢認識部５３は、この位置関係のクローラ１と縁石７０を、三次元計測センサ２５の中心を原点としたセンサ座標系で考え、図１１（ｄ）に示すように、縁石モデル７０Ｄをセンサ座標系で回転及び移動させることで、上述のラベル１が付された点群である当て嵌め対象領域の中から、縁石モデル７０Ｄの形に合致する形状の当て嵌め対象領域を検出し、縁石モデル７０Ｄを当て嵌める。当て嵌め対象領域に縁石モデル７０Ｄが当て嵌まったとき、センサ座標系における縁石モデル７０Ｄの姿勢及び位置がわかる。すなわち、実際の縁石７０がどのような姿勢で存在しているかを認識することができる。

姿勢認識部５３は、三次元計測センサ２５からのセンサ出力に対して、このような姿勢認識処理をフレーム毎に行う。これにより、例えばクローラ１が不整地走行等を行うことで三次元計測センサ２５に振動が生じても、移動制御には悪影響を受ける不都合を防止できる。また、このような姿勢認識処理を毎フレーム行うことにより、センサ座標系において３Ｄモデルを移動させる距離は、フレーム間で少ない距離で済むため、姿勢認識処理の処理時間を極短時間化することができ、略々リアルタイムでの姿勢認識処理を可能とすることができる。また、３Ｄモデルを当て嵌めることができれば、三次元計測センサ２５のセンサ座標系において、物体とクローラ１との相対的な位置関係がわかるため、クローラ１を物体の境界に沿って移動させることができる。

（他の３Ｄモデルの当て嵌め例）
図１２は、各３Ｄモデルの当て嵌め例を示している。図１２（ａ）は、ガードレール７５の３Ｄモデルであるガードレールモデル７５Ｄの当て嵌め例を示している。図１２（ａ）の場合、ガードレール７５は、クローラ１の前進方向の斜め右側に位置している。姿勢認識部５３は、上述のラベル１が付された点群である当て嵌め対象領域の中から、ガードレールモデル７５Ｄの形に合致する形状の当て嵌め対象領域を検出し、ガードレールモデル７５Ｄを当て嵌める。これにより、センサ座標系におけるガードレールモデル７５Ｄの姿勢及び位置を介して、実際のガードレール７５がどのような姿勢で存在しているかを認識することができる。

図１２（ｂ）は、段差７６の３Ｄモデルである段差モデル７６Ｄの当て嵌め例を示している。図１２（ｂ）の場合、段差７６は、クローラ１の右側に、クローラ１の前進方向に沿って形成されている。姿勢認識部５３は、上述のラベル１が付された点群である当て嵌め対象領域の中から、段差モデル７６Ｄの形に合致する形状の当て嵌め対象領域を検出し、段差モデル７６Ｄを当て嵌める。これにより、センサ座標系における段差モデル７６Ｄの姿勢及び位置を介して、実際の段差７６がどのような姿勢で存在しているかを認識することができる。

図１２（ｃ）は、ロードコーン７７の３Ｄモデルであるロードコーンモデル７７Ｄの当て嵌め例を示している。図１２（ｃ）の場合、ロードコーン７７は、クローラ１の前進方向の右斜め前側の道路上に載置されている。姿勢認識部５３は、上述のラベル１が付された点群である当て嵌め対象領域の中から、ロードコーンモデル７７Ｄの形に合致する形状の当て嵌め対象領域を検出し、ロードコーンモデル７７Ｄを当て嵌める。これにより、センサ座標系におけるロードコーンモデル７７Ｄの姿勢及び位置を介して、実際のロードコーン７７がどのような姿勢で存在しているかを認識することができる。

図１２（ｄ）は、側溝７８の３Ｄモデルである側溝モデル７８Ｄの当て嵌め例を示している。図１２（ｄ）の場合、側溝７８は、クローラ１の右側に、クローラ１の前進方向に沿って形成されている。姿勢認識部５３は、上述のラベル１が付された点群である当て嵌め対象領域の中から、側溝モデル７８Ｄの形に合致する形状の当て嵌め対象領域を検出し、側溝モデル７８Ｄを当て嵌める。これにより、センサ座標系における側溝モデル７８Ｄの姿勢及び位置を介して、実際の側溝７８がどのような姿勢で存在しているかを認識することができる。

このように、縁石７０のような比較的単純な物体の他、３Ｄモデルで表現できる物体であれば、様々な形状の物体で形成された境界を認識できる。例えば、図１２（ｂ）に示した段差７６は、横断面がアルファベットの「Ｚ」の文字の形状とした段差モデル７６Ｄを用いることで、実際の段差７６を認識できる。また、図１２（ｄ）に示した側溝７８は、横断面がカタカナの「コ」の文字の形状とした側溝モデル７８Ｄを用いることで、実際の側溝７８を認識できる。

なお、図１２（ｃ）に示したように、複数のロードコーン７７が所定の間隔で道路上に置かれている場合、一つのロードコーンモデル７７Ｄにより、各ロードコーン７７を、一つずつ認識しても良い。または、複数のロードコーンモデル７７Ｄを所定の間隔をおいて仮想的に連結したロードコーンモデル７７Ｄを用いて、複数のロードコーン７７を一度に認識しても良い。

また、同一形状の物体が長く連なって境界を形成している場合、三次元計測センサ２５が、例えばステレオカメラ装置のように，視野に限りがあるセンサでは、一度に境界全てを撮像することは困難となる。このような場合、姿勢認識部５３は、連続している境界を形成する一つの物体の一部の形状に対応する３Ｄモデルを、クローラ１の進行に応じて、境界を形成する物体に順次当て嵌める動作を繰り返し行う。換言すると、姿勢認識部５３は、当て嵌め対象領域の全長が、３Ｄモデルの全長よりも長い場合、当て嵌め対象領域に対する３Ｄモデルの当て嵌め位置を、クローラ１の進行に応じて更新する。

具体的には、図１３（ａ）に示すようにクローラ１の進行方向に沿って、一連のガードレール７５が設けられていたとする。この場合、姿勢認識部５３は、一連のガードレール７５の一部となるガードレールモデル７５Ｄを、図１３（ｂ）に示すようにクローラ１の進行に応じて、ガードレール７５に相当する当て嵌め対象領域に、順次、当て嵌める。換言すると、姿勢認識部５３は、ガードレールモデル７５Ｄを当て嵌める当て嵌め対象領域を、クローラ１の進行に応じて順次、更新する。これにより、三次元計測センサ２５の視野に限りがある場合でも、連続する物体を、クローラ１の進行に応じて順次認識できる。

また、姿勢認識部５３は、一連の物体を分割して認識できるため、一連の物体が曲線状に形成されている場合、及び、境界となる物体が断続的かつ直線上に設置されていない場合でも、正確に認識できる。このため、クローラ１を、物体の設置位置に応じて、正確かつ安定して移動させることができる。

（姿勢認識処理の流れ）
次に、姿勢認識部５３が、３Ｄモデルを当て嵌めて、物体の姿勢を認識する姿勢認識処理の流れを、図１４のフローチャートを用いて説明する。まず、３Ｄモデルは、事前に３Ｄモデル作成部６１により作成され、記憶制御部６３によりＨＤＤ２４に記憶されている。姿勢認識部５３は、認識対象となる３ＤモデルをＨＤＤ２４から取得する（ステップＳ１１）。

次に、姿勢認識部５３は、後述する３Ｄモデルの当て嵌まり具合を評価するための評価関数Ｅと、閾値Ｔを設定する（ステップＳ１２）。次に、姿勢認識部５３は、点群取得部５１及び領域抽出部５２を介して三次元計測センサ２５からのデータを取得する（ステップＳ１３）。次に、姿勢認識部５３は、領域抽出部５２により、上述のラベル１が付された当て嵌め対象領域を取得する（ステップＳ１４）。

次に、姿勢認識部５３は、ＨＤＤ２４から取得した３ＤモデルＭを、三次元計測センサ２５のセンサ座標系に投影する（ステップＳ１５）。次に、姿勢認識部５３は、評価関数Ｅに従い、当て嵌め対象領域と３Ｄモデルとの当て嵌り具合を示す評価値を計算する（ステップＳ１６）。そして、姿勢認識部５３は、算出した評価値と所定の閾値Ｔとを比較し（ステップＳ１７）、評価値が閾値Ｔより大きい場合（Ｅ＜Ｔ：ステップＳ１７：Ｎｏ）、３Ｄモデルを移動及び回転させ（ステップＳ２０）、再び評価値を計算する（ステップＳ１６）。姿勢認識部５３は、このようなステップＳ１６、ステップＳ１７及びステップＳ２０の処理を、評価値が閾値Ｔよりも小さな値となるまで（ステップＳ１７：Ｙｅｓ）、繰り返し実行する。

次に、姿勢認識部５３は、３ＤモデルＭの位置により、三次元計測センサ２５のカメラ制御を行うか否かを判別する（ステップＳ１８）。すなわち、例えばステレオカメラ装置等のように、三次元計測センサ２５としてセンシング範囲が制限されるセンサが設けられている場合、３ＤモデルＭがセンシング範囲外に出てしまうと、境界が認識困難となる。このため、姿勢認識部５３は、センシング範囲内に３ＤモデルＭが２／３以上位置しているか否か、又は、３ＤモデルＭの任意の端点が、画面内の中心にあるか否か、等の判別を行うことで、センシング範囲内に位置している３ＤモデルＭの範囲を判別する。

センシング範囲内に位置する３ＤモデルＭの範囲が所定以下の範囲となり、カメラ制御が必要になったと判別した場合（ステップＳ１８：Ｙｅｓ）、姿勢認識部５３は、３ＤモデルＭがセンシングの範囲内（中央）に位置するように、アクチュエータ２７を介して三次元計測センサ２５を移動制御する（ステップＳ２１）。３ＤモデルＭの位置は、センサ座標系により検出できる。このため、センサ座標系で検出した３ＤモデルＭの位置に基づいて、アクチュエータ２７をフィードバック制御することで、３ＤモデルＭを継続してセンシング範囲内に位置させることができる。

次に、センシング範囲内に位置する３ＤモデルＭの範囲が所定よりも大きな範囲となり、カメラ制御が不要になったと判別した場合（ステップＳ１８：Ｎｏ）、姿勢認識部５３は、センサ座標系で検出された３ＤモデルＭの位置及び姿勢の各情報を、記憶制御部６３を介してＨＤＤ２４等の記憶部に記憶する（ステップＳ１９）。なお、姿勢認識部５３は、３ＤモデルＭの姿勢の情報を、四元数（クオータニオン）又はオイラー角表示の情報形態で記憶部に記憶する。

（評価関数の設定）
次に、上述のステップＳ２で説明した当て嵌め対象領域に対する３Ｄモデルの当て嵌まり具合を評価するための評価関数Ｅの設定アルゴリズムを説明する。この実施の形態のクローラ１は、３Ｄモデルを当て嵌め対象領域に当て嵌めるためのアルゴリズムとしては、三次元の点群をそのまま利用するアルゴリズムと、画像との対応関係に基づいて点群を２次元の点群に変換して行うアルゴリズム等がある。

三次元の点群をそのまま利用するアルゴリズムとしては、例えばＩＣＰ（Iterative Closest Point）アルゴリズム等を用いることができる。このＩＣＰアルゴリズムは、点群の三次元座標値の距離情報に基づいて、二つのデータ（センサデータ、３Ｄモデル）の位置合わせを行うアルゴリズムである。評価関数Ｅとしては、二つのデータ点同士の距離の和を用い、所定の閾値以下になるまで３Ｄモデルを動かしながら位置合わせを行う。ＩＣＰアルゴリズムは、全データ点同士の距離計算を行うので処理時間を要する。しかし、ＩＣＰアルゴリズムは、サンプリングによる点の間引き処理、及び、データ点同士の近傍探索手法の最適化処理等を適用できる。また、ＩＣＰアルゴリズムは、ＧＰＵ（Graphics Processing Unit）を用いた並列計算により、計算速度を高速化できるため、十分にリアルタイム性を確保することができる。このＩＣＰアルゴリズムの参考文献１を以下に示す。

（参考文献１）
P.Besl and N. McKay. A Method for Registration of 3-D Shapes. IEEE Trans. Pattern Analysis and Machine Intelligence, 14:239-256, 1992.

画像との対応関係に基づいて点群を２次元の点群に変換して行うアルゴリズムとしては、３Ｄモデルベースドトラッキング（3D Model based tracking）アルゴリズムを用いることができる。このアルゴリズムは、ステレオカメラ装置等の画像情報と点群情報の両方を有している場合に用いることができる。このアルゴリズムは、カメラ内部行列を基に３Ｄモデルを画像空間上に投影し、３Ｄモデルと対象物の姿勢を画像上で一致させる。位置合わせを行うための特徴量としては、エッジベース、特徴点ベース、領域ベース等を用いることができる。この３Ｄモデルベースドトラッキングアルゴリズムの参考文献２〜参考文献４を以下に示す。

（参考文献２）
T. Drummond and R. Cipolla. Real-time visual tracking of complex struc- tures. IEEE Transactions on Pattern Analysis and Machine Intelligence, 24:932- 946, 2002.

（参考文献３）
Klein, Georg, and David Murray. "Parallel tracking and mapping for small AR workspaces." Mixed and Augmented Reality, 2007. ISMAR 2007.

（参考文献４）
V. A. Prisacariu and I. D. Reid. PWP3D: Real-time segmentation and tracking of 3D objects. In Proc 19th British Machine Vision Conference, 2009.

評価関数Ｅとしては、例えば参考文献４の場合、３Ｄモデルを画像上に投影した領域（シルエット）と、画像上に存在する物体の領域の一致度を確率同士の計算により求めた評価関数Ｅを用いている。３Ｄモデルの回転及び移動については、微分計算により求めることができ、ＧＰＵ（Graphics Processing Unit）を用いた並列計算により、高速な計算が可能である。

（移動制御動作）
次に、移動制御部５５の移動制御動作を説明する。なお、移動制御部５５は、図１５に示すように対向二輪モデルとなるクローラ１を、移動体として移動制御することとして説明を進める。しかし、対向二輪モデルの他、リモートコントローラ等で遠隔操作される空中を移動する移動体、又は、人が運転操作する自動車等移動体にも、以下に説明する移動制御をモデル化して適用可能である。

対向二輪モデルでは、右の車輪と左の車輪を独立に制御する。図１５に示すｖＬとｖＲは、それぞれ左の車輪と右の車輪の速度を示している。ｖは平均速度で、両輪の速度差ｄｖにより、旋回制御を行うことができる（ｖＬ＝ｖ−ｄｖ、ｖＲ＝ｖ＋ｄｖ）。

移動制御部５５は、図１６に示すようにクローラ１の移動方向ベクトルと、境界物となっている縁石７０の境界ベクトル（方向ベクトル）の角度差を算出することで、縁石７０に沿ったクローラ１の移動方向を算出する。また、移動制御部５５は、クローラ１の所定時間後（ｔ秒後）の移動位置と境界ベクトルの位置とを比較することで、縁石７０との間に所定の距離（ｍａｒｇｉｎ）を保ちながらクローラ１を移動制御する。なお、移動制御部５５は、三次元計測センサ２５のセンサ情報を取得するフレームと、現在のクローラ１の速度に基づいて、上述の所定時間ｔを決定する。

移動制御部５５は、制御値の計算に、クローラ１の原点として考える移動体座標系を用いる。クローラ１が沿って進行する縁石７０の軸方向は、図１６に示すように、縁石７０の中心を通る方向である。このため、移動制御部５５は、縁石７０の境界ベクトル（方向ベクトル）を、縁石７０の中心に沿って、クローラ１の移動方向に設定する。

移動制御部５５は、以下の数式に基づいて、クローラ１の左右車輪の速度差ｄｖを算出する。

ｄｖ＝ＫＬ（Ｃ（Ｌ）−（ｓｇｎＬ×ｍａｒｇｉｎ））＋Ｋｔθ

この数式において、「ＫＬ」は、クローラ１と縁石７０との間の距離を示している。また、「Ｋｔ」は、クローラ１の移動方向ベクトルと縁石７０の境界ベクトルとの角度差θの係数を示している。また、「margin」は、クローラ１が縁石７０に沿って移動する際に、クローラ１と縁石７０との間に保持する距離を示すパラメータである。移動制御部５５は、これらの数値を変更することで、クローラ１を縁石７０から「margin」分の距離を保持した状態で縁石７０に沿って移動させることができる。これらのパラメータは、予め手動で決められた数値を用いる。

また、左右車輪の速度差ｄｖが大きくなり過ぎると、margin付近の距離でクローラ１がふらつきながら移動する不都合を生ずる。このため、移動制御部５５は、以下の数１式の演算を行うことで、左右車輪の速度差ｄｖが大きくなり過ぎたときに、制御値が、閾値clip以上にならないように、クリッピング関数Ｃ（Ｌ）を算出して設定する。

この数式において、「Sgn」は、符号関数を示している。また、「Ｌ」はｔ秒後のクローラ１の移動位置と境界ベクトルとの間の距離を示している。また、「Ｌ」は符号付きの距離となっており、移動体座標系で考えた時、クローラ１の進行方向の左側に縁石７０が存在する場合はプラスの値となり、クローラ１の進行方向の右側に縁石７０がある場合はマイナスの値となる。このようなクリッピング関数Ｃ（Ｌ）を設定することで、スムーズに走行するようにクローラ１の走行を制御できる。

また、図１７に示すように、クローラ１の左右に縁石７０が存在する場合、移動制御部５５は、「margin」を各縁石７０の間の距離の１／２の距離に設定する。これにより、クローラ１を、各縁石７０の中間位置を、各縁石７０に沿って移動させることができる。この場合、縁石７０の３Ｄモデルである縁石モデル７０Ｄを二つ用意し、当て嵌め対象領域に当て嵌めることで、同時に二つの縁石７０を認識することができる。

（実施の形態の効果）
以上の説明から明らかなように、実施の形態のクローラ１は、三次元計測センサ２５からのセンサ出力に基づいて検出される通路上の物体を示す当て嵌め対象領域に、通路上の物体に対応する３Ｄモデルを当て嵌めることで、縁石７０等の境界物を認識する。そして、境界物に沿ってクローラ１を移動制御する。これにより、屋外環境又は不整地環境等であっても、簡単な演算で安定してクローラ１を走行制御できる。

また、当て嵌め対象領域に、通路上の物体に対応する３Ｄモデルを当て嵌める処理を、三次元計測センサ２５からのセンサ出力のフレーム毎に行う。隣接するフレーム間における３Ｄモデルの移動距離は、少ない距離の移動制御で済むため、略々リアルタイムで当て嵌め対象領域に対する３Ｄモデルの当て嵌め処理を行うことができる。このため、リアルタイムで安定して、境界物に沿ってクローラ１を走行制御できる。

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

１ クローラ
２ 三次元計測センサ
３ 縁石
４ キャタピラ
５ 本体
６ アーム部
７ アクチュエータ
８ アクチュエータ
９ アクチュエータ
１０ レール
２１ ＣＰＵ
２２ ＲＯＭ
２３ ＲＡＭ
２４ ＨＤＤ
２５ 三次元計測センサ
２６ モーションセンサ
２７ アクチュエータ
２８ 移動モータ
２９ 通信部
４１ 演算部
４２ 作成部
５１ 点群取得部
５２ 領域抽出部
５３ 姿勢認識部
５４ 三次元センサ制御部
５５ 移動制御部
６１ ３Ｄモデル作成部
６２ 地図作成部
６３ 記憶制御部
７０ 縁石
７０Ｄ 縁石モデル
７５ ガードレール
７５Ｄ ガードレールモデル
７６ 段差
７６Ｄ 段差モデル
７７ ロードコーン
７７Ｄ ロードコーンモデル
７８ 側溝
７８Ｄ 側溝モデル

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

Claims (9)

1. 検出部により検出された移動体の移動路の三次元環境から、各物体の存在位置及び姿勢を示す当て嵌め対象領域を抽出する抽出部と、
   前記当て嵌め対象領域に対応する物体の外形となる三次元モデルを、位置及び姿勢を調整して前記当て嵌め対象領域に当て嵌めることで、前記移動体を原点とした座標系における前記各物体の位置及び姿勢を認識する認識部と、
   認識された前記各物体の位置及び姿勢に応じて、前記移動体を移動制御するための制御信号を生成する生成部と
   を有する情報処理装置。
2. 前記抽出部は、前記各物体の姿勢と高さに基づいて、前記移動路を分離する境界物の前記当て嵌め対象領域を前記三次元環境から抽出し、
   前記認識部は、前記当て嵌め対象領域に対応する前記境界物の外形となる三次元モデルを、位置及び姿勢を調整して前記当て嵌め対象領域に当て嵌めることで、前記移動体を原点とした前記境界物の位置及び姿勢を認識し、
   前記生成部は、認識された前記境界物に沿って、前記移動体を移動制御するための制御信号を生成すること
   を特徴とする請求項１に記載の情報処理装置。
3. 前記認識部は、前記当て嵌め対象領域に前記三次元モデルを当て嵌める処理を、前記検出部からの検出出力のフレーム毎に行うこと
   を特徴とする請求項１又は請求項２に記載の情報処理装置。
4. 前記認識部は、前記当て嵌め対象領域の全長が前記三次元モデルの全長よりも長い場合、前記当て嵌め対象領域に対する前記三次元モデルの当て嵌め位置を、前記移動体の進行に応じて更新すること
   を特徴とする請求項１から請求項３のうち、いずれか一項に記載の情報処理装置。
5. 検出部により検出された移動体の移動路の三次元環境から、抽出部が、各物体の存在位置及び姿勢を示す当て嵌め対象領域を抽出する抽出ステップと、
   認識部が、前記当て嵌め対象領域に対応する物体の外形となる三次元モデルを、位置及び姿勢を調整して前記当て嵌め対象領域に当て嵌めることで、前記移動体を原点とした座標系における前記各物体の位置及び姿勢を認識する認識ステップと、
   生成部が、認識された前記各物体の位置及び姿勢に応じて、前記移動体を移動制御するための制御信号を生成する生成ステップと
   を有する情報処理方法。
6. コンピュータを、
   検出部により検出された移動体の移動路の三次元環境から、各物体の存在位置及び姿勢を示す当て嵌め対象領域を抽出する抽出部と、
   前記当て嵌め対象領域に対応する物体の外形となる三次元モデルを、位置及び姿勢を調整して前記当て嵌め対象領域に当て嵌めることで、前記移動体を原点とした座標系における前記各物体の位置及び姿勢を認識する認識部と、
   認識された前記各物体の位置及び姿勢に応じて、前記移動体を移動制御するための制御信号を生成する生成部として機能させること
   を特徴とする情報処理プログラム。
7. 前記抽出部は、前記各物体の姿勢と高さに基づいて、前記移動路を分離する境界物の前記当て嵌め対象領域を前記三次元環境から抽出し、
   前記認識部は、前記当て嵌め対象領域に対応する前記境界物の外形となる三次元モデルを、位置及び姿勢を調整して前記当て嵌め対象領域に当て嵌めることで、前記移動体を原点とした前記境界物の位置及び姿勢を認識し、
   前記生成部は、認識された前記境界物に沿って、前記移動体を移動制御するための制御信号を生成すること
   を特徴とする請求項６に記載の情報処理プログラム。
8. 前記認識部は、前記当て嵌め対象領域に前記三次元モデルを当て嵌める処理を、前記検出部からの検出出力のフレーム毎に行うこと
   を特徴とする請求項６又は請求項７に記載の情報処理プログラム。
9. 前記認識部は、前記当て嵌め対象領域の全長が前記三次元モデルの全長よりも長い場合、前記当て嵌め対象領域に対する前記三次元モデルの当て嵌め位置を、前記移動体の進行に応じて更新すること
   を特徴とする請求項６から請求項８のうち、いずれか一項に記載の情報処理プログラム。

Images