JP5910748B2 - 軌道生成装置、軌道生成方法および軌道生成プログラム - Google Patents

Description

本発明は、軌道生成装置、軌道生成方法および軌道生成プログラムに関し、特に、人工衛星や航空機等の移動体相互間や該移動体と地上局との間の光空間通信の際に、移動中の目標を追尾するための制御対象の軌道として生成した目標角度や目標角速度に到達する前に、軌道変更が発生した場合であっても正確にかつ短時間に軌道の再生成を行うことが可能な軌道生成装置、軌道生成方法および軌道生成プログラムに関する。より具体的には、軌道変更発生時の軌道として設定されていた出力角速度から軌道を再生成することによって、軌道の不連続を回避することができる軌道生成装置、軌道生成方法および軌道生成プログラムに関する。

図７は、特許文献１の特開平７−３０７７０３号公報「光空間通信装置および光空間通信装置用のミラー駆動機構」に記載された従来技術の軌道生成装置の内部構成を示すブロック構成図である。該特許文献１に記載された軌道生成装置においては、図７に示すように、制御対象１１の角度を現在角度として検出する角度検出器１２、目標を追尾させるための制御対象１１の目標角度および目標角速度を設定する指令生成器１５、角度検出器１２が出力する現在角度と指令生成器１５が設定した目標角度および目標角速度とから目標追尾用の軌道を生成して目標指令として制御器１３に出力する軌道生成器１６を少なくとも備え、制御器１３にて、軌道生成器１６が出力した目標指令と制御対象１１の現在角度とに基づいて、位相補償を施して制御対象１１の制御信号を生成し、該制御信号を、ドライバ１４を経由して制御対象１１に出力することにより、制御対象１１を目標指令として指示した目標角度および目標角速度に指向させる。

つまり、指令生成器１５にて設定した目標角度および目標角速度と角度検出器１２の出力である制御対象１１の現在角度とに基づいて軌道生成器１６にて軌道を生成し、制御器１３への目標指令として出力することによって、制御対象１１を目標指令の指示内容へと指向させるように制御している。この制御は、制御対象１１の現在角度の情報をフィードバック情報として用いたフィードバック制御系である。

図８は、図１に示した従来技術の軌道生成装置にて生成された軌道に関する時間応答を説明するための説明図である。図８において、横軸は時間を示し、縦軸は角速度を示している。図８に示すような軌道生成法を「基本プロファイル」と称している。以下に、図８に示す「基本プロファイル」について説明する。

時刻ｔ＝ｔ_ｓにおいて指令生成器１５から軌道生成指令が与えられ、基本プロファイルが生成されるものとする。ここで、軌道生成指令として与えられる目標角速度をω_ｄとし、初速'０'からの加速継続時間をｔ_αとすると、目標角加速度α_ｄは、次の式（１）で与えられる。

また、加速時間（加速終了時刻）をｔ_１、等速時間（等速終了時刻）をｔ_２、減速時間（減速終了時刻）をｔ_３とし、軌道生成指令として与えられる目標角度をθ_ｄ、時刻ｔにおける軌道生成器１６の出力角度をθ_ｒ、時刻ｔにおける軌道生成器１６の出力角速度をω_ｒとし、軌道生成開始時刻ｔ_ｓ（軌道生成時）における角度検出器２の出力である現在角度をθ_ｍｓとすると、加速時間ｔ_１、等速時間ｔ_２および減速時間ｔ_３は、それぞれ、次の式（２−１）、式（２−２）および式（２−３）で与えられる。

さらに、加速期間Ａ１１（ｔ_ｓ≦ｔ＜ｔ_ｓ＋ｔ_１の期間）、等速期間Ｅ１１（ｔ_ｓ＋ｔ_１≦ｔ＜ｔ_ｓ＋ｔ_２の期間）、減速期間Ｄ１１（ｔ_ｓ＋ｔ_２≦ｔ≦ｔ_ｓ＋ｔ_３の期間）および軌道生成終了後の終了後期間Ｆ１１（ｔ＞ｔ_ｓ＋ｔ_３の期間）それぞれにおける軌道生成器１６の出力角速度ω_ｒは、それぞれ、次の式（３−１）、式（３−２）、式（３−３）および式（３−４）で与えられる。

また、軌道生成器１６の出力角度θ_ｒは、サンプリング周波数をΔｔとすると、次の式（４）で与えられる。

ここで、制御器１３への目標指令としては、軌道生成器１６の出力である出力角度θ_ｒおよび出力角速度ω_ｒを用いる。

特開平７−３０７７０３号公報（第７−１０頁）

前記特許文献１に記載のような従来技術において、基本プロファイルに従って制御対象１１が現在角度θ_ｍｓから目標角度θ_ｄに到達する前に、目標角度θ_ｄに変更が発生した場合には、軌道の修正が必要になるが、この時の問題点について、図９Ａ及び図９Ｂを用いて説明する。図９Ａ及び図９Ｂは、図７に示した従来技術の軌道生成装置の課題について説明するための説明図であり、図９Ａには、横軸に時間、縦軸に軌道生成器１６の出力である目標角度を示し、図９Ｂには、横軸に時間、縦軸に目標角速度を示している。

図９Ａにおいて、太い実線ｌ１は、初期角度から目標角度Ａに達するまでの途中で軌道の再生成がなく、軌道を生成した場合の角度θ_ｒの推移を示し、図９Ｂにおいて、太い実線ｌ３は、初期角度から目標角度Ａに達するまでの途中で軌道の再生成がなく、軌道を生成した場合の角速度ω_ｒの推移を示している。

一方、図９Ａの点線ｌ２は、初期角度Ｃからの加速継続時間ｔ_αの経過毎に、目標角度が目標角度Ａ、目標角度Ｂと交互に変更になって、軌道を再生成する場合の角度θ_ｒの推移を示している。つまり、図９Ａにおいて、軌道Ｏ_１、Ｏ_３、Ｏ_５の場合の目標角度が目標角度Ａであり、軌道Ｏ_２、Ｏ_４の場合の目標角度が目標角度Ｂである。初期角度Ｃからの加速継続時間ｔ_αの経過毎に、図９Ｂに点線ｌ４にて示す角速度ω_ｒの推移のように、角速度ω_ｒが式（３−１）によって目標角速度ω_ｄに一旦は到達するが、同時に、軌道再生成が発生して、ｔ＝ｔ_ｓとなってしまうため、式（３−１）により、角速度ω_ｒ＝０になってしまう。したがって、初期角度Ｃから加速継続時間ｔ_αの経過毎に、図９Ａ、図９Ｂに示すように、角度θ_ｒ、角速度ω_ｒに不連続点が発生し、図９Ａ、図９Ｂの太い実線ｌ１、ｌ３に比べ、点線ｌ２、ｌ４に示すように、目標角度Ａまたは目標角度Ｂに到達するまでに時間がかかるとともに、不連続箇所において制御対象１１に不用意な振動を励起させるという問題がある。

