JP3394472B2 - スプライン補間による移動経路制御方法 - Google Patents

Description

【発明の詳細な説明】

【０００１】

【発明の属する技術分野】本発明は、スプライン補間に
よる移動経路制御方法に関し、さらに詳細には、自律的
に動的目標物に追従する移動ロボットなどの移動体に用
いて好適なスプライン補間による移動経路制御方法に関
し、特に、自律的に動的目標物を追従するとともに、所
定の移動速度を達成する移動ロボットなどの移動体のサ
ーボ・システムに用いて好適なスプライン補間による移
動経路制御方法に関する。

【０００２】

【従来の技術】従来より、自律的に動的目標物に追従す
るとともに、所定の移動速度を達成する移動ロボットが
知られている。

【０００３】また、所定の作業を行うように、アームな
どを所定の移動速度で移動させる工作機械なども知られ
ている。

【０００４】こうした移動ロボットやアームなどの移動
体の移動経路を制御するにあたっては、様々なスプライ
ン補間が用いられている。

【０００５】即ち、一般には、移動経路の初期位置から
目標位置まで速度および加速度の制限条件を考慮して係
数を固定し、多項式スプライン補間によって、移動体の
サーボ・システムに適した基準関数を得るようにしてい
る。

【０００６】この従来のスプライン補間による移動経路
制御方法によれば、移動体の移動時の速度が、移動開始
時から徐々に加速して高速になってゆき、所定の速度ま
で到達すると徐々に減速して低速になっていくように変
化するものであった。

【０００７】縦軸に速度をとるとともに横軸に時間をと
ったグラフにおいて、従来の手法による移動体の速度の
変化の曲線を示すと、山形形状の曲線となり、最大速
度、最大加速度が瞬間的にしか出せないことになる。

【０００８】即ち、従来のスプライン補間方法において
は、移動経路における初期位置から目標位置までの移動
に時間がかかるという問題点があった。

【０００９】また、従来のスプライン補間方法では、移
動経路の目標位置を固定しなければ計算できないので、
移動経路の目標位置が動的に変わるような移動制御には
使えないという問題点があった。

【００１０】

【発明が解決しようとする課題】本発明は、上記したよ
うな従来の技術の有する問題点に鑑みてなされたもので
あり、その目的とするところは、最大速度、最大加速度
を長時間維持し、縦軸に速度をとるとともに横軸に時間
をとったグラフにおいて、移動体の速度の変化を示す曲
線が台形形状の曲線となるような、より迅速に移動経路
における初期位置から目標位置まで移動することができ
るようにしたスプライン補間による移動経路制御方法を
提供しようとするものである。

【００１１】また、本発明の目的とするところは、移動
経路の目標位置が動的に変わるような場合にも適用可能
なスプライン補間による移動経路制御方法を提供しよう
とするものである。

【００１２】

【課題を解決するための手段】上記目的を達成するため
に、本発明のうち請求項１に記載の発明は、移動体の移
動経路を制御するためのスプライン補間による移動経路
制御方法において、移動体が移動経路の初期位置から目
標位置に到達するまで、所定の周期毎に以下の第１の処
理乃至第４の処理を順次に繰り返し行うようにしたもの
である。第１の処理乃至第４の処理とは、次に示す処理
である。 第１の処理 現在位置に対して、移動経路の初期位置と
目標位置との距離よりも短い距離の仮想目標位置を設定
する処理。 第２の処理 現在位置から仮想目標位置までの区間でス
プライン関数のスプライン係数を計算する処理。 第３の処理 第２の処理により得られたスプライン係数
を用いて現在位置から仮想目標位置までの区間でスプラ
イン補間計算を行う処理。 第４の処理 第３の処理により得られたスプライン補間
計算の結果に基づいて移動体の移動制御を実行する処
理。

【００１３】また、本発明のうち請求項２に記載の発明
は、本発明のうち請求項１に記載の発明において、上記
移動体は、自律的に全方向に移動可能な移動ロボットで
あるようにしたものである。

【００１４】

