JP2021114222A - ロボットシステムおよびその位置推定方法 - Google Patents

Abstract

【課題】複数のロボットで構成されるロボットシステムにおいて、ノイズを低減した精度の高いシステムを提供する。【解決手段】複数のロボット１００が連携することで、所定の機能を実行するロボットシステムにおいて、それぞれのロボットと無線ルータを介して接続されるサーバ１１０が、地図情報とセンサで検知された検知情報を照合して、ロボット１００の位置を推定する自己位置推定プログラム１１０２と、複数のロボットの位置関係を示す位置関係情報を用いて、他のロボットがセンサの照射範囲で映り込む確率を示す映り込み率を算出し、算出された映り込み率に応じて、位置推定部でロボット１００の位置を推定するためのセンサの照射方向を特定する回転台制御プログラム１１０３とを有する。【選択図】図３

Description

本発明は、移動体（以下、ロボットと称する）の位置を推定するロボット位置推定技術に関する。その中でも特に、ロボット位置推定のために、ロボットのセンサにおける照射等（含むスキャン）する方向を算出する技術に関する。なお、その位置の推定には、姿勢の推定を含む。

現在、人手不足や危険地帯など人での作業が困難な場所での作業を行うため、ロボットを利用することがなされている。この場合、ロボットが地図情報を参照して自身の自己位置を推定しながら、移動を制御する技術が提案されている。この技術の一例として、特許文献１が提案されている。

特許文献１には、ロボットの姿勢同定と地図生成を同時に行う場合の負荷増加を課題とする技術が開示されている。具体的には、「地図データを有するコントローラと移動するロボットから構成されるロボットシステムであって、前記ロボットは周囲の物体との距離を複数計測する距離センサと、前記地図データと照合することで前記ロボットの位置と角度を同定する同定装置を備え、前記コントローラは前記ロボットの位置と角度、及び、計測した前記距離を基に、前記地図データを生成又は更新する地図生成装置を備える」ロボットシステムが開示されている。

特開2009-93308号公報

ここで、ロボットの位置推定おいては、地図情報と推定の際の検知情報（センサから得られた周囲形状）をマッチングした結果を用いることになる（例えば、スキャンマッチング手法）。このため、検知情報にノイズが含まれる場合、正確な位置推定ができないとの課題が生じる。ノイズとしては、例えば、地図作成の際に存在しないオブジェクト（物体）がある。さらに、ロボットの動作においては、複数のロボットが連動して動作することがある。この場合、他のロボットが検知されると、ノイズとなり正確な位置推定ができないおそれがある。

ここで、特許文献１では、センサを有する複数のロボットを用いて検知した結果と地図を用いて、ロボットの位置、角度、地図データを生成するロボットシステムが開示されている。この場合、１つのロボットのセンサで他のロボットが検知されてしまい、センシングができない場所が発生し、位置推定の精度が下がってしまうという課題が生じる。

上記の課題を解決するために、本発明では、他のロボット等のオブジェクトが検知ないし映り込む確率を示す映り込み率が低い方向に向けて、ロボットの位置推定のための照射処理を実行する。なお、この際、映り込み率が低い方向については、オブジェクトの位置情報を用いて、照射角度を所定角度ずつずらしたシミュレーションを実行して映り込み率を算出することが好適である。

より具体的な本発明の構成は、以下のとおりである。

複数のロボットが連携することで、所定の機能を実行するロボットシステムにおいて、
センサを有する第１のロボット、第２のロボットおよび、前記第１のロボットおよび前記第２のロボットと通信回線を介して接続されるサーバを有し、前記サーバが、地図情報と前記センサで検知された検知情報を照合して、前記第１および第２のロボットの位置を推定する位置推定部と、前記第１のロボットおよび前記第２のロボットの位置関係を示す位置関係情報を用いて、前記第２のロボットが前記第１のロボットのセンサの照射範囲で検知ないし映り込む確率を示す映り込み率を算出し、算出された前記映り込み率に応じて、前記第１のロボットのセンサの照射方向を特定するセンサ方向制御部とを有する。

本発明でのロボット（移動体）には、狭義のロボット以外も含まれる。例えば、自動運転ないし運転支援で制御される車両、飛行物体、船舶が含まれ、これらの隊列制御への適用も可能である。車両については、トラック、バスなどが含まれる。

また、ロボットの位置推定には、ロボットでの自己位置推定が含まれる。

本発明によれば、ノイズの影響を低減したより精度の高いロボットの位置推定を実現可能である。

実施例１における全体システム構成を示す図 実施例１で用いられるロボットの外観を示す図 実施例１で用いられるロボットのソフトウェア構成を示す図 実施例１での映り込み率の算出を説明するための図 実施例１における最小映り込み率角度を求めるフローチャート 実施例１におけるロボットの照射範囲を説明する図 実施例２におけるロボットの移動経路を説明する図 実施例１の全体の処理シーケンス図 実施例２の処理を示すフローチャート

以下、本発明の実施例１について説明する。

まず、図１を用いて、本実施例の全体システム構成を説明する。本実施例では、ロボット１００が、無線ルータ１２０を介して、サーバ１１０と接続されている。ここで、ロボット１００は「×３」、つまり、３台など複数存在し、これらが連携して、所定の動作を実行する。例えば、あるロボット１００が他のロボット１００と連携し、サーバ１１０からの指示に従い、または、サーバ１１０へ動作状況を報告しながら動作する。動作の具体的な例は、後述する。

ここで、全体システムの各構成を説明する。ロボット１００は、LiDAR（Light Detection and Ranging、Laser Imaging Detection and Ranging）センサ１０１、回転台１０２、クローラ１０３、アーム１０４、ステレオカメラ１０５、モータ駆動回路１０６およびPC１０７を有する。LiDARセンサ１０１は、ロボット位置推定のために一定の幅を有する照射範囲で照射を実行し、計測データを取得する。照射範囲は、２次元でも３次元でも構わない。また、LiDARセンサ１０１（レーダ）の代わりに、赤外線を用いたり、ステレオカメラを用いてもよい。

