JP2019046336A - マルチコプタ及びその制御方法 - Google Patents

Abstract

【課題】実プロセスを適切にモデル化した回転運動及び並進運動の方程式に基づいて、機体の位置及び姿勢を安定に制御できるマルチコプタ及びその制御方法を提供する。【解決手段】制御装置１６が有する第二の目標値決定部２４は、姿勢・位置情報算出部２０が算出したヨー軸周りの角度、及び第一の目標値決定部２２が決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、定常状態を表した並進運動方程式に基づいて、機体１２の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する。第四の目標値決定部３０は、第二の目標値決定部２４が決定した合計推力の目標値、及び座標変換部２８が算出したＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づいて、各ロータ１４(1)〜１４(4)の推力の目標値を決定する。制御装置１６は、この推力の目標値に基づいて各ロータ１４(1)〜１４(4)の回転を制御する。【選択図】図２

Description

本発明は、複数のロータを有したマルチコプタ及びその制御方法に関する。

マルチコプタの位置及び姿勢を安定化する制御方法は、実プロセスのモデル化のしかたによって複数のタイプに分類することができる。例えば、実プロセスをモデル化せずにブラックボックスとするＰＩＤ制御、運動方程式等を基に実プロセスをモデル化して状態方程式に表し、これを用いて制御を行う状態フィードバック制御、状態フィードバック制御に加え、モデル化誤差や外乱・ノイズの影響を抑制すべく、これらの周波数特性を考慮して制御を行うＨ∞制御（Ｈ-infinity control）等である。

従来から多く使用されているのは、ＰＩＤ制御である。例えば姿勢制御の場合、図８に示すように、ＩＭＵ（inertial measurement unit）からＸ，Ｙ，Ｚ軸周りの角速度の検出値ω_x，ω_y，ω_zを取得し、この検出値からロール、ピッチ及びヨー軸周りの角度φ，θ，ψを算出し、これらと目標値（又は指令値）との偏差がゼロになるようにＰＩＤ（比例・微分・積分）の制御量を算出し、算出した制御量を経験則で各ロータの回転数に配分する。そして、各ロータの回転数の目標値をＥＳＣ（Electronic Speed Controller）に送信し、ＥＳＣの指令に基づいて各ロータが駆動される。マルチコプタをＰＩＤ制御する技術は、例えば特許文献１に開示されている。

特開２０１７−５２３８９号公報

実プロセスでは、各ロータの回転数とロール、ピッチ及びヨー軸周りの角度とが絡み合って制御が干渉したり、機体の振動や風等の外乱が影響したりするので、図８に示すようなＰＩＤ制御では、姿勢制御が不安定になる。特許文献１に開示された技術についても同様である。

そこで、運動方程式を基に実プロセスのモデル化を行い、状態フィードバック制御やＨ∞制御を行うことが試行されてきた。しかし、ほとんどがシミュレーションによる検討に留まっており、実用化に至った例は乏しい。その原因として、次のようなことが考えられる。

多くの場合、マルチコプタの回転運動は式（１）に示すオーラ―の方程式で表されている。

ここで、Ｉ_x，Ｉ_y，Ｉ_zは、それぞれＸ，Ｙ，Ｚ軸方向の主慣性モーメント、ω_x，ω_y，ω_zは、それぞれＸ，Ｙ，Ｚ軸周りの角速度、Ｎ_x，Ｎ_y，Ｎ_zは、それぞれＸ，Ｙ，Ｚ軸周りのトルクである。

オイラーの方程式は、連続回転している剛体には当てはまるが、マルチコプタのように、各ロータの推力や反トルクのアンバランスによって断続的に回転する剛体には当てはまらないと考えられる。例えば、ω_x＝ω_y＝ω_z＝０の状態からＸ軸だけにトルクＮ_xが働くと、式（１）中の第１式は、Ｉ_x・dω_x/dt＝Ｎ_xとなる。これは、トルクＮ_xがdt時間加わることによって、角運動量Ｉ_x・dω_x増加することを意味している。そうすると、角運動量保存の法則により、トルクＮ_xが無くなっても回転し続けることになるので、現実と矛盾する。

その他、角加速度dω/dtを求めるには、ノイズが重畳している角速度の検出値ωを微分する必要があり、その処理によってノイズ成分がさらに増大するので、現実的には角加速度dω/dtを安定的に求めることができないという問題もある。

このように、従来から使用され又は検討されているＰＩＤ制御、状態フィードバック制御、Ｈ∞制御は、それぞれに問題点があり、マルチコプタの位置及び姿勢を安定に制御することが困難であった。

本発明は、上記背景技術に鑑みて成されたものであり、実プロセスを適切にモデル化した回転運動及び並進運動の方程式に基づいて、機体の位置及び姿勢を安定に制御できるマルチコプタ及びその制御方法を提供することを目的とする。

