JP3795815B2

JP3795815B2 - 回転体の運動制御装置

Info

Publication number: JP3795815B2
Application number: JP2002062227A
Authority: JP
Inventors: 克彦山田
Original assignee: Mitsubishi Electric Corp
Current assignee: Mitsubishi Electric Corp
Priority date: 2002-03-07
Filing date: 2002-03-07
Publication date: 2006-07-12
Anticipated expiration: 2022-03-07
Also published as: JP2003263229A

Description

【０００１】
【発明の属する技術分野】
この発明は、回転体を高速で駆動するための回転体の運動制御装置に関するものである。
【０００２】
【従来の技術】
従来の回転体の運動制御装置は、あらかじめ運動時間とその時間に応じた速度パターンを用意しておいて、その速度パターンを実現するように駆動トルクを与えるようにしている。図７は、例えば特開２０００−１５９４６１号公報に示された従来の回転体の運動制御装置における運動パターンを示すグラフである。この図に示すように、あらかじめ運動時間を定めた何種類かの速度パターン（運動パターン１、運動パターン２、運動パターン３）を用意しておき、回転体の慣性に応じて適切な運動パターンを選択するようにしている。
【０００３】
【発明が解決しようとする課題】
しかしながら、従来の回転体の運動制御装置においては、運動時間があらかじめ定められているので、運動の境界条件（運動の初期時刻および終了時刻における角度、角速度、角加速度などの値）が運動時間によって変化するような場合には対応できず、また、運動の制約条件（回転運動を行う際の駆動トルクや角度、角速度の制約）を満たす中で最短時間となるものを求めることも困難であるという問題があった。
【０００４】
この発明は上述した従来例に係る問題点を解消するためになされたもので、運動の制約条件を満たしながら、運動の境界条件を満足するような運動を行わせることが可能であり、かつ運動時間によって境界条件が変化するような場合にも対応できる回転体の運動制御装置を得ることを目的とするものである。
【０００５】
【課題を解決するための手段】
この発明に係る回転体の運動制御装置は、回転体を駆動するための回転体の運動制御装置において、回転体の角度や角速度、駆動トルクの制約を満たすように回転体の運動時間を決定する運動時間決定部と、前記運動時間決定部で得られた運動時間をもとに回転体の運動の目標値を時間多項式として与える運動パターン生成部とを備え、前記運動時間決定部は、回転運動の境界条件を計算する境界条件計算部と、回転体の運動の制約条件と前記境界条件計算部で得られる運動の境界条件とに基づいて制約条件と境界条件を満たす範囲で最短の運動時間を求める最短時間計算部とを備え、前記境界条件計算部は、前記最短時間計算部で求められる運動時間に基づいて境界条件を求め、その結果を前記再度最短時間計算部に再度入力し、運動時間を求め直す過程を繰り返すことによって回転体の運動時間を決定し、回転体の運動の目標値を時間多項式として与えることを特徴とするものである。
【０００７】
また、前記運動時間決定部は、回転する各軸の制約条件を時間多項式の形で表現し、回転軸の数だけの時間多項式を用いて運動時間を求めることを特徴とするものである。
【０００８】
また、前記運動パターン生成部は、回転角度を時間多項式の形で表現して運動の境界条件を満足させるとともに、複数の時刻で回転角度の３階以上のいくつかの時間微分値を０に設定して回転角加速度の最大値を抑えることを特徴とするものである。
【０００９】
さらに、前記運動パターン生成部は、回転角度を時間多項式の形で表現して運動の境界条件を満足させるとともに、一つまたは複数の時刻で回転角度の２階以上のいくつかの時間微分値を０に設定して回転角加速度の最大値を抑えることを特徴とするものである。
【００１０】
【発明の実施の形態】
実施の形態１．
図１は、この発明の実施の形態１による回転体の運動制御装置を示す構成図である。図１に示すように、回転体１の運動を制御するために、その運動に要する時間を決定する運動時間決定部４と、この運動時間決定部４で求められた運動時間をもとに運動パターンを生成する運動パターン生成部５とを備える。運動パターン生成部５では、回転体１が運動するときの回転角度、角速度、角加速度などを運動時間の関数として求める。これらは、回転体１の運動の目標値となり、回転体１の運動は、この目標値と実際の回転角度、角速度などとの偏差に基づいて回転体１の駆動トルクを定めるフィードバック制御部２と、運動の目標値だけから回転体１の駆動トルクを定めるフィードフォワード制御部３とからそれぞれ出力される駆動トルクの和として定まる駆動トルクによって制御される。
【００１１】
回転体１の運動の目標値を定めるのに、運動時間決定部４と運動パターン生成部５とに分けて構成することで、運動時間決定部４によって運動の制約条件（回転運動を行う際の駆動トルクや角度、角速度の制約）を満たしつつ、運動パターン生成部５によって運動の境界条件（運動の初期時刻および終了時刻における角度、角速度、角加速度などの値）を満足するような運動の目標値を生成することが可能になり、かつ運動時間によって境界条件が変化するような場合にも対応できる。
【００１２】
また、図２は、回転体１の回転運動が複数回ある場合のブロック図である。図２に示すように、回転体１の回転運動が回転運動（１）から回転運動（ｎ）まで連続的に生じるような場合にも、運動時間決定部４と運動パターン生成部５とによって各回転運動の運動パターンを定めることができる。回転運動（１）から回転運動（ｎ）までの全体の運動に要する時間は、個々の回転運動に要する時間の和として得られる。すなわち、回転運動（１）の運動時間をＴ^（１）、回転運動（ｎ）の運動時間をＴ^（ｎ）とした場合、回転運動（１）から回転運動（ｎ）までの全体の運動に要する時間Ｔは、Ｔ＝Ｔ^（１）＋・・・＋Ｔ^（ｎ）として表すことができる。
【００１３】
図３は、運動時間決定部４の働きを示すフローチャートである。運動時間決定部４の働きは、回転体１の運動に要する時間をなるべく短くするように、その時間を決定することである。この計算は、以下に述べるような繰り返し計算となるので、まず、ステップＳ１において、繰り返し回数ｉの初期設定（ｉ＝０）を行う。また、運動時間Ｔについても、ステップＳ２において、Ｔ＝Ｔ_０として初期設定を行う。
【００１４】
ステップＳ３では、繰り返し回数の更新（ｉ＝ｉ＋１）を行う。次に、ステップＳ４において、回転運動の境界条件（θ_ｉ（Ｔ））を計算する。例えば回転体１が移動目標を追尾するアンテナである場合や、地表のある地点を撮像する航空機のような移動体である場合には、回転運動の目標角度は、移動目標や回転体１の運動の影響を受けて運動時間によって異なることになる。したがって、一般に、運動時間Ｔが更新される毎に運動の境界条件を求める必要がある。
【００１５】
次に、ステップＳ５において、運動の境界条件と運動の制約条件をもとに、制約条件を満足する最短の運動時間Ｔ_ｉを求め、Ｔ＝Ｔ_ｉとおく。ステップＳ６では繰り返し計算の収束の判定を行っている。もしも、こうして求められた運動時間Ｔ_ｉと前回求めた運動時間Ｔ_ｉ−１との差の絶対値がある値ΔＴ以下であれば、繰り返し計算は収束したものとみなして、ステップＳ７に示すように、運動時間決定部４の計算は終了する。それ以外の場合には、再度ステップＳ３に戻ってＳ３からＳ６の過程を繰り返して計算精度の向上を図る。
【００１６】
運動時間決定部４の計算の終了は、この図に示すように、前回の計算値との差によってもよいが、繰り返し計算の回数によって行ってもよく、また、これらの両者を併用して繰り返し計算の回数がある値以下であれば、前回の計算値との差によって終了させてもよい。いずれにしても、運動時間Ｔの値によって運動の境界条件を更新して、再度制約条件から運動時間を求め直す過程を繰り返すことが本質的である。
【００１７】
従って、実施の形態１によれば、回転体１の角速度や駆動トルクの制約を満たしながら最短時間で運動するように運動時間を決定する運動時間決定部４と、運動時間決定部４で得られた運動時間をもとに回転体１の運動パターンを時間関数の形で与える運動パターン生成部５とを備えたので、運動の制約条件（回転運動を行う際の駆動トルクや角度、角速度の制約）を満たしながら、運動の境界条件（運動の初期時刻および終了時刻における角度、角速度、角加速度などの値）を満足するような運動を行わせることが可能になり、かつ運動時間によって境界条件が変化するような場合にも対応できる。
【００１８】
また、運動時間決定部４は、回転運動の境界条件を計算する境界条件計算部（図３のステップＳ４）と、境界条件計算部で得られる運動の境界条件と運動の制約条件に基づいて制約条件を満たす最短の運動時間を求める最短時間計算部（図３のステップＳ５）とを備え、境界条件計算部は、最短時間計算部で求められる運動時間に基づいて境界条件を求め、その結果を再度最短時間計算部に入力するようにしたので、運動時間によって境界条件が変化するような場合にも、境界条件計算部と最短時間計算部での計算を必要な回数だけ繰り返し実行することで対応できる。
【００１９】
実施の形態２．
図４は、回転軸が多軸ある場合の運動時間決定部４の働きを示すフローチャートである。回転軸がアンテナにおけるアジマス軸とエレベーション軸のように２軸ある場合や、航空機におけるロール軸、ピッチ軸、ヨー軸のように３軸ある場合など多軸である場合も少なくない。この場合の運動時間決定部４の働きは、図３に示す場合とほとんど同じであるが、運動時間Ｔを決定するためには多軸の制約条件を同時に満たす必要がある。
【００２０】
そのため、図３に示す場合と同様に、ステップＳ１において、繰り返し回数ｉの初期設定（ｉ＝０）を行い、ステップＳ２において、運動時間の初期設定（Ｔ＝Ｔ_０）を行い、ステップＳ３において、繰り返し回数の更新（ｉ＝ｉ＋１）を行い、さらに、境界条件計算部であるステップＳ４において、各回転軸１，・・・，Ｎについて、回転運動の境界条件（θ_１ｉ（Ｔ），・・・，θ_Ｎｉ（Ｔ））を計算した後、最短時間計算部であるステップＳ５において、回転軸ｋに対する運動の制約条件を、
ｆ_ｋ（ｔ）≧０ｋ＝１、・・・、Ｎ
という形に整理して表す。ここで、Ｎは回転軸の数、ｔは時間である。
【００２１】
運動時間Ｔは、すべての回転軸についてこの不等式を満足する必要があるので、ステップＳ８において、
ｆ_１（ｔ）≧０、・・・、ｆ_Ｎ（ｔ）≧０
を同時に満足する最小のｔの値として運動時間Ｔ_ｉを求め、Ｔ＝Ｔ_ｉとおく。
そして、図３の場合と同様に、ステップＳ６において、繰り返し計算の収束の判定を行い、こうして求められた運動時間Ｔ_ｉと前回求めた運動時間Ｔ_ｉ−１との差の絶対値がある値ΔＴ以下であれば、繰り返し計算は収束したものとみなして、ステップＳ７に示すように、運動時間決定部４の計算は終了する。それ以外の場合には、再度ステップＳ３に戻ってＳ３からＳ６の過程を繰り返して計算精度の向上を図る。
このようにすれば、回転軸が多軸であっても１軸の場合と同様に運動時間を定めることができる。
【００２２】
従って、実施の形態２によれば、運動時間決定部４により、回転する各軸の制約条件を時間関数の形で表現し、回転軸の数だけの時間関数を用いて運動時間を求めるようにしたので、同時に回転運動を行う軸が複数個存在する場合にも、すべての軸において駆動トルクや角速度の制約条件を満足するような最短の運動時間を求めることができる。
【００２３】
実施の形態３．
図５は、運動パターン生成部５の出力として得られる回転体１のある回転軸回りの角度、角速度、角加速度の目標値の時間変化の一例である。この場合、運動パターンにおける角加速度がほぼ一定となる時間が長くなってその最大値が抑えられており、駆動トルクと角加速度はほぼ比例関係にあることから、駆動トルクの制約がつよい場合に運動時間を短くする上で効果的なパターンとなる。
【００２４】
このような運動パターンを生成する方法について以下に説明する。まず、ある回転軸回りの角度をθとして、これを次のように時間関数とする。
θ＝ｆ（ｔ）
関数ｆは次の運動の境界条件を満たす必要がある。
ｆ（ｔ_０）＝θ_０、ｄｆ／ｄｔ（ｔ_０）＝ｖ_０、ｄ^２ｆ／ｄｔ^２（ｔ_０）＝ａ_０
ｆ（ｔ_ｆ）＝θ_ｆ、ｄｆ／ｄｔ（ｔ_ｆ）＝ｖ_ｆ、ｄ^２ｆ／ｄｔ^２（ｔ_ｆ）＝ａ_ｆ
【００２５】
上式においてｔ_０、ｔ_ｆは運動の初期時刻と終端時刻を表しており、その差ｔ_ｆ−ｔ_０は運動時間決定部４の出力として得られている。また、θ_０、ｖ_０、ａ_０は初期時刻における角度、角速度、角加速度の値であり、θ_ｆ、ｖ_ｆ、ａ_ｆは終端時刻における角度、角速度、角加速度の値である。さらに、ｄｆ／ｄｔ、ｄ^２ｆ／ｄｔ^２は関数ｆの１階と２階の時間微分を表している。
【００２６】
さらに、角加速度が一定となる時間が長くなるように関数ｆに次の条件を加える。
ｄ^３ｆ／ｄｔ^３（ｔ_ｊ）＝０、ｄ⁴ｆ／ｄｔ⁴（ｔ_ｊ）＝０、・・・、（ｊ＝１、・・・、ｍ）
【００２７】
上式は、時刻ｔ＝ｔ_ｊ（ｊ＝１、・・・、ｍ）となる複数の時刻で回転角度の３階以上のいくつかの時間微分値を０に設定することを意味している。例えば図５においては、Ａ、Ｂ、Ｃ、Ｄで示される４つの時刻において、回転角度の３階および４階の時間微分値を０としており、図５に示されるように、Ａ〜Ｂ、Ｃ〜Ｄ付近において、角加速度の値をほぼ一定としてその最大値を抑えることができる。回転の駆動トルクや角加速度に制約がある場合には、その上限値をとる時間をなるべく長くとることで回転運動の時間を短くすることができるが、図５に示される運動パターンは、その点から効果的なパターンとみなすことができる。
【００２８】
さて、関数ｆは、上述したように運動の境界条件と角加速度をある区間においてほぼ一定とするための条件を満たす必要があるが、そのためには、例えば関数ｆを次のようにおけばよい。
ｆ（ｔ）＝ｃ_ｐｔ^ｐ＋ｃ_ｐ _- _１ｔ^ｐ ^- ^１＋・・・＋ｃ_０
【００２９】
この場合は、関数ｆをｐ次の時間多項式とおいて、その（ｐ＋１）個の係数ｃ_ｐ〜ｃ_０を適当に設定することによって関数ｆの条件を満足させる。初期時刻を与えれば、運動時間決定部４の出力から終端時刻は定まるので、３階以上のいくつかの時間微分値を０にする時刻を適当に設定することによって、関数ｆの満足すべき条件は、係数ｃ_ｐ〜ｃ_０に対する線形の関係式となる。したがって、この関係式を用いれば、係数ｃ_ｐ〜ｃ_０を容易に定めることができ、関数ｆを時間関数として求めることができる。
【００３０】
従って、実施の形態３によれば、運動パターン生成部５により、回転角度を時間関数の形で表現して運動の境界条件を満足させるとともに、複数の時刻で回転角度の３階以上のいくつかの時間微分値を０に設定するようにしたので、運動パターンにおける角加速度がほぼ一定となる時間を長くとることができ、駆動トルクの制約がつよい場合に運動時間を短くする上で効果的なパターンを生成することができる。
【００３１】
実施の形態４．
図６は、運動パターン生成部５の出力として得られる回転体１のある回転軸回りの角度、角速度、角加速度の目標値の時間変化の一例である。この場合、運動パターンにおける角速度がほぼ一定となる時間が長くなっており、回転角速度の最大値が抑えられるので、回転角速度の制約がつよい場合に運動時間を短くする上で効果的なパターンとなる。
【００３２】
このような運動パターンを生成する方法については、角加速度の最大値を抑えるような前述の場合とほぼ同様であるが、これを以下に説明する。まず、ある回転軸回りの角度をθとして、これを次のように時間関数とする。
θ＝ｆ（ｔ）
関数ｆは次の運動の境界条件を満たす必要がある。
ｆ（ｔ_０）＝θ_０、ｄｆ／ｄｔ（ｔ_０）＝ｖ_０、ｄ^２ｆ／ｄｔ^２（ｔ_０）＝ａ_０
ｆ（ｔ_ｆ）＝θ_ｆ、ｄｆ／ｄｔ（ｔ_ｆ）＝ｖ_ｆ、ｄ^２ｆ／ｄｔ^２（ｔ_ｆ）＝ａ_ｆ
【００３３】
さらに、角速度が一定となる時間が長くなるように関数ｆに次の条件を加える。
ｄ^２ｆ／ｄｔ^２（ｔ_ｊ）＝０、ｄ^３ｆ／ｄｔ^３（ｔ_ｊ）＝０、・・・（ｊ＝１、・・・、ｍ）
【００３４】
上式は時刻ｔ＝ｔ_ｊ（ｊ＝１、・・・、ｍ）となる一つまたは複数の時刻で回転角度の２階以上のいくつかの時間微分値を０に設定することを意味している。例えば図６においては、Ａで示される時刻において回転角度の２階および３階の時間微分値を０としており、図６に示されるように、Ａの前後において角速度の値をほぼ一定とすることができる。回転の角速度に制約がある場合には、その上限値をとる時間をなるべく長くとることで回転運動に要する時間を短くすることができるが、図６に示される運動パターンは、その点から効果的なパターンとみなすことができる。
【００３５】
さて、関数ｆは、上述したように運動の境界条件と角加速度をある区間においてほぼ一定とするための条件を満たす必要があるが、そのためには例えば関数ｆを次のようにおけばよい。
ｆ（ｔ）＝ｃ_ｐｔ^ｐ＋ｃ_ｐ _- _１ｔ^ｐ ^- ^１＋・・・＋ｃ_０
【００３６】
この場合は関数ｆをｐ次の時間多項式とおいて、その（ｐ＋１）個の係数ｃ_ｐ〜ｃ_０を適当に設定することによってｆの条件を満足させる。初期時刻を与えれば、運動時間決定部４の出力から終端時刻は定まるので、２階以上のいくつかの時間微分値を０にする時刻を適当に設定することによって、関数ｆの満足すべき条件は、係数ｃ_ｐ〜ｃ_０に対する線形の関係式となる。したがって、この関係式を用いれば係数ｃ_ｐ〜ｃ_０を容易に定めることができ、関数ｆを時間関数として求めることができる。
【００３７】
従って、実施の形態４によれば、運動パターン生成部５により、回転角度を時間関数の形で表現して運動の境界条件を満足させるとともに、一つまたは複数の時刻で回転角度の２階以上のいくつかの時間微分値を０に設定するようにしたので、運動パターンにおける角加速度の変化を抑えることができ、回転角速度の最大値が抑えられるので、回転角速度の制約がつよい場合に運動時間を短くする上で効果的なパターンを生成することができる。
【００３８】
【発明の効果】
以上のように、この発明によれば、回転体を駆動するための回転体の運動制御装置において、回転体の角度や角速度、駆動トルクの制約を満たすように回転体の運動時間を決定する運動時間決定部と、前記運動時間決定部で得られた運動時間をもとに回転体の運動の目標値を時間多項式として与える運動パターン生成部とを備え、前記運動時間決定部は、回転運動の境界条件を計算する境界条件計算部と、回転体の運動の制約条件と前記境界条件計算部で得られる運動の境界条件とに基づいて制約条件と境界条件を満たす範囲で最短の運動時間を求める最短時間計算部とを備え、前記境界条件計算部は、前記最短時間計算部で求められる運動時間に基づいて境界条件を求め、その結果を前記再度最短時間計算部に再度入力し、運動時間を求め直す過程を繰り返すことによって回転体の運動時間を決定し、回転体の運動の目標値を時間多項式として与えるようにしたので、運動の制約条件（回転運動を行う際の駆動トルクや角度、角速度の制約）を満たしながら、運動の境界条件（運動の初期時刻および終了時刻における角度、角速度、角加速度などの値）を満足するような運動を行わせることが可能になり、かつ運動時間によって境界条件が変化するような場合にも対応できる。また、運動時間によって境界条件が変化するような場合にも、境界条件計算部と最短時間計算部での計算を必要な回数だけ繰り返し実行することで対応できる。
【００４０】
また、前記運動時間決定部は、回転する各軸の制約条件を時間多項式の形で表現し、回転軸の数だけの時間多項式を用いて運動時間を求めるようにしたので、同時に回転運動を行う軸が複数個存在する場合にも、すべての軸において駆動トルクや角速度の制約条件を満足するような最短の運動時間を求めることができる。
【００４１】
また、前記運動パターン生成部は、回転角度を時間多項式の形で表現して運動の境界条件を満足させるとともに、複数の時刻で回転角度の３階以上のいくつかの時間微分値を０に設定するようにしたので、運動パターンにおける角加速度がほぼ一定となる時間を長くとることができ、駆動トルクの制約がつよい場合に運動時間を短くする上で効果的なパターンを生成することができる。
【００４２】
さらに、前記運動パターン生成部は、回転角度を時間多項式の形で表現して運動の境界条件を満足させるとともに、一つまたは複数の時刻で回転角度の２階以上のいくつかの時間微分値を０に設定するようにしたので、運動パターンにおける角加速度の変化を抑えることができ、回転角速度の最大値が抑えられるので、回転角速度の制約がつよい場合に運動時間を短くする上で効果的なパターンを生成することができる。
【図面の簡単な説明】
【図１】この発明の実施の形態１による回転体の運動制御装置を示す構成図である。
【図２】この発明の実施の形態１を説明するもので、回転体１の回転運動が複数回ある場合のブロック図である。
【図３】この発明の実施の形態１を説明するもので、図１の運動時間決定部４の働きを示すフローチャートである。
【図４】この発明の実施の形態２を説明するもので、回転軸が多軸ある場合の図１の運動時間決定部４の働きを示すフローチャートである。
【図５】この発明の実施の形態３を説明するもので、運動パターン生成部５の出力として得られる回転体１のある回転軸回りの角度、角速度、角加速度の目標値の時間変化の一例である。
【図６】この発明の実施の形態４を説明するもので、運動パターン生成部５の出力として得られる回転体１のある回転軸回りの角度、角速度、角加速度の目標値の時間変化の一例である。
【図７】例えば特開２０００−１５９４６１号公報に示された従来の回転体の運動制御装置における運動パターンを示すグラフである。
【符号の説明】
１回転体、２フィードバック制御部、３フィードフォワード制御部、４運動時間決定部、５運動パターン生成部。

