経路計画装置及び経路計画方法
【課題】経路計画の演算が迅速に行え、移動体の移動経路の平滑化が図れる経路計画装置及び経路計画方法を提供すること。
【解決手段】移動体の経路計画を行う経路計画装置１であって、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数Ｔ（ｘ）を用いてロボットの移動部分の移動経路の平滑化を行う移動経路平滑化部２１を備えている。このテナリー多項式関数を用いて移動体の移動経路の平滑化を行うことにより、移動体の平滑な移動経路を迅速
【解決手段】移動体の経路計画を行う経路計画装置１であって、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数Ｔ（ｘ）を用いてロボットの移動部分の移動経路の平滑化を行う移動経路平滑化部２１を備えている。このテナリー多項式関数を用いて移動体の移動経路の平滑化を行うことにより、移動体の平滑な移動経路を迅速
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、移動体の経路計画を行う経路計画装置及び経路計画方法に関するものである。
【背景技術】
【０００２】
従来、移動体の経路計画を行う装置及び方法として、特開２００３−２４１８１１号公報に記載されるように、予め指定された直線軌跡開始点と終了点の区間において加速度一定としたときの３次元空間中の線速度プロファイル、線速度プロファイルの１階微分値としての線加速度プロファイル、及び線速度プロファイルの２階微分値としての線加加速度プロファイルを生成し、これらの線速度プロファイル等に対してフィルタ処理を施すことによって平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルをそれぞれ生成し、これらの平滑化された線速度プロファイル等に基づいて補間周期毎の補間点の３次元位置を生成し、補間周期毎の補間点の３次元位置に対して逆キネマティクス演算を実施することによりロボット各関節軸の指令位置を生成し、ロボット各関節軸の指令位置に基づいてロボット各関節軸を動作させるものが知られている。
【０００３】
このようなロボットなどの移動体の経路計画を生成するにあたり、リアルタイムで移動体の動作を制御するためには、経路計画の演算を迅速に行うことが必要となってくる。
【特許文献１】特開２００３−２４１８１１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、このような経路計画の装置及び方法において、移動体の移動経路の平滑化、移動速度などの平滑化を図る上で経路計画の演算を迅速に行うことが難しい。例えば、移動速度を増減させる速度プロファイルを生成する場合、平滑化を図るためにＳ字状に速度を変化させようとすると、速度変化状態に応じて移動経路を複数の区間に分割し、その区間数に応じて複数の連立方程式を解いて区間における時間などを計算する必要がある。このため、経路計画の演算が複雑なものとなり、移動体の移動中にリアルタイムで経路演算することは非常に困難である。
【０００５】
そこで本発明は、このような問題点を解決するためになされたものであって、経路計画の演算が迅速に行え、移動体の移動経路の平滑化が図れる経路計画装置及び経路計画方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
すなわち、本発明に係る経路計画装置は、移動体の経路計画を行う経路計画装置において、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて前記移動体の移動経路上における位置の時間微分量の平滑化を行う移動経路平滑化手段を備えて構成されている。
【０００７】
この発明によれば、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて移動体の移動経路上における位置の時間微分量の平滑化を行うことにより、移動体の平滑な移動経路を迅速に算出することができる。
【０００８】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことが好ましい。
【０００９】
この発明によれば、テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことにより、移動経路の軌跡が平滑なものとなり、移動体の移動経路の円滑かつ高速な移動が可能となる。
【００１０】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、移動経路を直線経路区間と曲線経路区間に分別し、その曲線経路区間の経路の軌跡を前記テナリー多項式関数を用いて平滑化することが好ましい。
【００１１】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことが好ましい。
【００１２】
この発明によれば、テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことにより、移動速度の移動速度が平滑なものとなり、速度、加速度、ジャークなどの制限下で円滑かつ高速な移動が可能となる。
【００１３】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、移動経路を複数の区間に分別し、その区間における速度プロファイルを前記テナリー多項式関数を用いて生成することが好ましい。
【００１４】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、移動経路を複数の区間に分別し、区間と区間の繋ぎ目の速度を設定し、その設定された速度に応じて各区間における初速度と終速度を決定し、この初速度と終速度に基づいて前記区間の速度プロファイルを生成することが好ましい。
【００１５】
この発明によれば、移動経路を複数の区間に分別し、区間と区間の間の速度を設定し、その設定された速度に応じて各区間の初速度と終速度を決定し、初速度と終速度に基づいて区間の速度プロファイルを生成することにより、区間と区間の間の移動速度の連続性が良好なものとなり、移動経路全体の平滑化が図れる。
【００１６】
また本発明に係る経路計画装置において、前記テナリー多項式関数は、次の式（１）に示されるものであることを特徴とする請求項１〜６のいずれか一項に記載の経路計画装置。
【数１】
【００１７】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）。Ｃ、ａ０は定数。ｉは０又は正の整数。
【００１８】
この発明によれば、テナリー多項式関数を用いて移動経路を平滑化することにより、移動経路の速度、加速度、ジャークなどの制限や移動経路の軌跡の微分係数に対する制限に対応しつつ平滑な移動経路を迅速に演算することができる。
【００１９】
また本発明に係る経路計画方法は、移動体の経路計画を行う経路計画方法において、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて前記移動体の移動経路上における位置の時間微分量の平滑化を行う移動経路平滑化工程を備えて構成されている。
【００２０】
この発明によれば、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて移動体の移動経路上における位置の時間微分量の平滑化を行うことにより、移動体の平滑な移動経路を迅速に算出することができる。
【００２１】
また本発明に係る経路計画方法において、前記移動経路平滑化工程は、前記テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことが好ましい。
【００２２】
この発明によれば、テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことにより、移動経路の軌跡が平滑なものとなり、移動体の移動経路の円滑かつ高速な移動が可能となる。
【００２３】
また本発明に係る経路計画方法において、前記移動経路平滑化工程は、移動経路を直線経路区間と曲線経路区間に分別し、その曲線経路区間の経路の軌跡を前記テナリー多項式関数を用いて平滑化することが好ましい。
【００２４】
また本発明に係る経路計画方法において、前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことが好ましい。
【００２５】
この発明によれば、テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことにより、移動速度の移動速度が平滑なものとなり、速度、加速度、ジャークなどの制限下で円滑かつ高速な移動が可能となる。
【００２６】
また本発明に係る経路計画方法において、前記移動経路平滑化工程は、移動経路を複数の区間に分別し、その区間における速度プロファイルを前記テナリー多項式関数を用いて生成することが好ましい。
【００２７】
また本発明に係る経路計画方法において、前記移動経路平滑化工程は、移動経路を複数の区間に分別し、区間と区間の繋ぎ目の速度を設定し、その設定された速度に応じて各区間における初速度と終速度を決定し、この初速度と終速度に基づいて前記区間の速度プロファイルを生成することが好ましい。
【００２８】
この発明によれば、移動経路を複数の区間に分別し、区間と区間の間の速度を設定し、その設定された速度に応じて各区間の初速度と終速度を決定し、初速度と終速度に基づいて区間の速度プロファイルを生成することにより、区間と区間の間の移動速度の連続性が良好なものとなり、移動経路全体の平滑化が図れる。
【００２９】
また本発明に係る経路計画方法において、前記テナリー多項式関数は、次の式（１）に示されるものであることが好ましい。
【数１】
【００３０】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）。Ｃ、ａ０は定数。ｉは０又は正の整数。
【００３１】
この発明によれば、テナリー多項式関数を用いて移動経路を平滑化することにより、移動経路の速度、加速度、ジャークなどの制限や移動経路の軌跡の微分係数に対する制限に対応しつつ平滑な移動経路を迅速に演算することができる。
【発明の効果】
【００３２】
本発明によれば、テナリー多項式関数を用いて移動体の移動経路を演算することにより、経路計画の演算が迅速に行え、移動体の移動経路の平滑化を図ることができる。
【発明を実施するための最良の形態】
【００３３】
以下、添付図面を参照して本発明の実施の形態を詳細に説明する。なお、図面の説明において同一の要素には同一の符号を付し、重複する説明を省略する。
【００３４】
図１は本実施形態に係る経路計画装置の概略構成図である。
【００３５】
本実施形態に係る経路作成装置１及び経路作成方法は、多自由度リンク系のロボットの動作経路を作成する経路作成装置に適用したものである。本実施形態に係る経路作成装置１及び経路作成方法は、ロボットの移動部分が設定位置から他の設定位置までの動作する際の動作経路を作成する。
【００３６】
経路計画装置１は、ロボットの移動部分の移動経路（動作経路）について空間的な平滑化及び移動速度の平滑化の処理を行い、ロボットの移動部分の移動経路を生成する。ここで移動経路とは、位置などの空間的な移動経路のみならず、移動部分の速度、加速度、ジャークなどの時間的な要素を含むものである。
【００３７】
図１に示すように、経路計画装置１は、経路演算部２、記録部３及び出力部４を備えている。経路演算部２は、移動経路平滑化部２１、移動速度演算部２２、全体経路速度平滑化部２３を備えている。記録部３は、周辺情報記録部３１、経路記録部３２及びロボット情報記録部３３を備えている。経路演算部２は、ロボットの移動部分の経路演算に際し、必要な情報を記録部３から適宜入力する。
【００３８】
この経路計画装置１は、例えばＣＰＵ、ＲＯＭ、ＲＡＭを含むコンピュータを主体として構成され、移動経路平滑化部２１、移動速度演算部２３、全体経路速度平滑化部２３及び出力部４の機能をコンピュータプログラムによって実行可能に構成されている。なお、経路計画装置１において、移動経路平滑化部２１、移動速度演算部２３、全体経路速度平滑化部２３及び出力部４の一部又は全部を個別のハードウェア装置により構成して機能させるものであってもよい。
【００３９】
移動経路平滑化部２１は、移動経路の空間的な平滑化を行う移動経路平滑化手段であり、例えば取得した暫定的な移動経路の平滑化を行うものであって、特に基本移動経路の曲折部分の曲率又は曲率半径が所定条件を満たすように曲線経路区間における経路生成を行う。この経路生成の際、テナリー多項式（Ternary Polynomial）関数が用いられる。テナリー多項式関数を用いることにより、平滑化された移動経路を迅速に演算することができる。テナリー多項式関数の詳細については、後述する。
【００４０】
例えば、図２に示すように、移動経路平滑化部２１には、移動部分周辺にある障害物Ａを避けるように直線的な経路部分を接続した移動基本経路Ｐが与えられる。この移動基本経路Ｐは、ロボットがカメラやセンサなどの周辺環境検出手段によって検出し認識した経路であってもよいし、ロボット外部から与えられた経路や予め設定された経路であってもよい。
【００４１】
そして、移動経路平滑化部２１は、図３に示すように、移動基本経路Ｐを直線経路区間（Ｐ０、Ｐ２、Ｐ４、Ｐ６、Ｐ７、Ｐ９）と曲線経路区間（Ｐ１、Ｐ３、Ｐ５、Ｐ８）に分別して設定し、曲線経路区間について所定条件下で曲率又は曲率半径をそれぞれ設定しつつ、その区間の経路を演算する。
【００４２】
図１において、移動速度演算部２２は、移動経路の速度変化に関する平滑化を行い、移動経路の区間における移動速度プロファイルを生成する移動速度演算手段として機能するものである。例えば、移動速度演算部２２は、ロボットにおける速度、加速度（減速度を含む）、ジャークなどの速度に関する条件を満たしつつ、移動時間が短くなるように直線経路区間における移動速度プロファイルを生成する。この移動速度プロファイルの生成の際、テナリー多項式関数が用いられる。テナリー多項式関数を用いることにより、平滑化された移動速度プロファイルを迅速に演算することができる。
【００４３】
全体経路速度平滑化部２３は、移動経路の各区間の間における速度の連続性を考慮し速度変化の平滑化を図るものである。例えば、全体経路速度平滑化部２３は、まず最大許容加速度での速度を用いて生成した直線経路区間の速度プロファイルを曲線経路区間の速度制限を考慮して修正し、各区間の速度の連続性を担保して平滑化した全体経路の速度プロファイルを生成する。
【００４４】
周辺情報記録部３１は、ロボットの移動部分の周辺環境における情報を記録するものであり、例えばロボットの移動部分の動作に対する障害物の位置情報などを記録している。経路記録部３２は、ロボットの移動部分の移動経路について演算したものを記録するものである。ロボット情報記録部３３は、ロボットの移動部分における情報を主に記録するものであり、例えば移動部分における最大許容速度、最大許容加速度、最大許容ジャークなどが記録している。
【００４５】
記録部３には、移動経路情報が入力される。移動経路情報は、ロボットの移動部分の移動経路に関する情報であり、例えば障害物を避けて移動可能な領域情報などが該当する。この移動経路情報は、ロボット外部から入力されるものでもよいし、ロボットのカメラやセンサなどにより自律的に取得した情報であってもよい。出力部４は、ロボットの移動部分を移動させるアクチュエータ１０に作動信号を出力するものである。
【００４６】
図４は本実施形態に係る経路計画装置及び経路計画方法を適用したロボットの一例を示す図であり、図５は本実施形態に係る経路計画装置及び経路計画方法を適用したロボットの他の例を示す図である。
【００４７】
図４に示すように、ロボットＲ１は、多数の関節Ｊ１，・・・，Ｊｎを備えており、関節間がリンクＬ１，・・・，Ｌｎ＋１で接続されている。また、ロボットＲ１は、末端のリンクＬ１の一端が固定され、先端のリンクＬｎ＋１の一端にハンドＨが取り付けられている。各関節Ｊ１，・・・，Ｊｎは、アクチュエータ１０が内蔵されており、回転動作をそれぞれ行い、接続される２本のリンク間の角度Ｘ１，・・・，Ｘｎをそれぞれ変更する。
【００４８】
このロボットＲ１は、ｎ個の自由度を持つ。この自由度は、ｎ個の角度に対して座標軸を持つｎ次元座標空間（コンフィグレーション空間）における一点（Ｘ１，・・・，Ｘｎ）で表される。また、ロボットＲ１の実際の位置姿勢は、三次元空間（作業空間）におけるロボットＲ１の先端部Ｔ（リンクＬｎ＋１とハンドＨとの取付部）の座標系（Ｙ１，Ｙ２，Ｙ３）で表される。ロボットＲ１の先端部Ｔは、コンフィグレーション空間から作業空間への非線形写像の値として、Ｙ１＝ｆ１（Ｘ１，・・・，Ｘｎ）、Ｙ２＝ｆ２（Ｘ１，・・・，Ｘｎ）、Ｙ３＝ｆ３（Ｘ１，・・・，Ｘｎ）で定義される。したがって、コンフィグレーション空間の（Ｘ１，・・・，Ｘｎ）を規定すると、作業空間の（Ｙ１，Ｙ２，Ｙ３）が決まる。
【００４９】
このロボットＲ１は、ハンドＨもしくは先端部Ｔがロボットの移動部分として機能し、アクチュエータ１０への制御信号の出力によりハンドＨもしくは先端部Ｔの移動制御を行うことができる。
【００５０】
また、図５にはロボットの他の例を示している。ロボットＲ２は、人型のロボットであり、左右一対のアーム部Ａ１，Ａ２とハンドＨ１，Ｈ２を有している。ロボットＲ２は、１０個の関節Ｊ１，・・・，Ｊｎを備えており、１０個の自由度を持つ。ロボットＲ２は、自由度がコンフィグレーション空間における座標系（Ｘ１，・・・，Ｘ１０）で表され、実際の位置姿勢が作業空間における各先端部Ｔ１，Ｔ２の座標系（Ｙ１１，Ｙ１２，Ｙ１３）、（Ｙ２１，Ｙ２２，Ｙ２３）で表される。ロボットＲ２の場合、コンフィグレーション空間の（Ｘ１，・・・，Ｘ１０）を規定すると、作業空間の（Ｙ１１，Ｙ１２，Ｙ１３）、（Ｙ２１，Ｙ２２，Ｙ２３）が決まる。
【００５１】
このロボットＲ２は、ハンドＨ１、Ｈ２もしくは先端部Ｔ１、Ｔ２がロボットの移動部分として機能し、アクチュエータ１０への制御信号の出力によりハンドＨ１、Ｈ２もしくは先端部Ｔ１、Ｔ２の移動制御を行うことができる。
【００５２】
図６（ａ）〜（ｄ）は、テナリー多項式関数Ｔ（ｘ）の一例をグラフ表示した図である。
【００５３】
テナリー多項式関数は、関数値の増加領域、一定領域、減少領域の三領域を有し、一定領域を挟んで増加領域と減少領域が対称に表される多項式関数である。
【００５４】
このテナリー多項式関数Ｔ（ｘ）は、例えば次の式（１）により表される。
【数１】
【００５５】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）である。Ｃ、ａ０は定数である。ｉは０又は正の整数である。
【００５６】
図６（ａ）は、テナリー多項式関数の次数０のＴ０（ｘ）を示したものであり、矩形波状のプロファイルとなっている。図６（ｂ）は、テナリー多項式関数の次数１のＴ１（ｘ）を示したものであり、台形状のプロファイルとなっている。図６（ｃ）は、テナリー多項式関数の次数２のＴ２（ｘ）を示したものであり、滑らかに変化する台形状のプロファイルとなっている。図６（ｄ）は、テナリー多項式関数の次数３のＴ３（ｘ）を示したものであり、滑らかに変化する台形状のプロファイルとなっている。
【００５７】
図７に示すように、テナリー多項式関数Ｔｉ（ｘ）は、Ｔｉ（ｘ）−ｘの座標系においてＴｉ（ｘ）の波形を原点を中心に点対称に振り分けてＴ′ｉ＋１（ｘ）を生成し、このＴ′ｉ＋１（ｘ）を積分することによって、一つ次数の高いＴｉ＋１（ｘ）を生成することができる。
【００５８】
例えば、Ｔ０（ｘ）からＴ２（ｘ）を生成する場合、Ｔ０（ｘ）を対称分割（対称振分け）し積分してＴ１（ｘ）を生成し、Ｔ１（ｘ）を対称分割し積分してＴ２（ｘ）を生成してもよいし、Ｔ０（ｘ）を対称分割した後、さらに対称分割してＴ″２（ｘ）を生成し、Ｔ″２（ｘ）を二回積分してＴ２（ｘ）を生成してもよい。
【００５９】
また、図８に示すように、Ｔ１（ｘ）からＴ２（ｘ）を生成する場合、Ｔ０（ｘ）を積分し、Ｔ０（ｘ）を積分したものをｘ軸を中心に線対称に変換して対称化し、Ｔ０（ｘ）を積分したものを−（ａ２−ａ１）／２だけｘ軸に沿って移動させ、Ｔ０（ｘ）を積分し対称化したものを（ａ２−ａ１）／２だけｘ軸に沿って移動させ、それぞれ移動させたものを合成して、Ｔ２（ｘ）を生成してもよい。
【００６０】
テナリー多項式関数Ｔ（ｘ）におけるＣ、ａ０などの定数は、ロボットの移動部分における動作の制限値（上限値、下限値）に応じて設定すればよい。例えば、テナリー多項式関数Ｔ（ｘ）を用いて移動部分の速度プロファイルを生成する場合、図７において、Ｔ２（ｘ）を速度プロファイルとして設定すると、Ｔ２′（ｘ）は加速度状態を示し、Ｔ２″（ｘ）がジャーク状態を示すこととなる。
【００６１】
このため、Ｔ２″（ｘ）の制限値Ｃ、−Ｃとして移動部分のジャーク上限値、下限値を設定し、Ｔ２′（ｘ）の制限値Ｃ・ａ０、−Ｃ・ａ０として移動部分の加速度上限値、下限値を設定し、Ｔ２（ｘ）の制限値Ｃ・（ａ１−ａ０）・ａ０として移動部分の速度制限値を設定することにより、一義的に定数Ｃ、ａ０、ａ１の値が決まる。
【００６２】
このようなテナリー多項式関数Ｔ（ｘ）は、速度、加速度、ジャークなどの微分係数の制限を満たす演算が迅速に行える。このため、テナリー多項式関数Ｔ（ｘ）を用いて移動経路の演算を行うことにより、速度などの所定の制限を満たす移動経路を迅速に演算することができる。
【００６３】
次に、本実施形態に係る経路計画装置の動作及び経路計画方法について説明する。
【００６４】
図９に本実施形態に係る経路計画装置の動作及び経路計画方法についてのフローチャートを示す。
【００６５】
まず、図９のＳ１０に示すように、移動経路の取得処理が行われる。移動経路の取得処理は、ロボットの移動部分の基本的な移動基本経路を取得する処理であり、例えばロボットに設置されるカメラやセンサなどの周辺環境検出手段によって移動基本経路を取得する。また、ロボットに予め設定された経路を移動基本経路として設定してもよいし、ロボット外部から与えられた経路を移動基本経路としてもよい。
【００６６】
この移動経路の取得処理により、例えば、図２に示すように、移動基本経路Ｐが取得される。なお、この移動基本経路Ｐは平滑化を行う前の仮の経路である。取得された移動基本経路Ｐは経路記録部３２に記録される。
【００６７】
そして、Ｓ２０に移行し、移動経路の区間設定処理が行われる。この区間設定処理は、移動経路を直線経路区間と曲線経路区間に分けて設定する処理である。例えば、図２に示すように、移動基本経路Ｐにおいて、経路の曲折箇所Ｐｃから所定の距離範囲までの区間が曲線経路区間とされ、それ以外の区間が直線経路区間とされる。
【００６８】
そして、Ｓ３０に移行し、移動経路の空間的平滑化処理が行われる。この空間的平滑化処理は、移動経路における曲線経路区間の曲率を小さくし又は曲率半径を大きくして経路の空間的な平滑化を図る処理である。例えば、曲線経路区間において平滑化されたパス微分係数に対する制限を満足するようにカーネル関数の最適カーネルパラメータが計算され、そのカーネル関数によって曲線経路区間の経路軌跡が演算される。
【００６９】
その際、経路軌跡が障害物に衝突しないか否かが判断され、衝突する場合には曲率の大きい経路が演算される。このとき、曲率が設定値以上大きくなってしまう場合には、経路の平滑化が行われず、その曲線経路区間については曲折した二つの直線経路とされる。このような曲折した経路においては、ロボットの移動部分は移動を停止することとなる。
【００７０】
ここで、カーネル関数をＫ（ｘ）とすると、このカーネル関数Ｋ（ｘ）は、次の式（２）に示すように、テナリー多項式関数Ｔ（ｘ）を用いたものが用いられる。
【数２】
【００７１】
この式（２）のカーネル関数Ｋ（ｘ）において、曲線経路区間の曲折箇所の形状に応じて定数ａｎ−２などを設定し、また関数の次数を設定することで適切な経路軌跡を生成することができる。
【００７２】
このような移動経路の空間的平滑化処理によって、移動経路の曲折部が平滑化され、図３に示すように経路区間Ｐ０〜Ｐ９からなる移動経路が生成される。
【００７３】
そして、Ｓ４０に移行し、移動速度平滑化処理が行われる。移動速度平滑化処理は、移動経路における平滑化された速度プロファイルを生成する処理である。例えば、この移動速度平滑化処理は、経路区間の繋ぎ目における速度設定処理を行い、各区間の速度プロファイル生成処理を行い、各区間の速度プロファイルを繋ぎ合わせて移動経路全体の速度プロファイルを生成して、行われる。
【００７４】
経路区間の繋ぎ目における速度設定処理は、ロボットの移動部分の最大許容加速度、最大許容速度、曲線経路区間での制限速度に応じ、区間と区間の間の繋ぎ目における速度設定を行う処理である。
【００７５】
例えば、図１０（ａ）に示すように、まず移動経路の進行方向について、正の加速度を考慮し、フォワードフェーズ処理が行われる。フォワードフェーズ処理は、各区間を最大速度で移動した場合におけるに各区間の繋ぎ目の速度を演算する処理である。
【００７６】
そして、図１０（ｂ）に示すように、移動経路の進行の逆方向について、負の加速度を考慮し、バックフェーズ処理が行われる。バックフェーズ処理は、先の区間での減速や停止を考慮してフォワードフェーズ処理にて演算された速度を下方修正する処理である。このバックフェーズ処理により、各区間の繋ぎ目の速度が修正される。
【００７７】
このようなフォワードフェーズ処理及びバックフェーズ処理を含む速度設定処理を行うことにより、各区間の繋ぎ目の速度が設定され、各区間における初速度、終速度が決定される（図１０（ｃ）参照）。
【００７８】
各区間の速度プロファイル生成処理は、各区間における初速度、終速度、距離、最大許容速度、最大許容加速度、最大許容ジャークなどを考慮して行われる。
【００７９】
図１１に速度プロファイル生成処理における速度プロファイル生成過程の説明図である。
【００８０】
例えば、初速度Ｖ０、終速度Ｖｆの経路区間の速度プロファイルを生成する場合について説明すると、まず、図１１（ａ）に示すように、区間の始点から円滑に加速するプロファイルが生成される。このプロファイルは、初速度Ｖ０から終速度Ｖｆに加速する速度プロファイルである。
【００８１】
このとき、速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、図１２に示すように、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔが設定され、Ｔ２（ｔ）が速度プロファイルとして算出される。
【００８２】
まず、図１２（ａ）に示すように、まず初速度０から最高速Ｖｆ−Ｖ０に増加した後、終速度０となるＴ２（ｔ）が生成される。このとき、Ｔ２（ｔ）の最大加速度、最大ジャークが最大許容加速度、最大許容ジャークとなるようにする。すなわち、Ｔ２（ｔ）の最大加速度、最大ジャークが最大許容加速度、最大許容ジャークとなるようにＴ′２（ｔ）、Ｔ″２（ｔ）を設定し、このように設定したＴ′２（ｔ）、Ｔ″２（ｔ）からＴ２（ｔ）を生成する。これにより、短い時間で移動可能となる速度プロファイルが生成できる。
【００８３】
そして、生成したＴ２（ｔ）をＶ０だけ関数値増加方向に平行移動し、初速度Ｖ０から終速度Ｖｆまでのプロファイルが演算される（図１２（ａ）の右図参照）。
【００８４】
そして、図１１（ｂ）に示すように、区間の終点に向けて減速するプロファイルが生成される。このプロファイルは、速度Ｖ１から終速度Ｖｆに減速する速度プロファイルである。Ｖ１としては、最高許容速度より遅い速度値であって、最高許容速度に近い値が設定される。
【００８５】
このとき、速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、上述した図１２に示す手法を用い、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔを設定し、Ｔ２（ｔ）を速度プロファイルとして算出すればよい。
【００８６】
そして、図１１（ｃ）に示すように、区間の始点から加速するプロファイルの補正が行われる。このプロファイル生成は、初速度Ｖ０から速度Ｖ１に加速する速度プロファイルの生成である。
【００８７】
この速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、上述した図１２に示す手法を用い、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔを設定し、Ｖ０からＶ１に加速するＴ２（ｔ）を速度プロファイルとして算出すればよい。
【００８８】
ここで、図１１（ｃ）に示すように生成された速度プロファイルにおいて、より加速させるために使用可能な距離が設定距離より長い場合には、速度Ｖ１より速い速度Ｖ２が設定され、速度プロファイルが補正される。
【００８９】
すなわち、図１１（ｄ）に示すように、まず、区間の終点に向けて減速する速度プロファイルの補正が行われる。この速度プロファイルの補正は、速度Ｖ２から終速度Ｖｆに減速する速度プロファイルである。Ｖ１としては、最高許容速度より遅い速度値であって、速度Ｖ１より速い速度値である。
【００９０】
このとき、補正速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、上述した図１２に示す手法を用い、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔを設定し、Ｔ２（ｔ）を速度プロファイルとして算出すればよい。
【００９１】
次いで、図１１（ｅ）に示すように、区間の始点から加速する速度プロファイルの補正が行われる。この速度プロファイルの補正は、初速度Ｖ０から速度Ｖ２に加速する速度プロファイルを生成して行われる。
【００９２】
この速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、上述した図１２に示す手法を用い、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔを設定し、Ｖ０からＶ２に加速するＴ２（ｔ）を速度プロファイルとして算出すればよい。
【００９３】
このような各区間の速度プロファイル生成処理によれば、ロボットの移動部分が経路区間内を円滑な速度変化状態で移動し、短時間で移動可能な速度プロファイルを生成することができる。
【００９４】
また、各区間の速度プロファイル生成処理としては、上述した処理に限られず、テナリー多項式関数Ｔ（ｘ）を用いた他の処理によって行われてもよい。
【００９５】
例えば、初速度Ｖ０、終速度Ｖｆの経路区間の速度プロファイルを生成する場合、図１３に示すように、まず、区間の始点から初速度Ｖ０から最高許容速度ＶＭａｘに加速する速度プロファイルを生成し（図１３（ａ））、区間の終点に向けて最高許容速度ＶＭａｘから終速度Ｖｆまで減速する速度プロファイルを生成し（図１３（ｂ））、加速する速度プロファイルと減速する速度プロファイルと間の期間を最高許容速度ＶＭａｘとして速度プロファイルを生成し、それらの速度プロファイルを合成する。
【００９６】
この場合、経路区間を最短時間で移動可能であって平滑化された速度プロファイルを生成することができる。特に経路区間の距離が十分に長い場合に有効である。
【００９７】
このように、各区間の速度プロファイルを生成したら、図１０（ｃ）に示すように、各区間の速度プロファイルを繋ぎ合わせて移動経路全体の速度プロファイルを生成する。
【００９８】
そして、図９のＳ５０に移行し、出力処理が行われる。出力処理は、生成した移動経路の速度プロファイルを経路記録部３２に記録すると共に、その速度プロファイルに応じて移動制御信号をアクチュエータ１０に出力する処理である。これにより、アクチュエータ１０が作動し、ロボットの移動部分が平滑に移動し、短い時間で所定の動作を行う。
【００９９】
以上のように、本実施形態に係る経路計画装置及び経路計画方法によれば、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて移動体の移動経路上における位置の時間微分量の平滑化を行うことにより、移動体の平滑な移動経路を迅速に算出することができる。
【０１００】
特に、テナリー多項式関数として式（１）に示されるものを用いることにより、移動経路の速度、加速度、ジャークなどの制限や移動経路の軌跡の微分係数に対する制限に対応しつつ平滑な移動経路を迅速に演算することができる。このため、ロボットの移動部分の移動中にリアルタイムで移動経路の演算が可能となる。従って、移動途中に障害物などを検出した場合、それを回避する移動経路を演算して移動停止することなく移動動作を継続することができる。
【０１０１】
また、テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことにより、移動経路の軌跡が平滑なものとなり、移動体の移動経路の円滑かつ高速な移動が可能となる。その際、テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことにより、移動速度の移動速度が平滑なものとなり、速度、加速度、ジャークなどの制限下で円滑かつ高速な移動が可能となる。
【０１０２】
また、移動経路を複数の区間に分別し、区間と区間の間の速度を設定し、その設定された速度に応じて各区間の初速度と終速度を決定し、初速度と終速度に基づいて区間の速度プロファイルを生成することにより、区間と区間の間の移動速度の連続性が良好なものとなり、移動経路全体の平滑化が図れる。
【０１０３】
この場合、他の多項式補間法と異なり、移動経路の速度、加速度、ジャークなどの複数の微分係数の制限に容易に対応させることができ、移動経路の全体を最適化させるような微調整が可能である。
【０１０４】
なお、上述した実施形態は本発明に係る経路計画装置及び経路計画方法の最良な実施形態を説明したものであり、本発明に係る経路計画装置及び経路計画方法は本実施形態に記載したものに限定されるものではない。本発明に係る経路計画装置及び経路計画方法は、各請求項に記載した要旨を変更しないように実施形態に係る経路計画装置及び経路計画方法を変形し、又は他のものに適用したものであってもよい。
【０１０５】
例えば、ロボット、車両などの自律移動体を移動体とし、自律移動体の移動経路の計画に適用してもよい。この場合であっても、上述した実施形態と同様な作用効果が得られる。
【図面の簡単な説明】
【０１０６】
【図１】本発明の実施形態に係る経路計画装置の構成概要図である。
【図２】移動基本経路の説明図である。
【図３】図２の移動基本経路Ｐにおける直線経路区間と曲線経路区間の説明図である。
【図４】本実施形態に係る経路計画装置及び経路計画方法を適用したロボットの一例を示す図である。
【図５】本実施形態に係る経路計画装置及び経路計画方法を適用したロボットの他の例を示す図である。
【図６】テナリー多項式関数の一例を示した図である。
【図７】図６のテナリー多項式関数の生成方法の説明図である。
【図８】図６のテナリー多項式関数の生成方法の説明図である。
【図９】本実施形態に係る経路計画装置の動作及び経路計画方法を示すフローチャートである。
【図１０】本実施形態に係る経路計画装置の動作及び経路計画方法における移動速度平滑化処理の説明図である。
【図１１】本実施形態に係る経路計画装置の動作及び経路計画方法における移動速度平滑化処理の説明図である。
【図１２】本実施形態に係る経路計画装置の動作及び経路計画方法における移動速度平滑化処理の説明図である。
【図１３】本実施形態に係る経路計画装置の動作及び経路計画方法における移動速度平滑化処理の説明図である。
【符号の説明】
【０１０７】
１…経路計画装置、２…経路演算部、３…記録部、４…出力部、１０…アクチュエータ、２１…移動経路平滑化部、２２…移動速度演算部、２３…全体経路平滑化部、３１…周辺情報記録部、３２…経路記録部、３３…ロボット情報記録部。
【技術分野】
【０００１】
本発明は、移動体の経路計画を行う経路計画装置及び経路計画方法に関するものである。
【背景技術】
【０００２】
従来、移動体の経路計画を行う装置及び方法として、特開２００３−２４１８１１号公報に記載されるように、予め指定された直線軌跡開始点と終了点の区間において加速度一定としたときの３次元空間中の線速度プロファイル、線速度プロファイルの１階微分値としての線加速度プロファイル、及び線速度プロファイルの２階微分値としての線加加速度プロファイルを生成し、これらの線速度プロファイル等に対してフィルタ処理を施すことによって平滑化された線速度プロファイル、平滑化された線加速度プロファイル、及び平滑化された線加加速度プロファイルをそれぞれ生成し、これらの平滑化された線速度プロファイル等に基づいて補間周期毎の補間点の３次元位置を生成し、補間周期毎の補間点の３次元位置に対して逆キネマティクス演算を実施することによりロボット各関節軸の指令位置を生成し、ロボット各関節軸の指令位置に基づいてロボット各関節軸を動作させるものが知られている。
【０００３】
このようなロボットなどの移動体の経路計画を生成するにあたり、リアルタイムで移動体の動作を制御するためには、経路計画の演算を迅速に行うことが必要となってくる。
【特許文献１】特開２００３−２４１８１１号公報
【発明の開示】
【発明が解決しようとする課題】
【０００４】
しかしながら、このような経路計画の装置及び方法において、移動体の移動経路の平滑化、移動速度などの平滑化を図る上で経路計画の演算を迅速に行うことが難しい。例えば、移動速度を増減させる速度プロファイルを生成する場合、平滑化を図るためにＳ字状に速度を変化させようとすると、速度変化状態に応じて移動経路を複数の区間に分割し、その区間数に応じて複数の連立方程式を解いて区間における時間などを計算する必要がある。このため、経路計画の演算が複雑なものとなり、移動体の移動中にリアルタイムで経路演算することは非常に困難である。
【０００５】
そこで本発明は、このような問題点を解決するためになされたものであって、経路計画の演算が迅速に行え、移動体の移動経路の平滑化が図れる経路計画装置及び経路計画方法を提供することを目的とする。
【課題を解決するための手段】
【０００６】
すなわち、本発明に係る経路計画装置は、移動体の経路計画を行う経路計画装置において、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて前記移動体の移動経路上における位置の時間微分量の平滑化を行う移動経路平滑化手段を備えて構成されている。
【０００７】
この発明によれば、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて移動体の移動経路上における位置の時間微分量の平滑化を行うことにより、移動体の平滑な移動経路を迅速に算出することができる。
【０００８】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことが好ましい。
【０００９】
この発明によれば、テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことにより、移動経路の軌跡が平滑なものとなり、移動体の移動経路の円滑かつ高速な移動が可能となる。
【００１０】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、移動経路を直線経路区間と曲線経路区間に分別し、その曲線経路区間の経路の軌跡を前記テナリー多項式関数を用いて平滑化することが好ましい。
【００１１】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことが好ましい。
【００１２】
この発明によれば、テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことにより、移動速度の移動速度が平滑なものとなり、速度、加速度、ジャークなどの制限下で円滑かつ高速な移動が可能となる。
【００１３】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、移動経路を複数の区間に分別し、その区間における速度プロファイルを前記テナリー多項式関数を用いて生成することが好ましい。
【００１４】
また本発明に係る経路計画装置において、前記移動経路平滑化手段は、移動経路を複数の区間に分別し、区間と区間の繋ぎ目の速度を設定し、その設定された速度に応じて各区間における初速度と終速度を決定し、この初速度と終速度に基づいて前記区間の速度プロファイルを生成することが好ましい。
【００１５】
この発明によれば、移動経路を複数の区間に分別し、区間と区間の間の速度を設定し、その設定された速度に応じて各区間の初速度と終速度を決定し、初速度と終速度に基づいて区間の速度プロファイルを生成することにより、区間と区間の間の移動速度の連続性が良好なものとなり、移動経路全体の平滑化が図れる。
【００１６】
また本発明に係る経路計画装置において、前記テナリー多項式関数は、次の式（１）に示されるものであることを特徴とする請求項１〜６のいずれか一項に記載の経路計画装置。
【数１】
【００１７】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）。Ｃ、ａ０は定数。ｉは０又は正の整数。
【００１８】
この発明によれば、テナリー多項式関数を用いて移動経路を平滑化することにより、移動経路の速度、加速度、ジャークなどの制限や移動経路の軌跡の微分係数に対する制限に対応しつつ平滑な移動経路を迅速に演算することができる。
【００１９】
また本発明に係る経路計画方法は、移動体の経路計画を行う経路計画方法において、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて前記移動体の移動経路上における位置の時間微分量の平滑化を行う移動経路平滑化工程を備えて構成されている。
【００２０】
この発明によれば、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて移動体の移動経路上における位置の時間微分量の平滑化を行うことにより、移動体の平滑な移動経路を迅速に算出することができる。
【００２１】
また本発明に係る経路計画方法において、前記移動経路平滑化工程は、前記テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことが好ましい。
【００２２】
この発明によれば、テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことにより、移動経路の軌跡が平滑なものとなり、移動体の移動経路の円滑かつ高速な移動が可能となる。
【００２３】
また本発明に係る経路計画方法において、前記移動経路平滑化工程は、移動経路を直線経路区間と曲線経路区間に分別し、その曲線経路区間の経路の軌跡を前記テナリー多項式関数を用いて平滑化することが好ましい。
【００２４】
また本発明に係る経路計画方法において、前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことが好ましい。
【００２５】
この発明によれば、テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことにより、移動速度の移動速度が平滑なものとなり、速度、加速度、ジャークなどの制限下で円滑かつ高速な移動が可能となる。
【００２６】
また本発明に係る経路計画方法において、前記移動経路平滑化工程は、移動経路を複数の区間に分別し、その区間における速度プロファイルを前記テナリー多項式関数を用いて生成することが好ましい。
【００２７】
また本発明に係る経路計画方法において、前記移動経路平滑化工程は、移動経路を複数の区間に分別し、区間と区間の繋ぎ目の速度を設定し、その設定された速度に応じて各区間における初速度と終速度を決定し、この初速度と終速度に基づいて前記区間の速度プロファイルを生成することが好ましい。
【００２８】
この発明によれば、移動経路を複数の区間に分別し、区間と区間の間の速度を設定し、その設定された速度に応じて各区間の初速度と終速度を決定し、初速度と終速度に基づいて区間の速度プロファイルを生成することにより、区間と区間の間の移動速度の連続性が良好なものとなり、移動経路全体の平滑化が図れる。
【００２９】
また本発明に係る経路計画方法において、前記テナリー多項式関数は、次の式（１）に示されるものであることが好ましい。
【数１】
【００３０】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）。Ｃ、ａ０は定数。ｉは０又は正の整数。
【００３１】
この発明によれば、テナリー多項式関数を用いて移動経路を平滑化することにより、移動経路の速度、加速度、ジャークなどの制限や移動経路の軌跡の微分係数に対する制限に対応しつつ平滑な移動経路を迅速に演算することができる。
【発明の効果】
【００３２】
本発明によれば、テナリー多項式関数を用いて移動体の移動経路を演算することにより、経路計画の演算が迅速に行え、移動体の移動経路の平滑化を図ることができる。
【発明を実施するための最良の形態】
【００３３】
以下、添付図面を参照して本発明の実施の形態を詳細に説明する。なお、図面の説明において同一の要素には同一の符号を付し、重複する説明を省略する。
【００３４】
図１は本実施形態に係る経路計画装置の概略構成図である。
【００３５】
本実施形態に係る経路作成装置１及び経路作成方法は、多自由度リンク系のロボットの動作経路を作成する経路作成装置に適用したものである。本実施形態に係る経路作成装置１及び経路作成方法は、ロボットの移動部分が設定位置から他の設定位置までの動作する際の動作経路を作成する。
【００３６】
経路計画装置１は、ロボットの移動部分の移動経路（動作経路）について空間的な平滑化及び移動速度の平滑化の処理を行い、ロボットの移動部分の移動経路を生成する。ここで移動経路とは、位置などの空間的な移動経路のみならず、移動部分の速度、加速度、ジャークなどの時間的な要素を含むものである。
【００３７】
図１に示すように、経路計画装置１は、経路演算部２、記録部３及び出力部４を備えている。経路演算部２は、移動経路平滑化部２１、移動速度演算部２２、全体経路速度平滑化部２３を備えている。記録部３は、周辺情報記録部３１、経路記録部３２及びロボット情報記録部３３を備えている。経路演算部２は、ロボットの移動部分の経路演算に際し、必要な情報を記録部３から適宜入力する。
【００３８】
この経路計画装置１は、例えばＣＰＵ、ＲＯＭ、ＲＡＭを含むコンピュータを主体として構成され、移動経路平滑化部２１、移動速度演算部２３、全体経路速度平滑化部２３及び出力部４の機能をコンピュータプログラムによって実行可能に構成されている。なお、経路計画装置１において、移動経路平滑化部２１、移動速度演算部２３、全体経路速度平滑化部２３及び出力部４の一部又は全部を個別のハードウェア装置により構成して機能させるものであってもよい。
【００３９】
移動経路平滑化部２１は、移動経路の空間的な平滑化を行う移動経路平滑化手段であり、例えば取得した暫定的な移動経路の平滑化を行うものであって、特に基本移動経路の曲折部分の曲率又は曲率半径が所定条件を満たすように曲線経路区間における経路生成を行う。この経路生成の際、テナリー多項式（Ternary Polynomial）関数が用いられる。テナリー多項式関数を用いることにより、平滑化された移動経路を迅速に演算することができる。テナリー多項式関数の詳細については、後述する。
【００４０】
例えば、図２に示すように、移動経路平滑化部２１には、移動部分周辺にある障害物Ａを避けるように直線的な経路部分を接続した移動基本経路Ｐが与えられる。この移動基本経路Ｐは、ロボットがカメラやセンサなどの周辺環境検出手段によって検出し認識した経路であってもよいし、ロボット外部から与えられた経路や予め設定された経路であってもよい。
【００４１】
そして、移動経路平滑化部２１は、図３に示すように、移動基本経路Ｐを直線経路区間（Ｐ０、Ｐ２、Ｐ４、Ｐ６、Ｐ７、Ｐ９）と曲線経路区間（Ｐ１、Ｐ３、Ｐ５、Ｐ８）に分別して設定し、曲線経路区間について所定条件下で曲率又は曲率半径をそれぞれ設定しつつ、その区間の経路を演算する。
【００４２】
図１において、移動速度演算部２２は、移動経路の速度変化に関する平滑化を行い、移動経路の区間における移動速度プロファイルを生成する移動速度演算手段として機能するものである。例えば、移動速度演算部２２は、ロボットにおける速度、加速度（減速度を含む）、ジャークなどの速度に関する条件を満たしつつ、移動時間が短くなるように直線経路区間における移動速度プロファイルを生成する。この移動速度プロファイルの生成の際、テナリー多項式関数が用いられる。テナリー多項式関数を用いることにより、平滑化された移動速度プロファイルを迅速に演算することができる。
【００４３】
全体経路速度平滑化部２３は、移動経路の各区間の間における速度の連続性を考慮し速度変化の平滑化を図るものである。例えば、全体経路速度平滑化部２３は、まず最大許容加速度での速度を用いて生成した直線経路区間の速度プロファイルを曲線経路区間の速度制限を考慮して修正し、各区間の速度の連続性を担保して平滑化した全体経路の速度プロファイルを生成する。
【００４４】
周辺情報記録部３１は、ロボットの移動部分の周辺環境における情報を記録するものであり、例えばロボットの移動部分の動作に対する障害物の位置情報などを記録している。経路記録部３２は、ロボットの移動部分の移動経路について演算したものを記録するものである。ロボット情報記録部３３は、ロボットの移動部分における情報を主に記録するものであり、例えば移動部分における最大許容速度、最大許容加速度、最大許容ジャークなどが記録している。
【００４５】
記録部３には、移動経路情報が入力される。移動経路情報は、ロボットの移動部分の移動経路に関する情報であり、例えば障害物を避けて移動可能な領域情報などが該当する。この移動経路情報は、ロボット外部から入力されるものでもよいし、ロボットのカメラやセンサなどにより自律的に取得した情報であってもよい。出力部４は、ロボットの移動部分を移動させるアクチュエータ１０に作動信号を出力するものである。
【００４６】
図４は本実施形態に係る経路計画装置及び経路計画方法を適用したロボットの一例を示す図であり、図５は本実施形態に係る経路計画装置及び経路計画方法を適用したロボットの他の例を示す図である。
【００４７】
図４に示すように、ロボットＲ１は、多数の関節Ｊ１，・・・，Ｊｎを備えており、関節間がリンクＬ１，・・・，Ｌｎ＋１で接続されている。また、ロボットＲ１は、末端のリンクＬ１の一端が固定され、先端のリンクＬｎ＋１の一端にハンドＨが取り付けられている。各関節Ｊ１，・・・，Ｊｎは、アクチュエータ１０が内蔵されており、回転動作をそれぞれ行い、接続される２本のリンク間の角度Ｘ１，・・・，Ｘｎをそれぞれ変更する。
【００４８】
このロボットＲ１は、ｎ個の自由度を持つ。この自由度は、ｎ個の角度に対して座標軸を持つｎ次元座標空間（コンフィグレーション空間）における一点（Ｘ１，・・・，Ｘｎ）で表される。また、ロボットＲ１の実際の位置姿勢は、三次元空間（作業空間）におけるロボットＲ１の先端部Ｔ（リンクＬｎ＋１とハンドＨとの取付部）の座標系（Ｙ１，Ｙ２，Ｙ３）で表される。ロボットＲ１の先端部Ｔは、コンフィグレーション空間から作業空間への非線形写像の値として、Ｙ１＝ｆ１（Ｘ１，・・・，Ｘｎ）、Ｙ２＝ｆ２（Ｘ１，・・・，Ｘｎ）、Ｙ３＝ｆ３（Ｘ１，・・・，Ｘｎ）で定義される。したがって、コンフィグレーション空間の（Ｘ１，・・・，Ｘｎ）を規定すると、作業空間の（Ｙ１，Ｙ２，Ｙ３）が決まる。
【００４９】
このロボットＲ１は、ハンドＨもしくは先端部Ｔがロボットの移動部分として機能し、アクチュエータ１０への制御信号の出力によりハンドＨもしくは先端部Ｔの移動制御を行うことができる。
【００５０】
また、図５にはロボットの他の例を示している。ロボットＲ２は、人型のロボットであり、左右一対のアーム部Ａ１，Ａ２とハンドＨ１，Ｈ２を有している。ロボットＲ２は、１０個の関節Ｊ１，・・・，Ｊｎを備えており、１０個の自由度を持つ。ロボットＲ２は、自由度がコンフィグレーション空間における座標系（Ｘ１，・・・，Ｘ１０）で表され、実際の位置姿勢が作業空間における各先端部Ｔ１，Ｔ２の座標系（Ｙ１１，Ｙ１２，Ｙ１３）、（Ｙ２１，Ｙ２２，Ｙ２３）で表される。ロボットＲ２の場合、コンフィグレーション空間の（Ｘ１，・・・，Ｘ１０）を規定すると、作業空間の（Ｙ１１，Ｙ１２，Ｙ１３）、（Ｙ２１，Ｙ２２，Ｙ２３）が決まる。
【００５１】
このロボットＲ２は、ハンドＨ１、Ｈ２もしくは先端部Ｔ１、Ｔ２がロボットの移動部分として機能し、アクチュエータ１０への制御信号の出力によりハンドＨ１、Ｈ２もしくは先端部Ｔ１、Ｔ２の移動制御を行うことができる。
【００５２】
図６（ａ）〜（ｄ）は、テナリー多項式関数Ｔ（ｘ）の一例をグラフ表示した図である。
【００５３】
テナリー多項式関数は、関数値の増加領域、一定領域、減少領域の三領域を有し、一定領域を挟んで増加領域と減少領域が対称に表される多項式関数である。
【００５４】
このテナリー多項式関数Ｔ（ｘ）は、例えば次の式（１）により表される。
【数１】
【００５５】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）である。Ｃ、ａ０は定数である。ｉは０又は正の整数である。
【００５６】
図６（ａ）は、テナリー多項式関数の次数０のＴ０（ｘ）を示したものであり、矩形波状のプロファイルとなっている。図６（ｂ）は、テナリー多項式関数の次数１のＴ１（ｘ）を示したものであり、台形状のプロファイルとなっている。図６（ｃ）は、テナリー多項式関数の次数２のＴ２（ｘ）を示したものであり、滑らかに変化する台形状のプロファイルとなっている。図６（ｄ）は、テナリー多項式関数の次数３のＴ３（ｘ）を示したものであり、滑らかに変化する台形状のプロファイルとなっている。
【００５７】
図７に示すように、テナリー多項式関数Ｔｉ（ｘ）は、Ｔｉ（ｘ）−ｘの座標系においてＴｉ（ｘ）の波形を原点を中心に点対称に振り分けてＴ′ｉ＋１（ｘ）を生成し、このＴ′ｉ＋１（ｘ）を積分することによって、一つ次数の高いＴｉ＋１（ｘ）を生成することができる。
【００５８】
例えば、Ｔ０（ｘ）からＴ２（ｘ）を生成する場合、Ｔ０（ｘ）を対称分割（対称振分け）し積分してＴ１（ｘ）を生成し、Ｔ１（ｘ）を対称分割し積分してＴ２（ｘ）を生成してもよいし、Ｔ０（ｘ）を対称分割した後、さらに対称分割してＴ″２（ｘ）を生成し、Ｔ″２（ｘ）を二回積分してＴ２（ｘ）を生成してもよい。
【００５９】
また、図８に示すように、Ｔ１（ｘ）からＴ２（ｘ）を生成する場合、Ｔ０（ｘ）を積分し、Ｔ０（ｘ）を積分したものをｘ軸を中心に線対称に変換して対称化し、Ｔ０（ｘ）を積分したものを−（ａ２−ａ１）／２だけｘ軸に沿って移動させ、Ｔ０（ｘ）を積分し対称化したものを（ａ２−ａ１）／２だけｘ軸に沿って移動させ、それぞれ移動させたものを合成して、Ｔ２（ｘ）を生成してもよい。
【００６０】
テナリー多項式関数Ｔ（ｘ）におけるＣ、ａ０などの定数は、ロボットの移動部分における動作の制限値（上限値、下限値）に応じて設定すればよい。例えば、テナリー多項式関数Ｔ（ｘ）を用いて移動部分の速度プロファイルを生成する場合、図７において、Ｔ２（ｘ）を速度プロファイルとして設定すると、Ｔ２′（ｘ）は加速度状態を示し、Ｔ２″（ｘ）がジャーク状態を示すこととなる。
【００６１】
このため、Ｔ２″（ｘ）の制限値Ｃ、−Ｃとして移動部分のジャーク上限値、下限値を設定し、Ｔ２′（ｘ）の制限値Ｃ・ａ０、−Ｃ・ａ０として移動部分の加速度上限値、下限値を設定し、Ｔ２（ｘ）の制限値Ｃ・（ａ１−ａ０）・ａ０として移動部分の速度制限値を設定することにより、一義的に定数Ｃ、ａ０、ａ１の値が決まる。
【００６２】
このようなテナリー多項式関数Ｔ（ｘ）は、速度、加速度、ジャークなどの微分係数の制限を満たす演算が迅速に行える。このため、テナリー多項式関数Ｔ（ｘ）を用いて移動経路の演算を行うことにより、速度などの所定の制限を満たす移動経路を迅速に演算することができる。
【００６３】
次に、本実施形態に係る経路計画装置の動作及び経路計画方法について説明する。
【００６４】
図９に本実施形態に係る経路計画装置の動作及び経路計画方法についてのフローチャートを示す。
【００６５】
まず、図９のＳ１０に示すように、移動経路の取得処理が行われる。移動経路の取得処理は、ロボットの移動部分の基本的な移動基本経路を取得する処理であり、例えばロボットに設置されるカメラやセンサなどの周辺環境検出手段によって移動基本経路を取得する。また、ロボットに予め設定された経路を移動基本経路として設定してもよいし、ロボット外部から与えられた経路を移動基本経路としてもよい。
【００６６】
この移動経路の取得処理により、例えば、図２に示すように、移動基本経路Ｐが取得される。なお、この移動基本経路Ｐは平滑化を行う前の仮の経路である。取得された移動基本経路Ｐは経路記録部３２に記録される。
【００６７】
そして、Ｓ２０に移行し、移動経路の区間設定処理が行われる。この区間設定処理は、移動経路を直線経路区間と曲線経路区間に分けて設定する処理である。例えば、図２に示すように、移動基本経路Ｐにおいて、経路の曲折箇所Ｐｃから所定の距離範囲までの区間が曲線経路区間とされ、それ以外の区間が直線経路区間とされる。
【００６８】
そして、Ｓ３０に移行し、移動経路の空間的平滑化処理が行われる。この空間的平滑化処理は、移動経路における曲線経路区間の曲率を小さくし又は曲率半径を大きくして経路の空間的な平滑化を図る処理である。例えば、曲線経路区間において平滑化されたパス微分係数に対する制限を満足するようにカーネル関数の最適カーネルパラメータが計算され、そのカーネル関数によって曲線経路区間の経路軌跡が演算される。
【００６９】
その際、経路軌跡が障害物に衝突しないか否かが判断され、衝突する場合には曲率の大きい経路が演算される。このとき、曲率が設定値以上大きくなってしまう場合には、経路の平滑化が行われず、その曲線経路区間については曲折した二つの直線経路とされる。このような曲折した経路においては、ロボットの移動部分は移動を停止することとなる。
【００７０】
ここで、カーネル関数をＫ（ｘ）とすると、このカーネル関数Ｋ（ｘ）は、次の式（２）に示すように、テナリー多項式関数Ｔ（ｘ）を用いたものが用いられる。
【数２】
【００７１】
この式（２）のカーネル関数Ｋ（ｘ）において、曲線経路区間の曲折箇所の形状に応じて定数ａｎ−２などを設定し、また関数の次数を設定することで適切な経路軌跡を生成することができる。
【００７２】
このような移動経路の空間的平滑化処理によって、移動経路の曲折部が平滑化され、図３に示すように経路区間Ｐ０〜Ｐ９からなる移動経路が生成される。
【００７３】
そして、Ｓ４０に移行し、移動速度平滑化処理が行われる。移動速度平滑化処理は、移動経路における平滑化された速度プロファイルを生成する処理である。例えば、この移動速度平滑化処理は、経路区間の繋ぎ目における速度設定処理を行い、各区間の速度プロファイル生成処理を行い、各区間の速度プロファイルを繋ぎ合わせて移動経路全体の速度プロファイルを生成して、行われる。
【００７４】
経路区間の繋ぎ目における速度設定処理は、ロボットの移動部分の最大許容加速度、最大許容速度、曲線経路区間での制限速度に応じ、区間と区間の間の繋ぎ目における速度設定を行う処理である。
【００７５】
例えば、図１０（ａ）に示すように、まず移動経路の進行方向について、正の加速度を考慮し、フォワードフェーズ処理が行われる。フォワードフェーズ処理は、各区間を最大速度で移動した場合におけるに各区間の繋ぎ目の速度を演算する処理である。
【００７６】
そして、図１０（ｂ）に示すように、移動経路の進行の逆方向について、負の加速度を考慮し、バックフェーズ処理が行われる。バックフェーズ処理は、先の区間での減速や停止を考慮してフォワードフェーズ処理にて演算された速度を下方修正する処理である。このバックフェーズ処理により、各区間の繋ぎ目の速度が修正される。
【００７７】
このようなフォワードフェーズ処理及びバックフェーズ処理を含む速度設定処理を行うことにより、各区間の繋ぎ目の速度が設定され、各区間における初速度、終速度が決定される（図１０（ｃ）参照）。
【００７８】
各区間の速度プロファイル生成処理は、各区間における初速度、終速度、距離、最大許容速度、最大許容加速度、最大許容ジャークなどを考慮して行われる。
【００７９】
図１１に速度プロファイル生成処理における速度プロファイル生成過程の説明図である。
【００８０】
例えば、初速度Ｖ０、終速度Ｖｆの経路区間の速度プロファイルを生成する場合について説明すると、まず、図１１（ａ）に示すように、区間の始点から円滑に加速するプロファイルが生成される。このプロファイルは、初速度Ｖ０から終速度Ｖｆに加速する速度プロファイルである。
【００８１】
このとき、速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、図１２に示すように、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔが設定され、Ｔ２（ｔ）が速度プロファイルとして算出される。
【００８２】
まず、図１２（ａ）に示すように、まず初速度０から最高速Ｖｆ−Ｖ０に増加した後、終速度０となるＴ２（ｔ）が生成される。このとき、Ｔ２（ｔ）の最大加速度、最大ジャークが最大許容加速度、最大許容ジャークとなるようにする。すなわち、Ｔ２（ｔ）の最大加速度、最大ジャークが最大許容加速度、最大許容ジャークとなるようにＴ′２（ｔ）、Ｔ″２（ｔ）を設定し、このように設定したＴ′２（ｔ）、Ｔ″２（ｔ）からＴ２（ｔ）を生成する。これにより、短い時間で移動可能となる速度プロファイルが生成できる。
【００８３】
そして、生成したＴ２（ｔ）をＶ０だけ関数値増加方向に平行移動し、初速度Ｖ０から終速度Ｖｆまでのプロファイルが演算される（図１２（ａ）の右図参照）。
【００８４】
そして、図１１（ｂ）に示すように、区間の終点に向けて減速するプロファイルが生成される。このプロファイルは、速度Ｖ１から終速度Ｖｆに減速する速度プロファイルである。Ｖ１としては、最高許容速度より遅い速度値であって、最高許容速度に近い値が設定される。
【００８５】
このとき、速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、上述した図１２に示す手法を用い、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔを設定し、Ｔ２（ｔ）を速度プロファイルとして算出すればよい。
【００８６】
そして、図１１（ｃ）に示すように、区間の始点から加速するプロファイルの補正が行われる。このプロファイル生成は、初速度Ｖ０から速度Ｖ１に加速する速度プロファイルの生成である。
【００８７】
この速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、上述した図１２に示す手法を用い、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔを設定し、Ｖ０からＶ１に加速するＴ２（ｔ）を速度プロファイルとして算出すればよい。
【００８８】
ここで、図１１（ｃ）に示すように生成された速度プロファイルにおいて、より加速させるために使用可能な距離が設定距離より長い場合には、速度Ｖ１より速い速度Ｖ２が設定され、速度プロファイルが補正される。
【００８９】
すなわち、図１１（ｄ）に示すように、まず、区間の終点に向けて減速する速度プロファイルの補正が行われる。この速度プロファイルの補正は、速度Ｖ２から終速度Ｖｆに減速する速度プロファイルである。Ｖ１としては、最高許容速度より遅い速度値であって、速度Ｖ１より速い速度値である。
【００９０】
このとき、補正速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、上述した図１２に示す手法を用い、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔを設定し、Ｔ２（ｔ）を速度プロファイルとして算出すればよい。
【００９１】
次いで、図１１（ｅ）に示すように、区間の始点から加速する速度プロファイルの補正が行われる。この速度プロファイルの補正は、初速度Ｖ０から速度Ｖ２に加速する速度プロファイルを生成して行われる。
【００９２】
この速度プロファイルの生成は、テナリー多項式関数Ｔ（ｘ）を用いて行われる。例えば、上述した図１２に示す手法を用い、テナリー多項式関数Ｔ（ｘ）の変数ｘとして時間ｔを設定し、Ｖ０からＶ２に加速するＴ２（ｔ）を速度プロファイルとして算出すればよい。
【００９３】
このような各区間の速度プロファイル生成処理によれば、ロボットの移動部分が経路区間内を円滑な速度変化状態で移動し、短時間で移動可能な速度プロファイルを生成することができる。
【００９４】
また、各区間の速度プロファイル生成処理としては、上述した処理に限られず、テナリー多項式関数Ｔ（ｘ）を用いた他の処理によって行われてもよい。
【００９５】
例えば、初速度Ｖ０、終速度Ｖｆの経路区間の速度プロファイルを生成する場合、図１３に示すように、まず、区間の始点から初速度Ｖ０から最高許容速度ＶＭａｘに加速する速度プロファイルを生成し（図１３（ａ））、区間の終点に向けて最高許容速度ＶＭａｘから終速度Ｖｆまで減速する速度プロファイルを生成し（図１３（ｂ））、加速する速度プロファイルと減速する速度プロファイルと間の期間を最高許容速度ＶＭａｘとして速度プロファイルを生成し、それらの速度プロファイルを合成する。
【００９６】
この場合、経路区間を最短時間で移動可能であって平滑化された速度プロファイルを生成することができる。特に経路区間の距離が十分に長い場合に有効である。
【００９７】
このように、各区間の速度プロファイルを生成したら、図１０（ｃ）に示すように、各区間の速度プロファイルを繋ぎ合わせて移動経路全体の速度プロファイルを生成する。
【００９８】
そして、図９のＳ５０に移行し、出力処理が行われる。出力処理は、生成した移動経路の速度プロファイルを経路記録部３２に記録すると共に、その速度プロファイルに応じて移動制御信号をアクチュエータ１０に出力する処理である。これにより、アクチュエータ１０が作動し、ロボットの移動部分が平滑に移動し、短い時間で所定の動作を行う。
【００９９】
以上のように、本実施形態に係る経路計画装置及び経路計画方法によれば、関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて移動体の移動経路上における位置の時間微分量の平滑化を行うことにより、移動体の平滑な移動経路を迅速に算出することができる。
【０１００】
特に、テナリー多項式関数として式（１）に示されるものを用いることにより、移動経路の速度、加速度、ジャークなどの制限や移動経路の軌跡の微分係数に対する制限に対応しつつ平滑な移動経路を迅速に演算することができる。このため、ロボットの移動部分の移動中にリアルタイムで移動経路の演算が可能となる。従って、移動途中に障害物などを検出した場合、それを回避する移動経路を演算して移動停止することなく移動動作を継続することができる。
【０１０１】
また、テナリー多項式関数を用いて移動経路の空間的な平滑化を行うことにより、移動経路の軌跡が平滑なものとなり、移動体の移動経路の円滑かつ高速な移動が可能となる。その際、テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うことにより、移動速度の移動速度が平滑なものとなり、速度、加速度、ジャークなどの制限下で円滑かつ高速な移動が可能となる。
【０１０２】
また、移動経路を複数の区間に分別し、区間と区間の間の速度を設定し、その設定された速度に応じて各区間の初速度と終速度を決定し、初速度と終速度に基づいて区間の速度プロファイルを生成することにより、区間と区間の間の移動速度の連続性が良好なものとなり、移動経路全体の平滑化が図れる。
【０１０３】
この場合、他の多項式補間法と異なり、移動経路の速度、加速度、ジャークなどの複数の微分係数の制限に容易に対応させることができ、移動経路の全体を最適化させるような微調整が可能である。
【０１０４】
なお、上述した実施形態は本発明に係る経路計画装置及び経路計画方法の最良な実施形態を説明したものであり、本発明に係る経路計画装置及び経路計画方法は本実施形態に記載したものに限定されるものではない。本発明に係る経路計画装置及び経路計画方法は、各請求項に記載した要旨を変更しないように実施形態に係る経路計画装置及び経路計画方法を変形し、又は他のものに適用したものであってもよい。
【０１０５】
例えば、ロボット、車両などの自律移動体を移動体とし、自律移動体の移動経路の計画に適用してもよい。この場合であっても、上述した実施形態と同様な作用効果が得られる。
【図面の簡単な説明】
【０１０６】
【図１】本発明の実施形態に係る経路計画装置の構成概要図である。
【図２】移動基本経路の説明図である。
【図３】図２の移動基本経路Ｐにおける直線経路区間と曲線経路区間の説明図である。
【図４】本実施形態に係る経路計画装置及び経路計画方法を適用したロボットの一例を示す図である。
【図５】本実施形態に係る経路計画装置及び経路計画方法を適用したロボットの他の例を示す図である。
【図６】テナリー多項式関数の一例を示した図である。
【図７】図６のテナリー多項式関数の生成方法の説明図である。
【図８】図６のテナリー多項式関数の生成方法の説明図である。
【図９】本実施形態に係る経路計画装置の動作及び経路計画方法を示すフローチャートである。
【図１０】本実施形態に係る経路計画装置の動作及び経路計画方法における移動速度平滑化処理の説明図である。
【図１１】本実施形態に係る経路計画装置の動作及び経路計画方法における移動速度平滑化処理の説明図である。
【図１２】本実施形態に係る経路計画装置の動作及び経路計画方法における移動速度平滑化処理の説明図である。
【図１３】本実施形態に係る経路計画装置の動作及び経路計画方法における移動速度平滑化処理の説明図である。
【符号の説明】
【０１０７】
１…経路計画装置、２…経路演算部、３…記録部、４…出力部、１０…アクチュエータ、２１…移動経路平滑化部、２２…移動速度演算部、２３…全体経路平滑化部、３１…周辺情報記録部、３２…経路記録部、３３…ロボット情報記録部。
【特許請求の範囲】
【請求項１】
移動体の経路計画を行う経路計画装置において、
関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて前記移動体の移動経路上における位置の時間微分量の平滑化を行う移動経路平滑化手段を備えていること、
を特徴とする経路計画装置。
【請求項２】
前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路の空間的な平滑化を行うこと、
を特徴とする請求項１に記載の経路計画装置。
【請求項３】
前記移動経路平滑化手段は、移動経路を直線経路区間と曲線経路区間に分別し、その曲線経路区間の経路の軌跡を前記テナリー多項式関数を用いて平滑化すること、
を特徴とする請求項２に記載の経路計画装置。
【請求項４】
前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うこと、
を特徴とする請求項１に記載の経路計画装置。
【請求項５】
前記移動経路平滑化手段は、移動経路を複数の区間に分別し、その区間における速度プロファイルを前記テナリー多項式関数を用いて生成すること、
を特徴とする請求項４に記載の経路計画装置。
【請求項６】
前記移動経路平滑化手段は、移動経路を複数の区間に分別し、区間と区間の繋ぎ目の速度を設定し、その設定された速度に応じて各区間における初速度と終速度を決定し、この初速度と終速度に基づいて前記区間の速度プロファイルを生成すること、
を特徴とする請求項５に記載の経路計画装置。
【請求項７】
前記テナリー多項式関数は、次の式（１）に示されるものであることを特徴とする請求項１〜６のいずれか一項に記載の経路計画装置。
【数１】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）。Ｃ、ａ０は定数。ｉは０又は正の整数。
【請求項８】
移動体の経路計画を行う経路計画方法において、
関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて前記移動体の移動経路上における位置の時間微分量の平滑化を行う移動経路平滑化工程を備えていること、
を特徴とする経路計画方法。
【請求項９】
前記移動経路平滑化工程は、前記テナリー多項式関数を用いて移動経路の空間的な平滑化を行うこと、
を特徴とする請求項８に記載の経路計画方法。
【請求項１０】
前記移動経路平滑化工程は、移動経路を直線経路区間と曲線経路区間に分別し、その曲線経路区間の経路の軌跡を前記テナリー多項式関数を用いて平滑化すること、
を特徴とする請求項９に記載の経路計画方法。
【請求項１１】
前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うこと、
を特徴とする請求項８に記載の経路計画方法。
【請求項１２】
前記移動経路平滑化工程は、移動経路を複数の区間に分別し、その区間における速度プロファイルを前記テナリー多項式関数を用いて生成すること、
を特徴とする請求項１１に記載の経路計画方法。
【請求項１３】
前記移動経路平滑化工程は、移動経路を複数の区間に分別し、区間と区間の繋ぎ目の速度を設定し、その設定された速度に応じて各区間における初速度と終速度を決定し、この初速度と終速度に基づいて前記区間の速度プロファイルを生成すること、
を特徴とする請求項１２に記載の経路計画方法。
【請求項１４】
前記テナリー多項式関数は、次の式（１）に示されるものであることを特徴とする請求項８〜１３のいずれか一項に記載の経路計画方法。
【数１】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）。Ｃ、ａ０は定数。ｉは０又は正の整数。
【請求項１】
移動体の経路計画を行う経路計画装置において、
関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて前記移動体の移動経路上における位置の時間微分量の平滑化を行う移動経路平滑化手段を備えていること、
を特徴とする経路計画装置。
【請求項２】
前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路の空間的な平滑化を行うこと、
を特徴とする請求項１に記載の経路計画装置。
【請求項３】
前記移動経路平滑化手段は、移動経路を直線経路区間と曲線経路区間に分別し、その曲線経路区間の経路の軌跡を前記テナリー多項式関数を用いて平滑化すること、
を特徴とする請求項２に記載の経路計画装置。
【請求項４】
前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うこと、
を特徴とする請求項１に記載の経路計画装置。
【請求項５】
前記移動経路平滑化手段は、移動経路を複数の区間に分別し、その区間における速度プロファイルを前記テナリー多項式関数を用いて生成すること、
を特徴とする請求項４に記載の経路計画装置。
【請求項６】
前記移動経路平滑化手段は、移動経路を複数の区間に分別し、区間と区間の繋ぎ目の速度を設定し、その設定された速度に応じて各区間における初速度と終速度を決定し、この初速度と終速度に基づいて前記区間の速度プロファイルを生成すること、
を特徴とする請求項５に記載の経路計画装置。
【請求項７】
前記テナリー多項式関数は、次の式（１）に示されるものであることを特徴とする請求項１〜６のいずれか一項に記載の経路計画装置。
【数１】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）。Ｃ、ａ０は定数。ｉは０又は正の整数。
【請求項８】
移動体の経路計画を行う経路計画方法において、
関数値の増加領域、一定領域、減少領域の三領域を有するテナリー多項式関数を用いて前記移動体の移動経路上における位置の時間微分量の平滑化を行う移動経路平滑化工程を備えていること、
を特徴とする経路計画方法。
【請求項９】
前記移動経路平滑化工程は、前記テナリー多項式関数を用いて移動経路の空間的な平滑化を行うこと、
を特徴とする請求項８に記載の経路計画方法。
【請求項１０】
前記移動経路平滑化工程は、移動経路を直線経路区間と曲線経路区間に分別し、その曲線経路区間の経路の軌跡を前記テナリー多項式関数を用いて平滑化すること、
を特徴とする請求項９に記載の経路計画方法。
【請求項１１】
前記移動経路平滑化手段は、前記テナリー多項式関数を用いて移動経路における移動速度の平滑化を行うこと、
を特徴とする請求項８に記載の経路計画方法。
【請求項１２】
前記移動経路平滑化工程は、移動経路を複数の区間に分別し、その区間における速度プロファイルを前記テナリー多項式関数を用いて生成すること、
を特徴とする請求項１１に記載の経路計画方法。
【請求項１３】
前記移動経路平滑化工程は、移動経路を複数の区間に分別し、区間と区間の繋ぎ目の速度を設定し、その設定された速度に応じて各区間における初速度と終速度を決定し、この初速度と終速度に基づいて前記区間の速度プロファイルを生成すること、
を特徴とする請求項１２に記載の経路計画方法。
【請求項１４】
前記テナリー多項式関数は、次の式（１）に示されるものであることを特徴とする請求項８〜１３のいずれか一項に記載の経路計画方法。
【数１】
この式（１）において、次数０のＴ０（ｘ）は、Ｔ０（ｘ）＝Ｃ（−ａ０／２＜ｘ＜ａ０／２）、Ｔ０（ｘ）＝０（ｘ≦−ａ０／２、ｘ≧ａ０／２）。Ｃ、ａ０は定数。ｉは０又は正の整数。
【図１】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【図２】
【図３】
【図４】
【図５】
【図６】
【図７】
【図８】
【図９】
【図１０】
【図１１】
【図１２】
【図１３】
【公開番号】特開２００９−５３９２６（Ｐ２００９−５３９２６Ａ）
【公開日】平成２１年３月１２日（２００９．３．１２）
【国際特許分類】
【出願番号】特願２００７−２２００１４（Ｐ２００７−２２００１４）
【出願日】平成１９年８月２７日（２００７．８．２７）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
【公開日】平成２１年３月１２日（２００９．３．１２）
【国際特許分類】
【出願日】平成１９年８月２７日（２００７．８．２７）
【出願人】（０００００３２０７）トヨタ自動車株式会社 (59,920)
【Ｆターム（参考）】
[ Back to top ]