回転台１０２は、LiDARセンサ１０１を搭載し、その照射方向を変更するために回転する機能を有する。クローラ１０３は、無限軌道とも呼ばれ、ロボット１００を移動するものである。なお、クローラ１０３の代わりに車輪や移動用アーム（脚）を用いてもよい。

アーム１０４は、ロボット１００が物体を移動させるために用いるものである。ステレオカメラ１０５は、障害物の検知を行ったり、他のロボット１００の移動のために画像を撮影するものである。ここで、図２で後述するように、各ロボット１００は、アーム１０４およびステレオカメラ１０５のいずれか一方を備える構成としてもよい。

モータ駆動回路１０６は、回転台１０２、クローラ１０３、アーム１０４に搭載されるモータ（例えば、サーボモータ）を駆動させるための回路であり、PC１０７からの制御信号に従ってこれらの動作を制御する。

PC１０７は、サーバ１１０と通信し、当該ロボット１００の制御を行ったり、ロボット１００での動作状況を通信する機能を有する。このために、PC１０７は、CPU１０７１、メモリ１０７２、ストレージ１０７３、ネットワーク通信部１０７４および電源１０７５を有する。つまり、PC１０７は、いわゆるコンピュータで実現されるもので、メモリ１０７２に格納されるプログラムに従った演算をCPU１０７１で実行して、各機能を実行する。バッテリー１０８は、ロボット１００の電源となるもので、充電式か否かは問わない。

サーバ１１０は、いわゆるコンピュータで実現されるもので、CPU１１１、メモリ１１２、ストレージ１１３、ネットワーク通信部１１４および電源１１５を有する。そして、サーバ１１０は、いわゆるコンピュータで実現されるもので、メモリ１１２に格納されるプログラムに従った演算をCPU１１１で実行して、上述したロボット１００を制御するための各機能を実行する。

無線ルータ１２０は、サーバ１１０とロボット１００（PC１０７）を無線通信回線で接続する機能を有する。無線ルータ１２０は、通信機能を有すればよく、通信回線における通信形式は問わない。この場合、有線通信であってもよい。なお、通信経路は、ＴＣＰ／ＩＰベースとすることが好適である。

ディスプレイ１３０は、サーバ１１０と接続され、サーバ１１０からの情報を表示する。ディスプレイ１３０で表示される情報としては、ロボット１００のステレオカメラ１０５の画像やロボット１００の動作状況が含まれる。入力デバイス１４０は、操作者がサーバ１１０に対する入力を受け付けるもので、キーボード、マウス、Joystickなどで実現される。入力デバイス１４０からの入力により、ロボット１００の動作を制御可能である。なお、ディスプレイ１３０と入力デバイス１４０は、端末装置として実現してもよい。

次に、図２を用いて、ロボット１００の外観を説明する。ここでは、アームロボット１００−１とカメラロボット１００−２を例に説明する。

アームロボット１００−１は、LiDARセンサ１０１−１、回転台１０２−１、クローラ１０３−１、アーム１０４−１およびロボット筐体１０９−１を有する。ここで、ロボット筐体１０９−１の左右にクローラ１０３−１、上部にアーム１０４−１と回転台１０２−１とその上にLiDARセンサ１０１−１を搭載する。

また、LiDARセンサ１０１−１は車体後方、後ろ向きにレーザを照射するように（測距可能なように）設置されている。さらに、LiDARセンサ１０１−１は左右８０度、合計約１６０度の範囲など一定の照射範囲の測距が可能である。またさらに、LiDARセンサ１０１−１は回転機構の付いた回転台１０２−１の上に搭載されているので、LiDARセンサ１０１−１の向きを変えることができ、上述の照射範囲より広範囲の計測が可能になっている。

なお、LiDARセンサ１０１−１、回転台１０２−１、クローラ１０３−１、アーム１０４−１の機能は図１で説明した構成と同じであるため、その説明を省略する。そして、ロボット筐体１０９−１内に、モータ駆動回路１０６、PC１０７、バッテリー１０８を保持している。

カメラロボット１００−２は、LiDARセンサ１０１−２、回転台１０２−２、クローラ１０３−２、ステレオカメラ１０５−２およびロボット筐体１０９−２を有する。つまり、アームロボット１００−１と比較して、アーム１０４−１の代わりに、ステレオカメラ１０５−２が設けられている。このステレオカメラ１０５−２は、カメラロボット１００−２の車体前方、前向きに取り付けられおり、ステレオ視による周囲の３次元点群の取得や、物体の３次元測量が可能である。なお、その他機能は、図１およびアームロボット１００−１の各構成と同じであるため、その機能の説明は省略する。

ここで、アームロボット１００−１とカメラロボット１００−２とは、互いに連携し、動作する。つまり、カメラロボット１００−２のステレオカメラ１０５−２で撮像した画像に基づき、アームロボット１００−１のアーム１０４−１に搭載されているモータ（例えば、サーボモータ）を駆動し、画像内の物体を掴む等の動作を行う。ロボットの位置関係により、自身のLiDARセンサに他のロボットが映りこんでしまうと、それがノイズとなりロボットの正確な位置推定ができなくなるおそれがある。例えば、アームロボット１００−１がカメラロボット１００−２の前方に位置している場面では、アームロボット１００−１のLiDARセンサ１０１−１の照射方向を、アームロボット１００−１の位置およびカメラロボット１００−２の位置情報に基づいて、カメラロボット１００−２の映り込み率に従った制御を行う。

これら各ロボットおよびサーバ１１０の機能の詳細について、引き続き図３を用いて説明する。なお、ここでは、図１と同様に、ロボットをアームロボット１００−１とカメラロボット１００−２に区別しない形で説明する。また、本実施例では、３台のロボット１００とサーバ１１０で構成されるシステムを例に説明する。

