自律式移動体
【課題】左右の障害物の間を伸びている移動可能経路に沿って移動するための並進速度と角速度を自律的に決定して移動する移動体を提供する。
【解決手段】障害物に当接するまでの移動距離が長いほど高くなる距離評価値と、並進速度に依存して変化する速度評価値を利用し、総合評価値が最大となる並進速度と角速度を採用する装置を備えている。速度評価値計算装置は、狭いコーナに接近した時に、並進速度がコーナ通過時最適速度以下の範囲内にあれば高速であるほど高い速度評価値を計算するとともに、並進速度がコーナ通過時最適速度以上であれば高速であるほど低い速度評価値を計算する。その他の場合には、速度評価値計算装置が、並進速度が高速であるほど高い速度評価値を計算する。
【解決手段】障害物に当接するまでの移動距離が長いほど高くなる距離評価値と、並進速度に依存して変化する速度評価値を利用し、総合評価値が最大となる並進速度と角速度を採用する装置を備えている。速度評価値計算装置は、狭いコーナに接近した時に、並進速度がコーナ通過時最適速度以下の範囲内にあれば高速であるほど高い速度評価値を計算するとともに、並進速度がコーナ通過時最適速度以上であれば高速であるほど低い速度評価値を計算する。その他の場合には、速度評価値計算装置が、並進速度が高速であるほど高い速度評価値を計算する。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、自律式移動体に関する。すなわち、周囲に存在している障害物までの距離と方位を検出する障害物検出装置を搭載しており、左右の障害物の間を伸びている移動可能経路を探索し、移動可能経路に沿って移動するための並進速度と角速度を自律的に決定して移動する移動体に関する。
【背景技術】
【０００２】
到達目標点が指示され、移動開始点と到達目標点の間に障害物が存在する場合、移動開始点から障害物を避けながら到達目標点に至る軌道を計算する必要がある。その軌道を計算するために、ＤＷＡ（Dynamic Window Approach）という手法が開発されており、非特許文献１に記載されている。
ＤＷＡ手法では、移動体の並進速度ｖと角速度ｗを、所定時間毎に繰り返し決定する。ＤＷＡ手法では、所定時間毎に並進速度ｖと角速度ｗを仮定し、仮定した並進速度ｖと角速度ｗから次の３つの評価値を計算する。
偏角評価値：現在位置から到達目標点に向かう方向と、仮定した並進速度と仮定した角速度で決定される移動体の進行方向とがなす角度（偏角）θを評価した値。偏角θがゼロに近いほど偏角評価値が高く、偏角θがπに近いほど偏角評価値が低い。
距離評価値：仮定した並進速度ｖと角速度ｗによる場合に、現在位置から障害物に当接するまでに移動できる距離を評価した値。移動可能な距離が長いほど距離評価値が高い。
速度評価値：仮定した並進速度ｖを評価した値。並進速度ｖが高速なほど速度評価値が高い。
ＤＷＡ手法では、偏角評価値と距離評価値と速度評価値を合計した総合評価値が最も高くなる並進速度ｖと角速度ｗを採用する。ＤＷＡ手法では、所定時間毎に、総合評価値が最も高くなる並進速度ｖと角速度ｗに更新していく。ＤＷＡ手法によれば、所定時間毎に変化する並進速度ｖと角速度ｗの指令値を設定することができる。換言すれば、所定時間毎に目標点の位置を決定することができる。ＤＷＡ手法によると、障害物を避けながら、到達目標点に迅速に到達する軌道を探索することができる。
特許文献１に、所定時間毎に設定された目標点をたどって移動する技術が記載されている。所定時間毎の目標点を設定する手法は記載されていないが、ＤＷＡ手法を用いれば、所定時間毎の目標点を設定する過程を自動化することができる。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to collision avoidance” IEEE Robotics & Automation Magazine, Vol. 4, No. 1, pp. 23-33, March, 1997
【特許文献】
【０００４】
【特許文献１】特開２０００−３３０６３２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ＤＷＡ手法は、到達目標点の位置と障害物の位置の情報を必要とする。すなわち、地図情報を必要とする。地図情報が取得されていれば、障害物を避けながら、到達目標点に迅速に到達する軌道を探索することができる。
しかしながら、当面は到達目標点の情報が重要でなく、屈曲する移動経路に沿って移動し続ける軌道を探索したい場合もある。
【０００６】
当面は到達目標点に到達することが重要でなく、屈曲する移動経路に沿って移動し続ける軌道を探索したい場合、ＤＷＡ手法は使えないはずである。ＤＷＡ手法は到達目標点の情報を必要とするからである。
本発明では、屈曲する移動経路に沿って移動するための軌道を自動計算する技術を提供する。
【０００７】
本発明は、本来は到達目標点の情報を必要とするＤＷＡ手法が、到達目標点に到達することが重要でない場合にも利用可能であるという着想に立脚している。本発明は、必要時にのみ、視野内にあって検出可能な障害物の位置情報から目標点を暫定的に設定する手法を取り入れると、ＤＷＡ手法によって、屈曲する移動経路に沿って移動し続ける軌道を探索できるという着想に立脚している。
【課題を解決するための手段】
【０００８】
本明細書では、並進速度と角速度を自律的に決定し、屈曲する移動経路に沿って移動し続ける移動体を実現する技術を開示する。
その自律式移動体は、
(1) 移動体に搭載されており、移動体の周囲に存在している障害物までの距離と方位を検出する障害物検出装置と、
(2) 障害物検出装置が検出した結果から、障害物の間を伸びている移動可能経路のコーナ点の位置を算出するコーナ点位置算出装置と、
(3) コーナ点位置算出装置が算出したコーナ点の位置から、コーナ通過時に用いる目標点を設定する目標点設定装置と、
(4) 与えられた並進速度と角速度によって移動した場合に障害物に当接するまでに移動可能な距離から計算される距離評価値を計算する距離評価値計算装置と、
(5) 並進速度から計算される速度評価値を計算する速度評価値計算装置と、
(6) 距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値の合計値が最大となる並進速度と角速度を採用する並進速度と角速度の指令値決定装置を備えている。
(7) 前記(5)の速度評価値計算装置は、並進速度が高速であるほど高い速度評価値を計算する第１特性と、並進速度がコーナ通過時最適速度以下の範囲内であれば高速であるほど高い速度評価値を計算するとともに、並進速度がコーナ通過時最適速度以上の範囲内であれば高速であるほど低い速度評価値を計算する第２特性を備えており、目標点設定装置が設定した目標点までの距離が減速開始距離以下となった時に、第１特性から第２特性に切り換える特性切り換え装置を備えている。
ここでいう目標点までの距離とは、移動体の前後方向に直交する線分（左右線）上に目標点が位置する関係が得られるまでの前後方向に沿った移動距離をいう。
また本明細書でいう「以下と以上」は、数学的に厳密な意味での「以下と以上」ではなく、「未満と以上」の関係と、「以下と比較値より大きい」関係の両者を含む。
また本明細書でいう「軌跡」は経過時間の要素を含まない経路情報をいい、「軌道」は「軌跡」に経過時間の要素を加えたものをいう。
【０００９】
この自律式移動体は、障害物検出装置を備えており、周囲に存在している障害物までの距離と方位を検出する。これによって、あらかじめ地図情報を入力しておかなくても、移動可能な経路を知ることができる。
この自律式移動体は、移動経路の屈曲部（コーナ）までに距離がある間は、目標点を設定しない。目標点を設定しないために、ＤＷＡ手法で採用する偏角評価値を計算することができない。普通に考えれば、ＤＷＡ手法で採用することができないと理解される。しかしながら、この自律式移動体は、目標点を設定しないので計算できない偏角評価値を一定値に固定してしまう。すなわち、目標点に向かっているか否かを判断対象に含めないで軌道を探索する。左右の障害物の間を伸びている経路に沿って移動する場合は、左右の障害物の間を移動することが求められ、目標点に向かっているか否かは重大でないことに対応する。
この自律式移動体は、距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値の合計値が最大となる並進速度と角速度を採用する。これによって、左右の障害物の間を伸びている経路に沿って可能な限り高速に移動する軌道が探索される。距離評価値が大きな軌道を探索することから、障害物に当接しない軌道を探索することができる。
【００１０】
左右の障害物の間を伸びている経路に沿って移動するために、偏角評価値を一定値に固定するＤＷＡ手法を適用すると、移動速度が高速に過ぎて屈曲した経路を曲がりきれない軌道が探索される場合、あるいは、操舵角を変えるタイミングが早すぎて内側コーナ点に接近しすぎてしまう軌道が探索される場合が生じる。
偏角評価値を一定値に固定したＤＷＡ手法では、距離評価値×重み係数１＋速度評価値×重み係数２で計算される総合評価値が最大となる軌道を計算する。その重み係数１と２の値を適値に最適化することによって、コーナ接近時に適度に減速し、的確なタイミングで移動方向を変え始める軌道を計算するようにチューニングすることができる。しかしながら、重み係数を最適化する方法は汎用性に欠け、特定の屈曲パターンに対しては最適化することができても、他の屈曲パターンに対しては最適化されていないことが生じる。すなわち、重み係数を最適化する方法では、特定の屈曲パターンに対してはその屈曲パターンに沿って円滑に移動する軌道が計算できても、他の屈曲パターンには対応できないという問題を生じる。他の屈曲パターンに対しては、移動速度が高速に過ぎて屈曲した経路を曲がりきれないという軌道、あるいは、操舵角を変えるタイミングが早すぎて内側コーナ点に接近しすぎてそれ以上には移動方向を変えられないで立ち往生しまうという軌道が探索されてしまう。
【００１１】
本明細書で開示する自律式移動体は、偏角評価値を一定値に固定してＤＷＡ手法を適用する場合に生じる上記の問題点を解決する。
そのために、この自律式移動体は、コーナ点位置算出装置が特定したコーナ点から、コーナ通過時に用いる目標点を設定する目標点設定装置を備えている。例えば、内側コーナ点から所定距離だけ離れており、それに向かって移動体の進行方向を調整すれば、内側コーナ点に当接しないで曲がれる軌道が計算される結果をもたらす目標点を設定する。経路が非常に狭い場合には、内側コーナ点と外側コーナ点の中央に目標値を取ってもよい。なお、この目標点は、軌道計算のために一時的に用いるものであり、実際に目標点を通過する前に目標点の情報を消去してもよい。すなわち、必ずしも移動体が通過する点であるとは限らない。
この自律式移動体は、速度評価値を計算する２種類の特性を備えており、目標点までの距離が減速開始距離以下となった時に特性を切り換える。すなわち、目標点までの距離が減速開始距離以上の間は、並進速度が高速であるほど高い速度評価値を計算する第１特性に従って速度評価値を計算する。目標点までの距離が減速開始距離以下となった時に、第２特性に切り換える。すなわち、並進速度がコーナ通過時最適速度以下の範囲内では高速であるほど高い速度評価値を計算するとともに、並進速度がコーナ通過時最適速度以上の範囲内では高速であるほど低い速度評価値を計算する第２特性に従って速度評価値を計算する。
【００１２】
上記のように速度評価値の算出式を切り換えると、目標点までの距離が減速開始距離以上の間は高速で移動する軌道が探索される。目標点までの距離が減速開始距離以下となると、コーナ通過時最適速度にまで減速させる要素が強く働き、必要な減速がえられる。
本明細書で開示する自律式移動体は、コーナ部に接近するまでは高速で移動する軌道に従って移動し、コーナ部に接近するとコーナ通過時最適速度に向けて減速する。
コーナ通過時最適速度に向けて減速されるにつれて、実現可能な移動軌跡の曲率半径を小さくすることができる。この自律式移動体は、コーナ接近時にコーナ通過時最適速度に向けて減速し、内側コーナ点に当接しないで曲がれる軌跡を採用しながら、コーナを通過する。
【００１３】
本明細書で開示する技術は、基本的には偏角評価値を一定値に固定してＤＷＡ手法を適用するものであり、目標点設定装置が設定した目標点に向かう方向と移動体の進行方向との偏角から計算される偏角評価値を計算する偏角評価値計算装置は不可欠でない。しかしながら、コーナ接近時に目標点を設定することから、目標点に向かう方向と進行方向とがなす偏角を計算することが可能な期間があり、その期間のために、偏角から偏角評価値を計算する偏角評価値計算装置を備えていることが好ましい。
偏角評価値計算装置を活用する場合には、基本的ＤＷＡ手法によって、並進速度と角速度を決定することが好ましい。すなわち、距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値と、偏角評価値計算装置が計算した偏角評価値の合計値が最大となる並進速度と角速度を採用することが好ましい。
【００１４】
この場合、コーナ接近時には目標点に向けて移動体の姿勢を振る角速度が計算される。コーナを小回りしすぎることがなく、内側コーナ点に当接しないで曲がる軌跡が確実に採用される。
なお、目標点を設定している期間と、偏角評価値計算装置を活用する期間は一致しなくてもよい。目標点を設定していない期間に偏角評価値計算装置を活用することはできないことから、偏角評価値計算装置を活用する期間は目標点を設定している期間内に含まれている必要がある。目標点は、偏角評価値計算装置で利用されるだけでなく、速度評価値の計算にも用いられることから、偏角評価値計算装置を活用しない期間にまで目標点を設定することが意味を持つことがある。目標点を設定している期間のうちの一部期間でのみ、偏角評価値計算装置を活用することもある。
【００１５】
本明細書で開示する技術は、基本的には偏角評価値を一定値に固定してＤＷＡ手法を適用するものであり、基本的には目標点の設定を要しない。コーナ接近時に目標点が設定されていればよい。
その場合でも、高速で通過できる幅の広いコーナを通過する場合には、目標点を設定して減速軌道を誘導したり、移動体の指向方向を修正したりする必要がない。
そこで、移動可能経路の外側コーナ点までの距離が、狭コーナであることが判明する距離以下となった時に、目標点設定装置が目標点を設定するようにしてもよい。ここでいう外側コーナ点までの距離とは、移動体の前後方向に直交する線分上に外側コーナ点が位置する関係が得られるまでの前後方向に沿った移動距離をいう。
【００１６】
移動可能な経路の幅が広い場合、偏角評価値を一定値に固定するともに、並進速度が高速であるほど高評価するＤＷＡ手法によって、外側コーナ点までの距離を十分に確保してコーナを曲がる軌道を計算することができる。移動可能経路の幅が広い場合、目標点を設定して減速する軌道が計算されるように手配する必要もなければ、目標点を設定して移動体の指向方向を修正する必要もない。
それに対して移動経路の幅が狭い場合、内側コーナ点に当接しないためには外側コーナ点までの距離が狭くならざるをえない。外側コーナ点までの距離が所定値以下となれば、狭コーナに接近したことが判明する。この距離を狭コーナ判明距離という。
外側コーナ点までの距離が狭コーナ判明距離以下となった時にはじめて、目標点設定装置が目標値を設定するようにしてもよい。
外側コーナ点までの距離が狭コーナ判明距離以下となった時にはじめて目標値を設定するようにすると、狭コーナ通過時には強く減速し、広コーナ通過時には弱く減速する軌道が計算される。
【００１７】
目標点設定装置が目標点を設定するタイミングと、偏角評価値計算装置が偏角評価値を計算し始めるタイミングは、一致していてもよいし、前者より後者が遅れていてもよい。
すなわち、目標点設定装置が設定した目標点までの距離が偏角制御開始距離以下となった時に、偏角評価値計算装置が偏角評価値を計算し始めるようにしてもよい。狭コーナ通過時には、最初に外側コーナ点までの距離が狭コーナ判明距離以下となり、ついで、目標点までの距離が偏角制御開始距離以下となる。
広いコーナの通過時には目標点が設定されず、目標点までの距離が偏角制御開始距離以下となることがない。偏角評価値を一定値に固定したＤＷＡ手法で軌道を探索することになり、広いコーナを高速で通過する軌道が探索される。
偏角評価値計算装置は、目標点設定装置が設定した目標点までの距離が減速開始距離以下となった時に、偏角評価値の計算を終了してもよい。その後は、偏角評価値を一定値（例えばゼロ）に戻してもよい。
【００１８】
速度評価値計算装置の特性切り換え装置は、目標点設定装置が設定した目標点までの距離が減速終了距離以下となった時に、第２特性から第１特性に切り換えることが好ましい。
【００１９】
第２特性から第１特性に切り換えると、それ以降は加速する軌道に高評価値を与えながら最適軌道が探索される。
【発明の効果】
【００２０】
本明細書で開示する自律式移動体によると、コーナを曲がる際にはコーナ通過時最適速度以下であってコーナ通過時最適速度に近い速度ほど高評価値を与えるロジックによって最適軌道を計算することから、コーナ接近前の並進速度がコーナ通過時最適速度以上である場合には、コーナ接近時にコーナ通過時最適速度に向けてスムースに減速される軌道が計算される。コーナ通過時最適速度に向けて減速されることから、調整可能（採用可能）な軌跡のパターンが多様化され、狭いコーナでも走行不能状態に陥らないで通過する軌跡に沿って移動することが可能となる。移動可能な経路の幅が狭い場合には、狭い経路に沿って移動することが可能な速度にまでスムースに減速される。
請求項２の自律式移動体によると、コーナ通過時には内側コーナ点との当接が避けられる軌道をもたらす目標点に向かう姿勢に調整される。コーナ内側に向けて移動方向を変えるタイミングが早すぎることが防止される。
請求項３の自律式移動体によると、広いコーナを通過する際には目標点が設定されず、狭いコーナを通過する際には目標点が設定される。広いコーナを通過する際には、移動速度が速いほど高評価が与えられる関係を利用して軌道が計算され、狭いコーナを通過する際には、コーナ通過時最適速度に向けて減速するほど高評価が与えられる関係を利用して軌道が計算される。
請求項４の自律式移動体によると、目標点までの距離が偏角制御開始距離以下となった時に移動体の並進方向を調整する制御が採用される。偏角制御開始距離以下となった時にコーナ内側を向いている場合には、コーナ外側に向かう並進方向に調整される。小回りしすぎて内側コーナ点に当接することが防止される。
請求項５の自律式移動体によると、目標点までの距離が減速終了距離以下となった時以降は、加速する軌道に高評価値を与えながら最適軌道が計算される。コーナから離脱する際には、加速する軌道が計算される。
【図面の簡単な説明】
【００２１】
【図１】移動体と移動可能経路の一例を示す図。
【図２】移動体のハードウエア構成を示す図。
【図３】移動体の制御システムを示す図。
【図４】並進速度ｖと角速度ｗの選択範囲を概念的に示す図。
【図５】偏角評価値を説明する図。
【図６】距離評価値を説明する図。
【図７】速度評価値を説明する図。
【図８】従来技術で探索される軌道を示す図。
【図９】実施例で採用するロジックとパラメータの切り換えを例示する図。
【図１０】実施例で探索される軌道を示す図。
【図１１】経過時間に対する並進速度の変化パターンを新旧対比して示す図。
【発明を実施するための形態】
【００２２】
以下に説明する実施例の主要な特徴を列記する。
（第１特長）自律式移動体は、左右一対の駆動輪を備えており、その回転速度が独立に調整可能である。左右の回転速度の差によって、指定された角速度を実現する。
（第２特長）狭いコーナを通過する際に、下記の事象が順に発生し、下記の処理が順に実施される。
(a) 移動体から外側コーナ点までの距離が狭コーナ判明距離以下となる。そこで目標点を設定する。
(b) 移動体から目標点までの距離が偏角制御開始距離以下となる。そこで偏角評価値を計算し始める。
(c) 移動体から目標点までの距離が減速開始距離以下となる。そこで速度評価値計算装置が、第１特性から第２特性に切り換える。(d) 同時に、偏角評価値を一定値に戻す。
(e) 移動体から目標点までの距離が減速終了距離以下となる。そこで速度評価値計算装置が、第２特性から第１特性に切り換える。
(f) 移動体が目標点を通過する。そこで目標点を消去する。
（第３特長）狭いコーナを通過する際に、内側コーナ点と外側コーナ点を結ぶ線上に目標点を設定する。
（第４特長）狭いコーナを通過する際に、内側コーナ点と外側コーナ点の中点に目標点を設定する。
（第5特長）外側コーナ点までの距離が狭コーナ判明距離以下にならない広いコーナを通過する際には、目標点を設定しない。
【実施例】
【００２３】
図1は、左壁（左側障害物）５０と右壁（右側障害物）５２の間を伸びている移動可能経路Ｒと、その移動可能経路Ｒに沿って移動する自律式移動体２を示している。自律式移動体２は、左壁５０と右壁５２に当接しないで移動可能経路Ｒに沿って移動する軌道を計算し、計算された軌道に沿って移動する。自律式移動体２は、無人式でもよいし、搭乗者を移送するものであってもよい。搭乗者がいても搭乗者が操舵する必要はない。
【００２４】
図1の自律式移動体２の左右幅は0.55ｍであり、前後長は1.1ｍであり、移動可能経路Ｒの幅は1.0ｍであり、直角に屈曲しているコーナを通過する様子を示している。
自律式移動体２は、左駆動輪４と、右駆動輪６と、左前従動キャスタ８と、右前従動キャスタ１０を備えている。図２に示すように、左駆動輪４は左駆動輪モータ４ｂで駆動され、右駆動輪６は右駆動輪モータ６ｂで駆動される。左駆動輪モータ４ｂと右駆動輪モータ６ｂの回転速度は独立に調整することが可能である。左駆動輪モータ４ｂと右駆動輪モータ６ｂの間に回転速度差を与えれば、自律式移動体２は旋回中心３の周りに旋回する。
自律式移動体２に並進速度ｖと角速度ｗが指令された場合、左駆動輪モータ４ｂの回転速度ｖｌと、右駆動輪モータ６ｂの回転速度ｖｒを、下記の式（１）（２）を満たす値に調整することによって、自律式移動体２は指令された並進速度ｖと角速度ｗで移動する。
ｖ＝（ｖｌ＋ｖｒ）／２ 式（１）
ｗ＝ｖｌ−ｖｒ 式（２）
実際には、モータと車輪との間にある減速機の減速比や、車輪の直径等を加味した係数を乗算する必要があるが、これらは周知のことであり、上記式では係数を省略して表示している。
本実施例では、左駆動輪４と右駆動輪６が、移動軌跡に直交する線分上に維持される。本明細書でいう並進速度とは、左駆動輪４と右駆動輪６を結ぶ線分と移動軌跡との交点の移動速度をいう。本実施例では、左駆動輪４と右駆動輪６の回転速度を独立に調整することによって、指令された並進速度ｖと角速度ｗを実現する。これに代えて、左輪と右輪の方位を変える操舵機構を利用してもよい。この方式でも、指令された並進速度と指令された角速度に調整することができる。
【００２５】
自律式移動体２は、レーザ光の照射装置と、反射してくるレーザ光の受光装置と、照射方向と受光方向を旋回させるスキャン装置からなるスキャン式発光・受光装置１２を搭載している。スキャン式発光・受光装置１２は、水平面内を進むレーザ光を照射するとともに、移動体２の前後方向とレーザ光の照射方向との間の角度を経過時間に対して変化させる。水平面内を進むレーザ光は、移動可能面から立ち上がっている物体があれば、その物体で反射され、その反射光が受光装置で受光される。スキャン式発光・受光装置１２は、障害物までの距離とその方位（移動体２の前後方向となす角度）を検出する。スキャン式発光・受光装置１２は、レーザ光の照射方向をスキャンしながら所定時間毎に障害物までの距離とその方位を検出する。破線１４と１６は、スキャン式発光・受光装置１２による検出限界を示している。
自律式移動体２を基準とするｘ−ｙ座標系が設定されている。座標原点は、自律式移動体２の左右の駆動輪の中心位置（旋回中心）３に設定されている。ｘ軸は、自律式移動体２が前方に直進する方向に設定されている。ｙ軸は、右駆動輪６を通過している。スキャン式発光・受光装置１２が検出した障害物までの距離と方位の情報に基づいて、障害物のｘ−ｙ座標系における位置が特定される。
【００２６】
図１の黒点１００は、スキャン式発光・受光装置１２によって所定時間毎に検出された障害物の位置を示している。図示の場合、所定時間毎に検出された障害物の位置をｘ−ｙ座標系にプロットしてみると、一直線上に配置されている位置群１００ａと、他の直線上に配置されている位置群１００ｂと、さらに他の直線上に配置されている位置群１００ｃに分類される場合を例示している。
図３に示すように、自律式移動体２は、外側コーナ点・内側コーナ点位置算出部２４を備えている。コーナ点位置算出部２４は、２つの直線の交点に存在する障害物の位置ＯＵを抽出し、それを外側コーナ点ＯＵの位置とする。また、コーナ点位置算出部２４は、直線の端部に存在する障害物の位置ＩＮを抽出し、それを内側コーナ点ＩＮの位置とする。図１の破線１８は、内側コーナ点ＩＮによって制約される視野の限界を示している。障害物の位置１００ｄも直線の端部に位置しているが、これは視野の限界線上にあるので、内側コーナ点とはしない。
図３に示すように、自律式移動体２は、目標点設定部２６を備えている。目標点設定部２６は、図１に示すように、外側コーナ点ＯＵと内側コーナ点ＩＮの中間位置を目標点Ｔに設定する。
【００２７】
図２は、自律式移動体２のハードウエア構成を示し、コンピュータ２０を備えている。
左駆動輪４を駆動する左駆動輪モータ４ｂは、左駆動輪モータ用ドライバ４ａを介してコンピュータ２０に接続されており、コンピュータ２０が左駆動輪モータ４ｂの回転速度を時々刻々と制御する。左駆動輪モータ４ｂには左エンコーダ４ｃが接続されており、左エンコーダ４ｃはコンピュータ２０に接続されている。コンピュータ２０は左駆動輪モータ４ｂの回転速度を時々刻々と入力する。同様に、右駆動輪６を駆動する右駆動輪モータ６ｂは、右駆動輪モータ用ドライバ６ａを介してコンピュータ２０に接続されており、コンピュータ２０が右駆動輪モータ６ｂの回転速度を時々刻々と制御する。右駆動輪モータ６ｂには右エンコーダ６ｃが接続されており、右エンコーダ６ｃはコンピュータ２０に接続されている。コンピュータ２０は右駆動輪モータ６ｂの回転速度を時々刻々と入力する。
またスキャン式発光・受光装置１２は、スキャン式発光・受光装置ドライバ１２ａを介してコンピュータ２０に接続されている。コンピュータ２０は、スキャン式発光・受光装置１２を動かし、スキャン式発光・受光装置１２によって所定時間毎に検出される障害物までの距離と方位の情報を入力する。
【００２８】
図３は、コンピュータ２０とそのプログラムによって実現されている各種の処理機能を示している。コンピュータ２０は、所定時間毎にスキャン式発光・受光装置１２から障害物までの距離と方位の情報を入力し、所定時間毎に障害物のｘ−ｙ座標系における位置を算出する。コンピュータ２０とスキャン式発光・受光装置１２とプログラムによって、障害物検出部２２が実現される。
コンピュータ２０は、ｘ−ｙ座標系における障害物の位置から、２つの直線の交点に存在する障害物の位置ＯＵを抽出し、それを外側コーナ点ＯＵの位置とする。また、直線の端部に存在する障害物の位置ＩＮを抽出し、それを内側コーナ点ＩＮの位置とする。この処理によって外側コーナ点・内側コーナ点位置算出部２４が実現される。
コンピュータ２０は、外側コーナ点ＯＵと内側コーナ点ＩＮの中間位置Ｔを目標点Ｔに設定する。この処理によって目標点設定部２６が実現される。
【００２９】
コンピュータ２０は、並進速度ｖと角速度ｗの指令値を所定時間毎に決定する処理を実行する。この処理によって、並進速度と角速度の指令値決定部２８が実現される。
コンピュータ２０は、候補設定処理を実施し、その処理によって候補設定部３０が実現される。自律式移動体２の最大並進速度ｖは1m/secであり、ゼロ〜1m/secの間を0.1m/secの速度分解能で調整可能である。また、自律式移動体２の最大角速度ｗは0.65rad/secであり、−0.65rad/sec〜0.65rad/secの間を0.065rad/secの角速度分解能で調整可能である。図４は、横軸に並進速度ｖをとり、縦軸に角速度をとった２次元のテーブルを示しており、自律式移動体２で実現可能な並進速度ｖと角速度ｗの組み合わせの全部を示している。
左下がりのハッチの位置４０１は、現在の並進速度ｖと現在の角速度ｗを示している。矩形範囲４０２は、現在の並進速度ｖから自律式移動体２で実現可能な最大加速度で増速したときの所定時間後（本実施例では0.1sec後）の並進速度と、現在の並進速度ｖから自律式移動体２で実現可能な最大減速度で減速したときの所定時間後の並進速度と、現在の角速度ｗから自律式移動体２で実現可能な最大加速度で角速度を増速したときの所定時間後の角速度と、現在の角速度ｗから自律式移動体２で実現可能な最大減速度で角速度を減速したときの所定時間後の角速度の範囲を示している。すなわち、所定時間後（0.1sec後）に実現可能な並進速度ｖと角速度ｗの組み合わせを示している。
候補設定部３０は、矩形範囲４０２のなかから、並進速度ｖと角速度ｗの組み合わせを１個ずつ順に取り出す。
【００３０】
コンピュータ２０は、偏角から計算される評価値を計算する処理を実施し、その処理によって偏角評価値計算部３２が実現される。
図５（ａ）において、ベクトルｄは、移動体２の旋回中心位置３から目標点Ｔに向かうベクトルを示している。また、ベクトルｐは、候補設定部３０が設定した並進速度ｖと角速度ｗの組み合わせによるときの進行方向を示している。すなわち単位時間（本実施例では0.1秒）の間、並進速度をｖに維持して角速度をｗに維持した場合の位置の変化方向を示している。図５（ａ）では、単位時間だけ前には３ｘにあった移動体２が、単位時間後には３ｙに移動した場合を例示しており、ペクトルｐは３ｘから３ｙまで伸びている。θ１は、ベクトルｄとベクトルｐの偏角、すなわち、目標値に向かう方向と実際の進行方向の間に存在する偏角を示している。当然のことながら、偏角θ１がゼロに近い進行方向が好ましい。
図５（ｂ）は、偏角θ１と、偏角評価値head(v,w)の関係を示している。head(v,w)＝π―θ１の関係にあり、偏角θ１がゼロに近いほど大きな偏角評価値head(v,w)が与えられる。
【００３１】
コンピュータ２０は、障害物に当接するまでに移動可能な距離から計算される評価値を計算する処理を実施し、その処理によって距離評価値計算部３４が実現される。図６（ａ）において、参照番号６０１は、距離評価値計算時点における自律式移動体２の位置を示し、６０３は、その時点における並進速度ｖと角速度ｗを維持したときの移動軌跡を示している。黒点１０１〜１０５は、その時点で検出されている障害物の位置を示している。参照番号６０５は、移動軌跡６０３による場合に、移動体２が障害物１０１〜１０５に当接するタイミングにおける位置を示している。この場合、障害物１０１〜１０５のうち、移動体２が最初に障害物１０４に当接する場合を例示している。移動軌跡６０３の半径Ｒと、移動軌跡６０３の張る角度θ２を計算すると、移動体２が障害物１０１〜１０５のいずれかに最初に当接するまでに移動可能な距離が計算できる。
【００３２】
本実施例では、図６（ｂ）に示すように、移動軌跡６０３の張る角度θ２を計算するために、固定した移動体２に対して相対的に障害物１０１〜１０５が移動すると扱う。そのように扱うと、移動軌跡６０３が張る角度θ２と、障害物１０１〜１０５の各々の移動軌跡が張る角度θ２が等しいことがわかる。
計算をしないと、障害物１０１〜１０５のうちのどれが最初に移動体２に当接するかがわからない。また、計算しないと、移動体の前辺に当接するのか、左辺に当接するのか、右辺に当接するのかわからない。並進速度がマイナスであれば、後辺に当接する可能性もある。
【００３３】
そこで、本実施例では、障害物１０１〜１０５のそれぞれについて、移動体２に対する移動軌跡を計算し、前辺と交差するまでの角度と、左辺と交差するまでの角度と、右辺と交差するまでの角度と、後辺と交差するまでの角度を計算し、最小角度を計算する。この結果、障害物１０１〜１０５のそれぞれについて、移動体のどこかに当接するまでの角度がわかる。そこで、障害物１０１の最小角度と、障害物１０２の最小角度と、障害物１０３の最小角度と、障害物１０４の最小角度と、障害物１０５の最小角度のなかから最小のものを選び出せば、最初に移動体に当接する障害物が判る。図６（ｂ）は、最初に障害物１０４が移動体２の左辺に当接する場合を例示している。以上によって、角度θ２が計算できる。その結果、距離評価値計算時点における並進速度ｖと角速度ｗを維持したときにいずれかの障害物が移動体２のどこかに当接するまでに移動可能な距離が計算される。
【００３４】
障害物に当接するまでの移動距離が長い移動軌跡ほど好ましい。すなわち、長い移動距離を実現する並存速度ｖと角速度ｗの組み合わせほど好ましい。図６（ｃ）は、障害物に当接するまでの移動距離（Ｒ×θ２）と、距離評価値dist(v,w)の関係を示している。
図６（ｃ）に示すように、障害物に当接するまでの移動距離（Ｒ×θ２）が長い移動軌跡（並進速度ｖと角速度ｗの組み合わせで決まる）ほど、高い距離評価値dist(v,w)が与えられる。
【００３５】
コンピュータ２０は、さらに、速度評価値計算部３６を備えている。図７（１）は、並進速度ｖと速度評価値vel(v,w)の第１の関係を示している。並進速度ｖが高速なほど速度評価値vel(v,w)が高い。図７（２）は、並進速度ｖと速度評価値vel(v,w)の第２の関係を示している。図中のVTMは、コーナ通過時最適速度であり、本実施例では0.12m/secが採用されている。移動体２の最大減速度は2m/sec２であり、並進速度ｖと角速度ｗを決定する時間間隔は0.1secである。したがって、0.2m/sec以下の速度であれば、0.1sec後には停止させることができる。本実施例では、次に並進速度ｖと角速度ｗを決定するタイミングで並進速度ｖをゼロとできる範囲内に、コーナ通過時最適速度VTMをとっている。
図７（２）に示す第２の関係では、並進速度ｖがコーナ通過時最適速度VTM以下の範囲内であれば、並進速度ｖが高いほど高い速度評価値vel(v,w)をあたえる。ところが、並進速度ｖがコーナ通過時最適速度VTMを超えると、並進速度ｖが高いほど低い速度評価値vel(v,w)をあたえる。本実施例では、マイナスの評価値を与える。マイナスの評価値の絶対値が大きいほど、速度評価値は低い。
【００３６】
本実施例では、速度評価値vel(v,w)を計算するのに、２種類の関係を使いわける。検出された障害物の位置から、コーナに接近したことが判明した場合には、図７（２）の関係を利用して速度評価値vel(v,w)を計算する。その他の場合には、図７（１）の関係を利用して速度評価値vel(v,w)を計算する。コーナに接近したときの並進速度が、コーナ通過時最適速度VTMを超えていれば、コーナ通過時最適速度VTMに向けて減速する移動軌道を実現する並進速度ｖと角速度ｗが選択されるようにする。
【００３７】
コンピュータ２０によって実現される並進速度ｖと角速度ｗの指令値決定部２８は、候補設定部３０で選択された並進速度ｖと角速度ｗの組み合わせ毎に、偏角評価値head(v,w)と、距離評価値dist(v,w)と、速度評価値vel(v,w)を計算し、３つの評価値を合計した総合評価値を計算する。その処理を実現することで、総合評価値計算部３８が実現されている。並進速度ｖと角速度ｗの指令値決定部２８は、並進速度ｖと角速度ｗの組み合わせ毎に計算された総合評価値の中から、最大の評価値をもたらす並進速度ｖと角速度ｗを採用する。その処理によって、指令値決定部４０が実現されている。
実際には、総合評価値の計算のために、偏角評価値head(v,w)と、距離評価値dist(v,w)と、速度評価値vel(v,w)の各々に、重み係数α,β,γを乗じてから合計する。すなわち、
総合評価値＝α・head(v,w)＋β・dist(v,w)＋γ・vel(v,w)の式を用いる。
【００３８】
図３に示すように、コンピュータ２０は、左エンコーダ４ｃと右エンコーダ６ｃからエンコーダ情報を入手し、実際の並進速度ｖと実際の角速度ｗを検出する。自律式移動体２は、フィードバック制御部４２を備えており、左駆動輪モータ用ドライバ４ａと右駆動輪モータ用ドライバ６ａを制御し、実際の並進速度ｖと実際の角速度ｗを指令値決定部４０が決定した並進速度ｖと角速度ｗに一致させる。自律式移動体２は、指令値決定部４０が決定した並進速度ｖと角速度ｗによって移動する。
【００３９】
図８は、目標点を定めず、偏差評価値head(v,w)を一定値（この場合はゼロ）に維持し、かつ図７（１）の関係のみを利用して速度評価値vel(v,w)を計算した場合に計算される軌道を示している。図８（ａ）の場合、コーナ進入時に自律式移動体２を減速させる並進速度ｖが計算されず、コーナ進入時の移動速度が速すぎる軌道が計算された場合を例示している。この場合、正面の壁までの距離が相当程度に小さくなった場合に、距離評価値dist(v,w)が小さくなる。そこで、並進速度ｖを減速して軌跡の曲率半径を小さくすることによって、距離評価値dist(v,w)を大きくする軌道が選択され始める。しかしながら減速開始タイミングが遅すぎ、正面の壁面を避けることができない。自律式移動体２は、正面の壁に接近しすぎて移動不能となってしまう。
図８（ｂ）は、距離移動評価値dist(v,w)を大きくする軌道を選択した結果、角速度ｗを大きくして進路を変えるタイミングが早すぎる軌道が計算された場合を例示している。この場合、移動体２の右辺が内側コーナに接近しすぎ、それ以上に角速度を増大できなくなり、結局正面の壁を回避しきれない結果となってしまう。
【００４０】
総合評価値＝α・head(v,w)＋β・dist(v,w)＋γ・vel(v,w)を求める式において、係数α、β、γの関係を最適化すれば、図８（ａ）と（ｂ）の軌道を算出してしまうことを避けることができる。例えば、図８（ａ）の軌道が計算される場合、βを大きくしてγを減少させれば、より早い段階で減速し始める軌道が計算される。図８（ｂ）の軌道が計算される場合、βを小さくしてγを大きくすれば、カーブを回り始めるタイミングを遅らせることができる。しかしながら、係数α、β、γの関係を最適化する手法には限界があり、特定の形状の屈曲部をうまく曲がる軌道が計算されるようにすると、他の形状の屈曲部をうまく曲がる軌道が計算されなくなってしまう。
【００４１】
本実施例では、図９に示すように、下記の改良を加えた。
（ａ）外側コーナ点（図１のＯＵ）までの距離が２m以下になった時点で、目標点を設定する。外側コーナ点ＯＵと内側コーナ点ＩＮの間を２等分した点に目標点Ｔをおく。なお、ここでいう距離は、移動体２のｘ軸に沿った距離をいう。移動体２が正面の壁に直交する方向に移動している場合、正面の壁から移動体２の回転中心３までの距離が２mとなった時に、外側コーナ点ＯＵまでの距離が２mとなる。道幅が1mであり、移動体２の移動方向が正面の壁に直交する方向である場合、内側コーナ点ＩＮから回転中心３までの距離が１mとなった時に、外側コーナ点ＯＵまでの距離が２mとなる。移動体２の移動方向が正面の壁に直交しない方向である場合は、図１０の（ａ）に例示するように、移動体の前後方向に沿って、外側コーナ点ＯＵと旋回中心３の距離を計測する。
道幅が広い場合、外側コーナ点ＯＵまでの距離が２m以下にならないままに屈曲部を通過してしまうことがある。この場合は、目標点が設定されない。本実施例では、狭コーナ判明距離を２mとしている。外側コーナ点ＯＵまでの距離が狭コーナ判明距離以下にならなければ、目標点が設定されないから偏角評価値head(v,w)が計算されない。また、目標点までの距離によって、図７の（１）から（２）に切り換えることもない。広いコーナを通過する場合には、図７の（１）の関係で計算した速度評価値vel(v,w)と距離評価値dist(v,w)によって、内側コーナ点ＩＮの近傍を回転中心３が移動して屈曲部を通過する軌道が計算される。
【００４２】
（ｂ）目標点（図１のＴ）までの距離が1.5m以下になった時点で、偏角評価値head(v,w)の計算を開始する。この実施例では、偏角制御開始距離を1.5mとしている。目標点Ｔまでの距離が1.5m以下になって偏角評価値head(v,w)の計算を開始すると、それ以降は、目標点Ｔを指向する軌道が探索されることになる。コーナ進入時の移動体２の姿勢（移動体２が向いている方向）が、コーナを通過しやすい姿勢に矯正される。
なお前記したように、内側コーナ点（図１のＩＮ）までの距離が0mになっても外側コーナ点（図１のＯＵ）までの距離が２m以上あることがあり、この場合には目標点Ｔが設定されていないために、偏角評価値head(v,w)は計算されない。
【００４３】
（ｃ）目標点までの距離が１m以下となった時点で、速度評価値vel(v,w)の計算に用いる特性を、図７（１）から図７（２）に切り換える。目標点までの距離が１m以上の間は、図７（１）の特性が利用され、並進速度ｖが速い軌道を高評価するロジックで軌道が探索される。目標点までの距離が１m以下になると、コーナ通過時最適速度VTM以下ならばコーナ通過時最適速度VTMに近い軌道ほど高評価され、コーナ通過時最適速度VTMを超える軌道は非常に低く評価されるロジックで軌道が探索される。この結果、目標点Ｔまでの距離が１m以下となった時点の並進速度ｖがコーナ通過時最適速度VTMを超えていれば、それ以降は減速して並進速度ｖを低下させる軌道が高く評価されるようになる。本実施例では、減速開始距離を1mとしている。目標点までの距離が１m以下となった時点以降は、並進速度ｖを強く減速する軌道が優先的に探索されることになる。この実施例では、図７（２）に示すように、速度評価値が同じとなる２以上の並進速度ｖが存在しない特性に調整されており、コーナ通過時最適速度VTM以下の軌道とコーナ通過時最適速度VTM以上の軌道が交互に探索されることを防止している。
上記したように、外側コーナ点ＯＵと内側コーナ点ＩＮの間を２等分した点までの距離が１m以下となっても、外側コーナ点ＯＵまでの距離が２m以上あれば、目標点が設定されておらず、図７の（１）から（２）に切り換える処理が実行されない。
（ｄ）目標点までの距離が１m以下となった時点で、偏角評価値head(v,w)の計算を終了する。目標点までの距離が１m以下となった時点以降は、コーナ通過時最適速度VTM以下の範囲内でコーナ通過時最適速度VTMに近い速度を持つ軌道ほど高く評価され、移動可能な距離が長い軌道ほど高く評価される関係を用いることで、内側コーナ点をクリアしながらコーナを通過する軌道が探索される。偏角評価値head(v,w)を一定値に維持しても、内側コーナ点をクリアしながらコーナを通過する軌道が探索される。
【００４４】
（ｅ）目標点までの距離が0.7m以下となった時点で、速度評価値vel(v,w)の計算に用いる特性を、図７（２）から図７（１）に戻す。本実施例では、減速終了距離を0.7mとしている。（ａ）から（ｄ）の態様によって軌道を計算しながら目標点Ｔまでの距離が0.7m以下にまで接近すると、コーナ通過中の中間時点を越え、これ以降は加速してもコーナを安全に通過することができる状態となる。目標点Ｔまでの距離が0.7m以下となった時点で、速度評価値vel(v,w)の計算に用いる特性を、図７（２）から図７（１）に戻すと、並進速度が高速であるほど高い評価を与えるロジックに従って軌道が計算され、移動体の並進速度ｖが加速されていく。これ以降は加速してもコーナを安全に通過することができる状態になると、並進速度ｖが加速されていく軌道が探索される。
【００４５】
（ｆ）目標点までの距離が0mになったら、目標点の設定を解除する。前述したように、上記の距離は移動体２の前後方向に伸びるｘ軸に沿った距離であり、目標点までの距離が0m
となる位置は、旋回中心３の側方（ｙ軸方向）に目標点Ｔが位置している位置関係に等しい。その時点で、目標点Ｔの設定を解除する。
【００４６】
図９の（２）は、（ｂ）と（ｄ）の処理によって、偏角評価値がどのように計算されるかを示すものであり、（ｂ）以前と（ｄ）以降は、偏角評価値をゼロとする。偏角評価値は、最終的には総合評価値の計算に用いられ、最大総合評価値に選択に用いられる。偏角評価値がゼロでなくても、一定値であれば、ゼロと同じになる。（ｂ）〜（ｄ）の間のみ、目標値Ｔに向けた方向と移動体２の進行方向のなす偏角がゼロに近いほど高く評価するロジックにしたがって軌道が計算される。
【００４７】
図９（３）は、（ｃ）と（ｅ）の処理によって、速度評価値がどのように計算されるかを示すものであり、（ｃ）以前と（ｅ）以降は、並進速度ｖが速いほど高く評価するロジックにしたがって軌道が計算される。（ｃ）〜（ｅ）の間は、コーナ通過時最適速度VTM以下の範囲内であり、コーナ通過時最適速度VTMに近い並進速度ほど高く評価するロジックにしたがって軌道が計算される。
【００４８】
図１０は、本実施例で計算される軌道を示している。
（ａ）は、外側コーナ点ＯＵまでの距離が２mになった時点を示し、外側コーナ点ＯＵと内側コーナ点ＩＮの間を２等分した点に目標点Ｔを設定した状態を示す。なお、それ以前には目標点が設定されておらず、head(v,w)＝０であり，β・dist(v,w)＋γ・vel(v,w)の式から求められる総合評価値を最大する並進速度ｖと角速度ｗを所定時間毎に求めることで軌道を決めている。すなわち、障害物に当接するまでに移動できる距離を大きくし、しかも高速で移動する軌道ほど高く評価するロジックにしたがって、並進速度ｖと角速度ｗが決められる。（ａ）になるまでは、移動可能経路Ｒ内を障害物に衝突しないで高速で移動する軌道が探索される。
(ｂ)は、目標点Ｔまでの距離が1.5m以下になった時点を示しており、それ以降は、移動体の進行方法と目標点Ｔを向く方向とがなす偏角が小さいほど高く評価するロジックにしたがって軌道が探索される。目標点Ｔまでの距離が1.5m以下になると、移動体２の進行方向が目標点Ｔを指向する向きに変えられる。
（ｃ１）は、目標点Ｔまでの距離が1.0m以下になった時点を示しており、それ以降は、図７（２）の特性に従って速度評価値が計算される。コーナ通過時最適速度VTM以下であってコーナ通過時最適速度VTMに近い速度ほど高評価値を与えるロジックによって軌道を計算することから、目標点Ｔまでの距離が1.0m以下になると、移動体２の並進速度が速やかに減速される。
（ｃ２）は、目標点Ｔまでの距離が1.0m以下で0.7m以上の時点を示しており、十分に減速された状態を示している。十分に減速すると、小さな並進速度ｖと大きな角速度ｗの組み合わせが利用可能となり、曲率半径の小さな軌道（シャープに曲がる軌道）を採用することができる。なお、ｃ１の時点で、偏角評価値head(v,w)はゼロに戻されている。（ｂ）から（ｃ１）の間に、移動体２の進行方向が目標点Ｔを指向する向きに変えられているので、（ｃ１）以降は偏角評価値head(v,w)をゼロとしても、内側コーナ点ＩＮに当接しない軌道が計算される。
（ｅ）は、目標点Ｔまでの距離が0.7m以下になった時点を示しており、それ以降は、図７（１）の特性に従って速度評価値が計算される。移動体２の並進速度が速やかに増速される。
（ｆ）は、目標点Ｔまでの距離が0.0m以下になった時点を示しており、先に設定した目標値Ｔを消す。なお、目標点Ｔは、目標点Ｔまでの距離が0.7mになった時点以降は利用しないので、目標点Ｔまでの距離が0.7m〜0.0mのいずれかの時点で消去すればよい。
【００４９】
図１１の（Ａ）は、本実施例による場合の並進速度ｖの時間に対する変化パターンを示しており、（Ｂ）は、従来技術による場合の並進速度ｖの時間に対する変化パターンを示している。本実施例によるとコーナ進入時に減速され、小さな曲率半径の軌跡を実現できる速度でコーナに進入し、コーナ通過時の後半では増速する軌道が計算されることが確認される。従来技術によると、コーナ進入時に減速されず、コーナを通過できないで停止してしまうことがわかる。
【００５０】
本実施例では、制御に用いる特性の切り換え点等を下記のようにして決定した。
（１）コーナ通過時最適速度VTMは、最大減速度（2m/sec２）×軌道を計算する時間間隔（0.1sec）以下とした。本実施例では、余裕を見て0.12m／secとした。この場合、次の軌道計算タイミングで並進速度をゼロとすることができる。また、次の軌道計算タイミングで最小の並進速度と最大の角速度に調整して、極めて小さな曲率半径を持つ軌道に調整することを可能とする。
（２）増速運転を開始する位置（減速終了距離）は、目標点Ｔから旋回中心３までの距離が0.7mの位置とした。この位置は、移動体２の約前半分がコーナに進入した位置に相当し、その後はコーナから出る時点に相当する。移動体２の約前半分がコーナに進入した位置から、増速を重視するロジックを採用すると、コーナから速やかに脱出する軌道が計算される。
（３）図７の(２)の特性を利用して減速を重視するロジックを採用する期間は、最大並進速度（1m／sec）から最大減速度（2m/sec２）で減速して停止するまでに移動する距離（0.25m）以上とした。本実施例では、余裕を見て0.3mとした。目標点から旋回中心までの距離が0.7mとなるのに先だって0.3mの減速区間を設けると（目標点Ｔから旋回中心３までの距離が１m（減速開始距離）となった時に、図７の(２)の特性に切り換えると）、コーナ進入時の速度が十分に減速され、外側障害物にも内側コーナ点にも当接しない軌道が計算される。またその後は増速してもコーナからうまく抜け出す車両姿勢に調整される。
（４）目標点を指向する方向を重視して軌道を計算し始める位置は、目標点Ｔまでの距離が1.5m（偏角制御開始距離）の位置とした。その位置が近すぎると、移動体２の指向方向が矯正されないままにコーナに進入することになり、その位置が遠すぎると、移動体２の指向方向が矯正されすぎてしまう。後者の場合、２つの曲がり角が近距離で連続する場合に、滑らかな軌道が計算されなくなってしまう。本実施例の技術によると、２つの曲がり角が近距離で連続する場合にも、滑らかに通過する軌道が探索される。
【００５１】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
（１）本実施例では、目標点Ｔまでの距離が1.5〜1.0ｍの範囲では、偏角評価値head(v,w)を計算する。しかしながら、図７（１）から図７（２）に切り換えて速度評価値vel(v,w)を計算することによって、十分に減速する軌道が探索されるようにするだけで、コーナをスムースに通過する軌道が得られる環境もあり、偏角評価値head(v,w)を計算する処理を省略することもできる。
（２）本実施例では、外側コーナ点までの距離によって目標点を設定するが、経路幅が限定されている環境では、コーナ点が特定された時点で目標点を設定してもよい。
（３）本実施例では、外側コーナ点と内側コーナ点の中点に目標点を設定する。これに代えて、内側コーナ点から所定距離だけ離れた点に目標点を設定してもよい。移動体の幅を考慮して所定距離を設定すれば、内側コーナ点をクリアする軌道が計算されるようにする目標点Ｔを設定することができる。
（４）本実施例では、経路が分岐していない例で説明した。経路が分岐している場合、移動体２に「右または左」さえ指示してやれば、本実施例の技術によって、指示された側の分岐を通過する軌道を自律的に探索して移動することができる。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時の請求項に記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数の目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【００５２】
２：自律式移動体
３：旋回中心
４：左駆動輪
６：右駆動輪
８：左前従動キャスタ
１０：右前従動キャスタ
１２：スキャン式発光・受光装置
２０：コンピュータ
２２：障害物検出部
２４：外側コーナ点・内側コーナ点算出部
２６：目標点設定部
２８：並進速度と角速度の指令値決定部
３０：候補設定部
３２：偏角評価値計算部
３４：距離評価値計算部
３６：速度評価値計算部
３８：総合評価値計算部
３０：指令値決定部
４２：フィードバック制御部
４４：並進速度と角速度の検出部
【技術分野】
【０００１】
本発明は、自律式移動体に関する。すなわち、周囲に存在している障害物までの距離と方位を検出する障害物検出装置を搭載しており、左右の障害物の間を伸びている移動可能経路を探索し、移動可能経路に沿って移動するための並進速度と角速度を自律的に決定して移動する移動体に関する。
【背景技術】
【０００２】
到達目標点が指示され、移動開始点と到達目標点の間に障害物が存在する場合、移動開始点から障害物を避けながら到達目標点に至る軌道を計算する必要がある。その軌道を計算するために、ＤＷＡ（Dynamic Window Approach）という手法が開発されており、非特許文献１に記載されている。
ＤＷＡ手法では、移動体の並進速度ｖと角速度ｗを、所定時間毎に繰り返し決定する。ＤＷＡ手法では、所定時間毎に並進速度ｖと角速度ｗを仮定し、仮定した並進速度ｖと角速度ｗから次の３つの評価値を計算する。
偏角評価値：現在位置から到達目標点に向かう方向と、仮定した並進速度と仮定した角速度で決定される移動体の進行方向とがなす角度（偏角）θを評価した値。偏角θがゼロに近いほど偏角評価値が高く、偏角θがπに近いほど偏角評価値が低い。
距離評価値：仮定した並進速度ｖと角速度ｗによる場合に、現在位置から障害物に当接するまでに移動できる距離を評価した値。移動可能な距離が長いほど距離評価値が高い。
速度評価値：仮定した並進速度ｖを評価した値。並進速度ｖが高速なほど速度評価値が高い。
ＤＷＡ手法では、偏角評価値と距離評価値と速度評価値を合計した総合評価値が最も高くなる並進速度ｖと角速度ｗを採用する。ＤＷＡ手法では、所定時間毎に、総合評価値が最も高くなる並進速度ｖと角速度ｗに更新していく。ＤＷＡ手法によれば、所定時間毎に変化する並進速度ｖと角速度ｗの指令値を設定することができる。換言すれば、所定時間毎に目標点の位置を決定することができる。ＤＷＡ手法によると、障害物を避けながら、到達目標点に迅速に到達する軌道を探索することができる。
特許文献１に、所定時間毎に設定された目標点をたどって移動する技術が記載されている。所定時間毎の目標点を設定する手法は記載されていないが、ＤＷＡ手法を用いれば、所定時間毎の目標点を設定する過程を自動化することができる。
【先行技術文献】
【非特許文献】
【０００３】
【非特許文献１】D. Fox, W. Burgard, and S. Thrun, “The dynamic window approach to collision avoidance” IEEE Robotics & Automation Magazine, Vol. 4, No. 1, pp. 23-33, March, 1997
【特許文献】
【０００４】
【特許文献１】特開２０００−３３０６３２号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
ＤＷＡ手法は、到達目標点の位置と障害物の位置の情報を必要とする。すなわち、地図情報を必要とする。地図情報が取得されていれば、障害物を避けながら、到達目標点に迅速に到達する軌道を探索することができる。
しかしながら、当面は到達目標点の情報が重要でなく、屈曲する移動経路に沿って移動し続ける軌道を探索したい場合もある。
【０００６】
当面は到達目標点に到達することが重要でなく、屈曲する移動経路に沿って移動し続ける軌道を探索したい場合、ＤＷＡ手法は使えないはずである。ＤＷＡ手法は到達目標点の情報を必要とするからである。
本発明では、屈曲する移動経路に沿って移動するための軌道を自動計算する技術を提供する。
【０００７】
本発明は、本来は到達目標点の情報を必要とするＤＷＡ手法が、到達目標点に到達することが重要でない場合にも利用可能であるという着想に立脚している。本発明は、必要時にのみ、視野内にあって検出可能な障害物の位置情報から目標点を暫定的に設定する手法を取り入れると、ＤＷＡ手法によって、屈曲する移動経路に沿って移動し続ける軌道を探索できるという着想に立脚している。
【課題を解決するための手段】
【０００８】
本明細書では、並進速度と角速度を自律的に決定し、屈曲する移動経路に沿って移動し続ける移動体を実現する技術を開示する。
その自律式移動体は、
(1) 移動体に搭載されており、移動体の周囲に存在している障害物までの距離と方位を検出する障害物検出装置と、
(2) 障害物検出装置が検出した結果から、障害物の間を伸びている移動可能経路のコーナ点の位置を算出するコーナ点位置算出装置と、
(3) コーナ点位置算出装置が算出したコーナ点の位置から、コーナ通過時に用いる目標点を設定する目標点設定装置と、
(4) 与えられた並進速度と角速度によって移動した場合に障害物に当接するまでに移動可能な距離から計算される距離評価値を計算する距離評価値計算装置と、
(5) 並進速度から計算される速度評価値を計算する速度評価値計算装置と、
(6) 距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値の合計値が最大となる並進速度と角速度を採用する並進速度と角速度の指令値決定装置を備えている。
(7) 前記(5)の速度評価値計算装置は、並進速度が高速であるほど高い速度評価値を計算する第１特性と、並進速度がコーナ通過時最適速度以下の範囲内であれば高速であるほど高い速度評価値を計算するとともに、並進速度がコーナ通過時最適速度以上の範囲内であれば高速であるほど低い速度評価値を計算する第２特性を備えており、目標点設定装置が設定した目標点までの距離が減速開始距離以下となった時に、第１特性から第２特性に切り換える特性切り換え装置を備えている。
ここでいう目標点までの距離とは、移動体の前後方向に直交する線分（左右線）上に目標点が位置する関係が得られるまでの前後方向に沿った移動距離をいう。
また本明細書でいう「以下と以上」は、数学的に厳密な意味での「以下と以上」ではなく、「未満と以上」の関係と、「以下と比較値より大きい」関係の両者を含む。
また本明細書でいう「軌跡」は経過時間の要素を含まない経路情報をいい、「軌道」は「軌跡」に経過時間の要素を加えたものをいう。
【０００９】
この自律式移動体は、障害物検出装置を備えており、周囲に存在している障害物までの距離と方位を検出する。これによって、あらかじめ地図情報を入力しておかなくても、移動可能な経路を知ることができる。
この自律式移動体は、移動経路の屈曲部（コーナ）までに距離がある間は、目標点を設定しない。目標点を設定しないために、ＤＷＡ手法で採用する偏角評価値を計算することができない。普通に考えれば、ＤＷＡ手法で採用することができないと理解される。しかしながら、この自律式移動体は、目標点を設定しないので計算できない偏角評価値を一定値に固定してしまう。すなわち、目標点に向かっているか否かを判断対象に含めないで軌道を探索する。左右の障害物の間を伸びている経路に沿って移動する場合は、左右の障害物の間を移動することが求められ、目標点に向かっているか否かは重大でないことに対応する。
この自律式移動体は、距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値の合計値が最大となる並進速度と角速度を採用する。これによって、左右の障害物の間を伸びている経路に沿って可能な限り高速に移動する軌道が探索される。距離評価値が大きな軌道を探索することから、障害物に当接しない軌道を探索することができる。
【００１０】
左右の障害物の間を伸びている経路に沿って移動するために、偏角評価値を一定値に固定するＤＷＡ手法を適用すると、移動速度が高速に過ぎて屈曲した経路を曲がりきれない軌道が探索される場合、あるいは、操舵角を変えるタイミングが早すぎて内側コーナ点に接近しすぎてしまう軌道が探索される場合が生じる。
偏角評価値を一定値に固定したＤＷＡ手法では、距離評価値×重み係数１＋速度評価値×重み係数２で計算される総合評価値が最大となる軌道を計算する。その重み係数１と２の値を適値に最適化することによって、コーナ接近時に適度に減速し、的確なタイミングで移動方向を変え始める軌道を計算するようにチューニングすることができる。しかしながら、重み係数を最適化する方法は汎用性に欠け、特定の屈曲パターンに対しては最適化することができても、他の屈曲パターンに対しては最適化されていないことが生じる。すなわち、重み係数を最適化する方法では、特定の屈曲パターンに対してはその屈曲パターンに沿って円滑に移動する軌道が計算できても、他の屈曲パターンには対応できないという問題を生じる。他の屈曲パターンに対しては、移動速度が高速に過ぎて屈曲した経路を曲がりきれないという軌道、あるいは、操舵角を変えるタイミングが早すぎて内側コーナ点に接近しすぎてそれ以上には移動方向を変えられないで立ち往生しまうという軌道が探索されてしまう。
【００１１】
本明細書で開示する自律式移動体は、偏角評価値を一定値に固定してＤＷＡ手法を適用する場合に生じる上記の問題点を解決する。
そのために、この自律式移動体は、コーナ点位置算出装置が特定したコーナ点から、コーナ通過時に用いる目標点を設定する目標点設定装置を備えている。例えば、内側コーナ点から所定距離だけ離れており、それに向かって移動体の進行方向を調整すれば、内側コーナ点に当接しないで曲がれる軌道が計算される結果をもたらす目標点を設定する。経路が非常に狭い場合には、内側コーナ点と外側コーナ点の中央に目標値を取ってもよい。なお、この目標点は、軌道計算のために一時的に用いるものであり、実際に目標点を通過する前に目標点の情報を消去してもよい。すなわち、必ずしも移動体が通過する点であるとは限らない。
この自律式移動体は、速度評価値を計算する２種類の特性を備えており、目標点までの距離が減速開始距離以下となった時に特性を切り換える。すなわち、目標点までの距離が減速開始距離以上の間は、並進速度が高速であるほど高い速度評価値を計算する第１特性に従って速度評価値を計算する。目標点までの距離が減速開始距離以下となった時に、第２特性に切り換える。すなわち、並進速度がコーナ通過時最適速度以下の範囲内では高速であるほど高い速度評価値を計算するとともに、並進速度がコーナ通過時最適速度以上の範囲内では高速であるほど低い速度評価値を計算する第２特性に従って速度評価値を計算する。
【００１２】
上記のように速度評価値の算出式を切り換えると、目標点までの距離が減速開始距離以上の間は高速で移動する軌道が探索される。目標点までの距離が減速開始距離以下となると、コーナ通過時最適速度にまで減速させる要素が強く働き、必要な減速がえられる。
本明細書で開示する自律式移動体は、コーナ部に接近するまでは高速で移動する軌道に従って移動し、コーナ部に接近するとコーナ通過時最適速度に向けて減速する。
コーナ通過時最適速度に向けて減速されるにつれて、実現可能な移動軌跡の曲率半径を小さくすることができる。この自律式移動体は、コーナ接近時にコーナ通過時最適速度に向けて減速し、内側コーナ点に当接しないで曲がれる軌跡を採用しながら、コーナを通過する。
【００１３】
本明細書で開示する技術は、基本的には偏角評価値を一定値に固定してＤＷＡ手法を適用するものであり、目標点設定装置が設定した目標点に向かう方向と移動体の進行方向との偏角から計算される偏角評価値を計算する偏角評価値計算装置は不可欠でない。しかしながら、コーナ接近時に目標点を設定することから、目標点に向かう方向と進行方向とがなす偏角を計算することが可能な期間があり、その期間のために、偏角から偏角評価値を計算する偏角評価値計算装置を備えていることが好ましい。
偏角評価値計算装置を活用する場合には、基本的ＤＷＡ手法によって、並進速度と角速度を決定することが好ましい。すなわち、距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値と、偏角評価値計算装置が計算した偏角評価値の合計値が最大となる並進速度と角速度を採用することが好ましい。
【００１４】
この場合、コーナ接近時には目標点に向けて移動体の姿勢を振る角速度が計算される。コーナを小回りしすぎることがなく、内側コーナ点に当接しないで曲がる軌跡が確実に採用される。
なお、目標点を設定している期間と、偏角評価値計算装置を活用する期間は一致しなくてもよい。目標点を設定していない期間に偏角評価値計算装置を活用することはできないことから、偏角評価値計算装置を活用する期間は目標点を設定している期間内に含まれている必要がある。目標点は、偏角評価値計算装置で利用されるだけでなく、速度評価値の計算にも用いられることから、偏角評価値計算装置を活用しない期間にまで目標点を設定することが意味を持つことがある。目標点を設定している期間のうちの一部期間でのみ、偏角評価値計算装置を活用することもある。
【００１５】
本明細書で開示する技術は、基本的には偏角評価値を一定値に固定してＤＷＡ手法を適用するものであり、基本的には目標点の設定を要しない。コーナ接近時に目標点が設定されていればよい。
その場合でも、高速で通過できる幅の広いコーナを通過する場合には、目標点を設定して減速軌道を誘導したり、移動体の指向方向を修正したりする必要がない。
そこで、移動可能経路の外側コーナ点までの距離が、狭コーナであることが判明する距離以下となった時に、目標点設定装置が目標点を設定するようにしてもよい。ここでいう外側コーナ点までの距離とは、移動体の前後方向に直交する線分上に外側コーナ点が位置する関係が得られるまでの前後方向に沿った移動距離をいう。
【００１６】
移動可能な経路の幅が広い場合、偏角評価値を一定値に固定するともに、並進速度が高速であるほど高評価するＤＷＡ手法によって、外側コーナ点までの距離を十分に確保してコーナを曲がる軌道を計算することができる。移動可能経路の幅が広い場合、目標点を設定して減速する軌道が計算されるように手配する必要もなければ、目標点を設定して移動体の指向方向を修正する必要もない。
それに対して移動経路の幅が狭い場合、内側コーナ点に当接しないためには外側コーナ点までの距離が狭くならざるをえない。外側コーナ点までの距離が所定値以下となれば、狭コーナに接近したことが判明する。この距離を狭コーナ判明距離という。
外側コーナ点までの距離が狭コーナ判明距離以下となった時にはじめて、目標点設定装置が目標値を設定するようにしてもよい。
外側コーナ点までの距離が狭コーナ判明距離以下となった時にはじめて目標値を設定するようにすると、狭コーナ通過時には強く減速し、広コーナ通過時には弱く減速する軌道が計算される。
【００１７】
目標点設定装置が目標点を設定するタイミングと、偏角評価値計算装置が偏角評価値を計算し始めるタイミングは、一致していてもよいし、前者より後者が遅れていてもよい。
すなわち、目標点設定装置が設定した目標点までの距離が偏角制御開始距離以下となった時に、偏角評価値計算装置が偏角評価値を計算し始めるようにしてもよい。狭コーナ通過時には、最初に外側コーナ点までの距離が狭コーナ判明距離以下となり、ついで、目標点までの距離が偏角制御開始距離以下となる。
広いコーナの通過時には目標点が設定されず、目標点までの距離が偏角制御開始距離以下となることがない。偏角評価値を一定値に固定したＤＷＡ手法で軌道を探索することになり、広いコーナを高速で通過する軌道が探索される。
偏角評価値計算装置は、目標点設定装置が設定した目標点までの距離が減速開始距離以下となった時に、偏角評価値の計算を終了してもよい。その後は、偏角評価値を一定値（例えばゼロ）に戻してもよい。
【００１８】
速度評価値計算装置の特性切り換え装置は、目標点設定装置が設定した目標点までの距離が減速終了距離以下となった時に、第２特性から第１特性に切り換えることが好ましい。
【００１９】
第２特性から第１特性に切り換えると、それ以降は加速する軌道に高評価値を与えながら最適軌道が探索される。
【発明の効果】
【００２０】
本明細書で開示する自律式移動体によると、コーナを曲がる際にはコーナ通過時最適速度以下であってコーナ通過時最適速度に近い速度ほど高評価値を与えるロジックによって最適軌道を計算することから、コーナ接近前の並進速度がコーナ通過時最適速度以上である場合には、コーナ接近時にコーナ通過時最適速度に向けてスムースに減速される軌道が計算される。コーナ通過時最適速度に向けて減速されることから、調整可能（採用可能）な軌跡のパターンが多様化され、狭いコーナでも走行不能状態に陥らないで通過する軌跡に沿って移動することが可能となる。移動可能な経路の幅が狭い場合には、狭い経路に沿って移動することが可能な速度にまでスムースに減速される。
請求項２の自律式移動体によると、コーナ通過時には内側コーナ点との当接が避けられる軌道をもたらす目標点に向かう姿勢に調整される。コーナ内側に向けて移動方向を変えるタイミングが早すぎることが防止される。
請求項３の自律式移動体によると、広いコーナを通過する際には目標点が設定されず、狭いコーナを通過する際には目標点が設定される。広いコーナを通過する際には、移動速度が速いほど高評価が与えられる関係を利用して軌道が計算され、狭いコーナを通過する際には、コーナ通過時最適速度に向けて減速するほど高評価が与えられる関係を利用して軌道が計算される。
請求項４の自律式移動体によると、目標点までの距離が偏角制御開始距離以下となった時に移動体の並進方向を調整する制御が採用される。偏角制御開始距離以下となった時にコーナ内側を向いている場合には、コーナ外側に向かう並進方向に調整される。小回りしすぎて内側コーナ点に当接することが防止される。
請求項５の自律式移動体によると、目標点までの距離が減速終了距離以下となった時以降は、加速する軌道に高評価値を与えながら最適軌道が計算される。コーナから離脱する際には、加速する軌道が計算される。
【図面の簡単な説明】
【００２１】
【図１】移動体と移動可能経路の一例を示す図。
【図２】移動体のハードウエア構成を示す図。
【図３】移動体の制御システムを示す図。
【図４】並進速度ｖと角速度ｗの選択範囲を概念的に示す図。
【図５】偏角評価値を説明する図。
【図６】距離評価値を説明する図。
【図７】速度評価値を説明する図。
【図８】従来技術で探索される軌道を示す図。
【図９】実施例で採用するロジックとパラメータの切り換えを例示する図。
【図１０】実施例で探索される軌道を示す図。
【図１１】経過時間に対する並進速度の変化パターンを新旧対比して示す図。
【発明を実施するための形態】
【００２２】
以下に説明する実施例の主要な特徴を列記する。
（第１特長）自律式移動体は、左右一対の駆動輪を備えており、その回転速度が独立に調整可能である。左右の回転速度の差によって、指定された角速度を実現する。
（第２特長）狭いコーナを通過する際に、下記の事象が順に発生し、下記の処理が順に実施される。
(a) 移動体から外側コーナ点までの距離が狭コーナ判明距離以下となる。そこで目標点を設定する。
(b) 移動体から目標点までの距離が偏角制御開始距離以下となる。そこで偏角評価値を計算し始める。
(c) 移動体から目標点までの距離が減速開始距離以下となる。そこで速度評価値計算装置が、第１特性から第２特性に切り換える。(d) 同時に、偏角評価値を一定値に戻す。
(e) 移動体から目標点までの距離が減速終了距離以下となる。そこで速度評価値計算装置が、第２特性から第１特性に切り換える。
(f) 移動体が目標点を通過する。そこで目標点を消去する。
（第３特長）狭いコーナを通過する際に、内側コーナ点と外側コーナ点を結ぶ線上に目標点を設定する。
（第４特長）狭いコーナを通過する際に、内側コーナ点と外側コーナ点の中点に目標点を設定する。
（第5特長）外側コーナ点までの距離が狭コーナ判明距離以下にならない広いコーナを通過する際には、目標点を設定しない。
【実施例】
【００２３】
図1は、左壁（左側障害物）５０と右壁（右側障害物）５２の間を伸びている移動可能経路Ｒと、その移動可能経路Ｒに沿って移動する自律式移動体２を示している。自律式移動体２は、左壁５０と右壁５２に当接しないで移動可能経路Ｒに沿って移動する軌道を計算し、計算された軌道に沿って移動する。自律式移動体２は、無人式でもよいし、搭乗者を移送するものであってもよい。搭乗者がいても搭乗者が操舵する必要はない。
【００２４】
図1の自律式移動体２の左右幅は0.55ｍであり、前後長は1.1ｍであり、移動可能経路Ｒの幅は1.0ｍであり、直角に屈曲しているコーナを通過する様子を示している。
自律式移動体２は、左駆動輪４と、右駆動輪６と、左前従動キャスタ８と、右前従動キャスタ１０を備えている。図２に示すように、左駆動輪４は左駆動輪モータ４ｂで駆動され、右駆動輪６は右駆動輪モータ６ｂで駆動される。左駆動輪モータ４ｂと右駆動輪モータ６ｂの回転速度は独立に調整することが可能である。左駆動輪モータ４ｂと右駆動輪モータ６ｂの間に回転速度差を与えれば、自律式移動体２は旋回中心３の周りに旋回する。
自律式移動体２に並進速度ｖと角速度ｗが指令された場合、左駆動輪モータ４ｂの回転速度ｖｌと、右駆動輪モータ６ｂの回転速度ｖｒを、下記の式（１）（２）を満たす値に調整することによって、自律式移動体２は指令された並進速度ｖと角速度ｗで移動する。
ｖ＝（ｖｌ＋ｖｒ）／２ 式（１）
ｗ＝ｖｌ−ｖｒ 式（２）
実際には、モータと車輪との間にある減速機の減速比や、車輪の直径等を加味した係数を乗算する必要があるが、これらは周知のことであり、上記式では係数を省略して表示している。
本実施例では、左駆動輪４と右駆動輪６が、移動軌跡に直交する線分上に維持される。本明細書でいう並進速度とは、左駆動輪４と右駆動輪６を結ぶ線分と移動軌跡との交点の移動速度をいう。本実施例では、左駆動輪４と右駆動輪６の回転速度を独立に調整することによって、指令された並進速度ｖと角速度ｗを実現する。これに代えて、左輪と右輪の方位を変える操舵機構を利用してもよい。この方式でも、指令された並進速度と指令された角速度に調整することができる。
【００２５】
自律式移動体２は、レーザ光の照射装置と、反射してくるレーザ光の受光装置と、照射方向と受光方向を旋回させるスキャン装置からなるスキャン式発光・受光装置１２を搭載している。スキャン式発光・受光装置１２は、水平面内を進むレーザ光を照射するとともに、移動体２の前後方向とレーザ光の照射方向との間の角度を経過時間に対して変化させる。水平面内を進むレーザ光は、移動可能面から立ち上がっている物体があれば、その物体で反射され、その反射光が受光装置で受光される。スキャン式発光・受光装置１２は、障害物までの距離とその方位（移動体２の前後方向となす角度）を検出する。スキャン式発光・受光装置１２は、レーザ光の照射方向をスキャンしながら所定時間毎に障害物までの距離とその方位を検出する。破線１４と１６は、スキャン式発光・受光装置１２による検出限界を示している。
自律式移動体２を基準とするｘ−ｙ座標系が設定されている。座標原点は、自律式移動体２の左右の駆動輪の中心位置（旋回中心）３に設定されている。ｘ軸は、自律式移動体２が前方に直進する方向に設定されている。ｙ軸は、右駆動輪６を通過している。スキャン式発光・受光装置１２が検出した障害物までの距離と方位の情報に基づいて、障害物のｘ−ｙ座標系における位置が特定される。
【００２６】
図１の黒点１００は、スキャン式発光・受光装置１２によって所定時間毎に検出された障害物の位置を示している。図示の場合、所定時間毎に検出された障害物の位置をｘ−ｙ座標系にプロットしてみると、一直線上に配置されている位置群１００ａと、他の直線上に配置されている位置群１００ｂと、さらに他の直線上に配置されている位置群１００ｃに分類される場合を例示している。
図３に示すように、自律式移動体２は、外側コーナ点・内側コーナ点位置算出部２４を備えている。コーナ点位置算出部２４は、２つの直線の交点に存在する障害物の位置ＯＵを抽出し、それを外側コーナ点ＯＵの位置とする。また、コーナ点位置算出部２４は、直線の端部に存在する障害物の位置ＩＮを抽出し、それを内側コーナ点ＩＮの位置とする。図１の破線１８は、内側コーナ点ＩＮによって制約される視野の限界を示している。障害物の位置１００ｄも直線の端部に位置しているが、これは視野の限界線上にあるので、内側コーナ点とはしない。
図３に示すように、自律式移動体２は、目標点設定部２６を備えている。目標点設定部２６は、図１に示すように、外側コーナ点ＯＵと内側コーナ点ＩＮの中間位置を目標点Ｔに設定する。
【００２７】
図２は、自律式移動体２のハードウエア構成を示し、コンピュータ２０を備えている。
左駆動輪４を駆動する左駆動輪モータ４ｂは、左駆動輪モータ用ドライバ４ａを介してコンピュータ２０に接続されており、コンピュータ２０が左駆動輪モータ４ｂの回転速度を時々刻々と制御する。左駆動輪モータ４ｂには左エンコーダ４ｃが接続されており、左エンコーダ４ｃはコンピュータ２０に接続されている。コンピュータ２０は左駆動輪モータ４ｂの回転速度を時々刻々と入力する。同様に、右駆動輪６を駆動する右駆動輪モータ６ｂは、右駆動輪モータ用ドライバ６ａを介してコンピュータ２０に接続されており、コンピュータ２０が右駆動輪モータ６ｂの回転速度を時々刻々と制御する。右駆動輪モータ６ｂには右エンコーダ６ｃが接続されており、右エンコーダ６ｃはコンピュータ２０に接続されている。コンピュータ２０は右駆動輪モータ６ｂの回転速度を時々刻々と入力する。
またスキャン式発光・受光装置１２は、スキャン式発光・受光装置ドライバ１２ａを介してコンピュータ２０に接続されている。コンピュータ２０は、スキャン式発光・受光装置１２を動かし、スキャン式発光・受光装置１２によって所定時間毎に検出される障害物までの距離と方位の情報を入力する。
【００２８】
図３は、コンピュータ２０とそのプログラムによって実現されている各種の処理機能を示している。コンピュータ２０は、所定時間毎にスキャン式発光・受光装置１２から障害物までの距離と方位の情報を入力し、所定時間毎に障害物のｘ−ｙ座標系における位置を算出する。コンピュータ２０とスキャン式発光・受光装置１２とプログラムによって、障害物検出部２２が実現される。
コンピュータ２０は、ｘ−ｙ座標系における障害物の位置から、２つの直線の交点に存在する障害物の位置ＯＵを抽出し、それを外側コーナ点ＯＵの位置とする。また、直線の端部に存在する障害物の位置ＩＮを抽出し、それを内側コーナ点ＩＮの位置とする。この処理によって外側コーナ点・内側コーナ点位置算出部２４が実現される。
コンピュータ２０は、外側コーナ点ＯＵと内側コーナ点ＩＮの中間位置Ｔを目標点Ｔに設定する。この処理によって目標点設定部２６が実現される。
【００２９】
コンピュータ２０は、並進速度ｖと角速度ｗの指令値を所定時間毎に決定する処理を実行する。この処理によって、並進速度と角速度の指令値決定部２８が実現される。
コンピュータ２０は、候補設定処理を実施し、その処理によって候補設定部３０が実現される。自律式移動体２の最大並進速度ｖは1m/secであり、ゼロ〜1m/secの間を0.1m/secの速度分解能で調整可能である。また、自律式移動体２の最大角速度ｗは0.65rad/secであり、−0.65rad/sec〜0.65rad/secの間を0.065rad/secの角速度分解能で調整可能である。図４は、横軸に並進速度ｖをとり、縦軸に角速度をとった２次元のテーブルを示しており、自律式移動体２で実現可能な並進速度ｖと角速度ｗの組み合わせの全部を示している。
左下がりのハッチの位置４０１は、現在の並進速度ｖと現在の角速度ｗを示している。矩形範囲４０２は、現在の並進速度ｖから自律式移動体２で実現可能な最大加速度で増速したときの所定時間後（本実施例では0.1sec後）の並進速度と、現在の並進速度ｖから自律式移動体２で実現可能な最大減速度で減速したときの所定時間後の並進速度と、現在の角速度ｗから自律式移動体２で実現可能な最大加速度で角速度を増速したときの所定時間後の角速度と、現在の角速度ｗから自律式移動体２で実現可能な最大減速度で角速度を減速したときの所定時間後の角速度の範囲を示している。すなわち、所定時間後（0.1sec後）に実現可能な並進速度ｖと角速度ｗの組み合わせを示している。
候補設定部３０は、矩形範囲４０２のなかから、並進速度ｖと角速度ｗの組み合わせを１個ずつ順に取り出す。
【００３０】
コンピュータ２０は、偏角から計算される評価値を計算する処理を実施し、その処理によって偏角評価値計算部３２が実現される。
図５（ａ）において、ベクトルｄは、移動体２の旋回中心位置３から目標点Ｔに向かうベクトルを示している。また、ベクトルｐは、候補設定部３０が設定した並進速度ｖと角速度ｗの組み合わせによるときの進行方向を示している。すなわち単位時間（本実施例では0.1秒）の間、並進速度をｖに維持して角速度をｗに維持した場合の位置の変化方向を示している。図５（ａ）では、単位時間だけ前には３ｘにあった移動体２が、単位時間後には３ｙに移動した場合を例示しており、ペクトルｐは３ｘから３ｙまで伸びている。θ１は、ベクトルｄとベクトルｐの偏角、すなわち、目標値に向かう方向と実際の進行方向の間に存在する偏角を示している。当然のことながら、偏角θ１がゼロに近い進行方向が好ましい。
図５（ｂ）は、偏角θ１と、偏角評価値head(v,w)の関係を示している。head(v,w)＝π―θ１の関係にあり、偏角θ１がゼロに近いほど大きな偏角評価値head(v,w)が与えられる。
【００３１】
コンピュータ２０は、障害物に当接するまでに移動可能な距離から計算される評価値を計算する処理を実施し、その処理によって距離評価値計算部３４が実現される。図６（ａ）において、参照番号６０１は、距離評価値計算時点における自律式移動体２の位置を示し、６０３は、その時点における並進速度ｖと角速度ｗを維持したときの移動軌跡を示している。黒点１０１〜１０５は、その時点で検出されている障害物の位置を示している。参照番号６０５は、移動軌跡６０３による場合に、移動体２が障害物１０１〜１０５に当接するタイミングにおける位置を示している。この場合、障害物１０１〜１０５のうち、移動体２が最初に障害物１０４に当接する場合を例示している。移動軌跡６０３の半径Ｒと、移動軌跡６０３の張る角度θ２を計算すると、移動体２が障害物１０１〜１０５のいずれかに最初に当接するまでに移動可能な距離が計算できる。
【００３２】
本実施例では、図６（ｂ）に示すように、移動軌跡６０３の張る角度θ２を計算するために、固定した移動体２に対して相対的に障害物１０１〜１０５が移動すると扱う。そのように扱うと、移動軌跡６０３が張る角度θ２と、障害物１０１〜１０５の各々の移動軌跡が張る角度θ２が等しいことがわかる。
計算をしないと、障害物１０１〜１０５のうちのどれが最初に移動体２に当接するかがわからない。また、計算しないと、移動体の前辺に当接するのか、左辺に当接するのか、右辺に当接するのかわからない。並進速度がマイナスであれば、後辺に当接する可能性もある。
【００３３】
そこで、本実施例では、障害物１０１〜１０５のそれぞれについて、移動体２に対する移動軌跡を計算し、前辺と交差するまでの角度と、左辺と交差するまでの角度と、右辺と交差するまでの角度と、後辺と交差するまでの角度を計算し、最小角度を計算する。この結果、障害物１０１〜１０５のそれぞれについて、移動体のどこかに当接するまでの角度がわかる。そこで、障害物１０１の最小角度と、障害物１０２の最小角度と、障害物１０３の最小角度と、障害物１０４の最小角度と、障害物１０５の最小角度のなかから最小のものを選び出せば、最初に移動体に当接する障害物が判る。図６（ｂ）は、最初に障害物１０４が移動体２の左辺に当接する場合を例示している。以上によって、角度θ２が計算できる。その結果、距離評価値計算時点における並進速度ｖと角速度ｗを維持したときにいずれかの障害物が移動体２のどこかに当接するまでに移動可能な距離が計算される。
【００３４】
障害物に当接するまでの移動距離が長い移動軌跡ほど好ましい。すなわち、長い移動距離を実現する並存速度ｖと角速度ｗの組み合わせほど好ましい。図６（ｃ）は、障害物に当接するまでの移動距離（Ｒ×θ２）と、距離評価値dist(v,w)の関係を示している。
図６（ｃ）に示すように、障害物に当接するまでの移動距離（Ｒ×θ２）が長い移動軌跡（並進速度ｖと角速度ｗの組み合わせで決まる）ほど、高い距離評価値dist(v,w)が与えられる。
【００３５】
コンピュータ２０は、さらに、速度評価値計算部３６を備えている。図７（１）は、並進速度ｖと速度評価値vel(v,w)の第１の関係を示している。並進速度ｖが高速なほど速度評価値vel(v,w)が高い。図７（２）は、並進速度ｖと速度評価値vel(v,w)の第２の関係を示している。図中のVTMは、コーナ通過時最適速度であり、本実施例では0.12m/secが採用されている。移動体２の最大減速度は2m/sec２であり、並進速度ｖと角速度ｗを決定する時間間隔は0.1secである。したがって、0.2m/sec以下の速度であれば、0.1sec後には停止させることができる。本実施例では、次に並進速度ｖと角速度ｗを決定するタイミングで並進速度ｖをゼロとできる範囲内に、コーナ通過時最適速度VTMをとっている。
図７（２）に示す第２の関係では、並進速度ｖがコーナ通過時最適速度VTM以下の範囲内であれば、並進速度ｖが高いほど高い速度評価値vel(v,w)をあたえる。ところが、並進速度ｖがコーナ通過時最適速度VTMを超えると、並進速度ｖが高いほど低い速度評価値vel(v,w)をあたえる。本実施例では、マイナスの評価値を与える。マイナスの評価値の絶対値が大きいほど、速度評価値は低い。
【００３６】
本実施例では、速度評価値vel(v,w)を計算するのに、２種類の関係を使いわける。検出された障害物の位置から、コーナに接近したことが判明した場合には、図７（２）の関係を利用して速度評価値vel(v,w)を計算する。その他の場合には、図７（１）の関係を利用して速度評価値vel(v,w)を計算する。コーナに接近したときの並進速度が、コーナ通過時最適速度VTMを超えていれば、コーナ通過時最適速度VTMに向けて減速する移動軌道を実現する並進速度ｖと角速度ｗが選択されるようにする。
【００３７】
コンピュータ２０によって実現される並進速度ｖと角速度ｗの指令値決定部２８は、候補設定部３０で選択された並進速度ｖと角速度ｗの組み合わせ毎に、偏角評価値head(v,w)と、距離評価値dist(v,w)と、速度評価値vel(v,w)を計算し、３つの評価値を合計した総合評価値を計算する。その処理を実現することで、総合評価値計算部３８が実現されている。並進速度ｖと角速度ｗの指令値決定部２８は、並進速度ｖと角速度ｗの組み合わせ毎に計算された総合評価値の中から、最大の評価値をもたらす並進速度ｖと角速度ｗを採用する。その処理によって、指令値決定部４０が実現されている。
実際には、総合評価値の計算のために、偏角評価値head(v,w)と、距離評価値dist(v,w)と、速度評価値vel(v,w)の各々に、重み係数α,β,γを乗じてから合計する。すなわち、
総合評価値＝α・head(v,w)＋β・dist(v,w)＋γ・vel(v,w)の式を用いる。
【００３８】
図３に示すように、コンピュータ２０は、左エンコーダ４ｃと右エンコーダ６ｃからエンコーダ情報を入手し、実際の並進速度ｖと実際の角速度ｗを検出する。自律式移動体２は、フィードバック制御部４２を備えており、左駆動輪モータ用ドライバ４ａと右駆動輪モータ用ドライバ６ａを制御し、実際の並進速度ｖと実際の角速度ｗを指令値決定部４０が決定した並進速度ｖと角速度ｗに一致させる。自律式移動体２は、指令値決定部４０が決定した並進速度ｖと角速度ｗによって移動する。
【００３９】
図８は、目標点を定めず、偏差評価値head(v,w)を一定値（この場合はゼロ）に維持し、かつ図７（１）の関係のみを利用して速度評価値vel(v,w)を計算した場合に計算される軌道を示している。図８（ａ）の場合、コーナ進入時に自律式移動体２を減速させる並進速度ｖが計算されず、コーナ進入時の移動速度が速すぎる軌道が計算された場合を例示している。この場合、正面の壁までの距離が相当程度に小さくなった場合に、距離評価値dist(v,w)が小さくなる。そこで、並進速度ｖを減速して軌跡の曲率半径を小さくすることによって、距離評価値dist(v,w)を大きくする軌道が選択され始める。しかしながら減速開始タイミングが遅すぎ、正面の壁面を避けることができない。自律式移動体２は、正面の壁に接近しすぎて移動不能となってしまう。
図８（ｂ）は、距離移動評価値dist(v,w)を大きくする軌道を選択した結果、角速度ｗを大きくして進路を変えるタイミングが早すぎる軌道が計算された場合を例示している。この場合、移動体２の右辺が内側コーナに接近しすぎ、それ以上に角速度を増大できなくなり、結局正面の壁を回避しきれない結果となってしまう。
【００４０】
総合評価値＝α・head(v,w)＋β・dist(v,w)＋γ・vel(v,w)を求める式において、係数α、β、γの関係を最適化すれば、図８（ａ）と（ｂ）の軌道を算出してしまうことを避けることができる。例えば、図８（ａ）の軌道が計算される場合、βを大きくしてγを減少させれば、より早い段階で減速し始める軌道が計算される。図８（ｂ）の軌道が計算される場合、βを小さくしてγを大きくすれば、カーブを回り始めるタイミングを遅らせることができる。しかしながら、係数α、β、γの関係を最適化する手法には限界があり、特定の形状の屈曲部をうまく曲がる軌道が計算されるようにすると、他の形状の屈曲部をうまく曲がる軌道が計算されなくなってしまう。
【００４１】
本実施例では、図９に示すように、下記の改良を加えた。
（ａ）外側コーナ点（図１のＯＵ）までの距離が２m以下になった時点で、目標点を設定する。外側コーナ点ＯＵと内側コーナ点ＩＮの間を２等分した点に目標点Ｔをおく。なお、ここでいう距離は、移動体２のｘ軸に沿った距離をいう。移動体２が正面の壁に直交する方向に移動している場合、正面の壁から移動体２の回転中心３までの距離が２mとなった時に、外側コーナ点ＯＵまでの距離が２mとなる。道幅が1mであり、移動体２の移動方向が正面の壁に直交する方向である場合、内側コーナ点ＩＮから回転中心３までの距離が１mとなった時に、外側コーナ点ＯＵまでの距離が２mとなる。移動体２の移動方向が正面の壁に直交しない方向である場合は、図１０の（ａ）に例示するように、移動体の前後方向に沿って、外側コーナ点ＯＵと旋回中心３の距離を計測する。
道幅が広い場合、外側コーナ点ＯＵまでの距離が２m以下にならないままに屈曲部を通過してしまうことがある。この場合は、目標点が設定されない。本実施例では、狭コーナ判明距離を２mとしている。外側コーナ点ＯＵまでの距離が狭コーナ判明距離以下にならなければ、目標点が設定されないから偏角評価値head(v,w)が計算されない。また、目標点までの距離によって、図７の（１）から（２）に切り換えることもない。広いコーナを通過する場合には、図７の（１）の関係で計算した速度評価値vel(v,w)と距離評価値dist(v,w)によって、内側コーナ点ＩＮの近傍を回転中心３が移動して屈曲部を通過する軌道が計算される。
【００４２】
（ｂ）目標点（図１のＴ）までの距離が1.5m以下になった時点で、偏角評価値head(v,w)の計算を開始する。この実施例では、偏角制御開始距離を1.5mとしている。目標点Ｔまでの距離が1.5m以下になって偏角評価値head(v,w)の計算を開始すると、それ以降は、目標点Ｔを指向する軌道が探索されることになる。コーナ進入時の移動体２の姿勢（移動体２が向いている方向）が、コーナを通過しやすい姿勢に矯正される。
なお前記したように、内側コーナ点（図１のＩＮ）までの距離が0mになっても外側コーナ点（図１のＯＵ）までの距離が２m以上あることがあり、この場合には目標点Ｔが設定されていないために、偏角評価値head(v,w)は計算されない。
【００４３】
（ｃ）目標点までの距離が１m以下となった時点で、速度評価値vel(v,w)の計算に用いる特性を、図７（１）から図７（２）に切り換える。目標点までの距離が１m以上の間は、図７（１）の特性が利用され、並進速度ｖが速い軌道を高評価するロジックで軌道が探索される。目標点までの距離が１m以下になると、コーナ通過時最適速度VTM以下ならばコーナ通過時最適速度VTMに近い軌道ほど高評価され、コーナ通過時最適速度VTMを超える軌道は非常に低く評価されるロジックで軌道が探索される。この結果、目標点Ｔまでの距離が１m以下となった時点の並進速度ｖがコーナ通過時最適速度VTMを超えていれば、それ以降は減速して並進速度ｖを低下させる軌道が高く評価されるようになる。本実施例では、減速開始距離を1mとしている。目標点までの距離が１m以下となった時点以降は、並進速度ｖを強く減速する軌道が優先的に探索されることになる。この実施例では、図７（２）に示すように、速度評価値が同じとなる２以上の並進速度ｖが存在しない特性に調整されており、コーナ通過時最適速度VTM以下の軌道とコーナ通過時最適速度VTM以上の軌道が交互に探索されることを防止している。
上記したように、外側コーナ点ＯＵと内側コーナ点ＩＮの間を２等分した点までの距離が１m以下となっても、外側コーナ点ＯＵまでの距離が２m以上あれば、目標点が設定されておらず、図７の（１）から（２）に切り換える処理が実行されない。
（ｄ）目標点までの距離が１m以下となった時点で、偏角評価値head(v,w)の計算を終了する。目標点までの距離が１m以下となった時点以降は、コーナ通過時最適速度VTM以下の範囲内でコーナ通過時最適速度VTMに近い速度を持つ軌道ほど高く評価され、移動可能な距離が長い軌道ほど高く評価される関係を用いることで、内側コーナ点をクリアしながらコーナを通過する軌道が探索される。偏角評価値head(v,w)を一定値に維持しても、内側コーナ点をクリアしながらコーナを通過する軌道が探索される。
【００４４】
（ｅ）目標点までの距離が0.7m以下となった時点で、速度評価値vel(v,w)の計算に用いる特性を、図７（２）から図７（１）に戻す。本実施例では、減速終了距離を0.7mとしている。（ａ）から（ｄ）の態様によって軌道を計算しながら目標点Ｔまでの距離が0.7m以下にまで接近すると、コーナ通過中の中間時点を越え、これ以降は加速してもコーナを安全に通過することができる状態となる。目標点Ｔまでの距離が0.7m以下となった時点で、速度評価値vel(v,w)の計算に用いる特性を、図７（２）から図７（１）に戻すと、並進速度が高速であるほど高い評価を与えるロジックに従って軌道が計算され、移動体の並進速度ｖが加速されていく。これ以降は加速してもコーナを安全に通過することができる状態になると、並進速度ｖが加速されていく軌道が探索される。
【００４５】
（ｆ）目標点までの距離が0mになったら、目標点の設定を解除する。前述したように、上記の距離は移動体２の前後方向に伸びるｘ軸に沿った距離であり、目標点までの距離が0m
となる位置は、旋回中心３の側方（ｙ軸方向）に目標点Ｔが位置している位置関係に等しい。その時点で、目標点Ｔの設定を解除する。
【００４６】
図９の（２）は、（ｂ）と（ｄ）の処理によって、偏角評価値がどのように計算されるかを示すものであり、（ｂ）以前と（ｄ）以降は、偏角評価値をゼロとする。偏角評価値は、最終的には総合評価値の計算に用いられ、最大総合評価値に選択に用いられる。偏角評価値がゼロでなくても、一定値であれば、ゼロと同じになる。（ｂ）〜（ｄ）の間のみ、目標値Ｔに向けた方向と移動体２の進行方向のなす偏角がゼロに近いほど高く評価するロジックにしたがって軌道が計算される。
【００４７】
図９（３）は、（ｃ）と（ｅ）の処理によって、速度評価値がどのように計算されるかを示すものであり、（ｃ）以前と（ｅ）以降は、並進速度ｖが速いほど高く評価するロジックにしたがって軌道が計算される。（ｃ）〜（ｅ）の間は、コーナ通過時最適速度VTM以下の範囲内であり、コーナ通過時最適速度VTMに近い並進速度ほど高く評価するロジックにしたがって軌道が計算される。
【００４８】
図１０は、本実施例で計算される軌道を示している。
（ａ）は、外側コーナ点ＯＵまでの距離が２mになった時点を示し、外側コーナ点ＯＵと内側コーナ点ＩＮの間を２等分した点に目標点Ｔを設定した状態を示す。なお、それ以前には目標点が設定されておらず、head(v,w)＝０であり，β・dist(v,w)＋γ・vel(v,w)の式から求められる総合評価値を最大する並進速度ｖと角速度ｗを所定時間毎に求めることで軌道を決めている。すなわち、障害物に当接するまでに移動できる距離を大きくし、しかも高速で移動する軌道ほど高く評価するロジックにしたがって、並進速度ｖと角速度ｗが決められる。（ａ）になるまでは、移動可能経路Ｒ内を障害物に衝突しないで高速で移動する軌道が探索される。
(ｂ)は、目標点Ｔまでの距離が1.5m以下になった時点を示しており、それ以降は、移動体の進行方法と目標点Ｔを向く方向とがなす偏角が小さいほど高く評価するロジックにしたがって軌道が探索される。目標点Ｔまでの距離が1.5m以下になると、移動体２の進行方向が目標点Ｔを指向する向きに変えられる。
（ｃ１）は、目標点Ｔまでの距離が1.0m以下になった時点を示しており、それ以降は、図７（２）の特性に従って速度評価値が計算される。コーナ通過時最適速度VTM以下であってコーナ通過時最適速度VTMに近い速度ほど高評価値を与えるロジックによって軌道を計算することから、目標点Ｔまでの距離が1.0m以下になると、移動体２の並進速度が速やかに減速される。
（ｃ２）は、目標点Ｔまでの距離が1.0m以下で0.7m以上の時点を示しており、十分に減速された状態を示している。十分に減速すると、小さな並進速度ｖと大きな角速度ｗの組み合わせが利用可能となり、曲率半径の小さな軌道（シャープに曲がる軌道）を採用することができる。なお、ｃ１の時点で、偏角評価値head(v,w)はゼロに戻されている。（ｂ）から（ｃ１）の間に、移動体２の進行方向が目標点Ｔを指向する向きに変えられているので、（ｃ１）以降は偏角評価値head(v,w)をゼロとしても、内側コーナ点ＩＮに当接しない軌道が計算される。
（ｅ）は、目標点Ｔまでの距離が0.7m以下になった時点を示しており、それ以降は、図７（１）の特性に従って速度評価値が計算される。移動体２の並進速度が速やかに増速される。
（ｆ）は、目標点Ｔまでの距離が0.0m以下になった時点を示しており、先に設定した目標値Ｔを消す。なお、目標点Ｔは、目標点Ｔまでの距離が0.7mになった時点以降は利用しないので、目標点Ｔまでの距離が0.7m〜0.0mのいずれかの時点で消去すればよい。
【００４９】
図１１の（Ａ）は、本実施例による場合の並進速度ｖの時間に対する変化パターンを示しており、（Ｂ）は、従来技術による場合の並進速度ｖの時間に対する変化パターンを示している。本実施例によるとコーナ進入時に減速され、小さな曲率半径の軌跡を実現できる速度でコーナに進入し、コーナ通過時の後半では増速する軌道が計算されることが確認される。従来技術によると、コーナ進入時に減速されず、コーナを通過できないで停止してしまうことがわかる。
【００５０】
本実施例では、制御に用いる特性の切り換え点等を下記のようにして決定した。
（１）コーナ通過時最適速度VTMは、最大減速度（2m/sec２）×軌道を計算する時間間隔（0.1sec）以下とした。本実施例では、余裕を見て0.12m／secとした。この場合、次の軌道計算タイミングで並進速度をゼロとすることができる。また、次の軌道計算タイミングで最小の並進速度と最大の角速度に調整して、極めて小さな曲率半径を持つ軌道に調整することを可能とする。
（２）増速運転を開始する位置（減速終了距離）は、目標点Ｔから旋回中心３までの距離が0.7mの位置とした。この位置は、移動体２の約前半分がコーナに進入した位置に相当し、その後はコーナから出る時点に相当する。移動体２の約前半分がコーナに進入した位置から、増速を重視するロジックを採用すると、コーナから速やかに脱出する軌道が計算される。
（３）図７の(２)の特性を利用して減速を重視するロジックを採用する期間は、最大並進速度（1m／sec）から最大減速度（2m/sec２）で減速して停止するまでに移動する距離（0.25m）以上とした。本実施例では、余裕を見て0.3mとした。目標点から旋回中心までの距離が0.7mとなるのに先だって0.3mの減速区間を設けると（目標点Ｔから旋回中心３までの距離が１m（減速開始距離）となった時に、図７の(２)の特性に切り換えると）、コーナ進入時の速度が十分に減速され、外側障害物にも内側コーナ点にも当接しない軌道が計算される。またその後は増速してもコーナからうまく抜け出す車両姿勢に調整される。
（４）目標点を指向する方向を重視して軌道を計算し始める位置は、目標点Ｔまでの距離が1.5m（偏角制御開始距離）の位置とした。その位置が近すぎると、移動体２の指向方向が矯正されないままにコーナに進入することになり、その位置が遠すぎると、移動体２の指向方向が矯正されすぎてしまう。後者の場合、２つの曲がり角が近距離で連続する場合に、滑らかな軌道が計算されなくなってしまう。本実施例の技術によると、２つの曲がり角が近距離で連続する場合にも、滑らかに通過する軌道が探索される。
【００５１】
以上、本発明の具体例を詳細に説明したが、これらは例示に過ぎず、特許請求の範囲を限定するものではない。特許請求の範囲に記載の技術には、以上に例示した具体例を様々に変形、変更したものが含まれる。
（１）本実施例では、目標点Ｔまでの距離が1.5〜1.0ｍの範囲では、偏角評価値head(v,w)を計算する。しかしながら、図７（１）から図７（２）に切り換えて速度評価値vel(v,w)を計算することによって、十分に減速する軌道が探索されるようにするだけで、コーナをスムースに通過する軌道が得られる環境もあり、偏角評価値head(v,w)を計算する処理を省略することもできる。
（２）本実施例では、外側コーナ点までの距離によって目標点を設定するが、経路幅が限定されている環境では、コーナ点が特定された時点で目標点を設定してもよい。
（３）本実施例では、外側コーナ点と内側コーナ点の中点に目標点を設定する。これに代えて、内側コーナ点から所定距離だけ離れた点に目標点を設定してもよい。移動体の幅を考慮して所定距離を設定すれば、内側コーナ点をクリアする軌道が計算されるようにする目標点Ｔを設定することができる。
（４）本実施例では、経路が分岐していない例で説明した。経路が分岐している場合、移動体２に「右または左」さえ指示してやれば、本実施例の技術によって、指示された側の分岐を通過する軌道を自律的に探索して移動することができる。
また、本明細書または図面に説明した技術要素は、単独であるいは各種の組合せによって技術的有用性を発揮するものであり、出願時の請求項に記載の組合せに限定されるものではない。また、本明細書または図面に例示した技術は複数の目的を同時に達成し得るものであり、そのうちの一つの目的を達成すること自体で技術的有用性を持つものである。
【符号の説明】
【００５２】
２：自律式移動体
３：旋回中心
４：左駆動輪
６：右駆動輪
８：左前従動キャスタ
１０：右前従動キャスタ
１２：スキャン式発光・受光装置
２０：コンピュータ
２２：障害物検出部
２４：外側コーナ点・内側コーナ点算出部
２６：目標点設定部
２８：並進速度と角速度の指令値決定部
３０：候補設定部
３２：偏角評価値計算部
３４：距離評価値計算部
３６：速度評価値計算部
３８：総合評価値計算部
３０：指令値決定部
４２：フィードバック制御部
４４：並進速度と角速度の検出部
【特許請求の範囲】
【請求項１】
並進速度と角速度を自律的に決定する移動体であり、
移動体に搭載されており、移動体の周囲に存在している障害物までの距離と方位を検出する障害物検出装置と、
障害物検出装置が検出した結果から、障害物の間を伸びている移動可能経路のコーナ点の位置を算出するコーナ点位置算出装置と、
コーナ点位置算出装置が算出したコーナ点の位置から、コーナ通過時に用いる目標点を設定する目標点設定装置と、
与えられた並進速度と角速度によって移動した場合に障害物に当接するまでの移動距離から計算される距離評価値を計算する距離評価値計算装置と、
並進速度から計算される速度評価値を計算する速度評価値計算装置と、
距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値の合計値が最大となる並進速度と角速度を採用する並進速度と角速度の指令値決定装置を備えており、
速度評価値計算装置が、並進速度が高速であるほど高い速度評価値を計算する第１特性と、並進速度がコーナ通過時最適速度以下の範囲内であれば高速であるほど高い速度評価値を計算するとともに並進速度がコーナ通過時最適速度以上の範囲内であれば高速であるほど低い速度評価値を計算する第２特性を備えており、目標点設定装置が設定した目標点までの距離が減速開始距離以下となった時に、第１特性から第２特性に切り換える特性切り換え装置を備えていることを特徴とする自律式移動体。
【請求項２】
目標点設定装置が設定した目標点に向かう方向と移動体の進行方向の偏角から計算される偏角評価値を計算する偏角評価値計算装置を備えており、
並進速度と角速度の指令値決定装置が、距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値と、偏角評価値計算装置が計算した偏角評価値の合計値が最大となる並進速度と角速度を採用することを特徴とする請求項１の自律式移動体
【請求項３】
目標点設定装置が、移動可能経路の外側コーナ点までの距離が狭コーナ判明距離以下となった時に、目標点を設定することを特徴とする請求項２の自律式移動体。
【請求項４】
偏角評価値計算装置が、目標点設定装置が設定した目標点までの距離が偏角制御開始距離以下となった時に、偏角評価値を計算し始めることを特徴とする請求項３の自律式移動体。
【請求項５】
特性切り換え装置が、目標点設定装置が設定した目標点までの距離が減速終了距離以下となった時に、第２特性から第１特性に切り換えることを特徴とする請求項１から４のいずれかの１項に記載の自律式移動体。
【請求項１】
並進速度と角速度を自律的に決定する移動体であり、
移動体に搭載されており、移動体の周囲に存在している障害物までの距離と方位を検出する障害物検出装置と、
障害物検出装置が検出した結果から、障害物の間を伸びている移動可能経路のコーナ点の位置を算出するコーナ点位置算出装置と、
コーナ点位置算出装置が算出したコーナ点の位置から、コーナ通過時に用いる目標点を設定する目標点設定装置と、
与えられた並進速度と角速度によって移動した場合に障害物に当接するまでの移動距離から計算される距離評価値を計算する距離評価値計算装置と、
並進速度から計算される速度評価値を計算する速度評価値計算装置と、
距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値の合計値が最大となる並進速度と角速度を採用する並進速度と角速度の指令値決定装置を備えており、
速度評価値計算装置が、並進速度が高速であるほど高い速度評価値を計算する第１特性と、並進速度がコーナ通過時最適速度以下の範囲内であれば高速であるほど高い速度評価値を計算するとともに並進速度がコーナ通過時最適速度以上の範囲内であれば高速であるほど低い速度評価値を計算する第２特性を備えており、目標点設定装置が設定した目標点までの距離が減速開始距離以下となった時に、第１特性から第２特性に切り換える特性切り換え装置を備えていることを特徴とする自律式移動体。
【請求項２】
目標点設定装置が設定した目標点に向かう方向と移動体の進行方向の偏角から計算される偏角評価値を計算する偏角評価値計算装置を備えており、
並進速度と角速度の指令値決定装置が、距離評価値計算装置が計算した距離評価値と、速度評価値計算装置が計算した速度評価値と、偏角評価値計算装置が計算した偏角評価値の合計値が最大となる並進速度と角速度を採用することを特徴とする請求項１の自律式移動体
【請求項３】
目標点設定装置が、移動可能経路の外側コーナ点までの距離が狭コーナ判明距離以下となった時に、目標点を設定することを特徴とする請求項２の自律式移動体。
【請求項４】
偏角評価値計算装置が、目標点設定装置が設定した目標点までの距離が偏角制御開始距離以下となった時に、偏角評価値を計算し始めることを特徴とする請求項３の自律式移動体。
【請求項５】
特性切り換え装置が、目標点設定装置が設定した目標点までの距離が減速終了距離以下となった時に、第２特性から第１特性に切り換えることを特徴とする請求項１から４のいずれかの１項に記載の自律式移動体。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【公開番号】特開２０１２−４８２８８（Ｐ２０１２−４８２８８Ａ）
【公開日】平成２４年３月８日（２０１２．３．８）
【国際特許分類】
【出願番号】特願２０１０−１８７０４４（Ｐ２０１０−１８７０４４）
【出願日】平成２２年８月２４日（２０１０．８．２４）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
【公開日】平成２４年３月８日（２０１２．３．８）
【国際特許分類】
【出願日】平成２２年８月２４日（２０１０．８．２４）
【出願人】（０００００３６０９）株式会社豊田中央研究所 (4,200)
【Ｆターム（参考）】
[ Back to top ]