本発明は、機体と、前記機体の中心から離れた位置に放射状に配置された複数のロータと、前記各ロータの回転を制御する制御装置とを備えたマルチコプタであって、
前記制御装置は、前記機体の中心を原点として互いに直交するＸ、Ｙ及びＺ軸が定義され、前記機体のＸ、Ｙ及びＺ軸周りの角速度の検出値と、前記機体のＸ、Ｙ及びＺ軸方向の加速度の検出値とを取得する検出値取得部と、
前記検出値取得部が取得した各検出値から、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する姿勢・位置情報算出部と、
前記機体のＸ、Ｙ及びＺ軸方向の位置について、各目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、各偏差がゼロに近づくように、前記機体のＸ、Ｙ及びＺ軸方向の速度の目標値を決定するとともに、前記機体のヨー軸周りの角度について、目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のヨー軸周りの角速度の目標値を決定する第一の目標値決定部と、
前記姿勢・位置情報算出部が算出したヨー軸周りの角度、及び前記第一の目標値決定部が決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、定常状態を表した並進運動方程式に基づいて、前記機体の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する第二の目標値決定部と、
前記機体のロール及びピッチ軸周りの角度について、前記第二の目標値決定部が算出した目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のロール及びピッチ軸周りの角速度の目標値を決定する第三の目標値決定部と、
前記第一の目標値決定部が決定したヨー軸周りの角速度の目標値、及び前記第三の目標値決定部が決定したロール及びピッチ軸周りの角速度の目標値を、姿勢・位置情報算出部が算出したロール及びピッチ軸周りの角度を用いて座標変換し、前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値を算出する座標変換部と、
前記第二の目標値決定部が決定した前記機体の合計推力の目標値、及び前記座標変換部が算出した前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づいて、前記各ロータの推力の目標値を決定する第四の目標値決定部とを備え、
前記第四の目標値決定部が決定した前記各ロータの推力の目標値に基づいて、前記各ロータの回転を制御するマルチコプタである。

前記第四の目標値決定部において、前記機体のＸ，Ｙ，Ｚ軸周りの角速度と各ロータの推力との関係が、各ロータのホバリング時の標準角速度を用いて設定されていることが好ましい。また、前記第一及び第三の目標決定部は、前記偏差を算出した各項目のうち、前記偏差の絶対値が所定値以下の項目は、当該偏差をゼロとみなして、当該項目の速度又は角速度の目標値を決定する構成にすることが好ましい。

姿勢・位置情報算出部は、ランダムウォークを抑制する機能を有したカルマンフィルタを用いて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する構成にすることが好ましい。また、姿勢・位置情報算出部は、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出した後、さらにフィルタリング処理を行って平滑化した値を算出結果とする構成にすることが好ましい。

また、本発明は、機体と、前記機体の中心から離れた位置に放射状に配置された複数のロータとを備えたマルチコプタの制御方法であって、
前記機体の中心を原点として互いに直交するＸ、Ｙ及びＺ軸が定義され、前記機体のＸ、Ｙ及びＺ軸周りの角速度の検出値と、前記機体のＸ、Ｙ及びＺ軸方向の加速度の検出値とを取得する検出値取得ステップと、
前記検出値取得ステップで取得した各検出値から、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する姿勢・位置情報算出ステップと、
前記機体のＸ、Ｙ及びＺ軸方向の位置について、各目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、各偏差がゼロに近づくように、前記機体のＸ、Ｙ及びＺ軸方向の速度の目標値を決定するとともに、前記機体のヨー軸周りの角度について、目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のヨー軸周りの角速度の目標値を決定する第一の目標値決定ステップと、
前記姿勢・位置情報算出ステップで算出したヨー軸周りの角度、及び前記第一の目標値決定ステップで決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、定常状態を表した並進運動方程式に基づいて、前記機体の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する第二の目標値決定ステップと、
前記機体のロール及びピッチ軸周りの角度について、前記第二の目標値決定ステップで算出した目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のロール及びピッチ軸周りの角速度の目標値を決定する第三の目標値決定ステップと、
前記第一の目標値決定ステップで決定したヨー軸周りの角速度の目標値、及び前記第三の目標値決定ステップで決定したロール及びピッチ軸周りの角速度の目標値を、姿勢・位置情報算出ステップで算出したロール及びピッチ軸周りの角度を用いて座標変換し、前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値を算出する座標変換ステップと、
前記第二の目標値決定ステップで決定した前記機体の合計推力の目標値、及び前記座標変換ステップで算出した前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づいて、前記各ロータの推力の目標値を決定する第四の目標値決定ステップとを備え、
前記第四の目標値決定ステップで決定した前記各ロータの推力の目標値に基づいて、前記各ロータの回転を制御するマルチコプタの制御方法である。

前記第四の目標値決定ステップにおいて、前記機体のＸ，Ｙ，Ｚ軸周りの角速度と各ロータの推力との関係が、各ロータのホバリング時の標準角速度を用いて設定されていることが好ましい。また、前記第一及び第三の目標決定ステップにおいて、前記偏差を算出した各項目のうち、前記偏差の絶対値が所定値以下の項目は、当該偏差をゼロとみなして、当該項目の速度又は角速度の目標値を決定することが好ましい。

