JP2010112836A - 自己位置同定装置および該自己位置同定装置を備えた移動ロボット - Google Patents

Abstract

【課題】より汎用的な環境において地図を用いずに自己位置を求めることができる自己位置同定装置および、該自己位置同定装置を備えた移動ロボットを提供する。
【解決手段】環境情報の特徴点を抽出する特徴点抽出部１１と、連続する２フレーム間の特徴点集合を対応付ける特徴点対応部１２と、対応点集合から自己移動量を計算する自己移動量演算部１３と、自己移動量を座標変換して自己位置を求める位置変換部１４と、自己位置を記憶する位置記憶部１５と、デッドレコニング値を求めるデッドレコニング演算部１６とを備えた自己位置同定装置において、環境認識センサからの計測データを元に環境情報の特徴線分を抽出する特徴線分抽出部１７と、抽出された特徴線分から仮想特徴点を求める仮想特徴点抽出部１９と、連続する２フレーム間の特徴線分集合を対応付ける特徴線分対応部１８とを備える。
【選択図】図１

Description

本発明は、オフィス、工場等の施設内において、環境情報を計測し、その結果を用いて移動する移動ロボットに搭載された自己位置同定装置に関する。

車輪などの駆動機構によって移動ロボットは、エンコーダ等の内界センサを使って自己位置を求め制御を行っても、車輪と床面との間の滑りの発生などによって目標位置からの位置誤差が大きくなる。そのため、レーザセンサ、超音波センサなどの外界センサを用いて、周囲環境との位置関係を計測して自己位置同定し、目標位置とのずれを補正している。
従来の移動ロボットの自己位置同定では、外界センサとしてレーザレンジファインダのようなレーザセンサを用いて周期的に周囲環境を計測し、１周期分の計測データを１フレームとして、連続する２フレームについて、それぞれにおける特徴点集合を求め、フレーム間で特徴点の対応付けを行い、フレーム間で対応付けされた特徴点間の二乗距離の和を最小化するように移動ロボットの自己移動量（Δｘ，Δｙ，Δθ）を計算し、自己移動量を座標変換し自己位置を求めている（例えば、非特許文献１参照）。
また、レーザセンサによって得られた計測データから線分要素を生成し、線分要素の傾きと線分要素の中点の位置関係から線分要素の融合を行って線分情報を抽出して、予め記憶された地図情報と比較することにより自己位置を同定しているものもある（例えば、特許文献１参照）。

図９は非特許文献１において自己位置を計算する処理手順を示すフローチャートである。この図を用いて順を追って説明する。
ステップ１０１で、レーザセンサによって得られた計測データから特徴点を抽出する。計測データの中には、周囲環境のうち移動ロボットから遠い壁面などを計測したデータも含まれている。それらを排除するため閾値を設け、計測データ中、背景から閾値以上手前に突き出た部分を特徴点とする。
ステップ１０２で、デッドレコニング値を取り込み、今回取得した特徴点に対し座標変換を行い、絶対位置を求める。
なお、絶対位置とは移動ロボットが移動する環境内の予め決められた点を原点とする世界（ワールド）座標系における位置である。これに対し、移動ロボットを原点とするロボット座標系における位置を相対位置とする。
ステップ１０３で、前回の特徴点の絶対位置を取得する。
ステップ１０４で、特徴点の絶対位置を比較し特徴点間の距離が閾値よりも近いもの同士を対応付ける。
ステップ１０５で、ステップ１０４にて対応付けられた特徴点の組Ｐ_ｊ（Ｘ_ｊ，Ｙ_ｊ）（前回のフレームの特徴点）とＰ’_ｊ（Ｘ’_ｊ，Ｙ’_ｊ）（今回のフレームの特徴点）を特徴点の対応点集合として出力する。
ステップ１０６で、自己移動量（Δｘ，Δｙ，Δθ）を次の式（１）〜（３）により計算する。

ただし、式（１）〜（３）において、［・］は、ｊを１からＮまで変化させた場合の総和を表す。ここでＮは対応付けられた特徴点の組の数である。
ステップ１０７で、ステップ１０６で計算された自己移動量を前回の自己位置を用いて座標変換し今回の自己位置を計算する。また、特徴点の計測位置である特徴相対位置と今回の自己位置を使って特徴点の絶対位置も計算する。
ステップ１０８で、ステップ１０７で計算された自己位置および特徴点の絶対位置を保存する。

