JP6528641B2 - 自己位置推定方法 - Google Patents

Description

本発明は、自己位置推定方法に関し、特に、三次元点群をマッチングすることで自己位置を推定する技術に関する。

移動体の自己位置推定（ＳＬＡＭ：Simultaneous Localization And Mapping）には、距離センサを用いた技術が数多く提案されている（例えば、特許文献１）。しかし、距離センサは、一般的に、距離センサからの距離が離れるに従って、測定距離の誤差が大きくなる、ノイズが乗りやすくなる、といった問題がある。

特開２０１０−２３８２１７号公報

このような問題に対処する方法として、距離センサの出力として得られる距離データを用いて、環境地図に対して移動体の位置合わせを行って自己位置を推定するときに、その距離データのうち、ある一定距離以上のデータを破棄し、残りのデータを用いて位置合わせを行う方法が考えら得る。しかし、この方法では、距離センサに近い位置に幾何学的特長がない状況においては、位置合わせの精度が低下してしまうという問題がある。

例えばＩＣＰ（Iterative Closest Point）による位置合わせでは、データ上に特徴的な形状が存在することが重要である。例えば、図８に示すように、幾何学的特長がない平面形状を示すデータ同士では、移動体を正しい位置に合わせることができているか否かを判別することができない。それに対して、幾何学的特長がある形状（例えば凸部分がある形状）を示すデータ同士では、凸が重なる状態が正しく位置合わせができている状態として判別することができる。

このように、距離センサによって得られる距離データにおいて、一定距離以上のデータの精度が低いといっても、上述の点について考慮なくデータを破棄してしまうと、位置合わせの精度が低下し、自己位置の推定の精度が低下してしまうという問題がある。

本発明は、自己位置の推定の精度を向上することができる自己位置推定方法を提供するものである。

本発明の態様に係る自己位置推定方法は、距離センサによって周辺を観測することで、周辺表面を示す複数の三次元点を取得し、前記距離センサ又は他のセンサによる周辺の観測結果に基づいて、前記複数の三次元点の特徴量を算出し、前記複数の三次元点のうち、前記距離センサからの距離が所定の距離閾値未満である複数の三次元点と、前記距離センサからの距離が前記距離閾値以上であっても前記特徴量が特徴量閾値以上である複数の三次元点が所定数以上となる距離区間に含まれる複数の三次元点とを抽出し、前記抽出した複数の三次元点と、環境地図が示す複数の三次元点又は面とをマッチングすることで自己位置を推定するものである。

すなわち、近い位置にある三次元点の他に、遠い位置にある幾何学的特徴がある三次元点もマッチングに利用する三次元点として採用するようにしている。これによれば、距離センサに近い位置に幾何学的特徴がなく、遠い位置にある三次元点に測定誤差やノイズがある場合であっても、自己位置の推定の精度を向上することができる。

上述した本発明の態様によれば、自己位置の推定の精度を向上することができる自己位置推定方法を提供することができる。

実施の形態１に係るロボットの外部構成を示す図である。 実施の形態１に係るロボットの内部構成を示す図である。 実施の形態１に係るロボットのマッチングに利用する三次元点の判定方法を示す図である。 実施の形態２に係るロボットのマッチングに利用する三次元点の判定方法を示す図である。 実施の形態３に係るロボットのマッチングに利用する三次元点の判定方法を示す図である。 実施の形態４に係るロボットの内部構成を示す図である。 実施の形態５に係るロボットの内部構成を示す図である。 実施の形態で解決する課題を説明するための図である。

以下に図面を参照しながら、本発明の好適な実施の形態について説明する。以下の実施の形態に示す具体的な数値等は、発明の理解を容易とするための例示にすぎず、特に断る場合を除き、それに限定されるものではない。また、以下の記載及び図面では、説明の明確化のため、当業者にとって自明な事項等については、適宜、省略及び簡略化がなされている。