姿勢・位置情報算出ステップにおいて、ランダムウォークを抑制する機能を有したカルマンフィルタを用いて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出することが好ましい。また、姿勢・位置情報算出ステップにおいて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出した後、さらにフィルタリング処理を行って平滑化した値を算出結果とすることが好ましい。

本発明のマルチコプタ及びその制御方法は、定常状態を表した並進運動方程式、及びジャイロ効果を加えて定常状態を表した回転運動方程式を基に、実プロセスを現実的かつ実用的な手法でモデル化して制御の目標値を算出しているので、機体の位置及び姿勢を非常に安定に制御することができる。

また、姿勢・位置情報を算出するとき、ランダムウォークを抑制する機能を有したカルマンフィルタを使用したり、ノイズ成分を吸収するためのフィルタリング処理を行ったりすることによって、姿勢・位置情報をより高い精度で算出することができる。

本発明のマルチコプタの一実施形態の構造を示す平面図である。 図１のマルチコプタが有する制御装置の内部構成を示す機能ブロック図である。 図１のマルチコプタ及びその周囲に定義された複数の座標軸を示す図である。 図２の制御装置が有する姿勢・位置情報算出部の内部構成を示すブロック図である。 図２の制御装置が有する第一の目標値決定部の内部構成を示すブロック図である。 図２の制御回路が有する第三の目標値決定部の内部構成を示すブロック図である。 図１のマルチコプタの各ロータの回転に基づくジャイロ効果を示す図である。 一般的なＰＩＤ制御の流れを示す図である。

以下、本発明のマルチコプタ及びその制御方法の一実施形態について、図１〜図７に基づいて説明する。この実施形態のマルチコプタ１０は、いわゆるクワッドエックス型と呼ばれるマルチコプタで、図１に示すように、機体１２の中心Ｔから離れた位置に４つのロータ１４(1)〜１４(4)が、機体１２の前後に互いに平行に一対ずつ設けられ、放射状に配置されている。左前側の第一ロータ１４(1)及び右後側の第三ロータ１４(3)は反時計回りに回転し、左後側の第二ロータ１４(2)及び右前側の第四ロータ１４(4)は時計回りに回転することによって、各ロータ１４(1)〜１４(4)の回転による反トルクを打ち消すとともに、揚力を発生させる。機体１２を前方向に移動させるときは、後側の２つのロータ１４(2)，１４(3)の角速度を、前側の２つのロータ１４(1)，１４(4)の角速度よりも速くする。すると、機体１２の後部が浮き上がる形になり、前方向への推進力が発生する。

機体１２の内部には、４つのロータ１４(1)〜１４(4)の回転を制御する制御装置１６が搭載されている。制御装置１６は、図２に示すように、検出値取得部１８、姿勢・位置情報算出部２０、第一の目標値決定部２２、第二の目標値決定部２４、第三の目標値決定部２６、座標変換部２８、第四の目標値決定部３０及び第五の目標値決定部３２を備えている。

本発明のマルチコプタの制御方法の一実施形態は、検出値取得ステップ、姿勢・位置情報算出ステップ、第一の目標値決定ステップ、第二の目標値決定ステップ、第三の目標値決定ステップ、座標変換ステップ、及び第四の目標値決定ステップ、及び第五の目標値決定ステップにより構成され、制御装置１６の各ブロックによって実行される。

以下、制御装置１６の各ブロックの機能を順に説明する。なお、制御装置１６には、図３に示すように、あらかじめ複数の座標軸が定義されている。ＸＹＺの３軸は、機体１２の中心Ｔを原点として互いに直交する軸であり、Ｘ軸が機体１２の前後方向、Ｙ軸が機体１２の左右方向、Ｚ軸が機体１２の上下方向であり、鉛直方向である。また、13軸は、第一及び第三ロータ１４(1)，１４(3)の位置を結ぶ対角の方向、24軸は、第二及び第四ロータ１４(2)，１４(4)の位置を結ぶ対角の方向である。また、機体１２には、図示しないロール、ピッチ、ヨーの３軸がある。

図２に示す検出値取得部１８は、例えばＩＭＵにより構成され、加速度センサとジャイロセンサ等を使用して、機体１２のＸ、Ｙ及びＺ軸周りの角速度の検出値ω_x，ω_y，ω_zと、Ｘ、Ｙ及びＺ軸方向の加速度の検出値α_x，α_y，α_zとを取得する処理を行う（検出値取得ステップ）。

姿勢・位置情報算出部２０は、検出値取得部１８が取得した各検出値から、機体１２のＸ、Ｙ及びＺ軸方向の位置ｘ、ｙ，ｚと、ロール、ピッチ及びヨー軸周りの角度φ，θ，ψとを算出する処理を行う（姿勢・位置情報算出ステップ）。