（本発明の目的）
本発明は前述のような問題点に鑑みてなされたものであり、制御対象が目標角度に到達する前に、軌道の変更を要する場合が発生しても、角速度、角度の不連続点を発生させることなく、変更後の軌道を生成することができ、確実かつ短時間に目標角度に到達することが可能な軌道生成装置、軌道生成方法および軌道生成プログラムを提供することを、その目的としている。

前述の課題を解決するため、本発明による軌道生成装置、軌道生成方法および軌道生成プログラムは、主に、次のような特徴的な構成を採用している。

（１）本発明による軌道生成装置は、制御対象と、該制御対象の現在角度を出力する角度検出手段と、該制御対象の目標角度と目標角速度とを設定する指令生成手段と、前記現在角度と前記目標角度と前記目標角速度とに基づいて前記制御対象の軌道を与える出力角速度と出力角度とを生成して出力する軌道生成手段と、を少なくとも備えた軌道生成装置であって、
前記指令生成手段において、現在まで設定していた前記目標角度から新たな新目標角度への変化が発生した時、または、現在まで設定していた前記目標角速度から新たな新目標角速度への変化が発生した時、該変化の発生時点において前記軌道生成手段により生成されていた前記出力角速度と、該変化の発生時点における前記現在角度もしくは該変化の発生時点において前記軌道生成手段により生成されていた前記出力角度とに基づいて、前記軌道生成手段にて、前記新目標角度までの角度または前記新目標角速度までの角速度による変更軌道を再生成することを特徴とする。

（２）本発明による軌道生成方法は、制御対象の現在角度を出力する角度検出ステップと、該制御対象の目標角度と目標角速度とを設定する指令生成ステップと、前記現在角度と前記目標角度と前記目標角速度とに基づいて前記制御対象の軌道を与える出力角速度と出力角度とを生成して出力する軌道生成ステップと、を少なくとも有してなる軌道生成方法であって、
前記指令生成ステップにおいて、現在まで設定していた前記目標角度から新たな新目標角度への変化が発生した時、または、現在まで設定していた前記目標角速度から新たな新目標角速度への変化が発生した時、該変化の発生時点において前記軌道生成ステップにより生成されていた前記出力角速度と、該変化の発生時点における前記現在角度もしくは該変化の発生時点において前記軌道生成ステップにより生成されていた前記出力角度とに基づいて、前記軌道生成ステップにて、前記新目標角度までの角度または前記新目標角速度までの角速度による変更軌道を再生成することを特徴とする。

（３）本発明による軌道生成プログラムは、少なくとも前記（２）に記載の軌道生成方法を、コンピュータによって実行可能なプログラムとして実施していることを特徴とする。

本発明の軌道生成装置、軌道生成方法および軌道生成プログラムによれば、以下のような効果を奏することができる。

すなわち、制御対象が目標角度到達前に目標角度、目標角速度が変更になった場合には、軌道生成手段（軌道生成ステップ）が出力する出力角速度を維持した状態のまま変更軌道を再生成することを可能にしているので、軌道に角速度、角度の不連続点を発生させることなく、変更軌道の再生成が可能であり、制御対象に不用意な振動を励起させることを確実に防止することができる。

また、再生成した変更軌道は、指令生成手段（指令生成ステップ）にて設定した目標角度に到達することができるだけでなく、等速時において指令生成手段（指令生成ステップ）にて設定した目標角速度を満たすことも可能になる。言い換えると、目標角速度は一定の角速度のままにして、目標角度を何度変更しても、等速時には、常に、一定角速度の軌道を生成することができ、制御対象を一定角速度に制御するレート制御を実現することと目標角度に到達することとの両立を図ることが可能である。

本発明の実施形態に係る軌道生成装置の内部構成の一例を示すブロック構成図である。 図１に示す軌道生成装置の軌道生成器において加速からの軌道生成パターンの一例を説明するための説明図である。 図１に示す軌道生成装置の軌道生成器において等速からの軌道生成パターンの一例を説明するための説明図である。 図１に示す軌道生成装置の軌道生成器において減速からの軌道生成パターンの一例を説明するための説明図である。 図１に示す軌道生成装置の軌道生成器において停止からの軌道生成パターンの一例を説明するための説明図である。 図１ないし図５に示した軌道生成装置の動作の一例を説明するためのフローチャートである。 従来技術の軌道生成装置の内部構成を示すブロック構成図である。 図７に示した従来技術の軌道生成装置にて生成された軌道に関する時間応答を説明するための説明図である。 図７に示した従来技術の軌道生成装置の課題について説明するための説明図である。 図７に示した従来技術の軌道生成装置の課題について説明するための説明図である。

以下、本発明による軌道生成装置、軌道生成方法および軌道生成プログラムの好適な実施形態について添付図を参照して説明する。なお、以下の説明においては、本発明による軌道生成装置および軌道生成方法について説明するが、かかる軌道生成方法をコンピュータにより実行可能な軌道生成プログラムとして実施するようにしても良いし、あるいは、軌道生成プログラムをコンピュータにより読み取り可能な記録媒体に記録するようにしても良いことは言うまでもない。

（本発明の特徴）
本発明の実施形態の説明に先立って、本発明の特徴についてその概要をまず説明する。本発明は、制御対象と、該制御対象の現在角度を出力する角度検出手段と、該制御対象の目標角度と目標角速度とを設定する指令生成手段と、前記現在角度と前記目標角度と前記目標角速度とから軌道を生成する軌道生成手段とを少なくとも備えた軌道生成装置において、前記指令生成手段にて、設定していた現在の目標角度から新目標角度への変化または現在の目標角速度から新目標角速度への変化が発生した時、該変化の発生時点において前記軌道生成手段により生成されていた出力角速度と、該変化の発生時点における前記現在角度もしくは該変化の発生時点において前記軌道生成手段により生成されていた出力角度と、に基づいて、前記新目標角度までの角度または角速度による変更軌道を前記軌道生成手段にて再生成することを主要な特徴としている。