＜発明の実施の形態１＞
まず、図１を参照して、本実施の形態１に係るロボット１の外部構成について説明する。

ロボット１は、本体１０と、移動台車２０と、頭部３０と、アーム４０とを有している。ロボット１は、例えば、家庭内において人が行う作業を、人に代わり実行するための生活支援ロボットである。ロボット１は、例えば、任意の場所に移動し、物を拾い人に届ける、窓を開ける等の作業を実行する。

移動台車２０は、その上方で本体１０を支持するように、その上部が本体１０の下部に連結されている。移動台車２０は、その下部に設けられた車輪（図示せず）を駆動することでロボット１を移動させる。

頭部３０は、本体１０の上方で本体１０に支持されるように、本体１０の上部に連結されている。頭部３０は、その前方を距離センサ３１によって観測可能となるように、その前部に距離センサ３１が取り付けられている。距離センサ３１は、ロボット１の周辺を観測し、観測した周辺の各物体における各点までの距離を示す距離データを生成するセンサである。

アーム４０は、本体１０の前方に突出するように、本体１０の前部に接続されている。アーム４０は、多関節のアームである。アーム４０は、その先端部に、任意の物体を把持できるハンドを有している。これにより、ロボット１は、アーム４０の各関節の角度を調整することで、ハンドを所望の位置に移動させ、その位置に存在する物体をハンドで把持することができる。

距離センサ３１は、ロボット１の周辺を観測し、ロボット１（距離センサ３１）から、観測した周辺の各物体における各点までの距離を示す距離データを生成するセンサである。距離センサ３１は、距離データとして、距離画像（ポイントクラウド）を示す距離画像データを生成する。すなわち、距離データは、ロボット１（距離センサ３１）に対する周辺の各物体表面の点群を三次元で示す。以下、この点を「三次元点」とも呼ぶ。

距離センサ３１は、単体で構成されるセンサであってもよく、システムで構成されるセンサであってもよい。単体で構成されるセンサには、例えば、Ｋｉｎｅｃｔ（登録商標）又はＶｅｌｏｄｙｎｅ社のＬｉＤＡＲ等がある。また、システムで構成されるセンサには、例えば、ステレオカメラ又は搖動機構付ＬＲＦ（Laser Range Finder）等がある。

上述した構成により、ロボット１は、センサ３１によってロボット１の周辺を観測し、観測結果に基づいて、障害物となる物体を避けて移動する。

続いて、図２を参照して、本実施の形態１に係るロボット１の内部構成について説明する。

図２に示すように、ロボット１は、制御部１１と、アクチュエータ２１ａ、２１ｂと、車輪２２ａ、２２ｂと、距離センサ３１とを有する。制御部１１は、本体１０に含まれる。アクチュエータ２１ａ、２１ｂ及び車輪２２ａ、２２ｂは、移動台車２０に含まれる。距離センサ３１は、頭部３０に含まれる。

制御部１１は、ロボット１を統括的に制御する。制御部１１は、ＣＰＵ（Central Processing Unit）及び記憶部を有しており（図示せず）、そのＣＰＵが記憶部に格納されたプログラムを実行することで、制御部１１としての各種処理を実現する。すなわち、このプログラムは、制御部１１が有するＣＰＵに制御部１１としての各所処理を実行させるためのコードを含んでいる。ここで、記憶部は、上記プログラムや、その他の制御部１１が使用する各種情報が格納される。記憶部は、上記プログラムや各種情報を格納可能な記憶装置の少なくとも１つを含む。記憶装置として、例えば、メモリ及びハードディスク等の少なくとも１つを任意に使用してよい。

制御部１１は、距離センサ３１から出力された距離データを取得し、取得した距離データに基づいて、障害物となる物体を避けて移動する移動経路を算出し、算出した移動経路をロボット１が移動するように、アクチュエータ２１ａ、２１ｂを制御する。

