JP2021077003A - 走破可否判定装置及び移動体の制御装置 - Google Patents

Abstract

【課題】移動体の周辺の路面環境が走破可能であるか否かを適切に判定することができる走破可否判定装置などを提供する。【解決手段】コントローラは、ステレオカメラからの路面画像に基づき、点群データを取得し、セル４０の一辺の長さｄを、最大傾斜角度θｍａｘ及び最大高さｈｍａｘを用いて、ｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように決定し、点群データに基づき、所定の平面推定方法により、推定平面４２の式をセル４０毎に取得し、推定平面４２に基づき、路面環境が走破可能であるか否かをセル４０毎に判定する。【選択図】図６

Description

本発明は、移動体の周辺の路面環境が走破可能であるか否かを判定する走破可否判定装置などに関する。

従来、移動体の進行方向における障害物などを検出する障害物検出装置として特許文献１に記載されたものが知られている。この障害物検出装置は、清掃ロボットに搭載されたものであり、距離画像センサ及び演算装置などを備えている。この障害物検出装置では、距離画像センサの検出信号に基づいて、距離画像を取得し、この距離画像から、３次元座標データ及び投影画像が取得される。さらに、この投影画像から平面が検出され、３次元座標データの各点と平面との距離に基づいて、清掃ロボットの進行方向における障害物又は段差が検出される。

国際公開第２０１４／０６４９９０号

上記従来の障害物検出装置によれば、距離画像センサの検出信号に基づいて、３次元座標データ及び投影画像を取得している関係上、距離画像内で路面状況が頻繁に変化するような環境下、例えば多数の凹凸又は多数の障害物が存在するような路面環境下では、路面環境を適切に反映した平面を取得することができないという問題がある。したがって、この障害物検出装置の検出結果を用いた場合、移動体の周辺の路面環境が走破可能であるか否かを適切に判定できなくなってしまう。

本発明は、上記課題を解決するためになされたもので、移動体の周辺の路面環境が走破可能であるか否かを適切に判定することができる走破可否判定装置などを提供することを目的とする。

上記目的を達成するために、請求項１に係る発明は、移動体（ロボット装置２）の周辺の路面環境が走破可能であるか否かを判定する走破可否判定装置１であって、移動体（ロボット装置２）の周辺の路面環境を表す路面画像（距離画像データ）を取得する路面画像取得部（ステレオカメラ６）と、路面画像に基づき、路面画像内の路面及び物体を表す点群データ３２を、移動体（ロボット装置２）を基準とする３次元座標系のデータとして取得する点群データ取得部（コントローラ１０、ＳＴＥＰ２）と、３次元座標系のうちの所定の２次元座標軸で規定される平面を複数の区画（セル４０）に分割した場合において、複数の区画（セル４０）の各々に対応する２次元座標値を有する点群データ３２に基づき、所定の平面推定方法により、平面（推定平面４２）を区画（セル４０）毎に推定する平面推定部（コントローラ１０、ＳＴＥＰ３）と、平面（推定平面４２）に基づき、路面環境が走破可能であるか否かを区画（セル４０）毎に判定する走行可否判定部（コントローラ１０、ＳＴＥＰ５）と、を備え、平面推定部は、移動体（ロボット装置２）が登坂可能な斜面の最大傾斜角度θｍａｘと、移動体（ロボット装置２）が踏破可能な障害物の最大高さｈｍａｘとを用いて、区画（セル４０）の移動体（ロボット装置２）から離間する所定方向の長さを決定し、走行可否判定部は、平面（推定平面４２）の傾斜角度が最大傾斜角度θｍａｘ以下であり、かつ点群データ３２が平面（推定平面４２）に関して所定条件を満たしている場合、路面環境が走破可能であると判定することを特徴とする。

この走破可否判定装置によれば、移動体の周辺の路面環境を表す路面画像が取得され、路面画像に基づき、路面画像内の路面及び物体を表す点群データが、移動体を基準とする３次元座標系のデータとして取得される。さらに、３次元座標系のうちの所定の２次元座標軸で規定される平面を複数の区画に分割した場合において、複数の区画の各々に対応する２次元座標値を有する点群データに基づき、所定の平面推定方法により、平面が区画毎に推定される。このように、平面が区画毎に推定されるので、多数の凹凸又は多数の障害物が存在するような路面環境下でも、特許文献１の手法と異なり、平面を実際の路面環境が適切に反映されたものとして推定することができる。