【００１５】即ち、本発明によるスプライン補間による
移動経路制御方法は、例えば、三次多項式スプラインイ
ン補間方法などの多項式スプライン補間方法において、
図１に示すように、（１）０＜Ｔ^★≪Ｔとなるように、
移動経路の初期位置から目標位置までの補間時間Ｔより
極めて短い時間（以下、「サンプリング周期」と称す
る。）Ｔ^★でスプライン補間を行う毎に、スプライン係
数を再計算する、（２）上記した（１）で行うスプライ
ン係数の計算は、現在位置から移動経路の初期位置と目
標位置ｘ_ｄとの距離よりも短い距離Δｘ_Ｔのところに、
現在位置から目標位置ｘ_ｄの方向にサンプリング周期Ｔ
^★で移動する仮想目標位置を設定することによって行な
う、というものである。

【００１６】なお、図１ならびに上記した（１）および
（２）の説明は、理解を容易にするために一次元につい
て示してある。

【００１７】また、上記の説明から明らかであるが、サ
ンプリング周期Ｔ^★毎にスプライン係数の計算が行われ
るものであるから、現在位置および仮想目標位置はサン
プリング周期Ｔ^★毎に移動することになる。

【００１８】さらに、補間時間とサンプリング周期と
は、例えば、サンプリング周期を補間時間の１０００分
の１程度とするようにしてもよく、具体的には、補間時
間Ｔが１０〜２０ｓ（秒）であるならば、サンプリング
周期Ｔ^★は１０ｍｓ（ミリ秒）とすることができる。

【００１９】なお、サンプリング周期は補間時間よりも
短い時間ならばよく、任意の値を設定することができる
ものである。

【００２０】そして、この本発明によるスプライン補間
による移動経路制御方法によれば、（１）スプラインの
１次導関数、即ち、速度

【数１】 の所定値

【数２】 への漸近収束と、スプラインの１次導関数（１次導関数
は、速度を示す。）

【数３】 の台形形状の曲線が得られる、（２）スプラインの１次
導関数および２次導関数（２次導関数は、加速度を示
す。）を考慮に入れており、（３）スプラインの１次導
関数および２次導関数に対する制約条件を考慮に入れな
がら、確実に所望のスプラインの１次導関数を達成する
ことができる、（４）移動体の制御システムに用いる
と、位置ｘ(ｔ)と速度

【数４】 を制御することができる、（５）わずかなサンプリング
周期Ｔ^★でスプライン係数を再計算することは、移動体
の動作制御に適している、（６）移動体が移動ロボット
である場合には、移動ロボットのサーボ・システムに適
した基準関数を出し、目標位置が動的に変化する場合に
も追従することができるようになる、などという作用効
果がある。

【００２１】つまり、本発明によるスプライン補間によ
る移動経路制御方法は、「（１）現在位置から仮想目標
位置を設定する→（２）スプライン関数のスプライン係
数を計算する→（３）スプライン補間計算を行う→
（４）移動体の移動制御を実行する→（１）現在位置か
ら仮想目標位置を設定する→・・・」というループの処
理を、サンプリング周期Ｔ^★毎に行うものである。

【００２２】即ち、従来の手法と本発明によるスプライ
ン補間による移動経路制御方法とを比較すると、従来の
手法においては、（１）移動経路の初期位置と目標位置
とを固定する、（２）スプライン関数のスプライン係数
を計算して固定する、（３）サンプリング周期毎にスプ
ライン補間計算を行って移動体の移動制御を実行する、
という処理を行うものである。

【００２３】このように、従来の手法においては、移動
経路の初期位置と目標位置とを固定するとともに、スプ
ライン関数のスプライン係数を計算して固定した後に、
サンプリング周期毎に、スプライン補間計算と、スプラ
イン補間計算に基づく移動体の移動制御とが実行される
ものである。

【００２４】一方、本発明によるスプライン補間による
移動経路制御方法によれば、移動経路の初期位置と目標
位置とを固定した後には、「（１）現在位置から仮想目
標位置を設定する→（２）スプライン関数のスプライン
係数を計算する→（３）スプライン補間計算を行う→
（４）移動体の移動制御を実行する→（１）現在位置か
ら仮想目標位置を設定する→・・・」というループの処
理を、サンプリング周期毎に行うものである。