アクチュエータ２１ａ、２１ｂのそれぞれは、ロボット１の車輪２２ａ、２２ｂのそれぞれを駆動する。アクチュエータ２１ａは、制御部１１からの制御によって駆動され、車輪２２ａを回転させる。アクチュエータ２１ｂは、制御部１１からの制御によって駆動され、車輪２２ｂを回転させる。車輪２２ａ及び車輪２２ｂは、上述した移動台車２０の下部に取り付けられた車輪に該当する。

ここで、制御部１１は、勾配計算部１２と、利用判定部１３と、マッチング処理部１４として機能する。

勾配計算部１２は、距離センサ３１から取得した距離データが示す複数の三次元点のそれぞれについて、その三次元点の特徴量として、特定の方向に隣接する他の三次元点までの距離に対する、その三次元点までの距離の勾配を算出する。なお、ここでの距離とは、距離センサ３１によって観測した、上記ロボット１（距離センサ３１）から三次元点までの距離である。

利用判定部１３は、勾配計算部１２が計算した複数の三次元点のそれぞれの勾配の値に基づいて、その複数の三次元点の中から、環境地図とのマッチングに利用する複数の三次元点を判定する。ここで、環境地図とは、ロボット１の移動範囲内における各物体表面の三次元点群を地図として示す情報である。この環境地図は、予め用意された地図であってもよく、例えばＳＬＡＭ等の手法によって適宜更新される地図であってもよい。

マッチング処理部１４は、距離センサ３１から取得した距離データが示す複数の三次元点のうち、利用判定部１３がマッチングに利用すると判定した複数の三次元点と、環境地図が示す複数の三次元点とをマッチングすることで、自己位置（ロボット１の位置）を推定する。なお、マッチングの手法としては、例えば、ＩＣＰによる収束演算を利用する。このＩＣＰの手法として、Ｐｏｉｎｔ−ｔｏ−Ｐｌａｎｅ又はＰｏｉｎｔ−ｔｏ−Ｐｏｉｎｔ等の手法を利用してよい。よって、Ｐｏｉｎｔ−ｔｏ−Ｐｌａｎｅを利用する場合には、環境地図は、ロボット１の移動範囲内における各物体表面の面を地図として示す情報となる。

よって、制御部１０は、マッチング処理部１４による自己位置の推定結果に基づいて、障害物となる物体を避けて移動する移動経路を算出する。

続いて、図３を参照して、本実施の形態１に係る利用判定部１３におけるマッチングに利用する三次元点の判定方法について説明する。

まず、上述したように、距離センサ３１は、距離センサ３１（ロボット１）からの距離が近い場合には、測定距離の誤差も少なく、ノイズも少ない。よって、利用判定部１３は、図３に示すように、距離センサ３１（ロボット１）からの距離が所定の距離閾値未満である複数の三次元点は、マッチングに利用する三次元点として判定する。

一方で、図８を参照して説明したように、幾何学的特徴がある三次元点群は、ロボット１の位置合わせが正しく行われていることを判別しやすい。そのため、利用判定部１３は、図３に示すように、距離が距離閾値以上であっても、所定の単位距離当たりにおける、特徴量（勾配の値）が所定の特徴量閾値以上となる三次元点の数が特徴数閾値以上となる区間（距離センサ３１からの距離方向の区間）に含まれる複数の三次元点も、マッチングに利用する三次元点として判定する。なお、この特徴数閾値は、予め定められた値であってもよく、距離データが示す複数の三次元点数の所定割合として利用判定部１３によって算出される値であってもよい。

以上に説明したように、本実施の形態１では、距離センサによって周辺を観測することで、周辺表面を示す複数の三次元点を取得し、距離センサ又は他のセンサによる周辺の観測結果に基づいて、複数の三次元点の特徴量を算出し、複数の三次元点のうち、距離センサからの距離が所定の距離閾値未満である複数の三次元点と、距離センサからの距離が距離閾値以上であっても特徴量が特徴量閾値以上である複数の三次元点が所定数以上となる距離区間に含まれる複数の三次元点とを抽出し、抽出した複数の三次元点と、環境地図が示す複数の三次元点又は面とをマッチングすることで自己位置を推定するようにしている。