姿勢・位置情報算出部２０の内部は、例えば図４のように構成することができる。まず、検出値取得部１８が取得したＸ、Ｙ及びＺ軸周りの角速度の検出値ω_x，ω_y，ω_zを座標変換部３４で座標変換し、ロール、ピッチ及びヨー軸周りの角速度φ（ドット），θ（ドット），ψ（ドット）を算出し、その後、演算部３６ａ，３６ｂ，３６ｃで、ロール、ピッチ及びヨー軸周りの角度φ，θ，ψを各々算出する。

演算部３６ａ，３６ｂ，３６ｃは、単純に積分を行う構成にしてもよいが、例えば、所定のカルマンフィルタを使用して角度φ，θ，ψを推定する構成にすることが好ましい。これにより、白色雑音によるランダムウォーク（ゼロ点がランダムにドリフトする現象）を適切に抑制することができ、角度φ，θ，ψを高精度に算出することができる。

演算部３６ａ，３６ｂ，３６ｃで算出した角度φ，θ，ψは、座標変換部３４にフィードバックされ、次回の座標変換にも使用される。また、この角度φ，θ，ψは、そのまま姿勢・位置情報算出部１８の算出結果としてもよいが、例えば、ローパスフィルタ３８ａ，３８ｂ，３８ｃでフィルタリング処理を行い、平滑化した値φ（チルダ）、θ（チルダ）、ψ（チルダ）を算出結果とすることが好ましい。演算部３６ａ，３６ｂ，３６ｃで算出した角度φ，θ，ψには、機体１２の振動やノイズによる高周波成分が含まれているため、フィルタリング処理を行うことによって、これらの外乱を除去することができる。

さらに、姿勢・位置情報算出部１８は、検出値取得部１８が取得したＸ、Ｙ及びＺ軸方向の加速度の検出値α_x，α_y，α_zを座標変換部４０で座標変換し、慣性座標系におけるＸ、Ｙ及びＺ軸方向の加速度の検出値ｘ（ツードット），ｙ（ツードット），ｚ（ツードット）を算出する。この座標変換には、先に演算部３６ａ，３６ｂ，３６ｃで算出した角度φ，θ，ψも利用される。

次に、演算部４２ａ，４２ｂ，４２ｃでＸ、Ｙ及びＺ軸方向の位置ｘ、ｙ，ｚを各々算出する。演算部４２ａ，４２ｂ，４２ｃは、単純に積分を行う構成にしてもよいが、上記と同様に、所定のカルマンフィルタを使用して位置ｘ，ｙ，ｚを推定する構成にすることが好ましい。これにより、白色雑音によるランダムウォーク（ゼロ点がランダムにドリフトする現象）を適切に抑制することができ、位置ｘ，ｙ，ｚを高精度に算出することができる。

演算部４２ａ，４２ｂ，４２ｃで算出した位置ｘ，ｙ，ｚは、そのまま姿勢・位置情報算出部１８の算出結果としてもよいが、上記と同様に、ローパスフィルタ４４ａ，４４ｂ，４４ｃでフィルタリング処理を行い、平滑化した値ｘ（チルダ）、ｙ（チルダ）、ｚ（チルダ）を算出結果とすることが好ましい。演算部４２ａ，４２ｂ，４２ｃで算出した位置ｘ，ｙ，ｚには、機体１２の振動やノイズによる高周波成分が含まれているため、フィルタリング処理を行うことによって、これらの外乱を除去することができる。

図２に示す第一の目標値決定部２２は、機体１２のＸ、Ｙ及びＺ軸方向の位置について、各目標値ｘ_r，ｙ_r，ｚ_rと姿勢・位置情報算出部２０が算出した値ｘ，ｙ，ｚとの偏差ｄｘ，ｄｙ，ｄｚを算出し、各偏差ｄｘ，ｄｙ，ｄｚがゼロに近づくように、機体のＸ、Ｙ及びＺ軸方向の速度の目標値ｘ_r（ドット），ｙ_r（ドット），ｚ_r（ドット）を決定する処理を行う。さらに、機体１２のヨー軸周りの角度について、目標値ψ_rと姿勢・位置情報算出部２０が算出した値ψとの偏差ｄψを算出し、この偏差ｄψがゼロに近づくように、機体１２のヨー軸周りの角速度の目標値ψ_r（ドット）を決定する処理を行う（第一の目標値決定ステップ）。

偏差と目標値の関係は、図５の中のグラフに示すように、偏差の絶対値が大きいほど目標値の絶対値が大きくなる関係であることが好ましく、例えば関数式を用いて規定することができる。この場合、関数式は、１次関数や２次関数でもよいし、その他の関数でもよい。

このとき、偏差を算出した項目のうち、偏差の絶対値が所定の値（ａ１〜ａ４）以下の項目は、当該偏差をゼロとみなして目標値を決定することが好ましい。このような不感帯を設けることにより、微小な偏差に応答して制御がハンチングするのを防止することができる。