【００２５】その結果、縦軸に速度をとるとともに横軸
に時間をとったグラフにおいて、移動体の速度の変化を
示す曲線が、与えられた最大速度に漸近収束するような
台形形状の曲線となる。

【００２６】従って、最大速度をできるだけ長い時間維
持できるようになるので、移動経路の目標位置までの移
動時間が短くなり、また、移動経路の目標位置が動的に
変化しても、柔軟に追従することができるようになるも
のである。

【００２７】

【発明の実施の形態】以下、添付の図面を参照しなが
ら、本発明によるスプライン補間による移動経路制御方
法の実施の形態の一例を詳細に説明する。

【００２８】まず、次の式１に示す三次多項式を検討し
てみる。

【数５】 ・・・式１ ここで、ａ_ｉは実係数であり、上記したようにＴは補間
時間である（なお、Ｔ＞０である。）。そして、境界条
件を以下の式２に示す通りとする。

【数６】 ・・・式２ ここで、式２の（１）は初期位置を示し、式２の（２）
は初期速度を示し、式２の（３）は目標位置を示し、式
２の（４）は目標位置における速度を示す。

【００２９】式１の１次導関数および２次導関数はそれ
ぞれ、式３および式４に示す通りである。

【数７】 ・・・式３

【数８】 ・・・式４ そして、式１、式２および式３から、式５に示すａ_０、
ａ_１、ａ_２、ａ_３のスプライン係数が得られる。

【００３０】これら式５に示すａ_０、ａ_１、ａ_２、ａ_３
のスプライン係数を求めることにより、スプライン補間
が行われることになる。

【数９】 ・・・式５ ここで、 △ｘ_Ｔ＝ｘ_Ｔ−ｘ_０ である。

【００３１】本発明は、０＜Ｔ^★≪Ｔであり（なお、上
記したようにＴ^★はサンプリング周期である。）、

【数１０】 で、近似値

【数１１】 が、（ｋ−１）とｋとの数値間の境界でｘ（ｔ）と

【数１２】 との連続性が確保されるように、ｔ≧０の場合にサンプ
リング周期Ｔ^★（Ｔ^★＝定数）で式５に示すａ_０、
ａ_１、ａ_２、ａ_３のスプライン係数を再計算するもので
ある。

【００３２】即ち、本発明は、サンプリング周期Ｔ^★毎
に、式５に示すａ_０、ａ_１、ａ_２、ａ_３のスプライン係
数を得るための計算を繰り返し行うものである。

【００３３】つまり、図１に示すように、各ｋ回目の補
間処理は、距離Δｘ_Ｔに位置する仮想目標位置まで行わ
れ、この仮想目標位置は、目標位置ｘ_ｄの方向にサンプ
リング周期Ｔ^★毎にΔｘ_Ｔだけ順次移動されるものであ
る。

【００３４】本発明の目的は、

【数１３】 ・・・式６ となるように、ｘ（ｋＴ^★）（ｋ＝０，１，２，・・
・）から所定のｘ_ｄに補間することによって滑らかな連
続性のある関数ｘ（ｔ）を得ることであり、また、所定
の

【数１４】 に関して式７、式８に示す条件がｎ＞ｋの場合に成り立
つようにすることである。

【数１５】 ・・・式７ ここで、ε（ε＞０）は小さい定数であり、

【数１６】 ・・・式８ ここで、

【数１７】 は所定の定数である。

【００３５】所定のｘ_ｄの近似値における式５に示すス
プライン係数ａ_０、ａ_１、ａ_２、ａ _３の再計算値は、

【数１８】 ・・・式９ であり、ここで、

【数１９】 かつ

【数２０】 である。

【００３６】次に、１次導関数について説明すると、時
点ｔ（ｔ＝Ｔ^★≪Ｔ）の場合には、式３から式１０が得
られる。

【数２１】 ・・・式１０ ここで、

【数２２】 である。

【００３７】式５を考慮に入れると、式１０は、式１１
と書き直される。

【数２３】 ・・・式１１ ここで、