すなわち、近い位置にある三次元点の他に、遠い位置にある幾何学的特徴がある三次元点もマッチングに利用する三次元点として採用するようにしている。これによれば、距離センサ３１（ロボット１）に近い位置に幾何学的特徴がなく、遠い位置にある三次元点に測定誤差やノイズがある場合であっても、自己位置の推定の精度を向上することができる。

＜発明の実施の形態２＞
続いて、実施の形態２について説明する。以下の説明では、実施の形態１と同様の内容については、同一の符号を付す等して、適宜その説明を省略する。本実施の形態２に係るロボット１の外部構成及び内部構成については、実施の形態１に係るロボット１の外部構成及び内部構成と同様であるため、説明を省略する。

本実施の形態２は、実施の形態１と比較して、利用判定部１３によるマッチングに利用する三次元点の判定方法が異なる。図４を参照して、本実施の形態２に係る利用判定部１３におけるマッチングに利用する三次元点の判定方法について説明する。

本実施の形態２に係る利用判定部１３は、図４に示すように、距離センサ３１から取得した距離データが示す複数の三次元点のうち、特徴量（勾配の値）が所定の特徴量閾値以上である三次元点と、その三次元点を中心とした判定距離内に含まれる複数の三次元点とを、マッチングに利用する三次元点として決定する。ここで、上述の判定距離は、所定の値であってもよく、中心となる三次元点の特徴量（勾配の値）に比例するように利用判定部１３が決定してもよい。より具体的には、三次元点を中心とした判定距離内に含まれる複数の三次元点とは、その三次元点を中心として、上記判定距離を半径とした球に含まれる複数の三次元点となる。

ここで、上述の実施の形態２に係る判定方法は、実施の形態１に係る判定方法と組み合わせて利用するようにしてもよい。すなわち、利用判定部１３は、距離が所定の距離閾値未満である複数の三次元点と、距離が距離閾値以上であっても特徴量が特徴量閾値（第１の特徴量閾値）以上である複数の三次元点が所定数以上となる距離区間に含まれる複数の三次元点との中から、上述の特徴量（勾配の値）が所定の特徴量閾値（第２の特徴量閾値）以上である三次元点を判定するようにしてもよい。

言い換えると、利用判定部１３は、距離が所定の距離閾値未満である複数の三次元点以外、かつ、距離が距離閾値以上であっても特徴量が特徴量閾値（第１の特徴量閾値）以上である複数の三次元点が所定数以上となる距離区間に含まれる複数の三次元点以外となる三次元点は、上述の特徴量（勾配の値）が所定の特徴量閾値（第２の特徴量閾値）以上である三次元点を判定する対象から除外してもよい。

なお、上述の第１の特徴量閾値と、上述の第２の特徴量閾値とは、同一の値であってもよく、異なる値であってもよい。

以上に説明したように、本実施の形態２では、複数の三次元点のうち、特徴量（勾配の値）が所定の特徴量閾値以上である三次元点と、その三次元点を中心とした判定距離内に含まれる複数の三次元点とを抽出し、抽出した複数の三次元点と、環境地図が示す複数の三次元点とをマッチングすることで自己位置を推定するようにしている。

すなわち、幾何学的特徴がある三次元点をマッチングに利用する三次元点として採用するようにしている。これによれば、自己位置の推定の精度を向上することができる。

＜発明の実施の形態３＞
続いて、実施の形態３について説明する。以下の説明では、実施の形態１と同様の内容については、同一の符号を付す等して、適宜その説明を省略する。本実施の形態３に係るロボット１の外部構成及び内部構成については、実施の形態１に係るロボット１の外部構成及び内部構成と同様であるため、説明を省略する。