図１０は特許文献１において自己位置を計算する処理手順を示すフローチャートである。この図を用いて順を追って説明する。
ステップ２０１で、４つの隣接する測定点から線分要素を生成し、線分要素の傾きと線分要素の中点の位置関係から線分要素の融合を行って線分情報を抽出する。また、２線分の距離、角度関係を判定し、オクルージョンなどによって生じる欠損を補間している。
ステップ２０２で、すべての線分について２本ずつの組み合わせを作り、組み合わせられた２線分の長さ、傾き、位置関係から５つの特徴量を抽出する。
ステップ２０３で、地図情報として予め生成されたバイナリトリーの検索を行い、５つの特徴量に近似度の高い検索ユニットを選択する。これにより地図との比較が行われたことになる。そして、選択された検索ユニットから位置候補を算出し、位置候補を密度地図上に割り当て最も密度の高い小領域の位置を最尤位置候補として選定し、自己位置として同定する。
このように、従来の自己位置同定は、外界センサを用いて、環境との位置関係を計測して自己位置を求めているのである。
特開平４−２１６４８７号公報（第３−５頁、図４） 根岸, 三浦, 白井「全方位ステレオとレーザレンジファインダの統合による移動ロボットの地図生成」日本ロボット学会誌Vol. 21 No. 6, pp.690〜696, 2003

しかしながら非特許文献１の自己位置同定は、計測データの中から角部などを特徴点として抽出してフレーム間に対応付けを行っているが、廊下など、移動ロボットの両側に平らな壁面が存在し計測データが直線になっている場合には対応付けができず、自己位置が求められないという問題があった。
また、特許文献１のような場合は、計測データから特徴を求め、地図と比較して自己位置を求めているため、自己位置を求めるのに地図が必要であるという問題があった。また、計測データから線分を抽出して特徴を誘導するが、線分要素の中点の位置関係から線分要素の融合を行って線分情報を得るため、線分要素の長さによって融合の可否の結果が変わってきて正しく線分抽出できないというような問題もあった。また、２線分の距離、角度関係を判定し、欠損を補間しているが、欠損を補間できない場合、特徴量を求められないという問題があった。
さらには、補間する形状を段差や角に限定しているため、実際の形状が補間した形状と異なる場合に地図と比較して誤った結果を出力する可能性があった。
本発明はこのような問題点に鑑みてなされたものであり、角の多い複雑な環境だけでなく、廊下など両側に平らな壁面があるような環境やあるいはそれらが複合した環境、さらには、オクルージョンなどにより欠損がある環境等、より汎用的な環境において地図を用いずに自己位置を求めることができる自己位置同定装置および、該自己位置同定装置を備えた移動ロボットを提供することを目的とする。

上記問題を解決するため、本発明は、次のように構成したのである。
請求項１に記載の発明は、環境認識センサよって周期的に得られる計測データを元に環境情報の特徴点を抽出する特徴点抽出部と、１周期分の前記計測データを１フレームとして、連続する２フレーム間の特徴点集合を対応付ける特徴点対応部と、対応点集合から自己移動量を計算する自己移動量演算部と、前記自己移動量を座標変換して自己位置を求める位置変換部と、前記自己位置を記憶する位置記憶部と、内界センサからの情報を元にデッドレコニング値を求めるデッドレコニング演算部とを備えた自己位置同定装置において、前記環境認識センサからの計測データを元に環境情報の特徴線分を抽出する特徴線分抽出部と、抽出された前記特徴線分から仮想特徴点を求める仮想特徴点抽出部と、前記連続する２フレーム間の特徴線分集合を対応付ける特徴線分対応部と、を備えたことを特徴とするものである。

また、請求項２に記載の発明は、前記特徴線分抽出部は、前記環境認識センサからの計測データを複数の測定点群に分割し、各測定点群内において、測定点の最小二乗近似直線と各測定点との距離が所定の閾値以内となるよう前記測定点群をさらに分割して線分を抽出することを特徴とするものである。

また、請求項３に記載の発明は、前記特徴線分対応部は、前記連続する２フレーム間の特徴線分集合の対応付けにおいて２つの線分の傾きの一致度と線分の端点の絶対位置とを基に端点と線分の重なり具合を評価し、線分に重なった端点ともう一方の線分の両端点の絶対位置から、対応点の相対位置を求めることを特徴とするものである。