而して、制御対象が目標角度到達前であっても、軌道に角速度、角度の不連続点を発生させることなく、変化の発生時点において前記軌道生成手段により生成されていた出力角速度を維持したまま、軌道を変更することが可能になり、制御対象に不用意な振動を励起させることを確実に防止することができるとともに、制御対象をより短時間に目標角度にまで到達させることができる。

（本発明の実施形態の構成例）
次に、本発明による軌道生成装置の実施形態として、その構成例を説明する。図１は、本発明の実施形態に係る軌道生成装置の内部構成の一例を示すブロック構成図である。図１に示す軌道生成装置は、図７に示した従来技術の軌道生成装置の場合と同様、目標追尾用の制御対象１、制御対象１の現在角度を出力する角度検出器２、軌道生成器６が目標指令として生成した軌道と角度検出器２が検出した現在角度とから制御対象１を制御する制御信号を生成する制御器３、制御器３が生成した制御信号を制御対象１に対して出力するドライバ４、制御対象１を目標に追尾させるための目標角度と目標角速度とを設定する指令生成器５、および、角度検出器２が検出した現在角度と指令生成器５が設定した目標角度および目標角速度とに基づいて目標追尾用の制御対象１の軌道を与える出力角速度と出力角度とを目標指令として生成して出力する軌道生成器６を少なくとも含んで構成されている。

ここで、図１に示す軌道生成装置においては、図７に示した従来技術の軌道生成装置の場合と同様、角度検出器２が出力する現在角度と指令生成器５にて指示した目標角度および目標角速度とに基づいて、制御器３にて位相補償を施して制御対象１の制御信号を生成し、該制御信号を、ドライバ４を経由して制御対象１に出力することにより、制御対象１を指令生成器５にて指示した目標角度および目標角速度に指向させる。

つまり、指令生成器５にて設定した目標角度および目標角速度と角度検出器２の出力である制御対象１１の現在角度とに基づいて軌道生成器６にて軌道を生成し、制御器１３への目標指令として出力することによって、制御対象１１を目標指令の指示内容へと指向させるように制御している。この制御は、図７に示した従来技術の軌道生成装置の場合と同様、制御対象１の現在角度の情報をフィードバック情報として用いたフィードバック制御系である。ただし、図１に示す軌道生成装置においては、特に、軌道生成器６が有する機能が、図７に示した従来技術の軌道生成装置の軌道生成器１６の場合とは大きく異なっている。

次に、図１に示す軌道生成装置の軌道生成器６の機能の詳細について説明する。まず、時刻ｔ＝ｔ_ｓにおいて、指令生成器５にて軌道生成指令が与えられた場合（つまり、軌道修正開始時刻ｔ_ｓにおいて目標角度か目標角速度かのいずれかが変更になったことが指令生成器５から軌道生成器６に通知されてきた場合）について、目標角度をθ_ｄ、目標角速度をω_ｄ、目標角速度の正数表示をω_ｄｄ（＞０）、目標角加速度をα_ｄとし、かつ、
（１）目標角度θ_ｄ≧軌道生成器６の出力角度θ_ｒｓの場合は、目標角速度ω_ｄおよび目標角加速度α_ｄが次の式（５−１）で与えられ、
（２）目標角度θ_ｄ＜軌道生成器６の出力角度θ_ｒｓの場合は、目標角速度ω_ｄおよび目標角加速度α_ｄが次の式（５−２）で与えられるものとする。

ここで、ｔ_αは、初速'０'からの加速継続時間であり、θ_ｒｓは、軌道修正開始時刻ｔ_ｓ （軌道修正時）における軌道生成器６の出力角度である。

本実施形態における軌道生成器６における軌道生成のパターンとしては、加速からの軌道生成パターン、等速からの軌道生成パターン、減速からの軌道生成パターン、停止からの軌道生成パターンの４パターンがあり、以下に順次説明する。なお、軌道生成器６内に備えている比較手段によって、目標角度θ_ｄまたは目標角速度ω_ｄの変化の発生時点における軌道生成器６の出力角速度ω_ｒと目標角速度ω_ｄとを比較した結果に基づいて、４つの軌道生成パターンのいずれを用いて変更軌道を再生成するかが選択される。

ここで、加速時間（加速終了時刻）をｔ_１、等速時間（等速終了時刻）をｔ_２、減速時間（減速終了時刻）をｔ_３とする。また、目標角度をθ_ｄ、目標角速度をω_ｄ、時刻ｔにおける軌道生成器６の出力である出力角度をθ_ｒ、時刻ｔにおける軌道生成器６の出力である出力角速度をω_ｒとする。また、軌道修正開始時刻ｔ_ｓ（軌道修正時）における軌道生成器６の出力である出力角度をθ_ｒｓとし、軌道修正時刻ｔ_ｓ（軌道修正時）における軌道生成器６の出力である出力角速度をω_ｒｓとする。

また、角度検出器２の出力である現在角度をθ_ｍとする。また、時刻ｔ＞（軌道修正開始時刻ｔ_ｓ＋減速時間ｔ_３）の関係が成立している時間帯（すなわち、制御対象１が目標角度θ_ｄに到達した後の時間帯）において、指令生成器５から軌道生成指令が与えられた場合は、その時点（軌道修正開始時刻ｔ_ｓ）における軌道生成器６の出力角度θ_ｒｓは、現在角度θ_ｍに変更され、
（軌道生成器６の出力角度θ_ｒｓ＝現在角度θ_ｍ）
になるものとする。

（１）加速からの軌道生成パターン
図２に示すように、加速から軌道を生成する場合は、加速時間（加速終了時刻）ｔ_１、等速時間（等速終了時刻）ｔ_２および減速時間（減速終了時刻）ｔ_３は、それぞれ、次の式（６−１）、式（６−２）および式（６−３）で与えられる。ここで、図２は、図１に示す軌道生成装置の軌道生成器６において加速からの軌道生成パターンの一例を説明するための説明図である。