さらに、移動体が登坂可能な斜面の最大傾斜角度と、移動体が踏破可能な障害物の最大高さとを用いて、区画の移動体から離間する所定方向の長さが決定されるので、移動体の登坂能力及び障害物の踏破能力に基づいて、平面を推定する際の区画の所定方向のサイズを適切に設定することができる。これに加えて、平面の傾斜角度が最大傾斜角度以下であり、かつ点群データが平面に関して所定条件を満たしている場合、路面環境が走破可能であると判定される。この場合、区画における点群データと平面との間の関係は、その区画の路面環境が走破可能であるかを適切に表すものであるので、点群データが平面に関して所定条件を満たしているか否かに応じて、路面環境が走破可能であるか否かを区画毎に適切に判定することができる。

請求項２に係る発明は、請求項１に記載の走破可否判定装置１において、区画（セル４０）は、正方形に構成され、正方形の一辺が所定方向に沿うように配置されており、所定方向の長さは、正方形の一辺の長さであり、平面推定部は、一辺の長さをｄとし、最大傾斜角度をθｍａｘとし、最大高さをｈｍａｘとした場合、ｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように、一辺の長さｄを決定することを特徴とする。

この走破可否判定装置によれば、区画は、正方形に構成され、正方形の一辺が所定方向に沿うように配置されており、所定方向の長さは、正方形の一辺の長さになっている。そのため、前述したように、所定の平面推定方法により、平面を区画毎に推定した場合、正方形の区画の、移動体から所定方向に離間した方の一辺側に段差が存在する条件下では、平面が区画の移動体に近い方の一辺と段差の上端とを結ぶ傾斜面として推定される可能性がある（図６参照）。これに対して、この走破可否判定装置によれば、区画の所定方向の長さをｄとし、最大傾斜角度をθｍａｘとし、最大高さをｈｍａｘとした場合、ｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように、所定方向の長さｄが決定される。

したがって、推定された傾斜面の角度をθとし、段差の高さをｈとした際、ｈ＞ｈｍａｘが成立している場合には、θ＞θｍａｘが成立することで、その区画の路面環境が走破不可能であると判定することができる。一方、ｈ≦ｈｍａｘが成立している場合には、θ≦θｍａｘが成立することで、その区画の路面環境が走破可能であると判定することができる。以上のように、平面が正方形の区画の一辺と段差の上端とを結ぶ傾斜面として推定された場合でも、その区画の路面環境が走破可能であるか否かを適切に判定することができる。

請求項３に係る発明は、請求項１又は２に記載の走破可否判定装置１において、所定条件は、点群データ３２と平面（推定平面４２）との距離を区画（セル４０）毎に算出した際の距離Ｄが平面（推定平面４２）を基準とする所定範囲内の領域に含まれる点群データ３２において、点群データ３２と平面（推定平面４２）との符号付き距離の最大値Ｄｍａｘと最小値Ｄｍｉｎとの差分ＤＤが最大高さｈｍａｘ以下であることを特徴とする。

この場合、点群データと平面との距離を区画毎に算出した際の距離が平面を基準とする所定範囲内の領域に含まれる点群データにおいて、点群データと平面との符号付き距離の最大値と最小値との差分は、区画の路面環境の最も高い位置と最も低い位置との間の高さと見なせる。したがって、この走破可否判定装置によれば、点群データと平面との符号付き距離の最大値と最小値との差分が最大高さ以下であるときには、路面環境が走破可能であると判定され、差分が最大高さより大きいときには、路面環境が走破不能であると判定されることになる。すなわち、上記の所定条件を用いることによって、移動体が路面環境の最も高い位置と最も低い位置との間の高さを乗り越えて走破できるか否かを適切に判定することができる。

請求項４に係る発明は、請求項３に記載の走破可否判定装置１において、所定範囲は、点群データ３２と平面（推定平面４２）との距離Ｄの２乗平均平方根をσとした場合において、平面（推定平面４２）から±２σの範囲に設定されていることを特徴とする。

この走破可否判定装置によれば、所定範囲が、点群データと平面との距離の２乗平均平方根をσとした場合において、平面から±２σの範囲に設定されている。この場合、統計学的には、９５％程度の点群データが、平面から±２σの範囲内に存在すると見なすことができるので、所定範囲を平面から±２σの範囲に設定することによって、５％程度の外れ値を除外し、９５％程度の点群データを用いて、路面環境が走破可能であるか否かを区画毎に適切に判定することができる。