ここで、各ロボット１００とサーバ１１０は、それぞれ図示した各種プログラムを有する。これら各プログラムは、OS／ミドルウェア３００、３１０上で演算を行う。また、本実施例では、ミドルウェアとして、ロボット制御用ミドルウェア（ROS:Robot Operating System）を用いる。ROSを用いた各プログラムは、トピック（ROSトピック）と呼ばれる名前付きのデータを送受信し合うことで処理を行う。具体的には、プログラムの入力データをトピックとして受け取り、処理結果をトピックとして出力する。また、プログラム間は相互にトピックの送受信を繰り返す。このように、ROSでは、分散処理のアーキテクチャを採用している。

次に、ロボット１００およびサーバ１１０それぞれのソフトウェア構成を説明する。まず、ロボット１００は、それぞれPC107で用いるプログラムとして、LiDARセンサデータ送信プログラム１０１０、回転台駆動プログラム１０２０、クローラ駆動プログラム１０３０、アーム駆動プログラム１０４０、ステレオカメラ画像送信プログラム１０５０を有する。これらは、図１に示すストレージ１０７３ないしメモリ１０７２に格納され、CPU１０７１でその内容に従った演算が実行される。

また、これらは、サーバ１１０や他のロボット１００との通信した結果も用いて、演算される。

そして、LiDARセンサデータ送信プログラム１０１０は、LiDARセンサ１０１で検知した計測データを取得し、サーバ１１０など外部に送信するプログラムであり、そのROSトピックは以下のとおりである。
入力トピック：なし
出力トピック：LiDARセンサ１０１での計測データ
回転台駆動プログラム１０２０は、モータ駆動回路１０６に対して制御信号を出力し、回転台１０２の回転を制御するプログラムであり、そのROSトピックは以下のとおりである。
入力トピック：回転台１０２の向き
出力トピック：なし
クローラ駆動プログラム１０３０は、モータ駆動回路１０６に対して制御信号を出力し、クローラ１０３の駆動を制御するプログラムであり、そのROSトピックは以下のとおりである。また、クローラ１０３の駆動には、ロボット１００の移動量と回転角度を左右のクローラ１０３の回転量に変換し、左右のクローラ１０３を回転させることが含まれる。
入力トピック：ロボット１００の移動量と回転角度
出力トピック：なし
アーム駆動プログラム１０４０は、モータ駆動回路１０６に対して制御信号を出力し、アーム１０４の駆動(例えば、関節の回転、移動)を制御するプログラムであり、そのROSトピックは以下のとおりである。
入力トピック：アーム１０４関節の回転角
出力トピック：なし
ステレオカメラ画像送信プログラム１０５０は、ステレオカメラ１０５で撮像された画像を取得し、サーバ１１０など外部に送信するプログラムであり、そのROSトピックは以下のとおりである。
入力トピック：なし
出力トピック：ステレオカメラ１０５の画像
なお、アームロボット１００−１ではステレオカメラ画像送信プログラム１０５０を、カメラロボット１００−２ではアーム駆動プログラム１０４０の保持をそれぞれ省略してもよい。

次に、サーバ１１０のソフトウェア構成を説明する。サーバ１１０は、プログラムとして、点群地図管理プログラム１１０１、自己位置推定プログラム１１０２、回転台制御プログラム１１０３、経路生成プログラム１１０４、クローラ制御プログラム１１０５、アーム制御プログラム１１０６、ステレオマッチングプログラム１１０７および画面表示プログラム１１０８を有する。これらは、図１に示すストレージ１１３ないしメモリ１１２に格納され、CPU１１１でその内容に従った演算が実行される。

点群地図管理プログラム１１０１は、ストレージ１１３に格納された点群地図のうち、ロボット１００の移動空間における点群地図を管理する。この管理には、他のプログラム（含むロボット１００のプログラム）への点群地図の配信が含まれる。なお、点群地図とは、無数の点(ｘ,ｙ,ｚ)で示される点群により構成された地図である。また、本プログラムでのROSトピックは以下のとおりである。
入力：ファイルに格納された点群地図
出力トピック：点群地図
自己位置推定プログラム１１０２は、ロボットの位置を推定する処理を実行するプログラムである。その内容は、点群地図と、LiDARセンサ１０１からの計測データ（点群）とを、スキャンマッチングと呼ばれる手法を用いて照合することで行う。スキャンマッチングアルゴリズムとして、NDT（Normal Distributions Transform）やICP(Iterative Closest Point)等を用いることが可能である。これらにおいては、短い時間間隔で取得した点群A、点群B、…、点群Zの位置合わせを行うことで、点群A、点群B、…、点群Zそれぞれに対するロボットの相対位置・姿勢を推定することになる。また、この技術を用いることで、点群地図を作成できる。さらに、NDTでは、地図空間をボクセル(立方体)に区切り、ボクセル毎の点の集合を正規分布で近似する。このため、計算量はセンサから得られる点群のサイズに比例し地図の大きさに依存しない処理ができる。