また、加速期間Ａ１（ｔ_ｓ≦ｔ＜ｔ_ｓ＋ｔ_１の期間）、等速期間Ｅ１（ｔ_ｓ＋ｔ_１≦ｔ＜ｔ_ｓ＋ｔ_２の期間）、減速期間Ｄ１（ｔ_ｓ＋ｔ_２≦ｔ≦ｔ_ｓ＋ｔ_３の期間）および軌道生成終了後の終了後期間Ｆ１（ｔ＞ｔ_ｓ＋ｔ_３の期間）の各期間における軌道生成器６の出力角速度ω_ｒは、それぞれ、次の式（７−１）、式（７−２）、式（７−３）および式（７−４）で与えられる。

すなわち、図２に示すように、軌道生成器６の出力角速度ω_ｒは、加速期間Ａ１においては、目標角加速度α_ｄで加速する式（７−１）で与えられ、等速期間Ｅ１においては、一定角速度の式（７−２）で与えられ、減速期間Ｄ１においては、目標角加速度α_ｄで減速する式（７−３）で与えられて、軌道生成終了後の終了後期間Ｆ１においては、式（７−４）に示すように'０'になる。

したがって、軌道修正時刻ｔ_ｓから減速時間（減速終了時刻）ｔ_３に到達するまでの間の出力角速度ω_ｒの変化によって囲まれる面積は、図２に示すように、
（目標角度θ_ｄ−軌道修正時刻ｔ_ｓにおける軌道生成器６の出力角度θ_ｒｓ）
として与えられる。

また、軌道生成器６の出力である出力角度θ_ｒは、サンプリング周波数をΔｔとすると、次の式（８）で与えられる。

（２）等速からの軌道生成パターン
図３に示すように、等速から軌道を生成する場合は、加速時間（加速終了時刻）ｔ_１、等速時間（等速終了時刻）ｔ_２および減速時間（減速終了時刻）ｔ_３は、それぞれ、次の式（９−１）、式（９−２）および式（９−３）で与えられる。ここで、図３は、図１に示す軌道生成装置の軌道生成器６において等速からの軌道生成パターンの一例を説明するための説明図である。

また、等速期間Ｅ２（ｔ_ｓ≦ｔ＜ｔ_ｓ＋ｔ_２の期間）、減速期間Ｄ２（ｔ_ｓ＋ｔ_２≦ｔ≦ｔ_ｓ＋ｔ_３の期間）および軌道生成終了後の終了後期間Ｆ２（ｔ＞ｔ_ｓ＋ｔ_３の期間）の各期間における軌道生成器６の出力角速度ω_ｒは、それぞれ、次の式（１０−１）、式（１０−２）および式（１０−３）で与えられる。

すなわち、図３に示すように、軌道生成器６の出力角速度ω_ｒは、等速期間Ｅ２においては、一定角速度の式（１０−１）で与えられ、減速期間Ｄ２においては、目標角加速度α_ｄで減速する式（１０−２）で与えられて、軌道生成終了後の終了後期間Ｆ２においては、式（１０−３）に示すように'０'になる。

したがって、軌道修正時刻ｔ_ｓから減速時間（減速終了時刻）ｔ_３に到達するまでの間の出力角速度ω_ｒの変化によって囲まれる面積は、図３に示すように、図２の場合と同様、
（目標角度θ_ｄ−軌道修正時刻ｔ_ｓにおける軌道生成器６の出力角度θ_ｒｓ）
として与えられる。

また、軌道生成器６の出力である出力角度θ_ｒは、サンプリング周波数をΔｔとすると、式（８）と同様の次の式（１１）で与えられる。

（３）減速からの軌道生成パターン
図４に示すように、減速から軌道を生成する場合は、加速時間（この場合は初期減速時間すなわち初期減速終了時刻）ｔ_１、等速時間（等速終了時刻）ｔ_２および減速時間（減速終了時刻）ｔ_３は、それぞれ、次の式（１２−１）、式（１２−２）および式（１２−３）で与えられる。ここで、図４は、図１に示す軌道生成装置の軌道生成器６において減速からの軌道生成パターンの一例を説明するための説明図である。

また、初期減速期間Ａ３（ｔ_ｓ≦ｔ＜ｔ_ｓ＋ｔ_１の期間）、等速期間Ｅ３（ｔ_ｓ＋ｔ_１≦ｔ＜ｔ_ｓ＋ｔ_２の期間）、減速期間Ｄ３（ｔ_ｓ＋ｔ_２≦ｔ≦ｔ_ｓ＋ｔ_３の期間）および軌道生成終了後の終了後期間Ｆ３（ｔ＞ｔ_ｓ＋ｔ_３の期間）の各期間における軌道生成器６の出力角速度ω_ｒは、それぞれ、次の式（１３−１）、式（１３−２）、式（１３−３）および式（１３−４）で与えられる。

すなわち、図４に示すように、軌道生成器６の出力角速度ω_ｒは、初期減速期間Ａ３においては、目標角加速度α_ｄで減速する式（１３−１）で与えられ、等速期間Ｅ３においては、一定角速度の式（１３−２）で与えられ、減速期間Ｄ３においては、目標角加速度α_ｄで減速する式（１３−３）で与えられて、軌道生成終了後の終了後期間Ｆ３においては、式（１３−４）に示すように'０'になる。

したがって、軌道修正時刻ｔ_ｓから減速時間（減速終了時刻）ｔ_３に到達するまでの間の出力角速度ω_ｒの変化によって囲まれる面積は、図４に示すように、図２の場合と同様、
（目標角度θ_ｄ−軌道修正時刻ｔ_ｓにおける軌道生成器６の出力角度θ_ｒｓ）
として与えられる。

また、軌道生成器６の出力である出力角度θ_ｒは、サンプリング周波数をΔｔとすると、式（８）と同様の次の式（１４）で与えられる。

（４）停止からの軌道生成パターン
図５に示すように、停止時間（停止終了時刻）をｔ_０とすると、停止から軌道を生成する場合は、停止時間（停止終了時刻）ｔ_０、加速時間（加速終了時刻）ｔ_１、等速時間（等速終了時刻）ｔ_２および減速時間（減速終了時刻）ｔ_３は、それぞれ、次の式（１５−１）、式（１５−２）、式（１５−３）および式（１５−４）で与えられる。ここで、図５は、図１に示す軌道生成装置の軌道生成器６において停止からの軌道生成パターンの一例を説明するための説明図である。