請求項５に係る移動体の制御装置１は、請求項１ないし４のいずれかに記載の走破可否判定装置１と、走破可否判定装置１の判定結果に応じて、移動体（ロボット装置２）の移動経路を決定する移動経路決定部（コントローラ１０、ＳＴＥＰ７）と、移動体（ロボット装置２）が移動経路で移動するように、移動体（ロボット装置２）を制御する制御部（コントローラ１０、ＳＴＥＰ８）と、を備えることを特徴とする。

この移動体の制御装置によれば、走破可否判定装置の判定結果に応じて、移動体の移動経路が決定されるので、この移動経路を、路面環境が走破可能であるか否かを適切に判定した結果に応じて決定することができる。さらに、そのような移動経路で移動するように、移動体が制御されるので、移動体を円滑に移動させることができる。

本発明の一実施形態に係る制御装置及び走破可否判定装置を適用したロボット装置の構成を示す斜視図である。 制御装置の電気的な構成を示すブロック図である。 制御装置の機能的な構成を示すブロック図である。 路面環境の一例を示す図である。 点群データの取得例を示す図である。 推定平面の一例を示す図である。 障害物がロボット装置の前方に存在するときの一例を示す図である。 コストマップの作成例を示す図である。 ロボット装置の制御処理を示すフローチャートである。

以下、図面を参照しながら、本発明の一実施形態に係る制御装置及び走破可否判定装置について説明する。なお、本実施形態の制御装置は走破可否判定装置も兼用しているので、以下の説明では、制御装置について説明するとともに、その中で、走破可否判定装置の機能及び構成についても説明する。

図１に示すように、本実施形態の制御装置１は、移動体としてのロボット装置２を制御するものである。このロボット装置２は、４輪の自走車両タイプのものであり、装置本体３と、装置本体３の前後の下側に配置された左右の前輪４，４及び左右の後輪５，５（１つのみ図示）と、装置本体３の前端部の上側に設けられたステレオカメラ６などを備えている。

左右の前輪４，４の各々は、前輪支持部材４ａを介して装置本体３に連結されており、この前輪支持部材４ａによって、水平軸線周りに回転自在に支持されている。また、前輪支持部材４ａは、装置本体３に対して鉛直軸線周り回動自在に取り付けられており、後述するアクチュエータ１４に機械的に連結されている。このアクチュエータ１４によって、前輪支持部材４ａを介して前輪４の角度が変更され、それにより、ロボット装置２の走行方向が変更される。

また、左右の後輪５，５の各々は、装置本体３に水平軸線周りに回転自在に取り付けられており、後述する駆動モータ１５に機械的に連結されている。この駆動モータ１５によって、後輪５が駆動されることにより、ロボット装置２が走行する。

さらに、ステレオカメラ６は、ロボット装置２の前方周辺の路面を撮像し、撮像した距離画像データを後述するコントローラ１０に送信する。このステレオカメラ６の取付位置及び取付角度は、後述する点群データの取得結果が所定の誤差範囲（例えば数十ミリ）以内に収まるような値に設定されている。なお、本実施形態では、ステレオカメラ６が路面画像取得部に相当する。

次に、制御装置１について説明する。図２に示すように、制御装置１は、コントローラ１０を備えている。このコントローラ１０は、ロボット装置２に搭載されており、ＣＰＵ、ＲＡＭ、ＲＯＭ、Ｅ２ＰＲＯＭ、Ｉ／Ｏインターフェース及び各種の電気回路（いずれも図示せず）などからなるマイクロコンピュータで構成されている。なお、本実施形態では、コントローラ１０が、点群データ取得部、平面推定部、走行可否判定部、移動経路決定部及び制御部に相当する。

このコントローラ１０には、ステレオカメラ６、加速度センサ１１、ジャイロセンサ１２、無線通信装置１３、アクチュエータ１４及び駆動モータ１５が電気的に接続されている。このコントローラ１０では、ステレオカメラ６からの画像信号に基づき、後述するように、ロボット装置２の前方周辺の路面環境における平面推定が実行される。

また、コントローラ１０は、無線通信装置１３を介して、ロボット装置２の目的地を示す指令信号を受信するとともに、ロボット装置２の現在位置をＧＰＳによって取得する。さらに、コントローラ１０は、加速度センサ１１及びジャイロセンサ１２の検出信号に基づいて、ロボット装置２の走行速度及び姿勢を算出する。これに加えて、コントローラ１０は、後述するように、平面推定結果に応じて、走行経路を決定し、この走行経路でロボット１が走行するように、アクチュエータ１４及び駆動モータ１５を制御する。