なお、これらスキャンマッチングアルゴリズムにおける際の出力トピックのロボットの位置と姿勢は、位置（x,y,z）とクォータニオンと呼ばれる姿勢パラメータ(qx,qy,qz,qw)からなる、７つのパラメータで表現される。なお、本プログラムでのROSトピックは以下のとおりである。
入力トピック：点群地図
入力トピック：LiDARセンサ１０１の計測データ
出力トピック：ロボット１００の位置と姿勢
回転台制御プログラム１１０３は、回転台１０２に乗ったLiDARセンサ１０１−１あるいは１０１−２の方向を指示するプログラムある。回転台制御プログラム１１０３は、センサLiDAR１０１の照射範囲内に存在する他のロボット１００の映り込み率を算出し、それが低くなるような(例えば、一定値以下や算出される映り込み率が最小)、回転台１０２の向きを求めて送信する。ここで、回転台１０２の向きは、後述するように最小映り込み率角度として算出される。なお、本プログラムの処理の詳細（映り込み率の算出）についても後述する。送信された回転台１０２の向きは、前述した、回転台駆動プログラム１０２０で受信され、回転台１０２の向きへと反映される。本プログラムでのROSトピックは以下のとおりである。
入力トピック：ロボット１００の位置と姿勢
出力トピック：回転台１０２の向き（最小映り込み率角度）
経路生成プログラム１１０４は、点群地図上でロボット１００の移動経路を求める。移動経路を求めるため、まずロボット１００の目的地を入力デバイス１４０からの指定を受け付ける(例えば、マウスクリック)。そして、現在位置から目的値までの経路を１０ｃｍ等の一定間隔の点でつないだリストとして出力する。また、点群地図上、点が一定以上密集している場所をロボット１００が通行不能な障害物（壁）とみなし、障害物がない空間を対象に、ダイクストラ法、エースター法などのアルゴリズムを用い経路を求める。なお、本プログラムでのROSトピックは以下のとおりである。
入力トピック：点群地図
入力トピック：ロボット１００の位置と姿勢
入力トピック（マウスクリック等の受け付け）：目的地
出力トピック：経路（１０ｃｍ間隔の点のリスト）
クローラ制御プログラム１１０５は、現在のロボット１００の位置・姿勢から次の点に向かわせるのに必要なロボット１００の移動量と回転角度を求める。つまり、クローラ１０３の動作を制御するために必要な情報を求める。なお、ロボット１００の移動量と回転角度は、ロボット１００のクローラ駆動プログラム１０３０で出力される制御信号を生成するために用いられる。また、クローラ制御プログラム１１０５が制御信号を生成したり、ロボット１００の移動量と回転角度に基づく制御指令を生成したりしてもよい。なお、ロボット１００の駆動方式は、クローラに限定されないため、クローラ制御プログラム１１０５は移動制御プログラムとも称することが可能である。また、本プログラムでのROSトピックは以下のとおりである。
入力トピック：経路
入力トピック：ロボットの位置と姿勢
出力トピック：ロボットの移動量と回転角度
アーム制御プログラム１１０６は、入力デバイス１４０の例えば、Joystickを用いた、アーム１０４の関節角度の指定を受け付け、その回転角を算出する。なお、本プログラムでのROSトピックは以下のとおりである。
入力トピック：ジョイスティックからの入力情報
出力トピック：アーム関節の回転角
ステレオマッチングプログラム１１０７は、ステレオカメラ１０５の画像からステレオマッチングアルゴリズムを用いて３D点群を生成する。なお、本プログラムでのROSトピックは以下のとおりである。
入力トピック：ステレオカメラ１０５の画像
出力トピック：３D点群
画面表示プログラム１１０８は、ディスプレイ１３０の画面に、点群地図、ロボット１００の位置・姿勢、経路、回転台１０２の向き、３D点群などの情報を重畳表示する。なお、表示する情報はこれらに限定されず、また、表示の形式も重畳表示に限定されない。また、本プログラムでのROSトピックは以下のとおりである。
入力トピック：点群地図
入力トピック：ロボット１００の位置・姿勢
入力トピック：経路
入力トピック：回転台１０２の向き
入力トピック：３D点群
出力トピック：なし
なお、上述のロボット１００、サーバ１１０の各プログラムは１つのプログラムとして構成し、それぞれをプログラムモジュールとしてもよい。さらに、サーバ１１０の機能（プログラム）を、ロボット１００の少なくとも１つに設け、自律的にロボット１００が他のロボットも含め制御してもよい。

次に、図８を用いて、本実施例の全体の処理シーケンスを説明する。ここでは、アームロボット１００−１における回転台１０２の制御を例に説明する。

まず、ステップＳ８０１、Ｓ８０２において、アームロボット１００−１、カメラロボット１００−２、３のLiDARのセンサそれぞれの計測値を無線ルータ１２０を介して、サーバ１１０に送信する。LiDARセンサの計測値とは、周囲形状の点群情報である。サーバ１１０では、Ｓ８０３においてこれら受信した点群情報と、点群地図管理プログラム１１０１内の点群地図とを、スキャンマッチングアルゴリズムを用いて照合することで、アームロボット１００−１、カメラロボット１００−２，３の自己位置推定結果を得る。

次に、ステップＳ８０４において、サーバ１１０が映り込み率を算出する。この算出方法の詳細は、図４を用いて後述する。

次に、ステップＳ８０５において、サーバ１１０が映り込み率を用いて、最小映り込み率角度を算出する。そして、ステップＳ８０６において、サーバ１１０が求められた最小映り込み率角度ないし回転台１０２に対する当該角度への制御指令を、アームロボット１００−１に送信する。これを受けて、ステップＳ８０７において、アームロボット１００−１では回転台１０２を回転させる。つまり、アームロボット１００−１のLiDARセンサ１０１をカメラロボット１００−２、３が映り込みを低下させる方向へ向けることになる。これらステップＳ８０５〜Ｓ８０７の詳細は、図５を用いて後述する。

なお、図８では、アームロボット１００−１についての処理だが、カメラロボット１０−２、３のそれぞれについても同様の処理を実行する。

次に、図４を用い、図１〜３を適宜参照しながら、映り込み率の算出について説明する。つまり、図８のステップＳ８０４の詳細を説明する。この算出は、回転台制御プログラム１１０３により実行される。

ここで、映り込み率の考え方について、まず説明する。図４に示すように、複数のロボット（１００−１〜３）が同じ空間に存在して動作させる場合、以下の課題がある。ロボットの位置推定に用いる地図データの作成の際に、その空間に配置されていなかったロボットがいるという状況は、照合失敗の原因となる。これは、地図データ作成の際と、ロボットの位置推定の際に矛盾を生じるノイズが存在するため、マッチングが失敗する可能性が高いことを示す。そこで、他のロボットが配置されているという状況が、観測されなければノイズを除去することが可能になる。つまり、他のロボットが観測されなければ照合されることもなく、ノイズとなることもない。そこで、他のロボットがいない方向（存在する可能性が低い）にLiDARセンサ１０１を向けて得られた点群に対して地図データと照合を行うこととした。