また、停止期間Ｓ４（ｔ_ｓ≦ｔ＜ｔ_ｓ＋ｔ_０の期間）、加速期間Ａ４（ｔ_ｓ＋ｔ_０≦ｔ＜ｔ_ｓ＋ｔ_１の期間）、等速期間Ｅ４（ｔ_ｓ＋ｔ_１≦ｔ＜ｔ_ｓ＋ｔ_２の期間）、減速期間Ｄ４（ｔ_ｓ＋ｔ_２≦ｔ≦ｔ_ｓ＋ｔ_３の期間）および軌道生成終了後の終了後期間Ｆ４（ｔ＞ｔ_ｓ＋ｔ_３の期間）の各期間における軌道生成器６の出力角速度ω_ｒは、それぞれ、次の式（１６−１）、式（１６−２）、式（１６−３）、式（１６−４）および式（１６−５）で与えられる。

すなわち、図５に示すように、軌道生成器６の出力角速度ω_ｒは、停止期間Ｓ４においては、目標角加速度α_ｄで減速する式（１６−１）で与えられ、加速期間Ａ４においては、目標角加速度α_ｄで加速する式（１６−２）で与えられ、等速期間Ｅ４においては、一定角速度の式（１６−３）で与えられ、減速期間Ｄ４においては、目標角加速度α_ｄで減速する式（１６−４）で与えられて、軌道生成終了後の終了後期間Ｆ４においては、式（１６−５）に示すように'０'になる。

したがって、軌道修正時刻ｔ_ｓから減速時間（減速終了時刻）ｔ_３に到達するまでの間の出力角速度ω_ｒの変化によって囲まれる面積は、図５に示すように、図２の場合と同様、
（目標角度θ_ｄ−軌道修正時刻ｔ_ｓにおける軌道生成器６の出力角度θ_ｒｓ）
として与えられる。

また、軌道生成器６の出力である出力角度θ_ｒは、サンプリング周波数をΔｔとすると、式（８）と同様の次の式（１７）で与えられる。

（実施形態の動作の説明）
次に、図１ないし図５に本発明の一実施形態として示した軌道生成装置の動作について図６のフローチャートを用いて説明する。図６は、図１ないし図５に示した軌道生成装置の動作の一例を説明するためのフローチャートであり、前述した４つの軌道生成パターン（すなわち、加速からの軌道生成パターン、等速からの軌道生成パターン、減速からの軌道生成パターン、停止からの軌道生成パターンの４パターン）を切り替える動作の一例を説明している。

図６のフローチャートにおいて、まず、軌道変更の要否すなわち目標角度θ_ｄまたは目標角速度ω_ｄｄ（＞０）の変更の有無を判定する（ステップＳ１）。指令生成器５において、目標角度θ_ｄまたは目標角速度ω_ｄｄが変更されて、つまり、現在まで設定していた目標角度θ_ｄから新たな新目標角度への変化が発生して、または、現在まで設定していた目標角速度ω_ｄｄ（＞０）から新たな新目標角速度への変化が発生して、軌道の変更が必要な場合には（ステップＳ１のＹＥＳ）、該変化の発生時点として、軌道修正開始時刻ｔ_ｓ（軌道修正時）を、
ｔ_ｓ＝ｔ
と設定することにより、現在時刻ｔに更新する（ステップＳ２）。

次いで、加速から変更軌道を再生成するパターンか否かをまず判定する（ステップＳ３）。前記変化の発生時点の軌道修正開始時刻ｔ_ｓ（軌道修正時）における目標角速度ω_ｄが軌道生成器６の出力角速度ω_ｒｓよりも大きい（０＜ω_ｒｓ＜ω_ｄ、または、０＞ω_ｒｓ＞ω_ｄ）場合は（ステップＳ３のＹＥＳ）、加速から変更軌道を再生成するパターンであるものと判定して、図２の説明図において説明したように、軌道生成時にさらなる加速が必要であるので、加速からの軌道生成パターンを選択して、前述した式（６−１）、式（６−２）および式（６−３）を用いて、加速時間ｔ_１、等速時間ｔ_２および減速時間ｔ_３それぞれを計算する（ステップＳ６）。

一方、前記変化の発生時点の軌道修正開始時刻ｔ_ｓ（軌道修正時）における目標角速度ω_ｄが軌道生成器６の出力角速度ω_ｒｓよりも大きくない場合は（ステップＳ３のＮＯ）、加速からの変更軌道の再生成が不要な場合であるものと判定して、次に、等速から変更軌道を再生成するパターンか否かを判定する（ステップＳ４）。前記変化の発生時点の軌道修正開始時刻ｔ_ｓ（軌道修正時）における目標角速度ω_ｄが軌道生成器６の出力角速度ω_ｒｓと等しい（ω_ｒｓ＝ω_ｄ）場合は（ステップＳ４のＹＥＳ）、等速から変更軌道を再生成するパターンであるものと判定して、図３の説明図において説明したように、等速からの軌道生成パターンを選択して、前述した式（９−１）、式（９−２）および式（９−３）を用いて、加速時間ｔ_１、等速時間ｔ_２および減速時間ｔ_３それぞれを計算する（ステップＳ７）。

また、前記変化の発生時点の軌道修正開始時刻ｔ_ｓ（軌道修正時）における目標角速度ω_ｄが軌道生成器６の出力角速度ω_ｒｓと等しくない場合は（ステップＳ４のＮＯ）、等速からの変更軌道の再生成が不要な場合であるものと判定して、次に、減速から変更軌道を再生成するパターンか否かを判定する（ステップＳ５）。前記変化の発生時点の軌道修正開始時刻ｔ_ｓ（軌道修正時）における目標角速度ω_ｄと軌道生成器６の出力角速度ω_ｒｓとの符号（sgn）を比較して、両者（sgn）の符号が一致した場合は（ステップＳ５のＹＥＳ）、減速から変更軌道を再生成するパターンであるものと判定して、図４の説明図において説明したように、減速からの軌道生成パターンを選択して、前述した式（１２−１）、式（１２−２）および式（１２−３）を用いて、加速時間（初期減速時間）ｔ_１、等速時間ｔ_２および減速時間ｔ_３それぞれを計算する（ステップＳ８）。