次に、図３を参照しながら、本実施形態の制御装置１の機能的な構成、及び走破可否判定方法の原理などについて説明する。同図３に示すように、制御装置１は、点群データ取得部２１、平面推定部２２、走行可否判定部２３、コストマップ作成部２４及び走行経路決定部２５を備えており、これらの要素２１〜２５は、具体的にはコントローラ１０によって構成されている。

この点群データ取得部２１では、以下に述べるように、ステレオカメラ６からの距離画像データから点群データが取得される。まず、ステレオカメラ６からの距離画像データに対して、所定の画像処理を施すことにより、カメラ座標系における点群データが取得される。

次いで、これらのカメラ座標系における点群データに対して座標変換処理を施すことにより、ロボット装置２を基準とする座標系における点群データが取得される。このロボット装置２を基準とする座標系は、ロボット装置２が水平な路面上に位置していると仮定した場合において、ロボット装置２の進行方向をｘ軸とし、左右の水平方向をｙ軸とし、鉛直方向をｚ軸とする３次元の直交座標系である。すなわち、ロボット装置２が水平な路面上に位置している場合、その路面が３次元座標系のｘ−ｙ平面に相当する。

例えば、図４に示すように、ロボット装置２の前方の路面環境が階段３１が存在する傾斜面３０である場合、点群データ取得部２１では、図５に示すような点群データ３２が取得される。これらの点群データ３２は、３次元座標データであって、各々のｘ−ｙ座標値が、上記ｘ−ｙ平面内において台形の領域３３内に分布するデータとして取得される。同図５において、多数の矩形の区画は、コストマップの多数のセル４０を表しており、これらのセル４０は、上記ｘ−ｙ平面を複数の正方形の区画に分割したものに相当する。

この場合、各セル４０の一辺の長さｄは、ロボット装置２が登坂可能な斜面の最大傾斜角度をθｍａｘとし、ロボット装置２が踏破可能な障害物の最大高さをｈｍａｘとした場合、ｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように設定される。その理由については後述する。点群データ取得部２１では、以上のように点群データ３２を取得した後、それらの取得結果が平面推定部２２に出力される。

平面推定部２２では、点群データ取得部２１で取得された点群データ３２を用いて、以下に述べる（Ａ１）〜（Ａ６）の算出処理がセル４０毎に実行される。それにより、推定平面の式及び推定平面の傾斜角度θなどのパラメータが、点群データ３２が存在する全てのセル４０すなわち領域３３内に少なくとも一部が位置する全てのセル４０において、算出される。なお、以下の説明では、点群データ３２が存在する全てのセル４０をまとめて「全有効セル４０」という。

まず、下記の（Ａ１）〜（Ａ５）のＲＡＮＳＡＣ手法により、推定平面の式がセル４０毎に導出される。

（Ａ１）任意の１つのセル４０に含まれる点群データ３２、すなわちｘ−ｙ座標値が１つのセル４０に対応している点群データ３２から、３つの点の３次元データをランダムサンプリングする。
（Ａ２）ランダムサンプリングした３点が直線状に並んでおらず、かつこれら３点を頂点とする三角形が所定値以上の面積を有している場合には、これら３点を含む推定平面の式を導出する。
（Ａ３）推定平面とセル４０に含まれる各点群の距離を算出し、その算出距離が所定のしきい値を超えている点の数をカウントする。以下、算出距離が所定のしきい値を超えている値のことを「外れ値」という。
（Ａ４）外れ値のカウント数が、今回以前に推定されたどの平面よりも少ない場合には、その平面情報（平面の式）を記憶する。また、外れ値のカウント数が、所定のしきい値よりも小さい場合には、そのまま終了する。
（Ａ５）任意の１つのセル４０に含まれる点群データ３２に対して、以上の（Ａ１）〜（Ａ４）の処理を数回〜数百回繰り返し実行した後、最も外れ値の少なかった平面の式を推定平面の式として採用する。

以上のように、推定平面の式をセル４０毎に導出した後、下記の（Ａ６）〜（Ａ９）の算出処理がセル４０毎に実行される。

（Ａ６）セル４０の推定平面の傾斜角度θを算出する。この傾斜角度θは、前述したｘ−ｙ平面に対する推定平面の傾斜角度として算出される。
（Ａ７）セル４０の推定平面とセル４０に含まれる全ての点群データ３２との距離Ｄを、推定平面を原点とする符号付きの値として算出する。
（Ａ８）それらの距離Ｄの２乗平均平方根σを算出する。
（Ａ９）セル４０の推定平面に対して±２σの範囲内に存在する点群データ３２の距離Ｄの最大値Ｄｍａｘと最小値Ｄｍｉｎを取得し、最大値Ｄｍａｘと最小値Ｄｍｉｎとの差分ＤＤ（＝Ｄｍａｘ−Ｄｍｉｎ）を算出する。