本実施例では、この他のロボットがいない方向を判定するために、映り込み率を評価指標として用いることとした。ここで、映り込み率とは、照射範囲で検知ないし映り込む確率である。例えば、照射範囲に１度ずつの視線を設定した場合、他のロボットがヒットする視線数÷視線全体の視線数で求められる。

以下、映り込み率の算出の手順を説明する。なお、本手順では、アームロボット１００−１のLiDARセンサ１０１−１での計測データを用い、カメラロボット１００−２、３の映り込み率を算出することを例示して説明する。但し、カメラロボット１００−２や１００−３におけるアームロボット１００−１や他のカメラロボット１００−３や１００−２に対する映り込み率および図５に示す最小映り込み率の算出を、それぞれが独立ない並行して実行してもよい。

なお、図４に示す事例は、何らかの方法を用いて、点群地図をあらかじめ作成しておき、この結果を用いてアームロボット１００−１の稼働を制御することを想定している。何らかの方法としては、アームロボット１００−１、カメラロボット１００−２、３のいずれかがによって予め地図を作成しておいても良い。また、別のロボットでの作成や人手での作業に基づき作成してもよい。

まず、本算出における前提処理について説明する。サーバ１１０の自己位置推定プログラム１１０２（図３）おいて、点群地図管理プログラム１１０１（図３）から配信される点群地図とLiDARセンサ１０１−１（図２）から得られた点群の形状を照合する。そして、最もマッチする場所を、アームロボット１００−１（図２）の位置として出力する。

ここで、映り込み率の算出で理由される図４に示す空間のLiDARセンサ座標系を説明する。LiDARセンサ座標系とは、図４のようにLiDARセンサ１０１−１の中心を原点、正面をX軸、左側９０度の位置をY軸とする座標系と定義する。
（処理１）
次に、回転台制御プログラム１１０３は、点群地図管理プログラム１１０１で配信される点群地図を座標変換し、LiDAR座標系上に、オブジェクトを描画する。ここでの描画されるオブジェクトは、図４の壁オブジェクト４２０と柱オブジェクト４２１である。なお、本実施例での描画、線を引くなどは、可視化しなくともよく、シミュレーションでの演算上の処理であればよい。

次に、回転台制御プログラム１１０３は、各カメラロボット１００−２、３の現在位置を座標変換して、LiDAR座標系上にオブジェクトとして描画する。ここでのオブジェクトは、図４に示すカメラロボット１００−２のオブジェクト４０１（以下、カメラロボットオブジェクト４０１）およびカメラロボット１００−３のオブジェクト４０２（以下、カメラロボットオブジェクト４０２）である。

その際、ロボットの上面積に相当する面積を持った円をロボットの射影として描画する。なお、本オブジェクトの形状は、円以外の矩形、楕円などとしてもよい。

このように、オブジェクトを描画するために、回転台制御プログラム１１０３は、アームロボット１００−１や各カメラロボット１００−２の位置情報を自己位置推定プログラム１１０２より取得する。

ここで、回転台制御プログラム１１０３は、LiDARセンサ１０１−１の向きによっては、自身のアーム１０４−１がセンサに映ってしまうことがある。そこで、アーム１０４−１の付け根の位置を座標変換し、LiDAR座標系上に、当該アーム１０４−１のオブジェクトを描画する。ここでのオブジェクトは、図４に示すアームロボット１００−１のアーム１０４−１のオブジェクト４０３（以下、アームオブジェクト４０３）である。その際、アーム断面積に相当するする面積を持った円をアームの射影として描画する。なお、本オブジェクトの形状は、円以外の矩形、楕円などとしてもよい。
（処理２）
回転台制御プログラム１１０３は、LiDARセンサ１０１−１のレーザ照射を計算機上でシミュレートして、センサ照射範囲のうち、カメラロボットオブジェクト４０１、４０２それぞれが映り込む割合を求める。

ここで、本実施例のLiDARセンサ１０１−１のレーザ照射角度がＸ軸に対して−８０度〜＋８０度の範囲と仮定する。そして、回転台制御プログラム１１０３は、−８０度から＋８０度まで１度刻みで原点から放射状に線を引いていく。１６１本の線が引ける。この線は、図４に示す破線矢印（４０４）である。この１６１本の線に対し、処理１で描いたオブジェクト（４０１、４０２、４０３、４２０、４２１）に対して衝突判定を行う。衝突判定は、衝突したら１、しなかったら０とする。また、衝突判定とは、破線矢印（４０４）に、各オブジェクトのいずれかが座標上で重なるかを判定する。各破線矢印（４０４）がカメラロボット１００−２、３の到達する前に、点群（壁オブジェクト４２０や柱オブジェクト４２１）にぶつかった場合は、ロボットに衝突しない（０）と判定する。なお、衝突判定の対象には、アームオブジェクト４０３を含めないでもよい。

この判定を１６１本分行った後、回転台制御プログラム１１０３は、１６１本の衝突判定のうち衝突する（１）の総和を求め、それを１６１で割った値を映り込み率として算出する。

ここで、アームロボット１００−１には回転台１０２−１が設けられ、その上にLiDARセンサ１０１−１が設置されている。この回転台１０２−１の角度に応じて、映り込み率は異なってくる。回転台１０２−１の各角度（例えば、１度ずつの全角度）に対して、映り込み率を求めることで、最も映り込み率が小さくなる回転台１０２−１の角度（最小映り込み率角度）を求めることができる。なお、衝突判定を１６１本分、各角度について実行するため、直前の衝突判定の結果を用いてもよい。例えば、１度〜１６０度までの衝突判定を行った後に、２度〜１６１度の衝突判定を行うと、２度〜１６０度の各衝突判定が重複する。このため、これらは直前の判定結果を用い、次は１６１度分の衝突判定を行えばよい。また、本手法は直前だけでなく、それ以前の判定結果との重複（最大公約）部分の結果を流用してもよい。