また、前記変化の発生時点の軌道修正開始時刻ｔ_ｓ（軌道修正時）における目標角速度ω_ｄと軌道生成器６の出力角速度ω_ｒｓとの符号（sgn）が一致しない場合は（ステップＳ５のＮＯ）、停止からの変更軌道の再生成を行う場合であるものと判定して、前述した式（１５−１）、式（１５−２）、式（１５−３）および式（１５−４）を用いて、停止時間ｔ_０、加速時間ｔ_１、等速時間ｔ_２および減速時間ｔ_３それぞれを計算する（ステップＳ９）。

次に、ステップＳ１において、目標角度θ_ｄまたは目標角速度ω_ｄｄの変更がなく、変更軌道の再生成が不要な場合（ステップＳ１のＮＯ）、または、変更軌道の再生成が必要な場合としてステップＳ６、ステップＳ７、ステップＳ８、ステップＳ９それぞれにおける時間の計算処理が終了した場合には、ステップＳ１０に移行する。ステップＳ１０においては、変更軌道の再生成が不要な場合は、軌道生成器６は、今までと同じ出力角速度ω_ｒ、出力角度θ_ｒを目標指令として出力する（ステップＳ１０）。

一方、変更軌道の再生成が必要な場合おいては、軌道生成器６は、新たな新目標角度までの角度または新たな新目標角速度までの角速度による変更軌道を再生成するために、前記変化の発生時点において軌道生成器６により生成されていた出力角速度ω_ｒｓと、前記変化の発生時点における制御対象１の現在角度もしくは前記変化の発生時点において軌道生成器６により生成されていた出力角度θ_ｒｓとに基づいて、前記変化の発生時点の軌道修正開始時刻ｔ_ｓ（軌道修正時）以降の任意の時刻ｔにおける軌道生成器６の出力角速度ω_ｒおよび該時刻ｔにおける軌道生成器６の出力角度θ_ｒを、それぞれの場合に応じた式（７−１）、式（７−２）、式（７−３）、式（７−４）および式（８）、または、式（１０−１）、式（１０−２）、式（１０−３）および式（１１）、または、式（１３−１）、式（１３−２）、式（１３−３）、式（１３−４）および式（１４）、または、式（１６−１）、式（１６−２）、式（１６−３）、式（１６−４）、式（１６−５）および式（１４）のいずれかに基づいて、計算して、制御器３への目標指令として出力する（ステップＳ１０）。

つまり、ステップＳ１０においては、４つの軌道生成パターン毎に以下のように処理する。ステップＳ６において加速からの軌道生成パターンを実行しようとする場合は、次にステップＳ１において軌道変更が必要になるまで、式（７−１）、式（７−２）、式（７−３）、式（７−４）に基づいて、加速期間Ａ１、等速期間Ｅ１、減速期間Ｄ１、終了後期間Ｆ１の各期間毎の時刻ｔにおける軌道生成器６の出力角速度ω_ｒを計算し、式（８）に基づいて、時刻ｔにおける軌道生成器６の出力角度θ_ｒを計算する。

また、ステップＳ７において等速からの軌道生成パターンを実行しようとする場合は、次にステップＳ１において軌道変更が必要になるまで、式（１０−１）、式（１０−２）、式（１０−３）に基づいて、等速期間Ｅ２、減速期間Ｄ２、終了後期間Ｆ２の各期間毎の時刻ｔにおける軌道生成器６の出力角速度ω_ｒを計算し、式（１１）に基づいて、時刻ｔにおける軌道生成器６の出力角度θ_ｒを計算する。

また、ステップＳ８において減速からの軌道生成パターンを実行しようとする場合は、次にステップＳ１において軌道変更が必要になるまで、式（１３−１）、式（１３−２）、式（１３−３）、式（１３−４）に基づいて、初期減速期間Ａ３、等速期間Ｅ３、減速期間Ｄ３、終了後期間Ｆ３の各期間毎の時刻ｔにおける軌道生成器６の出力角速度ω_ｒを計算し、式（１４）に基づいて、時刻ｔにおける軌道生成器６の出力角度θ_ｒを計算する。

また、ステップＳ９において停止からの軌道生成パターンを実行しようとする場合は、次にステップＳ１において軌道変更が必要になるまで、式（１６−１）、式（１６−２）、式（１６−３）、式（１６−４）、式（１６−５）に基づいて、停止期間Ｓ４、加速期間Ａ４、等速期間Ｅ４、減速期間Ｄ４、終了後期間Ｆ４の各期間毎の時刻ｔにおける軌道生成器６の出力角速度ω_ｒを計算し、式（１７）に基づいて、時刻ｔにおける軌道生成器６の出力角度θ_ｒを計算する。

この結果、軌道生成器６において再生成された変更軌道は、等速時に指令生成器５において設定されていた目標角速度ω_ｄまたは新たに設定された新目標角速度ω_ｄを満たす軌道として生成することができる。

ステップＳ１０の計算を終了すると、次に、現在時刻ｔを、
ｔ＝ｔ＋Δｔ
とサンプリング周期Δｔだけ更新して、次のサンプリング周期に移行した時点で、ステップＳ１に復帰する（ステップＳ１１）。

なお、ステップＳ３、Ｓ４、Ｓ５の各比較処理は、再生成すべき変更軌道の軌道生成パターンとして、４つの軌道生成パターンのいずれに該当しているかを選択するために、前記変化の発生時点における軌道生成器６の出力角速度ω_ｒｓと目標角速度ω_ｄとを比較する比較ステップ（または比較手段）を提供していることになる。また、ステップＳ１において軌道変更の要否を判定する際に用いる目標角速度としては、正数側のω_ｄｄ（＞０）だけを用いる場合に限るものではなく、正負のいずれの場合も含む目標角速度ω_ｄを用いて判定するようにしても勿論構わない。

以上のごとく動作する本実施形態の軌道生成装置や軌道生成方法においては、加速からの軌道生成パターン、等速からの軌道生成パターン、減速からの軌道生成パターン、停止からの軌道生成パターンの４つの軌道生成パターンのいずれかを選択して用いることにより、制御対象１が目標角度に到達する前に、目標角度θ_ｄまたは目標角速度ω_ｄが変更になった場合であっても、軌道生成器６の出力角速度ω_ｒを維持した状態のまま、軌道を再生成することを可能にし、而して、軌道に角速度、角度の不連続点を発生させることなく、変更後の軌道を生成することが可能となり、制御対象１に不用意な振動を励起させる事態を確実に防止することができる。

