JP2009136987A

JP2009136987A - 移動ロボット、及び床面形状データの補正方法

Info

Publication number: JP2009136987A
Application number: JP2007318272A
Authority: JP
Inventors: Yutaka Takaoka; 豊高岡
Original assignee: Toyota Motor Corp
Current assignee: Toyota Motor Corp
Priority date: 2007-12-10
Filing date: 2007-12-10
Publication date: 2009-06-25

Abstract

【課題】床面形状データの歪みを正確に補正することができる移動ロボット、及び床面形状データの補正方法を提供すること。
【解決手段】本発明にかかる移動ロボット１００は、移動ロボット１００が移動する床面の形状データを検出する床面形状検出センサ１０と、床面形状検出センサ１０により検出された床面形状データを補正するデータ補正部１３とを備えた移動ロボット１００であって、データ補正部１３は、床面形状検出センサ１０のセンシング領域の一部である基準領域における床面形状データの傾き補正量を算出し、当該算出した傾き補正量に基づいて、床面形状データの全体を補正するものである。
【選択図】図２

Description

本発明は移動ロボット、及び床面形状データの補正方法に関する。

近時、自ら移動経路の環境を認識して目的地へと移動する自律移動ロボットが開発されている。このような自律移動ロボットでは、移動経路上に存在する段差などの障害物を検出・認識し、検出・認識結果に基づいて作成される移動経路に従って目的地へと移動する。移動ロボットが３次元空間内を移動する歩行ロボットである場合には、歩行の際に移動ロボットは、床面上に存在する段差などの障害物を検出・認識した上で着地位置を決定する。このため、床面上を安定して歩行するためには、移動ロボットの着地位置候補となり得る床面や障害物上の平面をより正確に検出することが重要となる。

一般的に、移動ロボットが移動する際には、移動ロボット自身の振動や揺れによって、検出された床面形状データに歪みが発生する。このため、段差などの障害物を含む平面を正確に検出するためには、検出された床面形状データの歪みを補正する必要がある。

従来、検出された床面形状データの歪みをロボットの移動量であるオドメトリに基づいて補正する手法が良く知られているものの、オドメトリに基づく補正方法では、高さ方向の歪みを正確に補正することが困難である。また、例えばジャイロや加速度センサといった内界センサを用いることでオドメトリによる精度を補う手法も知られているが、床面形状データを目標とする精度で検出することはできず、不十分なものである。

一方、特許文献１には、理想的な基準床面における姿勢センサの検出結果（基準データ）を予め算出して記憶しておき、実際に検出された姿勢センサの検出結果（現状値）について、基準データに近づけるように現状値を補正するロボットの制御方法が開示されている。特許文献１記載のロボットの制御方法では、例えばその時点の現状値に近い値を示す基準データの次の値を次の姿勢の目標（目標値）として設定することで、基準データを参照して目標値を簡易に設定することができる。
特開２００５−７５２８号公報

しかしながら、特許文献１記載の従来技術は姿勢センサの検出結果を補正するものであり、移動ロボットが移動する平面を正確に検出可能とする床面形状データの補正技術を開示するものではない。即ち、特許文献１記載の従来技術では、移動ロボットの外部環境を認識するために必要とする床面形状データを補正することができないため、障害物を正確に認識することができず、床面や障害物上の平面を正確に検出することができないものである。

また、段差などの障害物を含む平面を正確に検出するためには、検出された床面形状データの高さ成分の精度を確保することが重要となるものの、従来技術では高さ方向の歪みを正確に補正することが困難であった。さらに、移動ロボットが歩行ロボットである場合には、移動ロボット自身の振動や揺れに起因して床面形状データにロール方向への歪みが発生するという問題点がある。

このように、従来技術によれば床面形状データの歪みを正確に補正することができないという問題点があった。従って、床面の形状を正確に検出することができないため段差などの障害物を含む平面を正確に検出することができず、障害物を正確に認識することができないという問題がある。

本発明は、かかる課題を解決するためになされたものであり、床面における段差などの障害物を正確に認識することができる移動ロボット、及び床面形状データの補正方法を提供することを目的とする。

本発明にかかる移動ロボットは、移動ロボットが移動する床面の形状データを検出する床面形状検出センサと、前記床面形状検出センサにより検出された床面形状データを補正するデータ補正部とを備えた移動ロボットであって、前記データ補正部は、前記床面形状検出センサのセンシング領域の一部である基準領域における床面形状データの傾き補正量を算出し、当該算出した傾き補正量に基づいて、前記床面形状データの全体を補正するものである。

これにより、移動ロボット自身の振動や揺れに起因して発生するロール方向の歪みについて、センシング領域の一部である基準領域の傾きに基づいて、床面形状データの全体を容易且つ正確に補正することができる。従って、床面の形状を正確に検出することができるため、段差などの障害物を含む平面を正確に検出することができる。即ち、床面における段差などの障害物を正確に認識することができる。

また、前記データ補正部は、前記基準領域における床面形状データが水平面を表すデータとなるように、前記傾き補正量を算出するようにしてもよい。これにより、検出された床面形状データが高さ方向に水平となるように補正され、平面をより正確に検出することができる。

さらにまた、前記データ補正部は、前記基準領域における床面形状データの高さ補正量を算出し、当該算出した高さ補正量に基づいて、前記床面形状データの全体を補正するようにしてもよい。これにより、検出された床面形状データが高さ方向に基準領域の基準高さとなるように補正され、移動ロボットの撓みや足つきに起因する歪みを効果的に補正して、平面をより正確に検出することができる。

また、前記データ補正部は、前記基準領域における床面形状データが床面を表すデータとなるように、前記高さ補正量を算出するようにしてもよい。これにより、基準領域を例えば床面とした場合に、検出された床面形状データが高さ方向に床面の基準高さとなるように補正され、平面をより容易かつ正確に検出することができる。

さらにまた、前記移動ロボットは、前記床面形状検出センサのセンシング領域のうち、前記床面上に配置された障害物を含むように処理対象領域を決定する処理領域決定部を更に備え、前記データ補正部は、前記決定した処理対象領域における前記床面形状データを補正するようにしてもよい。このように、床面形状検出センサのセンシング領域のうち、平面検出の際に精度が要求される障害物を含む領域を処理対象領域として決定する処理領域決定部を更に備えることで、補正が特に必要な床面形状データのみを効率的に補正することができる。