また、請求項４に記載の発明は、前記端点と線分の重なり具合の評価は、前記２つの線分の長さを比較して短線分と長線分に選別し、短線分の端点と長線分との距離が所定の閾値以内であるか否かによって行うことを特徴とするものである。

また、請求項５に記載の発明は、前記仮想特徴点抽出部は、前記特徴線分抽出部にて抽出された複数の特徴線分を延長した直線の交点を求め。前記交点を仮想特徴点として抽出することを特徴とするものである。

また、請求項６に記載の発明は、前記特徴点の数と前記仮想特徴点の数との合計が予め定められた定数より大きい場合には、前記特徴線分対応部における特徴線分の対応付けを行わないことを特徴とするものである。

また、請求項７に記載の発明は、前記内界センサとして方位センサを備え、前記自己移動量において、姿勢回転量のみを前記方位センサの情報から求め、今回フレームのデータを前記姿勢回転量だけ回転させたデータから特徴点、特徴線分、および仮想特徴点を求めて前回フレームのデータとの対応付けを行い、前記自己移動量演算部にて、前記対応点集合から各点の並進移動量を求め、平均を取ることにより自己の並進移動量を求め前記姿勢回転量と合わせて自己移動量とすることを特徴とするものである。

また、請求項８に記載の発明は、前記内界センサとして方位センサを備え、前記自己移動量において、姿勢回転量のみを前記方位センサの情報から求め、今回フレームのデータを前記姿勢回転量だけ回転させたデータから特徴点、特徴線分、および仮想特徴点を求めて前回フレームのデータとの対応付けを行い、前記自己移動量演算部にて、前記対応点集合から各点の並進移動量を求め、投票によって最頻値を取ることにより自己の並進移動量を求め前記姿勢回転量と合わせて自己移動量とすることを特徴とするものである。

また、請求項９に記載の発明は、請求項１乃至８に記載された自己位置同定装置を備えたことを特徴とする移動ロボットである。

請求項１乃至８に記載の発明によると、特徴点だけでなく仮想特徴点を含めた線分も併用して対応を行っており、仮想特徴点は、２フレームの計測データのずれ量を求める際の対応点として求めているだけなので実際の形状と一致している必要がなく、オクルージョンなどにより欠損があっても、形状を補間せずに２フレームの計測データのずれ量を正確に求めることができる。そのため、角の多い複雑な環境だけでなく、廊下など両側に平らな壁面があるような環境やあるいはそれらが複合した環境、さらには、欠損がある環境等、より汎用的な環境において地図を用いずに自己位置を求めることができる。
また請求項９に記載の発明によると、自己位置同定装置によって地図を用いずに自己位置を求めて環境内を移動可能な移動ロボットを実現できる。

以下、本発明の実施の形態について図を参照して説明する。

図１は、本発明の自己位置同定装置の内部構成を示す図である。自己位置同定装置２は、例えば、ＣＰＵやメモリを内蔵した計算機であり、特徴点抽出部１１、特徴点対応部１２、自己移動量演算部１３、位置変換部１４、位置記憶部１５、デッドレコニング演算部１６、特徴線分抽出部１７、特徴線分対応部１８、仮想特徴点抽出部１９から構成され、環境認識センサなどの情報を入力して移動ロボットの自己位置を計算し出力する。

特徴点抽出部１１は、環境認識センサからの情報を元に所定の周期で移動ロボット周辺の環境形状を認識し、角部などの特徴点集合を抽出する。
特徴点集合はこの周期に応じて抽出される。定期的に得られる特徴点情報を１フレームとすると、特徴点対応部１２では、その特徴点集合をフレームごとに保存して、時間的に連続する２フレーム間の特徴点集合を特徴点の絶対位置を評価して対応付ける。
自己移動量演算部１３は、特徴点対応部１２から出力された対応点集合を用いてフレーム間で対応付けられた特徴点間の二乗距離の和を最小化するように移動ロボットの自己移動量を計算する。位置変換部１４は、自己移動量演算部１３から出力された自己移動量を座標変換して自己位置を計算する。
また、今回のフレームの特徴点、仮想特徴点、特徴線分端点について、相対位置（以降、特徴相対位置と呼ぶ）を入力し、特徴相対位置と自己位置を使って特徴点、仮想特徴点、特徴線分端点の絶対位置（以降、特徴位置と呼ぶ）も計算する。