【数２４】 ・・・式１２

【数２５】 ・・・式１３ である。

【００３８】もし、式５に示すスプライン係数ａ_０、ａ
_１、ａ_２、ａ_３をサンプリング周期Ｔ^★で再計算する
と、式１１によって、

【数２６】 ・・・式１４ を導き出すことができる。

【００３９】同様に、次の式１５が得られる。

【数２７】 ・・・式１５ ０＜ｂ＜であるから、式１５の限界値は、式１６であ
る。

【数２８】 ・・・式１６ そして、

【数２９】 ・・・式１７ とすると、式１３は、

【数３０】 ・・・式１８ と書き直される。

【００４０】式１６および式１８によって、以下の式が
得られる。

【数３１】 ・・・式１９ 式１２を式１９に代入すると、

【数３２】 ・・・式２０ となる。

【数３３】 は、これらの計算で補助定数であるので、

【数３４】 として式２０を、

【数３５】 ・・・式２１ と書き直すことができる。

【００４１】ｖが、

【数３６】 ・・・式２２ と算出されれば、式２１によって条件を示す式７が満た
される。

【数３７】 だから、近似値

【数３８】 を使うことができる。

【００４２】次に、２次導関数について説明すると、時
点ｔ＝ｋＴ^★、

【数３９】 であり、サンプリング周期Ｔ^★で式５に示すスプライン
係数ａ_０、ａ_１、ａ_２、ａ_３を再計算すると、式４およ
び式５から、

【数４０】 ・・・式２３ が得られる。

【００４３】式１７および式２２を考慮に入れると、式
２３は、

【数４１】 ・・・式２４ と書き直される。

【００４４】式２４から、

【数４２】 の値は，

【数４３】 と

【数４４】 との偏差によって異なる。式７を考慮に入れると、この
偏差は新たな

【数４５】 を与えると極値を有すると推断できる。式２４による
と、所定の

【数４６】 および

【数４７】 の場合、常に、Δｘ_Ｔ≠0であって、

【数４８】 ・・・式２５ であることがわかる。

【００４５】ここで、

【数４９】 は、所定の定数である。

【数５０】 だから、式２４は、

【数５１】 ・・・式２６ で概算できる。

【００４６】そして、本発明による移動体の移動経路の
スプライン補間方法を適用するためには、式１、式３お
よび式４は、以下のように書き直される。

【数５２】 ・・・式２７

【数５３】 ・・・式２８

【数５４】 ・・・式２９ここで、ｔ∈［ｋＴ^★，（ｋ＋１）Ｔ^★]
（ｋ＝０，１，２，・・・）であり、係数は、

【数５５】 ・・・式３０ である。

【００４７】そして、上記した本発明によるスプライン
補間による移動経路制御方法の主な特徴は、以下の通り
である。

【００４８】・ ｘ（ｔ）および

【数５６】 は連続している。

【００４９】・

【数５７】 は、所定値

【数５８】 に傾き、収束速度はΔｘ_Ｔの値で決まる。

【００５０】・

【数５９】 は、

【数６０】 に傾くが、

【数６１】 は限定され、ゼロに傾く。

【００５１】また、上記した実施の形態は、三次元の場
合に容易に拡張することができる。

【００５２】即ち、所定の座標系に対する平面内の移動
体の座標（ｘ，ｙ，θ）の３次元座標とし、１次導関数
（速度）および２次導関数（加速度）としてそれぞれ、

【数６２】 ・・・式３１ とする。

【００５３】そうすると、各ｘ_ｊ（ｔ）（ｊ＝１，２，
３，）が、本発明によるスプライン補間による移動経路
制御方法によって得られる。

【００５４】次に、上記した本発明によるスプライン補
間による移動経路制御方法を用いたシミュレーションの
結果について説明する。

【００５５】このシミュレーションにおいては、式１、
式５に基づいた従来の軌道（移動経路）の創成と、サン
プリング周期Ｔ^★で式５に示すａ_０、ａ_１、ａ_２、ａ_３
のスプライン係数の再計算を行う式２７、式３０に基づ
いた本発明によるスプライン補間による移動経路制御方
法の軌道創成とを比較することを目的としている。