そして、以上の（Ａ１）〜（Ａ９）の算出処理が全有効セル４０に対して実行されることにより、推定平面の傾斜角度θ及び差分ＤＤが全有効セル４０で算出される。その後、以上のように算出された傾斜角度θ及び差分ＤＤが、平面推定部２２から走行可否判定部２３に出力される。

走行可否判定部２３では、平面推定部２２で算出された傾斜角度θ及び差分ＤＤに基づき、走行可否の判定がセル４０毎に実施される。具体的には、任意の１つのセル４０において、以下に述べる２つの条件（Ｂ１）〜（Ｂ２）がすべて満たされているときには、そのセル４０が走行可能であると判定され、それ以外のときには、そのセル４０が走行不能であると判定される。

（Ｂ１）傾斜角度θが最大傾斜角度θｍａｘ以下であること。
（Ｂ２）差分ＤＤが最大高さｈｍａｘ以下であること。

そして、上記２つの条件（Ｂ１）〜（Ｂ２）に基づく走行可否の判定を全有効セル４０に対して実行した後、その判定結果が、走行可否判定部２３からコストマップ作成部２４に出力される。この走行可否の判定において、上記（Ｂ１）〜（Ｂ２）の条件を用いる理由については後述する。

ここで、前述したように、各セル４０の一辺の長さｄが、ｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように設定されている理由について、図６を参照しながら説明する。同図６に示すように、任意のセル４０内の領域が水平であって、その前端部に高さｈの段差４１が存在している環境下において、平面推定部２２での平面推定により、セル４０の手前側から段差４１の縁まで延びる推定平面４２の式が取得された場合を想定する。

この場合、推定平面４２の傾斜角度θに関して、ｔａｎθ＝ｈ／ｄが成立するので、この高さｈがｈ＞ｈｍａｘの場合には、θ＞θｍａｘが成立し、上記（Ｂ１）の条件により、走破不能であると判定されることになる。一方、ｈ≦ｈｍａｘの場合には、θ≦θｍａｘが成立し、上記（Ｂ１）の条件により、走破可能であると判定されることになる。

以上のように、各セル４０の一辺の長さｄをｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように設定することにより、ロボット装置２が登坂可能な斜面の最大傾斜角度θｍａｘと、ロボット装置２が踏破可能な障害物の最大高さｈｍａｘとの双方の条件を反映させながら、走行可否の判定を適切に実施できることが判る。本実施形態では、この理由により、各セル４０の一辺の長さｄが、ｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように設定されている。

次に、前述したように、セル４０の走行可否の判定において（Ｂ１）〜（Ｂ２）の条件を用いる理由について説明する。まず、前述したように、セル４０の推定平面４２とセル４０に含まれる全ての点群データ３２との距離Ｄを、推定平面４２を原点とする符号付きの値として算出し、それらの２乗平均平方根σを算出した場合、統計学的には、９５％の点群データ３２が、推定平面４２から±２σの範囲内に存在すると見なすことができる。

さらに、推定平面４２から±２σの範囲内に存在する距離Ｄの最大値Ｄｍａｘと最小値Ｄｍｉｎとの差分ＤＤは、セル４０の最も高い位置と最も低い位置との間の高さと見なすことができる。

例えば、図７に示すように、ロボット装置２が水平な路面４３上に位置し、その前方に踏破不可能な障害物４４が存在する路面環境下において、例えば、障害物４４が存在するセル４０の推定平面４２が路面４３と同一の面として推定された場合、θ＜θｍａｘが成立するとともに、Ｄｍｉｎ＝０となる。それにより、±２σの範囲内に存在する距離Ｄの最大値Ｄｍａｘと最小値Ｄｍｉｎとの差分ＤＤは、この障害物４４の高さになる。その結果、（Ｂ１）の条件は満たされるものの、ＤＤ＞ｈｍａｘが成立することで、上記（Ｂ２）の条件が満たされない状態となり、このセル４０は、走破不可能なセル４０と判定されることになる。

一方、図７に示す路面環境下において、障害物４４が存在するセル４０の推定平面が路面４３から障害物４４の頂点に対して延びる斜面（図示せず）として推定された場合、θ＞θｍａｘが成立することで、（Ｂ１）の条件が不成立となる。それにより、このセル４０は、走破不可能なセル４０と判定されることになる。したがって、前述した（Ｂ１）〜（Ｂ２）の条件により、障害物が存在する路面環境下でも、セル４０の走破可否を適切に判定できることになる。