位置記憶部１５は、位置変換部１４から出力された自己位置および特徴位置を保存する。デッドレコニング演算部１６は、位置記憶部１５から出力された自己位置にエンコーダなどによる内界センサ情報から求めた移動量を積算することよりデッドレコニング値を計算する。
特徴線分抽出部１７は、特徴点抽出部１１と同様に環境認識センサの情報を元に環境形状を認識するが、特徴点集合ではなく壁面部などの特徴線分集合を抽出する。
特徴線分対応部１８は、特徴点対応部１２のように、連続する２フレーム間の特徴線分集合を線分の傾きと線分の端点の絶対位置を評価して対応付ける。
仮想特徴点抽出部１９は、特徴線分抽出部１７から抽出された特徴線分集合から隣接する２本の特徴線分を取り出し、その２線分をそれぞれ延長した直線の交点を求める。求められた交点を仮想の特徴点として出力する。

図２（ａ）および（ｂ）は、本発明の自己位置同定装置２を搭載した移動ロボットの内部構成を示す図である。移動ロボット１は、自己位置同定装置２、移動ロボット制御装置３、モータ４、エンコーダ５、駆動機構６、環境認識センサ７から構成される。移動ロボット制御装置３は、自己位置同定装置２で計算された自己位置を取得し、指令生成およびサーボ制御を行ってモータ４の駆動制御を行う。モータ４は、駆動機構６を駆動する。エンコーダ５は、モータ４の回転量を計測し内界センサ情報を生成する。内界センサ情報はエンコーダだけでなくジャイロセンサ等の方位センサを移動ロボット１に組み込んで適用するようにしても良い。駆動機構６は、モータ４の出力軸に接続された車輪あるいは操舵軸を複数組有し移動ロボット１を駆動する。
環境認識センサ７は、例えば２次元レーザレンジファインダのような２次元スキャン型距離センサであり、そのレーザ光線が、左右に傾いていない平面（スキャン平面）内において、センサ取付位置を中心にして扇状にスキャンされ、壁などの周囲物体までの距離を計測する。このような構成において、自己位置同定装置２は、環境認識センサ７およびエンコーダ５からの情報を取り込んで自己位置を計算し、移動ロボット１を制御するために必要な情報を生成しているのである。
本発明が従来技術と異なる部分は、自己位置同定装置２に特徴線分抽出部１７、特徴線分対応部１８、仮想特徴点抽出部１９を設けた点である。

図３は自己位置同定装置２において特徴線分を用いて自己位置を計算する処理手順を示すフローチャートである。この図を用いて本発明における処理を順を追って説明する。
なお説明を簡単にするため、環境認識センサ７を原点とするセンサ座標系はロボット座標系と同一であるとする。
ステップ１では、特徴線分抽出部１７にて次に述べるような方法で環境認識センサ７による複数の測定点を線分に分割して直線を抽出する。
まず隣接する測定点間の距離を評価して、連続した測定点群に分割する。すなわち、隣接測定点間の距離が大きい場合、その隣接測定点間で測定対象が不連続になっているものとして、測定点の列を分解して測定点群とする。
測定点列を測定点群に分解する様子を図４に示す。物体１、物体２について環境認識センサ７によって計測を行った場合、物体１と物体２の間で測定点の間の距離が大きくなるため、測定点列が測定点群１、２に分解される。
続いて各測定点群に対し、線分の抽出を行う。線分の抽出はまず、連続する測定点に対する最小二乗近似直線を生成して、測定点との最大距離が予め定められた閾値内であるか評価する。そして最大距離が閾値内であれば隣接する測定点と合併して前記と同様の評価を行う。もし測定点と最小二乗近似直線の距離が閾値を超えた場合は、該測定点の前の測定点で分割し、該測定点を始点として次の線分を抽出する。このような分割を繰り返し行うことで測定点群内において線分を抽出する。