また、前記処理領域決定部は、前記処理対象領域が更に前記基準領域の一部を含むように前記処理対象領域を決定する。このように、床面形状検出センサのセンシング領域のうち、平面検出の際に精度が要求される障害物を含む領域を処理対象領域として決定することで、処理対象領域の一部である基準領域に基づいて、処理対象領域における床面形状データの全体を補正する。

さらにまた、前記移動ロボットは、前記処理領域決定部により決定された前記処理対象領域が前記基準領域を含まない場合に、前記移動ロボットの姿勢を安定させる動作を行うようにしてもよい。このように、処理対象領域が基準領域を含まない場合に、移動ロボットの姿勢を安定させる動作を行うことで、移動ロボットが移動中の場合に比べて、床面形状データの歪みの発生を抑制することができる。

また、前記移動ロボットが、移動手段としての複数の脚部を備え、前記処理領域決定部により決定された前記処理対象領域が前記基準領域を含まない場合に、その場足踏み動作を行うようにしてもよい。このように、処理対象領域が基準領域を含まない場合に、移動ロボットがその場足踏み動作を行うことで、移動ロボットが移動中の場合に比べて、移動ロボットを停止させずに床面形状データの歪みの発生を抑制することができる。

さらにまた、前記床面形状検出センサが、前記移動ロボットの水平軸を中心に揺動可能に取り付けられたレーザレンジファインダであり、前記データ補正部が、前記レーザレンジファインダによるレーザ光の走査線毎に、当該走査線上に位置する複数の床面形状データのうち前記基準領域における床面形状データに基づいて、前記傾き補正量を算出し、当該算出した傾き補正量に基づいて、前記床面形状データの全体を補正するようにしてもよい。このように、レーザレンジファインダを移動ロボットの水平軸を中心に揺動可能に取り付けて床面形状データを検出することで、レーザレンジファインダを移動ロボットの鉛直軸を中心に揺動可能に取り付けた場合に比べて、移動ロボットの左右方向により広範囲に亘って床面形状データを検出することができる。さらに、測定のバラツキを低減することができ、より精度の高い床面形状データを取得することができる。また、レーザ光の走査線毎に床面形状データを補正することで、ロール方向の揺れに起因する歪みを含んだ床面形状データを容易に補正することができる。

さらにまた、前記データ補正部が、前記レーザレンジファインダによるレーザ光の走査線毎に、当該走査線上に位置する複数の床面形状データのうち前記基準領域における床面形状データに基づいて、前記基準領域における床面形状データの高さ補正量を算出し、当該算出した高さ補正量に基づいて、前記床面形状データの全体を補正するようにしてもよい。このように、レーザ光の走査線毎に、床面形状データを高さ方向に基準領域の基準高さとなるように補正することで、移動ロボットの撓みや足つきに起因する歪みを容易かつ効果的に補正することができる。

また、前記基準領域が前記床面上に配置された障害物を除く領域とする。このように基準領域を床面上に配置された障害物を除く領域とすることで、床面と移動ロボットとの位置関係に基づいて、床面上の床面形状データを簡便な構成で判定することができる。

さらにまた、前記移動ロボットが、移動手段としての２つの脚部を備え、２足歩行運動により移動を行うようにしてもよい。このように、２足歩行型のロボットである場合においても、ロール方向の揺れに起因する歪みを効果的に補正することができる。

本発明にかかる床面形状データの補正方法は、床面形状データを補正する床面形状データの補正方法であって、移動ロボットが移動する床面の形状データを検出するデータ検出ステップと、前記検出された床面形状データを補正するデータ補正ステップとを備え、前記データ補正ステップでは、前記データ検出ステップでの検出対象領域の一部である基準領域における床面形状データの傾き補正量を算出し、当該算出した傾き補正量に基づいて、前記床面形状データの全体を補正するものである。

これにより、移動ロボット自身の振動や揺れに起因して発生するロール方向の歪みについて、床面形状データの検出対象領域の一部である基準領域の傾きに基づいて、床面形状データの全体を容易且つ正確に補正することができる。従って、床面の形状を正確に検出することができるため、段差などの障害物を含む平面を正確に検出することができる。即ち、床面における段差などの障害物を正確に認識することができる。

また、前記データ補正ステップでは、前記基準領域における床面形状データが水平面を表すデータとなるように、前記傾き補正量を算出するようにしてもよい。これにより、検出された床面形状データが高さ方向に水平となるように補正され、平面をより正確に検出することができる。

さらにまた、前記データ補正部は、前記基準領域における床面形状データが床面を表すデータとなるように高さ補正量を算出し、当該算出した高さ補正量に基づいて、前記床面形状データの全体を補正するようにしてもよい。これにより、検出された床面形状データが高さ方向に床面の基準高さとなるように補正され、移動ロボットの撓みや足つきに起因する歪みを効果的に補正して、平面をより正確に検出することができる。

また、前記移動ロボットは、前記床面形状検出センサのセンシング領域のうち、前記基準領域の一部と前記床面上に配置された障害物とを含むように処理対象領域を決定する処理領域決定部を更に備え、前記データ補正部は、前記決定した処理対象領域における前記床面形状データを補正するようにしてもよい。このように、床面形状検出センサのセンシング領域のうち、平面検出の際に精度が要求される障害物を含む領域を処理対象領域として決定する処理領域決定部を更に備えることで、補正が特に必要な床面形状データのみを効率的に補正することができる。