【００５６】図２には、式１、式５に基づいた従来の軌
道創成の一例が示されている。ここで、境界条件は、

【数６３】 である。最高速度は、０．１５ｍ／ｓにセットされてい
る。

【００５７】一方、図３には、サンプリング周期Ｔ^★で
式５に示すａ_０、ａ_１、ａ_２、ａ_３のスプライン係数の
再計算を行う式２７、式３０に基づく本発明による軌道
創成の一例が示されており，境界条件は図２の例と同じ
である。所望の速度は、

【数６４】 であり、△ｘ_Ｔ＝０．１５ｍである。

【００５８】図３から分かるように、本発明によるスプ
ライン補間による移動経路制御方法によれば、速度

【数６５】 について台形形状の曲線のグラフが得られ、図２の場合
に比べてスプライン補間の総時間を短縮することができ
る。

【００５９】さらに、本発明者の実験によると、本発明
によるスプライン補間による移動経路制御方法の軌道創
成の方法は、図４に示されているような移動体、即ち、
自律的に全方向に移動可能な移動ロボットの制御に適用
することができるものである。

【００６０】そして、図５には、図４に示す移動ロボッ
トの制御システムのブロック構成図が示されている。

【００６１】図５において、符号１０は制御部を示し、
制御部１０には、スプライン補間を実行するブロックＳ
ＰＬ１２と、駆動輪の駆動サーボ・システムを示すブロ
ックＳＣ１４とが設けられている。

【００６２】また、ブロックＤＡＣ１６はデジタル／ア
ナログ変換器を示し、ブロックＭ１８はモーターを示し
ている。

【００６３】そして、図５において、Ｊ（θ）＝ＫＦ
（θ）は、ヤコビ行列であり、Ｋが係数の対角線行列、
Ｆ（θ）が座標変換の直交行列、θが移動ロボットの方
向角度である。

【００６４】なお、この移動ロボットにおいては、移動
ロボットのフレームと周辺物体との相対的距離を計測す
るとともに、所定の座標系に対する移動ロボットの位置
と向きを算出するのに必要なデータを提供するセンサ・
ユニットを備えているものとする。

【００６５】また、図５に示す構成をさらに詳細に説明
すると、ブロックＳＣ１４は、移動ロボットのブロック
ＳＰＬ１２が出力した動作制御命令（基準関数）を実行
する駆動サーボ・ユニットである。

【００６６】さらに、ブロックＳＰＬ１２は、センサ・
ユニット（図示せず）によって移動ロボットに必要な方
位角度（向き）、位置および速度を求めることにより、
移動ロボットが動的目標物を追従することができるよう
に制御するものとする。

【００６７】なお、センサ・ユニットは、移動ロボット
の周辺環境をモニターするとともに、目標物を検知して
位置確認するために使うセンサ・データを収集する働き
をする。こうしたセンサ・ユニットは、例えば、移動ロ
ボットのフレームと周辺物体との相関距離を求めるのに
必要なデータを提供する光センサ（ＣＣＤカメラなどの
センサ）からなるものである。

【００６８】また、センサ・ユニットは、所定の座標系
に対する移動ロボットの位置を求めるためのセンサ（積
算距離計、ジャイロスコープなどのセンサ）も含むもの
である。

【００６９】さらに、上記したブロックＳＣ１４により
示されるサーボ・ユニットは、ブロックＳＰＬ１２が出
力した動作制御命令（基準関数）を実行するサーボ・シ
ステムを含むものであり、サーボ・ユニットは、命令さ
れた移動ロボットの位置と向き（方位角度）および所定
の動作速度を確実に達成する。

【００７０】なお、上記した移動ロボットの制御システ
ムは、具体的にはマイクロ・コンピュータにより実現さ
れるものである。

【００７１】そして、図６における移動ロボットの初期
位置の座標は、ｘ＝０．７ｍ、ｙ＝１．１ｍである。所
望の移動速度は０．１５ｍ／ｓに設定されており、所望
の角速度は０．２８ｒａｄ／ｓに設定されている。