図５（ａ）、（ｂ）は、ある測定点群において線分を抽出する様子を説明する図である。図５（ａ）では測定点２と最小二乗近似直線との距離が閾値を超えているのでその前の測定点１で測定点群を分割し、その時の最小二乗近似直線の一部を線分として抽出する。
そして、測定点２を始点として新たな線分を抽出する。このようにして図５（ｂ）に示すような線分１および線分２が抽出される。線分の端点は隣接する線分を延長して線分同士が交わる場合はその交点とし、交わらない場合あるいは測定点群の始点や終点の場合は分割の始点または終点となる測定点とする。

ステップ２では、特徴線分抽出部１７によって抽出された特徴線分集合から隣接する２本の特徴線分を仮想特徴点抽出部１９にて取り出し、その２線分をそれぞれ延長した直線の交点を求める。求められた交点を仮想特徴点として特徴点対応部１２に出力する。
図６（ａ）（ｂ）は、仮想特徴点を抽出する様子を説明する図であり、図６（ａ）では、線分１と線分２の間に欠損があり、特徴点はないが、それぞれの線分を延長した直線の交点を求めることにより仮想特徴点を求めるようにする。図６（ｂ）でも、図６（ａ）と同様にして仮想特徴点を求めるようにするが、仮想特徴点は図６（ｂ）のように線分４上にあって欠損した部分になくてもよい。なお、特徴点抽出部１１から抽出された特徴点の数と仮想特徴点の数との合計が予め定められた定数より大きければ、ステップ４から６までの処理は行わない。

ステップ３では、デッドレコニング演算部１６からデッドレコニング値を取り込み、今回取得した線分の端点および仮想特徴点に対し座標変換を行い、絶対位置を求める。
ステップ４では、位置記憶部１５から前回の特徴位置すなわち前回の特徴線分集合の端点の絶対位置を取得する。
ステップ５では、特徴線分対応部１８にて、２フレーム間の線分集合対応付けを、線分の傾きの一致度とステップ３、４で求めた端点の位置とから、線分の重なり具合を評価して行う。

図７は、線分集合の対応付けをするときの説明図である。図７のｘｙ座標系は前述の絶対位置の基準となる世界座標系である。
まず各フレームの線分集合から線分を１本ずつ取り出し、取り出された２つの線分の長さを比較して長い方を長線分、短い方を短線分として選別する。その後、長線分と短線分の傾きの一致度と長線分と短線分の重なり具合の評価を行う。線分の傾きの一致度は図７に示すようにｘ軸とのなす角度をそれぞれθ_ｌ、θ_ｓとし、また閾値をｅ_１として次式により評価する。

線分の重なり具合の評価では、図７に示すように短線分の端点と長線分との距離ｒが閾値ｅ_２より小さいかを次式により判定する。ただし、短線分の端点から長線分に降ろした垂線が長線分と交わらない場合（長線分上に交点がない場合）は、除外して該端点に対しては判定を行わない。

上式のｘ_ｌ４、ｙ_ｌ４は端点４のｘ座標、ｙ座標、ｘｉ、ｙｉ（ｉ＝０、１）は短線分の端点のｘ座標、ｙ座標である。判定を短線分の両端点に関し行い、少なくとも１つが式（５）（６）の条件を満たせば長線分と短線分とが重なっていると評価する。
なお長線分と短線分は、正確には図８のように一部がほぼ重なるような位置関係となるが、図７においては長線分、短線分の各端点や距離ｒについての説明を分かり易くするために拡大して長線分、短線分を離して描いている。

続いてステップ６で、長線分に重なった短線分の端点の絶対位置と長線分の両端点の絶対位置とから長線分上の対応点（図８の端点１’）の相対位置（ロボット座標系位置）を求める。
図８は対応点の位置を求める説明図であり、この図をもとに説明する。端点１に対応する端点１’の絶対位置は同じであるので、図８のように長線分に重なった端点１と長線分の両端点３、４の絶対位置から端点１’が長線分上のどこにあるか、すなわち、式（７）で表される長線分の式における直線パラメータｔを式（８）により求める。

ただし、ｐ’は端点１’の絶対位置を表すベクトル、ｄ’は端点４（絶対位置）からみた端点３（絶対位置）の位置ベクトル、ａ’は端点４の絶対位置を表すベクトル、ｅ’は端点４（絶対位置）からみた端点１（絶対位置）の位置ベクトル、| |はベクトルの大きさを表す。その後、各端点の相対位置は環境認識センサの計測データよりわかっているため、式（８）で求められた直線パラメータｔから端点１’の相対位置を次式により求める。

