障害物回避支援装置、障害物回避支援方法及び移動体
【課題】最近接距離の点の切り替わりが発生する状況において、移動体の障害物の回避動作を支援するのに好適な障害物回避支援装置、障害物回避支援方法及び障害物回避支援装置を備えた移動体を提供する。
【解決手段】障害物回避支援部１０１を、レーザーレンジセンサ２００で測定した障害物の距離情報をメモリに記憶する距離情報記憶部１１０と、メモリに記憶された距離情報に基づき斥力を計算する斥力計算部１１１と、計算された斥力を軸方向の成分に分離する斥力成分分離部１１２と、分離された軸成分から軸毎に正方向の最大値と負方向の最大値とを抽出し、軸毎に抽出した正方向と負方向の最大値の和を計算し、計算結果を各軸の軸成分とした回避用斥力を生成する回避用斥力生成部１１３とを含む構成とした。
【解決手段】障害物回避支援部１０１を、レーザーレンジセンサ２００で測定した障害物の距離情報をメモリに記憶する距離情報記憶部１１０と、メモリに記憶された距離情報に基づき斥力を計算する斥力計算部１１１と、計算された斥力を軸方向の成分に分離する斥力成分分離部１１２と、分離された軸成分から軸毎に正方向の最大値と負方向の最大値とを抽出し、軸毎に抽出した正方向と負方向の最大値の和を計算し、計算結果を各軸の軸成分とした回避用斥力を生成する回避用斥力生成部１１３とを含む構成とした。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、ポテンシャル法を用いた移動体の障害物の回避技術に関する。
【背景技術】
【０００２】
従来、移動型ロボット等のアクチュエータ等の動力によって移動が可能な移動体において、その移動経路上に存在する障害物を回避する経路計画法として、ポテンシャル法が用いられている。
ポテンシャル法を用いた経路計画では、移動目標位置からは引力を、障害物からは斥力を受けるポテンシャル関数を設計し、設計したポテンシャル関数の低い方向に移動体を移動させることで、障害物を避けながら目標位置まで到達できる経路を生成している。
【０００３】
ポテンシャル法を用いた移動体の移動制御技術として、例えば、特許文献１の移動体及びその移動制御方法、特許文献２の障害物回避装置および移動体などが開示されている。
特許文献１には、ポテンシャル法を用いてロボットの移動制御を行うにあたって、走行制御コントローラが、検知した障害物に関する仮想斥力を計算する際に、設定される予測スパンＴiに相当する時間が経過した後のロボット及び障害物の予測位置から得られる予測最近傍距離ρTiと、ロボットと障害物との相対速度と、ロボットと障害物との許容最小距離ρ0と、を少なくとも考慮して計算を行う発明が記載されている。
【０００４】
また、特許文献２には、電動車椅子に搭載された回避装置が、操作部から入力されるユーザの指示速度Ｗと対物センサから入力される周囲の障害物の状況とを、学習部の格納部に格納し、これに応じて速度Ｕを修正して駆動装置に出力する。更に、格納部に格納した情報と速度センサにて検出する実際の速度Ｙとを抽出部にて比較して、強化学習を行う。更に、抽出部が適切な反発力の係数を学習し、反発力生成部が反発力Ｆを計算して、ユーザの指示速度Ｗに基づく速度Ｕｍに加算することによって、速度Ｕを修正し、電動車椅子の速度Ｕを駆動装置、動力部によって実現する発明が記載されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００９−２８８９３０号公報
【特許文献２】特開２００５−２９３１５４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記特許文献１及び特許文献２の技術においては、障害物から受ける斥力（反発力）の計算に、障害物との最近接（近傍）距離を用いている。そのため、例えば、図９（ａ）に示すような曲がり角や、図９（ｂ）に示す両側が壁の通路（廊下等）、または、移動経路上に複数の障害物が存在するような場合に、移動体の移動に伴って最近接距離の点の切り替わりが発生する。例えば、図９（ａ）及び（ｂ）に示すように、移動体がＡ点からＢ点に移動して、最近接距離の点の切り替わりが発生すると、図９（ａ）に対応する図１１（ａ）、図９（ｂ）に対応する図１１（ｂ）に示すように、斥力ベクトルの急激な変化が発生する。そのため、この変化点における斥力ベクトルを用いて移動制御を行うと、移動体の急激な速度変化等の不具合が生じる恐れがある。なお、図１１において、横軸は移動体の位置であり、縦軸は、図９中の斜線で示す障害物から移動体が仮想的に受ける斥力である。また、図９中の黒点は、距離の測定点を示す。
【０００７】
そこで、本発明は、このような従来の技術の有する未解決の課題に着目してなされたものであって、最近接距離の点の切り替わりが発生する状況において、移動体の障害物の回避動作を支援するのに好適な障害物回避支援装置、障害物回避支援方法及び障害物回避支援装置を備えた移動体を提供することを目的としている。
【課題を解決するための手段】
【０００８】
〔発明１〕 上記目的を達成するために、発明１の障害物回避支援装置は、移動体の移動経路上に存在する障害物の回避動作を支援する障害物回避支援装置であって、
前記移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報を記憶する距離情報記憶部と、
前記距離情報記憶部に記憶された距離情報に基づき、ポテンシャル関数を用いて前記障害物の前記複数の測定点の各測定点から仮想的に受ける斥力を計算する斥力計算部と、
前記斥力計算部で計算した各測定点に対応する斥力を前記測定点の座標を規定する各軸の軸成分に分離する斥力成分分離部と、
前記各測定点の前記分離した軸成分に基づき、前記障害物の回避動作に係る斥力である回避用斥力を生成する回避用斥力生成部と、を備え、
前記回避用斥力生成部は、前記各測定点に対応する斥力の軸成分における各軸の正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算し、該計算結果に基づき前記回避用斥力を生成する。
【０００９】
このような構成であれば、斥力計算部によって、移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報に基づき、ポテンシャル関数を用いて障害物の複数の測定点の各測定点から仮想的に受ける斥力を計算することができる。更に、斥力成分分離部によって、斥力計算部で計算した各測定点に対応する斥力を軸成分に分離することができる。更に、回避用斥力生成部によって、軸成分分離部で分離した各測定点に対応する斥力の軸成分に基づき、各軸の正方向の最大値と負方向の最大値との和を計算し、該計算結果に基づき障害物の回避用斥力を生成することができる。
例えば、回避用斥力として、計算した各軸の軸成分の最大値の和を各軸の成分とした斥力（ベクトル）を生成することができる。
【００１０】
〔発明２〕 更に、発明２の障害物回避支援装置は、発明１の障害物回避支援装置において、前記測定点の座標が、互いに直交するｘ軸及びｙ軸により規定される２次元座標（ｘ，ｙ）で表され、ｎ番目（ｎは、測定点の総数をＮとした場合に１≦ｎ≦Ｎの自然数）の測定点に対応する斥力におけるｘ軸方向の成分がＦpx（ｎ）、ｙ軸方向の成分がＦpy（ｎ）で表される場合に、前記回避用斥力生成部は、前記回避用斥力におけるｘ軸方向の成分Ｆpx及びｙ軸方向の成分Ｆpyを、下式（１）〜（２）に基づき計算し、
Ｆpx＝Ｆpxp＋Ｆpxn ・・・（１）
Ｆpy＝Ｆpyp＋Ｆpyn ・・・（２）
【００１１】
前記式（１）〜（２）において、Ｆpxpは、Ｎ個の測定点に対応する斥力におけるｘ軸の正方向の成分の最大値であり、Ｆpxnは、Ｎ個の測定点に対応する斥力におけるｘ軸の負方向の成分の最大値であり、Ｆpypは、Ｎ個の測定点に対応する斥力におけるｙ軸の正方向の成分の最大値であり、Ｆpynは、Ｎ個の測定点に対応する斥力におけるｙ軸の負方向の成分の最大値であり、前記式（１）〜（２）において、前記Ｆpxpが負の値の場合は当該Ｆpxpの値をゼロとし、前記Ｆpypが負の値の場合は当該Ｆpypの値をゼロとし、前記Ｆpxnが正の値の場合は当該Ｆpxnの値をゼロとし、前記Ｆpynが正の値の場合は当該Ｆpynの値をゼロとする。
このような構成であれば、回避用斥力生成部は、斥力成分分離部で分離したｘ軸方向の成分及びｙ軸方向の成分を用いて、上式（１）〜（２）に基づき回避用斥力Ｆpx及びＦpyを生成することができる。
【００１２】
〔発明３〕 更に、発明３の障害物回避支援装置は、発明２の障害物回避支援装置において、前記測定点の座標が、前記ｘ軸及び前記ｙ軸の双方に直交するｚ軸を更に含む３次元座標（ｘ，ｙ，ｚ）で表され、ｎ番目の測定点に対応する斥力におけるｚ軸方向の成分がＦpz（ｎ）で表される場合に、前記回避用斥力生成部は、前記回避用斥力におけるｚ軸方向の成分Ｆpzを、下式（３）に基づき計算し、
Ｆpz＝Ｆpzp＋Ｆpzn ・・・（３）
【００１３】
前記式（３）において、Ｆpzpは、Ｎ個の測定点に対応する斥力におけるｚ軸の正方向の成分の最大値であり、Ｆpznは、Ｎ個の測定点に対応する斥力におけるｚ軸の負方向の成分の最大値であり、前記Ｆpzpが負の値の場合は当該Ｆpzpの値をゼロとし、前記Ｆpznが正の値の場合は当該Ｆpznの値をゼロとする。
このような構成であれば、回避用斥力生成部によって、回避用斥力Ｆpx及びＦpyに加えて、斥力成分分離部で分離したｚ軸方向の成分を用いて、上式（３）に基づき回避用斥力Ｆpzを生成することができる。
【００１４】
〔発明４〕 更に、発明４の障害物回避支援装置は、発明１又は２の障害物回避支援装置において、前記測定点の座標が、互いに直交するｘ軸及びｙ軸により規定される２次元座標（ｘ，ｙ）で表される場合に、前記斥力計算部は、ｎ番目（ｎは、測定点の総数をＮとした場合に１〜Ｎの自然数）の測定点に対応する斥力Ｕ（ｎ）を下式（４）に示すポテンシャル関数に基づき計算し、前記斥力成分分離部は、前記斥力Ｕ（ｎ）を、下式（５）〜（６）に基づき、ｘ軸方向の成分Ｆpx（ｎ）及びｙ軸方向の成分Ｆpy（ｎ）に分離する。
Ｕ（ｎ）＝η／２・（１／ρn−１／ρ0）2 ・・・（４）
Ｆpx（ｎ）＝∂Ｕ（ｎ）／∂ｘ ・・・（５）
Ｆpy（ｎ）＝∂Ｕ（ｎ）／∂ｙ ・・・（６）
【００１５】
但し、前記式（４）において、ηは、正の重み定数であり、ρ0は、正の定数であって前記ポテンシャル関数が影響を与える範囲であり、ρnは、前記移動体の現在位置からｎ番目の測定点までの距離である。
このような構成であれば、斥力計算部によって、ｎ番目の測定点に対応する斥力Ｕ（ｎ）を上式（４）に示すポテンシャル関数に基づき計算することができる。更に、斥力成分分離部によって、上式（５）〜（６）に基づき、斥力Ｕ（ｎ）を、ｘ軸方向の成分Ｆpx（ｎ）及びｙ軸方向の成分Ｆpy（ｎ）に分離することができる。
【００１６】
〔発明５〕 更に、発明５の障害物回避支援装置は、発明１又は３の障害物回避支援装置において、前記測定点の座標が、各々が互いに直交するｘ軸、ｙ軸及びｚ軸により規定される３次元座標（ｘ，ｙ，ｚ）で表される場合に、前記斥力計算部は、ｎ番目（ｎは、測定点の総数をＮとした場合に１〜Ｎの自然数）の測定点に対応する斥力Ｕ（ｎ）を下式（７）に示すポテンシャル関数に基づき計算し、前記斥力成分分離部は、前記斥力Ｕ（ｎ）を、下式（８）〜（１０）に基づき、ｘ軸方向の成分Ｆpx（ｎ）、ｙ軸方向の成分Ｆpy（ｎ）及びｚ軸方向の成分Ｆpz（ｎ）に分離する。
Ｕ（ｎ）＝η／２・（１／ρn−１／ρ0）2 ・・・（７）
Ｆpx（ｎ）＝∂Ｕ（ｎ）／∂ｘ ・・・（８）
Ｆpy（ｎ）＝∂Ｕ（ｎ）／∂ｙ ・・・（９）
Ｆpz（ｎ）＝∂Ｕ（ｎ）／∂ｚ ・・・（１０）
【００１７】
但し、上式（７）において、ηは、正の重み定数であり、ρ0は、正の定数であって前記ポテンシャル関数が影響を与える範囲であり、ρnは、前記移動体の現在位置からｎ番目の測定点までの距離である。
このような構成であれば、斥力計算部によって、ｎ番目の測定点に対応する斥力Ｕ（ｎ）を上式（７）に示すポテンシャル関数に基づき計算することができる。更に、斥力成分分離部によって、上式（８）〜（１０）に基づき、斥力Ｕ（ｎ）を、ｘ軸方向の成分Ｆpx（ｎ）、ｙ軸方向の成分Ｆpy（ｎ）及びｚ軸方向の成分Ｆpz（ｎ）に分離することができる。
【００１８】
〔発明６〕 一方、上記目的を達成するために、発明６の障害物回避支援方法は、移動体の移動経路上に存在する障害物の回避動作を支援する障害物回避支援方法であって、
前記移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報の記憶された距離情報記憶手段の前記距離情報に基づき、ポテンシャル関数を用いて前記障害物の前記複数の測定点の各測定点から仮想的に受ける斥力を計算する斥力計算ステップと、
前記斥力計算ステップで計算した各測定点に対応する斥力を前記測定点の座標を規定する各軸の軸成分に分離する斥力成分分離ステップと、
前記各測定点の前記分離した軸成分に基づき、前記障害物の回避動作に係る斥力である回避用斥力を生成する回避用斥力生成ステップと、を含み、
前記回避用斥力生成ステップにおいては、前記各測定点に対応する斥力の軸成分における各軸の正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算し、該計算結果に基づき前記回避用斥力を生成する。
これにより、発明１の障害物回避支援装置と同等の作用を得ることができる。
【００１９】
〔発明７〕 また、上記目的を達成するために、発明７の移動体は、基体と、
前記基体を移動させるアクチュエータを有する移動機構と、
移動経路上に存在する障害物の形状に沿った複数の測定点に対する距離情報を測定する距離情報測定部と、
操作部を介した利用者からの当該移動体の移動方向の情報を含む移動に係る指示入力を受け付ける指示入力受付部と、
請求項１乃至請求項５のいずれか１項に記載の障害物回避支援装置と、
前記障害物回避支援装置において前記距離情報測定部で測定した距離情報に基づき計算した回避用斥力と前記指示入力受付部で受け付けた指示入力とに基づき、前記アクチュエータを制御するアクチュエータ制御部と、を備える。
このような構成であれば、発明１乃至５のいずれか１の障害物回避支援装置と同等の作用を得ることができる。
【発明の効果】
【００２０】
以上説明したように、発明１〜５の障害物回避支援装置、発明６の障害物回避支援方法及び発明７の移動体によれば、障害物の回避（経路計画）に係る斥力ベクトルを生成する際に、障害物の各測定点から受ける斥力を軸成分に分離し、分離した軸成分のうち正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算する。そして、計算した各軸の最大値の和を各軸の成分とした回避用斥力を生成することができる。
【００２１】
これにより、障害物の回避（経路計画）において、最大斥力を用いることから、従来の最近接距離を用いて生成した斥力ベクトルと比較して、最近接距離が切り替わりによる影響を受けなくなる。具体的に、連続的に変化する斥力ベクトルを生成することができる。従って、移動体の障害物回避時の移動制御において、最近接距離の切り替わりによって急激に変化する斥力ベクトルが用いられるのを防ぐことができ、安全且つ安定した移動体の障害物の回避制御を行うことができるという効果が得られる。
【００２２】
また、レーザーレンジファインダや距離画像センサ等の測域センサによって測定した測定点群の距離情報から求めた斥力の総和等を用いて回避用斥力を計算することが考えられる。この場合に、測域センサを用いた距離測定において、障害物上の測定点群は、測域センサから障害物までの距離に応じてその密度（粒状性）が変化する。そのため、同じ大きさの物体でも、その距離によって測定点群の密度が変化し、障害物上の測定点の数が変化してしまう。また、比較的小さな障害物は、測定点の数が少なくなるためその斥力の影響が小さくなってしまい、接触、衝突の可能性が上昇する。
上記発明１〜７によれば、斥力の軸成分の正方向と負方向の最大値の和を用いるため、障害物の大小や測定点群の密度などに影響を受けない回避用斥力を計算することができるという効果が得られる。
【図面の簡単な説明】
【００２３】
【図１】（ａ）は、脚車輪型ロボット１の正面図であり、（ｂ）は、脚車輪型ロボット１の側面図である。
【図２】レーザーレンジセンサ２００の距離測定の原理を説明する図である。
【図３】（ａ）は、測距センサを、垂直方向の軸（ｚ軸）まわりに回転駆動させたときの測定距離と回転角度θとの関係を示す図であり、（ｂ）は、測距センサを、水平方向の軸（ｙ軸）まわりに回転駆動したときの測定平面と回転角度φとの関係を示す図である。
【図４】脚車輪型ロボット１の移動制御システムを示すブロック図である。
【図５】移動制御システムの備える、障害物回避時の移動制御に係る機能構成を示すブロック図である。
【図６】障害物回避支援部１０１の詳細な機能構成を示すブロック図である。
【図７】回避用斥力Ｆpの生成処理を示すフローチャートである。
【図８】障害物回避時のモータ制御処理を示すフローチャートである。
【図９】（ａ）及び（ｂ）は、障害物と脚車輪型ロボット１との位置関係の例を示す図である。
【図１０】（ａ）及び（ｂ）は、脚車輪型ロボット１が、図９（ａ）及び（ｂ）に示すＡ点からＢ点へと移動したときの位置の変化に対する本発明の回避用斥力の変化を示す図である。
【図１１】（ａ）及び（ｂ）は、脚車輪型ロボット１が、図９（ａ）及び（ｂ）に示すＡ点からＢ点へと移動したときの位置の変化に対する従来の回避用斥力の変化を示す図である。
【図１２】実施の形態の案内用ロボットの側面図であり、案内用ロボット８１の構成を示す図である。
【発明を実施するための形態】
【００２４】
〔第１の実施の形態〕
以下、本発明の第１の実施の形態を図面に基づき説明する。図１〜図１０は、本発明に係る障害物回避支援装置、障害物回避支援方法及び移動体の第１の実施の形態を示す図である。
本実施の形態は、本発明に係る障害物回避支援装置を、移動体である、脚部による歩行移動及び車輪による走行が可能な脚車輪型ロボットに適用したものである。
【００２５】
まず、図１に基づき、本実施の形態に係る脚車輪型ロボットの外観構成を説明する。
図１（ａ）は、脚車輪型ロボット１の正面図であり、（ｂ）は、脚車輪型ロボット１の側面図である。
脚車輪型ロボット１は、図１（ａ）及び（ｂ）に示すように、基体１０と、基体１０に連結された４つの脚部１２とを有して構成されている。
基体１０の前方には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。また、基体１０の後方には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。
【００２６】
回転関節１４は、脚車輪型ロボット１の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。
各脚部１２には、２つの回転関節１６、１８が設けられている。回転関節１６、１８は、回転関節１４が図１４のような状態であるときは、脚車輪型ロボット１の側面と直交する方向を軸方向として回転する。したがって、脚部１２は、それぞれ３自由度を有する。
【００２７】
各脚部１２の先端には、回転関節１６、１８と軸方向を同一にして駆動輪２０が回転可能に設けられている。駆動輪２０は、回転関節１４の回転によりヨー軸周りに回動する。つまり、回転関節１４の回転を制御することで、走行移動時の操舵制御が行われる。
各脚部１２の先端には、脚車輪型ロボット１の移動経路上に存在する物体までの脚先からの距離を測定する脚先センサ３８が設けられている。
【００２８】
一方、基体１０の正面の下部中央には、水平面レーザー光を照射する水平レーザ２６が設けられている。また、基体１０の正面の中央左右には、垂直面レーザー光を照射する垂直レーザー２８、３０がそれぞれ設けられている。
基体１０の正面の上部中央には、水平面レーザー光および垂直面レーザー光の反射光を含む画像を撮影するカメラ３２が設けられている。
【００２９】
また、カメラ３２の下側には、レーザーレンジセンサ２００が配設されている。
水平レーザー２６は、カメラ３２で水平面レーザー光の反射光を含む画像が撮影できるように下方に所定角度傾けて設けられている。同様に、垂直レーザー２８は、カメラ３２で垂直面レーザー光の反射光を含む画像が撮影できるように右方に所定角度傾けて設けられ、垂直レーザー３０は、左方に所定角度傾けて設けられている。
【００３０】
なお、各脚部１２の回転関節１４〜１８には、回転関節１４〜１８を回転駆動する関節モータ４０がそれぞれ設けられている。また、各脚部１２の駆動輪２０には、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。
レーザーレンジセンサ２００は、図示しないが、センシングプロセッサと、測定範囲内に存在する物体上の測定点までの距離を測定する測距センサと、測距センサを垂直方向の軸まわり及び水平方向の軸まわりに回転駆動する回転機構とを含んで構成されている。
【００３１】
更に、レーザーレンジセンサ２００は、図示しないが、回転機構を垂直方向の軸まわりに回転駆動する第１モータと、水平方向の軸まわりに回転駆動する第２モータと、これらモータの回転角度位置を検出するエンコーダと、センシングプロセッサからの指令信号及び各エンコーダからの信号に基づき各モータを制御するドライバとを含んで構成されている。
ドライバは、センシングプロセッサからの指令信号において指定された走査角度範囲及び走査角度単位（例えば、０．３６°などの所定の回転角度）に基づき、第１及び第２モータの回転軸を、指定された走査角度単位ずつ回転させる制御を行う。
【００３２】
ここで、測距センサの原点位置（走査角度が０°の位置）における、レーザーの出力方向の軸をｘ軸、ｘ軸に対して直交する２つの軸のうち一方の軸をｙ軸（本実施の形態では、センサの横方向と水平方向の軸となる）、他方の軸をｚ軸（本実施の形態では、垂直方向の軸となる）とする。
【００３３】
第１モータは、測距センサのレーザー出力部（不図示）及び受光部（不図示）を、垂直方向の軸まわり（ｚ軸）に回転駆動するように設けられ、第２モータは、出力部及び受光部を水平方向の軸（ｙ軸）まわりに回転駆動するように設けられている。そして、ドライバからの制御信号に応じて、第１モータは、自己の回転軸を指定された走査角度単位（回転角度θ）ずつ回転駆動し、第２モータは、自己の回転軸を指定された走査角度単位（回転角度φ）ずつ回転駆動する。
【００３４】
ここで、図２は、レーザーレンジセンサ２００の距離測定の原理を説明する図である。
レーザーレンジセンサ２００は、測距センサが、指定された走査角度単位ずつ回動し、且つ回動する毎に、図２に示すように、レーザー出力部からレーザー光を出力すると共に、出力光に対する物体（図２中の障害物）からの反射光を受光部で受光する。そして、各回転角度（走査角度）に応じた距離（図２中の測定距離Ｌ（物体と受光部との間の距離））を測定する。
【００３５】
また、図３（ａ）は、測距センサを、垂直方向の軸（ｚ軸）まわりに回転駆動させたときの測定距離と回転角度θとの関係を示す図であり、（ｂ）は、測距センサを、水平方向の軸（ｙ軸）まわりに回転駆動したときの測定平面と回転角度φとの関係を示す図である。
例えば、図３（ａ）に示すように、測距センサを、ｚ軸まわりに指定された走査角度単位ずつ回動させながら、原点位置に対する各回転角度（図３（ａ）中のθ1、θ2、θ3）に応じた距離情報（図３（ａ）中のＬ（θ1）、Ｌ（θ2）、Ｌ（θ3））を測定する。
【００３６】
また、第１モータの回転軸の回転中心と、レーザーの走査軌道線の両端とを結んで形成される平面が、ｘｙ測定平面（物体が存在しない場合は扇形の平面）となる。
ｘｙ測定平面の走査処理（第１走査処理）が完了する毎に、図３（ｂ）に示すように、測距センサを、ｙ軸まわりに指定された走査角度単位（φ）ずつ回動させる。この走査処理を第２走査処理という。
【００３７】
そして、第１走査処理と第２走査処理とを交互に連続して行うことにより、第１走査処理によって形成される測定平面を、ｙ軸まわりに連続して形成する。これにより、測定範囲内（走査角度範囲内）に存在する物体の３次元の距離情報を測定することが可能である。
また、図３（ｂ）に示すように、第２走査処理後の各測定点の距離情報を、Ｌ（θi，φj）と表記する。ここで、ｉは、ｚ軸まわりの走査角度に応じて各測定点に付与される通し番号であり、ｊは、ｙ軸まわりの走査角度に応じて各測定点に付与される通し番号である。
【００３８】
本実施の形態において、脚車輪型ロボット１は、ＣＰＵ６０において、レーザーレンジセンサ２００で測定した自己の移動経路上に存在する障害物の距離情報及び速度情報に基づき、ポテンシャル法を用いた障害物の回避動作を含む移動制御を行うようになっている。
また、レーザーレンジセンサ２００のセンシングプロセッサにおいて、ノイズ除去のために、測距センサで測定した回転座標系の距離情報に対してメディアンフィルタを用いたフィルタリング処理を実行する。また、センシングプロセッサにおいて、測定対象の物体の移動速度を計算する処理を実行する。
【００３９】
次に、図４に基づき、脚車輪型ロボット１の移動制御システムについて説明する。
ここで、図４は、脚車輪型ロボット１の移動制御システムを示すブロック図である。
脚車輪型ロボット１の４本の各脚部１２の回転関節１４，１６，１８には、図４に示すように、各回転関節１４，１６，１８を回転駆動する関節モータ４０がそれぞれ設けられている。各関節モータ４０には、関節モータ４０の回転角度位置を検出するエンコーダ４２と、モータ指令信号およびエンコーダ４２の出力信号に基づいて関節モータ４０の駆動を制御するドライバ４４が設けられている。
【００４０】
各脚部１２の駆動輪２０には、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。各車輪モータ５０には、車輪モータ５０の回転角度位置を検出するエンコーダ５２と、モータ指令信号およびエンコーダ５２の出力信号に基づいて車輪モータ５０の駆動を制御するドライバ５４が設けられている。
脚車輪型ロボット１は、さらに、ＣＰＵ６０と、脚車輪型ロボット１の姿勢を検出する３軸姿勢センサ７０と、カメラ３２の画像信号を処理するビジョンプロセッサ７２と、レーザーレンジセンサ２００と、を有して構成される。
【００４１】
３軸姿勢センサ７０は、ジャイロ若しくは加速度センサ、またはその両方を有し、地軸に対して脚車輪型ロボット１の姿勢の傾きを検出する。
脚車輪型ロボット１は、さらに、脚車輪型ロボット１を無線通信を用いて遠隔操作可能な操作部３００等と無線通信を行う無線通信部７４と、ビジョンプロセッサ７２、無線通信部７４およびレーザーレンジセンサ２００とＣＰＵ６０の入出力を中継するハブ７６と、警告音等を出力するスピーカ７８とを有して構成される。
【００４２】
ＣＰＵ６０は、モータ指令出力Ｉ／Ｆ６１を介してドライバ４４、５４にモータ指令信号を出力し、角度取込Ｉ／Ｆ６２を介してエンコーダ４２、５２の出力信号を入力する。
ＣＰＵ６０は、レーザーレンジセンサ２００に対して、通信Ｉ／Ｆ６４及びハブ７６を介して、第１及び第２モータの回転角度位置を検出するエンコーダの出力信号を入力し、第１及び第２モータのドライバにモータ指令信号を出力する。
【００４３】
ＣＰＵ６０は、センサ入力Ｉ／Ｆ６３を介して、平面内走査脚先センサ３８、レーザーレンジセンサ２００および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。また、通信Ｉ／Ｆ６４を介してハブ７６と信号の入出力を行い、サウンド出力Ｉ／Ｆ６５を介してスピーカ７８に音声信号を出力する。
ＣＰＵ６０は、レーザーレンジセンサ２００から距離情報が入力されると、そのセンサ座標系の座標をグローバル座標系に変換する座標変換処理を行う。座標変換処理では、脚車輪型ロボット１中心のグローバル座標をＸc、Ｙc、Ｚc、脚車輪型ロボット１の姿勢（ロール角、ピッチ角、ヨー角）をφ、θ、ψとして、センサ座標系における各測定点を、グローバル座標系の測定点（Ｘs、Ｙs、Ｚs）に変換する。
【００４４】
また、歩行動作時において、３軸姿勢センサ７０のセンサ信号、並びにグローバル座標系に変換された各測定点に基づいて逆運動学計算および重心計算を行い、この計算結果に基づいて脚先（駆動輪）の着地位置を決定し、物体平面までの距離を算出し、脚先と物体平面の位置関係を算出し、決定した着地位置および算出した両距離に基づいてドライバ４４、５４へのモータ指令信号を生成し、生成したモータ指令信号をドライバ４４、５４に出力する。
【００４５】
また、車輪走行動作時において、グローバル座標系に変換された障害物の各測定点に基づいてポテンシャル法を用いた斥力の計算を行い、この計算結果に基づいてドライバ４４、５４へのモータ指令信号を生成し、生成したモータ指令信号をドライバ４４、５４に出力する。
なお、図示していないが、脚車輪型ロボット１は、専用のプログラムやロボットの制御に必要なパラメータ値等を記憶するＲＯＭや、プログラムの実行に必要なデータを一時記憶するＲＡＭ等のメモリを備えている。
【００４６】
次に、図５及び図６に基づき、移動制御システムにおいて実行される、脚車輪型ロボット１の車輪走行時における障害物回避時の移動制御処理に係る機能構成について説明する。
ここで、図５は、移動制御システムの備える、障害物回避時の移動制御に係る機能構成を示すブロック図である。また、図６は、障害物回避支援部１０１の詳細な機能構成を示すブロック図である。
【００４７】
図５に示すように、移動制御システムは、車輪走行時における障害物回避時の移動制御に係る機能構成部である、障害物回避支援部１０１と、モータ制御部１０２とを含む移動制御部１００を備えている。
障害物回避支援部１０１は、レーザーレンジセンサ２００で測定した障害物の各測定点の距離情報に基づき、ポテンシャル法を用いて、脚車輪型ロボット１が障害物の各測定点から受ける斥力を計算する機能を有している。更に、各測定点の斥力の計算結果に基づき、障害物回避用の斥力である回避用斥力を計算する機能を有している。
【００４８】
具体的に、障害物回避支援部１０１は、図６に示すように、距離情報記憶部１１０と、斥力計算部１１１と、斥力成分分離部１１２と、回避用斥力生成部１１３とを含んで構成される。
距離情報記憶部１１０は、レーザーレンジセンサ２００から通信Ｉ／Ｆ６４を介して入力される距離情報を、ＲＡＭ等のメモリに記憶する機能を有している。
斥力計算部１１１は、上式（４）又は（７）のポテンシャル関数に基づき、メモリに記憶されたＮ個の測定点１〜Ｎ（Ｎは、２以上の自然数）の距離情報を用いて、Ｎ個の各測定点に対応する斥力Ｕ（ｎ）（ｎは、１〜Ｎの自然数）を計算する機能を有している。
【００４９】
ここで、上式（４）及び（７）は、障害物から受ける斥力を計算するためにKhatibにより設計された公知のポテンシャル関数であり、ηは、正の重み定数であり、ρ0は、正の定数であってポテンシャル関数が影響を与える範囲であり、ρnは、脚車輪型ロボット１と障害物上のｎ番目の測定点との距離である。このとき、斥力はポテンシャル関数の傾く方向への力を受ける。
【００５０】
また、本実施の形態においては、障害物の回避支援モードとして、２次元モードと３次元モードとが設定可能となっている。２次元モードは、上述した、第１走査処理及び第２走査処理を実行して測定された障害物の距離情報のうち、所定の１つのｚ座標のｘｙ平面の測定点の距離情報を用いて回避用斥力を計算するモードであり、３次元モードは、全ての測定点の距離情報を用いて回避用斥力を計算するモードである。従って、モードに応じてＮの値が変わる。
【００５１】
具体的に、斥力計算部１１１は、２次元モードが設定されている場合に、所定の１つのｚ座標（例えば、原点の座標）のｘｙ平面の測定情報を用いて斥力の計算を行い、３次元モードが設定されている場合に、全て又は移動に係る範囲に対応する一部の３次元の測定情報を用いて斥力の計算を行う。
斥力成分分離部１１２は、斥力計算部１１１で計算された障害物上のＮ個の測定点に対応する斥力Ｕ（１）〜Ｕ（Ｎ）を、上式（５）〜（６）又は上式（８）〜（１０）に基づき、軸毎の成分に分離する機能を有している。
【００５２】
具体的に、斥力成分分離部１１２は、２次元モードが設定されているときは、上式（５）〜（６）に基づき、斥力Ｕ（ｎ）をｘ軸の成分Ｆpx（ｎ）と、ｙ軸の成分Ｆpy（ｎ）とに分離する。また、３次元モードが設定されているときは、上式（８）〜（１０）に基づき、斥力Ｕ（ｎ）をｘ軸の成分Ｆpx（ｎ）、ｙ軸の成分Ｆpy（ｎ）及びｚ軸の成分Ｆpz（ｎ）に分離する。ここで、各式における（ｘ，ｙ，ｚ）は、上記ＣＰＵ６０で座標変換されたグローバル座標系の（Ｘs、Ｙs、Ｚs）に対応している。
回避用斥力生成部１１３は、下式（１１）〜（１６）に基づき、斥力成分分離部１１２で分離された、斥力Ｕ（１）〜Ｕ（Ｎ）の各軸成分のうち、正方向の最大値と負方向の最大値とを軸毎に抽出する機能を有している。
【００５３】
【数１】
【００５４】
なお、上式（１１）は、ｘ軸成分Ｆpx（１）〜（Ｎ）から最大値Ｆpxpを抽出する関数であり、上式（１３）は、ｙ軸成分Ｆpy（１）〜（Ｎ）から最大値Ｆpypを抽出する関数である。また、上式（１５）は、ｚ軸成分Ｆpz（１）〜（Ｎ）から最大値Ｆpzpを抽出する関数である。基本的に、上式（１１）及び（１３）は、各軸成分の正方向の最大値を抽出することを目的とした関数である。以降は、抽出される値の正負に係わらずＦpxp、Ｆpyp及びＦpzpを、各軸成分の正方向の最大値として扱うこととする。
【００５５】
また、上式（１２）は、ｘ軸成分Ｆpx（１）〜（Ｎ）から最小値Ｆpxnを抽出する関数であり、上式（１４）は、ｙ軸成分Ｆpy（１）〜（Ｎ）から最小値Ｆpynを抽出する関数である。また、上式（１６）は、ｚ軸成分Ｆpz（１）〜（Ｎ）から最小値Ｆpznを抽出する関数である。基本的に、上式（１２）及び（１４）は、各軸成分の負方向の最大値を抽出することを目的とした関数である。以降は、抽出される値の正負に係わらずＦpxn、Ｆpyn及びＦpznを、各軸成分の負方向の最大値として扱うこととする。
【００５６】
更に、回避用斥力生成部１１３は、上式（１）〜（３）に基づき、抽出した軸毎の正方向の最大値と負方向の最大値との和を計算し、この計算結果から回避用斥力Ｆpを生成する機能を有している。この回避用斥力Ｆpは、障害物を回避するために移動制御で用いる斥力である。
回避用斥力生成部１１３は、具体的に、２次元モードが設定されている場合に、まず、上式（１１）〜（１４）に基づき、ｘ軸の斥力成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnを計算し、ｙ軸の斥力成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynを計算する。
【００５７】
次に、回避用斥力生成部１１３は、上式（１）〜（２）に基づき、ｘ軸成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnの和であるＦpxと、ｙ軸成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynの和であるＦpyとを計算する。このとき、Ｆpxpが負の値の場合はＦpxpの値をゼロ（０）にし、Ｆpypが負の値の場合はＦpypの値をゼロ（０）にする。このような状況は、例えば、障害物から負方向の斥力しか受けないときに生じる。更に、Ｆpxnが正の値の場合はＦpxnの値をゼロ（０）にし、Ｆpynが正の値の場合はＦpynの値をゼロ（０）にする。このような状況は、例えば、障害物から正方向の斥力しか受けないときに生じる。
更に、回避用斥力生成部１１３は、上記ｘ軸及びｙ軸についての正方向の最大値と負方向の最大値との和の計算結果Ｆpx及びＦpyに基づき、下式（１７）に示すように、Ｆpxをｘ軸成分とし、Ｆpyをｙ軸成分としたベクトルである回避用斥力Ｆpを生成する。
【００５８】
【数２】
【００５９】
また、回避用斥力生成部１１３は、３次元モードが設定されている場合に、上式（１１）〜（１６）に基づき、ｘ軸の斥力成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnを計算し、ｙ軸の斥力成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynを計算し、ｚ軸の斥力成分の正方向の最大値Ｆpzp及び負方向の最大値Ｆpznを計算する。更に、上式（１）〜（３）に基づき、ｘ軸成分の正方向の最大値Ｆpxpと負方向の最大値Ｆpxnとの和であるＦpxと、ｙ軸成分の正方向の最大値Ｆpypと負方向の最大値Ｆpynとの和であるＦpyと、ｚ軸成分の正方向の最大値Ｆpzpと負方向の最大値Ｆpznとの和であるＦpzとを計算する。そして、下式（１８）に示すように、Ｆpxをｘ軸成分とし、Ｆpyをｙ軸成分とし、Ｆpzをｚ軸成分としたベクトルである回避用斥力Ｆpを生成する。
なお、ｚ軸成分についても、Ｆpzpが正の値の場合はＦpzpの値をゼロ（０）にし、Ｆpznが正の値の場合はＦpznの値をゼロ（０）にする。}
【００６０】
【数３】
【００６１】
回避用斥力生成部１１３は、回避用斥力Ｆpを生成すると、生成したＦpをモータ制御部１０２に出力する。
図５に戻って、モータ制御部１０２は、操作部３００からの指示入力と、レーザーレンジセンサ２００からの障害物の位置情報と、障害物回避支援部１０１からの回避用斥力Ｆpと、各エンコーダからの回転角度情報とに基づき、障害物の回避動作を含む走行制御のためのモータ指令信号を生成する機能を有している。
【００６２】
例えば、利用者の操作入力が障害物方向への移動を示していた場合に、モータを停止する指令信号を生成したり、自動で障害物を回避する軌道を移動させる指令信号を生成したりする。
なお、本実施の形態において、上記障害物回避支援部１０１の各構成部及びモータ制御部１０２の各機能は、ＣＰＵ６０において専用のプログラムを実行することで実現される。
【００６３】
次に、図７に基づき、障害物回避支援部１０１の回避用斥力Ｆpの生成処理の流れを説明する。ここで、図７は、回避用斥力Ｆpの生成処理を示すフローチャートである。
回避用斥力Ｆpの生成処理は、ＣＰＵ６０が、ＲＯＭに記憶された専用のプログラムを読み出し、当該読み出したプログラムを実行することで実現される処理であり、処理が開始されると、図７に示すように、まず、ステップＳ１００に移行する。
【００６４】
ステップＳ１００では、斥力計算部１１１において、利用者の操作に応じた操作部３００からの通信Ｉ／Ｆ６４を介した操作入力があったか否かを判定し、操作入力があったと判定した場合(Yes)は、ステップＳ１０２に移行する。一方、操作入力がなかったと判定した場合(No)は、操作入力があるまで判定処理を繰り返す。
ステップＳ１０２に移行した場合は、斥力計算部１１１において、メモリから距離情報を取得して、ステップＳ１０４に移行する。
【００６５】
ステップＳ１０４では、斥力計算部１１１において、取得した距離情報に基づき、障害物があるか否かを判定し、障害物があると判定した場合(Yes)は、ステップＳ１０６に移行し、そうでない場合(No)は、一連の処理を終了する。
ステップＳ１０６に移行した場合は、斥力計算部１１１において、上式（４）又は（７）のポテンシャル関数に基づき、Ｎ個の距離情報の各距離情報を用いて各測定点に対応する斥力Ｕ（ｎ）を計算して、ステップＳ１０８に移行する。
【００６６】
ステップＳ１０８では、斥力成分分離部１１２において、ステップＳ１０６で計算した斥力Ｕ（ｎ）を、上式（５）〜（６）又は（８）〜（１０）に基づき、各軸成分に分離して、ステップＳ１１０に移行する。
ステップＳ１１０では、斥力成分分離部１１２において、Ｎ個の測定点について、斥力Ｕ（１）〜Ｕ（Ｎ）の計算及びこれらの各軸成分への分離処理が終了したか否かを判定し、終了したと判定した場合(Yes)は、ステップＳ１１２に移行する。また、終了していないと判定した場合(No)は、ステップＳ１０６に移行する。
【００６７】
ステップＳ１１２に移行した場合は、回避用斥力生成部１１３において、設定されたモードに応じて、上式（１１）及び（１３）又は上式（１１）、（１３）及び（１５）に基づき、Ｎ個の測定点に対応する軸成分の中から軸毎に最大値を抽出して、ステップＳ１１４に移行する。
ステップＳ１１４では、回避用斥力生成部１１３において、設定されたモードに応じて、上式（１２）及び（１４）又は上式（１２）、（１４）及び（１６）に基づき、Ｎ個の測定点に対応する軸成分の中から軸毎に負方向の最大値を抽出して、ステップＳ１１６に移行する。
【００６８】
ステップＳ１１６では、回避用斥力生成部１１３において、設定されたモードに応じた全ての軸の正方向の最大値の中に負の値のものがあるか否かを判定し、負の値のものがあると判定した場合(Yes)は、ステップＳ１１８に移行し、そうでない場合(No)は、ステップＳ１２０に移行する。
ステップＳ１１８に移行した場合は、回避用斥力生成部１１３において、負の値となっている正方向の最大値Ｆp-pをゼロ（０）に変更して、ステップＳ１２０に移行する。なお、Ｆp-pの「−」部分は、ｘ，ｙ，ｚのうち負の値となる軸が全て該当する。
【００６９】
ステップＳ１２０では、回避用斥力生成部１１３において、設定されたモードに応じた全ての軸の負方向の最大値の中に正の値のものがあるか否かを判定する。そして、正の値のものがあると判定した場合(Yes)は、ステップＳ１２２に移行し、そうでない場合(No)は、ステップＳ１２４に移行する。
ステップＳ１２２に移行した場合は、回避用斥力生成部１１３において、正の値となっている負方向の最大値Ｆp-nをゼロ（０）に変更して、ステップＳ１２４に移行する。なお、Ｆp-nの「−」部分は、ｘ，ｙ，ｚのうち正の値となる軸が全て該当する。
【００７０】
一方、ステップＳ１２４に移行した場合は、回避用斥力生成部１１３において、上式（１）〜（２）又は（１）〜（３）に基づき、軸毎に正方向と負方向の最大値の和を計算して、ステップＳ１２６に移行する。
ステップＳ１２６では、回避用斥力生成部１１３において、ステップＳ１２４の計算結果に基づき、回避用斥力Ｆpを生成して、ステップＳ１２８に移行する。
ステップＳ１２８では、回避用斥力生成部１１３において、ステップＳ１２６で生成した回避用斥力Ｆpを、モータ制御部１０２に出力して、一連の処理を終了する。
なお、上記一連の処理は、脚車輪型ロボット１の移動に伴って、新たに測定されるＮ個の測定情報が記憶される毎に繰り返し行われる。
【００７１】
次に、図８に基づき、モータ制御部１０２における、障害物回避時のモータ制御処理の流れを説明する。
ここで、図８は、障害物回避時のモータ制御処理を示すフローチャートである。
障害物回避時のモータ制御処理は、ＣＰＵ６０が、ＲＯＭに記憶された専用のプログラムを読み出し、当該読み出したプログラムを実行することで実現される処理であり、処理が開始されると、図８に示すように、まず、ステップＳ２００に移行する。
ステップＳ２００では、モータ制御部１０２において、利用者の操作に応じた操作部３００からの通信Ｉ／Ｆ６４を介した操作入力があったか否かを判定し、操作入力があったと判定した場合(Yes)は、ステップＳ２０２に移行する。一方、操作入力がなかったと判定した場合(No)は、操作入力があるまで判定処理を繰り返す。
【００７２】
ステップＳ２０２に移行した場合は、モータ制御部１０２において、障害物回避支援部１０１から回避用斥力Ｆpを取得したか否かを判定し、取得したと判定した場合(Yes)は、ステップＳ２０４に移行し、そうでない場合(No)は、取得するまで判定処理を繰り返す。
ステップＳ２０４に移行した場合は、モータ制御部１０２において、通信Ｉ／Ｆ６４を介して、障害物の位置情報を取得して、ステップＳ２０６に移行する。
【００７３】
ステップＳ２０６では、モータ制御部１０２において、角度取込Ｉ／Ｆ６２を介して、各モータの回転角度情報を取得して、ステップＳ２０８に移行する。
ステップＳ２０８では、モータ制御部１０２において、操作入力、回避用斥力、位置情報及び回転角度情報に基づき、障害物を回避するためのモータ指令を生成して、ステップＳ２１０に移行する。
ステップＳ２１０では、モータ制御部１０２において、モータ指令出力Ｉ／Ｆ６１を介して、モータ指令信号を各モータのドライバに出力して、ステップＳ２００に移行する。
【００７４】
次に、図９〜図１１に基づき、本実施の形態の動作を説明する。
ここで、図９（ａ）は、曲がり角を形成する壁（障害物）と脚車輪型ロボット１との位置関係の一例を示す図であり、（ｂ）は、直線路を形成する壁と脚車輪型ロボット１との位置関係の一例を示す図である。また、図１０（ａ）は、図９（ａ）の曲がり角を脚車輪型ロボット１がＡ点からＢ点へと移動したときの位置の変化に対する本実施の形態の回避用斥力の変化を示す図である。また、図１０（ｂ）は、図９（ｂ）の直線路を脚車輪型ロボット１がＡ点からＢ点へと移動したときの位置の変化に対する本実施の形態の回避用斥力の変化を示す図である。なお、図９中の黒点は、斜線で示す障害物に対する距離の測定点を示す。
脚車輪型ロボット１は、電源の投入に応じて、レーザーレンジセンサ２００に対して、ＣＰＵ６０から指令信号を与えて、指定走査角度範囲及び指定走査角度単位の距離測定を開始させる。
【００７５】
一方、レーザーレンジセンサ２００は、ＣＰＵ６０からの指令信号に応じて、センシングプロセッサにおいて指定された走査角度範囲及び走査角度単位を設定する。
ここで、レーザーレンジセンサ２００は、測距範囲２０〜４０９５[ｍｍ]、最大走査角度範囲２４０°、角度分解能０．３６°の２次元レンジセンサを備えていることとする。
また、第１走査処理に対して、走査角度範囲−４０°〜＋４０°及び走査角度単位０．３６°が設定され、第２走査処理に対して、走査角度範囲２４０°及び走査角度単位１０°が設定されたとする（この場合は、ｘｙ平面において扇形の測定平面が９つ形成される）。
【００７６】
次に、センシングプロセッサは、第１走査処理に対して設定された走査角度範囲及び走査角度単位（θ）に基づき、ドライバに指令信号を出力する。
２次元レンジセンサは、ドライバにおいて、センシングプロセッサからの指令信号及びエンコーダからの出力信号に基づき、第１モータの回転軸を回転駆動して、２次元レンジセンサを、垂直方向の軸（ｚ軸）まわりに、−４０°〜＋４０°の範囲を０．３６°刻みで走査しながら測距を行う。
引き続き、水平方向の軸（ｙ軸）まわりに、回転角度１０°回動して、再び、−４０°〜＋４０°の範囲を０．３６°刻みで走査しながら測距を行う（第２走査処理）。
上記のようにして、第１走査処理及び第２走査処理を行うことで、上記走査範囲内にある障害物の距離情報を測定する。
【００７７】
また、各測定距離の情報（回転座標系）は、データ列Ｌ（θi，φj）として、センシングプロセッサへと出力される。
また、１つの測定平面に対する第１走査処理が終了すると、センシングプロセッサは、第１走査処理において測定された距離情報に対して、メディアンフィルタを用いたフィルタリング処理を行う。これにより、測定情報におけるノイズ成分を除去する。
【００７８】
次に、センシングプロセッサは、フィルタリング処理後の回転座標系の距離情報を、直交座標系の距離情報に変換し、直交座標系に変換された距離情報をハブ７６及び通信Ｉ／Ｆ６４を介してＣＰＵ６０に入力する。また、センシングプロセッサは、測定した距離情報に基づき障害物の速度を算出し、算出した速度情報を、ハブ７６及び通信Ｉ／Ｆ６４を介してＣＰＵ６０に入力する。入力された距離情報は、距離情報記憶部１１０において、メモリに記憶される。
【００７９】
距離情報は、脚車輪型ロボット１の移動経路上に、図９（ａ）に示すような曲がり角がある場合は、図９（ａ）中の黒点に示すように、曲がり角を構成する壁等の障害物の形状に沿った座標情報を有する。また、脚車輪型ロボット１が、図９（ｂ）に示す直線路を移動する場合は、図９（ｂ）中の黒点に示すように、通路を形成する壁などの障害物の形状に沿った座標情報を有する。但し、図９（ａ）及び（ｂ）に示す例は、２次元座標の測定点を示すものである。
【００８０】
一方、障害物回避支援部１０１は、斥力計算部１１１において、操作部３００を介した利用者からの操作入力があったことを判定すると（ステップＳ１００の「Yes」の分岐）、メモリから距離情報（Ｎ個）を取得する（ステップＳ１０２）。操作部３００は、前進用ボタン、後進用ボタン、旋回ボタンなどのボタン操作によるもの、アナログレバーによる操作（レバーを倒した方向に倒した角度に応じた速度で走行）によるものなどが該当する。
【００８１】
次に、斥力計算部１１１において、２次元モードが設定されている場合は上式（４）に、３次元モードが設定されている場合は上式（７）に基づき、取得した各距離情報（測定点）に対する斥力Ｕ（ｎ）を計算する（ステップＳ１０４）。ここで、ｎは、１〜Ｎ（Ｎは測定点の総数）の自然数とする。
斥力Ｕ（ｎ）が計算されると、斥力成分分離部１１２において、斥力Ｕ（ｎ）を、２次元モードが設定されている場合は上式（５）〜（６）に、３次元モードが設定されている場合は上式（８）〜（１０）に基づき、各軸成分に分離する（ステップＳ１０８）。このステップＳ１０６〜Ｓ１０８の処理は、Ｎ個の測定点について全てが終了するまで繰り返し行われる。
これにより、Ｎ個の測定点について、斥力Ｕ（１）〜Ｕ（Ｎ）として、「η／２・（１／ρ１−１／ρ0）2」〜「η／２・（１／ρＮ−１／ρ0）2」が算出される。
【００８２】
また、斥力Ｕ（１）〜Ｕ（Ｎ）が、２次元モードの場合は、ｘ軸成分「Ｆpx（１）〜Ｆpx（Ｎ）」、及びｙ軸成分「Ｆpy（１）〜Ｆpy（Ｎ）」に分離される。また、３次元モードの場合は、ｘ軸成分「Ｆpx（１）〜Ｆpx（Ｎ）」、ｙ軸成分「Ｆpy（１）〜Ｆpy（Ｎ）」、及びｚ軸成分「Ｆpz（１）〜Ｆpz（Ｎ）」に分離される。
Ｎ個の測定点について、上記計算が終了すると（ステップＳ１１０の「Yes」の分岐）、次に、回避用斥力生成部１１３において、分離された軸成分から軸毎の正方向の最大値及び負方向の最大値を抽出する（ステップＳ１１２〜Ｓ１１４）。
具体的に、２次元モードが設定されている場合は上式（１１）〜（１４）に基づき、「Ｆpx（１）〜Ｆpx（Ｎ）」から、ｘ軸成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnを抽出し、「Ｆpy（１）〜Ｆpy（Ｎ）」から、ｙ軸成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynを抽出する。
【００８３】
また、３次元モードが設定されている場合は上式（１１）〜（１６）に基づき、上記ｘ軸成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnと、ｙ軸成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynとに加えて、「Ｆpz（１）〜Ｆpz（Ｎ）」から、ｚ軸成分の正方向の最大値Ｆpzp及び負方向の最大値Ｆpznを抽出する。
回避用斥力生成部１１３は、次に、全ての軸成分の正方向の最大値の中に負の値のものが含まれているか否かを判定し（ステップＳ１１６）、負の値のものが含まれている場合（ステップＳ１１６の「Yes」の分岐）は、負の値となっている最大値を全てゼロ（０）に変更する（ステップＳ１１８）。具体的に、回避用斥力生成部１１３は、２次元モードが設定されている場合は、Ｆpxp及びＦpypのうち負の値となっているものをゼロに変更し、３次元モードが設定されている場合は、Ｆpxp、Ｆpyp及びＦpzpのうち負の値となっているものをゼロに変更する。
【００８４】
つまり、脚車輪型ロボット１が、各軸成分について、障害物から負の斥力しか受けていない場合に、該当する軸の軸成分の正方向の最大値をゼロにする。
引き続き、回避用斥力生成部１１３は、全ての軸成分の負方向の最大値の中に正の値のものが含まれているか否かを判定する（ステップＳ１２０）。そして、正の値のものが含まれている場合（ステップＳ１２０の「Yes」の分岐）は、正の値となっている最大値を全てゼロ（０）に変更する（ステップＳ１２２）。
【００８５】
つまり、脚車輪型ロボット１が、各軸成分について、障害物から正の斥力しか受けていない場合に、該当する軸の軸成分の負方向の最大値をゼロにする。具体的に、回避用斥力生成部１１３は、２次元モードが設定されている場合は、Ｆpxn及びＦpynのうち正の値となっているものをゼロに変更し、３次元モードが設定されている場合は、Ｆpxn、Ｆpyn及びＦpznのうち正の値となっているものをゼロに変更する。
【００８６】
また、全ての軸成分の正方向の最大値がいずれも正の値で、かつ負方向の最大値がいずれも負の値となっている場合（ステップＳ１１６，Ｓ１２０の双方で「No」の分岐）は、正方向及び負方向の最大値はいずれもそのままの値とする。
そして、上記判定及び変更処理後に、回避用斥力生成部１１３は、軸毎に正方向の最大値と負方向の最大値との和を計算する（ステップＳ１２４）。
【００８７】
具体的に、回避用斥力生成部１１３は、２次元モードが設定されている場合は、上式（１）〜（２）に基づき、「Ｆpx＝Ｆpxp＋Ｆpxn」と「Ｆpy＝Ｆpyp＋Ｆpyn」とを計算する。
また、回避用斥力生成部１１３は、３次元モードが設定されている場合は、上式（１）〜（３）に基づき、「Ｆpx＝Ｆpxp＋Ｆpxn」と「Ｆpy＝Ｆpyp＋Ｆpyn」と、「Ｆpz＝Ｆpzp＋Ｆpzn」とを計算する。
【００８８】
そして、回避用斥力生成部１１３は、２次元モードが設定されている場合は、上記計算結果であるｘ軸成分「Ｆpx」と、ｙ軸成分「Ｆpy」とを軸成分として、上式（１７）で示される回避用斥力Ｆpを生成する（ステップＳ１２６）。
また、回避用斥力生成部１１３は、３次元モードが設定されている場合は、上記計算結果であるｘ軸成分「Ｆpx」と、ｙ軸成分「Ｆpy」と、ｚ軸成分「Ｆpz」を軸成分として、上式（１８）で示される回避用斥力Ｆpを生成する（ステップＳ１２６）。
更に、回避用斥力生成部１１３は、生成した回避用斥力Ｆpを、モータ制御部１０２に出力する（ステップＳ１２８）。
【００８９】
一方、モータ制御部１０２は、利用者の操作入力を検出し且つ障害物回避支援部１０１から回避用斥力Ｆpを取得すると（ステップＳ２００及びＳ２０２の「Yes」の分岐）、レーザーレンジセンサ２００から、通信Ｉ／Ｆ６４を介して入力される障害物の位置情報を取得する（ステップＳ２０４）。なお、ここでは、障害物は曲がり角（固定物）であるため、障害物の速度は「０」となる。
【００９０】
次に、角度取込Ｉ／Ｆ６２を介して入力される各モータの回転角度情報を取得し（ステップＳ２０６）、操作入力、回避用斥力Ｆp、位置情報及び回転角度情報に基づき、障害物を回避しつつ、Ａ点からＢ点に移動するためのモータ指令を生成する（ステップＳ２０８）。そして、生成したモータ指令に基づきモータ指令信号をモータ指令出力Ｉ／Ｆ６１を介して、各モータに出力する（ステップＳ２１０）。
これにより、各モータが制御されて脚車輪型ロボット１は、モータ指令信号に応じた方向に、モータ指令信号に応じた速度で車輪走行する。
障害物回避支援部１０１及びモータ制御部１０２の上記一連の処理は、操作入力がある間、繰り返し行われる。
【００９１】
このようにして、脚車輪型ロボット１が、図９（ａ）に示すように、曲がり角に対して、Ａ点からＢ点に移動したとする。この場合に、回避用斥力Ｆpは、Ａ点からＢ点への位置変化に対して、２次元モードが設定されている場合に、図１０（ａ）に示すように、最近接距離の切り替えが生じていても、ｘ軸成分Ｆpx及びｙ軸成分Ｆpyが共に連続的な変化を示すものとなる。
【００９２】
一方、図９（ａ）に示すＡ点からＢ点への移動を行った場合に、従来技術のように最近接距離を用いて計算した回避用斥力は、図１１（ａ）に示すように、Ａ点からＢ点への中間位置において、最近接距離の切り替えが発生しており、中間位置でＦpx及びＦpyの急激な変化が生じている。
また、図９（ｂ）に示すように、ｘ軸の正方向側及び負方向側に障害物が存在し、ｙ軸の正方向側及び負方向側の測定範囲内に障害物が存在しない直線路を、図９（ｂ）中のＡ点からＢ点へと移動した場合も、上記曲がり角のときと同様に回避用斥力Ｆpを計算することができる。
【００９３】
但し、ｙ軸方向に障害物が存在しないため、回避用斥力Ｆpにおけるｙ軸成分Ｆpyがゼロとなる。従って、回避用斥力Ｆpの変化は、ｘ軸成分Ｆpxの変化となる。この場合も、図１０（ｂ）に示すように、最近接距離の切り替えが生じていても、ｘ軸成分Ｆpxは、連続的な変化を示すものとなる。また、このときのｘ軸成分の最大値Ｆpxpの変化は、図中の破線に示すように連続的な変化となり、ｘ軸成分の負方向の最大値Ｆpxnの変化も、図中の点線に示すように連続的な変化となっている。
【００９４】
一方、図９（ｂ）に示すＡ点からＢ点への移動を行った場合に、従来技術のように最近接距離を用いて計算した回避用斥力Ｆpは、図１１（ｂ）に示すように、Ａ点からＢ点への中間位置において、最近接距離の切り替えが発生しており、中間位置でＦp（Ｆpx）の急激な変化が生じている。
このようにして、本実施の形態の脚車輪型ロボット１は、レーザーレンジセンサ２００において、障害物の２次元又は３次元の距離情報を測定し、測定した距離情報に基づき、障害物回避支援部１０１において、障害物の各測定点から仮想的に受ける斥力を計算することができる。
【００９５】
更に、障害物回避支援部１０１において、計算した斥力を軸成分に分離し、分離した軸成分から軸毎に正方向の最大値及び負方向の最大値を抽出し、抽出した正方向と負方向の最大値の和を軸毎に計算し、この計算結果から回避用斥力Ｆpを生成することができる。
更に、モータ制御部１０２において、操作部３００からの操作入力と、回避用斥力Ｆpと、レーザーレンジセンサ２００から入力される障害物の速度情報と、各モータのエンコーダから入力される回転角度情報とに基づきモータ指令信号を生成することができる。
【００９６】
これにより、移動体の移動位置に応じて最近接距離の切り替わりが生じても、回避用斥力は連続して変化するので、速度の急激な変化等が生じない安定した移動制御を行うことができる。
また、斥力の軸成分の正方向の最大値と負方向の最大値との和を用いて回避用斥力Ｆpを生成するようにしたので、障害物の大小や測定点群の密度などに影響を受けない回避用斥力Ｆpを生成することができる。
【００９７】
〔第２の実施の形態〕
次に、本発明の第２の実施の形態を図面に基づき説明する。図１２は、本発明に係る障害物回避支援装置、障害物回避支援方法及び移動体の第２の実施の形態を示す図である。
本実施の形態は、本発明に係る障害物回避支援装置を、移動体である、車輪による走行移動が可能な案内用ロボットに適用したものである。
【００９８】
図１２は、本実施の形態の案内用ロボットの側面図であり、案内用ロボット８１の構成を示す図である。
図１２に示すように、本実施の形態の案内用ロボット８１は、基体８２と、左右一対の移動用車輪８４Ｒ及び８４Ｌと、２つの距離情報測定部８６Ｆ及び８６Ｂと、把持部８８と、入力値検出手段９０と、移動モード切り換え操作部９２と、スピーカ９４と、２つのキャスタ装置９６Ｆ及び９６Ｂとを備えている。
基体８２は、上面視で八角形をなす中空体により形成されている。なお、基体８２の形状は、これに限定するものではなく、例えば、上面視で円形をなす形状に形成してもよい。
【００９９】
移動用車輪８４Ｒ及び８４Ｌは、後述する移動用アクチュエータ１６２が発生する駆動力により回転する車輪である。そして、移動用車輪８４Ｒ及び８４Ｌは、移動用アクチュエータ１６２が発生する駆動力を伝達可能なプーリ等を介して、移動用アクチュエータ１６２に接続されている。また、一対の車輪は、それぞれ、基体８２の下面において、基体８２の中心を基準とした両側面側へ回転可能に配置されている。なお、図１２中では、移動用車輪８４を形成する一対の車輪のうち、左側移動用車輪８４Ｌのみを示している。
【０１００】
距離情報測定部８６Ｆ及び８６Ｂは、上記第１の実施の形態におけるレーザーレンジセンサ２００と同様の構成を有したセンサから形成されたものである。距離情報測定部８６Ｆは基体８２の前面側に配置され、距離情報測定部８６Ｂは基体８２の後面側に配置されている。なお、距離情報測定部８６Ｆ及び８６Ｂは、レーザレンジセンサに限定されるものではなく、例えば、赤外線センサや超音波センサ等を用いて形成してもよい。
【０１０１】
具体的に、距離情報測定部８６Ｆ及び８６Ｂは、上記第１の実施の形態のレーザーレンジセンサ２００と同様に第１走査処理及び第２走査処理を行って、案内用ロボット８１の前面側及び後面側の３次元の測定範囲内にある障害物の距離情報を測定する。なお、距離情報測定部８６Ｆ及び８６Ｂは、第１の走査処理のみを行い、２次元の測定範囲内にある障害物の距離情報を測定する構成としてもよい。
【０１０２】
把持部８は、フレーム部（不図示）と、グリップ部（不図示）とを備え、フレーム部は、入力値検出部（不図示）を介して、基体８２の上面に取り付けられている。
グリップ部は、被案内者（利用者）が片手で把持可能な略円柱状に形成されている。
入力値検出部は、互いに直交する三軸（ｘ，ｙ，ｚ）の方向に付与される力と、これら三軸の軸回りのモーメントをそれぞれ検出可能な六軸力センサであり、フレーム部と基体８２の上面との間に介装されている。
【０１０３】
これにより、入力値検出部は、グリップ部を把持する被案内者により、グリップ部を介した把持部８８への入力を、互いに直交する三軸の方向に付与される力と、これら三軸の軸回りのモーメントとして、それぞれ検出可能に形成されている。
キャスタ装置９６は、キャスタ（不図示）と、伸縮用アクチュエータ（不図示）と、床反力検出部（不図示）とを備えている。
【０１０４】
キャスタは、平衡用車輪と、平衡用車輪支持部と、キャスタ支持軸とを有している。
伸縮用アクチュエータは、キャスタを上下動させてキャスタ装置９６を伸縮させるアクチュエータである。また、伸縮用アクチュエータは、直線運動する直動軸を有しており、キャスタフレームにより支持されている。したがって、キャスタは、基体８２と平衡用車輪との間に介装し、且つ平衡用車輪の回転軸と直交する方向へ伸縮する伸縮部を形成している。
【０１０５】
また、案内用ロボット８１は、上記第１の実施の形態における、図４に示す移動システムと同様の移動システムを備えている。具体的に、図４に示す移動システムから、脚部１２に関連する関節モータ４０、エンコーダ４２、ドライバ４４及び脚先センサ３８等の不要な構成要素を取り除き、レーザーレンジセンサ２００を距離情報測定部８６Ｆ及び８６Ｂに、操作部３００を把持部８８に、スピーカ７８をスピーカ９４にそれぞれ置き換えた構成となる。更に、車輪２０（×４）を駆動する各車輪モータ５０、エンコーダ５２及びドライバ５４を、移動用車輪９６Ｆ及び９６Ｂを駆動するアクチュエータ１６２、エンコーダ及びドライバに置き換えた構成となる。更に、キャスタ装置９６を駆動する伸縮用アクチュエータ、ドライバ、床反力検出部、これらの入出力Ｉ／Ｆ等の案内用ロボット８１に特有の要素を加えた構成となる。
【０１０６】
また、案内用ロボット８１は、上記第１の実施の形態の脚車輪型ロボット１と同様に、車輪走行時における障害物回避時の移動制御に係る機能構成部である、障害物回避支援部１０１と、モータ制御部１０２とを含む移動制御部１００を備えている。
本実施の形態において、障害物回避支援部１０１は、距離情報測定部８６Ｆ及び８６Ｂからの障害物の距離情報に基づき、上記第１の実施の形態と同様に、上式（１）〜（１８）に基づき、回避用斥力Ｆpを計算する。
【０１０７】
また、モータ制御部１０２は、把持部８８からの六軸力センサを介した力入力と、距離情報測定部８６Ｆ又は８６Ｂからの障害物の位置情報と、障害物回避支援部１０１からの回避用斥力Ｆpと、各エンコーダからの回転角度情報とを取得する。そして、これら取得した情報に基づき、障害物の回避動作を含む走行制御のためのアクチュエータ１６２のドライバへの指令信号を生成する。
【０１０８】
このようにして、本実施の形態の案内用ロボット８１は、距離情報測定部８６Ｆ及び８６Ｂにおいて、障害物の２次元又は３次元の距離情報を測定することができる。そして、測定した距離情報に基づき、障害物回避支援部１０１において、案内用ロボット８１の移動経路上（前面側又は後面側の測定範囲内）に存在する障害物の各測定点から仮想的に受ける斥力を計算することができる。
【０１０９】
更に、障害物回避支援部１０１において、計算した斥力を軸成分に分離し、分離した軸成分から軸毎に正方向の最大値及び負方向の最大値を抽出し、抽出した正方向及び負方向の最大値の和を軸毎に計算し、この計算結果から回避用斥力Ｆpを生成することができる。
更に、モータ制御部１０２において、把持部８８からの力入力と、回避用斥力Ｆpと、距離情報測定部８６Ｆ及び８６Ｂから入力される障害物の位置情報と、各モータのエンコーダから入力される回転角度情報とに基づきモータ指令信号を生成することができる。
【０１１０】
これにより、案内用ロボット８１の移動位置に応じて最近接距離の切り替わりが生じても、回避用斥力は連続して変化するので、速度の急激な変化等が生じない安定した移動制御を行うことができる。
また、斥力の軸成分の正方向の最大値と負方向の最大値との和を用いて回避用斥力Ｆpを生成するようにしたので、障害物の大小や測定点群の密度などに影響を受けない回避用斥力Ｆpを生成することができる。
【０１１１】
上記各実施の形態において、距離情報記憶部１１０及びメモリは、距離情報記憶部又は距離情報記憶手段に対応し、斥力計算部１１１は、斥力計算部に対応し、斥力成分分離部１１２は、斥力成分分離部に対応する。
また、上記第１の実施の形態において、回避用斥力生成部１１３は、回避用斥力生成部に対応し、モータ制御部１０２、ドライバ４４，５４は、アクチュエータ制御部に対応する。
【０１１２】
また、上記第２の実施の形態において、回避用斥力生成部１１３は、回避用斥力生成部に対応し、モータ制御部１０２、ドライバ５４は、アクチュエータ制御部に対応する。
また、上記第１の実施の形態において、ステップＳ１０６は、斥力計算ステップに対応し、ステップＳ１０８は、斥力成分分離ステップに対応し、ステップＳ１１０〜Ｓ１２６は、回避用斥力生成ステップに対応する。
【０１１３】
また、上記各実施の形態において、脚車輪型ロボット１又は案内用ロボット８１のＣＰＵ６０、メモリ及び障害物回避支援部１０１によって構成される構成部は、障害物回避支援装置に対応し、脚車輪型ロボット１及び案内用ロボット８１は、移動体に対応する。
なお、上記各実施の形態において、障害物回避支援装置を、脚車輪型ロボット１及び案内用ロボット８１のＣＰＵ６０やメモリ等のハードウェア資源を共用して、ＣＰＵ６０でメモリに記憶された専用のプログラムを実行することで障害物回避支援部１０１の各機能を実現する構成としたが、この構成に限らない。
【０１１４】
例えば、別途プロセッサ及びメモリを有し、該プロセッサによって専用のプログラムを実行して障害物回避支援部１０１の各機能を実現する独立した装置として構成してもよい。
また、専用のプログラムを実行するソフトウェアによる処理ではなく、障害物回避支援部１０１の各処理の一部又は全部を電気回路等のハードウェアを用いて行う構成としてもよい。
【０１１５】
また、上記各実施の形態において、斥力の計算に、上式（４）及び（７）に示す、Khatibの設計したポテンシャル関数を用いる構成を説明したが、この構成に限らず、斥力を計算可能な他の公知のポテンシャル関数を用いる構成としてもよい。
また、上記第１の実施の形態において、本発明に係る障害物回避支援装置を、脚車輪型ロボットに適用し、上記第２の実施の形態において、本発明に係る障害物回避支援装置を、案内用ロボットに適用する例を説明したが、これに限らず、他の移動手段を用いて移動を行うロボットや、船舶、自動車などの他の移動体に適用することも可能である。
【０１１６】
また、上記各実施の形態において、本発明に係る障害物回避支援装置を、利用者の操作に応じて移動動作を行うロボットに適用したが、この構成に限らず、自律移動型のロボット等の自律移動を行うことが可能な移動体に適用することも可能である。
また、上記第１の実施の形態においては、レーザーレンジセンサ２００によって障害物までの距離情報を測定する構成としたが、この構成に限らず、距離画像センサ等の他の測距センサを用いる構成としてもよい。
【０１１７】
また、上記各実施の形態では、レーザーレンジセンサ２００において測距センサ自身を回転させて測定範囲の走査を行っているが、これに限らず、測距センサの光軸上に挿入したミラーを回転させて走査を行う構成としてもよい。
また、上記各実施の形態は、本発明の好適な具体例であり、技術的に好ましい種々の限定が付されているが、本発明の範囲は、上記の説明において特に本発明を限定する旨の記載がない限り、これらの形態に限られるものではない。また、上記の説明で用いる図面は、図示の便宜上、部材ないし部分の縦横の縮尺は実際のものとは異なる模式図である。
また、本発明は上記各実施の形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
【符号の説明】
【０１１８】
１ 脚車輪型ロボット
１００ 移動制御部
１０１ 障害物回避支援部
１０２ モータ制御部
１１０ 距離情報記憶部
１１１ 斥力計算部
１１２ 斥力成分分離部
１１３ 回避用斥力生成部
２００ レーザーレンジセンサ
３００ 操作部
４０、５０ モータ
４２、５２ エンコーダ
１２ｂ、１４、４４、５４ ドライバ
６０ ＣＰＵ
６１ モータ指令出力Ｉ／Ｆ
６２ 角度取込Ｉ／Ｆ
６４ 通信Ｉ／Ｆ
７４ 無線通信部
７６ ハブ
８１ 案内用ロボット
８６Ｆ及び８６Ｂ 距離情報測定部
【技術分野】
【０００１】
本発明は、ポテンシャル法を用いた移動体の障害物の回避技術に関する。
【背景技術】
【０００２】
従来、移動型ロボット等のアクチュエータ等の動力によって移動が可能な移動体において、その移動経路上に存在する障害物を回避する経路計画法として、ポテンシャル法が用いられている。
ポテンシャル法を用いた経路計画では、移動目標位置からは引力を、障害物からは斥力を受けるポテンシャル関数を設計し、設計したポテンシャル関数の低い方向に移動体を移動させることで、障害物を避けながら目標位置まで到達できる経路を生成している。
【０００３】
ポテンシャル法を用いた移動体の移動制御技術として、例えば、特許文献１の移動体及びその移動制御方法、特許文献２の障害物回避装置および移動体などが開示されている。
特許文献１には、ポテンシャル法を用いてロボットの移動制御を行うにあたって、走行制御コントローラが、検知した障害物に関する仮想斥力を計算する際に、設定される予測スパンＴiに相当する時間が経過した後のロボット及び障害物の予測位置から得られる予測最近傍距離ρTiと、ロボットと障害物との相対速度と、ロボットと障害物との許容最小距離ρ0と、を少なくとも考慮して計算を行う発明が記載されている。
【０００４】
また、特許文献２には、電動車椅子に搭載された回避装置が、操作部から入力されるユーザの指示速度Ｗと対物センサから入力される周囲の障害物の状況とを、学習部の格納部に格納し、これに応じて速度Ｕを修正して駆動装置に出力する。更に、格納部に格納した情報と速度センサにて検出する実際の速度Ｙとを抽出部にて比較して、強化学習を行う。更に、抽出部が適切な反発力の係数を学習し、反発力生成部が反発力Ｆを計算して、ユーザの指示速度Ｗに基づく速度Ｕｍに加算することによって、速度Ｕを修正し、電動車椅子の速度Ｕを駆動装置、動力部によって実現する発明が記載されている。
【先行技術文献】
【特許文献】
【０００５】
【特許文献１】特開２００９−２８８９３０号公報
【特許文献２】特開２００５−２９３１５４号公報
【発明の概要】
【発明が解決しようとする課題】
【０００６】
しかしながら、上記特許文献１及び特許文献２の技術においては、障害物から受ける斥力（反発力）の計算に、障害物との最近接（近傍）距離を用いている。そのため、例えば、図９（ａ）に示すような曲がり角や、図９（ｂ）に示す両側が壁の通路（廊下等）、または、移動経路上に複数の障害物が存在するような場合に、移動体の移動に伴って最近接距離の点の切り替わりが発生する。例えば、図９（ａ）及び（ｂ）に示すように、移動体がＡ点からＢ点に移動して、最近接距離の点の切り替わりが発生すると、図９（ａ）に対応する図１１（ａ）、図９（ｂ）に対応する図１１（ｂ）に示すように、斥力ベクトルの急激な変化が発生する。そのため、この変化点における斥力ベクトルを用いて移動制御を行うと、移動体の急激な速度変化等の不具合が生じる恐れがある。なお、図１１において、横軸は移動体の位置であり、縦軸は、図９中の斜線で示す障害物から移動体が仮想的に受ける斥力である。また、図９中の黒点は、距離の測定点を示す。
【０００７】
そこで、本発明は、このような従来の技術の有する未解決の課題に着目してなされたものであって、最近接距離の点の切り替わりが発生する状況において、移動体の障害物の回避動作を支援するのに好適な障害物回避支援装置、障害物回避支援方法及び障害物回避支援装置を備えた移動体を提供することを目的としている。
【課題を解決するための手段】
【０００８】
〔発明１〕 上記目的を達成するために、発明１の障害物回避支援装置は、移動体の移動経路上に存在する障害物の回避動作を支援する障害物回避支援装置であって、
前記移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報を記憶する距離情報記憶部と、
前記距離情報記憶部に記憶された距離情報に基づき、ポテンシャル関数を用いて前記障害物の前記複数の測定点の各測定点から仮想的に受ける斥力を計算する斥力計算部と、
前記斥力計算部で計算した各測定点に対応する斥力を前記測定点の座標を規定する各軸の軸成分に分離する斥力成分分離部と、
前記各測定点の前記分離した軸成分に基づき、前記障害物の回避動作に係る斥力である回避用斥力を生成する回避用斥力生成部と、を備え、
前記回避用斥力生成部は、前記各測定点に対応する斥力の軸成分における各軸の正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算し、該計算結果に基づき前記回避用斥力を生成する。
【０００９】
このような構成であれば、斥力計算部によって、移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報に基づき、ポテンシャル関数を用いて障害物の複数の測定点の各測定点から仮想的に受ける斥力を計算することができる。更に、斥力成分分離部によって、斥力計算部で計算した各測定点に対応する斥力を軸成分に分離することができる。更に、回避用斥力生成部によって、軸成分分離部で分離した各測定点に対応する斥力の軸成分に基づき、各軸の正方向の最大値と負方向の最大値との和を計算し、該計算結果に基づき障害物の回避用斥力を生成することができる。
例えば、回避用斥力として、計算した各軸の軸成分の最大値の和を各軸の成分とした斥力（ベクトル）を生成することができる。
【００１０】
〔発明２〕 更に、発明２の障害物回避支援装置は、発明１の障害物回避支援装置において、前記測定点の座標が、互いに直交するｘ軸及びｙ軸により規定される２次元座標（ｘ，ｙ）で表され、ｎ番目（ｎは、測定点の総数をＮとした場合に１≦ｎ≦Ｎの自然数）の測定点に対応する斥力におけるｘ軸方向の成分がＦpx（ｎ）、ｙ軸方向の成分がＦpy（ｎ）で表される場合に、前記回避用斥力生成部は、前記回避用斥力におけるｘ軸方向の成分Ｆpx及びｙ軸方向の成分Ｆpyを、下式（１）〜（２）に基づき計算し、
Ｆpx＝Ｆpxp＋Ｆpxn ・・・（１）
Ｆpy＝Ｆpyp＋Ｆpyn ・・・（２）
【００１１】
前記式（１）〜（２）において、Ｆpxpは、Ｎ個の測定点に対応する斥力におけるｘ軸の正方向の成分の最大値であり、Ｆpxnは、Ｎ個の測定点に対応する斥力におけるｘ軸の負方向の成分の最大値であり、Ｆpypは、Ｎ個の測定点に対応する斥力におけるｙ軸の正方向の成分の最大値であり、Ｆpynは、Ｎ個の測定点に対応する斥力におけるｙ軸の負方向の成分の最大値であり、前記式（１）〜（２）において、前記Ｆpxpが負の値の場合は当該Ｆpxpの値をゼロとし、前記Ｆpypが負の値の場合は当該Ｆpypの値をゼロとし、前記Ｆpxnが正の値の場合は当該Ｆpxnの値をゼロとし、前記Ｆpynが正の値の場合は当該Ｆpynの値をゼロとする。
このような構成であれば、回避用斥力生成部は、斥力成分分離部で分離したｘ軸方向の成分及びｙ軸方向の成分を用いて、上式（１）〜（２）に基づき回避用斥力Ｆpx及びＦpyを生成することができる。
【００１２】
〔発明３〕 更に、発明３の障害物回避支援装置は、発明２の障害物回避支援装置において、前記測定点の座標が、前記ｘ軸及び前記ｙ軸の双方に直交するｚ軸を更に含む３次元座標（ｘ，ｙ，ｚ）で表され、ｎ番目の測定点に対応する斥力におけるｚ軸方向の成分がＦpz（ｎ）で表される場合に、前記回避用斥力生成部は、前記回避用斥力におけるｚ軸方向の成分Ｆpzを、下式（３）に基づき計算し、
Ｆpz＝Ｆpzp＋Ｆpzn ・・・（３）
【００１３】
前記式（３）において、Ｆpzpは、Ｎ個の測定点に対応する斥力におけるｚ軸の正方向の成分の最大値であり、Ｆpznは、Ｎ個の測定点に対応する斥力におけるｚ軸の負方向の成分の最大値であり、前記Ｆpzpが負の値の場合は当該Ｆpzpの値をゼロとし、前記Ｆpznが正の値の場合は当該Ｆpznの値をゼロとする。
このような構成であれば、回避用斥力生成部によって、回避用斥力Ｆpx及びＦpyに加えて、斥力成分分離部で分離したｚ軸方向の成分を用いて、上式（３）に基づき回避用斥力Ｆpzを生成することができる。
【００１４】
〔発明４〕 更に、発明４の障害物回避支援装置は、発明１又は２の障害物回避支援装置において、前記測定点の座標が、互いに直交するｘ軸及びｙ軸により規定される２次元座標（ｘ，ｙ）で表される場合に、前記斥力計算部は、ｎ番目（ｎは、測定点の総数をＮとした場合に１〜Ｎの自然数）の測定点に対応する斥力Ｕ（ｎ）を下式（４）に示すポテンシャル関数に基づき計算し、前記斥力成分分離部は、前記斥力Ｕ（ｎ）を、下式（５）〜（６）に基づき、ｘ軸方向の成分Ｆpx（ｎ）及びｙ軸方向の成分Ｆpy（ｎ）に分離する。
Ｕ（ｎ）＝η／２・（１／ρn−１／ρ0）2 ・・・（４）
Ｆpx（ｎ）＝∂Ｕ（ｎ）／∂ｘ ・・・（５）
Ｆpy（ｎ）＝∂Ｕ（ｎ）／∂ｙ ・・・（６）
【００１５】
但し、前記式（４）において、ηは、正の重み定数であり、ρ0は、正の定数であって前記ポテンシャル関数が影響を与える範囲であり、ρnは、前記移動体の現在位置からｎ番目の測定点までの距離である。
このような構成であれば、斥力計算部によって、ｎ番目の測定点に対応する斥力Ｕ（ｎ）を上式（４）に示すポテンシャル関数に基づき計算することができる。更に、斥力成分分離部によって、上式（５）〜（６）に基づき、斥力Ｕ（ｎ）を、ｘ軸方向の成分Ｆpx（ｎ）及びｙ軸方向の成分Ｆpy（ｎ）に分離することができる。
【００１６】
〔発明５〕 更に、発明５の障害物回避支援装置は、発明１又は３の障害物回避支援装置において、前記測定点の座標が、各々が互いに直交するｘ軸、ｙ軸及びｚ軸により規定される３次元座標（ｘ，ｙ，ｚ）で表される場合に、前記斥力計算部は、ｎ番目（ｎは、測定点の総数をＮとした場合に１〜Ｎの自然数）の測定点に対応する斥力Ｕ（ｎ）を下式（７）に示すポテンシャル関数に基づき計算し、前記斥力成分分離部は、前記斥力Ｕ（ｎ）を、下式（８）〜（１０）に基づき、ｘ軸方向の成分Ｆpx（ｎ）、ｙ軸方向の成分Ｆpy（ｎ）及びｚ軸方向の成分Ｆpz（ｎ）に分離する。
Ｕ（ｎ）＝η／２・（１／ρn−１／ρ0）2 ・・・（７）
Ｆpx（ｎ）＝∂Ｕ（ｎ）／∂ｘ ・・・（８）
Ｆpy（ｎ）＝∂Ｕ（ｎ）／∂ｙ ・・・（９）
Ｆpz（ｎ）＝∂Ｕ（ｎ）／∂ｚ ・・・（１０）
【００１７】
但し、上式（７）において、ηは、正の重み定数であり、ρ0は、正の定数であって前記ポテンシャル関数が影響を与える範囲であり、ρnは、前記移動体の現在位置からｎ番目の測定点までの距離である。
このような構成であれば、斥力計算部によって、ｎ番目の測定点に対応する斥力Ｕ（ｎ）を上式（７）に示すポテンシャル関数に基づき計算することができる。更に、斥力成分分離部によって、上式（８）〜（１０）に基づき、斥力Ｕ（ｎ）を、ｘ軸方向の成分Ｆpx（ｎ）、ｙ軸方向の成分Ｆpy（ｎ）及びｚ軸方向の成分Ｆpz（ｎ）に分離することができる。
【００１８】
〔発明６〕 一方、上記目的を達成するために、発明６の障害物回避支援方法は、移動体の移動経路上に存在する障害物の回避動作を支援する障害物回避支援方法であって、
前記移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報の記憶された距離情報記憶手段の前記距離情報に基づき、ポテンシャル関数を用いて前記障害物の前記複数の測定点の各測定点から仮想的に受ける斥力を計算する斥力計算ステップと、
前記斥力計算ステップで計算した各測定点に対応する斥力を前記測定点の座標を規定する各軸の軸成分に分離する斥力成分分離ステップと、
前記各測定点の前記分離した軸成分に基づき、前記障害物の回避動作に係る斥力である回避用斥力を生成する回避用斥力生成ステップと、を含み、
前記回避用斥力生成ステップにおいては、前記各測定点に対応する斥力の軸成分における各軸の正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算し、該計算結果に基づき前記回避用斥力を生成する。
これにより、発明１の障害物回避支援装置と同等の作用を得ることができる。
【００１９】
〔発明７〕 また、上記目的を達成するために、発明７の移動体は、基体と、
前記基体を移動させるアクチュエータを有する移動機構と、
移動経路上に存在する障害物の形状に沿った複数の測定点に対する距離情報を測定する距離情報測定部と、
操作部を介した利用者からの当該移動体の移動方向の情報を含む移動に係る指示入力を受け付ける指示入力受付部と、
請求項１乃至請求項５のいずれか１項に記載の障害物回避支援装置と、
前記障害物回避支援装置において前記距離情報測定部で測定した距離情報に基づき計算した回避用斥力と前記指示入力受付部で受け付けた指示入力とに基づき、前記アクチュエータを制御するアクチュエータ制御部と、を備える。
このような構成であれば、発明１乃至５のいずれか１の障害物回避支援装置と同等の作用を得ることができる。
【発明の効果】
【００２０】
以上説明したように、発明１〜５の障害物回避支援装置、発明６の障害物回避支援方法及び発明７の移動体によれば、障害物の回避（経路計画）に係る斥力ベクトルを生成する際に、障害物の各測定点から受ける斥力を軸成分に分離し、分離した軸成分のうち正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算する。そして、計算した各軸の最大値の和を各軸の成分とした回避用斥力を生成することができる。
【００２１】
これにより、障害物の回避（経路計画）において、最大斥力を用いることから、従来の最近接距離を用いて生成した斥力ベクトルと比較して、最近接距離が切り替わりによる影響を受けなくなる。具体的に、連続的に変化する斥力ベクトルを生成することができる。従って、移動体の障害物回避時の移動制御において、最近接距離の切り替わりによって急激に変化する斥力ベクトルが用いられるのを防ぐことができ、安全且つ安定した移動体の障害物の回避制御を行うことができるという効果が得られる。
【００２２】
また、レーザーレンジファインダや距離画像センサ等の測域センサによって測定した測定点群の距離情報から求めた斥力の総和等を用いて回避用斥力を計算することが考えられる。この場合に、測域センサを用いた距離測定において、障害物上の測定点群は、測域センサから障害物までの距離に応じてその密度（粒状性）が変化する。そのため、同じ大きさの物体でも、その距離によって測定点群の密度が変化し、障害物上の測定点の数が変化してしまう。また、比較的小さな障害物は、測定点の数が少なくなるためその斥力の影響が小さくなってしまい、接触、衝突の可能性が上昇する。
上記発明１〜７によれば、斥力の軸成分の正方向と負方向の最大値の和を用いるため、障害物の大小や測定点群の密度などに影響を受けない回避用斥力を計算することができるという効果が得られる。
【図面の簡単な説明】
【００２３】
【図１】（ａ）は、脚車輪型ロボット１の正面図であり、（ｂ）は、脚車輪型ロボット１の側面図である。
【図２】レーザーレンジセンサ２００の距離測定の原理を説明する図である。
【図３】（ａ）は、測距センサを、垂直方向の軸（ｚ軸）まわりに回転駆動させたときの測定距離と回転角度θとの関係を示す図であり、（ｂ）は、測距センサを、水平方向の軸（ｙ軸）まわりに回転駆動したときの測定平面と回転角度φとの関係を示す図である。
【図４】脚車輪型ロボット１の移動制御システムを示すブロック図である。
【図５】移動制御システムの備える、障害物回避時の移動制御に係る機能構成を示すブロック図である。
【図６】障害物回避支援部１０１の詳細な機能構成を示すブロック図である。
【図７】回避用斥力Ｆpの生成処理を示すフローチャートである。
【図８】障害物回避時のモータ制御処理を示すフローチャートである。
【図９】（ａ）及び（ｂ）は、障害物と脚車輪型ロボット１との位置関係の例を示す図である。
【図１０】（ａ）及び（ｂ）は、脚車輪型ロボット１が、図９（ａ）及び（ｂ）に示すＡ点からＢ点へと移動したときの位置の変化に対する本発明の回避用斥力の変化を示す図である。
【図１１】（ａ）及び（ｂ）は、脚車輪型ロボット１が、図９（ａ）及び（ｂ）に示すＡ点からＢ点へと移動したときの位置の変化に対する従来の回避用斥力の変化を示す図である。
【図１２】実施の形態の案内用ロボットの側面図であり、案内用ロボット８１の構成を示す図である。
【発明を実施するための形態】
【００２４】
〔第１の実施の形態〕
以下、本発明の第１の実施の形態を図面に基づき説明する。図１〜図１０は、本発明に係る障害物回避支援装置、障害物回避支援方法及び移動体の第１の実施の形態を示す図である。
本実施の形態は、本発明に係る障害物回避支援装置を、移動体である、脚部による歩行移動及び車輪による走行が可能な脚車輪型ロボットに適用したものである。
【００２５】
まず、図１に基づき、本実施の形態に係る脚車輪型ロボットの外観構成を説明する。
図１（ａ）は、脚車輪型ロボット１の正面図であり、（ｂ）は、脚車輪型ロボット１の側面図である。
脚車輪型ロボット１は、図１（ａ）及び（ｂ）に示すように、基体１０と、基体１０に連結された４つの脚部１２とを有して構成されている。
基体１０の前方には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。また、基体１０の後方には、２本の脚部１２が回転関節１４を介して左右対称の位置に連結されている。
【００２６】
回転関節１４は、脚車輪型ロボット１の底面と直交する方向を軸方向として回転する。すなわち、ヨー軸回りに回転する。
各脚部１２には、２つの回転関節１６、１８が設けられている。回転関節１６、１８は、回転関節１４が図１４のような状態であるときは、脚車輪型ロボット１の側面と直交する方向を軸方向として回転する。したがって、脚部１２は、それぞれ３自由度を有する。
【００２７】
各脚部１２の先端には、回転関節１６、１８と軸方向を同一にして駆動輪２０が回転可能に設けられている。駆動輪２０は、回転関節１４の回転によりヨー軸周りに回動する。つまり、回転関節１４の回転を制御することで、走行移動時の操舵制御が行われる。
各脚部１２の先端には、脚車輪型ロボット１の移動経路上に存在する物体までの脚先からの距離を測定する脚先センサ３８が設けられている。
【００２８】
一方、基体１０の正面の下部中央には、水平面レーザー光を照射する水平レーザ２６が設けられている。また、基体１０の正面の中央左右には、垂直面レーザー光を照射する垂直レーザー２８、３０がそれぞれ設けられている。
基体１０の正面の上部中央には、水平面レーザー光および垂直面レーザー光の反射光を含む画像を撮影するカメラ３２が設けられている。
【００２９】
また、カメラ３２の下側には、レーザーレンジセンサ２００が配設されている。
水平レーザー２６は、カメラ３２で水平面レーザー光の反射光を含む画像が撮影できるように下方に所定角度傾けて設けられている。同様に、垂直レーザー２８は、カメラ３２で垂直面レーザー光の反射光を含む画像が撮影できるように右方に所定角度傾けて設けられ、垂直レーザー３０は、左方に所定角度傾けて設けられている。
【００３０】
なお、各脚部１２の回転関節１４〜１８には、回転関節１４〜１８を回転駆動する関節モータ４０がそれぞれ設けられている。また、各脚部１２の駆動輪２０には、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。
レーザーレンジセンサ２００は、図示しないが、センシングプロセッサと、測定範囲内に存在する物体上の測定点までの距離を測定する測距センサと、測距センサを垂直方向の軸まわり及び水平方向の軸まわりに回転駆動する回転機構とを含んで構成されている。
【００３１】
更に、レーザーレンジセンサ２００は、図示しないが、回転機構を垂直方向の軸まわりに回転駆動する第１モータと、水平方向の軸まわりに回転駆動する第２モータと、これらモータの回転角度位置を検出するエンコーダと、センシングプロセッサからの指令信号及び各エンコーダからの信号に基づき各モータを制御するドライバとを含んで構成されている。
ドライバは、センシングプロセッサからの指令信号において指定された走査角度範囲及び走査角度単位（例えば、０．３６°などの所定の回転角度）に基づき、第１及び第２モータの回転軸を、指定された走査角度単位ずつ回転させる制御を行う。
【００３２】
ここで、測距センサの原点位置（走査角度が０°の位置）における、レーザーの出力方向の軸をｘ軸、ｘ軸に対して直交する２つの軸のうち一方の軸をｙ軸（本実施の形態では、センサの横方向と水平方向の軸となる）、他方の軸をｚ軸（本実施の形態では、垂直方向の軸となる）とする。
【００３３】
第１モータは、測距センサのレーザー出力部（不図示）及び受光部（不図示）を、垂直方向の軸まわり（ｚ軸）に回転駆動するように設けられ、第２モータは、出力部及び受光部を水平方向の軸（ｙ軸）まわりに回転駆動するように設けられている。そして、ドライバからの制御信号に応じて、第１モータは、自己の回転軸を指定された走査角度単位（回転角度θ）ずつ回転駆動し、第２モータは、自己の回転軸を指定された走査角度単位（回転角度φ）ずつ回転駆動する。
【００３４】
ここで、図２は、レーザーレンジセンサ２００の距離測定の原理を説明する図である。
レーザーレンジセンサ２００は、測距センサが、指定された走査角度単位ずつ回動し、且つ回動する毎に、図２に示すように、レーザー出力部からレーザー光を出力すると共に、出力光に対する物体（図２中の障害物）からの反射光を受光部で受光する。そして、各回転角度（走査角度）に応じた距離（図２中の測定距離Ｌ（物体と受光部との間の距離））を測定する。
【００３５】
また、図３（ａ）は、測距センサを、垂直方向の軸（ｚ軸）まわりに回転駆動させたときの測定距離と回転角度θとの関係を示す図であり、（ｂ）は、測距センサを、水平方向の軸（ｙ軸）まわりに回転駆動したときの測定平面と回転角度φとの関係を示す図である。
例えば、図３（ａ）に示すように、測距センサを、ｚ軸まわりに指定された走査角度単位ずつ回動させながら、原点位置に対する各回転角度（図３（ａ）中のθ1、θ2、θ3）に応じた距離情報（図３（ａ）中のＬ（θ1）、Ｌ（θ2）、Ｌ（θ3））を測定する。
【００３６】
また、第１モータの回転軸の回転中心と、レーザーの走査軌道線の両端とを結んで形成される平面が、ｘｙ測定平面（物体が存在しない場合は扇形の平面）となる。
ｘｙ測定平面の走査処理（第１走査処理）が完了する毎に、図３（ｂ）に示すように、測距センサを、ｙ軸まわりに指定された走査角度単位（φ）ずつ回動させる。この走査処理を第２走査処理という。
【００３７】
そして、第１走査処理と第２走査処理とを交互に連続して行うことにより、第１走査処理によって形成される測定平面を、ｙ軸まわりに連続して形成する。これにより、測定範囲内（走査角度範囲内）に存在する物体の３次元の距離情報を測定することが可能である。
また、図３（ｂ）に示すように、第２走査処理後の各測定点の距離情報を、Ｌ（θi，φj）と表記する。ここで、ｉは、ｚ軸まわりの走査角度に応じて各測定点に付与される通し番号であり、ｊは、ｙ軸まわりの走査角度に応じて各測定点に付与される通し番号である。
【００３８】
本実施の形態において、脚車輪型ロボット１は、ＣＰＵ６０において、レーザーレンジセンサ２００で測定した自己の移動経路上に存在する障害物の距離情報及び速度情報に基づき、ポテンシャル法を用いた障害物の回避動作を含む移動制御を行うようになっている。
また、レーザーレンジセンサ２００のセンシングプロセッサにおいて、ノイズ除去のために、測距センサで測定した回転座標系の距離情報に対してメディアンフィルタを用いたフィルタリング処理を実行する。また、センシングプロセッサにおいて、測定対象の物体の移動速度を計算する処理を実行する。
【００３９】
次に、図４に基づき、脚車輪型ロボット１の移動制御システムについて説明する。
ここで、図４は、脚車輪型ロボット１の移動制御システムを示すブロック図である。
脚車輪型ロボット１の４本の各脚部１２の回転関節１４，１６，１８には、図４に示すように、各回転関節１４，１６，１８を回転駆動する関節モータ４０がそれぞれ設けられている。各関節モータ４０には、関節モータ４０の回転角度位置を検出するエンコーダ４２と、モータ指令信号およびエンコーダ４２の出力信号に基づいて関節モータ４０の駆動を制御するドライバ４４が設けられている。
【００４０】
各脚部１２の駆動輪２０には、駆動輪２０を回転駆動する車輪モータ５０がそれぞれ設けられている。各車輪モータ５０には、車輪モータ５０の回転角度位置を検出するエンコーダ５２と、モータ指令信号およびエンコーダ５２の出力信号に基づいて車輪モータ５０の駆動を制御するドライバ５４が設けられている。
脚車輪型ロボット１は、さらに、ＣＰＵ６０と、脚車輪型ロボット１の姿勢を検出する３軸姿勢センサ７０と、カメラ３２の画像信号を処理するビジョンプロセッサ７２と、レーザーレンジセンサ２００と、を有して構成される。
【００４１】
３軸姿勢センサ７０は、ジャイロ若しくは加速度センサ、またはその両方を有し、地軸に対して脚車輪型ロボット１の姿勢の傾きを検出する。
脚車輪型ロボット１は、さらに、脚車輪型ロボット１を無線通信を用いて遠隔操作可能な操作部３００等と無線通信を行う無線通信部７４と、ビジョンプロセッサ７２、無線通信部７４およびレーザーレンジセンサ２００とＣＰＵ６０の入出力を中継するハブ７６と、警告音等を出力するスピーカ７８とを有して構成される。
【００４２】
ＣＰＵ６０は、モータ指令出力Ｉ／Ｆ６１を介してドライバ４４、５４にモータ指令信号を出力し、角度取込Ｉ／Ｆ６２を介してエンコーダ４２、５２の出力信号を入力する。
ＣＰＵ６０は、レーザーレンジセンサ２００に対して、通信Ｉ／Ｆ６４及びハブ７６を介して、第１及び第２モータの回転角度位置を検出するエンコーダの出力信号を入力し、第１及び第２モータのドライバにモータ指令信号を出力する。
【００４３】
ＣＰＵ６０は、センサ入力Ｉ／Ｆ６３を介して、平面内走査脚先センサ３８、レーザーレンジセンサ２００および３軸姿勢センサ７０からそれぞれセンサ信号を入力する。また、通信Ｉ／Ｆ６４を介してハブ７６と信号の入出力を行い、サウンド出力Ｉ／Ｆ６５を介してスピーカ７８に音声信号を出力する。
ＣＰＵ６０は、レーザーレンジセンサ２００から距離情報が入力されると、そのセンサ座標系の座標をグローバル座標系に変換する座標変換処理を行う。座標変換処理では、脚車輪型ロボット１中心のグローバル座標をＸc、Ｙc、Ｚc、脚車輪型ロボット１の姿勢（ロール角、ピッチ角、ヨー角）をφ、θ、ψとして、センサ座標系における各測定点を、グローバル座標系の測定点（Ｘs、Ｙs、Ｚs）に変換する。
【００４４】
また、歩行動作時において、３軸姿勢センサ７０のセンサ信号、並びにグローバル座標系に変換された各測定点に基づいて逆運動学計算および重心計算を行い、この計算結果に基づいて脚先（駆動輪）の着地位置を決定し、物体平面までの距離を算出し、脚先と物体平面の位置関係を算出し、決定した着地位置および算出した両距離に基づいてドライバ４４、５４へのモータ指令信号を生成し、生成したモータ指令信号をドライバ４４、５４に出力する。
【００４５】
また、車輪走行動作時において、グローバル座標系に変換された障害物の各測定点に基づいてポテンシャル法を用いた斥力の計算を行い、この計算結果に基づいてドライバ４４、５４へのモータ指令信号を生成し、生成したモータ指令信号をドライバ４４、５４に出力する。
なお、図示していないが、脚車輪型ロボット１は、専用のプログラムやロボットの制御に必要なパラメータ値等を記憶するＲＯＭや、プログラムの実行に必要なデータを一時記憶するＲＡＭ等のメモリを備えている。
【００４６】
次に、図５及び図６に基づき、移動制御システムにおいて実行される、脚車輪型ロボット１の車輪走行時における障害物回避時の移動制御処理に係る機能構成について説明する。
ここで、図５は、移動制御システムの備える、障害物回避時の移動制御に係る機能構成を示すブロック図である。また、図６は、障害物回避支援部１０１の詳細な機能構成を示すブロック図である。
【００４７】
図５に示すように、移動制御システムは、車輪走行時における障害物回避時の移動制御に係る機能構成部である、障害物回避支援部１０１と、モータ制御部１０２とを含む移動制御部１００を備えている。
障害物回避支援部１０１は、レーザーレンジセンサ２００で測定した障害物の各測定点の距離情報に基づき、ポテンシャル法を用いて、脚車輪型ロボット１が障害物の各測定点から受ける斥力を計算する機能を有している。更に、各測定点の斥力の計算結果に基づき、障害物回避用の斥力である回避用斥力を計算する機能を有している。
【００４８】
具体的に、障害物回避支援部１０１は、図６に示すように、距離情報記憶部１１０と、斥力計算部１１１と、斥力成分分離部１１２と、回避用斥力生成部１１３とを含んで構成される。
距離情報記憶部１１０は、レーザーレンジセンサ２００から通信Ｉ／Ｆ６４を介して入力される距離情報を、ＲＡＭ等のメモリに記憶する機能を有している。
斥力計算部１１１は、上式（４）又は（７）のポテンシャル関数に基づき、メモリに記憶されたＮ個の測定点１〜Ｎ（Ｎは、２以上の自然数）の距離情報を用いて、Ｎ個の各測定点に対応する斥力Ｕ（ｎ）（ｎは、１〜Ｎの自然数）を計算する機能を有している。
【００４９】
ここで、上式（４）及び（７）は、障害物から受ける斥力を計算するためにKhatibにより設計された公知のポテンシャル関数であり、ηは、正の重み定数であり、ρ0は、正の定数であってポテンシャル関数が影響を与える範囲であり、ρnは、脚車輪型ロボット１と障害物上のｎ番目の測定点との距離である。このとき、斥力はポテンシャル関数の傾く方向への力を受ける。
【００５０】
また、本実施の形態においては、障害物の回避支援モードとして、２次元モードと３次元モードとが設定可能となっている。２次元モードは、上述した、第１走査処理及び第２走査処理を実行して測定された障害物の距離情報のうち、所定の１つのｚ座標のｘｙ平面の測定点の距離情報を用いて回避用斥力を計算するモードであり、３次元モードは、全ての測定点の距離情報を用いて回避用斥力を計算するモードである。従って、モードに応じてＮの値が変わる。
【００５１】
具体的に、斥力計算部１１１は、２次元モードが設定されている場合に、所定の１つのｚ座標（例えば、原点の座標）のｘｙ平面の測定情報を用いて斥力の計算を行い、３次元モードが設定されている場合に、全て又は移動に係る範囲に対応する一部の３次元の測定情報を用いて斥力の計算を行う。
斥力成分分離部１１２は、斥力計算部１１１で計算された障害物上のＮ個の測定点に対応する斥力Ｕ（１）〜Ｕ（Ｎ）を、上式（５）〜（６）又は上式（８）〜（１０）に基づき、軸毎の成分に分離する機能を有している。
【００５２】
具体的に、斥力成分分離部１１２は、２次元モードが設定されているときは、上式（５）〜（６）に基づき、斥力Ｕ（ｎ）をｘ軸の成分Ｆpx（ｎ）と、ｙ軸の成分Ｆpy（ｎ）とに分離する。また、３次元モードが設定されているときは、上式（８）〜（１０）に基づき、斥力Ｕ（ｎ）をｘ軸の成分Ｆpx（ｎ）、ｙ軸の成分Ｆpy（ｎ）及びｚ軸の成分Ｆpz（ｎ）に分離する。ここで、各式における（ｘ，ｙ，ｚ）は、上記ＣＰＵ６０で座標変換されたグローバル座標系の（Ｘs、Ｙs、Ｚs）に対応している。
回避用斥力生成部１１３は、下式（１１）〜（１６）に基づき、斥力成分分離部１１２で分離された、斥力Ｕ（１）〜Ｕ（Ｎ）の各軸成分のうち、正方向の最大値と負方向の最大値とを軸毎に抽出する機能を有している。
【００５３】
【数１】
【００５４】
なお、上式（１１）は、ｘ軸成分Ｆpx（１）〜（Ｎ）から最大値Ｆpxpを抽出する関数であり、上式（１３）は、ｙ軸成分Ｆpy（１）〜（Ｎ）から最大値Ｆpypを抽出する関数である。また、上式（１５）は、ｚ軸成分Ｆpz（１）〜（Ｎ）から最大値Ｆpzpを抽出する関数である。基本的に、上式（１１）及び（１３）は、各軸成分の正方向の最大値を抽出することを目的とした関数である。以降は、抽出される値の正負に係わらずＦpxp、Ｆpyp及びＦpzpを、各軸成分の正方向の最大値として扱うこととする。
【００５５】
また、上式（１２）は、ｘ軸成分Ｆpx（１）〜（Ｎ）から最小値Ｆpxnを抽出する関数であり、上式（１４）は、ｙ軸成分Ｆpy（１）〜（Ｎ）から最小値Ｆpynを抽出する関数である。また、上式（１６）は、ｚ軸成分Ｆpz（１）〜（Ｎ）から最小値Ｆpznを抽出する関数である。基本的に、上式（１２）及び（１４）は、各軸成分の負方向の最大値を抽出することを目的とした関数である。以降は、抽出される値の正負に係わらずＦpxn、Ｆpyn及びＦpznを、各軸成分の負方向の最大値として扱うこととする。
【００５６】
更に、回避用斥力生成部１１３は、上式（１）〜（３）に基づき、抽出した軸毎の正方向の最大値と負方向の最大値との和を計算し、この計算結果から回避用斥力Ｆpを生成する機能を有している。この回避用斥力Ｆpは、障害物を回避するために移動制御で用いる斥力である。
回避用斥力生成部１１３は、具体的に、２次元モードが設定されている場合に、まず、上式（１１）〜（１４）に基づき、ｘ軸の斥力成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnを計算し、ｙ軸の斥力成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynを計算する。
【００５７】
次に、回避用斥力生成部１１３は、上式（１）〜（２）に基づき、ｘ軸成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnの和であるＦpxと、ｙ軸成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynの和であるＦpyとを計算する。このとき、Ｆpxpが負の値の場合はＦpxpの値をゼロ（０）にし、Ｆpypが負の値の場合はＦpypの値をゼロ（０）にする。このような状況は、例えば、障害物から負方向の斥力しか受けないときに生じる。更に、Ｆpxnが正の値の場合はＦpxnの値をゼロ（０）にし、Ｆpynが正の値の場合はＦpynの値をゼロ（０）にする。このような状況は、例えば、障害物から正方向の斥力しか受けないときに生じる。
更に、回避用斥力生成部１１３は、上記ｘ軸及びｙ軸についての正方向の最大値と負方向の最大値との和の計算結果Ｆpx及びＦpyに基づき、下式（１７）に示すように、Ｆpxをｘ軸成分とし、Ｆpyをｙ軸成分としたベクトルである回避用斥力Ｆpを生成する。
【００５８】
【数２】
【００５９】
また、回避用斥力生成部１１３は、３次元モードが設定されている場合に、上式（１１）〜（１６）に基づき、ｘ軸の斥力成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnを計算し、ｙ軸の斥力成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynを計算し、ｚ軸の斥力成分の正方向の最大値Ｆpzp及び負方向の最大値Ｆpznを計算する。更に、上式（１）〜（３）に基づき、ｘ軸成分の正方向の最大値Ｆpxpと負方向の最大値Ｆpxnとの和であるＦpxと、ｙ軸成分の正方向の最大値Ｆpypと負方向の最大値Ｆpynとの和であるＦpyと、ｚ軸成分の正方向の最大値Ｆpzpと負方向の最大値Ｆpznとの和であるＦpzとを計算する。そして、下式（１８）に示すように、Ｆpxをｘ軸成分とし、Ｆpyをｙ軸成分とし、Ｆpzをｚ軸成分としたベクトルである回避用斥力Ｆpを生成する。
なお、ｚ軸成分についても、Ｆpzpが正の値の場合はＦpzpの値をゼロ（０）にし、Ｆpznが正の値の場合はＦpznの値をゼロ（０）にする。}
【００６０】
【数３】
【００６１】
回避用斥力生成部１１３は、回避用斥力Ｆpを生成すると、生成したＦpをモータ制御部１０２に出力する。
図５に戻って、モータ制御部１０２は、操作部３００からの指示入力と、レーザーレンジセンサ２００からの障害物の位置情報と、障害物回避支援部１０１からの回避用斥力Ｆpと、各エンコーダからの回転角度情報とに基づき、障害物の回避動作を含む走行制御のためのモータ指令信号を生成する機能を有している。
【００６２】
例えば、利用者の操作入力が障害物方向への移動を示していた場合に、モータを停止する指令信号を生成したり、自動で障害物を回避する軌道を移動させる指令信号を生成したりする。
なお、本実施の形態において、上記障害物回避支援部１０１の各構成部及びモータ制御部１０２の各機能は、ＣＰＵ６０において専用のプログラムを実行することで実現される。
【００６３】
次に、図７に基づき、障害物回避支援部１０１の回避用斥力Ｆpの生成処理の流れを説明する。ここで、図７は、回避用斥力Ｆpの生成処理を示すフローチャートである。
回避用斥力Ｆpの生成処理は、ＣＰＵ６０が、ＲＯＭに記憶された専用のプログラムを読み出し、当該読み出したプログラムを実行することで実現される処理であり、処理が開始されると、図７に示すように、まず、ステップＳ１００に移行する。
【００６４】
ステップＳ１００では、斥力計算部１１１において、利用者の操作に応じた操作部３００からの通信Ｉ／Ｆ６４を介した操作入力があったか否かを判定し、操作入力があったと判定した場合(Yes)は、ステップＳ１０２に移行する。一方、操作入力がなかったと判定した場合(No)は、操作入力があるまで判定処理を繰り返す。
ステップＳ１０２に移行した場合は、斥力計算部１１１において、メモリから距離情報を取得して、ステップＳ１０４に移行する。
【００６５】
ステップＳ１０４では、斥力計算部１１１において、取得した距離情報に基づき、障害物があるか否かを判定し、障害物があると判定した場合(Yes)は、ステップＳ１０６に移行し、そうでない場合(No)は、一連の処理を終了する。
ステップＳ１０６に移行した場合は、斥力計算部１１１において、上式（４）又は（７）のポテンシャル関数に基づき、Ｎ個の距離情報の各距離情報を用いて各測定点に対応する斥力Ｕ（ｎ）を計算して、ステップＳ１０８に移行する。
【００６６】
ステップＳ１０８では、斥力成分分離部１１２において、ステップＳ１０６で計算した斥力Ｕ（ｎ）を、上式（５）〜（６）又は（８）〜（１０）に基づき、各軸成分に分離して、ステップＳ１１０に移行する。
ステップＳ１１０では、斥力成分分離部１１２において、Ｎ個の測定点について、斥力Ｕ（１）〜Ｕ（Ｎ）の計算及びこれらの各軸成分への分離処理が終了したか否かを判定し、終了したと判定した場合(Yes)は、ステップＳ１１２に移行する。また、終了していないと判定した場合(No)は、ステップＳ１０６に移行する。
【００６７】
ステップＳ１１２に移行した場合は、回避用斥力生成部１１３において、設定されたモードに応じて、上式（１１）及び（１３）又は上式（１１）、（１３）及び（１５）に基づき、Ｎ個の測定点に対応する軸成分の中から軸毎に最大値を抽出して、ステップＳ１１４に移行する。
ステップＳ１１４では、回避用斥力生成部１１３において、設定されたモードに応じて、上式（１２）及び（１４）又は上式（１２）、（１４）及び（１６）に基づき、Ｎ個の測定点に対応する軸成分の中から軸毎に負方向の最大値を抽出して、ステップＳ１１６に移行する。
【００６８】
ステップＳ１１６では、回避用斥力生成部１１３において、設定されたモードに応じた全ての軸の正方向の最大値の中に負の値のものがあるか否かを判定し、負の値のものがあると判定した場合(Yes)は、ステップＳ１１８に移行し、そうでない場合(No)は、ステップＳ１２０に移行する。
ステップＳ１１８に移行した場合は、回避用斥力生成部１１３において、負の値となっている正方向の最大値Ｆp-pをゼロ（０）に変更して、ステップＳ１２０に移行する。なお、Ｆp-pの「−」部分は、ｘ，ｙ，ｚのうち負の値となる軸が全て該当する。
【００６９】
ステップＳ１２０では、回避用斥力生成部１１３において、設定されたモードに応じた全ての軸の負方向の最大値の中に正の値のものがあるか否かを判定する。そして、正の値のものがあると判定した場合(Yes)は、ステップＳ１２２に移行し、そうでない場合(No)は、ステップＳ１２４に移行する。
ステップＳ１２２に移行した場合は、回避用斥力生成部１１３において、正の値となっている負方向の最大値Ｆp-nをゼロ（０）に変更して、ステップＳ１２４に移行する。なお、Ｆp-nの「−」部分は、ｘ，ｙ，ｚのうち正の値となる軸が全て該当する。
【００７０】
一方、ステップＳ１２４に移行した場合は、回避用斥力生成部１１３において、上式（１）〜（２）又は（１）〜（３）に基づき、軸毎に正方向と負方向の最大値の和を計算して、ステップＳ１２６に移行する。
ステップＳ１２６では、回避用斥力生成部１１３において、ステップＳ１２４の計算結果に基づき、回避用斥力Ｆpを生成して、ステップＳ１２８に移行する。
ステップＳ１２８では、回避用斥力生成部１１３において、ステップＳ１２６で生成した回避用斥力Ｆpを、モータ制御部１０２に出力して、一連の処理を終了する。
なお、上記一連の処理は、脚車輪型ロボット１の移動に伴って、新たに測定されるＮ個の測定情報が記憶される毎に繰り返し行われる。
【００７１】
次に、図８に基づき、モータ制御部１０２における、障害物回避時のモータ制御処理の流れを説明する。
ここで、図８は、障害物回避時のモータ制御処理を示すフローチャートである。
障害物回避時のモータ制御処理は、ＣＰＵ６０が、ＲＯＭに記憶された専用のプログラムを読み出し、当該読み出したプログラムを実行することで実現される処理であり、処理が開始されると、図８に示すように、まず、ステップＳ２００に移行する。
ステップＳ２００では、モータ制御部１０２において、利用者の操作に応じた操作部３００からの通信Ｉ／Ｆ６４を介した操作入力があったか否かを判定し、操作入力があったと判定した場合(Yes)は、ステップＳ２０２に移行する。一方、操作入力がなかったと判定した場合(No)は、操作入力があるまで判定処理を繰り返す。
【００７２】
ステップＳ２０２に移行した場合は、モータ制御部１０２において、障害物回避支援部１０１から回避用斥力Ｆpを取得したか否かを判定し、取得したと判定した場合(Yes)は、ステップＳ２０４に移行し、そうでない場合(No)は、取得するまで判定処理を繰り返す。
ステップＳ２０４に移行した場合は、モータ制御部１０２において、通信Ｉ／Ｆ６４を介して、障害物の位置情報を取得して、ステップＳ２０６に移行する。
【００７３】
ステップＳ２０６では、モータ制御部１０２において、角度取込Ｉ／Ｆ６２を介して、各モータの回転角度情報を取得して、ステップＳ２０８に移行する。
ステップＳ２０８では、モータ制御部１０２において、操作入力、回避用斥力、位置情報及び回転角度情報に基づき、障害物を回避するためのモータ指令を生成して、ステップＳ２１０に移行する。
ステップＳ２１０では、モータ制御部１０２において、モータ指令出力Ｉ／Ｆ６１を介して、モータ指令信号を各モータのドライバに出力して、ステップＳ２００に移行する。
【００７４】
次に、図９〜図１１に基づき、本実施の形態の動作を説明する。
ここで、図９（ａ）は、曲がり角を形成する壁（障害物）と脚車輪型ロボット１との位置関係の一例を示す図であり、（ｂ）は、直線路を形成する壁と脚車輪型ロボット１との位置関係の一例を示す図である。また、図１０（ａ）は、図９（ａ）の曲がり角を脚車輪型ロボット１がＡ点からＢ点へと移動したときの位置の変化に対する本実施の形態の回避用斥力の変化を示す図である。また、図１０（ｂ）は、図９（ｂ）の直線路を脚車輪型ロボット１がＡ点からＢ点へと移動したときの位置の変化に対する本実施の形態の回避用斥力の変化を示す図である。なお、図９中の黒点は、斜線で示す障害物に対する距離の測定点を示す。
脚車輪型ロボット１は、電源の投入に応じて、レーザーレンジセンサ２００に対して、ＣＰＵ６０から指令信号を与えて、指定走査角度範囲及び指定走査角度単位の距離測定を開始させる。
【００７５】
一方、レーザーレンジセンサ２００は、ＣＰＵ６０からの指令信号に応じて、センシングプロセッサにおいて指定された走査角度範囲及び走査角度単位を設定する。
ここで、レーザーレンジセンサ２００は、測距範囲２０〜４０９５[ｍｍ]、最大走査角度範囲２４０°、角度分解能０．３６°の２次元レンジセンサを備えていることとする。
また、第１走査処理に対して、走査角度範囲−４０°〜＋４０°及び走査角度単位０．３６°が設定され、第２走査処理に対して、走査角度範囲２４０°及び走査角度単位１０°が設定されたとする（この場合は、ｘｙ平面において扇形の測定平面が９つ形成される）。
【００７６】
次に、センシングプロセッサは、第１走査処理に対して設定された走査角度範囲及び走査角度単位（θ）に基づき、ドライバに指令信号を出力する。
２次元レンジセンサは、ドライバにおいて、センシングプロセッサからの指令信号及びエンコーダからの出力信号に基づき、第１モータの回転軸を回転駆動して、２次元レンジセンサを、垂直方向の軸（ｚ軸）まわりに、−４０°〜＋４０°の範囲を０．３６°刻みで走査しながら測距を行う。
引き続き、水平方向の軸（ｙ軸）まわりに、回転角度１０°回動して、再び、−４０°〜＋４０°の範囲を０．３６°刻みで走査しながら測距を行う（第２走査処理）。
上記のようにして、第１走査処理及び第２走査処理を行うことで、上記走査範囲内にある障害物の距離情報を測定する。
【００７７】
また、各測定距離の情報（回転座標系）は、データ列Ｌ（θi，φj）として、センシングプロセッサへと出力される。
また、１つの測定平面に対する第１走査処理が終了すると、センシングプロセッサは、第１走査処理において測定された距離情報に対して、メディアンフィルタを用いたフィルタリング処理を行う。これにより、測定情報におけるノイズ成分を除去する。
【００７８】
次に、センシングプロセッサは、フィルタリング処理後の回転座標系の距離情報を、直交座標系の距離情報に変換し、直交座標系に変換された距離情報をハブ７６及び通信Ｉ／Ｆ６４を介してＣＰＵ６０に入力する。また、センシングプロセッサは、測定した距離情報に基づき障害物の速度を算出し、算出した速度情報を、ハブ７６及び通信Ｉ／Ｆ６４を介してＣＰＵ６０に入力する。入力された距離情報は、距離情報記憶部１１０において、メモリに記憶される。
【００７９】
距離情報は、脚車輪型ロボット１の移動経路上に、図９（ａ）に示すような曲がり角がある場合は、図９（ａ）中の黒点に示すように、曲がり角を構成する壁等の障害物の形状に沿った座標情報を有する。また、脚車輪型ロボット１が、図９（ｂ）に示す直線路を移動する場合は、図９（ｂ）中の黒点に示すように、通路を形成する壁などの障害物の形状に沿った座標情報を有する。但し、図９（ａ）及び（ｂ）に示す例は、２次元座標の測定点を示すものである。
【００８０】
一方、障害物回避支援部１０１は、斥力計算部１１１において、操作部３００を介した利用者からの操作入力があったことを判定すると（ステップＳ１００の「Yes」の分岐）、メモリから距離情報（Ｎ個）を取得する（ステップＳ１０２）。操作部３００は、前進用ボタン、後進用ボタン、旋回ボタンなどのボタン操作によるもの、アナログレバーによる操作（レバーを倒した方向に倒した角度に応じた速度で走行）によるものなどが該当する。
【００８１】
次に、斥力計算部１１１において、２次元モードが設定されている場合は上式（４）に、３次元モードが設定されている場合は上式（７）に基づき、取得した各距離情報（測定点）に対する斥力Ｕ（ｎ）を計算する（ステップＳ１０４）。ここで、ｎは、１〜Ｎ（Ｎは測定点の総数）の自然数とする。
斥力Ｕ（ｎ）が計算されると、斥力成分分離部１１２において、斥力Ｕ（ｎ）を、２次元モードが設定されている場合は上式（５）〜（６）に、３次元モードが設定されている場合は上式（８）〜（１０）に基づき、各軸成分に分離する（ステップＳ１０８）。このステップＳ１０６〜Ｓ１０８の処理は、Ｎ個の測定点について全てが終了するまで繰り返し行われる。
これにより、Ｎ個の測定点について、斥力Ｕ（１）〜Ｕ（Ｎ）として、「η／２・（１／ρ１−１／ρ0）2」〜「η／２・（１／ρＮ−１／ρ0）2」が算出される。
【００８２】
また、斥力Ｕ（１）〜Ｕ（Ｎ）が、２次元モードの場合は、ｘ軸成分「Ｆpx（１）〜Ｆpx（Ｎ）」、及びｙ軸成分「Ｆpy（１）〜Ｆpy（Ｎ）」に分離される。また、３次元モードの場合は、ｘ軸成分「Ｆpx（１）〜Ｆpx（Ｎ）」、ｙ軸成分「Ｆpy（１）〜Ｆpy（Ｎ）」、及びｚ軸成分「Ｆpz（１）〜Ｆpz（Ｎ）」に分離される。
Ｎ個の測定点について、上記計算が終了すると（ステップＳ１１０の「Yes」の分岐）、次に、回避用斥力生成部１１３において、分離された軸成分から軸毎の正方向の最大値及び負方向の最大値を抽出する（ステップＳ１１２〜Ｓ１１４）。
具体的に、２次元モードが設定されている場合は上式（１１）〜（１４）に基づき、「Ｆpx（１）〜Ｆpx（Ｎ）」から、ｘ軸成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnを抽出し、「Ｆpy（１）〜Ｆpy（Ｎ）」から、ｙ軸成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynを抽出する。
【００８３】
また、３次元モードが設定されている場合は上式（１１）〜（１６）に基づき、上記ｘ軸成分の正方向の最大値Ｆpxp及び負方向の最大値Ｆpxnと、ｙ軸成分の正方向の最大値Ｆpyp及び負方向の最大値Ｆpynとに加えて、「Ｆpz（１）〜Ｆpz（Ｎ）」から、ｚ軸成分の正方向の最大値Ｆpzp及び負方向の最大値Ｆpznを抽出する。
回避用斥力生成部１１３は、次に、全ての軸成分の正方向の最大値の中に負の値のものが含まれているか否かを判定し（ステップＳ１１６）、負の値のものが含まれている場合（ステップＳ１１６の「Yes」の分岐）は、負の値となっている最大値を全てゼロ（０）に変更する（ステップＳ１１８）。具体的に、回避用斥力生成部１１３は、２次元モードが設定されている場合は、Ｆpxp及びＦpypのうち負の値となっているものをゼロに変更し、３次元モードが設定されている場合は、Ｆpxp、Ｆpyp及びＦpzpのうち負の値となっているものをゼロに変更する。
【００８４】
つまり、脚車輪型ロボット１が、各軸成分について、障害物から負の斥力しか受けていない場合に、該当する軸の軸成分の正方向の最大値をゼロにする。
引き続き、回避用斥力生成部１１３は、全ての軸成分の負方向の最大値の中に正の値のものが含まれているか否かを判定する（ステップＳ１２０）。そして、正の値のものが含まれている場合（ステップＳ１２０の「Yes」の分岐）は、正の値となっている最大値を全てゼロ（０）に変更する（ステップＳ１２２）。
【００８５】
つまり、脚車輪型ロボット１が、各軸成分について、障害物から正の斥力しか受けていない場合に、該当する軸の軸成分の負方向の最大値をゼロにする。具体的に、回避用斥力生成部１１３は、２次元モードが設定されている場合は、Ｆpxn及びＦpynのうち正の値となっているものをゼロに変更し、３次元モードが設定されている場合は、Ｆpxn、Ｆpyn及びＦpznのうち正の値となっているものをゼロに変更する。
【００８６】
また、全ての軸成分の正方向の最大値がいずれも正の値で、かつ負方向の最大値がいずれも負の値となっている場合（ステップＳ１１６，Ｓ１２０の双方で「No」の分岐）は、正方向及び負方向の最大値はいずれもそのままの値とする。
そして、上記判定及び変更処理後に、回避用斥力生成部１１３は、軸毎に正方向の最大値と負方向の最大値との和を計算する（ステップＳ１２４）。
【００８７】
具体的に、回避用斥力生成部１１３は、２次元モードが設定されている場合は、上式（１）〜（２）に基づき、「Ｆpx＝Ｆpxp＋Ｆpxn」と「Ｆpy＝Ｆpyp＋Ｆpyn」とを計算する。
また、回避用斥力生成部１１３は、３次元モードが設定されている場合は、上式（１）〜（３）に基づき、「Ｆpx＝Ｆpxp＋Ｆpxn」と「Ｆpy＝Ｆpyp＋Ｆpyn」と、「Ｆpz＝Ｆpzp＋Ｆpzn」とを計算する。
【００８８】
そして、回避用斥力生成部１１３は、２次元モードが設定されている場合は、上記計算結果であるｘ軸成分「Ｆpx」と、ｙ軸成分「Ｆpy」とを軸成分として、上式（１７）で示される回避用斥力Ｆpを生成する（ステップＳ１２６）。
また、回避用斥力生成部１１３は、３次元モードが設定されている場合は、上記計算結果であるｘ軸成分「Ｆpx」と、ｙ軸成分「Ｆpy」と、ｚ軸成分「Ｆpz」を軸成分として、上式（１８）で示される回避用斥力Ｆpを生成する（ステップＳ１２６）。
更に、回避用斥力生成部１１３は、生成した回避用斥力Ｆpを、モータ制御部１０２に出力する（ステップＳ１２８）。
【００８９】
一方、モータ制御部１０２は、利用者の操作入力を検出し且つ障害物回避支援部１０１から回避用斥力Ｆpを取得すると（ステップＳ２００及びＳ２０２の「Yes」の分岐）、レーザーレンジセンサ２００から、通信Ｉ／Ｆ６４を介して入力される障害物の位置情報を取得する（ステップＳ２０４）。なお、ここでは、障害物は曲がり角（固定物）であるため、障害物の速度は「０」となる。
【００９０】
次に、角度取込Ｉ／Ｆ６２を介して入力される各モータの回転角度情報を取得し（ステップＳ２０６）、操作入力、回避用斥力Ｆp、位置情報及び回転角度情報に基づき、障害物を回避しつつ、Ａ点からＢ点に移動するためのモータ指令を生成する（ステップＳ２０８）。そして、生成したモータ指令に基づきモータ指令信号をモータ指令出力Ｉ／Ｆ６１を介して、各モータに出力する（ステップＳ２１０）。
これにより、各モータが制御されて脚車輪型ロボット１は、モータ指令信号に応じた方向に、モータ指令信号に応じた速度で車輪走行する。
障害物回避支援部１０１及びモータ制御部１０２の上記一連の処理は、操作入力がある間、繰り返し行われる。
【００９１】
このようにして、脚車輪型ロボット１が、図９（ａ）に示すように、曲がり角に対して、Ａ点からＢ点に移動したとする。この場合に、回避用斥力Ｆpは、Ａ点からＢ点への位置変化に対して、２次元モードが設定されている場合に、図１０（ａ）に示すように、最近接距離の切り替えが生じていても、ｘ軸成分Ｆpx及びｙ軸成分Ｆpyが共に連続的な変化を示すものとなる。
【００９２】
一方、図９（ａ）に示すＡ点からＢ点への移動を行った場合に、従来技術のように最近接距離を用いて計算した回避用斥力は、図１１（ａ）に示すように、Ａ点からＢ点への中間位置において、最近接距離の切り替えが発生しており、中間位置でＦpx及びＦpyの急激な変化が生じている。
また、図９（ｂ）に示すように、ｘ軸の正方向側及び負方向側に障害物が存在し、ｙ軸の正方向側及び負方向側の測定範囲内に障害物が存在しない直線路を、図９（ｂ）中のＡ点からＢ点へと移動した場合も、上記曲がり角のときと同様に回避用斥力Ｆpを計算することができる。
【００９３】
但し、ｙ軸方向に障害物が存在しないため、回避用斥力Ｆpにおけるｙ軸成分Ｆpyがゼロとなる。従って、回避用斥力Ｆpの変化は、ｘ軸成分Ｆpxの変化となる。この場合も、図１０（ｂ）に示すように、最近接距離の切り替えが生じていても、ｘ軸成分Ｆpxは、連続的な変化を示すものとなる。また、このときのｘ軸成分の最大値Ｆpxpの変化は、図中の破線に示すように連続的な変化となり、ｘ軸成分の負方向の最大値Ｆpxnの変化も、図中の点線に示すように連続的な変化となっている。
【００９４】
一方、図９（ｂ）に示すＡ点からＢ点への移動を行った場合に、従来技術のように最近接距離を用いて計算した回避用斥力Ｆpは、図１１（ｂ）に示すように、Ａ点からＢ点への中間位置において、最近接距離の切り替えが発生しており、中間位置でＦp（Ｆpx）の急激な変化が生じている。
このようにして、本実施の形態の脚車輪型ロボット１は、レーザーレンジセンサ２００において、障害物の２次元又は３次元の距離情報を測定し、測定した距離情報に基づき、障害物回避支援部１０１において、障害物の各測定点から仮想的に受ける斥力を計算することができる。
【００９５】
更に、障害物回避支援部１０１において、計算した斥力を軸成分に分離し、分離した軸成分から軸毎に正方向の最大値及び負方向の最大値を抽出し、抽出した正方向と負方向の最大値の和を軸毎に計算し、この計算結果から回避用斥力Ｆpを生成することができる。
更に、モータ制御部１０２において、操作部３００からの操作入力と、回避用斥力Ｆpと、レーザーレンジセンサ２００から入力される障害物の速度情報と、各モータのエンコーダから入力される回転角度情報とに基づきモータ指令信号を生成することができる。
【００９６】
これにより、移動体の移動位置に応じて最近接距離の切り替わりが生じても、回避用斥力は連続して変化するので、速度の急激な変化等が生じない安定した移動制御を行うことができる。
また、斥力の軸成分の正方向の最大値と負方向の最大値との和を用いて回避用斥力Ｆpを生成するようにしたので、障害物の大小や測定点群の密度などに影響を受けない回避用斥力Ｆpを生成することができる。
【００９７】
〔第２の実施の形態〕
次に、本発明の第２の実施の形態を図面に基づき説明する。図１２は、本発明に係る障害物回避支援装置、障害物回避支援方法及び移動体の第２の実施の形態を示す図である。
本実施の形態は、本発明に係る障害物回避支援装置を、移動体である、車輪による走行移動が可能な案内用ロボットに適用したものである。
【００９８】
図１２は、本実施の形態の案内用ロボットの側面図であり、案内用ロボット８１の構成を示す図である。
図１２に示すように、本実施の形態の案内用ロボット８１は、基体８２と、左右一対の移動用車輪８４Ｒ及び８４Ｌと、２つの距離情報測定部８６Ｆ及び８６Ｂと、把持部８８と、入力値検出手段９０と、移動モード切り換え操作部９２と、スピーカ９４と、２つのキャスタ装置９６Ｆ及び９６Ｂとを備えている。
基体８２は、上面視で八角形をなす中空体により形成されている。なお、基体８２の形状は、これに限定するものではなく、例えば、上面視で円形をなす形状に形成してもよい。
【００９９】
移動用車輪８４Ｒ及び８４Ｌは、後述する移動用アクチュエータ１６２が発生する駆動力により回転する車輪である。そして、移動用車輪８４Ｒ及び８４Ｌは、移動用アクチュエータ１６２が発生する駆動力を伝達可能なプーリ等を介して、移動用アクチュエータ１６２に接続されている。また、一対の車輪は、それぞれ、基体８２の下面において、基体８２の中心を基準とした両側面側へ回転可能に配置されている。なお、図１２中では、移動用車輪８４を形成する一対の車輪のうち、左側移動用車輪８４Ｌのみを示している。
【０１００】
距離情報測定部８６Ｆ及び８６Ｂは、上記第１の実施の形態におけるレーザーレンジセンサ２００と同様の構成を有したセンサから形成されたものである。距離情報測定部８６Ｆは基体８２の前面側に配置され、距離情報測定部８６Ｂは基体８２の後面側に配置されている。なお、距離情報測定部８６Ｆ及び８６Ｂは、レーザレンジセンサに限定されるものではなく、例えば、赤外線センサや超音波センサ等を用いて形成してもよい。
【０１０１】
具体的に、距離情報測定部８６Ｆ及び８６Ｂは、上記第１の実施の形態のレーザーレンジセンサ２００と同様に第１走査処理及び第２走査処理を行って、案内用ロボット８１の前面側及び後面側の３次元の測定範囲内にある障害物の距離情報を測定する。なお、距離情報測定部８６Ｆ及び８６Ｂは、第１の走査処理のみを行い、２次元の測定範囲内にある障害物の距離情報を測定する構成としてもよい。
【０１０２】
把持部８は、フレーム部（不図示）と、グリップ部（不図示）とを備え、フレーム部は、入力値検出部（不図示）を介して、基体８２の上面に取り付けられている。
グリップ部は、被案内者（利用者）が片手で把持可能な略円柱状に形成されている。
入力値検出部は、互いに直交する三軸（ｘ，ｙ，ｚ）の方向に付与される力と、これら三軸の軸回りのモーメントをそれぞれ検出可能な六軸力センサであり、フレーム部と基体８２の上面との間に介装されている。
【０１０３】
これにより、入力値検出部は、グリップ部を把持する被案内者により、グリップ部を介した把持部８８への入力を、互いに直交する三軸の方向に付与される力と、これら三軸の軸回りのモーメントとして、それぞれ検出可能に形成されている。
キャスタ装置９６は、キャスタ（不図示）と、伸縮用アクチュエータ（不図示）と、床反力検出部（不図示）とを備えている。
【０１０４】
キャスタは、平衡用車輪と、平衡用車輪支持部と、キャスタ支持軸とを有している。
伸縮用アクチュエータは、キャスタを上下動させてキャスタ装置９６を伸縮させるアクチュエータである。また、伸縮用アクチュエータは、直線運動する直動軸を有しており、キャスタフレームにより支持されている。したがって、キャスタは、基体８２と平衡用車輪との間に介装し、且つ平衡用車輪の回転軸と直交する方向へ伸縮する伸縮部を形成している。
【０１０５】
また、案内用ロボット８１は、上記第１の実施の形態における、図４に示す移動システムと同様の移動システムを備えている。具体的に、図４に示す移動システムから、脚部１２に関連する関節モータ４０、エンコーダ４２、ドライバ４４及び脚先センサ３８等の不要な構成要素を取り除き、レーザーレンジセンサ２００を距離情報測定部８６Ｆ及び８６Ｂに、操作部３００を把持部８８に、スピーカ７８をスピーカ９４にそれぞれ置き換えた構成となる。更に、車輪２０（×４）を駆動する各車輪モータ５０、エンコーダ５２及びドライバ５４を、移動用車輪９６Ｆ及び９６Ｂを駆動するアクチュエータ１６２、エンコーダ及びドライバに置き換えた構成となる。更に、キャスタ装置９６を駆動する伸縮用アクチュエータ、ドライバ、床反力検出部、これらの入出力Ｉ／Ｆ等の案内用ロボット８１に特有の要素を加えた構成となる。
【０１０６】
また、案内用ロボット８１は、上記第１の実施の形態の脚車輪型ロボット１と同様に、車輪走行時における障害物回避時の移動制御に係る機能構成部である、障害物回避支援部１０１と、モータ制御部１０２とを含む移動制御部１００を備えている。
本実施の形態において、障害物回避支援部１０１は、距離情報測定部８６Ｆ及び８６Ｂからの障害物の距離情報に基づき、上記第１の実施の形態と同様に、上式（１）〜（１８）に基づき、回避用斥力Ｆpを計算する。
【０１０７】
また、モータ制御部１０２は、把持部８８からの六軸力センサを介した力入力と、距離情報測定部８６Ｆ又は８６Ｂからの障害物の位置情報と、障害物回避支援部１０１からの回避用斥力Ｆpと、各エンコーダからの回転角度情報とを取得する。そして、これら取得した情報に基づき、障害物の回避動作を含む走行制御のためのアクチュエータ１６２のドライバへの指令信号を生成する。
【０１０８】
このようにして、本実施の形態の案内用ロボット８１は、距離情報測定部８６Ｆ及び８６Ｂにおいて、障害物の２次元又は３次元の距離情報を測定することができる。そして、測定した距離情報に基づき、障害物回避支援部１０１において、案内用ロボット８１の移動経路上（前面側又は後面側の測定範囲内）に存在する障害物の各測定点から仮想的に受ける斥力を計算することができる。
【０１０９】
更に、障害物回避支援部１０１において、計算した斥力を軸成分に分離し、分離した軸成分から軸毎に正方向の最大値及び負方向の最大値を抽出し、抽出した正方向及び負方向の最大値の和を軸毎に計算し、この計算結果から回避用斥力Ｆpを生成することができる。
更に、モータ制御部１０２において、把持部８８からの力入力と、回避用斥力Ｆpと、距離情報測定部８６Ｆ及び８６Ｂから入力される障害物の位置情報と、各モータのエンコーダから入力される回転角度情報とに基づきモータ指令信号を生成することができる。
【０１１０】
これにより、案内用ロボット８１の移動位置に応じて最近接距離の切り替わりが生じても、回避用斥力は連続して変化するので、速度の急激な変化等が生じない安定した移動制御を行うことができる。
また、斥力の軸成分の正方向の最大値と負方向の最大値との和を用いて回避用斥力Ｆpを生成するようにしたので、障害物の大小や測定点群の密度などに影響を受けない回避用斥力Ｆpを生成することができる。
【０１１１】
上記各実施の形態において、距離情報記憶部１１０及びメモリは、距離情報記憶部又は距離情報記憶手段に対応し、斥力計算部１１１は、斥力計算部に対応し、斥力成分分離部１１２は、斥力成分分離部に対応する。
また、上記第１の実施の形態において、回避用斥力生成部１１３は、回避用斥力生成部に対応し、モータ制御部１０２、ドライバ４４，５４は、アクチュエータ制御部に対応する。
【０１１２】
また、上記第２の実施の形態において、回避用斥力生成部１１３は、回避用斥力生成部に対応し、モータ制御部１０２、ドライバ５４は、アクチュエータ制御部に対応する。
また、上記第１の実施の形態において、ステップＳ１０６は、斥力計算ステップに対応し、ステップＳ１０８は、斥力成分分離ステップに対応し、ステップＳ１１０〜Ｓ１２６は、回避用斥力生成ステップに対応する。
【０１１３】
また、上記各実施の形態において、脚車輪型ロボット１又は案内用ロボット８１のＣＰＵ６０、メモリ及び障害物回避支援部１０１によって構成される構成部は、障害物回避支援装置に対応し、脚車輪型ロボット１及び案内用ロボット８１は、移動体に対応する。
なお、上記各実施の形態において、障害物回避支援装置を、脚車輪型ロボット１及び案内用ロボット８１のＣＰＵ６０やメモリ等のハードウェア資源を共用して、ＣＰＵ６０でメモリに記憶された専用のプログラムを実行することで障害物回避支援部１０１の各機能を実現する構成としたが、この構成に限らない。
【０１１４】
例えば、別途プロセッサ及びメモリを有し、該プロセッサによって専用のプログラムを実行して障害物回避支援部１０１の各機能を実現する独立した装置として構成してもよい。
また、専用のプログラムを実行するソフトウェアによる処理ではなく、障害物回避支援部１０１の各処理の一部又は全部を電気回路等のハードウェアを用いて行う構成としてもよい。
【０１１５】
また、上記各実施の形態において、斥力の計算に、上式（４）及び（７）に示す、Khatibの設計したポテンシャル関数を用いる構成を説明したが、この構成に限らず、斥力を計算可能な他の公知のポテンシャル関数を用いる構成としてもよい。
また、上記第１の実施の形態において、本発明に係る障害物回避支援装置を、脚車輪型ロボットに適用し、上記第２の実施の形態において、本発明に係る障害物回避支援装置を、案内用ロボットに適用する例を説明したが、これに限らず、他の移動手段を用いて移動を行うロボットや、船舶、自動車などの他の移動体に適用することも可能である。
【０１１６】
また、上記各実施の形態において、本発明に係る障害物回避支援装置を、利用者の操作に応じて移動動作を行うロボットに適用したが、この構成に限らず、自律移動型のロボット等の自律移動を行うことが可能な移動体に適用することも可能である。
また、上記第１の実施の形態においては、レーザーレンジセンサ２００によって障害物までの距離情報を測定する構成としたが、この構成に限らず、距離画像センサ等の他の測距センサを用いる構成としてもよい。
【０１１７】
また、上記各実施の形態では、レーザーレンジセンサ２００において測距センサ自身を回転させて測定範囲の走査を行っているが、これに限らず、測距センサの光軸上に挿入したミラーを回転させて走査を行う構成としてもよい。
また、上記各実施の形態は、本発明の好適な具体例であり、技術的に好ましい種々の限定が付されているが、本発明の範囲は、上記の説明において特に本発明を限定する旨の記載がない限り、これらの形態に限られるものではない。また、上記の説明で用いる図面は、図示の便宜上、部材ないし部分の縦横の縮尺は実際のものとは異なる模式図である。
また、本発明は上記各実施の形態に限定されるものではなく、本発明の目的を達成できる範囲での変形、改良等は本発明に含まれるものである。
【符号の説明】
【０１１８】
１ 脚車輪型ロボット
１００ 移動制御部
１０１ 障害物回避支援部
１０２ モータ制御部
１１０ 距離情報記憶部
１１１ 斥力計算部
１１２ 斥力成分分離部
１１３ 回避用斥力生成部
２００ レーザーレンジセンサ
３００ 操作部
４０、５０ モータ
４２、５２ エンコーダ
１２ｂ、１４、４４、５４ ドライバ
６０ ＣＰＵ
６１ モータ指令出力Ｉ／Ｆ
６２ 角度取込Ｉ／Ｆ
６４ 通信Ｉ／Ｆ
７４ 無線通信部
７６ ハブ
８１ 案内用ロボット
８６Ｆ及び８６Ｂ 距離情報測定部
【特許請求の範囲】
【請求項１】
移動体の移動経路上に存在する障害物の回避動作を支援する障害物回避支援装置であって、
前記移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報を記憶する距離情報記憶部と、
前記距離情報記憶部に記憶された距離情報に基づき、ポテンシャル関数を用いて前記障害物の前記複数の測定点の各測定点から仮想的に受ける斥力を計算する斥力計算部と、
前記斥力計算部で計算した各測定点に対応する斥力を前記測定点の座標を規定する各軸の軸成分に分離する斥力成分分離部と、
前記各測定点の前記分離した軸成分に基づき、前記障害物の回避動作に係る斥力である回避用斥力を生成する回避用斥力生成部と、を備え、
前記回避用斥力生成部は、前記各測定点に対応する斥力の軸成分における各軸の正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算し、該計算結果に基づき前記回避用斥力を生成することを特徴とする障害物回避支援装置。
【請求項２】
前記測定点の座標が、互いに直交するｘ軸及びｙ軸により規定される２次元座標（ｘ，ｙ）で表され、ｎ番目（ｎは、測定点の総数をＮとした場合に１≦ｎ≦Ｎの自然数）の測定点に対応する斥力におけるｘ軸方向の成分がＦpx（ｎ）、ｙ軸方向の成分がＦpy（ｎ）で表される場合に、前記回避用斥力生成部は、前記回避用斥力におけるｘ軸方向の成分Ｆpx及びｙ軸方向の成分Ｆpyを、下式（１）〜（２）に基づき計算し、
Ｆpx＝Ｆpxp＋Ｆpxn ・・・（１）
Ｆpy＝Ｆpyp＋Ｆpyn ・・・（２）
前記式（１）〜（２）において、Ｆpxpは、Ｎ個の測定点に対応する斥力におけるｘ軸の正方向の成分の最大値であり、Ｆpxnは、Ｎ個の測定点に対応する斥力におけるｘ軸の負方向の成分の最大値であり、Ｆpypは、Ｎ個の測定点に対応する斥力におけるｙ軸の正方向の成分の最大値であり、Ｆpynは、Ｎ個の測定点に対応する斥力におけるｙ軸の負方向の成分の最大値であり、前記式（１）〜（２）において、前記Ｆpxpが負の値の場合は当該Ｆpxpの値をゼロとし、前記Ｆpypが負の値の場合は当該Ｆpypの値をゼロとし、前記Ｆpxnが正の値の場合は当該Ｆpxnの値をゼロとし、前記Ｆpynが正の値の場合は当該Ｆpynの値をゼロとすることを特徴とする請求項１に記載の障害物回避支援装置。
【請求項３】
前記測定点の座標が、前記ｘ軸及び前記ｙ軸の双方に直交するｚ軸を更に含む３次元座標（ｘ，ｙ，ｚ）で表され、ｎ番目の測定点に対応する斥力におけるｚ軸方向の成分がＦpz（ｎ）で表される場合に、前記回避用斥力生成部は、前記回避用斥力におけるｚ軸方向の成分Ｆpzを、下式（３）に基づき計算し、
Ｆpz＝Ｆpzp＋Ｆpzn ・・・（３）
前記式（３）において、Ｆpzpは、Ｎ個の測定点に対応する斥力におけるｚ軸の正方向の成分の最大値であり、Ｆpznは、Ｎ個の測定点に対応する斥力におけるｚ軸の負方向の成分の最大値であり、前記Ｆpzpが負の値の場合は当該Ｆpzpの値をゼロとし、前記Ｆpznが正の値の場合は当該Ｆpznの値をゼロとすることを特徴とする請求項２に記載の障害物回避支援装置。
【請求項４】
前記測定点の座標が、互いに直交するｘ軸及びｙ軸により規定される２次元座標（ｘ，ｙ）で表される場合に、前記斥力計算部は、ｎ番目（ｎは、測定点の総数をＮとした場合に１〜Ｎの自然数）の測定点に対応する斥力Ｕ（ｎ）を下式（４）に示すポテンシャル関数に基づき計算し、前記斥力成分分離部は、前記斥力Ｕ（ｎ）を、下式（５）〜（６）に基づき、ｘ軸方向の成分Ｆpx（ｎ）及びｙ軸方向の成分Ｆpy（ｎ）に分離することを特徴とする請求項１又は請求項２に記載の障害物回避支援装置。
Ｕ（ｎ）＝η／２・（１／ρn−１／ρ0）2 ・・・（４）
Ｆpx（ｎ）＝∂Ｕ（ｎ）／∂ｘ ・・・（５）
Ｆpy（ｎ）＝∂Ｕ（ｎ）／∂ｙ ・・・（６）
但し、前記式（４）において、ηは、正の重み定数であり、ρ0は、正の定数であって前記ポテンシャル関数が影響を与える範囲であり、ρnは、前記移動体の現在位置からｎ番目の測定点までの距離である。
【請求項５】
前記測定点の座標が、各々が互いに直交するｘ軸、ｙ軸及びｚ軸により規定される３次元座標（ｘ，ｙ，ｚ）で表される場合に、前記斥力計算部は、ｎ番目（ｎは、測定点の総数をＮとした場合に１〜Ｎの自然数）の測定点に対応する斥力Ｕ（ｎ）を下式（７）に示すポテンシャル関数に基づき計算し、前記斥力成分分離部は、前記斥力Ｕ（ｎ）を、下式（８）〜（１０）に基づき、ｘ軸方向の成分Ｆpx（ｎ）、ｙ軸方向の成分Ｆpy（ｎ）及びｚ軸方向の成分Ｆpz（ｎ）に分離することを特徴とする請求項１又は請求項３に記載の障害物回避支援装置。
Ｕ（ｎ）＝η／２・（１／ρn−１／ρ0）2 ・・・（７）
Ｆpx（ｎ）＝∂Ｕ（ｎ）／∂ｘ ・・・（８）
Ｆpy（ｎ）＝∂Ｕ（ｎ）／∂ｙ ・・・（９）
Ｆpz（ｎ）＝∂Ｕ（ｎ）／∂ｚ ・・・（１０）
但し、上式（７）において、ηは、正の重み定数であり、ρ0は、正の定数であって前記ポテンシャル関数が影響を与える範囲であり、ρnは、前記移動体の現在位置からｎ番目の測定点までの距離である。
【請求項６】
移動体の移動経路上に存在する障害物の回避動作を支援する障害物回避支援方法であって、
前記移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報の記憶された距離情報記憶手段の前記距離情報に基づき、ポテンシャル関数を用いて前記障害物の前記複数の測定点の各測定点から仮想的に受ける斥力を計算する斥力計算ステップと、
前記斥力計算ステップで計算した各測定点に対応する斥力を前記測定点の座標を規定する各軸の軸成分に分離する斥力成分分離ステップと、
前記各測定点の前記分離した軸成分に基づき、前記障害物の回避動作に係る斥力である回避用斥力を生成する回避用斥力生成ステップと、を含み、
前記回避用斥力生成ステップにおいては、前記各測定点に対応する斥力の軸成分における各軸の正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算し、該計算結果に基づき前記回避用斥力を生成することを特徴とする障害物回避支援方法。
【請求項７】
基体と、
前記基体を移動させるアクチュエータを有する移動機構と、
移動経路上に存在する障害物の形状に沿った複数の測定点に対する距離情報を測定する距離情報測定部と、
操作部を介した利用者からの当該移動体の移動方向の情報を含む移動に係る指示入力を受け付ける指示入力受付部と、
請求項１乃至請求項５のいずれか１項に記載の障害物回避支援装置と、
前記障害物回避支援装置において前記距離情報測定部で測定した距離情報に基づき計算した回避用斥力と前記指示入力受付部で受け付けた指示入力とに基づき、前記アクチュエータを制御するアクチュエータ制御部と、を備えることを特徴とする移動体。
【請求項１】
移動体の移動経路上に存在する障害物の回避動作を支援する障害物回避支援装置であって、
前記移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報を記憶する距離情報記憶部と、
前記距離情報記憶部に記憶された距離情報に基づき、ポテンシャル関数を用いて前記障害物の前記複数の測定点の各測定点から仮想的に受ける斥力を計算する斥力計算部と、
前記斥力計算部で計算した各測定点に対応する斥力を前記測定点の座標を規定する各軸の軸成分に分離する斥力成分分離部と、
前記各測定点の前記分離した軸成分に基づき、前記障害物の回避動作に係る斥力である回避用斥力を生成する回避用斥力生成部と、を備え、
前記回避用斥力生成部は、前記各測定点に対応する斥力の軸成分における各軸の正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算し、該計算結果に基づき前記回避用斥力を生成することを特徴とする障害物回避支援装置。
【請求項２】
前記測定点の座標が、互いに直交するｘ軸及びｙ軸により規定される２次元座標（ｘ，ｙ）で表され、ｎ番目（ｎは、測定点の総数をＮとした場合に１≦ｎ≦Ｎの自然数）の測定点に対応する斥力におけるｘ軸方向の成分がＦpx（ｎ）、ｙ軸方向の成分がＦpy（ｎ）で表される場合に、前記回避用斥力生成部は、前記回避用斥力におけるｘ軸方向の成分Ｆpx及びｙ軸方向の成分Ｆpyを、下式（１）〜（２）に基づき計算し、
Ｆpx＝Ｆpxp＋Ｆpxn ・・・（１）
Ｆpy＝Ｆpyp＋Ｆpyn ・・・（２）
前記式（１）〜（２）において、Ｆpxpは、Ｎ個の測定点に対応する斥力におけるｘ軸の正方向の成分の最大値であり、Ｆpxnは、Ｎ個の測定点に対応する斥力におけるｘ軸の負方向の成分の最大値であり、Ｆpypは、Ｎ個の測定点に対応する斥力におけるｙ軸の正方向の成分の最大値であり、Ｆpynは、Ｎ個の測定点に対応する斥力におけるｙ軸の負方向の成分の最大値であり、前記式（１）〜（２）において、前記Ｆpxpが負の値の場合は当該Ｆpxpの値をゼロとし、前記Ｆpypが負の値の場合は当該Ｆpypの値をゼロとし、前記Ｆpxnが正の値の場合は当該Ｆpxnの値をゼロとし、前記Ｆpynが正の値の場合は当該Ｆpynの値をゼロとすることを特徴とする請求項１に記載の障害物回避支援装置。
【請求項３】
前記測定点の座標が、前記ｘ軸及び前記ｙ軸の双方に直交するｚ軸を更に含む３次元座標（ｘ，ｙ，ｚ）で表され、ｎ番目の測定点に対応する斥力におけるｚ軸方向の成分がＦpz（ｎ）で表される場合に、前記回避用斥力生成部は、前記回避用斥力におけるｚ軸方向の成分Ｆpzを、下式（３）に基づき計算し、
Ｆpz＝Ｆpzp＋Ｆpzn ・・・（３）
前記式（３）において、Ｆpzpは、Ｎ個の測定点に対応する斥力におけるｚ軸の正方向の成分の最大値であり、Ｆpznは、Ｎ個の測定点に対応する斥力におけるｚ軸の負方向の成分の最大値であり、前記Ｆpzpが負の値の場合は当該Ｆpzpの値をゼロとし、前記Ｆpznが正の値の場合は当該Ｆpznの値をゼロとすることを特徴とする請求項２に記載の障害物回避支援装置。
【請求項４】
前記測定点の座標が、互いに直交するｘ軸及びｙ軸により規定される２次元座標（ｘ，ｙ）で表される場合に、前記斥力計算部は、ｎ番目（ｎは、測定点の総数をＮとした場合に１〜Ｎの自然数）の測定点に対応する斥力Ｕ（ｎ）を下式（４）に示すポテンシャル関数に基づき計算し、前記斥力成分分離部は、前記斥力Ｕ（ｎ）を、下式（５）〜（６）に基づき、ｘ軸方向の成分Ｆpx（ｎ）及びｙ軸方向の成分Ｆpy（ｎ）に分離することを特徴とする請求項１又は請求項２に記載の障害物回避支援装置。
Ｕ（ｎ）＝η／２・（１／ρn−１／ρ0）2 ・・・（４）
Ｆpx（ｎ）＝∂Ｕ（ｎ）／∂ｘ ・・・（５）
Ｆpy（ｎ）＝∂Ｕ（ｎ）／∂ｙ ・・・（６）
但し、前記式（４）において、ηは、正の重み定数であり、ρ0は、正の定数であって前記ポテンシャル関数が影響を与える範囲であり、ρnは、前記移動体の現在位置からｎ番目の測定点までの距離である。
【請求項５】
前記測定点の座標が、各々が互いに直交するｘ軸、ｙ軸及びｚ軸により規定される３次元座標（ｘ，ｙ，ｚ）で表される場合に、前記斥力計算部は、ｎ番目（ｎは、測定点の総数をＮとした場合に１〜Ｎの自然数）の測定点に対応する斥力Ｕ（ｎ）を下式（７）に示すポテンシャル関数に基づき計算し、前記斥力成分分離部は、前記斥力Ｕ（ｎ）を、下式（８）〜（１０）に基づき、ｘ軸方向の成分Ｆpx（ｎ）、ｙ軸方向の成分Ｆpy（ｎ）及びｚ軸方向の成分Ｆpz（ｎ）に分離することを特徴とする請求項１又は請求項３に記載の障害物回避支援装置。
Ｕ（ｎ）＝η／２・（１／ρn−１／ρ0）2 ・・・（７）
Ｆpx（ｎ）＝∂Ｕ（ｎ）／∂ｘ ・・・（８）
Ｆpy（ｎ）＝∂Ｕ（ｎ）／∂ｙ ・・・（９）
Ｆpz（ｎ）＝∂Ｕ（ｎ）／∂ｚ ・・・（１０）
但し、上式（７）において、ηは、正の重み定数であり、ρ0は、正の定数であって前記ポテンシャル関数が影響を与える範囲であり、ρnは、前記移動体の現在位置からｎ番目の測定点までの距離である。
【請求項６】
移動体の移動経路上に存在する障害物の回避動作を支援する障害物回避支援方法であって、
前記移動体の移動経路上に存在する障害物の形状に沿った複数の測定点に対して測定された距離情報の記憶された距離情報記憶手段の前記距離情報に基づき、ポテンシャル関数を用いて前記障害物の前記複数の測定点の各測定点から仮想的に受ける斥力を計算する斥力計算ステップと、
前記斥力計算ステップで計算した各測定点に対応する斥力を前記測定点の座標を規定する各軸の軸成分に分離する斥力成分分離ステップと、
前記各測定点の前記分離した軸成分に基づき、前記障害物の回避動作に係る斥力である回避用斥力を生成する回避用斥力生成ステップと、を含み、
前記回避用斥力生成ステップにおいては、前記各測定点に対応する斥力の軸成分における各軸の正方向の成分の最大値と負方向の成分の最大値との和を軸毎に計算し、該計算結果に基づき前記回避用斥力を生成することを特徴とする障害物回避支援方法。
【請求項７】
基体と、
前記基体を移動させるアクチュエータを有する移動機構と、
移動経路上に存在する障害物の形状に沿った複数の測定点に対する距離情報を測定する距離情報測定部と、
操作部を介した利用者からの当該移動体の移動方向の情報を含む移動に係る指示入力を受け付ける指示入力受付部と、
請求項１乃至請求項５のいずれか１項に記載の障害物回避支援装置と、
前記障害物回避支援装置において前記距離情報測定部で測定した距離情報に基づき計算した回避用斥力と前記指示入力受付部で受け付けた指示入力とに基づき、前記アクチュエータを制御するアクチュエータ制御部と、を備えることを特徴とする移動体。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【公開番号】特開２０１１−２５３３６１（Ｐ２０１１−２５３３６１Ａ）
【公開日】平成２３年１２月１５日（２０１１．１２．１５）
【国際特許分類】
【出願番号】特願２０１０−１２６９１５（Ｐ２０１０−１２６９１５）
【出願日】平成２２年６月２日（２０１０．６．２）
【出願人】（０００００４２０４）日本精工株式会社 (8,378)
【Ｆターム（参考）】
【公開日】平成２３年１２月１５日（２０１１．１２．１５）
【国際特許分類】
【出願日】平成２２年６月２日（２０１０．６．２）
【出願人】（０００００４２０４）日本精工株式会社 (8,378)
【Ｆターム（参考）】
[ Back to top ]