また、前記データ検出ステップでは、前記移動ロボットに設けられた光源からのレーザ光を前記移動ロボットの左右方向へと走査すると共に、前記光源を前記移動ロボットの水平軸を中心に揺動することで前記床面形状データを検出し、前記データ補正ステップでは、レーザ光の走査線毎に、当該走査線上に位置する複数の床面形状データのうち前記基準領域における床面形状データに基づいて、前記傾き補正量を算出し、当該算出した傾き補正量に基づいて、前記床面形状データの全体を補正するようにしてもよい。このように、光源からのレーザ光を移動ロボットの左右方向へと走査すると共に、光源を移動ロボットの水平軸を中心に揺動することにより床面形状データの検出を行うことで、光源からのレーザ光を移動ロボットの前後方向へと走査すると共に、光源を移動ロボットの鉛直軸を中心に揺動することにより床面形状データの検出を行う場合に比べて、移動ロボットの左右方向により広範囲に亘って床面形状データを検出することができる。さらに、測定のバラツキを低減することができ、より精度の高い床面形状データを取得することができる。また、レーザ光の走査線毎に床面形状データを補正することで、ロール方向の揺れに起因する歪みを含んだ床面形状データを容易に補正することができる。

本発明によれば、床面形状データの歪みを正確に補正することができる移動ロボット、及び床面形状データの補正方法を提供することを目的とする。

以下、本発明を適用した具体的な実施の形態について、図面を参照しながら詳細に説明する。尚、各図面において、同一要素には同一の符号を付しており、説明の明確化のため、必要に応じて重複説明を省略する。

発明の実施の形態１．
本実施の形態にかかる移動ロボットは、２つの脚リンクを有する脚式移動ロボットである。さらに、移動ロボットはデータ補正装置１を有する。データ補正装置１は、床面形状検出センサとしてのレーザレンジファインダにより得られる床面形状データを入力して、センサのセンシング領域内に設けられた基準領域に含まれる床面形状データの補正量を算出し、算出した補正量に基づいて、床面形状データの全体を補正する装置である。

移動ロボットは、データ補正装置１によって補正された床面形状データに基づいて、移動ロボットが歩行する歩行面の３次元情報を含む環境地図データを決定し、移動ロボットの２つの脚リンクそれぞれの先端に設けられた足平の着地位置を決定する。さらに、移動ロボットは、決定された足平着地位置を実現するための動作データを生成し、生成された動作データをもとに移動ロボットの関節群をアクチュエータによって駆動することで歩行する。

図１を用いて、本発明にかかる移動ロボット１００の構成について説明する。図１は、移動ロボット１００を側方（移動ロボット１００の移動する方向を向いて左方向）から見た様子を概略的に表す概略図であり、移動ロボット１００に搭乗者Ｐが搭乗している様子を表している。また、図１に示すように、床面形状検出センサとしてのレーザレンジファインダ１０が移動ロボット１００に取り付けられる。尚、図１においては、説明の便宜上、移動体１が進行する向き（前後方向）をｘ軸、移動体１が進行する方向について水平方向に直交する向き（左右方向）をｙ軸、移動体の移動する平面から鉛直方向に延びる向き（上下方向）をｚ軸とし、これらの３軸からなる座標系を用いて説明する。即ち、図１中において、前記ｘ軸は紙面に向かって左右方向、ｙ軸は紙面の奥行方法、ｚ軸は紙面中の上下方向を示す。

図１に示すように、移動ロボット１００は、搭乗者Ｐが搭乗する載置台（図示せず）を含む上体部１０１と、上体部１０１を支持する腰部（図示せず）を介して回動自在に固定される脚部１０４と、腰部に取り付けられたレーザレンジファインダ１０とにより構成される。脚部１０４は、右脚１０２及び左脚１０３により構成され、右脚１０２は、右膝関節１０２ｃ、右足首関節１０２ｅ、右足平１０２ｆを備え、同様に、左脚１０３は、左膝関節１０３ｃ、左足首関節１０３ｅ、左足平１０３ｆを備える。図示しないモータからの駆動力が、同じく図示しないプーリ及びベルトを介して伝達されることで、各関節が所望の角度に駆動され、その結果、右脚１０２及び左脚１０３に所望の動きをさせることができる。

移動ロボット１００の上体部１０１には、腰部（図示せず）を介して移動ロボット１００が歩行する床面の床面形状データを検出するレーザレンジファインダ１０が設けられている。より具体的には、例えば図１においては、レーザレンジファインダ１０を、水平方向から移動ロボット１００の足元に向けて傾斜させて設置し、移動ロボット１００の足元近傍を含む前方に向けて赤外線などのレーザ光を照射し、その反射光を受光させる。このようにすると、移動ロボット１００の足元近傍の床面形状データについて高い分解能を確保することができ、密度の高い足元近傍の床面形状データを取得することができる。

レーザレンジファインダ１０は、床面上の障害物を含む外部環境の距離画像データを取得する。尚、距離画像データとは、環境に存在する計測対象までの距離値を各画素の画素値として有する２次元データである。後述するように、距離画像データの座標変換を行うことにより、外界の３次元の点群データを生成することができる。

さらに、レーザレンジファインダ１０は、移動ロボット１００の腰部に備えられており、水平軸を中心に揺動可能な構成となっている。これにより、移動ロボット１００の移動方向前方をスキャンすることができる。より具体的には、例えば図１においては、移動方向前方（ここではｘ軸方向）に向けて、レーザレンジファインダ１０が移動ロボット１００の水平軸（ここではy軸）を中心として上下に揺動可能に取り付けられている。このようにすると、側面視では、レーザ光によるスキャン範囲を移動ロボット１００の移動方向前方に向かって、上下に広がった扇形形状となるように構成することができ、移動に必要な領域のみを効率的にスキャンすることができる。

ここで、レーザレンジファインダ１０が、２次元の距離画像データから３次元の点群データを生成する方法について説明する。まず、レーザレンジファインダ１０を上下に揺動させずに固定した場合に、レーザ光を床面に垂直に移動方向前方に向かって左右に照射することで、床面に垂直な平面上における障害物の距離画像データを取得する。そして、レーザレンジファインダ１０を移動ロボット１００の水平軸を中心に、移動方向前方に向かって上下に揺動させながらスキャンすることで、複数の異なる平面上における距離画像データを取得する。次いで、レーザレンジファインダ１０を揺動可能に支持する関節の角度に基づいて複数の距離画像データを座標変換して位置合わせする。これにより、複数平面上で得られた距離画像データの統合を行うことができる。または、複数の距離画像データから対応点を求めることによって、距離画像データ相互の位置合わせを行ってもよい。このようにして、前方の領域に含まれる３次元の距離画像データを簡便な構成によって容易に取得することができる。