ただし、ｐは端点１’の相対位置を表すベクトル、ｄは端点４（相対位置）からみた端点３（相対位置）の位置ベクトル、ａは端点４の相対位置を表すベクトルである。
そして、端点１と端点１’のうち、前回値と今回値を対応させることにより、特徴線分の対応点の組Ｐ_ｊ（Ｘ_ｊ，Ｙ_ｊ）（前回値）とＰ’_ｊ（Ｘ’_ｊ，Ｙ’_ｊ）（今回値）が特徴線分の対応点集合として求められる。
具体的には、端点１が元々前回値のデータであれば端点１をＰに追加登録し端点１’をＰ’に追加登録する。端点１が今回値ならば逆に端点１をＰ’に登録し、端点１’をＰに登録する。

ステップ７では、ステップ６で求めた特徴線分の対応点集合を特徴点および仮想特徴点の対応点集合に加えて対応点集合として出力する。ステップ８で、自己移動量（Δｘ，Δｙ，Δθ）を式（１）〜（３）により計算する。
ステップ９で、位置変換部１４にて、ステップ８で計算された自己移動量を前回の自己位置を用いて座標変換し今回の自己位置（絶対位置）を計算する。また、今回の特徴相対位置と自己位置を使って特徴位置も計算する。
ステップ１０で、ステップ９で計算された自己位置および特徴位置を位置記憶部１５に保存する。

このように、本発明では特徴線分抽出部１７、特徴線分対応部１８、仮想特徴点抽出部１９によって特徴点だけでなく仮想特徴点を含めた線分も併用して対応を行っており、仮想特徴点は、２フレームの計測データのずれ量を求める際の対応点として求めているだけなので実際の形状と一致している必要がなく、オクルージョンなどにより欠損があっても、形状を補間せずに２フレームの計測データのずれ量を正確に求めることができる。そのため、角の多い複雑な環境だけでなく、廊下など両側に平らな壁面がある環境や、それらが複合した環境、さらには、欠損がある環境等、より汎用的な環境において地図を用いずに自己位置を求めることができる。

実施例１では、自己移動量演算部１３において特徴点対応部１２から出力された対応点集合を用いて式（１）〜（３）により自己移動量（Δｘ，Δｙ，Δθ）を計算していたが、本実施例ではジャイロ等の方位センサを具備した構成にし、姿勢回転量Δθは、方位センサから取得される姿勢θの値、すなわち前回フレーム計測時の値と今回フレーム計測時の値との差分として求め、今回フレームのデータをΔθだけ回転させたデータから特徴点、特徴線分、および仮想特徴点を求めて、前回フレームのデータとの対応付けを行う。
その後自己移動量演算部１３にて対応点集合Ｐ_ｊ（Ｘ_ｊ，Ｙ_ｊ）（前回値）とＰ’_ｊ（Ｘ’_ｊ，Ｙ’_ｊ）（今回値）から各点の並進移動量（ΔＸ_ｊ，ΔＹ_ｊ）＝（Ｘ_ｊ−Ｘ’_ｊ，Ｙ_ｊ−Ｙ’_ｊ）を求め、次式のように平均を取ることにより自己の並進移動量Δｘ，Δｙを求め、Δθと合わせて自己移動量とする。

ただし、式（１０）（１１）において、［・］は、ｊを１からＮまで変化させた場合の総和を表す。ここでＮは対応付けられた特徴点の組の数である。
また、平均を取って自己の並進移動量を求める代わりに、各点の並進移動量（ΔＸ_ｊ，ΔＹ_ｊ）を精度、処理速度に応じて単位を変えて整数化し、整数化されたΔＸ_ｊ，ΔＹ_ｊに関し、投票で最頻値を取ることにより自己の並進移動量を求めてもよい。

本発明の自己位置同定装置の装置構成図 本発明の自己位置同定装置を搭載した移動ロボットの装置構成図 本発明の自己位置同定装置の処理手順を示すフローチャート 本発明の測定点列を測定点群に分解する説明図 本発明の測定点群から線分を抽出する説明図 本発明の仮想特徴点を抽出する説明図 本発明の線分集合対応付けの説明図 本発明の対応端点の位置を求める説明図 従来例１の自己位置同定計算の処理手順を示すフローチャート 従来例２の自己位置同定計算の処理手順を示すフローチャート