また、図示しないが、例えば、ロボット装置２の前方に多数の凹凸が存在する路面環境の場合、セル４０の推定平面が傾斜面として推定される状態となる可能性が高い。これに加えて、±２σの範囲内に存在する距離Ｄの最大値Ｄｍａｘと最小値Ｄｍｉｎとの差分ＤＤは、そのセル４０内の最も高い位置と最も低い位置との間の高さと見なすことができる。したがって、前述した（Ｂ１）〜（Ｂ２）の条件により、多数の凹凸が存在する路面環境下でも、セル４０の走破可否を適切に判定できることになる。

さらに、図示しないが、例えば、ロボット装置２の前方に段差が存在する路面環境下の場合、その段差が存在するセル４０において±２σの範囲内に存在する距離Ｄの最大値Ｄｍａｘと最小値Ｄｍｉｎとの差分ＤＤは、その段差の高さと見なすことができる。また、前述したように、段差が存在するセル４０の推定平面が傾斜面として推定されたときでも、その傾斜角度θを最大傾斜角度θｍａｘと比較することで、セル４０の走行可否を判定できることになる。したがって、前述した（Ｂ１）〜（Ｂ２）の条件により、段差が存在する路面環境下でも、セル４０の走破可否を適切に判定できることになる。

以上の理由により、本実施形態の走行可否判定部２３では、（Ｂ１）〜（Ｂ２）の条件を用いて、セル４０の走行可否の判定が実行される。

また、コストマップ作成部２４では、全有効セル４０における走行可否の判定結果に基づき、図８に示すようなコストマップ５０が作成される。同図８に示すように、このコストマップ５０では、領域３３内のセル４０が４種類のセル４０ａ〜４０ｄに区分けされる。この場合、最も濃い点描で示す走行不能セル４０ａは、走行不能と判定されたセルであり、ロボット装置２が走行するのを絶対に回避すべき領域に相当する。

また、走行不能セル４０ａよりも薄い点描で示す第１近傍セル４０ｂは、走行不能と判定された走行不能セル４０ａの近傍エリアを表すセルであり、ロボット装置２が走行するのを回避した方がよい領域に相当する。さらに、第１近傍セル４０ｂよりも薄い点描で示す第２近傍セル４０ｃは、第１近傍セル４０ｂの近傍エリアを表すセルであり、ロボット装置２が走行する際、可能であれば走行するのを回避した方がよい領域に相当する。

一方、点描のない走行可能セル４０ｄは、走行可能と判定されたセルであり、ロボット装置２が走行すべき領域に相当する。以上のように作成されたコストマップ５０は、コストマップ作成部２４から走行経路決定部２５に出力される。

走行経路決定部２５では、ロボット装置２の目的地、現在地及び上記のコストマップ５０に基づき、ロボット装置２の走行経路が決定される。この走行経路は、図示しないが、コストマップ５０の走行可能セル４０ｄを通過するように決定される。

次に、図９を参照しながら、本実施形態におけるロボット装置２の制御処理について説明する。この制御処理は、ロボット装置２の走行状態を制御するものであり、コントローラ１０によって実行される。

同図に示すように、まず、ステレオカメラ６からの路面画像データが読み込まれる（図９／ＳＴＥＰ１）。

次いで、前述したように、路面画像データに対して所定の画像処理を施すことにより、カメラ座標系における点群データが取得され、これらのカメラ座標系における点群データに対して座標変換処理を施すことにより、ロボット装置２を基準とする座標系における点群データ３２が取得される（図９／ＳＴＥＰ２）。

次に、前述した（Ａ１）〜（Ａ５）のＲＡＮＳＡＣ手法により、推定平面４２の式が全有効セル４０において導出される（図９／ＳＴＥＰ３）。

このように推定平面４２の式が全有効セル４０において導出された後、前述した（Ａ６）〜（Ａ９）の算出処理により、推定平面の傾斜角度θ及び差分ＤＤが全有効セル４０において算出される（図９／ＳＴＥＰ４）。

次いで、前述した２つの条件（Ｂ１）〜（Ｂ２）に基づき、走行可否が全有効セル４０において判定される（図９／ＳＴＥＰ５）。

次に、上記の走行可否の判定結果に基づき、前述したようなコストマップ５０が作成される（図９／ＳＴＥＰ６）。