続いて以下では、本実施の形態にかかる移動ロボット１００の制御系について説明する。移動ロボット１００の制御系の構成を図２に示す。図２に示すように、データ補正装置１は、処理領域決定部１２及びデータ補正部１３を有する。

図２において、レーザレンジファインダ１０は、上述したように、移動ロボット１００の外部環境の距離画像データを取得する。より具体的には、例えば図３に示すように、移動ロボット１００が移動方向（ここではｘ軸方向）へ移動する際に、レーザレンジファインダ１０より移動方向前方に向けてレーザ光を照射することにより、移動方向前方の距離画像データを検出する。尚、床面形状検出センサとして、レーザレンジファインダに代えてステレオカメラを用いてもよい。つまり、ＣＣＤ（Charge Coupled Device）イメージセンサ又はＣＭＯＳ（Complementary Metal Oxide Semiconductor）イメージセンサ等の撮像素子を備えた複数のカメラを備え、これら複数のカメラによって撮影された画像の視差を利用して距離画像データを生成してもよい。より具体的には、複数のカメラによって撮影した画像データから対応点を検出し、ステレオ視によって対応点の３次元位置を復元する。ここで、複数の撮影画像における対応点の探索は、複数の撮影画像に対する時空間微分の拘束式を用いた勾配法や相関法等の公知の手法を適用して行えばよい。

３次元位置データ生成部１１は、距離画像データの座標変換を行って床面形状データとしての３次元位置データ群を生成する。尚、３次元位置データ群とは、距離画像データに含まれる多数の計測点の位置ベクトルを３次元直交座標系で表したデータの集合である。例えば図４の画像は、移動ロボット１００により生成される３次元位置データ群の一例を示している。尚、図４は、床面に平行なｘｙ平面に３次元位置データ群がプロットされたものであり、プロットされた各点について高さ情報（ｚ軸方向の位置情報）が存在する。

ここで図４（ａ）に示す画像は、レーザレンジファインダ１０を移動ロボット１００の水平軸を中心に揺動可能な構成とした場合における、移動ロボット１００により生成される３次元位置データ群の一例を示している。一方、図４（ｂ）に示す画像は、レーザレンジファインダ１０を移動ロボット１００の鉛直軸を中心に揺動可能な構成とした場合における、移動ロボット１００により生成される３次元位置データ群の一例を示している。図に示すように、レーザレンジファインダ１０を移動ロボット１００の水平軸を中心に揺動可能に取り付けて３次元位置データを検出することで、レーザレンジファインダ１０を移動ロボット１００の鉛直軸を中心に揺動可能に取り付けた場合に比べて、移動ロボット１００の左右方向により広範囲に亘って３次元位置データを検出することができる。従って、レーザレンジファインダ１０を揺動させる構成を変更することで、移動ロボット１００の足元付近の認識範囲を拡大させることができ、例えば、左右方向への旋回動作を行うために必要となる範囲の３次元位置データをより容易に取得することができる。

また、３次元位置データ生成部１１は、生成した３次元位置データの歪みをオドメトリを用いて補正する。より具体的には、レーザレンジファインダ１０によるスキャン中に、移動ロボット１００が移動することによって発生する検出誤差を補正する。移動ロボット１００はスキャン中に移動するため、検出される３次元位置データは移動ロボット１００の移動量に応じた検出誤差を含む。このため、オドメトリにより、各関節軸に設けられたエンコーダなどの出力に基づいて移動ロボット１００の移動量を算出して、移動ロボット１００が移動することによって発生する検出誤差を補正する。

さらにまた、３次元位置データ生成部１１は、移動ロボット１００の上体部１０１に設けられたジャイロ（図示せず）からの出力に基づいて、３次元位置データ群のピッチ方向の歪みを補正する。即ち、移動ロボット１００の腰部に設けられたジャイロ（図示せず）の出力からピッチ方向の回転成分を検出し、３次元位置データのピッチ方向の検出誤差を補正する。これにより、歩行ロボットにおいて３次元位置データに与える影響が大きいピッチ方向の検出誤差を補正することができる。

処理領域決定部１２は、レーザレンジファインダ１０のセンシング領域内に後述するデータ補正処理の対象とする処理対象領域を決定する。移動ロボット１００のオドメトリとレーザレンジファインダ１０の取り付け角度とから、床面上に配置された障害物を含むように処理対象領域を決定する。このように、センシング領域のうち、平面検出の際に精度が要求される障害物を含む領域を処理対象領域として決定することで、補正が特に必要な３次元位置データのみを効率的に補正することができる。また、処理領域決定部１２は、障害物に加えて基準領域の一部をさらに含むように処理対象領域を決定することで、処理対象領域の一部である基準領域に基づいて、処理対象領域における３次元位置データの全体を効率的に補正する。

尚、基準領域は、例えば移動ロボット１００が移動を禁止される禁止領域や障害物を含む領域に対しては設定しないものとし、歪みが存在しない場合において、基準領域に含まれる３次元位置データが水平となる領域を基準領域として設定することが望ましい。即ち、基準領域は水平である床面などにおける領域に設定されることが好ましい。また、移動ロボット１００の足元前方の近傍領域を基準領域として設定した場合には、後述する歩行計画において、基準領域が水平な床面における領域となるように歩行が計画されるようにしてもよい。

データ補正部１３は、基準領域における３次元位置データの傾き補正量を算出し、当該算出した傾き補正量に基づいて、３次元位置データ生成部１１により生成された３次元位置データ群を補正する。また、算出した傾き補正量に基づいて基準領域における３次元位置データを補正した後、基準領域における３次元位置データの高さ補正量をさらに算出して、当該算出した高さ補正量に基づいて３次元位置データ群を補正する。尚、データ補正部１３による３次元位置データ群の補正方法の詳細については後述する。