符号の説明

１ 移動ロボット
２ 自己位置同定装置
３ 移動ロボット制御装置
４ モータ
５ エンコーダ
６ 駆動機構
７ 環境認識センサ
１１ 特徴点抽出部
１２ 特徴点対応部
１３ 自己移動量演算部
１４ 位置変換部
１５ 位置記憶部
１６ デッドレコニング演算部
１７ 特徴線分抽出部
１８ 特徴線分対応部
１９ 仮想特徴点抽出部

Claims (9)

1. 環境認識センサよって周期的に得られる計測データを元に環境情報の特徴点を抽出する特徴点抽出部と、
   １周期分の前記計測データを１フレームとして、連続する２フレーム間の特徴点集合を対応付ける特徴点対応部と、
   対応点集合から自己移動量を計算する自己移動量演算部と、
   前記自己移動量を座標変換して自己位置を求める位置変換部と、
   前記自己位置を記憶する位置記憶部と、
   内界センサからの情報を元にデッドレコニング値を求めるデッドレコニング演算部とを備えた自己位置同定装置において、
   前記環境認識センサからの計測データを元に環境情報の特徴線分を抽出する特徴線分抽出部と、
   抽出された前記特徴線分から仮想特徴点を求める仮想特徴点抽出部と、
   前記連続する２フレーム間の特徴線分集合を対応付ける特徴線分対応部と、
   を備えたことを特徴とする自己位置同定装置。
2. 前記特徴線分抽出部は、前記環境認識センサからの計測データを複数の測定点群に分割し、各測定点群内において、測定点の最小二乗近似直線と各測定点との距離が所定の閾値以内となるよう前記測定点群をさらに分割して線分を抽出することを特徴とする請求項１記載の自己位置同定装置。
3. 前記特徴線分対応部は、前記連続する２フレーム間の特徴線分集合の対応付けにおいて
   ２つの線分の傾きの一致度と線分の端点の絶対位置とを基に端点と線分の重なり具合を評価し、
   線分に重なった端点ともう一方の線分の両端点の絶対位置から、対応点の相対位置を求めることを特徴とする請求項１記載の自己位置同定装置。
4. 前記端点と線分の重なり具合の評価は、前記２つの線分の長さを比較して短線分と長線分に選別し、短線分の端点と長線分との距離が所定の閾値以内であるか否かによって行うことを特徴とする請求項３記載の自己位置同定装置。
5. 前記仮想特徴点抽出部は、前記特徴線分抽出部にて抽出された複数の特徴線分を延長した直線の交点を求め。前記交点を仮想特徴点として抽出することを特徴とする請求項１記載の自己位置同定装置。
6. 前記特徴点の数と前記仮想特徴点の数との合計が予め定められた定数より大きい場合には、前記特徴線分対応部における特徴線分の対応付けを行わないことを特徴とする請求項１記載の自己位置同定装置。
7. 前記内界センサとして方位センサを備え、前記自己移動量において、姿勢回転量のみを前記方位センサの情報から求め、今回フレームのデータを前記姿勢回転量だけ回転させたデータから特徴点、特徴線分、および仮想特徴点を求めて前回フレームのデータとの対応付けを行い、
   前記自己移動量演算部にて、前記対応点集合から各点の並進移動量を求め、平均を取ることにより自己の並進移動量を求め前記姿勢回転量と合わせて自己移動量とすることを特徴とする請求項１乃至６のいずれか１項記載の自己位置同定装置。
8. 前記内界センサとして方位センサを備え、前記自己移動量において、姿勢回転量のみを前記方位センサの情報から求め、今回フレームのデータを前記姿勢回転量だけ回転させたデータから特徴点、特徴線分、および仮想特徴点を求めて前回フレームのデータとの対応付けを行い、
   前記自己移動量演算部にて、前記対応点集合から各点の並進移動量を求め、投票によって最頻値を取ることにより自己の並進移動量を求め前記姿勢回転量と合わせて自己移動量とすることを特徴とする請求項１乃至６のいずれか１項記載の自己位置同定装置。
9. 請求項１乃至８に記載された自己位置同定装置を備えたことを特徴とする移動ロボット。