また、偏差を算出した項目のうち、偏差の絶対値が所定の値（ｂ１〜ｂ４）を超えた項目は、目標値の絶対値を一定の値に制限し、制御が暴走するのを防止するとよい。なお、目標値の絶対値を一定の値に制限することは、第一の目標値決定部２２に実行させてもよいが、他のブロックの動作を利用して実行することも可能である。例えば、検出部取得部１８が有する増幅回路の出力電圧のサチュレーションを利用し、各検出値の絶対値が一定以上の値にならないように制限すれば、同様の作用効果が得られる。

図２に示す第二の目標値決定部２４は、Ｘ、Ｙ及びＺ軸方向の速度ｘ（ドット），ｙ（ドット），ｚ（ドット）が、第一の目標値決定部２２が決定したＸ、Ｙ及びＺ軸方向の速度の目標値ｘ_r（ドット），ｙ_r（ドット），ｚ_r（ドット）と等しくなるように、機体１２の合計推力の目標値Ｆ_r、ロール及びピッチ軸周りの角度の目標値φ_r，θ_rを決定する処理を行う。具体的には、以下に説明するように、姿勢・位置情報算出部２０が算出したヨー軸周りの角度ψを既知の値とし、定常状態を表した並進運動方程式に基づく演算処理を行う（第二の目標値決定ステップ）。

一般に、航空機の並進運動方程式は、式（２）のように表すことができる。

ここで、ｇは重力加速度、ｍは機体の質量であり、Ａ_x，Ａ_y，Ａ_zは、それぞれＸ，Ｙ及びＺ軸方向の空気抵抗係数である。また、Ｓ_φ，Ｓ_θ，Ｓ_ψは、それぞれsinθ，sinφ，sinψであり、Ｃ_φ，Ｃ_θ，Ｃ_ψは、それぞれcosθ，cosφ，cosψである。

次に、定常状態を考えるため、式（２）の加速度ｘ（ツードット），ｙ（ツードット），ｚ（ツードット）をゼロとすると、次の式（３）の連立方程式が得られる。

そして、式（３）の３つの式を展開することにより、Ｆを求める式（４）、φを求める式（５）、θを求める式（６）をそれぞれ導出することができる。

このように、定常状態を想定することによって、Ｆ_，φ，θは非常にシンプルな式で求めることができる。

第二の目標値決定部２４は、姿勢・位置情報算出部２０が算出したヨー軸周りの角度ψ、及び第一の目標値決定部が決定したＸ、Ｙ及びＺ軸方向の速度の目標値ｘ_r（ドット），ｙ_r（ドット），ｚ_r（ドット）を受け、これらを式（４）、（５）、（６）の右辺に代入して左辺のＦ，φ，θを算出し、各算出結果を、機体の合計推力の目標値Ｆ_r、ロール及びピッチ軸周りの角度の目標値φ_r，θ_rに決定する。

第三の目標値決定部２６は、機体１２のロール及びピッチ軸周りの角度について、第二の目標値決定部２４が算出した目標値φ_r，θ_rと姿勢・位置情報算出部２０が算出した値との偏差ｄφ，ｄθを算出し、この偏差ｄφ，ｄθがゼロに近づくように、機体１２のロール及びピッチ軸周りの角速度の目標値φ_r（ドット），θ_r（ドット）を決定する処理を行う（第三の目標値決定ステップ）。

偏差と目標値の関係は、図６の中のグラフに示すように、偏差の絶対値が大きいほど目標値の絶対値が大きくなる関係であることが好ましく、例えば関数式を用いて規定することができる。この場合、関数式は、１次関数や２次関数でもよいし、その他の関数でもよい。

このとき、偏差を算出した項目のうち、偏差の絶対値が所定の値（ａ５，ａ６）以下の項目は、当該偏差をゼロとみなして目標値を決定することが好ましい。このような不感帯を設けることにより、微小な偏差応答して制御がハンチングするのを防止することができる。

また、偏差を算出した項目のうち、偏差の絶対値が所定の値（ｂ５，ｂ６）を超えた項目は、目標値の絶対値を一定の値に制限し、制御が暴走するのを防止するとよい。なお、目標値の絶対値を一定の値に制限することは、第三の目標値決定部２６に実行させてもよいが、他のブロックの動作を利用して実行することも可能である。例えば、検出部取得部１８が有する増幅回路の出力電圧のサチュレーションを利用し、各検出値の絶対値が一定以上の値にならないように制限すれば、同様の作用効果が得られる。

図２に示す座標変換部２８は、第一の目標値決定部２２が決定したヨー軸周りの角速度の目標値ψ_r（ドット）、第三の目標値決定部２６が決定したロール及びピッチ軸周りの角速度の目標値φ_r（ドット），θ_r（ドット）を座標変換し、機体１２のＸ、Ｙ及びＺ軸周りの角速度の目標値ω_xr，ω_yr，ω_zrを算出する処理を行う（座標変換ステップ）。