環境地図生成部１４は、３次元位置データから平面を検出し、複数の平面の集合としての環境地図を生成する。さらに具体的に述べると、本実施の形態の環境地図は、２次元平面（ｘｙ平面とする）を格子状に分割したグリッドデータとして生成される。環境地図の各グリッドに対応するデータとして、各グリッドのｘｙ平面に垂直なｚ軸方向の高さ、各グリッドが属する平面を一意に識別可能な平面ＩＤ、及び、各グリッドが属する平面の法線ベクトルが保持される。環境地図生成部１４は、ほぼ同じ高さの隣接するグリッドの集合を１つの平面と認識し、認識した１つの平面に対して一意な平面ＩＤを付与する。

例えば図５（ａ）に示す環境１６０を移動ロボット１００が歩行する場合、床面に相当する平面Ｐ１１及び障害物１６１の上面に相当する平面Ｐ１２が環境地図生成部１４により検出される。また、環境地図生成部１４によって、平面Ｐ１１が移動可能領域として認識され、平面Ｐ１２が障害物領域として認識される。このとき、環境地図生成部１４によって生成される環境地図２００は図５（ｂ）に示すようになる。図５（ｂ）の環境地図２００は、各グリッドのデータとして、各平面のそれぞれを一意に識別可能な平面ＩＤ、各グリッドのｚ軸方向の高さ、及び法線ベクトル（ｎａ，ｎｂ，ｎｃ）が保持される。尚、隣接するグリッド間の平面ＩＤの違い及びｚ軸方向の高さの違いによって右足平１０２ｆ及び左足平１０３ｆの着地位置における路面の凹凸を検出するため、環境地図２００を構成する各グリッドの面積は、右足平１０２ｆ及び左足平１０３ｆの底面の面積より十分に小さくなるよう選択するとよい。

さらに図５（ｂ）の環境地図２００は、各グリッドのデータとして領域種別を示す情報、つまり移動可能領域であるか障害物領域であるかを示す識別情報が保持される。図５（ｂ）の例では、床面Ｐ１１に対応する領域Ｒ１１が移動可能領域であり、障害物１６１に対応する領域Ｒ１２（図５（ｂ）の斜線領域）が障害物領域である。

歩行計画部１５は、環境地図生成部１４によって生成された環境地図を参照して目標位置を決定し、決定した目標位置に到達するための右足平１０２ｆ及び左足平１０３ｆの着地位置を算出する。

動作生成部１６は、歩行計画部１６によって生成された右足平１０２ｆ及び左足平１０３ｆの着地位置を実現するための動作データを生成する。ここで、動作データとは、移動ロボット１００のＺＭＰ位置、重心位置、右足平１０２ｆ及び左足平１０３ｆの位置及び姿勢、並びに、移動ロボット１００の上体部１０１の位置及び姿勢を含む。

制御部１７は、動作生成部１６によって生成された動作データを入力し、逆運動学演算によって各関節の目標関節角度を算出する。さらに、制御部１７は、算出した目標関節角度とエンコーダ１９によって計測した現在の関節角度をもとに各関節を駆動するためのトルク制御値を算出する。制御部１７によって算出されたトルク制御値に従って各関節を駆動するためのアクチュエータ１８を動作させることにより、移動ロボット１００の歩行が行われる。

続いて以下では、データ補正装置１による補正方法の詳細を図６乃至１６を用いて説明する。図６は、データ補正装置１による補正手順を示すフローチャートである。図７及び８は、レーザレンジファインダ１０を用いたセンシング処理の概要を説明するための図である。図９及び１０は、取得した３次元位置データの分類方法を説明するための図である。図１１及び１２は、取得した３次元位置データの歪みを説明するための図である。図１３及び１４は、処理対象領域の決定処理を説明するための図である。図１５及び１６は、３次元位置データの補正処理を説明するための図である。

図６に示すように、ステップＳ１０１では、３次元位置データ生成部１１は、レーザレンジファインダ１０により取得された距離画像データから、３次元位置データを生成し取得する。図７は、レーザレンジファインダ１０を用いたセンシング処理の概要を説明するための上視図である。図７に示すように、移動方向（ここではｘ軸方向）前方に向けて、レーザ光を照射してスキャンを行う。このとき、レーザレンジファインダ１０を移動ロボット１００のヨー方向へと回転させながらレーザ光を左右方向に照射することで、距離画像データを取得する。即ち、図において、レーザレンジファインダ１０を矢印Ｙａｗで示す方向へと回転させながら、レーザ光を矢印Ｄで示す方向に照射する。ラインＬは、レーザレンジファインダ１０により照射されるレーザ光の走査線を模式的に示したものであり、各ライン上に複数の距離画像データが存在する。そして、レーザ光の走査線Ｌ毎に、複数の距離画像データを測定し、黒丸で示す３次元位置データを生成する。

図８は、レーザレンジファインダ１０を用いたセンシング処理の概要を説明するための斜視図である。図８に示すように、レーザレンジファインダ１０をヨー方向へと回転させると共に、移動ロボット１００の水平軸を中心にピッチ方向に揺動させながらスキャンすることで、複数の異なる平面上における距離画像データを取得する。即ち、図において、レーザレンジファインダ１０を、矢印Ｙａｗで示す方向へと回転させながら矢印Ｐｉｔｃｈで示す方向へと揺動させてレーザ光を照射することでスキャンする。ここで、レーザレンジファインダ１０を用いてスキャン処理を実行するために要する処理時間については、１本のラインＬ上における全ての距離画像データを測定するのに要する時間は１０ｍｓであり、全てのラインＬ上の距離画像データを測定するのに要する時間Ｔは９００ｍｓである(即ち、時間Ｔで９０本のラインＬをスキャンする)。尚、ライン上の測定点間の時間差ＴＤは０．０２ｍｓであるため測定の精度は高く、同一ライン上の測定点同士では整合性を失わない。一方、測定ラインＬ間の時間差ＴＬは１０ｍｓであるため測定の精度は低く、異なる測定ライン同士では整合性を失ってしまう。