さらに、本実施形態の軌道生成装置や軌道生成方法においては、加速からの軌道生成パターン、等速からの軌道生成パターン、減速からの軌道生成パターン、停止からの軌道生成パターンの４つの軌道生成パターンのいずれかを選択して用いることにより、制御対象１が目標角度に到達する前に、目標角度θ_ｄまたは目標角速度ω_ｄが変更になった場合であっても、軌道生成器６の出力角速度ω_ｒを維持した状態のまま、軌道を再生成することを可能にし、而して、制御対象１が目標角度θ_ｄに到達するまでの時間を短縮することが可能になる。

また、再生成された変更軌道を、指令生成器５にて設定した目標角度θ_ｄに到達することができるだけでなく、等速時に指令生成器５において設定されていた目標角速度ω_ｄまたは新たに設定された新目標角速度ω_ｄを満たす軌道とすることも可能になる。言い換えると、目標角速度ω_ｄは一定のまま、目標角度θ_ｄを何度変更しても、等速時は、常に、一定角速度の軌道を生成することができ、制御対象１を一定角速度に制御するレート制御の実現と目標角度θ_ｄへの到達との両立が可能になる。

なお、以上の実施形態の説明においては、回転軸として１軸の場合を前提にして説明している。しかし、本発明は、１軸に限定されるものではなく、複数軸存在する場合であっても、各軸それぞれ毎に独立に前述した本実施形態の動作と同じ動作を行わせることにより、本発明に係る軌道生成装置や軌道生成方法を適用することができる。

また、以上の実施形態の説明においては、制御対象１として回転する制御対象を前提にして説明している。しかし、本発明は、制御対象１として、かくのごとき回転する制御対象のみに限定されるものではない。例えば、制御対象１として並進する制御対象であっても構わなく、かかる場合には、前述した角度を位置、角速度を速度に置き換えることによって、本発明に係る軌道生成装置や軌道生成方法を適用することができる。

また、以上の実施形態の説明においては、フィードバック制御系として、制御対象１の現在角度の情報をフィードバックする角度フィードバック制御系を前提にして説明している。しかし、本発明は、角度フィードバック制御系の場合のみに限定されるものではなく、角度を検出することができ、かつ、目標指令として、角度や角速度を用いる制御系であれば、如何なる制御系であっても、本発明に係る軌道生成装置や軌道生成方法を適用することができることは言うまでもない。

（実施形態の効果の説明）
以上に詳細に説明したように、本実施形態においては、以下のような効果を得ることができる。

すなわち、制御対象１が目標角度到達前に目標角度、目標角速度が変更になった場合には、軌道生成器６が出力する出力角速度を維持した状態のまま変更軌道を再生成することを可能にしているので、軌道に角速度、角度の不連続点を発生させることなく、変更軌道を再生成することが可能であり、制御対象１に不用意な振動を励起させることを確実に防止することができる。

また、再生成した変更軌道は、指令生成器５にて設定した目標角度に到達することができるだけでなく、等速時において指令生成器５にて設定した目標角速度を満たすことが可能になる。言い換えると、目標角速度は一定の角速度のままにして、目標角度を何度変更しても、等速時には、常に、一定角速度の軌道を生成することができ、制御対象１を一定角速度に制御するレート制御を実現することと目標角度に到達することとの両立を図ることが可能である。

さらに、本実施形態においては、従来の技術に比して次のような効果も得られる。例えば特開２００６−２５２０８１号公報「ベルト位置決め装置」や特開２００７−３２０５２８号公報「人工衛星の姿勢制御装置」のような従来の技術においては、制御対象１が目標角度到達前に目標角度、目標角速度が変更になった場合のように、旧目標角度に基づいて生成された旧速度プロフィール（加速、等速、減速）による追従の途中において、旧目標角度から新目標角度への目標角度の変更が発生した場合を想定していなく、加速、等速、減速により旧目標角度に到達した後で、改めて、変更後の新目標角度への新速度プロフィール（加速、等速、減速）を生成しなくてはならない。

これに対して、本実施形態においては、変更の発生時点における軌道生成器６の出力角速度ω_ｒｓと目標角速度ω_ｄとの比較結果に基づいて、前述した４つの軌道生成パターンのいずれかを選択して用いることによって、従来の技術における旧目標角度への到達前の減速と新目標角度への移動開始時の加速とが不要になり、旧速度プロフィールによる追従の途中で旧目標角度から新目標角度への変更が発生した場合であっても、変化の発生時点における状況に応じて選択した軌道生成パターンに基づいて、加速、等速、減速の３つの段階（または、停止も含む４つの段階）または等速、減速の２つの段階で目標角度に到達することができ、従来の技術よりも、より短い時間で目標角度に到達することができる。

また、従来の技術の中には、例えば特開２００１−３３０６５６号公報「移動体用方向検出器とその外乱除去方法」のように、ミサイルの誘導則（純追尾航法、比例航法）を参照しているものもある。かくのごときミサイルの誘導則を参照する場合は、ミサイルの初速から目標までの軌道を生成することになるが、軌道追従中のミサイルの目標速度を任意に設定することはできないし、また、ミサイルの誘導則の算出式が複雑であり、他の技術と組み合わせて、ミサイルの目標速度を任意に設定することができるように変更することも容易ではない。

これに対して、本実施形態においては、制御対象１が目標角度到達前に目標角度、目標角速度が変更になった場合であっても、現在角度、変更後の目標角度、目標角速度に基づいて不連続点を伴うことがない変更軌道を再生成することが可能であるので、ミサイルに適用する場合においても、軌道追従中のミサイルの目標速度を任意に設定することができるだけでなく、ミサイルの初速から目標速度まで連続的に軌道を生成することができ、ミサイルの軌道からのずれを小さく抑えることができる。

本発明は、対象とする分野を人工衛星や航空機等の飛翔移動体の分野のみに限るものではなく、地上移動体、水上移動体も含む如何なる移動体の分野であっても差し支えなく、かくのごとき移動を伴う目標を追尾する制御対象の軌道を生成することが必要な分野に容易に適用することが可能である。

以上、本発明の好適な実施形態の構成を説明した。しかし、かかる実施形態は、本発明の単なる例示に過ぎず、何ら本発明を限定するものではないことに留意されたい。本発明の要旨を逸脱することなく、特定用途に応じて種々の変形変更が可能であることが、当業者には容易に理解できよう。