このように、計算することで、回転台１０２−１を得られた角度にセットすることで、カメラロボット１００−２、３ロボットの存在による照合処理に与える影響を排除、または最小化できる。

このため、回転台制御プログラム１１０３は、図５に示すフローチャートに従って、最小映り込み率角度を求める（図８のステップＳ８０５の処理）。以下ではアームロボット１００−１における最小映り込み率角度を求める例を示す。但し、上述したように、カメラロボット（１００−２や１００−３）が、アームロボット１００−１や他のカメラロボット（１００−３や１００−２）に対する最小映り込み率の算出を、それぞれが独立ない並行して実行してもよい。

まず、ステップＳ５００において、回転台制御プログラム１１０３は、回転台１０２−１の回転範囲をその一例として、−９０度〜＋９０度と設定する。

そして、ステップＳ５０１において、回転台制御プログラム１１０３は、回転角度を変数(i)(以下、単に角度(i))として、−９０度〜＋９０度まで１度刻みでループさせる。つまり、−９０度〜＋９０度の範囲外（No）であれば処理を終了し、範囲内（Yes）であればステップＳ５０２以降の処理を繰り返す。

次に、ステップＳ５０２において、回転台制御プログラム１１０３は、夫々の角度(i)について、それぞれ映り込み率（ratio）、つまり、個別映り込み率を求める。

そして、ステップＳ５０３において、回転台制御プログラム１１０３は、それまで求められた映り込み率の最小値（min_ratio）とステップＳ５０２で求められた個別映り込み率（ratio）を比較する。この結果、最小値（min_ratio）が小さい場合（No）、ステップＳ５０５に進む。最小値（min_ratio）が大きい場合（Yes）、ステップＳ５０４に進む。

そして、ステップＳ５０３において、回転台制御プログラム１１０３は、ステップＳ５０２での角度(i)の値をmin_iとして保持する。また、この際、ステップＳ５０２で求められた個別映り込み率（ratio）をmin_iと対応付けて保持する。なお、ステップＳ５０３での保持は、メモリ１１２に保持する。

次に、ステップＳ５０５において、回転台制御プログラム１１０３は、角度の変数(i)に１加え、ｉ＋１としてステップＳ５０１へ戻る。

そして、回転台制御プログラム１１０３は、Endの時点の min_iに示される回転台１０２−１の向き（角度）を最小映り込み率角度として、特定する。

ここで、最小映り込み率角度の別案について説明する。映り込み率が十分小さければ、ロボットが映っていても、ロボットの位置推定における影響は軽微といえる。このため、予め映り込み率の閾値をストレージ１１３に格納しておき、ステップＳ５０２で求めた映り込み率が、閾値を超えていなければ、ステップＳ５０２での角度(i)を最小映り込み率角度とする。

また、閾値を超えていれば、その時点のステップＳ５０２での角度(i)の近傍から探索していく。例えば、角度(i)に対して、＋1度、−１度、＋２度、−２度と探索していき、映り込み率が閾値を満たした時点の角度を、最小映り込み率角度とする。このようにすることで、回転台１０２−１を回転量や頻度あるいは最小映り込み率角度を算出するための計算量を抑止できる。さらに、回転台１０２を備えないロボットについても、実施例１を適用することが可能である。この場合、角度(i)を−０度〜＋０度と設定し、上記の処理を実行する。

以上で、図５の説明を終了するが、本実施例では、引き続き以下の処理を行う。回転台制御プログラム１１０３は、ネットワーク通信部１１４を利用し、無線ルータ１２０を介して、最小映り込み率角度を、アームロボット１００−１に送信する。アームロボット１００−１の回転台駆動プログラム１０２０は、最小映り込み率角度に従って制御信号を生成する。そして、モータ駆動回路１０６は制御信号に従って、回転台１０２−１のモータを、最小映り込み率角度の示す角度に制御する。

次に、LiDARセンサデータ送信プログラム１０１０はLiDARセンサ１０１より得られた計測データを、ネットワーク通信部１０７４を利用し無線ルータ１２０を介して、サーバ１１０に送信する。

サーバ１１０では、自己位置推定プログラム１１０２により、上述のスキャンマッチング手法などを用いて、アームロボット１００−１の位置を推定する。以降、この位置に基づいて、アームロボット１００−１やカメラロボット１００−２、３の移動を制御する。

上述の処理により、アームロボット１００−１、カメラロボット１００−２およびカメラロボット１００−３のそれぞれで、算出された回転角度でLiDARセンサ１０１により照射された状況を図６に示す。アームロボット１００−１は、瓦礫６１１をアーム１０４−１でつかもうとする状況である。２台のカメラロボット１００−２、３が、アームロボット１００−１の両サイドに配置されている。この配置において、各ロボット１００のLiDARセンサ１０１の照射範囲は、６０１に示す範囲となる（グレー地と破線矢印）。また、図６に示すように、各ロボット１００は、互いにLiDARセンサ１０１の照射範囲６０１から外れていることがわかる。これは、上述の処理により、各LiDARセンサ１０１の照射範囲外としつつ、以下の制御を行う。各カメラロボット１００−２、３がアームロボット１００−１の作業風景をステレオカメラ１０５で映る場所に移動し、その車体を瓦礫６１１やアームロボット１００−１の方向へ向けていることで実現する。その上で、カメラロボット１００−２、３のLiDARセンサ１０１を、お互いの車体が映りこまない方向へ向けている。なお、本例における移動経路の算出については、実施例２で説明する。

次に、実施例２として、ロボットの移動経路を算出する方法を説明する。実施例２では、実施例１で算出した最小映り込み率角度を利用して、各ロボット１００の移動経路を算出する。このため、各ロボット１００の構成および最小映り込み率角度の算出は、実施例１と同じである。そこで、これらの説明は省略し、移動経路算出に絞って説明する。