次いで、ステップＳ１０２では、処理領域決定部１２は、障害物を含む処理対象領域を決定する。まず、ステップＳ１０１で取得した３次元位置データ群について、移動ロボット１００のオドメトリとレーザレンジファインダ１０の取り付け位置・角度の幾何情報とから床面領域を示す３次元位置データを抽出し、障害物領域を示す３次元位置データと分類する。より具体的には、図９に示すように、移動ロボット１００が接地する床面を基準として、データの歪みを考慮した幅Ｗを持つ検出領域を設定し、当該検出領域に含まれる３次元位置データを床面領域の３次元位置データとして抽出する。図１０に示すように、床面領域を示す３次元位置データ（図において黒丸で示す）と、障害物領域を示す３次元位置データ（図において白丸で示す）に分類する。

そして処理領域決定部１２は、障害物領域を含むラインを探索することにより、処理対象領域を決定する。このとき、探索されるラインによって囲まれる領域内に、床面領域に属する３次元位置データの一部を更に含むようにラインを決定する。例えば図１１は、処理対象領域の決定処理の一例を示す上視図である。環境１６０において、障害物領域Ｐ１２を含むラインａ及びｂを探索し、ラインａ及びｂにより囲まれる処理範囲を処理対象領域として決定する。また、図１２（ａ）及び（ｂ）は、処理対象領域の決定処理の他の例を示す上視図であり、図１２（ｃ）に示すように、複数のラインａ乃至ｄを用いて処理対象領域を決定するようにしてもよい。

次いで、ステップＳ１０３では、データ補正装置１は、ステップＳ１０２で決定した処理対象領域が床面領域Ｐ１１に属する３次元位置データを含むか否かを判定する。決定されたラインにより囲まれる処理対象領域が、障害物領域Ｐ１２に加えて床面領域Ｐ１１の一部を更に含む場合には、後述するステップＳ１０４乃至１０６において処理対象領域内の３次元位置データの歪みを補正した上で、平面を検出する。一方、例えば図１２（ｄ）に示すように、処理対象領域内に床面領域Ｐ１１を含むことができない場合には、障害物領域Ｐ１２の手前で移動ロボット１００にその場足踏み動作を実行させ、移動ロボット１００の振動を抑制させながら再度スキャンを行わせる（ステップＳ１０７）。このように、床面領域に属する３次元位置データを含まない場合に、移動ロボット１００がその場足踏み動作を行うことで、移動ロボット１００が移動中の場合に比べて、移動ロボット１００を停止させずに３次元位置データの歪みの発生を抑制することができる。特に、移動ロボット１００の歩行中にスキャンを実行する場合には、移動ロボット１００の剛性や機械的振動によって、異なるライン間での高さ（ｚ値）が揃わずに取得した３次元位置データ全体に歪みが発生するものの、その場足踏み動作を実行させながらスキャンすることで、異なるライン間での高さ（ｚ値）を揃えることができ、全体として整合性のとれた３次元位置データを取得することができる。尚、移動ロボット１００に実行させる動作はその場足踏み動作に限定されず、移動ロボット１００の姿勢を安定させる動作であればよい。

次いで、ステップ１０４では、データ補正部１３は、処理対象領域に含まれる３次元位置データについて、床面領域における３次元位置データの傾き補正量を算出し、当該算出した傾き補正量に基づいて、３次元位置データ群を補正する。ここで、処理対象領域に含まれる３次元位置データは、図１３に示すようにロール方向への歪みを含む。図１３（ａ）は、取得した３次元位置データの歪みを説明するための斜視図である。図１３（ｂ）は、図１３（ａ）に示した３次元位置データ群をｘ軸方向から見た場合の正面図である。図１３（ｂ）に示すように、移動ロボット１００のロール方向への振動によって、ラインＬ上の３次元位置データ全体が回転軸Ｃを中心に歪み、歪みのない本来の床面角度に対して角度偏差が発生する。そこで、最小二乗法により、床面上の３次元位置データからラインＬの傾きを推定して傾き補正量とし、傾きが０となるようにラインＬ上の全ての３次元位置データを回転する。例えば、図１４に示すように、処理対象領域内のラインＬ１上の３次元位置データについて、最小二乗法により、床面領域Ｐ１１上の３次元位置データからラインＬ１の回帰直線ＲＬ１の傾きを推定し、当該傾きが０となるようにラインＬ１上の全ての３次元位置データを回転する。これにより、検出された３次元位置データが高さ方向に水平となるように補正され、平面をより正確に検出することができる。

次いで、ステップ１０５では、データ補正部１３は、床面領域における３次元位置データの高さ補正量を算出し、当該算出した高さ補正量に基づいて、３次元位置データ群を補正する。処理対象領域に含まれる３次元位置データは、図１５に示すように高さ方向への歪みを含む。図１５（ａ）は、取得した３次元位置データの歪みを説明するための斜視図である。図１５（ｂ）は、図１５（ａ）に示した３次元位置データ群をｙ軸方向から見た場合の側面図である。図１５（ｂ）に示すように、移動ロボット１００の剛性や機械的振動によって、異なるライン間での高さ（ｚ値）が揃わずに３次元位置データが歪み、歪みのない本来の高さ（ｚ値）に対して高さ偏差が発生する。そこで、ラインＬ毎にラインＬの高さ偏差を計算し、高さが０となるようにラインＬ上の全ての３次元位置データの高さ偏差を相殺（オフセット）する。より具体的には、処理対象領域内の各ラインＬ上の３次元位置データについて、床面領域Ｐ１１上の３次元位置データから各ラインＬの高さ偏差を算出して高さ補正量とし、当該高さ偏差が０となるようにラインＬ上の全ての３次元位置データの高さを補正する。例えば、図１６は高さ補正処理実行後の様子を説明するための図であり、異なるライン間での高さ（ｚ値）を揃えることができ、全体として整合性のとれた３次元位置データを取得することができる。従って、検出された３次元位置データが高さ方向に床面領域の基準高さとなるように補正され、移動ロボット１００の撓みや足つきに起因する歪みを効果的に補正して、平面をより正確に検出することができる。

次いで、ステップＳ１０６では、環境地図生成部１４は、上述したように３次元位置データから平面を検出し、複数の平面の集合としての環境地図を生成する。