この出願は、２０１２年７月２４日に出願された日本出願特願２０１２−１６３６６９を基礎とする優先権を主張し、その開示の全てをここに取り込む。

１ 制御対象
２ 角度検出器
３ 制御器
４ ドライバ
５ 指令生成器
６ 軌道生成器
１１ 制御対象
１２ 角度検出器
１３ 制御器
１４ ドライバ
１５ 指令生成器
１６ 軌道生成器
Ａ 目標角度
Ａ１ 加速期間
Ａ３ 初期減速期間
Ａ４ 加速期間
Ａ１１ 加速期間
Ｂ 目標角度
Ｃ 初期角度
Ｄ１ 減速期間
Ｄ２ 減速期間
Ｄ３ 等速期間
Ｄ４ 等速期間
Ｄ１１ 等速期間
Ｅ１ 等速期間
Ｅ２ 等速期間
Ｅ３ 等速期間
Ｅ４ 等速期間
Ｅ１１ 等速期間
Ｆ１ 終了後期間
Ｆ２ 終了後期間
Ｆ３ 終了後期間
Ｆ４ 終了後期間
Ｆ１１ 終了後期間
ｌ１ 太い実線
ｌ２ 点線
ｌ３ 太い実線
ｌ４ 点線
Ｏ_１ 軌道
Ｏ_２ 軌道
Ｏ_３ 軌道
Ｏ_４ 軌道
Ｏ_５ 軌道
Ｓ４ 停止期間
ｔ_α 加速継続時間

Claims (7)

1. 制御対象と、該制御対象の現在角度を出力する角度検出手段と、該制御対象の目標角度と目標角速度とを設定する指令生成手段と、前記現在角度と前記目標角度と前記目標角速度とに基づいて前記制御対象の軌道を与える出力角速度と出力角度とを生成して出力する軌道生成手段と、を少なくとも備え、
   前記指令生成手段において、現在まで設定していた前記目標角度から新たな新目標角度への変化が発生した時、または、現在まで設定していた前記目標角速度から新たな新目標角速度への変化が発生した時、該変化の発生時点において前記軌道生成手段により生成されていた前記出力角速度と、該変化の発生時点における前記現在角度もしくは該変化の発生時点において前記軌道生成手段により生成されていた前記出力角度とに基づいて、前記軌道生成手段にて、前記新目標角度までの角度または前記新目標角速度までの角速度による変更軌道を再生成し、
   前記軌道生成手段は、前記変化の発生時点における前記出力角速度と前記目標角速度とを比較する比較手段をさらに備え、
   前記軌道生成手段にて再生成する前記変更軌道として、前記比較手段の比較結果に基づいて、加速からの軌道を生成するパターン、等速からの軌道を生成するパターン、減速からの軌道を生成するパターン、停止からの軌道を生成するパターンの中からいずれかを選択して設定する、
   軌道生成装置。
2. 前記軌道生成手段にて再生成する前記変更軌道として、
   前記変化の発生時点における前記目標角速度が前記出力角速度よりも大きい場合は、加速からの軌道を生成するパターンを選択して設定し、
   前記変化の発生時点における前記目標角速度が前記出力角速度と等しい場合は、等速からの軌道を生成するパターンを選択して設定し、
   前記変化の発生時点における前記目標角速度と前記出力角速度との符号が同じ場合は、減速からの軌道を生成するパターンを選択して設定し、
   前記いずれの場合にも該当しない場合は、停止からの軌道を生成するパターンを選択して設定する、
   請求項１に記載の軌道生成装置。
3. 前記軌道生成手段にて再生成される前記変更軌道を、等速時に前記指令生成手段において設定される前記目標角速度または前記新目標角速度を満たす軌道とすることを特徴とする請求項１又は２に記載の軌道生成装置。
4. 制御対象の現在角度を出力する角度検出ステップと、該制御対象の目標角度と目標角速度とを設定する指令生成ステップと、前記現在角度と前記目標角度と前記目標角速度とに基づいて前記制御対象の軌道を与える出力角速度と出力角度とを生成して出力する軌道生成ステップと、を少なくとも有し、
   前記指令生成ステップにおいて、現在まで設定していた前記目標角度から新たな新目標角度への変化が発生した時、または、現在まで設定していた前記目標角速度から新たな新目標角速度への変化が発生した時、該変化の発生時点において前記軌道生成ステップにより生成されていた前記出力角速度と、該変化の発生時点における前記現在角度もしくは該変化の発生時点において前記軌道生成ステップにより生成されていた前記出力角度とに基づいて、前記軌道生成ステップにて、前記新目標角度までの角度または前記新目標角速度までの角速度による変更軌道を再生成し、
   前記軌道生成ステップは、前記変化の発生時点における前記出力角速度と前記目標角速度とを比較する比較ステップをさらに有し、
   前記軌道生成ステップにて再生成する前記変更軌道として、前記比較ステップの比較結果に基づいて、加速からの軌道を生成するパターン、等速からの軌道を生成するパターン、減速からの軌道を生成するパターン、停止から軌道を生成するパターンの中からいずれかを選択して設定する、
   軌道生成方法。
5. 前記軌道生成ステップにて再生成する前記変更軌道として、
   前記変化の発生時点における前記目標角速度が前記出力角速度よりも大きい場合は、加速からの軌道を生成するパターンを選択して設定し、
   前記変化の発生時点における前記目標角速度が前記出力角速度と等しい場合は、等速からの軌道を生成するパターンを選択して設定し、
   前記変化の発生時点における前記目標角速度と前記出力角速度との符号が同じ場合は、減速からの軌道を生成するパターンを選択して設定し、
   前記いずれの場合にも該当しない場合は、停止から軌道を生成するパターンを選択して設定する、
   請求項４に記載の軌道生成方法。
6. 前記軌道生成ステップにて再生成される前記変更軌道を、等速時に前記指令生成ステップにおいて設定される前記目標角速度または前記新目標角速度を満たす軌道とすることを特徴とする請求項４又は５に記載の軌道生成方法。
7. 請求項４〜６のいずれか１項に記載の軌道生成方法を、コンピュータによって実行可能なプログラムとして実施していることを特徴とする軌道生成プログラム。

Images