(57)【要約】【課題】実プロセスを適切にモデル化した回転運動及び並進運動の方程式に基づいて、機体の位置及び姿勢を安定に制御できるマルチコプタ及びその制御方法を提供する。【解決手段】制御装置１６が有する第二の目標値決定部２４は、姿勢・位置情報算出部２０が算出したヨー軸周りの角度、及び第一の目標値決定部２２が決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、定常状態を表した並進運動方程式に基づいて、機体１２の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する。第四の目標値決定部３０は、第二の目標値決定部２４が決定した合計推力の目標値、及び座標変換部２８が算出したＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づいて、各ロータ１４(1)〜１４(4)の推力の目標値を決定する。制御装置１６は、この推力の目標値に基づいて各ロータ１４(1)〜１４(4)の回転を制御する。【選択図】図２
2019046336.tif 000002
【特許請求の範囲】
【請求項１】
機体と、前記機体の中心から離れた位置に放射状に配置された複数のロータと、前記各ロータの回転を制御する制御装置とを備えたマルチコプタにおいて、
前記制御装置は、
前記機体の中心を原点として互いに直交するＸ、Ｙ及びＺ軸が定義され、前記機体のＸ、Ｙ及びＺ軸周りの角速度の検出値と、前記機体のＸ、Ｙ及びＺ軸方向の加速度の検出値とを取得する検出値取得部と、
前記検出値取得部が取得した各検出値から、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する姿勢・位置情報算出部と、
前記機体のＸ、Ｙ及びＺ軸方向の位置について、各目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、各偏差がゼロに近づくように、前記機体のＸ、Ｙ及びＺ軸方向の速度の目標値を決定するとともに、
前記機体のヨー軸周りの角度について、目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のヨー軸周りの角速度の目標値を決定する第一の目標値決定部と、
前記姿勢・位置情報算出部が算出したヨー軸周りの角度、及び前記第一の目標値決定部が決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、定常状態を表した並進運動方程式に基づいて、前記機体の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する第二の目標値決定部と、
前記機体のロール及びピッチ軸周りの角度について、前記第二の目標値決定部が算出した目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のロール及びピッチ軸周りの角速度の目標値を決定する第三の目標値決定部と、
前記第一の目標値決定部が決定したヨー軸周りの角速度の目標値、及び前記第三の目標値決定部が決定したロール及びピッチ軸周りの角速度の目標値を、姿勢・位置情報算出部が算出したロール及びピッチ軸周りの角度を用いて座標変換し、前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値を算出する座標変換部と、
前記第二の目標値決定部が決定した前記機体の合計推力の目標値、及び前記座標変換部が算出した前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づいて、前記各ロータの推力の目標値を決定する第四の目標値決定部とを備え、
前記第四の目標値決定部が決定した前記各ロータの推力の目標値に基づいて、前記各ロータの回転を制御することを特徴とするマルチコプタ。
【請求項２】
前記第四の目標値決定部において、前記機体のＸ，Ｙ，Ｚ軸周りの角速度と各ロータの推力との関係が、各ロータのホバリング時の標準角速度を用いて設定されている請求項１記載のマルチコプタ。
【請求項３】
前記第一及び第三の目標決定部は、前記偏差を算出した各項目のうち、
前記偏差の絶対値が所定値以下の項目は、当該偏差をゼロとみなして、当該項目の速度又は角速度の目標値を決定する請求項１又は２記載のマルチコプタ。
【請求項４】
姿勢・位置情報算出部は、ランダムウォークを抑制する機能を有したカルマンフィルタを用いて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する請求項１乃至３のいずれか記載のマルチコプタ。
【請求項５】
姿勢・位置情報算出部は、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出した後、さらにフィルタリング処理を行って平滑化した値を算出結果とする請求項１乃至４のいずれか記載のマルチコプタ。
【請求項６】
機体と、前記機体の中心から離れた位置に放射状に配置された複数のロータとを備えたマルチコプタの制御方法において、
前記機体の中心を原点として互いに直交するＸ、Ｙ及びＺ軸が定義され、前記機体のＸ、Ｙ及びＺ軸周りの角速度の検出値と、前記機体のＸ、Ｙ及びＺ軸方向の加速度の検出値とを取得する検出値取得ステップと、
前記検出値取得ステップで取得した各検出値から、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する姿勢・位置情報算出ステップと、
前記機体のＸ、Ｙ及びＺ軸方向の位置について、各目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、各偏差がゼロに近づくように、前記機体のＸ、Ｙ及びＺ軸方向の速度の目標値を決定するとともに、
前記機体のヨー軸周りの角度について、目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のヨー軸周りの角速度の目標値を決定する第一の目標値決定ステップと、
前記姿勢・位置情報算出ステップで算出したヨー軸周りの角度、及び前記第一の目標値決定ステップで決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、定常状態を表した並進運動方程式に基づいて、前記機体の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する第二の目標値決定ステップと、
前記機体のロール及びピッチ軸周りの角度について、前記第二の目標値決定ステップで算出した目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のロール及びピッチ軸周りの角速度の目標値を決定する第三の目標値決定ステップと、
前記第一の目標値決定ステップで決定したヨー軸周りの角速度の目標値、及び前記第三の目標値決定ステップで決定したロール及びピッチ軸周りの角速度の目標値を、姿勢・位置情報算出ステップで算出したロール及びピッチ軸周りの角度を用いて座標変換し、前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値を算出する座標変換ステップと、
前記第二の目標値決定ステップで決定した前記機体の合計推力の目標値、及び前記座標変換ステップで算出した前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づいて、前記各ロータの推力の目標値を決定する第四の目標値決定ステップとを備え、
前記第四の目標値決定ステップで決定した前記各ロータの推力の目標値に基づいて、前記各ロータの回転を制御することを特徴とするマルチコプタの制御方法。
【請求項７】
前記第四の目標値決定ステップにおいて、前記機体のＸ，Ｙ，Ｚ軸周りの角速度と各ロータの推力との関係が、各ロータのホバリング時の標準角速度を用いて設定されている請求項６記載のマルチコプタの制御方法。
【請求項８】
前記第一及び第三の目標決定ステップにおいて、前記偏差を算出した各項目のうち、
前記偏差の絶対値が所定値以下の項目は、当該偏差をゼロとみなして、当該項目の速度又は角速度の目標値を決定する請求項６又は７記載のマルチコプタの制御方法。
【請求項９】
姿勢・位置情報算出ステップにおいて、ランダムウォークを抑制する機能を有したカルマンフィルタを用いて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する請求項６乃至８のいずれか記載のマルチコプタの制御方法。
【請求項１０】
姿勢・位置情報算出ステップにおいて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出した後、さらにフィルタリング処理を行って平滑化した値を算出結果とする請求項６乃至９のいずれか記載のマルチコプタの制御方法。
【発明の詳細な説明】
【技術分野】
【０００１】
本発明は、複数のロータを有したマルチコプタ及びその制御方法に関する。
【背景技術】
【０００２】
マルチコプタの位置及び姿勢を安定化する制御方法は、実プロセスのモデル化のしかたによって複数のタイプに分類することができる。例えば、実プロセスをモデル化せずにブラックボックスとするＰＩＤ制御、運動方程式等を基に実プロセスをモデル化して状態方程式に表し、これを用いて制御を行う状態フィードバック制御、状態フィードバック制御に加え、モデル化誤差や外乱・ノイズの影響を抑制すべく、これらの周波数特性を考慮して制御を行うＨ∞制御（Ｈ-infinity control）等である。
【０００３】
従来から多く使用されているのは、ＰＩＤ制御である。例えば姿勢制御の場合、図８に示すように、ＩＭＵ（inertial measurement unit）からＸ，Ｙ，Ｚ軸周りの角速度の検出値ωx，ωy，ωzを取得し、この検出値からロール、ピッチ及びヨー軸周りの角度φ，θ，ψを算出し、これらと目標値（又は指令値）との偏差がゼロになるようにＰＩＤ（比例・微分・積分）の制御量を算出し、算出した制御量を経験則で各ロータの回転数に配分する。そして、各ロータの回転数の目標値をＥＳＣ（Electronic Speed Controller）に送信し、ＥＳＣの指令に基づいて各ロータが駆動される。マルチコプタをＰＩＤ制御する技術は、例えば特許文献１に開示されている。
【先行技術文献】
【特許文献】
【０００４】
【特許文献１】特開２０１７−５２３８９号公報
【発明の概要】
【発明が解決しようとする課題】
【０００５】
実プロセスでは、各ロータの回転数とロール、ピッチ及びヨー軸周りの角度とが絡み合って制御が干渉したり、機体の振動や風等の外乱が影響したりするので、図８に示すようなＰＩＤ制御では、姿勢制御が不安定になる。特許文献１に開示された技術についても同様である。
【０００６】
そこで、運動方程式を基に実プロセスのモデル化を行い、状態フィードバック制御やＨ∞制御を行うことが試行されてきた。しかし、ほとんどがシミュレーションによる検討に留まっており、実用化に至った例は乏しい。その原因として、次のようなことが考えられる。
【０００７】
多くの場合、マルチコプタの回転運動は式（１）に示すオーラ―の方程式で表されている。
【数１】
2019046336.tif 000003
ここで、Ｉ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を決定する処理を行う。具体的には、以下に説明するように、姿勢・位置情報算出部２０が算出したヨー軸周りの角度ψを既知の値とし、定常状態を表した並進運動方程式に基づく演算処理を行う（第二の目標値決定ステップ）。
【００３８】
一般に、航空機の並進運動方程式は、式（２）のように表すことができる。
【数２】
2019046336.tif 000004
ここで、ｇは重力加速度、ｍは機体の質量であり、Ａx，Ａy，Ａzは、それぞれＸ，Ｙ及びＺ軸方向の空気抵抗係数である。また、Ｓφ，Ｓθ，Ｓψは、それぞれsinθ，sinφ，sinψであり、Ｃφ，Ｃθ，Ｃψは、それぞれcosθ，cosφ，cosψである。
【００３９】
次に、定常状態を考えるため、式（２）の加速度ｘ（ツードット），ｙ（ツードット），ｚ（ツードット）をゼロとすると、次の式（３）の連立方程式が得られる。
【数３】
2019046336.tif 000005
【００４０】
そして、式（３）の３つの式を展開することにより、Ｆを求める式（４）、φを求める式（５）、θを求める式（６）をそれぞれ導出することができる。
【数４】
2019046336.tif 000006
【数５】
2019046336.tif 000007
【数６】
2019046336.tif 000008
【００４１】
このように、定常状態を想定することによって、Ｆ，φ，θは非常にシンプルな式で求めることができる。
【００４２】
第二の目標値決定部２４は、姿勢・位置情報算出部２０が算出したヨー軸周りの角度ψ、及び第一の目標値決定部が決定したＸ、Ｙ及びＺ軸方向の速度の目標値ｘr（ドット），ｙr（ドット），ｚr（ドット）を受け、これらを式（４）、（５）、（６）の右辺に代入して左辺のＦ，φ，θを算出し、各算出結果を、機体の合計推力の目標値Ｆr、ロール及びピッチ軸周りの角度の目標値φr，θrに決定する。
【００４３】
第三の目標値決定部２６は、機体１２のロール及びピッチ軸周りの角度について、第二の目標値決定部２４が算出した目標値φr，θrと姿勢・位置情報算出部２０が算出した値との偏差ｄφ，ｄθを算出し、この偏差ｄφ，ｄθがゼロに近づくように、機体１２のロール及びピッチ軸周りの角速度の目標値φr（ドット），θr（ドット）を決定する処理を行う（第三の目標値決定ステップ）。
【００４４】
偏差と目標値の関係は、図６の中のグラフに示すように、偏差の絶対値が大きいほど目標値の絶対値が大きくなる関係であることが好ましく、例えば関数式を用いて規定することができる。この場合、関数式は、１次関数や２次関数でもよいし、その他の関数でもよい。
【００４５】
このとき、偏差を算出した項目のうち、偏差の絶対値が所定の値（ａ５，ａ６）以下の項目は、当該偏差をゼロとみなして目標値を決定することが好ましい。このような不感帯を設けることにより、微小な偏差応答して制御がハンチングするのを防止することができる。
【００４６】
また、偏差を算出した項目のうち、偏差の絶対値が所定の値（ｂ５，ｂ６）を超えた項目は、目標値の絶対値を一定の値に制限し、制御が暴走するのを防止するとよい。なお、目標値の絶対値を一定の値に制限することは、第三の目標値決定部２６に実行させてもよいが、他のブロックの動作を利用して実行することも可能である。例えば、検出部取得部１８が有する増幅回路の出力電圧のサチュレーションを利用し、各検出値の絶対値が一定以上の値にならないように制限すれば、同様の作用効果が得られる。
【００４７】
図２に示す座標変換部２８は、第一の目標値決定部２２が決定したヨー軸周りの角速度の目標値ψr（ドット）、第三の目標値決定部２６が決定したロール及びピッチ軸周りの角速度の目標値φr（ドット），θr（ドット）を座標変換し、機体１２のＸ、Ｙ及びＺ軸周りの角速度の目標値ωxr，ωyr，ωzrを算出する処理を行う（座標変換ステップ）。
【００４８】
一般に、座標変換は、式（７）を用いて行うことができる。具体的には、姿勢・位置情報算出部２０が算出したロール及びピッチ軸周りの角度φ，θを既知の値として右辺のＳφ，Ｓθ，Ｃφ，Ｃθに代入するとともに、各目標値ψr（ドット），φr（ドット），θr（ドット）を右辺に代入することによって、左辺の各目標値ωxr，ωyr，ωzrを算出する。
【数７】
2019046336.tif 000009
【００４９】
第四の目標決定部３０は、第二の目標値決定部２４が決定した機体１２の合計推力の目標値Ｆr、及び座標変換部２８が算出した機体１２のＸ、Ｙ及びＺ軸周りの角速度の目標値ωxr，ωyr，ωzrから、各ロータ１４(1)〜１４(4)の各推力の目標値Ｆ1r，Ｆ2r，Ｆ3r，Ｆ4rを決定する処理を行う。具体的には、以下に説明するように、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づく演算処理を行う（第四の目標値決定ステップ）。
【００５０】
一般に、ロータの推力はロータの角速度の２乗に比例するので、ロータ推力係数（比例係数）をｋ、各ロータ１４(1)〜１４(4)の角速度をΩ1，Ω2，Ω3，Ω4とすると、次の式（８）が成立する。
【数８】
2019046336.tif 000010
【００５１】
また、図７に示すように、13軸周りの軸トルクをτ13，24軸周りの軸トルクをτ24、機体１２の中心Ｔから各ロータ１４(1)〜１４(4)までの距離（トルクアーム）をｄとすると、次の式（９）が成立する。
【数９】
2019046336.tif 000011
【００５２】
次に、ジャイロ効果について考える。第一及び第三ロータ１４(1)，１４(3)は、トルク軸（13軸）と直交するスピン軸周りに慣性モーメントＩを有し、角速度Ω1，Ω3で回転しているので、角運動量ＩΩ1，ＩΩ3を持っている。そこに軸トルクτ13が加わると、ジャイロ効果によって、スピン軸及びトルク軸（13軸）と直交するプリセッション軸周りに、角速度ω1p，ω3pが生じる。同様に、第二及び第四ロータ１４(2)，１４(4)は、トルク軸（24軸）と直交するスピン軸周りに慣性モーメントＩを有し、角速度Ω2，Ω4で回転しているので、角運動量ＩΩ2，ＩΩ4を持っている。そこに軸トルクτ24が加わると、ジャイロ効果によって、スピン軸及びトルク軸（24軸）と直交するプリッション軸周りに、角速度ω2p，ω4pが生じる。各プリセッション軸周りの角速度ω1p，ω2p，ω3p，ω4pは、次の式（１０）のように表すことができる。
【数１０】
2019046336.tif 000012
【００５３】
次に、座標変換部２８が算出したＸ及びＹ軸周りの角速度ωx，ωyと、各プリセッション軸周りの角速度ω1p，ω2p，ω3p，ω4pとの関係を考える。ωx，ωyとω1p，ω2p，ω3p，ω4pとの関係は、式（１１）のように表すことができる。
【数１１】
2019046336.tif 000013
【００５４】
そして、式（１１）に式（１０）を代入してω1p，ω2p，ω3p，ω4pを消去し、さらに式（９）を代入してτ13，τ24を消去すると、Ｘ、Ｙ及びＺ軸周りの角速度ωx，ωyと各ロータの推力Ｆ1，Ｆ2，Ｆ3，Ｆ4との関係を示す式（１２）が得られる。
【数１２】
2019046336.tif 000014
【００５５】
さらに、計算を簡単化するため、式（１２）の各ロータの角速度Ω1，Ω2，Ω3，Ω4をすべて既知の値であるホバリング時の標準角速度Ωhに近似すると、次の式（１３）が得られる。
【数１３】
2019046336.tif 000015
【００５６】
このように、Ω1，Ω2，Ω3，Ω4をすべて標準角速度Ωhに近似することによって、ωx，ωyとＦ1，Ｆ2，Ｆ3，Ｆ4との関係を、非常にシンプルな式で表すことができる。
【００５７】
次に、座標変換部２８が算出したＺ軸周りの角速度ωzについて考える。Ｚ軸周りの回転は、各ロータの回転に対する反トルク（時計回り／反時計回り）が関係し、それらの差分によって生じる。一般に、各ロータの反トルクはロータの角速度の２乗に比例し、ロータの推力に比例する。したがって、各ロータ１４(1)〜１４(4)の反トルクをＴ1，Ｔ2，Ｔ3，Ｔ4、トルク変換係数（推力と反トルクとの比例係数）をμとすると、式（１４）が成立する。なお、式（１４）では、反時計回りの反トルクを正、時計回りの反トルクを負としている。
【数１４】
2019046336.tif 000016
【００５８】
ここで、反トルクＴ1，Ｔ2，Ｔ3，Ｔ4が機体に作用し、機体１２がＺ軸周りに角速度ωzで反時計回りに回転している状態を考える。各ロータ１４(1)〜１４(4)の角運動量Ｌ1，Ｌ2，Ｌ3，Ｌ4は、Ｌ1＝ＩΩ1，Ｌ2＝ＩΩ2，Ｌ3＝ＩΩ3，Ｌ4＝ＩΩ4と表すことができるので、オイラーの回転運動方程式により、次の式（１５）が成立する。
【数１５】
2019046336.tif 000017
【００５９】
次に、定常状態を考えるため、式（１５）の左辺の第１項をゼロとし、さらに式（１４）を代入してＴ1，Ｔ2，Ｔ3，Ｔ4を消去すると、次の式（１６）が得られる。
【数１６】
2019046336.tif 000018
【００６０】
さらに、計算を簡単化するため、式（１６）の各ロータの角速度Ω1，Ω2，Ω3，Ω4をすべて既知の値であるホバリング時の標準角速度Ωhに近似すると、次の式（１７）が得られる。
【数１７】
2019046336.tif 000019
【００６１】
このように、Ω1，Ω2，Ω3，Ω4をすべて標準角速度Ωhに近似することによって、ωzとＦ1，Ｆ2，Ｆ3，Ｆ4との関係を、非常にシンプルな式で表すことができる。
【００６２】
そして、式（１３）、（１７）の関係、及びＦ＝Ｆ1＋Ｆ2＋Ｆ3＋Ｆ4の関係をまとめると式（１８）のようになり、これを変形することによって式（１９）が得られる。
【数１８】
2019046336.tif 000020
【数１９】
2019046336.tif 000021
【００６３】
第四の目標値決定部３０は、第二の目標値決定部２４が決定した合計推力の目標値Ｆr、及び座標変換部２８が算出したＸ、Ｙ及びＺ軸周りの角速度の目標値ωxr，ωyr，ωzrを受け、これらを式（１９）の右辺に代入して左辺のＦ1，Ｆ2，Ｆ3，Ｆ4を算出し、各算出結果を各ロータ１４(1)〜１４(4)の推力の目標値Ｆ1r，Ｆ2r，Ｆ3r，Ｆ4rに決定する。
【００６４】
制御装置１６は、各ロータ１４(1)〜１４(4)の推力が、目標値Ｆ1r，Ｆ2r，Ｆ3r，Ｆ4rになるように、各ロータ１４(1)〜１４(4)の回転を制御する。
【００６５】
例えば、ロータの推力を角速度で制御する場合、次の式（２０）を利用することができる。式（２０）は、上記の式（８）を変形したものである。
【数２０】
2019046336.tif 000022
【００６６】
具体的には、図２に示すように、第五の目標値決定部３２を設けるとよい。第五の目標値決定部３２は、第四の目標値決定部３０が決定した各ロータ１４(1)〜１４(4)の推力の目標値Ｆ1r，Ｆ2r，Ｆ3r，Ｆ4rを受け、これらを式（２０）の右辺に代入して左辺のΩ1，Ω2，Ω3，Ω4を算出し、各算出結果を各ロータ１４(1)〜１４(4)の角速度の目標値Ω1r，Ω2r，Ω3r，Ω4rに決定する。そして、制御装置１６は、各ロータ１４(1)〜１４(4)の角速度がΩ1r，Ω2r，Ω3r，Ω4rになるように制御する。
【００６７】
以上説明したように、マルチコプタ１０及び制御装置１６が実行する制御方法は、定常状態を表した並進運動方程式、及びジャイロ効果を加えて定常状態を表した回転運動方程式を基に、実プロセスを現実的かつ実用的にモデル化して制御の目標値を算出しているので、機体１２の位置及び姿勢を非常に安定に制御することができる。
【００６８】
なお、本発明のマルチコプタ及びその制御方法は、上記実施形態に限定されるものではない。例えば、上述したマルチコプタ１０の制御方法に関する式（２）〜式（２０）は一例であり、各式の一部を変更し、より厳密な計算を行うようにしてもよい。
【００６９】
その他、上記のマルチコプタ１０はクワッドエックス型であるが、同様の考え方でクワッドプラス型を想定した並進運動方程式及び回転運動方程式を設定すれば、クワッドプラス型においても同様の効果を得ることができる。トライコプタ、ヘキサコプタ、オクトコプタについても同様である。
【符号の説明】
【００７０】
１０  マルチコプタ
１２  機体
１４(1)〜１４(4)  第一〜第四ロータ
１６  制御装置
１８  検出値取得部
２０  位置・姿勢情報算出部
２２  第一の目標値決定部
２４  第二の目標値決定部
２６  第三の目標値決定部
２８  座標変換部
３０  第四の目標値決定部
３２  第五の目標値決定部
【図１】
2019046336.tif 000023
【図２】
2019046336.tif 000024
【図３】
2019046336.tif 000025
【図４】
2019046336.tif 000026
【図５】
2019046336.tif 000027
【図６】
2019046336.tif 000028
【図７】
2019046336.tif 000029
【図８】
2019046336.tif 000030