尚、上述した処理は、典型的なコンピュータシステムを用いて実現可能である。具体的には、一定の時間間隔で発生するタイマ割り込みに応じて、図６のフローチャートに示した処理をコンピュータシステムに行わせるためのプログラムをコンピュータシステムに実行させればよい。尚、図６のフローチャートに示した処理をコンピュータシステムに実行させるためのプログラムは、１のプログラムである必要はなく、処理内容により分割された複数のプログラムモジュールによって構成されてもよい。

続いて、図１７及び１８を用いて本発明の実施の形態にかかるデータ補正装置１によって達成されるデータ補正の様子とその効果について説明する。図１７は、取得された３次元位置データ群を示す上視図である。図１７に示すように、床面に平行なｘｙ平面において、移動ロボット１００の移動方向前方に障害物領域Ｐ１２ａ及び１２ｂが存在する。尚、図１７は、床面に平行なｘｙ平面に３次元位置データ群がプロットされたものであり、プロットされた各点について高さ情報（ｚ軸方向の位置情報）が存在する。

図１８は、図１７に示した３次元位置データ群を矢印Ａの方向（ｙ軸方向）から見た場合の側面図である。ここで、図１８（ａ）は補正処理実行以前の歪みを含む状態を示す図である。図１８（ａ）に示すように、検出された３次元位置データ群の高さ成分は水平とならず、歪んだものとなっている。高さ方向の歪みによる高さ誤差は±２３ｍｍとなっており、所望の高さ誤差の範囲内で得ることができていない。このため、このような歪みを含んだ３次元位置データ群に基づいては、正確に平面を検出することができない。一方、図１８（ｂ）は、図１８（ａ）に示した３次元位置データ群に対して、上述した補正処理を実行した結果を示す図である。図１８（ｂ）に示すように、高さ方向の歪みによる高さ誤差は±５ｍｍとなっており、所望の高さ誤差の範囲内で得ることができている。これにより、床面の形状を正確に検出することができるため、段差などの障害物を含む平面を正確に検出することができる。即ち、床面における段差などの障害物を正確に認識することができる。

以上説明したように、取得した３次元位置データ群について、センシング領域の一部である床面領域における３次元位置データの傾き補正量を算出し、当該算出した傾き補正量に基づいて、３次元位置データの全体を補正することで、３次元位置データの全体を容易且つ正確に補正することができる。従って、床面の形状を正確に検出することができるため、段差などの障害物を含む平面を正確に検出することができる。即ち、床面における段差などの障害物を正確に認識することができる。さらに、オドメトリのみに基づいて補正を行った場合と比較して、より精度良く補正することができる。また、上述した補正処理は、リアルタイムにスキャンする３次元位置データについて実行が可能である。

また、取得した３次元位置データ群について、傾き補正量に加えて高さ補正量を算出し、当該算出した高さ補正量に基づいて、３次元位置データの全体をさらに補正することで、検出された３次元位置データが高さ方向に水平である床面領域の基準高さとなるように補正され、移動ロボット１００の撓みや足つきに起因する歪みを効果的に補正して、平面をより正確に検出することができる。

その他の実施の形態．
発明の実施の形態１は、脚式移動ロボットに本発明を適用した一例を示したが、車輪等のその他の移動手段を有する移動ロボットに対しても本発明を適用可能である。

また、上述した実施の形態１では、データ補正装置１による補正処理について説明したが、データ補正装置１による補正処理に加えてオドメトリによる補正処理を実行させるようにしてもよい。オドメトリによる補正処理を実行する場合には、データ補正装置１による補正処理の実行の前後にオドメトリによる補正処理を実行させることができる。

さらに、本発明は上述した実施の形態のみに限定されるものではなく、既に述べた本発明の要旨を逸脱しない範囲において種々の変更が可能であることは勿論である。

本発明の実施の形態にかかる移動ロボットの構成を示す側面図である。本発明の実施の形態にかかる移動ロボットが有する制御系のブロック構成図である。本発明の実施の形態にかかる移動ロボットが移動する際に、レーザレンジファインダによるスキャンの様子を説明するための図である。本発明の実施の形態にかかる３次元位置データ群の一例を示す図である。本発明の実施の形態にかかる移動ロボットが移動する環境の一例及び環境地図の一例を示す図である。本発明の実施の形態にかかるデータ補正装置の処理内容を示すフローチャートである。本発明の実施の形態にかかるレーザレンジファインダを用いたセンシング処理の概要を説明するための図である。本発明の実施の形態にかかるレーザレンジファインダを用いたセンシング処理の概要を説明するための図である。本発明の実施の形態にかかるデータ補正方法を説明するための図である。本発明の実施の形態にかかるデータ補正装置が取得した３次元位置データの分類処理を説明するための図である。本発明の実施の形態にかかるデータ補正装置が取得した３次元位置データの分類処理を説明するための図である。本発明の実施の形態にかかるデータ補正装置が取得した３次元位置データの歪みを説明するための図である。本発明の実施の形態にかかるデータ補正装置が取得した３次元位置データの歪みを説明するための図である。本発明の実施の形態にかかるデータ補正装置による処理対象領域の決定処理を説明するための図である。本発明の実施の形態にかかるデータ補正装置による処理対象領域の決定処理を説明するための図である。本発明の実施の形態にかかるデータ補正装置による３次元位置データの補正処理を説明するための図である。本発明の実施の形態にかかるデータ補正装置による３次元位置データの補正処理を説明するための図である。本発明の実施の形態にかかるデータ補正装置による補正処理の効果を説明するための図である。本発明の実施の形態にかかるデータ補正装置による補正処理の効果を説明するための図である。

符号の説明

１データ補正装置、
１０レーザレンジファインダ、１１３次元位置データ生成部、１２処理領域決定部、１３データ補正部、１４環境地図生成部、１５歩行計画部、１６動作生成部、
１７制御部、１８アクチュエータ、１９エンコーダ

１００移動ロボット、１０４脚部、
１０２右脚、１０２ｃ右膝関節、１０２ｅ右足首関節、１０２ｆ右足平、
１０３左脚、１０３ｃ左膝関節、１０３ｅ左足首関節、１０３ｆ左足平、

１６０環境、１６１障害物、Ｐ１１、Ｐ１２平面、２００環境地図、
Ｐ搭乗者、Ｒ１１、１２領域、Ｌライン、Ｗ幅、ＲＬ回帰直線