図７は、各ロボット１００の移動経路を示す図である。１００−１ｃ、１００−２ｃ、１００−３ｃがそれぞれアームロボット１００−１、カメラロボット１００−２、カメラロボット１００−３の移動における終点（作業位置）を示す。これは、図６に示す各ロボットの位置に対応している。ここに対し、アームロボット１００−１、カメラロボット１００−２、カメラロボット１００−３それぞれの移動の出発点（現在位置）である１００−１ａ、１００−２ａ、１００−３ｃからの経路を算出する。各経路は、１００−１ｂ、１００−２ｂ、１００−３ｂで表している。つまり、各ロボット１００は、作業のために、出発始点から終点へ移動する必要がある。ここで、単に移動すると、各ロボット１００の移動順やその経路によっては、映り込み率が上昇することがあり得る。そこで、本実施例では、映り込み率を考慮した移動経路、特にロボット１００の移動順の算出を行う。以下、その内容を説明する。

まず、処理の前提として、処理ステップを単位時間ごとに行い、各ロボット１００は単位時間において、移動に関して現在位置から「進む」「留まる」のいずれかの動作を行うとする。また、「進む」については、「終点」への方向に、予め定めた単位距離分進むとする。

以下、図９を用いて、本実施例の処理フローを説明する。

まず、ステップＳ９０１において、サーバ１１０の経路生成プログラム１１０４が、夫々のロボット１００を進めるか、留まらせるかの動作を決定するための乱数を発生する。

次に、ステップＳ９０２において、経路生成プログラム１１０４が、発生した乱数により各ロボットが「留まる」ことを示すかを判定する。この結果、「留まる」ことを示す（Yes）の場合、ステップＳ９０１に戻り、再度乱数を発生させ、引き直す。また、Noの場合は、ステップＳ９０３に進む。

次に、ステップＳ９０３において、経路生成プログラム１１０４は、各ロボット１００の映り込み率(min_ratio)を算出する。これは、実施例１（図４）に示す方法で算出する。

そして、ステップＳ９０４において、経路生成プログラム１１０４は、各ロボット１００の映り込み率(min_ratio)のそれぞれを足し合わせて、映り込み率の合算値(ratio_sum)を算出する。

ステップＳ９０５において、経路生成プログラム１１０４は、この処理を複数回繰り返し実行し、その中で、ratio_sumが最も小さくなる、各ロボット１００の移動順序を採用する。この複数回とは、所定の回数としてもよいし、各ロボットの移動（「進む」「留まる」）の組み合わせの数としてもよい。

そして、図９に示す処理フローを、ロボットが終点に到着するまで実行する。つまり、ステップＳ９０６において、経路生成プログラム１１０４は、ロボットが終点に到着したかを判断し、到着した（Yes）の場合は処理を終了し、到着していない場合はステップＳ９０１に戻る。なお、図９のフローチャートでは、ロボットが終点に到着するまで実行したが、一定回数の処理ステップ（単位時間）分を処理してもよい（経路の途中まで）。この場合、一定回数の処理ステップ（単位時間）分の処理を積み上げて、終点までの処理を行うことがより好適である。

ここで、終点に到着するとは、いずれのロボットが最も早く到着するまでとしてもよいし、各ロボットが終点に到着するまで実行するとしてもよい。後者の場合、先行して終点に到着したロボットについては、上記の経路生成プログラム１１０４の処理の対象から外す、つまり、未着のロボットに対して実行することはより望ましい。この場合、終点に到着したロボットについては、乱数の代わりに「留まる」となるように制御してもよい。

以上のように、実施例２では、どのロボットを「進む」とし、どのロボットを「留まる」とするかを決定していく。このことにより、各処理ステップ（単位時間）において「進む」のロボットの移動順序が「留まる」のロボットより移動順序が先になる。また、各処理ステップの結果の積み重ねにより、ロボット１００の移動順序も全体として特定できる。

さらに、実施例２は、組み合わせ最適化問題と捉えることが可能である。このため、高速に解くための様々な方式を採用できる。例えば、遺伝学的アルゴリズムを用いることを採用できる。

さらに、回転台１０２乃至回転機能を有さないロボットについても、本実施例を適用することが可能である。これは、実施例１でも言及したように、回転台１０２の回転角度範囲における角度(i)を−０度〜＋０度（回転範囲０度）と設定して、ロボットの映り込み率の合算値（ratio_sum）を算出することで対応できる。

以上で、実施例１および２の説明を終了する。なお、実施例１および２では、サーバ１１０が各処理を実行していたが、ロボット１００で実行してもよい。この場合、各ロボットから代表ロボットを特定して、当該ロボットでサーバ１１０の処理（実施例１、２）を実行することが好適である。その際、ロボット１００に搭載したPC１０７で実行される。

さらに、実施例１、２は、狭義のロボット以外の移動体（車両等）や、自身では移動機能を有さず他装置により移動させる物体にも適用可能である。これらの場合でも、各移動体は、位置推定を行う機能を有することが望ましい。

１００…ロボット、１０１…LiDARセンサ、１０２…回転台、１０３…クローラ、１０４…アーム、１０５…ステレオカメラ、１０６…モータ駆動回路、１０７…PC、１０８…バッテリー、１１０…サーバ、１１１…CPU、１１２…メモリ、１１３…ストレージ、１１４…ネットワーク通信部、１１５…電源、１２０…無線ルータ、１３０…ディスプレイ、１４０…入力デバイス

Claims (15)

1. 複数のロボットが連携することで、所定の機能を実行するロボットシステムにおいて、
   センサを有する第１のロボット、第２のロボットおよび、前記第１のロボットおよび前記第２のロボットと通信回線を介して接続されるサーバを有し、
   前記サーバが、
   地図情報と前記センサで検知された検知情報を照合して、前記第１および前記第２のロボットの位置を推定する位置推定部と、
   前記第１のロボットおよび前記第２のロボットの位置関係を示す位置関係情報を用いて、前記第２のロボットが前記第１のロボットのセンサの照射範囲で検知ないし映り込む確率を示す映り込み率を算出し、算出された前記映り込み率に応じて、前記第１のロボットのセンサの照射方向を特定するセンサ方向制御部とを有することを特徴とするロボットシステム。