本実施の形態３は、実施の形態１と比較して、利用判定部１３によるマッチングに利用する三次元点の判定方法、及び、マッチング処理部１４の処理内容が異なる。図４を参照して、本実施の形態３に係る利用判定部１３によるマッチングに利用する三次元点の判定方法、及び、マッチング処理部１４の処理内容について説明する。

本実施の形態３に係る利用判定部１３は、距離センサ３１から取得した距離データが示す複数の三次元点を、マッチングに利用する三次元点として決定する。また、利用判定部１３は、図４に示すように、複数の三次元点のそれぞれについて、その三次元点が含まれる所定の単位距離当たりにおける、特徴量（勾配の値）が所定の特徴量閾値以上となる三次元点の数に比例する重み係数を算出する。

本実施の形態３に係るマッチング処理部１４は、利用判定部１３が算出した重み係数に基づいて、重み係数が大きい三次元点はマッチングにおいて位置合わせが強く行われ、重み係数が小さい三次元点はマッチングにおいて位置合わせが弱く行われるように、マッチングを行う。言い換えると、マッチング処理部１４は、三次元点の重み係数が大きくなるにしたがって、その三次元点と、環境地図においてその三次元点に対応する三次元点とを、より近くに位置合わせするように、ロボット１の位置合わせを行う。

これは、例えば、ＩＣＰによる収束演算において、距離データの各三次元点と、環境地図の各三次元点との距離の２乗和を最小化する計算の際に算出する三次元点間の誤差に重み係数を乗算することで実現する。よって、特徴量（勾配の値）が所定の特徴量閾値以上となる三次元点の数が０となる単位距離に含まれる三次元点については、重み係数も０となり、位置合わせに全く利用されなくなる。

ここで、上述の実施の形態３に係る判定方法は、実施の形態１に係る判定方法と組み合わせて利用するようにしてもよい。すなわち、利用判定部１３は、距離が所定の距離閾値未満である複数の三次元点と、距離が距離閾値以上であっても特徴量が特徴量閾値以上である複数の三次元点が所定数以上となる距離区間に含まれる複数の三次元点とを、マッチングに利用する三次元点として決定し、その三次元点のみについて重み係数を算出するようにしてもよい。

以上に説明したように、本実施の形態３では、複数の三次元点のそれぞれについて、その三次元点が含まれる所定の単位距離当たりにおける、特徴量が所定の特徴量閾値以上となる三次元点の数に比例する重み係数を算出するようにしている。そして、三次元点の重み係数が大きくなるにしたがって、その三次元点と、環境地図においてその三次元点に対応する三次元点とを、より近くに位置合わせするように、複数の三次元点と、環境地図が示す複数の三次元点とをマッチングすることで自己位置を推定するようにしている。

すなわち、幾何学的特徴がある三次元点について位置合わせが強く行われるように、マッチングを行っている。これによれば、自己位置の推定の精度を向上することができる。

＜発明の実施の形態４＞
続いて、実施の形態４について説明する。以下の説明では、実施の形態１と同様の内容については、同一の符号を付す等して、適宜その説明を省略する。

続いて、図６を参照して、本実施の形態４に係るロボット１の内部構成について説明する。

図６に示すように、本実施の形態４に係るロボット１は、さらに、画像センサ３２を有する。また、本実施の形態４に係るロボット１は、実施の形態１に係るロボット１と比較して、勾配計算部１２に代えて、勾配計算部１５と、センサ間対応付け部１６とを有する。画像センサ３２は、頭部３０に含まれる。また、制御部１１が、勾配計算部１５及びセンサ間対応付け部１６として機能する。

なお、画像センサ３２は、ロボット１の頭部３０において、距離センサ３１と同様の位置に取り付けられることになるため、本実施の形態４に係るロボット１の外部構成については、図示しての説明は省略する。