一般に、座標変換は、式（７）を用いて行うことができる。具体的には、姿勢・位置情報算出部２０が算出したロール及びピッチ軸周りの角度φ，θを既知の値として右辺のＳ_φ，Ｓ_θ，Ｃ_φ，Ｃ_θに代入するとともに、各目標値ψ_r（ドット），φ_r（ドット），θ_r（ドット）を右辺に代入することによって、左辺の各目標値ω_xr，ω_yr，ω_zrを算出する。

第四の目標決定部３０は、第二の目標値決定部２４が決定した機体１２の合計推力の目標値Ｆ_r、及び座標変換部２８が算出した機体１２のＸ、Ｙ及びＺ軸周りの角速度の目標値ω_xr，ω_yr，ω_zrから、各ロータ１４(1)〜１４(4)の各推力の目標値Ｆ_1r，Ｆ_2r，Ｆ_3r，Ｆ_4rを決定する処理を行う。具体的には、以下に説明するように、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づく演算処理を行う（第四の目標値決定ステップ）。

一般に、ロータの推力はロータの角速度の２乗に比例するので、ロータ推力係数（比例係数）をｋ、各ロータ１４(1)〜１４(4)の角速度をΩ₁，Ω₂，Ω₃，Ω₄とすると、次の式（８）が成立する。

また、図７に示すように、13軸周りの軸トルクをτ₁₃，24軸周りの軸トルクをτ₂₄、機体１２の中心Ｔから各ロータ１４(1)〜１４(4)までの距離（トルクアーム）をｄとすると、次の式（９）が成立する。

次に、ジャイロ効果について考える。第一及び第三ロータ１４(1)，１４(3)は、トルク軸（13軸）と直交するスピン軸周りに慣性モーメントＩを有し、角速度Ω₁，Ω₃で回転しているので、角運動量ＩΩ₁，ＩΩ₃を持っている。そこに軸トルクτ₁₃が加わると、ジャイロ効果によって、スピン軸及びトルク軸（13軸）と直交するプリセッション軸周りに、角速度ω_1p，ω_3pが生じる。同様に、第二及び第四ロータ１４(2)，１４(4)は、トルク軸（24軸）と直交するスピン軸周りに慣性モーメントＩを有し、角速度Ω₂，Ω₄で回転しているので、角運動量ＩΩ₂，ＩΩ₄を持っている。そこに軸トルクτ₂₄が加わると、ジャイロ効果によって、スピン軸及びトルク軸（24軸）と直交するプリッション軸周りに、角速度ω_2p，ω_4pが生じる。各プリセッション軸周りの角速度ω_1p，ω_2p，ω_3p，ω_4pは、次の式（１０）のように表すことができる。

次に、座標変換部２８が算出したＸ及びＹ軸周りの角速度ω_x，ω_yと、各プリセッション軸周りの角速度ω_1p，ω_2p，ω_3p，ω_4pとの関係を考える。ω_x，ω_yとω_1p，ω_2p，ω_3p，ω_4pとの関係は、式（１１）のように表すことができる。

そして、式（１１）に式（１０）を代入してω_1p，ω_2p，ω_3p，ω_4pを消去し、さらに式（９）を代入してτ₁₃，τ₂₄を消去すると、Ｘ、Ｙ及びＺ軸周りの角速度ω_x，ω_yと各ロータの推力Ｆ₁，Ｆ₂，Ｆ₃，Ｆ₄との関係を示す式（１２）が得られる。

さらに、計算を簡単化するため、式（１２）の各ロータの角速度Ω₁，Ω₂，Ω₃，Ω₄をすべて既知の値であるホバリング時の標準角速度Ω_hに近似すると、次の式（１３）が得られる。

このように、Ω₁，Ω₂，Ω₃，Ω₄をすべて標準角速度Ω_hに近似することによって、ω_x，ω_yとＦ₁，Ｆ₂，Ｆ₃，Ｆ₄との関係を、非常にシンプルな式で表すことができる。

次に、座標変換部２８が算出したＺ軸周りの角速度ω_zについて考える。Ｚ軸周りの回転は、各ロータの回転に対する反トルク（時計回り／反時計回り）が関係し、それらの差分によって生じる。一般に、各ロータの反トルクはロータの角速度の２乗に比例し、ロータの推力に比例する。したがって、各ロータ１４(1)〜１４(4)の反トルクをＴ₁，Ｔ₂，Ｔ₃，Ｔ₄、トルク変換係数（推力と反トルクとの比例係数）をμとすると、式（１４）が成立する。なお、式（１４）では、反時計回りの反トルクを正、時計回りの反トルクを負としている。

ここで、反トルクＴ₁，Ｔ₂，Ｔ₃，Ｔ₄が機体に作用し、機体１２がＺ軸周りに角速度ω_zで反時計回りに回転している状態を考える。各ロータ１４(1)〜１４(4)の角運動量Ｌ₁，Ｌ₂，Ｌ₃，Ｌ₄は、Ｌ₁＝ＩΩ₁，Ｌ₂＝ＩΩ₂，Ｌ₃＝ＩΩ₃，Ｌ₄＝ＩΩ₄と表すことができるので、オイラーの回転運動方程式により、次の式（１５）が成立する。