Claims

移動ロボットが移動する床面の形状データを検出する床面形状検出センサと、
前記床面形状検出センサにより検出された床面形状データを補正するデータ補正部とを備えた移動ロボットであって、
前記データ補正部は、前記床面形状検出センサのセンシング領域の一部である基準領域における床面形状データの傾き補正量を算出し、当該算出した傾き補正量に基づいて、前記床面形状データの全体を補正する移動ロボット。
前記データ補正部は、前記基準領域における床面形状データが水平面を表すデータとなるように、前記傾き補正量を算出する
ことを特徴とする請求項１記載の移動ロボット。
前記データ補正部は、前記基準領域における床面形状データの高さ補正量を算出し、当該算出した高さ補正量に基づいて、前記床面形状データの全体を補正する
ことを特徴とする請求項１又は２記載の移動ロボット。
前記データ補正部は、前記基準領域における床面形状データが床面を表すデータとなるように、前記高さ補正量を算出する
ことを特徴とする請求項３記載の移動ロボット。
前記移動ロボットは、前記床面形状検出センサのセンシング領域のうち、前記床面上に配置された障害物を含むように処理対象領域を決定する処理領域決定部を更に備え、
前記データ補正部は、前記決定した処理対象領域における前記床面形状データを補正する
ことを特徴とする請求項１乃至４いずれか１項記載の移動ロボット。
前記処理領域決定部は、前記処理対象領域が更に前記基準領域の一部を含むように前記処理対象領域を決定する
ことを特徴とする請求項５記載の移動ロボット。
前記移動ロボットは、前記処理領域決定部により決定された前記処理対象領域が前記基準領域を含まない場合に、前記移動ロボットの姿勢を安定させる動作を行う
ことを特徴とする請求項５又は６記載の移動ロボット。
前記移動ロボットが、移動手段としての複数の脚部を備え、
前記処理領域決定部により決定された前記処理対象領域が前記基準領域を含まない場合に、その場足踏み動作を行う
ことを特徴とする請求項７記載の移動ロボット。
前記床面形状検出センサが、前記移動ロボットの水平軸を中心に揺動可能に取り付けられたレーザレンジファインダであり、
前記データ補正部が、前記レーザレンジファインダによるレーザ光の走査線毎に、当該走査線上に位置する複数の床面形状データのうち前記基準領域における床面形状データに基づいて、前記傾き補正量を算出し、当該算出した傾き補正量に基づいて、前記床面形状データの全体を補正する
ことを特徴とする請求項１記載の移動ロボット。
前記データ補正部が、前記レーザレンジファインダによるレーザ光の走査線毎に、当該走査線上に位置する複数の床面形状データのうち前記基準領域における床面形状データに基づいて、前記基準領域における床面形状データの高さ補正量を算出し、当該算出した高さ補正量に基づいて、前記床面形状データの全体を補正する
ことを特徴とする請求項９記載の移動ロボット。
前記基準領域が前記床面上に配置された障害物を除く領域である
ことを特徴とする請求項１乃至１０いずれか１項記載の移動ロボット。
前記移動ロボットが、移動手段としての２つの脚部を備え、２足歩行運動により移動を行う
ことを特徴とする請求項１乃至１１いずれか１項記載の移動ロボット。
床面形状データを補正する床面形状データの補正方法であって、
移動ロボットが移動する床面の形状データを検出するデータ検出ステップと、
前記検出された床面形状データを補正するデータ補正ステップとを備え、
前記データ補正ステップでは、前記データ検出ステップでの検出対象領域の一部である基準領域における床面形状データの傾き補正量を算出し、当該算出した傾き補正量に基づいて、前記床面形状データの全体を補正する
床面形状データの補正方法。
前記データ補正ステップでは、前記基準領域における床面形状データが水平面を表すデータとなるように、前記傾き補正量を算出する
ことを特徴とする請求項１３記載の床面形状データの補正方法。
前記データ補正部は、前記基準領域における床面形状データが床面を表すデータとなるように高さ補正量を算出し、当該算出した高さ補正量に基づいて、前記床面形状データの全体を補正する
ことを特徴とする請求項１３又は１４記載の床面形状データの補正方法。
前記移動ロボットは、前記床面形状検出センサのセンシング領域のうち、前記基準領域の一部と前記床面上に配置された障害物とを含むように処理対象領域を決定する処理領域決定部を更に備え、
前記データ補正部は、前記決定した処理対象領域における前記床面形状データを補正する
ことを特徴とする請求項１３乃至１５いずれか１項記載の床面形状データの補正方法。
前記移動ロボットは、前記処理領域決定部により決定された前記処理対象領域が前記基準領域を含まない場合に、前記移動ロボットの姿勢を安定させる動作を行う
ことを特徴とする請求項１６記載の床面形状データの補正方法。
前記データ検出ステップでは、前記移動ロボットに設けられた光源からのレーザ光を前記移動ロボットの左右方向へと走査すると共に、前記光源を前記移動ロボットの水平軸を中心に揺動することで前記床面形状データを検出し、
前記データ補正ステップでは、レーザ光の走査線毎に、当該走査線上に位置する複数の床面形状データのうち前記基準領域における床面形状データに基づいて、前記傾き補正量を算出し、当該算出した傾き補正量に基づいて、前記床面形状データの全体を補正する
ことを特徴とする請求項１３記載の床面形状データの補正方法。
前記データ補正部が、前記レーザレンジファインダによるレーザ光の走査線毎に、当該走査線上に位置する複数の床面形状データのうち前記基準領域における床面形状データに基づいて、前記基準領域における床面形状データの高さ補正量を算出し、当該算出した高さ補正量に基づいて、前記床面形状データの全体を補正する
ことを特徴とする請求項１８記載の床面形状データの補正方法。
前記基準領域が前記床面上に配置された障害物を除く領域である
ことを特徴とする請求項１３乃至１９いずれか１項記載の床面形状データの補正方法。
前記移動ロボットが、移動手段としての２つの脚部を備え、２足歩行運動により移動を行う
ことを特徴とする請求項１３乃至２０いずれか１項記載の床面形状データの補正方法。