2. 請求項１に記載のロボットシステムにおいて、
   前記センサ方向制御部は、
   複数の照射方向それぞれにおいて、前記第２のロボットが前記照射範囲において検知ないし映り込む個別映り込み率を求め、求められた前記個別映り込み率から前記映り込み率を算出することを特徴とするロボットシステム。
3. 請求項２に記載のロボットシステムにおいて、
   前記センサ方向制御部は、
   前記映り込み率として、前記個別映り込み率のうち最も小さい個別映り込み率を用い、
   前記センサの照射方向として、前記最も小さい個別映り込み率の照射方向である最小映り込み率角度を用いることを特徴とするロボットシステム。
4. 請求項１に記載のロボットシステムにおいて、
   前記サーバは、
   さらに、前記第２のロボットを、前記センサの照射方向での照射範囲を特定する情報を用いて、前記照射範囲を避けて移動する移動方向を特定する移動制御部を有することを特徴とするロボットシステム。
5. 請求項３に記載のロボットシステムにおいて、
   前記センサ方向制御部は、前記第１のロボットおよび前記第２のロボットそれぞれの最小映り込み率角度を算出し、
   前記サーバは、さらに、算出された各最小映り込み率角度を足し合わせた結果を用いて、前記第１のロボットおよび前記第２のロボットの移動順序を決定する経路生成部を有することを特徴とするロボットシステム。
6. ロボットシステムを構成する少なくとも１つのロボットの位置を推定するロボットシステムでの位置推定方法において、
   前記ロボットシステムは、センサを有する第１のロボット、第２のロボットおよび、前記第１のロボットおよび前記第２のロボットと通信回線を介して接続されるサーバを有し、
   前記サーバによる、
   地図情報と前記センサで検知された検知情報を照合して、前記第１および第２のロボットの位置を推定するステップと、
   前記第１のロボットおよび前記第２のロボットの位置関係を示す位置関係情報を用いて、前記第２のロボットが前記第１のロボットのセンサの照射範囲で検知ないし映り込む確率を示す映り込み率を算出するステップと、
   算出された前記映り込み率に応じて、前記第１のロボットのセンサの照射方向を特定するステップを有することを特徴とするロボットシステムでの位置推定方法。
7. 請求項６に記載のロボットシステムでの位置推定方法において、
   前記サーバによる、
   複数の照射方向それぞれにおいて、前記第２のロボットが前記照射範囲において検知ないし映り込む個別映り込み率を求め、求められた前記個別映り込み率から前記映り込み率を算出するステップを有することを特徴とするロボットシステムでの位置推定方法。
8. 請求項７に記載のロボットシステムでの位置推定方法において、
   前記映り込み率として、前記個別映り込み率のうち最も小さい個別映り込み率を用い、
   前記センサの照射方向として、前記最も小さい個別映り込み率の照射方向である最小映り込み率角度を用いることを特徴とするロボットシステムでの位置推定方法。
9. 請求項６に記載のロボットシステムでの位置推定方法において、
   前記サーバによる、
   前記第２のロボットを、前記センサの照射方向での照射範囲を特定する情報を用いて、前記照射範囲を避けて移動する移動方向を特定するステップを有することを特徴とするロボットシステムでの位置推定方法。
10. 請求項８に記載のロボットシステムでの位置推定方法において、
    前記サーバによる、
    前記第１のロボットおよび前記第２のロボットそれぞれの最小映り込み率角度を算出するステップと、
    算出された各最小映り込み率角度を足し合わせた結果を用いて、前記第１のロボットおよび前記第２のロボットの移動順序を決定するステップを有することを特徴とするロボットシステムでの位置推定方法。
11. 複数のロボットが連携することで、所定の機能を実行するロボットシステムを構成するサーバ装置において、
    前記ロボットシステムを構成するセンサを有する第１のロボットおよび第２のロボットとネットワークを介して接続するネットワーク接続部と、
    地図情報と前記センサで検知された検知情報を照合して、前記第１および第２のロボットの位置を推定する位置推定部と、
    前記第１のロボットおよび前記第２のロボットの位置関係を示す位置関係情報を用いて、前記第２のロボットが前記第１のロボットのセンサの照射範囲で検知ないし映り込む確率を示す映り込み率を算出し、算出された前記映り込み率に応じて、前記第１のロボットの前記センサの照射方向を特定するセンサ方向制御部とを有することを特徴とするサーバ装置。
12. 請求項１１に記載のサーバ装置において、
    前記センサ方向制御部は、
    複数の照射方向それぞれにおいて、前記第２のロボットが前記照射範囲において検知ないし映り込む個別映り込み率を求め、求められた前記個別映り込み率から前記映り込み率を算出することを特徴とするサーバ装置。
13. 請求項１２に記載のサーバ装置において、
    前記センサ方向制御部は、
    前記映り込み率として、前記個別映り込み率のうち最も小さい個別映り込み率を用い、
    前記センサの照射方向として、前記最も小さい個別映り込み率の照射方向である最小映り込み率角度を用いることを特徴とするサーバ装置。
14. 請求項１１に記載のサーバ装置において、
    さらに、前記第２のロボットを、前記センサの照射方向での照射範囲を特定する情報を用いて、前記照射範囲を避けて移動する移動方向を特定する移動制御部を有することを特徴とするサーバ装置。
15. 請求項１３に記載のサーバ装置において、
    前記センサ方向制御部は、前記第１のロボットおよび前記第２のロボットそれぞれの最小映り込み率角度を算出し、
    さらに、算出された各最小映り込み率角度を足し合わせた結果を用いて、前記第１のロボットおよび前記第２のロボットの移動順序を決定する経路生成部を有することを特徴とするサーバ装置。

Images