【００７２】まず、移動ロボットは、目標位置（０．
２，２．７）へ移動する。しかし、時間ΔＴ（ΔＴ＝５
秒）の後、目標位置は、位置（−１．６，２．２）に変
わる。図６から分かるように、移動ロボットは、適切に
反応して、新たな目標位置へ移動する。これに対応する
ロボットの動作作動のグラフが図７に示されている。

【００７３】また、本発明者は、上記したような移動ロ
ボットを２台用いた実験も行った。この実験において
は、１台の移動ロボットを先導車とし、もう１台の移動
ロボットを当該先導車を追尾する従動車とするようにな
されている。

【００７４】そして、操作者が、ディスプレー上に示さ
れた環境地図内の目標位置と先導車の動作速度を設定す
る。動作時に、先導車の位置が地図上に描かれ、操作者
は、移動ロボットに搭載されているＣＣＤカメラによっ
て収集された環境に関する視覚データを見る。従動車
は、先導車に見た目には連結されており、視覚上連結さ
れたトレーラのように動く。

【００７５】こうした移動ロボットは、検査業務（例え
ば、発電所の装置の検査業務）や配達作業（例えば、店
舗や病院における配達作業）を行うために用いる。ま
た、娯楽目的に（例えば、ロボットによるサッカー競
技）使うこともできる。

【００７６】

【発明の効果】本発明は、以上説明したように構成され
ているので、より迅速に移動経路における初期位置から
目標位置まで移動することができるようにしたスプライ
ン補間による移動経路制御方法を提供することができる
という優れた効果を奏する。

【００７７】また、本発明は、以上説明したように構成
されているので、移動経路の目標位置が動的に変わるよ
うな場合にも適用可能なスプライン補間による移動経路
制御方法を提供することができるという優れた効果を奏
する。

【図面の簡単な説明】

【図１】本発明によるスプライン補間による移動経路制
御方法の原理を説明するための概念説明図である。

【図２】スプライン係数の再計算を行わない従来の軌道
創成の一例を示すグラフである。

【図３】スプライン係数の再計算を行う本発明による軌
道創成の一例を示すグラフである。

【図４】本発明を実施する全方向移動ロボットの斜視図
である。

【図５】図４に示す移動ロボットの制御システムのブロ
ック構成図である。

【図６】本発明を実施した際における移動ロボットの
（ｘ，ｙ）面上での移動経路を示すグラフである。

【図７】移動ロボットの図６に示した移動を示す図３に
対応するグラフである。

【符号の説明】

１０ 制御部 １２ ブロックＳＰＬ １４ ブロックＳＣ １６ ブロックＤＡＣ １８ ブロックＭ

フロントページの続き (56)参考文献 特開 昭63−106011（ＪＰ，Ａ) 特開 昭64−7110（ＪＰ，Ａ) 特開 昭59−111508（ＪＰ，Ａ) 特開 昭58−3002（ＪＰ，Ａ) (58)調査した分野(Int.Cl.⁷，ＤＢ名) G05D 1/02

Claims (2)

(57)【特許請求の範囲】
1. 【請求項１】 移動体の移動経路を制御するためのスプ
   ライン補間による移動経路制御方法において、 移動体が移動経路の初期位置から目標位置に到達するま
   で、所定の周期毎に以下の第１の処理乃至第４の処理を
   順次に繰り返し行うスプライン補間による移動経路制御
   方法： 第１の処理 現在位置に対して、移動経路の初期位置と
   目標位置との距離よりも短い距離の仮想目標位置を設定
   する、 第２の処理 現在位置から仮想目標位置までの区間でス
   プライン関数のスプライン係数を計算する、 第３の処理 第２の処理により得られたスプライン係数
   を用いて現在位置から仮想目標位置までの区間でスプラ
   イン補間計算を行う、 第４の処理 第３の処理により得られたスプライン補間
   計算の結果に基づいて移動体の移動制御を実行する。
2. 【請求項２】 請求項１に記載のスプライン補間による
   移動経路制御方法において、 前記移動体は、自律的に全方向に移動可能な移動ロボッ
   トであるものであるスプライン補間による移動経路制御
   方法。