画像センサ３２は、ロボット１の周辺を観測（撮像）し、観測（撮像）した周辺の画像を示す画像データを生成するセンサである。画像センサ３２は、例えば、単眼カメラである。

勾配計算部１５は、画像センサ３２から生成された画像データが示す画像における複数の画素のそれぞれについて、その画素の特徴量として、その画素と、特定の方向に隣接する他の画素との間の画素値の勾配を算出する。

センサ間対応付け部１６は、距離センサ３１から取得した距離データが示す複数の三次元点のそれぞれについて、その三次元点の特徴量として、勾配計算部１５が算出した特徴量（勾配の値）のうち、その三次元点に対応する画素の特徴量を取得する。これは、複数の三次元点について、距離データにおける三次元点と、この三次元点と同位置の画素とを対応付けた情報を制御部１１の記憶部に予め格納しておき、センサ間対応付け部１６が、その情報に基づいて実施すればよい。

以降の処理については、三次元点の特徴量として、距離の勾配の値に代えて、画素値の勾配の値を利用すること以外は、実施の形態１と同様であるため、説明を省略する。

以上に説明したように、三次元点の特徴量として、距離の勾配の値に代えて、画素値の勾配の値を利用することもできる。

＜発明の実施の形態５＞
続いて、実施の形態５について説明する。以下の説明では、実施の形態４と同様の内容については、同一の符号を付す等して、適宜その説明を省略する。本実施の形態５に係るロボット１の外部構成については、実施の形態５に係るロボット１の外部構成と同様であるため、説明を省略する。

続いて、図７を参照して、本実施の形態５に係るロボット１の内部構成について説明する。

図７に示すように、本実施の形態５に係るロボット１は、実施の形態４に係るロボット１と比較して、勾配計算部１５に代えて、画像特徴抽出部１７を有する。制御部１１が、画像特徴抽出部１７として機能する。

画像特徴抽出部１７は、画像センサ３２から生成された画像データが示す画像における複数の画素のそれぞれについて、その画素の特徴量として、Ｃｏｌｏｒ−ＳＩＦＴ特徴量などの既知の画像特徴量のうち、任意の画像特徴量を算出する。

以降の処理については、三次元点の特徴量として、画素値の勾配の値に代えて、任意の画素特徴量を利用すること以外は、実施の形態４と同様であるため、説明を省略する。

以上に説明したように、三次元点の特徴量として、画素値の勾配の値に代えて、任意の画素特徴量を利用することもできる。

なお、本発明は上記の実施の形態に限られたものではなく、趣旨を逸脱しない範囲で適宜変更することが可能である。

１ ロボット
１０ 本体
１１ 制御部
１２、１５ 勾配計算部
１３ 利用判定部
１４ マッチング処理部
１６ センサ間対応付け部
１７ 画像特徴抽出部
２０ 移動台車
２１ａ、２１ｂ アクチュエータ
２２ａ、２２ｂ 車輪
３０ 頭部
３１ 距離センサ
３２ 画像センサ
４０ アーム

Claims (1)

1. 距離センサによって周辺を観測することで、周辺表面を示す複数の三次元点を取得し、
   前記距離センサ又は他のセンサによる周辺の観測結果に基づいて、前記複数の三次元点の特徴量を算出し、
   前記複数の三次元点のうち、前記距離センサからの距離が所定の距離閾値未満である複数の三次元点と、前記距離センサからの距離が前記距離閾値以上であっても前記特徴量が第１の特徴量閾値以上である複数の三次元点が所定数以上となる距離区間に含まれる複数の三次元点とを抽出し、
   さらに、前記抽出した複数の三次元点の中から、前記特徴量が第２の特徴量閾値以上である複数の三次元点を抽出し、
   前記第２の特徴量閾値に基づき前記抽出した複数の三次元点及びそのそれぞれを中心とした所定判定距離内に含まれる複数の三次元点と、環境地図が示す複数の三次元点又は面と、をマッチングすることで自己位置を推定する
   自己位置推定方法。

Images