Claims

回転体を駆動するための回転体の運動制御装置において、
回転体の角度や角速度、駆動トルクの制約を満たすように回転体の運動時間を決定する運動時間決定部と、
前記運動時間決定部で得られた運動時間をもとに回転体の運動の目標値を時間多項式として与える運動パターン生成部と
を備え、
前記運動時間決定部は、回転運動の境界条件を計算する境界条件計算部と、回転体の運動の制約条件と前記境界条件計算部で得られる運動の境界条件とに基づいて制約条件と境界条件を満たす範囲で最短の運動時間を求める最短時間計算部とを備え、前記境界条件計算部は、前記最短時間計算部で求められる運動時間に基づいて境界条件を求め、その結果を前記再度最短時間計算部に再度入力し、運動時間を求め直す過程を繰り返すことによって回転体の運動時間を決定し、回転体の運動の目標値を時間多項式として与える
ことを特徴とする回転体の運動制御装置。
請求項１に記載の回転体の運動制御装置において、
前記運動時間決定部は、回転する各軸の制約条件を時間多項式の形で表現し、回転軸の数だけの時間多項式を用いて運動時間を求めることを特徴とする回転体の運動制御装置。
請求項１または２に記載の回転体の運動制御装置において、
前記運動パターン生成部は、回転角度を時間多項式の形で表現して運動の境界条件を満足させるとともに、複数の時刻で回転角度の３階以上のいくつかの時間微分値を０に設定して回転角加速度の最大値を抑えることを特徴とする回転体の運動制御装置。
請求項１または２に記載の回転体の運動制御装置において、
前記運動パターン生成部は、回転角度を時間多項式の形で表現して運動の境界条件を満足させるとともに、一つまたは複数の時刻で回転角度の２階以上のいくつかの時間微分値を０に設定して回転角加速度の最大値を抑えることを特徴とする回転体の運動制御装置。