次に、定常状態を考えるため、式（１５）の左辺の第１項をゼロとし、さらに式（１４）を代入してＴ₁，Ｔ₂，Ｔ₃，Ｔ₄を消去すると、次の式（１６）が得られる。

さらに、計算を簡単化するため、式（１６）の各ロータの角速度Ω₁，Ω₂，Ω₃，Ω₄をすべて既知の値であるホバリング時の標準角速度Ω_hに近似すると、次の式（１７）が得られる。

このように、Ω₁，Ω₂，Ω₃，Ω₄をすべて標準角速度Ω_hに近似することによって、ω_zとＦ₁，Ｆ₂，Ｆ₃，Ｆ₄との関係を、非常にシンプルな式で表すことができる。

そして、式（１３）、（１７）の関係、及びＦ＝Ｆ₁＋Ｆ₂＋Ｆ₃＋Ｆ₄の関係をまとめると式（１８）のようになり、これを変形することによって式（１９）が得られる。

第四の目標値決定部３０は、第二の目標値決定部２４が決定した合計推力の目標値Ｆ_r、及び座標変換部２８が算出したＸ、Ｙ及びＺ軸周りの角速度の目標値ω_xr，ω_yr，ω_zrを受け、これらを式（１９）の右辺に代入して左辺のＦ₁，Ｆ₂，Ｆ₃，Ｆ₄を算出し、各算出結果を各ロータ１４(1)〜１４(4)の推力の目標値Ｆ_1r，Ｆ_2r，Ｆ_3r，Ｆ_4rに決定する。

制御装置１６は、各ロータ１４(1)〜１４(4)の推力が、目標値Ｆ_1r，Ｆ_2r，Ｆ_3r，Ｆ_4rになるように、各ロータ１４(1)〜１４(4)の回転を制御する。

例えば、ロータの推力を角速度で制御する場合、次の式（２０）を利用することができる。式（２０）は、上記の式（８）を変形したものである。

具体的には、図２に示すように、第五の目標値決定部３２を設けるとよい。第五の目標値決定部３２は、第四の目標値決定部３０が決定した各ロータ１４(1)〜１４(4)の推力の目標値Ｆ_1r，Ｆ_2r，Ｆ_3r，Ｆ_4rを受け、これらを式（２０）の右辺に代入して左辺のΩ₁，Ω₂，Ω₃，Ω₄を算出し、各算出結果を各ロータ１４(1)〜１４(4)の角速度の目標値Ω_1r，Ω_2r，Ω_3r，Ω_4rに決定する。そして、制御装置１６は、各ロータ１４(1)〜１４(4)の角速度がΩ_1r，Ω_2r，Ω_3r，Ω_4rになるように制御する。

以上説明したように、マルチコプタ１０及び制御装置１６が実行する制御方法は、定常状態を表した並進運動方程式、及びジャイロ効果を加えて定常状態を表した回転運動方程式を基に、実プロセスを現実的かつ実用的にモデル化して制御の目標値を算出しているので、機体１２の位置及び姿勢を非常に安定に制御することができる。

なお、本発明のマルチコプタ及びその制御方法は、上記実施形態に限定されるものではない。例えば、上述したマルチコプタ１０の制御方法に関する式（２）〜式（２０）は一例であり、各式の一部を変更し、より厳密な計算を行うようにしてもよい。

その他、上記のマルチコプタ１０はクワッドエックス型であるが、同様の考え方でクワッドプラス型を想定した並進運動方程式及び回転運動方程式を設定すれば、クワッドプラス型においても同様の効果を得ることができる。トライコプタ、ヘキサコプタ、オクトコプタについても同様である。

１０ マルチコプタ
１２ 機体
１４(1)〜１４(4) 第一〜第四ロータ
１６ 制御装置
１８ 検出値取得部
２０ 位置・姿勢情報算出部
２２ 第一の目標値決定部
２４ 第二の目標値決定部
２６ 第三の目標値決定部
２８ 座標変換部
３０ 第四の目標値決定部
３２ 第五の目標値決定部

Claims (10)