このようにコストマップを作成した後、コストマップ５０、ロボット装置２の目的地及び現在地に基づき、ロボット装置２の走行経路（図示せず）が決定される（図９／ＳＴＥＰ７）。

次いで、走行制御が実行される（図９／ＳＴＥＰ８）。この走行制御では、上記のように決定された走行経路で走行するように、ロボット装置２のアクチュエータ１４及び駆動モータ１５が制御される。以上のように、走行制御が実行された後、本処理が終了する。

以上のように、本実施形態の制御装置１によれば、ロボット装置２のステレオカメラ６によって、ロボット装置２の前方周辺の路面が撮像され、その撮像した距離画像データがコントローラ１０に入力される。コントローラ１０では、距離画像データからロボット装置２を基準とする３次元座標系における点群データ３２が取得され、３次元座標系のうちのｘ−ｙ座標軸で規定される平面を多数のセル４０に分割した場合において、ｘ−ｙ座標値が１つのセル４０に対応している点群データ３２に基づき、前述した（Ａ１）〜（Ａ２）のＲＡＮＳＡＣ手法により、推定平面の式がセル４０毎に取得される。

このように、推定平面の式がセル４０毎に取得されるので、多数の凹凸又は多数の障害物が存在するような路面環境下でも、特許文献１の手法と異なり、推定平面の式を実際の路面環境が適切に反映されたものとして精度よく取得することができる。特に、前述したＲＡＮＳＡＣ手法の場合、最小２乗法などを用いる平面推定手法と異なり、外れ値を除外した点群データを用いて、平面推定が実施されるので、外れ値の影響を受けにくくなる分、平面の推定精度を向上させることができる。

さらに、前述した（Ｂ１）〜（Ｂ２）の条件を用いて、各セル４０の走行可否が判定される。この場合、セル４０の一辺の長さｄは、ｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように設定されているので、（Ｂ１）の条件を用いることにより、最大傾斜角度θｍａｘ及び最大高さｈｍａｘの双方の条件を反映させながら、走行可否の判定を適切に実施することができる。

また、前述したように、セル４０の推定平面４２とセル４０に含まれる全ての点群データ３２との距離Ｄを、推定平面４２を原点とする符号付きの値として算出し、それらの２乗平均平方根σを算出した場合、統計学的には、９５％の点群データ３２が、推定平面４２から±２σの範囲内に存在すると見なすことができる。

さらに、推定平面４２から±２σの範囲内に存在する距離Ｄの最大値Ｄｍａｘと最小値Ｄｍｉｎとの差分ＤＤは、セル４０の最も高い位置と最も低い位置との間の高さと見なすことができる。したがって、前述した（Ｂ１）〜（Ｂ２）の条件を用いることにより、急な傾斜面、大きな障害物、多数の凹凸又は段差が存在する路面環境下でも、セル４０の走破可否を適切に判定することができる。

また、以上のように実施されるセル４０の可否判定結果に基づいて、コストマップ５０が作成され、このコストマップ５０を用いることにより、ロボット装置２の走行経路が決定される。そして、この走行経路で走行するように、ロボット装置２が制御されるので、ロボット装置２を円滑に走行させることができる。

なお、実施形態は、移動体として、ロボット装置２を用いた例であるが、本発明の移動体は、これに限らず、路面上を移動するものであればよい。例えば、移動体として、倒立振子型ロボット又は自動運転車両を用いてもよい。

また、実施形態は、路面画像取得部として、ステレオカメラ６を用いた例であるが、本発明の路面画像取得部は、これに限らず、移動体の周辺の路面環境を表す路面画像を取得するものであればよい。例えば、路面画像取得部として、ＬＩＤＡＲを用いてもよく、ＬＩＤＡＲとカメラを組みあわせて用いてもよい。

さらに、実施形態は、区画としてのセル４０を正方形に構成した例であるが、これに代えて、本発明の区画を、三角形、長方形、又は五角以上の多角形に構成してもよい。その場合においても、移動体が登坂可能な斜面の最大傾斜角度と、移動体が踏破可能な障害物の最大高さとを用いて、区画の移動体から離間する所定方向の長さを決定すればよい。

一方、実施形態は、区画の所定方向の長さｄを、ｄ＝ｈｍａｘ／ｔａｎθｍａｘの式により決定した例であるが、本発明の各区画の一辺の長さの決定方法は、これに限らず、移動体が登坂可能な斜面の最大傾斜角度と、移動体が踏破可能な障害物の最大高さとを用いて決定する方法であればよい。例えば、最大傾斜角度及び最大高さに応じて、マップ検索により、各区画の一辺の長さを決定するように構成してもよい。

