(57)【要約】【課題】状況に応じた滑らかな移動経路を迅速に取得することができる移動経路取得装置を提供する。【解決手段】移動経路取得装置１は、予め移動経路を取得し、移動経路を再計算する移動経路取得装置であって、移動経路上に制御点を設定し制御点に関する情報を設定する制御点設定部１２と、制御点に関する情報に基づいて隣り合う制御点間の経路を円弧で近似する円弧生成部１３と、円弧を用いて移動経路を再計算する再計算部１４とを備えることで、移動経路を取得し、取得した移動経路に制御点ＰＡ、ＰＢを設け、制御点間の経路を制御点ＰＡ、ＰＢに関する情報に基づいて円弧ＰＡＰＣ及び円弧ＰＣＰＢで近似して移動経路を滑らかにすることができる。【選択図】図１
2009032185.tif 000002
【特許請求の範囲】
【請求項１】
予め移動経路を取得し、前記移動経路を再計算する移動経路取得装置であって、
前記移動経路上に制御点を設定し、前記制御点に関する情報を設定する制御点設定手段と、
前記制御点に関する情報に基づいて、隣り合う前記制御点間の経路を円弧で近似する円弧近似手段と、
前記円弧を用いて前記移動経路を再計算する再計算手段と、
を備えることを特徴とする移動経路取得装置。
【請求項２】
予め取得した前記移動経路を曲線で近似する曲線近似手段を備え、
前記制御点設定手段は、前記曲線近似手段によって近似された前記曲線を前記移動経路とすることを特徴とする請求項１に記載の移動経路取得装置。
【請求項３】
前記第１の円弧近似手段は、二つの円弧を用いて隣り合う前記制御点間の移動経路を近似することを特徴とする請求項１又は２に記載の移動経路取得装置。
【請求項４】
前記制御点設定手段は、前記制御点に関する情報として、前記制御点の位置ベクトル及び速度ベクトルを設定することを特徴とする請求項１〜３の何れか一項に記載の移動経路取得装置。
【請求項５】
前記円弧近似手段は、所定の半径より大きな半径の円弧を用いて近似することを特徴とする請求項１〜４の何れか一項に記載の移動経路取得装置。
【請求項６】
前記移動経路の制御点を結ぶ直線部分の長さに応じて、直線又は円弧を選択して前記移動経路を再計算する再計算手段を備えることを特徴とする請求項１又は５に記載の移動経路取得装置。
【請求項７】
前記再計算手段によって算出した移動経路に基づいて、障害物に衝突するか否かを判定する衝突判定手段を備え、
前記再計算手段は、前記衝突判定手段で障害物に衝突すると判定した場合には、より半径の小さい円の円弧を用いて前記移動経路を再計算すること、
を特徴とする請求項６に記載の移動経路取得装置。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動経路取得装置に関するものである。
【背景技術】
【０００２】
従来、物体の移動経路を取得する装置は、移動機器、例えば自律移動型ロボットや自動運転制御を行う車両等に好適に用いられている。このような移動機器は、入力又は生成した移動経路に基づいて目的地へ移動する（例えば、特許文献１参照）。特許文献１に記載の移動経路取得装置は、ジグザグ状の移動経路を生成し、その移動経路を複数の関数を用いて滑らかな移動経路とする。
【特許文献１】米国特許６１３４４８６号公報
【発明の開示】
【発明が解決しようとする課題】
【０００３】
しかしながら、従来の移動経路取得装置にあっては、移動速度の計算処理の負荷が大きいため、移動中に突然障害物が移動経路内に現れた場合等、突然の速度変化に対して計算処理が間に合わずに状況に応じた対応ができない恐れがある。
【０００４】
そこで本発明は、このような技術課題を解決するためになされたものであって、状況に応じた滑らかな移動経路を迅速に取得することができる移動経路取得装置を提供することを目的とする。
【課題を解決するための手段】
【０００５】
すなわち本発明に係る移動経路取得装置は、予め移動経路を取得し、前記移動経路を再計算する移動経路取得装置であって、前記移動経路上に制御点を設定し、前記制御点に関する情報を設定する制御点設定手段と、前記制御点に関する情報に基づいて、隣り合う前記制御点間の経路を円弧で近似する円弧近似手段と、前記円弧を用いて前記移動経路を再計算する再計算手段とを備えて構成される。
【０００６】
この発明によれば、移動経路を取得し、取得した移動経路に制御点を設け、隣り合う制御点間の経路をそれらの制御点に関する情報に基づいて円弧で近似して移動経路を滑らかにすることができる。これにより、単純な円弧を用いて、曲線の移動経路を制御点の状態を考慮した移動経路に再計算することができる。よって、取得する移動経路が変更された場合、その変更に対応した滑らかな移動経路を迅速に取得することができる。
【０００７】
ここで、移動経路取得装置は、予め取得した前記移動経路を曲線で近似する曲線近似手段を備え、前記制御点設定手段は、前記曲線近似手段によって近似された前記曲線を前記移動経路としてもよい。
【０００８】
また、移動経路取得装置は、前記第１の円弧近似手段は、二つの円弧を用いて隣り合う前記制御点間の移動経路を近似することが好適である。
【０００９】
このように構成することで、隣り合う制御点間の経路をそれらの制御点に関する情報に基づいて二つの円弧で近似して移動経路を滑らかにすることができる。これにより、移動経路を精度良く滑らかに近似することができる。
【００１０】
また、移動経路取得装置において、前記制御点設定手段は、前記制御点に関する情報として、前記制御点の位置ベクトル及び速度ベクトルを設定することが好適である。このように構成することで、制御点の速度を考慮して二つの円弧を決定することができるため、移動機器の速度変化が滑らかになるように移動経路を取得することができる。
【００１１】
また、移動経路取得装置において、前記円弧近似手段は、所定の半径より大きな半径の円弧を用いて近似することが好適である。このように構成することで、所定の半径を移動機器に備わる駆動部分が実際に旋回可能な半径に設定して、移動機器の駆動能力に対応した円弧状の移動経路を生成できる。これにより、移動機器の駆動能力に対応した移動経路を迅速に生成することができる。
【００１２】
また、移動経路取得装置は、前記移動経路の制御点を結ぶ直線部分の長さに応じて、直線又は円弧を選択して前記移動経路を再計算する再計算手段を備えることが好適である。
【００１３】
このように構成することで、移動経路を取得し、取得した移動経路の制御点を結ぶ直線部分の長さに応じて直線又は円弧によって近似することができる。例えば、ジグザグ状の移動経路を入力し、所定の区間の移動経路が短い場合には、複数の円弧を組み合わせて移動経路を近似し、所定の区間の移動経路が長い場合には、直線と円弧とを組み合わせて移動経路を近似することができる。これにより、ジグザグ状の移動経路を円弧と直線からなる滑らかな移動経路に迅速に再計算することができる。
【００１４】
さらに、移動経路取得装置は、前記再計算手段によって算出した移動経路に基づいて、障害物に衝突するか否かを判定する衝突判定手段を備え、前記再計算手段は、前記衝突判定手段で障害物に衝突すると判定した場合には、より半径の小さい円の円弧を用いて前記移動経路を再計算することが好適である。
【００１５】
このように構成することで、近似した移動経路を用いて移動機器が走行した場合に障害物に衝突するか否かを判定し、衝突すると判定した場合には、より半径の小さい円の円弧を用いて、移動経路を再度近似することができる。これにより、障害物に衝突する移動経路となることを回避できるので、安全な移動経路を算出することができる。
【発明の効果】
【００１６】
本発明によれば、状況に応じた滑らかな移動経路を迅速に取得することができる。
【発明を実施するための最良の形態】
【００１７】
以下、添付図面を参照して本発明の実施形態について説明する。なお、図面の説明において同一の要素には同一の符号を付し、重複する説明を省略する。
【００１８】
（第１実施形態）
図１は、第１実施形態に係る移動経路取得装置１を備えたロボット２の構成図である。本実施形態に係る移動経路取得装置１は、移動機器の道順が示された移動経路情報を取得し、取得した移動経路情報を周辺環境に応じてリアルタイムに更新する装置であって、例えばロボットや車両の自動走行制御などに好適に用いられる装置である。
【００１９】
移動経路取得装置１が備わるロボット２は、周辺の環境を認識するためのカメラ３、ロボット２の動作を制御するためのＥＣＵ６、ＥＣＵ６により制御される電動モータ８、電動モータ８により駆動される車輪９、通信を行うための通信部７、地図ＤＢ４及び経路情報ＤＢ５を備えている。ここで、ＥＣＵ（Electronic  Control  Unit）とは、電子制御するコンピュータであり、ＣＰＵ（Central  Processing  Unit）、ＲＯＭ（Read  Only  Memory）、ＲＡＭ（Random  Access  Memory）、および入出力インターフェイスなどを備えて構成されている。
【００２０】
カメラ３は、ロボット２が進行する方向の画像を取得するＣＣＤカメラと、取得した画像を画像処理することによって、障害物の有無を認識する画像処理部（不図示）とを有している。この画像処理部は、ＣＣＤカメラで取得した画像からエッジ抽出やパターン認識処理などによって障害物を抽出して認識する機能を有している。また、カメラ3は、取得した情報をＥＣＵ６へ出力する機能を有している。
【００２１】
地図ＤＢ４は、地図情報が格納されたデータベースであり、ＥＣＵ６から参照可能に構成されている。また、経路情報ＤＢ５は、ロボット２が走行する予定の大まかな経路が格納されているデータベースであり、ＥＣＵ６から参照可能に構成されている。経路情報ＤＢ５に格納されている経路情報は、例えば出発地点から目的地点までの移動経路であり、さらに移動経路上の所定地点での目標速度情報や目標到達時刻情報を含んでいる。
【００２２】
通信部７は、外部と通信し、自動走行に必要な情報を入力する機能を有している。例えば通信部７は、ロボット２の現在地点の座標を入力する機能を有している。また、通信部７は、入力した情報をＥＣＵ６へ出力する機能を有している。
【００２３】
ＥＣＵ６は、移動経路生成部６１、アクチュエータ制御部６２及び移動経路再計算処理部（移動経路取得装置）１を備えている。
【００２４】
移動経路生成部６１は、ロボット２が移動する大まかな経路を生成又は入力する機能を有している。例えば、出発地点及び目標地点、経路情報ＤＢ５から入力した経路情報、及び地図ＤＢ４から入力した地図情報を用いて大まかな経路情報を生成する機能を有している。出発地点、目標地点及び到着時刻等は、例えば、通信部７を介して入力される。経路情報が示す移動経路は、直線経路が連続して構成され、ジグザグ状の形状となる。移動経路生成部６１は、移動経路の屈折箇所の位置情報を抽出する機能を有している。また、移動経路生成部６１は、生成した経路情報、カメラ３から入力した障害物に関する情報、通信部７から入力した現在位置情報及び地図ＤＢ４から入力した地図情報等を用いて、走行中に障害物を認識した場合に障害物を避けるために経路情報を変更する機能を有している。さらに、移動経路生成部６１は、経路情報を移動経路再計算処理部１へ出力する機能を有している。
【００２５】
移動経路再計算処理部１は、経路情報を取得して再計算する機能を有しており、曲線近似部（曲線近似手段）１１、制御点設定部（制御点設定手段）１２、円弧生成部（円弧近似手段）１３、再計算部（再計算手段）１４を備えて構成される。
【００２６】
曲線近似部１１は、移動経路生成部６１から経路情報を入力し、移動経路を十分滑らかな曲線に近似する機能を有している。曲線近似部１１は、入力したジグザグ状の移動経路の屈折箇所における位置情報に基づいて、移動経路を曲線に近似する。ここでは、計算負荷の少ない公知の近似手段が用いられ、例えば、ノンパラメトリック近似が用いられる。また、曲線近似部１１は、算出した曲線の経路情報を制御点設定部１２へ出力する機能を有している。
【００２７】
制御点設定部１２は、経路上に制御点を設定し、設定した制御点に関する情報を設定する機能を有している。制御点設定部１２は、例えば、曲線の移動経路上を通る点を制御点として設定し、その制御点の位置ベクトル、及びロボット２がその制御点を通過する際の速度ベクトルを制御点に関する情報として設定する。位置ベクトル及び速度ベクトルは、経路情報から取得した値が設定される。また、制御点設定部１２は、設定した情報を円弧生成部１３へ出力する機能を有している。
【００２８】
円弧生成部１３は、二つの地点を滑らかに結ぶ円弧を生成する機能を有している。円弧生成部１３は、与えられた二つの地点のうち、一方の地点を通る円と他方の地点を通る円とを所定の条件に基づいて算出し、算出した二つの円の円弧を用いて二つの地点を滑らかに結ぶ曲線を生成する機能を有している。また、円弧生成部１３は算出した曲線に関する情報を再計算部１４へ出力する機能を有している。
【００２９】
再計算部１４は、円弧生成部１３から入力した曲線に関する情報に基づいて、経路情報を再計算する機能を有している。また、再計算した経路情報をアクチュエータ制御部６２へ出力する機能を有している。
【００３０】
アクチュエータ制御部６２は、再計算部１４から経路情報を入力し、入力した経路情報に基づいて電動モータ８へ制御信号を出力する機能を有している。電動モータ８は、アクチュエータ制御部６２から制御信号を入力し、制御信号に基づいて車輪９を駆動させる機能を有している。
【００３１】
このように構成されたロボット２は、与えられた経路情報に沿って走行し、ロボット２の進行方向に障害物がある場合には、障害物を避けるようにジグザグ状の経路を変更し、変更されたジグザグ状の経路を滑らかな曲線に近似し、近似した曲線を速度情報に基づいて、滑らかでかつ移動機器が移動可能な移動経路を再計算し、再計算した移動経路に基づいて走行する。
【００３２】
次に、本実施形態に係る移動速度取得装置１の動作について、図２〜４を用いて説明する。図２は、本実施形態に係る移動速度取得装置１を備えたロボット２の動作を示すフローチャート、図３及び４は、再計算した移動経路を示す概要図である。
【００３３】
移動速度取得装置１を備えるロボット２は、目的地に向かって出発したタイミングで図２に示す制御処理を実行し、所定の間隔で図２に示す制御処理を繰り返し実行する。
【００３４】
ロボット２は、情報入力処理から開始する（Ｓ１０）。Ｓ１０の処理は、移動経路生成部６１で実行され、移動経路を生成するための情報を入力する処理である。移動経路生成部６１は、カメラ３から障害物情報を、地図ＤＢ４から地図情報を、経路情報ＤＢ５から経路情報を、通信部７から目的地や目標到着時刻、現在地等を入力する。Ｓ１０の処理が終了すると、移動経路計画処理へ移行する（Ｓ１２）。
【００３５】
Ｓ１２の処理は、移動経路生成部６１で実行され、移動経路を計画する処理である。移動経路生成部６１は、Ｓ１０の処理で得られた情報に基づいて、図２の（ａ）に示すようなジグザグ状の移動経路を生成する。また、移動経路生成部６１は、生成した移動経路の屈折箇所の座標を抽出する。Ｓ１２の処理が終了すると、移動経路の平滑化処理へ移行する（Ｓ１４）。
【００３６】
Ｓ１４の処理は、曲線近似部１１で実行され、ジグザグ状の移動経路を平滑化し、曲線とする処理である。曲線近似部１１は、Ｓ１２の処理で入力した移動経路の屈折箇所の座標に基づいて、例えばノンパラメトリック近似を用いてジグザグ状の移動経路を図２の（ｂ）に示すような曲線に近似する。この曲線は、ロボット２の速度情報や、ロボット２の移動能力の制約等は考慮されずに生成される。Ｓ１４の処理が終了すると、速度情報設定処理へ移行する（Ｓ１６）。
【００３７】
Ｓ１６の処理は、制御点設定部１２で実行され、移動経路上に制御点を設定し、設定した制御点に対応する速度情報を設定する処理である。制御点設定部１２は、Ｓ１４の処理で近似した曲線上に制御点を設定し、設定した制御点の位置情報に対応した速度情報を経路情報ＤＢ５から入力し、制御点の速度情報を設定する。Ｓ１６の処理が終了すると、移動経路の平滑化処理へ移行する（Ｓ１８）。
【００３８】
Ｓ１８の処理は、円弧生成部１３及び再計算部１４で実行され、曲線の移動経路をロボット２の駆動能力及び速度情報を考慮して平滑化する処理である。円弧生成部１３は、Ｓ１６の処理で設定した制御点の位置ベクトル及び速度ベクトルに基づいて、２つの制御点を結ぶ曲線を生成する。この曲線は、例えば、２つの制御点のうち、一方の制御点を通る円の円弧と他方の制御点を通る円の円弧を繋ぐことによって形成される。以下では、二つの制御点を結ぶ曲線の生成方法について説明する。例えば、図３に示すように、制御点ＰＡ及び制御点ＰＢを結ぶ曲線を生成するとする。制御点の位置ベクトルをＰＡ及びＰＢ、制御点ＰＡの通過速度を示す速度ベクトルｔＡ、制御点ＰＢの通過速度を示す速度ベクトルｔＢとし、制御点ＰＡを通る半径ｒＡの円をＣＡ、制御点ＰＢを通る半径ｒＢの円をＣＢ、円ＣＡと円ＣＢとの交点をＰＣ、円ＣＡの制御点ＰＡにおける接線と円ＣＡの交点ＰＣにおける接線とが交差する交点をＰ´Ａ、円ＣＢの制御点ＰＢにおける接線と円ＣＢの交点ＰＣにおける接線とが交差する交点をＰ´Ｂ、ロボット２全体の速度ベクトルをＶ、所定の変数をα及びβとすると、円ＣＡ及び円ＣＢは以下に示す式（１）〜（７）を満たすように設定する。
【数１】
2009032185.tif 000003
【数２】
2009032185.tif 000004
【数３】
2009032185.tif 000005
【数４】
2009032185.tif 000006
【数５】
2009032185.tif 000007
【数６】
2009032185.tif 000008
【数７】
2009032185.tif 000009
【００３９】
ここで、上記式を満たす円ＣＡ及び円ＣＢは、以下式（８）の条件を満たすように設定することで一義的に決定される。
【数８】
2009032185.tif 000010
【００４０】
なお、円ＣＡ及び円ＣＢのそれぞれの半径ｒＡ及びｒＢは、所定値以上の半径となるように設定する。所定値は、ロボット２の旋回可能半径であり、駆動輪９等の駆動系機器によって決定される値である。
【００４１】
このようにして算出した円ＣＡの円弧ＰＡＰＣ及び円ＣＢの円弧ＰＣＰＢを用いて、制御点ＰＡＰＢ間の経路を近似することができる。図３においては、円ＣＡ及び円ＣＢが互いに重なった状態となるが、上記の式の計算結果によっては、図４に示すように円ＣＡ及び円ＣＢが互いに重ならない状態となる場合もある。このような場合は、移動経路が円弧ＰＡＰＣ及び円弧ＰＣＰＢを結んだＳ字形状となる。
【００４２】
図２の制御処理に戻り、Ｓ１８の処理、すなわち移動経路の平滑化処理が終了すると、制御信号の生成処理へ移行する（Ｓ２０）。
【００４３】
Ｓ２０の処理は、アクチュエータ制御部６２で実行され、Ｓ２０の処理で再計算し平滑化した移動経路に基づいて、アクチュエータを制御する処理である。アクチュエータ制御部６２は、再計算し平滑化した移動経路に基づいて、例えば電動モータ８の制御信号を生成し、車輪９を動作させる。Ｓ２０の処理が終了すると、図２に示す制御処理を終了する。
【００４４】
以上のように、第１実施形態に係る移動経路取得装置１によれば、移動経路を取得し、取得した移動経路に制御点ＰＡ、ＰＢを設け、制御点間の経路を制御点ＰＡ、ＰＢに関する情報に基づいて円弧ＰＡＰＣ及び円弧ＰＣＰＢで近似して移動経路を滑らかにすることができる。これにより、単純な円弧を用いて、曲線の移動経路を制御点ＰＡ、ＰＢの状態を考慮した移動経路に再計算することができる。よって、取得する移動経路が変更された場合、その変更に対応した滑らかな移動経路を迅速に取得することができる。
【００４５】
また、第１実施形態に係る移動経路取得装置１によれば、制御点設定部１２が曲線近似部１１によって近似された曲線上に制御点を設けることができるので、より迅速に滑らかな移動経路を生成することができる。
【００４６】
また、第１実施形態に係る移動経路取得装置１によれば、円弧生成部１３が二つの円弧ＰＡＰＣ及び円弧ＰＣＰＢを用いて隣り合う制御点間の移動経路を近似することができるので、移動経路を精度良く滑らかに近似することができる。
【００４７】
また、第１実施形態に係る移動経路取得装置１によれば、制御点の位置ベクトルＰＡ、ＰＢ及び速度ベクトルｔＡ、ｔＢを設定し、制御点ＰＡ、ＰＢの速度を考慮して二つの円弧を決定することができるため、ロボット２の速度変化が滑らかになるように移動経路を取得することができる。
【００４８】
さらに、第１実施形態に係る移動経路取得装置１によれば、ロボット２に備わる駆動部分である車輪９等が実際に旋回可能な半径を持つ円ＣＡ、ＣＢを用いて円弧を算出することで、ロボット２の駆動能力に対応した移動経路を迅速に生成することができる。
【００４９】
（第２実施形態）
図５は、第２実施形態に係る移動経路取得装置１０を備えたロボット２の構成図である。本実施形態に係る移動経路取得装置１０は、移動機器の道順が示された移動経路情報を取得し、取得した移動経路情報を周辺環境に応じてリアルタイムに更新する装置であって、第１実施形態に係る移動経路取得装置１とほぼ同様に構成される。よって、第１実施形態と同様の構成については同様の符号を付して説明を省略し、相違点について詳細に説明する。
【００５０】
ＥＣＵ６に備わる移動経路再計算処理部（移動経路取得装置）１０は、第１実施形態の移動経路取得装置１が備える曲線近似部１１を備えておらず、衝突判定部（衝突判定手段）１５を備える点で相違する。また、移動経路取得装置１０に備わる円弧生成部１３及び再計算部１４は、移動経路取得装置１に備わる円弧生成部１３及び再計算部１４と比べて入出力先及び一部機能が相違する。
【００５１】
制御点設定部１２は、経路情報生成部６１からジグザグ状の移動経路における屈折箇所の座標を入力する機能を備えている。また、入力した座標を制御点として設定し、円弧生成部１３へ出力する機能を有している。
【００５２】
円弧生成部１３は、制御点設定部１２から入力した制御点の座標に基づいて、屈折箇所間の移動経路を滑らかな円弧で近似する機能を備えている。さらに、後述する衝突判定部１５の判定結果に基づいて、より半径が小さい円からなる円弧を生成する機能を有している。また、算出した円弧に関する情報を衝突判定部１５へ出力する機能を備えている。
【００５３】
衝突判定部１５は、円弧生成部１３が算出した滑らかな移動経路を入力し、ロボット２が移動経路に沿って走行すると障害物に衝突するのか否かを判定する機能を有している。例えば、衝突判定部１５は、円弧生成部１３が生成した円の半径に基づいて、障害物と衝突するか否かを判定する機能を有している。衝突判定部１５は、衝突するか否かの判定結果を円弧生成部１３へ出力する機能を有している。また、衝突判定部１５は、再計算処理部１４から入力した経路情報に基づいて、障害物と衝突するか否かを判定する機能を有している。
【００５４】
再計算部１４は、円弧生成部１３から入力した円弧に関する情報、及び屈折箇所の座標に基づいて、経路情報を再計算する機能を有している。また、再計算した経路情報をアクチュエータ制御部６２へ出力する機能を有している。
【００５５】
このように構成されたロボット２は、第１実施形態と同様に、与えられた経路情報に沿って走行し、ロボット２の進行方向に障害物がある場合には、障害物を避けるようにジグザグ状の経路を変更し、変更されたジグザグ状の経路を滑らかでかつ移動機器が走行可能な移動経路へ再計算し、再計算した移動経路に基づいて走行する。
【００５６】
次に、本実施形態に係る移動経路取得装置１０の動作について、図６〜１４を用いて説明する。図６〜８は、本実施形態に係る移動経路取得装置１０を備えたロボット２の動作を示すフローチャート、図９〜１４は、再計算した移動経路を示す概要図である。
【００５７】
移動速度取得装置１０を備えるロボット２は、目的地に向かって出発したタイミングで図６に示す制御処理を実行し、所定の間隔で図６に示す制御処理を繰り返し実行する。
【００５８】
ロボット２は、情報入力処理から開始する（Ｓ３０）。Ｓ３０の処理は、移動経路生成部６１で実行され、移動経路を生成するための情報を入力する処理である。移動経路生成部６１は、カメラ３から障害物情報を、地図ＤＢ４から地図情報を、経路情報ＤＢ５から経路情報を、通信部７から目的地や目標到着時刻、現在地等を入力する。Ｓ３０の処理が終了すると、移動経路計画処理へ移行する（Ｓ３２）。
【００５９】
Ｓ３２の処理は、移動経路生成部６１で実行され、移動経路を計画する処理である。移動経路生成部６１は、Ｓ３０の処理で得られた情報に基づいて、図６の（ａ）に示すようなジグザグ状の移動経路を生成する。また、移動経路生成部６１は、生成した移動経路の屈折箇所の座標を抽出する。Ｓ３２の処理が終了すると、移動経路の平滑化処理へ移行する（Ｓ３４）。
【００６０】
Ｓ３４の処理は、ジグザグ状の移動経路を平滑化する処理である。ここで、Ｓ３４の処理を図７及び８に示すフローチャートを用いて説明する。図７及び８に示すフローチャートは、図６に示すＳ３４の処理の詳細であり、Ｓ３２の処理で入力した屈折箇所の全てが処理し終わるまで繰り返し実行される。
【００６１】
図７に示すように、移動経路取得装置１０は、移動経路の平滑化のために制御点情報の入力処理から開始する（Ｓ５０）。
【００６２】
Ｓ５０の処理は、制御点設定部１２で実行され、屈折箇所の座標を制御点の座標として入力する処理である。以下では、平滑化対象の移動経路における制御点の総数がＮ個であり、経路先頭からｉ番目（ｉは１からＮ−２までの自然数）の制御点の位置ベクトルをＰｉとして説明する。Ｓ５０の処理が終了すると、制御点選択処理へ移行する（Ｓ５２）。
【００６３】
Ｓ５２の処理は、制御点設定部１２で実行され、制御点同士を直線で結んだジグザグ状の経路において、移動経路上に連続する三つの制御点を選択する処理である。制御点設定部１２は、制御点Ｐｉ、制御点Ｐｉ＋１及び制御点Ｐｉ＋２の三つの制御点を選択する。例えば、制御点ＰＡ、制御点ＰＢ及び制御点ＰＣがこの順で連続して移動経路上に並んでいるとすると、制御点ＰＡを制御点Ｐｉ、制御点ＰＢを制御点Ｐｉ＋１、制御点ＰＣを制御点Ｐｉ＋２として選択する。以下では、制御点Ｐｉと制御点Ｐｉ＋１とに挟まれた線分ＰｉＰｉ＋１（線分ＰＡＰＢ）の長さをｌＡ、制御点Ｐｉ＋１と制御点Ｐｉ＋２とに挟まれた線分Ｐｉ＋１Ｐｉ＋２（線分ＰＢＰＣ）の長さをｌＢとする。Ｓ６２の処理が終了すると、第１の円生成処理へ移行する（Ｓ６４）。
【００６４】
Ｓ５４の処理は、円弧生成部１３で実行され、三つの制御点から構成される二つの線分に接する第１の円Ｃｉを算出し、円弧で移動経路を近似する処理である。すなわち、線分ＰＡＰＢ及び線分ＰＢＰＣが共通接線になるように第１の円Ｃｉを設定する。このとき、第１の円Ｃｉの半径は最大となるように設定する。例えば、ｌＡがｌＢより小さい場合には、線分ＰＡＰＢと第１の円Ｃｉとの接点を制御点ＰＡとし、線分ＰＢＰＣと第１の円Ｃｉとの接点を点ＰＡ´として、以下式（９）を満たすように点ＰＡ´を設定する。
【数９】
2009032185.tif 000011
【００６５】
式（９）を満たす場合、例えば図９に示すような円Ｃｉを設定して、線分ＰＡＰＢ及び線分ＰＢＰＣからなるジグザグ状の移動経路を滑らかな円弧ＰＡＰＡ´、円弧ＰＡ´ＰＣで近似することができる。
【００６６】
一方、例えば、ｌＡがｌＢより小さくない場合には、線分ＰＢＰＣと円Ｃｉとの接点を制御点ＰＣとし、線分ＰＡＰＢと円Ｃｉとの接点を点ＰＡ´として、以下式（１０）を満たすように点ＰＡ´を設定する。
【数１０】
2009032185.tif 000012
【００６７】
式（１０）を満たす場合、例えば図１０に示すような円Ｃｉを設定して、線分ＰＡＰＢ及び線分ＰＢＰＣからなるジグザグ状の移動経路を滑らかな円弧ＰＡＰＡ´、円弧ＰＡ´ＰＣで近似することができる。Ｓ５４の処理が終了すると、衝突判定処理へ移行する（Ｓ５６）。
【００６８】
Ｓ５６の処理は、衝突判定部１５で実行され、Ｓ５４の処理で設定した円の一部である円弧に沿ってロボット２が走行したとすると、障害物に衝突するか否かを判定する処理である。衝突するか否かの判定は、例えば生成した円が障害物と重なるか否かによって判定される。Ｓ５６の処理で、例えば図１１に示すように、Ｓ５４の処理で生成した円Ｃｉが障害物Ｏと重ならないと判定した場合には、生成した円Ｃｉの円弧ＰＡＰＡ´で近似した移動経路をロボット２が走行しても安全であるとして、次の制御点選択処理へ移行する（図８に示すＳ６４）。
【００６９】
図８に示すＳ６４の処理は、制御点設定部１２で実行され、Ｓ５２の処理と同様に制御点同士を直線で結んだジグザグ状の経路において、移動経路上に連続する三つの制御点を選択する処理である。Ｓ５２の処理で選択した制御点を制御点Ｐｉ、制御点Ｐｉ＋１、制御点Ｐｉ＋２とすると、Ｓ６４の処理では、制御点Ｐｉ＋１、制御点Ｐｉ＋２及び制御点Ｐｉ＋３を選択する。例えば、図１１に示すように、制御点ＰＢ、制御点ＰＣに続いて、制御点ＰＤが連続して移動経路上に並んでいるとすると、制御点ＰＤを制御点Ｐｉ＋３として選択する。以下では、制御点Ｐｉ＋２と制御点Ｐｉ＋３とに挟まれた線分Ｐｉ＋２Ｐｉ＋３（線分ＰＣＰＤ）の長さをｌＣとする。Ｓ６４の処理が終了すると、第２の円生成処理へ移行する（Ｓ６６）。
【００７０】
Ｓ６６の処理は、円弧生成部１３で実行され、Ｓ６４の処理で選択した三つの制御点から構成される二つの線分に接する第2の円Ｃｉ＋１を算出し、円弧で移動経路を近似する処理である。すなわち、線分ＰＢＰＣ及び線分ＰＣＰＤが共通接線になるように円Ｃｉ＋１を設定する。設定方法は、Ｓ５４の処理と同様である。Ｓ６６の処理が終了すると、衝突判定処理へ移行する（Ｓ６８）。
【００７１】
Ｓ６８の処理は、衝突判定部１５で実行され、Ｓ６６の処理で設定した円の一部である円弧に沿ってロボット２が走行したとすると、障害物に衝突するか否かを判定する処理である。衝突するか否かの判定は、例えば生成した円が障害物と重なるか否かによって判定される。Ｓ６８の処理で、Ｓ６６の処理において生成した第２の円Ｃｉ＋１が障害物Ｏと重ならないと判定した場合には、円弧で近似した移動経路をロボット２が走行しても安全であるとして、重なり判定処理へ移行する（Ｓ７６）。
【００７２】
Ｓ７６の処理は、再計算部１４で実行され、Ｓ５４の処理で算出した第１の円Ｃｉと、Ｓ６６の処理で算出した第２の円Ｃｉ＋１とが重なるか否かを判定する処理である。Ｓ７６の処理で、第１の円Ｃｉと第２の円Ｃｉ＋１とが重なると判定した場合には、第３及び第４の円生成処理へ移行する（Ｓ７８）。
【００７３】
Ｓ７８の処理は、円弧生成部１３で実行され、第１の円Ｃｉと第２の円Ｃｉ＋１とが重なる場合において、第１の円Ｃｉの円弧と第２の円Ｃｉ＋１の円弧とを滑らかに接続する条件を満たす第３の円Ｃｉ´及び第４の円Ｃｉ＋１´を生成する処理である。例えば、第３の円Ｃｉ´は、線分ＰＡＰＢ及び線分ＰＢＰＣが共通接線となるように設定し、線分ＰＢＰＣとの接点をＩ´とすると、第４の円Ｃｉ＋１´は、線分ＰＢＰＣの接点をＩ´として、且つ線分ＰＣＰＤに接するように設定する。すなわち、第４の円Ｃｉ＋１´と線分ＰＣＰＤとが接する接点を制御点ＰＤ´´とすると、以下式を満たすように設定する。
【数１１】
2009032185.tif 000013
【００７４】
式（１１）を満たす場合、例えば図１２に示すような第３の円Ｃｉ´及び第４の円Ｃｉ＋１´を設定することができる。Ｓ７８の処理が終了すると、近似処理へ移行する（Ｓ８０）。
【００７５】
Ｓ８０の処理は、再計算部１４で実行され、算出した円を組み合わせてジグザグ状となっている移動経路部分を滑らかな曲線に近似する処理である。Ｓ５４の処理で生成した第１の円Ｃｉ、Ｓ６６の処理で生成した第２の円Ｃｉ＋１、Ｓ７８の処理で生成した第３の円Ｃｉ´及び第４の円Ｃｉ＋１´を組み合わせて、例えば図１２に示すように、線分ＰＡＰＢ、線分ＰＢＰＣ及び線分ＰＣＰＤからなるジグザグ状の経路を、円弧ＰＡＰＡ´´、円弧ＰＡ´´Ｉ´、円弧Ｉ´ＰＤ´´及び円弧ＰＤ´´ＰＤからなる経路に近似する。Ｓ８０の処理が終了すると、変更箇所の記録処理へ移行する（Ｓ８２）。
【００７６】
Ｓ８２の処理は、ＥＣＵ６で実行され、ジグザグ状の経路を曲線で近似した箇所を記録する処理である。例えば、ＥＣＵ６が備えるメモリ上に記録する。Ｓ８２の処理が終了すると、図７及び図８に示す制御処理を終了する。
【００７７】
一方、図７に示すＳ５６の処理において、例えば図１３に示すように、第１の円Ｃｉの円弧で近似した移動経路をロボット２が走行すると障害物Ｏに衝突すると判定した場合には、制御点変更処理へ移行する（Ｓ５８）。Ｓ５８の処理は、制御点設定部１２で実行され、Ｓ５４の処理で生成した円Ｃｉをより半径の小さい円Ｃｉ´´に再生成するために、制御点を変更する処理である。例えば、以下式を満たすような制御点に変更する。
【数１２】
2009032185.tif 000014
【００７８】
式（１２）で示すように制御点を小さくすることで、連続する二つの線分に接する円の半径を小さくすることができる。Ｓ５８の処理が終了すると、限界値確認処理へ移行する（Ｓ６０）。
【００７９】
Ｓ６０の処理は、制御点設定部１２で実行され、Ｓ５８の処理回数が許容できる範囲か否かを判定する処理である。例えば、Ｓ５８の処理が所定の回数、すなわち限界値より多く繰り返したか否かを確認する。Ｓ６０の処理において、設定した限界値より多く計算処理をしていないと判定した場合には、第１の円生成処理へ移行し、Ｓ５８で設定した制御点に基づいて、再び第１の円の生成処理を行う（Ｓ５４）。一方、Ｓ６０の処理において、設定した限界値より多く計算処理をしたと判定した場合には、現状維持処理へ移行する（Ｓ６２）。
【００８０】
Ｓ６２の処理は、ＥＣＵ６で実行され、近似する対象の制御点間の経路が円弧で近似することができないため、その制御点間の経路の近似を行わないことを決定する処理である。Ｓ６２の処理が終了すると、図７及び図８の制御処理を終了する。
【００８１】
一方、図８に示すＳ６８の処理において、第２の円Ｃｉ＋１の円弧で近似した移動経路をロボット２が走行すると障害物Ｏに衝突すると判定した場合には、制御点変更処理へ移行する（Ｓ７０）。Ｓ７０の処理は、制御点設定部１２で実行され、Ｓ６８の処理で生成した第２の円Ｃｉ＋１をより半径の小さい円に再生成するために、制御点を変更する処理である。処理内容は、図７に示すＳ５８の処理と同様である。Ｓ７０の処理が終了すると、限界値確認処理へ移行する（Ｓ７２）。
【００８２】
Ｓ７２の処理は、制御点設定部１２で実行され、Ｓ７０の処理回数が許容できる範囲か否かを判定する処理である。例えば、Ｓ７０の処理が所定の回数、すなわち限界値より多く繰り返したか否かを確認する。Ｓ７２の処理において、設定した限界値より多く計算処理をしていないと判定した場合には、第２の円生成処理へ移行し、Ｓ７０で設定した制御点に基づいて、再び第２の円の生成処理を行う（Ｓ６６）。一方、Ｓ７２の処理において、設定した限界値より多く計算処理をしたと判定した場合には、現状維持処理へ移行する（Ｓ７４）。
【００８３】
Ｓ７４の処理は、ＥＣＵ６で実行され、近似する対象の制御点間の経路が円弧で近似することができないため、その制御点間の経路の近似を行わないことを決定する処理である。Ｓ７４の処理が終了すると、図７及び図８の制御処理を終了する。
【００８４】
また、Ｓ７６の処理において、Ｓ５４の処理で算出した第１の円Ｃｉと、Ｓ６６の処理で算出した第２の円Ｃｉ＋１とが重ならないと判定した場合には、置き換え処理へ移行する（Ｓ８４）。
【００８５】
Ｓ８４の処理は、再計算部１４で実行され、直線と円弧を用いて移動経路を近似し、ジグザグ状の移動経路を近似した移動経路に置き換える処理である。例えば、図１４に示すように、線分ＰＡＰＢ、線分ＰＢＰＣ及び線分ＰＣＰＤからなるジグザグ状の経路を、円弧ＰＡＰＡ´、直線ＰＡ´ＰＢ´、円弧ＰＢ´ＰＤからなる経路に近似する。Ｓ８４の処理が終了すると、変更箇所の記録処理へ移行する（Ｓ８２）。
【００８６】
以上のように、図７及び図８に示す制御処理を実行することにより、線分で構成されたジグザグ状の移動経路の屈折箇所を、二つの円弧又は円弧及び直線によって近似することができ、全ての制御点において図７及び図８に示す制御処理を実行することによって、図６の（ｂ）に示すように、移動経路全体を屈折箇所の無い滑らかな移動経路に近似することができる。
【００８７】
図６のＳ３４の処理に戻り、ジグザグ状の移動経路を平滑化する処理が終了すると、衝突判定処理へ移行する（Ｓ３６）。
【００８８】
Ｓ３６の処理は、衝突判定部１５で実行され、Ｓ３４の処理で求めた移動経路をロボット２が走行すると障害物と衝突するか否かを判定する処理である。Ｓ３６の処理において、ロボット２が障害物と衝突すると判定した場合には、Ｓ３４の処理を再実行する。その際、衝突すると判定した箇所において、Ｓ５８の処理やＳ７０の処理と同様に、円の半径をさらに小さくして近似を行う。他方、Ｓ３６の処理において、ロボット２が障害物と衝突しないと判定した場合には、速度情報設定処理へ移行する（Ｓ３８）。
【００８９】
Ｓ３８の処理は、制御点設定部１２で実行され、各移動経路上において、速度情報を設定する処理である。例えば、図６の（ｃ）に示すように、制御点設定部１２は、Ｓ３８の処理で近似した移動経路上に速度情報を設定する。Ｓ３８の処理が終了すると、制御信号の生成処理へ移行する（Ｓ４０）。
【００９０】
Ｓ４０の処理は、アクチュエータ制御部６２で実行され、Ｓ３４の処理で再計算し平滑化した移動経路に基づいて、アクチュエータを制御する処理である。アクチュエータ制御部６２は、再計算し平滑化した移動経路に基づいて、例えば電動モータ８の制御信号を生成し、車輪９を動作させる。Ｓ４０の処理が終了すると、図６に示す制御処理を終了する。
【００９１】
このように、図６に示す制御処理を実行することで、ジグザグ状の移動経路を直線と円弧を用いて滑らかな移動経路に近似することができる。
【００９２】
以上のように、第２実施形態に係る移動経路取得装置１０によれば、移動経路を取得し、取得した移動経路に制御点ＰＡ、ＰＢ及びＰＣを設け、制御点間の経路を制御点ＰＡ、ＰＢ及びＰＣに関する情報に基づいて円弧ＰＡＰＡ´及び円弧ＰＡ´ＰＣで近似して移動経路を滑らかにすることができる。これにより、単純な円弧を用いて、曲線の移動経路を再計算することができる。よって、取得する移動経路が変更された場合、その変更に対応した滑らかな移動経路を迅速に取得することができる。
【００９３】
また、第２実施形態に係る移動経路取得装置１０によれば、円弧生成部１３が二つの円Ｃｉ及びＣｉ＋１を用いて各制御点間の移動経路を近似することや、四つの円Ｃｉ、円Ｃｉ＋１、円Ｃｉ´及びＣｉ＋１´を用いて移動経路を近似することによって、移動経路を精度良く滑らかに近似することができる。
【００９４】
また、第２実施形態に係る移動経路取得装置１０によれば、ロボット２に備わる駆動部分である車輪９等が実際に旋回可能な半径を持つ円Ｃｉ、Ｃｉ＋１、Ｃｉ´、Ｃｉ´´及びＣｉ＋１´を用いて円弧を算出することで、ロボット２の駆動能力に対応した移動経路を迅速に生成することができる。
【００９５】
また、第２実施形態に係る移動経路取得装置１０によれば、移動経路を取得し、取得した移動経路の制御点を結ぶ直線部分の長さに応じて直線又は円弧によって近似することができる。例えば、ジグザグ状の移動経路を入力し、所定の区間の移動経路が短い場合には、複数の円弧を組み合わせて移動経路を近似し、所定の区間の移動経路が長い場合には、直線と円弧とを組み合わせて移動経路を近似することができる。これにより、ジグザグ状の移動経路を円弧と直線からなる滑らかな移動経路に迅速に再計算することができる。
【００９６】
さらに、第２実施形態に係る移動経路取得装置１０によれば、近似した移動経路を用いてロボット２が走行した場合に障害物に衝突するか否かを判定し、衝突すると判定した場合には、より半径の小さい円の円弧を用いて、移動経路を再度近似することができる。これにより、障害物に衝突する移動経路となることを回避できるので、安全な移動経路を算出することができる。
【００９７】
なお、上述した各実施形態は本発明に係る移動経路取得装置の一例を示すものである。本発明に係る移動経路取得装置は、これらの各実施形態に係る移動経路取得装置に限られるものではなく、各請求項に記載した要旨を変更しない範囲で、各実施形態に係る移動経路取得装置を変形し、又は他のものに適用したものであってもよい。
【００９８】
例えば、上記各実施形態に係る移動経路取得装置を、ロボット２に採用した例を説明したが、車両やロボットアーム等に採用してもよく、要はジグザグ状の目標移動軌跡に基づいて移動する移動機器であればよい。このような移動機器に採用することによって、その移動機器の駆動能力を考慮した滑らかな移動経路を動的に生成することが可能となる。
【図面の簡単な説明】
【００９９】
【図１】第１実施形態に係る移動経路取得装置を備えたロボットの構成図である。
【図２】図１の移動経路取得装置を備えたロボットの動作を示すフローチャートである。
【図３】図１の移動経路取得装置が再計算した移動経路を示す概要図である。
【図４】図１の移動経路取得装置が再計算した移動経路を示す概要図である。
【図５】第２実施形態に係る移動経路取得装置を備えたロボットの構成図である。
【図６】図５の移動経路取得装置を備えたロボットの動作を示すフローチャートである。
【図７】図５の移動経路取得装置を備えたロボットの動作を示すフローチャートである。
【図８】図５の移動経路取得装置を備えたロボットの動作を示すフローチャートである。
【図９】図５の移動経路取得装置が再計算した移動経路を示す概要図である。
【図１０】図５の移動経路取得装置が再計算した移動経路を示す概要図である。
【図１１】図５の移動経路取得装置が再計算した移動経路を示す概要図である。
【図１２】図５の移動経路取得装置が再計算した移動経路を示す概要図である。
【図１３】図５の移動経路取得装置が再計算した移動経路を示す概要図である。
【図１４】図５の移動経路取得装置が再計算した移動経路を示す概要図である。
【符号の説明】
【０１００】
１,１０…移動経路再計算処理部（移動経路取得装置）、１１…曲線近似部（曲線近似手段）、１２…制御点設定部（制御点設定手段）、１３…円弧生成部（円弧近似手段）、１４…再計算部（再計算手段）、１５…衝突判定部（衝突判定手段）。
【図１】
2009032185.tif 000015
【図２】
2009032185.tif 000016
【図３】
2009032185.tif 000017
【図４】
2009032185.tif 000018
【図５】
2009032185.tif 000019
【図６】
2009032185.tif 000020
【図７】
2009032185.tif 000021
【図８】
2009032185.tif 000022
【図９】
2009032185.tif 000023
【図１０】
2009032185.tif 000024
【図１１】
2009032185.tif 000025
【図１２】
2009032185.tif 000026
【図１３】
2009032185.tif 000027
【図１４】
2009032185.tif 000028