1. 機体と、前記機体の中心から離れた位置に放射状に配置された複数のロータと、前記各ロータの回転を制御する制御装置とを備えたマルチコプタにおいて、
   前記制御装置は、
   前記機体の中心を原点として互いに直交するＸ、Ｙ及びＺ軸が定義され、前記機体のＸ、Ｙ及びＺ軸周りの角速度の検出値と、前記機体のＸ、Ｙ及びＺ軸方向の加速度の検出値とを取得する検出値取得部と、
   前記検出値取得部が取得した各検出値から、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する姿勢・位置情報算出部と、
   前記機体のＸ、Ｙ及びＺ軸方向の位置について、各目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、各偏差がゼロに近づくように、前記機体のＸ、Ｙ及びＺ軸方向の速度の目標値を決定するとともに、
   前記機体のヨー軸周りの角度について、目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のヨー軸周りの角速度の目標値を決定する第一の目標値決定部と、
   前記姿勢・位置情報算出部が算出したヨー軸周りの角度、及び前記第一の目標値決定部が決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、定常状態を表した並進運動方程式に基づいて、前記機体の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する第二の目標値決定部と、
   前記機体のロール及びピッチ軸周りの角度について、前記第二の目標値決定部が算出した目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のロール及びピッチ軸周りの角速度の目標値を決定する第三の目標値決定部と、
   前記第一の目標値決定部が決定したヨー軸周りの角速度の目標値、及び前記第三の目標値決定部が決定したロール及びピッチ軸周りの角速度の目標値を、姿勢・位置情報算出部が算出したロール及びピッチ軸周りの角度を用いて座標変換し、前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値を算出する座標変換部と、
   前記第二の目標値決定部が決定した前記機体の合計推力の目標値、及び前記座標変換部が算出した前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づいて、前記各ロータの推力の目標値を決定する第四の目標値決定部とを備え、
   前記第四の目標値決定部が決定した前記各ロータの推力の目標値に基づいて、前記各ロータの回転を制御することを特徴とするマルチコプタ。
2. 前記第四の目標値決定部において、前記機体のＸ，Ｙ，Ｚ軸周りの角速度と各ロータの推力との関係が、各ロータのホバリング時の標準角速度を用いて設定されている請求項１記載のマルチコプタ。
3. 前記第一及び第三の目標決定部は、前記偏差を算出した各項目のうち、
   前記偏差の絶対値が所定値以下の項目は、当該偏差をゼロとみなして、当該項目の速度又は角速度の目標値を決定する請求項１又は２記載のマルチコプタ。
4. 姿勢・位置情報算出部は、ランダムウォークを抑制する機能を有したカルマンフィルタを用いて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する請求項１乃至３のいずれか記載のマルチコプタ。
5. 姿勢・位置情報算出部は、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出した後、さらにフィルタリング処理を行って平滑化した値を算出結果とする請求項１乃至４のいずれか記載のマルチコプタ。
6. 機体と、前記機体の中心から離れた位置に放射状に配置された複数のロータとを備えたマルチコプタの制御方法において、
   前記機体の中心を原点として互いに直交するＸ、Ｙ及びＺ軸が定義され、前記機体のＸ、Ｙ及びＺ軸周りの角速度の検出値と、前記機体のＸ、Ｙ及びＺ軸方向の加速度の検出値とを取得する検出値取得ステップと、
   前記検出値取得ステップで取得した各検出値から、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する姿勢・位置情報算出ステップと、
   前記機体のＸ、Ｙ及びＺ軸方向の位置について、各目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、各偏差がゼロに近づくように、前記機体のＸ、Ｙ及びＺ軸方向の速度の目標値を決定するとともに、
   前記機体のヨー軸周りの角度について、目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のヨー軸周りの角速度の目標値を決定する第一の目標値決定ステップと、
   前記姿勢・位置情報算出ステップで算出したヨー軸周りの角度、及び前記第一の目標値決定ステップで決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、定常状態を表した並進運動方程式に基づいて、前記機体の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する第二の目標値決定ステップと、
   前記機体のロール及びピッチ軸周りの角度について、前記第二の目標値決定ステップで算出した目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のロール及びピッチ軸周りの角速度の目標値を決定する第三の目標値決定ステップと、
   前記第一の目標値決定ステップで決定したヨー軸周りの角速度の目標値、及び前記第三の目標値決定ステップで決定したロール及びピッチ軸周りの角速度の目標値を、姿勢・位置情報算出ステップで算出したロール及びピッチ軸周りの角度を用いて座標変換し、前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値を算出する座標変換ステップと、
   前記第二の目標値決定ステップで決定した前記機体の合計推力の目標値、及び前記座標変換ステップで算出した前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づいて、前記各ロータの推力の目標値を決定する第四の目標値決定ステップとを備え、
   前記第四の目標値決定ステップで決定した前記各ロータの推力の目標値に基づいて、前記各ロータの回転を制御することを特徴とするマルチコプタの制御方法。
7. 前記第四の目標値決定ステップにおいて、前記機体のＸ，Ｙ，Ｚ軸周りの角速度と各ロータの推力との関係が、各ロータのホバリング時の標準角速度を用いて設定されている請求項６記載のマルチコプタの制御方法。
8. 前記第一及び第三の目標決定ステップにおいて、前記偏差を算出した各項目のうち、
   前記偏差の絶対値が所定値以下の項目は、当該偏差をゼロとみなして、当該項目の速度又は角速度の目標値を決定する請求項６又は７記載のマルチコプタの制御方法。
9. 姿勢・位置情報算出ステップにおいて、ランダムウォークを抑制する機能を有したカルマンフィルタを用いて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する請求項６乃至８のいずれか記載のマルチコプタの制御方法。
10. 姿勢・位置情報算出ステップにおいて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出した後、さらにフィルタリング処理を行って平滑化した値を算出結果とする請求項６乃至９のいずれか記載のマルチコプタの制御方法。

Images