また、実施形態は、所定の平面推定手法として、前述した（Ａ１）〜（Ａ２）のＲＡＮＳＡＣ手法を用いた例であるが、本発明の所定の平面推定手法は、これに限らず、平面を区画毎に推定するものであればよい。例えば、前述した（Ａ１）の処理において、ランダムサンプリングする点群データの数を４つ以上にした手法を用いてもよく、その場合には、前述した（Ａ２）の処理において、サンプリングデータに基づき、最小２乗法などによって平面推定を実施すればよい。また、実施形態の前述した（Ａ２）の処理においても、ランダムサンプリングしたデータから、最小２乗法を用いて平面を推定する手法を用いてもよい。

一方、実施形態は、移動体としてのロボット装置２の前方周辺の路面環境における平面推定を実行した例であるが、ステレオカメラ６を、ロボット装置２の左右両側及び後ろ側の少なくとも１箇所に追加して配置し、ロボット装置２の左右方向及び後方の少なくとも１方の周辺の路面環境における平面推定を実行するように構成してもよい。

１ 制御装置、走破可否判定装置
２ ロボット装置（移動体）
６ ステレオカメラ（路面画像取得部）
１０ コントローラ（点群データ取得部、平面推定部、走行可否判定部、移動経路決 定部、制御部）
３２ 点群データ
４０ セル（区画）
４２ 推定平面（平面）
ｄ セルの一辺の長さ
θｍａｘ 最大傾斜角度
ｈｍａｘ 最大高さ
Ｄ 符号付き距離
Ｄｍａｘ 符号付き距離の最大値
Ｄｍｉｎ 符号付き距離の最小値
ＤＤ 差分
σ ２乗平均平方根

Claims (5)

1. 移動体の周辺の路面環境が走破可能であるか否かを判定する走破可否判定装置であって、
   前記移動体の周辺の前記路面環境を表す路面画像を取得する路面画像取得部と、
   当該路面画像に基づき、当該路面画像内の路面及び物体を表す点群データを、前記移動体を基準とする３次元座標系のデータとして取得する点群データ取得部と、
   前記３次元座標系のうちの所定の２次元座標軸で規定される平面を複数の区画に分割した場合において、当該複数の区画の各々に対応する２次元座標値を有する前記点群データに基づき、所定の平面推定方法により、平面を前記区画毎に推定する平面推定部と、
   前記平面に基づき、前記路面環境が走破可能であるか否かを前記区画毎に判定する走行可否判定部と、を備え、
   前記平面推定部は、前記移動体が登坂可能な斜面の最大傾斜角度と、前記移動体が踏破可能な障害物の最大高さとを用いて、前記区画の前記移動体から離間する所定方向の長さを決定し、
   前記走行可否判定部は、前記平面の傾斜角度が前記最大傾斜角度以下であり、かつ前記点群データが前記平面に関して所定条件を満たしている場合、前記路面環境が走破可能であると判定することを特徴とする走破可否判定装置。
2. 請求項１に記載の走破可否判定装置において、
   前記区画は、正方形に構成され、当該正方形の一辺が前記所定方向に沿うように配置されており、
   前記所定方向の長さは、前記正方形の前記一辺の長さであり、
   前記平面推定部は、前記一辺の長さをｄとし、前記最大傾斜角度をθｍａｘとし、前記最大高さをｈｍａｘとした場合、ｄ＝ｈｍａｘ／ｔａｎθｍａｘが成立するように、前記一辺の長さｄを決定することを特徴とする走破可否判定装置。
3. 請求項１又は２に記載の走破可否判定装置において、
   前記所定条件は、前記点群データと前記平面との距離を前記区画毎に算出した際の当該距離が前記平面を基準とする所定範囲内の領域に含まれる前記点群データにおいて、当該点群データと前記平面との符号付き距離の最大値と最小値との差分が前記最大高さ以下であることを特徴とする走破可否判定装置。
4. 請求項３に記載の走破可否判定装置において、
   前記所定範囲は、前記点群データと前記平面との前記距離の２乗平均平方根をσとした場合において、前記平面から±２σの範囲に設定されていることを特徴とする走破可否判定装置。
5. 請求項１ないし４のいずれかに記載の走破可否判定装置と、
   当該走破可否判定装置の判定結果に応じて、前記移動体の移動経路を決定する移動経路決定部と、
   前記移動体が当該移動経路で移動するように、前記移動体を制御する制御部と、
   を備えることを特徴とする移動体の制御装置。

Images