JP6970424B2

JP6970424B2 - Multicopter and its control method

Info

Publication number: JP6970424B2
Application number: JP2017170821A
Authority: JP
Inventors: 武雄島次; 範雄紺屋
Original assignee: Fadrone
Current assignee: Fadrone
Priority date: 2017-09-06
Filing date: 2017-09-06
Publication date: 2021-11-24
Anticipated expiration: 2037-09-06
Also published as: JP2019046336A

Description

本発明は、複数のロータを有したマルチコプタ及びその制御方法に関する。 The present invention relates to a multicopter having a plurality of rotors and a control method thereof.

マルチコプタの位置及び姿勢を安定化する制御方法は、実プロセスのモデル化のしかたによって複数のタイプに分類することができる。例えば、実プロセスをモデル化せずにブラックボックスとするＰＩＤ制御、運動方程式等を基に実プロセスをモデル化して状態方程式に表し、これを用いて制御を行う状態フィードバック制御、状態フィードバック制御に加え、モデル化誤差や外乱・ノイズの影響を抑制すべく、これらの周波数特性を考慮して制御を行うＨ∞制御（Ｈ-infinity control）等である。 Control methods that stabilize the position and orientation of the multicopter can be classified into multiple types depending on how the actual process is modeled. For example, in addition to state feedback control and state feedback control, in which the actual process is modeled and expressed in a state equation based on PID control, which is a black box without modeling the actual process, motion equation, etc., and control is performed using this. , H-infinity control, etc., which controls in consideration of these frequency characteristics in order to suppress the influence of modeling error and disturbance / noise.

従来から多く使用されているのは、ＰＩＤ制御である。例えば姿勢制御の場合、図８に示すように、ＩＭＵ（inertial measurement unit）からＸ，Ｙ，Ｚ軸周りの角速度の検出値ω_x，ω_y，ω_zを取得し、この検出値からロール、ピッチ及びヨー軸周りの角度φ，θ，ψを算出し、これらと目標値（又は指令値）との偏差がゼロになるようにＰＩＤ（比例・微分・積分）の制御量を算出し、算出した制御量を経験則で各ロータの回転数に配分する。そして、各ロータの回転数の目標値をＥＳＣ（Electronic Speed Controller）に送信し、ＥＳＣの指令に基づいて各ロータが駆動される。マルチコプタをＰＩＤ制御する技術は、例えば特許文献１に開示されている。 PID control has been widely used in the past. For example, in the case of attitude control, as shown in FIG. 8, the detected values ω _x , ω _y , ω _z of the angular velocities around the X, Y, and Z axes are acquired from the IMU (inertial measurement unit), and rolls are obtained from these detected values. Calculate the angles φ, θ, and ψ around the pitch and yaw axis, and calculate and calculate the control amount of PID (proportional / differential / integral) so that the deviation between these and the target value (or command value) becomes zero. The controlled amount is distributed to the rotation speed of each rotor according to the empirical rule. Then, the target value of the rotation speed of each rotor is transmitted to the ESC (Electronic Speed Controller), and each rotor is driven based on the command of the ESC. A technique for controlling PID of a multicopter is disclosed in, for example, Patent Document 1.

特開２０１７−５２３８９号公報Japanese Unexamined Patent Publication No. 2017-52389

実プロセスでは、各ロータの回転数とロール、ピッチ及びヨー軸周りの角度とが絡み合って制御が干渉したり、機体の振動や風等の外乱が影響したりするので、図８に示すようなＰＩＤ制御では、姿勢制御が不安定になる。特許文献１に開示された技術についても同様である。 In the actual process, the rotation speed of each rotor is entangled with the roll, pitch, and angle around the yaw axis, which interferes with control, and is affected by vibrations of the aircraft and disturbances such as wind. In PID control, attitude control becomes unstable. The same applies to the technique disclosed in Patent Document 1.

そこで、運動方程式を基に実プロセスのモデル化を行い、状態フィードバック制御やＨ∞制御を行うことが試行されてきた。しかし、ほとんどがシミュレーションによる検討に留まっており、実用化に至った例は乏しい。その原因として、次のようなことが考えられる。 Therefore, it has been tried to model the actual process based on the equation of motion and perform state feedback control and H∞ control. However, most of the studies are limited to simulations, and there are few examples that have been put into practical use. The possible causes are as follows.

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

ここで、Ｉ_x，Ｉ_y，Ｉ_zは、それぞれＸ，Ｙ，Ｚ軸方向の主慣性モーメント、ω_x，ω_y，ω_zは、それぞれＸ，Ｙ，Ｚ軸周りの角速度、Ｎ_x，Ｎ_y，Ｎ_zは、それぞれＸ，Ｙ，Ｚ軸周りのトルクである。 In many cases, the rotational motion of the multicopter is expressed by the equation of the aura shown in the equation (1).

_{_{Here, I x, I y, I}} z , respectively X, Y, principal moments of inertia of the Z-axis _{_{direction, ω x, ω y, ω}} z is X respectively, Y, angular velocity around the Z-axis, N _x, N _y and N _z are torques around the X, Y, and Z axes, respectively.

オイラーの方程式は、連続回転している剛体には当てはまるが、マルチコプタのように、各ロータの推力や反トルクのアンバランスによって断続的に回転する剛体には当てはまらないと考えられる。例えば、ω_x＝ω_y＝ω_z＝０の状態からＸ軸だけにトルクＮ_xが働くと、式（１）中の第１式は、Ｉ_x・dω_x/dt＝Ｎ_xとなる。これは、トルクＮ_xがdt時間加わることによって、角運動量Ｉ_x・dω_x増加することを意味している。そうすると、角運動量保存の法則により、トルクＮ_xが無くなっても回転し続けることになるので、現実と矛盾する。 Euler's equations apply to rigid bodies that rotate continuously, but not to rigid bodies that rotate intermittently due to the imbalance of thrust and anti-torque of each rotor, such as multicopters. For example, _{when the torque N x} acts only on the X axis from the state of _{ω x} = ω _y = ω _z = 0, the first equation in the equation (1) becomes I _x · dω _x / dt = N _x . This means that the angular momentum I _x · dω _x increases when the torque N _{x is added for dt time.} Then, according to the law of conservation of angular momentum, the _{rotation continues even if the torque N x} disappears, which contradicts the reality.

その他、角加速度dω/dtを求めるには、ノイズが重畳している角速度の検出値ωを微分する必要があり、その処理によってノイズ成分がさらに増大するので、現実的には角加速度dω/dtを安定的に求めることができないという問題もある。 In addition, in order to obtain the angular acceleration dω / dt, it is necessary to differentiate the detected value ω of the angular velocity on which the noise is superimposed, and the noise component further increases due to the processing, so in reality, the angular acceleration dω / dt There is also a problem that it cannot be obtained stably.

このように、従来から使用され又は検討されているＰＩＤ制御、状態フィードバック制御、Ｈ∞制御は、それぞれに問題点があり、マルチコプタの位置及び姿勢を安定に制御することが困難であった。 As described above, the PID control, the state feedback control, and the H∞ control, which have been conventionally used or studied, have problems in each, and it is difficult to stably control the position and posture of the multicopter.

本発明は、上記背景技術に鑑みて成されたものであり、実プロセスを適切にモデル化した回転運動及び並進運動の方程式に基づいて、機体の位置及び姿勢を安定に制御できるマルチコプタ及びその制御方法を提供することを目的とする。 The present invention has been made in view of the above background technology, and is a multicopter capable of stably controlling the position and attitude of the aircraft and its control based on the equations of rotational motion and translational motion that appropriately model the actual process. The purpose is to provide a method.

本発明は、機体と、前記機体の中心から離れた位置に放射状に配置された複数のロータと、前記各ロータの回転を制御する制御装置とを備えたマルチコプタであって、
前記制御装置は、前記機体の中心を原点として互いに直交するＸ、Ｙ及びＺ軸が定義され、前記機体のＸ、Ｙ及びＺ軸周りの角速度の検出値と、前記機体のＸ、Ｙ及びＺ軸方向の加速度の検出値とを取得する検出値取得部と、
前記検出値取得部が取得した各検出値から、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する姿勢・位置情報算出部と、
前記機体のＸ、Ｙ及びＺ軸方向の位置について、各目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、各偏差がゼロに近づくように、前記機体のＸ、Ｙ及びＺ軸方向の速度の目標値を決定するとともに、前記機体のヨー軸周りの角度について、目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のヨー軸周りの角速度の目標値を決定する第一の目標値決定部と、
前記姿勢・位置情報算出部が算出したヨー軸周りの角度、及び前記第一の目標値決定部が決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、前記機体のＸ、Ｙ及びＺ軸方向の加速度をゼロとして定常状態を表した並進運動方程式に基づいて、前記機体の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する第二の目標値決定部と、
前記機体のロール及びピッチ軸周りの角度について、前記第二の目標値決定部が算出した目標値と前記姿勢・位置情報算出部が算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のロール及びピッチ軸周りの角速度の目標値を決定する第三の目標値決定部と、
前記第一の目標値決定部が決定したヨー軸周りの角速度の目標値、及び前記第三の目標値決定部が決定したロール及びピッチ軸周りの角速度の目標値を、姿勢・位置情報算出部が算出したロール及びピッチ軸周りの角度を用いて座標変換し、前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値を算出する座標変換部と、
前記第二の目標値決定部が決定した前記機体の合計推力の目標値、及び前記座標変換部が算出した前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えた回転運動方程式であって、各ロータの角速度をすべて既知の値であるホバリング時の標準角速度に近似して定常状態を表した回転運動方程式に基づいて、前記各ロータの推力の目標値を決定する第四の目標値決定部とを備え、
前記第四の目標値決定部が決定した前記各ロータの推力の目標値に基づいて、前記各ロータの回転を制御するマルチコプタである。 The present invention is a multicopter including an airframe, a plurality of rotors radially arranged at positions away from the center of the airframe, and a control device for controlling the rotation of each rotor.
In the control device, X, Y and Z axes orthogonal to each other with the center of the aircraft as the origin are defined, the detection values of the angular velocities around the X, Y and Z axes of the aircraft and the X, Y and Z of the aircraft. A detection value acquisition unit that acquires the detection value of axial acceleration,
A posture / position information calculation unit that calculates the positions of the aircraft in the X, Y, and Z axis directions and the angles around the roll, pitch, and yaw axes of the aircraft from each detection value acquired by the detection value acquisition unit. ,
With respect to the positions of the aircraft in the X, Y and Z-axis directions, the deviation between each target value and the value calculated by the attitude / position information calculation unit is calculated, and the X of the aircraft is set so that each deviation approaches zero. The target value of the velocity in the Y and Z axis directions is determined, and the deviation between the target value and the value calculated by the attitude / position information calculation unit is calculated for the angle around the yaw axis of the aircraft, and this deviation is zero. The first target value determination unit that determines the target value of the angular velocity around the yaw axis of the aircraft so as to approach
From the angle around the yaw axis calculated by the posture / position information calculation unit and the target values of the speeds in the X, Y and Z axis directions determined by the first target value determination unit, the X, Y and Z of the aircraft. A second target value determination unit that determines the target value of the total thrust of the aircraft, the target value of the roll and the angle around the pitch axis, based on the translational equation of motion that expresses the steady state with the acceleration in the axial direction as zero.
With respect to the roll of the machine and the angle around the pitch axis, the deviation between the target value calculated by the second target value determination unit and the value calculated by the attitude / position information calculation unit is calculated, and this deviation approaches zero. As described above, the third target value determining unit for determining the target value of the angular velocity around the roll and pitch axis of the aircraft, and the like.
The posture / position information calculation unit determines the target value of the angular velocity around the yaw axis determined by the first target value determination unit and the target value of the angular velocity around the roll and pitch axes determined by the third target value determination unit. Coordinate conversion unit that calculates the target value of the angular velocity around the X, Y and Z axes of the aircraft by performing coordinate conversion using the roll and the angle around the pitch axis calculated by
The gyro effect was added from the target value of the total thrust of the aircraft determined by the second target value determination unit and the target value of the angular velocity around the X, Y and Z axes of the aircraft calculated by the coordinate conversion unit. The target value of the thrust of each rotor is determined based on the rotational equation of motion that expresses the steady state by approximating the angular velocity of each rotor to the standard angular velocity at the time of hovering, which is a known value. Equipped with a fourth target value determination unit
It is a multicopter that controls the rotation of each rotor based on the target value of the thrust of each rotor determined by the fourth target value determination unit.

前記第一及び第三の目標決定部は、前記偏差を算出した各項目のうち、前記偏差の絶対値が所定値以下の項目は、当該偏差をゼロとみなして、当該項目の速度又は角速度の目標値を決定する構成にすることが好ましい。 Among the items for which the deviation is calculated, the first and third target determination units consider that the deviation is zero for the item whose absolute value of the deviation is equal to or less than the predetermined value, and the velocity or the angular velocity of the item. It is preferable to have a configuration that determines the target value.

姿勢・位置情報算出部は、ランダムウォークを抑制する機能を有したカルマンフィルタを用いて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する構成にすることが好ましい。また、姿勢・位置情報算出部は、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出した後、さらにフィルタリング処理を行って平滑化した値を算出結果とする構成にすることが好ましい。 The attitude / position information calculation unit uses a Kalman filter having a function of suppressing a random walk to determine the positions of the aircraft in the X, Y and Z axis directions and the angles around the roll, pitch and yaw axes of the aircraft. It is preferable to have a configuration for calculation. In addition, the attitude / position information calculation unit calculates the positions of the aircraft in the X, Y, and Z axis directions and the angles around the roll, pitch, and yaw axes of the aircraft, and then further performs filtering processing to smooth the aircraft. It is preferable to use the calculated value as the calculation result.

また本発明は、機体と、前記機体の中心から離れた位置に放射状に配置された複数のロータとを備えたマルチコプタの制御方法であって、
前記機体の中心を原点として互いに直交するＸ、Ｙ及びＺ軸が定義され、前記機体のＸ、Ｙ及びＺ軸周りの角速度の検出値と、前記機体のＸ、Ｙ及びＺ軸方向の加速度の検出値とを取得する検出値取得ステップと、
前記検出値取得ステップで取得した各検出値から、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出する姿勢・位置情報算出ステップと、
前記機体のＸ、Ｙ及びＺ軸方向の位置について、各目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、各偏差がゼロに近づくように、前記機体のＸ、Ｙ及びＺ軸方向の速度の目標値を決定するとともに、前記機体のヨー軸周りの角度について、目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のヨー軸周りの角速度の目標値を決定する第一の目標値決定ステップと、
前記姿勢・位置情報算出ステップで算出したヨー軸周りの角度、及び前記第一の目標値決定ステップで決定したＸ、Ｙ及びＺ軸方向の速度の目標値から、前記機体のＸ、Ｙ及びＺ軸方向の加速度をゼロとして定常状態を表した並進運動方程式に基づいて、前記機体の合計推力の目標値、ロール及びピッチ軸周りの角度の目標値を決定する第二の目標値決定ステップと、
前記機体のロール及びピッチ軸周りの角度について、前記第二の目標値決定ステップで算出した目標値と前記姿勢・位置情報算出ステップで算出した値との偏差を算出し、この偏差がゼロに近づくように、前記機体のロール及びピッチ軸周りの角速度の目標値を決定する第三の目標値決定ステップと、
前記第一の目標値決定ステップで決定したヨー軸周りの角速度の目標値、及び前記第三の目標値決定ステップで決定したロール及びピッチ軸周りの角速度の目標値を、姿勢・位置情報算出ステップで算出したロール及びピッチ軸周りの角度を用いて座標変換し、前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値を算出する座標変換ステップと、
前記第二の目標値決定ステップで決定した前記機体の合計推力の目標値、及び前記座標変換ステップで算出した前記機体のＸ、Ｙ及びＺ軸周りの角速度の目標値から、ジャイロ効果を加えた回転運動方程式であって、各ロータの角速度をすべて既知の値であるホバリング時の標準角速度に近似して定常状態を表した回転運動方程式に基づいて、前記各ロータの推力の目標値を決定する第四の目標値決定ステップとを備え、
前記第四の目標値決定ステップで決定した前記各ロータの推力の目標値に基づいて、前記各ロータの回転を制御するマルチコプタの制御方法である。 Further, the present invention is a method for controlling a multicopter including an airframe and a plurality of rotors radially arranged at positions away from the center of the airframe.
The X, Y, and Z axes that are orthogonal to each other with the center of the aircraft as the origin are defined, and the detected values of the angular velocities around the X, Y, and Z axes of the aircraft and the acceleration in the X, Y, and Z axis directions of the aircraft are defined. The detection value acquisition step to acquire the detection value and
A posture / position information calculation step for calculating the positions of the aircraft in the X, Y, and Z axis directions and the angles around the roll, pitch, and yaw axes of the aircraft from the detected values acquired in the detection value acquisition step. ,
For the positions in the X, Y and Z-axis directions of the aircraft, the deviation between each target value and the value calculated in the posture / position information calculation step is calculated, and the X of the aircraft is set so that each deviation approaches zero. The target value of the velocity in the Y and Z axis directions is determined, and the deviation between the target value and the value calculated in the posture / position information calculation step is calculated for the angle around the yaw axis of the aircraft, and this deviation is zero. The first target value determination step for determining the target value of the angular velocity around the yaw axis of the aircraft so as to approach
From the angle around the yaw axis calculated in the posture / position information calculation step and the target values of the speeds in the X, Y and Z axis directions determined in the first target value determination step, the X, Y and Z of the aircraft. A second target value determination step for determining the target value of the total thrust of the aircraft, the target value of the roll and the angle around the pitch axis, based on the translational equation of motion expressing the steady state with the axial acceleration as zero.
With respect to the roll of the aircraft and the angle around the pitch axis, the deviation between the target value calculated in the second target value determination step and the value calculated in the attitude / position information calculation step is calculated, and this deviation approaches zero. As described above, the third target value determination step for determining the target value of the angular velocity around the roll and pitch axis of the aircraft, and
The posture / position information calculation step sets the target value of the angular velocity around the yaw axis determined in the first target value determination step and the target value of the angular velocity around the roll and pitch axes determined in the third target value determination step. In the coordinate conversion step of calculating the target value of the angular velocity around the X, Y and Z axes of the aircraft by performing coordinate conversion using the roll and the angle around the pitch axis calculated in step 1.
The gyro effect was added from the target value of the total thrust of the aircraft determined in the second target value determination step and the target value of the angular velocity around the X, Y and Z axes of the aircraft calculated in the coordinate conversion step. The target value of the thrust of each rotor is determined based on the rotational equation of motion that expresses the steady state by approximating the angular velocity of each rotor to the standard angular velocity at the time of hovering, which is a known value. With a fourth target value determination step
It is a control method of a multicopter that controls the rotation of each rotor based on the target value of the thrust of each rotor determined in the fourth target value determination step.

前記第一及び第三の目標決定ステップにおいて、前記偏差を算出した各項目のうち、前記偏差の絶対値が所定値以下の項目は、当該偏差をゼロとみなして、当該項目の速度又は角速度の目標値を決定することが好ましい。 Of the items for which the deviation is calculated in the first and third target determination steps, the item whose absolute value of the deviation is equal to or less than a predetermined value is regarded as zero and the velocity or angular velocity of the item is determined. It is preferable to determine the target value.

姿勢・位置情報算出ステップにおいて、ランダムウォークを抑制する機能を有したカルマンフィルタを用いて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出することが好ましい。また、姿勢・位置情報算出ステップにおいて、前記機体のＸ、Ｙ及びＺ軸方向の位置と、前記機体のロール、ピッチ及びヨー軸周りの角度とを算出した後、さらにフィルタリング処理を行って平滑化した値を算出結果とすることが好ましい。 In the attitude / position information calculation step, a Kalman filter having a function of suppressing a random walk is used to determine the positions of the aircraft in the X, Y and Z axis directions and the angles around the roll, pitch and yaw axes of the aircraft. It is preferable to calculate. Further, in the attitude / position information calculation step, after calculating the positions of the aircraft in the X, Y and Z axis directions and the angles around the roll, pitch and yaw axes of the aircraft, further filtering processing is performed for smoothing. It is preferable to use the calculated value as the calculation result.

本発明のマルチコプタ及びその制御方法は、定常状態を表した並進運動方程式、及びジャイロ効果を加えて定常状態を表した回転運動方程式を基に、実プロセスを現実的かつ実用的な手法でモデル化して制御の目標値を算出しているので、機体の位置及び姿勢を非常に安定に制御することができる。 The multicopter of the present invention and its control method model the actual process by a realistic and practical method based on the translational equation of motion representing the steady state and the rotational equation of motion expressing the steady state by adding the gyro effect. Since the control target value is calculated, the position and attitude of the aircraft can be controlled very stably.

また、姿勢・位置情報を算出するとき、ランダムウォークを抑制する機能を有したカルマンフィルタを使用したり、ノイズ成分を吸収するためのフィルタリング処理を行ったりすることによって、姿勢・位置情報をより高い精度で算出することができる。 In addition, when calculating posture / position information, the posture / position information can be made more accurate by using a Kalman filter that has a function to suppress random walks and by performing filtering processing to absorb noise components. Can be calculated with.

本発明のマルチコプタの一実施形態の構造を示す平面図である。It is a top view which shows the structure of one Embodiment of the multicopter of this invention. 図１のマルチコプタが有する制御装置の内部構成を示す機能ブロック図である。It is a functional block diagram which shows the internal structure of the control device which a multicopter of FIG. 1 has. 図１のマルチコプタ及びその周囲に定義された複数の座標軸を示す図である。It is a figure which shows the multicopter of FIG. 1 and a plurality of coordinate axes defined around it. 図２の制御装置が有する姿勢・位置情報算出部の内部構成を示すブロック図である。It is a block diagram which shows the internal structure of the posture / position information calculation part which the control device of FIG. 2 has. 図２の制御装置が有する第一の目標値決定部の内部構成を示すブロック図である。It is a block diagram which shows the internal structure of the 1st target value determination part which the control device of FIG. 2 has. 図２の制御回路が有する第三の目標値決定部の内部構成を示すブロック図である。It is a block diagram which shows the internal structure of the 3rd target value determination part which the control circuit of FIG. 2 has. 図１のマルチコプタの各ロータの回転に基づくジャイロ効果を示す図である。It is a figure which shows the gyro effect based on the rotation of each rotor of the multicopter of FIG. 一般的なＰＩＤ制御の流れを示す図である。It is a figure which shows the flow of the general PID control.

以下、本発明のマルチコプタ及びその制御方法の一実施形態について、図１〜図７に基づいて説明する。この実施形態のマルチコプタ１０は、いわゆるクワッドエックス型と呼ばれるマルチコプタで、図１に示すように、機体１２の中心Ｔから離れた位置に４つのロータ１４(1)〜１４(4)が、機体１２の前後に互いに平行に一対ずつ設けられ、放射状に配置されている。左前側の第一ロータ１４(1)及び右後側の第三ロータ１４(3)は反時計回りに回転し、左後側の第二ロータ１４(2)及び右前側の第四ロータ１４(4)は時計回りに回転することによって、各ロータ１４(1)〜１４(4)の回転による反トルクを打ち消すとともに、揚力を発生させる。機体１２を前方向に移動させるときは、後側の２つのロータ１４(2)，１４(3)の角速度を、前側の２つのロータ１４(1)，１４(4)の角速度よりも速くする。すると、機体１２の後部が浮き上がる形になり、前方向への推進力が発生する。 Hereinafter, an embodiment of the multicopter of the present invention and a control method thereof will be described with reference to FIGS. 1 to 7. The multicopter 10 of this embodiment is a so-called quad-X type multicopter, and as shown in FIG. 1, four rotors 14 (1) to 14 (4) are located at positions away from the center T of the machine body 12, and the machine body 12 is provided. A pair of them are provided parallel to each other in front of and behind the tongue, and are arranged in a radial pattern. The first rotor 14 (1) on the left front side and the third rotor 14 (3) on the right rear side rotate counterclockwise, and the second rotor 14 (2) on the left rear side and the fourth rotor 14 (4) on the right front side ( By rotating clockwise, 4) cancels the anti-torque caused by the rotation of each rotor 14 (1) to 14 (4) and generates lift. When moving the machine body 12 in the forward direction, the angular velocities of the two rotors 14 (2) and 14 (3) on the rear side are made faster than the angular velocities of the two rotors 14 (1) and 14 (4) on the front side. .. Then, the rear portion of the machine body 12 is lifted, and a propulsive force in the forward direction is generated.

機体１２の内部には、４つのロータ１４(1)〜１４(4)の回転を制御する制御装置１６が搭載されている。制御装置１６は、図２に示すように、検出値取得部１８、姿勢・位置情報算出部２０、第一の目標値決定部２２、第二の目標値決定部２４、第三の目標値決定部２６、座標変換部２８、第四の目標値決定部３０及び第五の目標値決定部３２を備えている。 Inside the machine body 12, a control device 16 for controlling the rotation of the four rotors 14 (1) to 14 (4) is mounted. As shown in FIG. 2, the control device 16 includes a detection value acquisition unit 18, a posture / position information calculation unit 20, a first target value determination unit 22, a second target value determination unit 24, and a third target value determination. A unit 26, a coordinate conversion unit 28, a fourth target value determination unit 30, and a fifth target value determination unit 32 are provided.

本発明のマルチコプタの制御方法の一実施形態は、検出値取得ステップ、姿勢・位置情報算出ステップ、第一の目標値決定ステップ、第二の目標値決定ステップ、第三の目標値決定ステップ、座標変換ステップ、及び第四の目標値決定ステップ、及び第五の目標値決定ステップにより構成され、制御装置１６の各ブロックによって実行される。 One embodiment of the multicopter control method of the present invention includes a detection value acquisition step, a posture / position information calculation step, a first target value determination step, a second target value determination step, a third target value determination step, and coordinates. It is composed of a conversion step, a fourth target value determination step, and a fifth target value determination step, and is executed by each block of the control device 16.

以下、制御装置１６の各ブロックの機能を順に説明する。なお、制御装置１６には、図３に示すように、あらかじめ複数の座標軸が定義されている。ＸＹＺの３軸は、機体１２の中心Ｔを原点として互いに直交する軸であり、Ｘ軸が機体１２の前後方向、Ｙ軸が機体１２の左右方向、Ｚ軸が機体１２の上下方向であり、鉛直方向である。また、13軸は、第一及び第三ロータ１４(1)，１４(3)の位置を結ぶ対角の方向、24軸は、第二及び第四ロータ１４(2)，１４(4)の位置を結ぶ対角の方向である。また、機体１２には、図示しないロール、ピッチ、ヨーの３軸がある。 Hereinafter, the functions of each block of the control device 16 will be described in order. As shown in FIG. 3, a plurality of coordinate axes are defined in advance in the control device 16. The three axes of XYZ are axes orthogonal to each other with the center T of the machine body 12 as the origin, the X axis is the front-rear direction of the machine body 12, the Y axis is the left-right direction of the machine body 12, and the Z axis is the vertical direction of the machine body 12. It is in the vertical direction. The 13th axis is the diagonal direction connecting the positions of the first and third rotors 14 (1) and 14 (3), and the 24th axis is the second and fourth rotors 14 (2) and 14 (4). It is the diagonal direction connecting the positions. Further, the machine body 12 has three axes of roll, pitch, and yaw (not shown).

図２に示す検出値取得部１８は、例えばＩＭＵにより構成され、加速度センサとジャイロセンサ等を使用して、機体１２のＸ、Ｙ及びＺ軸周りの角速度の検出値ω_x，ω_y，ω_zと、Ｘ、Ｙ及びＺ軸方向の加速度の検出値α_x，α_y，α_zとを取得する処理を行う（検出値取得ステップ）。 The detection value acquisition unit 18 shown in FIG. 2 is composed of, for example, an IMU, and uses an acceleration sensor, a gyro sensor, or the like to detect values ω _x , ω _y , ω of angular velocities around the X, Y, and Z axes of the machine body 12. _The _{process of acquiring z} _{and the detected values α x} , α _y , and α z of the acceleration in the X, Y, and Z axis directions is performed (detection value acquisition step).

姿勢・位置情報算出部２０は、検出値取得部１８が取得した各検出値から、機体１２のＸ、Ｙ及びＺ軸方向の位置ｘ、ｙ，ｚと、ロール、ピッチ及びヨー軸周りの角度φ，θ，ψとを算出する処理を行う（姿勢・位置情報算出ステップ）。 The attitude / position information calculation unit 20 determines the positions x, y, z of the machine body 12 in the X, Y and Z axis directions from the detected values acquired by the detection value acquisition unit 18, and the angles around the roll, pitch and yaw axes. Performs the process of calculating φ, θ, and ψ (posture / position information calculation step).

姿勢・位置情報算出部２０の内部は、例えば図４のように構成することができる。まず、検出値取得部１８が取得したＸ、Ｙ及びＺ軸周りの角速度の検出値ω_x，ω_y，ω_zを座標変換部３４で座標変換し、ロール、ピッチ及びヨー軸周りの角速度φ（ドット），θ（ドット），ψ（ドット）を算出し、その後、演算部３６ａ，３６ｂ，３６ｃで、ロール、ピッチ及びヨー軸周りの角度φ，θ，ψを各々算出する。 The inside of the posture / position information calculation unit 20 can be configured as shown in FIG. 4, for example. _{First, the detected values ω x} , ω _y , ω _z of the angular velocities around the X, Y and Z axes acquired by the detected value acquisition unit 18 are coordinate-converted by the coordinate conversion unit 34, and the angular velocities φ around the roll, pitch and yaw axes are converted. (Dot), θ (dot), and ψ (dot) are calculated, and then the calculation units 36a, 36b, and 36c calculate the roll, pitch, and angles φ, θ, and ψ around the yaw axis, respectively.

演算部３６ａ，３６ｂ，３６ｃは、単純に積分を行う構成にしてもよいが、例えば、所定のカルマンフィルタを使用して角度φ，θ，ψを推定する構成にすることが好ましい。これにより、白色雑音によるランダムウォーク（ゼロ点がランダムにドリフトする現象）を適切に抑制することができ、角度φ，θ，ψを高精度に算出することができる。 The arithmetic units 36a, 36b, and 36c may be configured to simply perform integration, but for example, it is preferable to configure the arithmetic units 36a, 36b, and 36c to estimate the angles φ, θ, and ψ using a predetermined Kalman filter. As a result, a random walk (a phenomenon in which the zero point drifts randomly) due to white noise can be appropriately suppressed, and the angles φ, θ, and ψ can be calculated with high accuracy.

演算部３６ａ，３６ｂ，３６ｃで算出した角度φ，θ，ψは、座標変換部３４にフィードバックされ、次回の座標変換にも使用される。また、この角度φ，θ，ψは、そのまま姿勢・位置情報算出部１８の算出結果としてもよいが、例えば、ローパスフィルタ３８ａ，３８ｂ，３８ｃでフィルタリング処理を行い、平滑化した値φ（チルダ）、θ（チルダ）、ψ（チルダ）を算出結果とすることが好ましい。演算部３６ａ，３６ｂ，３６ｃで算出した角度φ，θ，ψには、機体１２の振動やノイズによる高周波成分が含まれているため、フィルタリング処理を行うことによって、これらの外乱を除去することができる。 The angles φ, θ, and ψ calculated by the calculation units 36a, 36b, and 36c are fed back to the coordinate conversion unit 34 and used for the next coordinate conversion. Further, the angles φ, θ, and ψ may be the calculation results of the posture / position information calculation unit 18 as they are, but for example, the values φ (tilde) smoothed by filtering with the low-pass filters 38a, 38b, 38c. , Θ (tilde), ψ (tilde) are preferably the calculation results. Since the angles φ, θ, and ψ calculated by the calculation units 36a, 36b, and 36c contain high-frequency components due to vibration and noise of the machine body 12, these disturbances can be removed by performing filtering processing. can.

さらに、姿勢・位置情報算出部１８は、検出値取得部１８が取得したＸ、Ｙ及びＺ軸方向の加速度の検出値α_x，α_y，α_zを座標変換部４０で座標変換し、慣性座標系におけるＸ、Ｙ及びＺ軸方向の加速度の検出値ｘ（ツードット），ｙ（ツードット），ｚ（ツードット）を算出する。この座標変換には、先に演算部３６ａ，３６ｂ，３６ｃで算出した角度φ，θ，ψも利用される。 _{Further, the attitude / position information calculation unit 18 coordinates the detected values α x} , α _y , α _z of the accelerations in the X, Y, and Z-axis directions acquired by the detection value acquisition unit 18 by the coordinate conversion unit 40, and the inertial force. The detected values x (two dots), y (two dots), and z (two dots) of the accelerations in the X, Y, and Z axis directions in the coordinate system are calculated. For this coordinate conversion, the angles φ, θ, and ψ previously calculated by the calculation units 36a, 36b, and 36c are also used.

次に、演算部４２ａ，４２ｂ，４２ｃでＸ、Ｙ及びＺ軸方向の位置ｘ、ｙ，ｚを各々算出する。演算部４２ａ，４２ｂ，４２ｃは、単純に積分を行う構成にしてもよいが、上記と同様に、所定のカルマンフィルタを使用して位置ｘ，ｙ，ｚを推定する構成にすることが好ましい。これにより、白色雑音によるランダムウォーク（ゼロ点がランダムにドリフトする現象）を適切に抑制することができ、位置ｘ，ｙ，ｚを高精度に算出することができる。 Next, the calculation units 42a, 42b, and 42c calculate the positions x, y, and z in the X, Y, and Z-axis directions, respectively. The arithmetic units 42a, 42b, and 42c may be configured to simply perform integration, but it is preferable that the arithmetic units 42a, 42b, and 42c are configured to estimate the positions x, y, and z using a predetermined Kalman filter as described above. As a result, a random walk (a phenomenon in which the zero point drifts randomly) due to white noise can be appropriately suppressed, and the positions x, y, and z can be calculated with high accuracy.

演算部４２ａ，４２ｂ，４２ｃで算出した位置ｘ，ｙ，ｚは、そのまま姿勢・位置情報算出部１８の算出結果としてもよいが、上記と同様に、ローパスフィルタ４４ａ，４４ｂ，４４ｃでフィルタリング処理を行い、平滑化した値ｘ（チルダ）、ｙ（チルダ）、ｚ（チルダ）を算出結果とすることが好ましい。演算部４２ａ，４２ｂ，４２ｃで算出した位置ｘ，ｙ，ｚには、機体１２の振動やノイズによる高周波成分が含まれているため、フィルタリング処理を行うことによって、これらの外乱を除去することができる。 The positions x, y, and z calculated by the calculation units 42a, 42b, and 42c may be used as they are as the calculation results of the posture / position information calculation unit 18, but the filtering process is performed by the low-pass filters 44a, 44b, 44c in the same manner as above. It is preferable to use the smoothed values x (tilde), y (tilde), and z (tilde) as the calculation results. Since the positions x, y, and z calculated by the calculation units 42a, 42b, and 42c contain high-frequency components due to vibration and noise of the machine body 12, these disturbances can be removed by performing filtering processing. can.

図２に示す第一の目標値決定部２２は、機体１２のＸ、Ｙ及びＺ軸方向の位置について、各目標値ｘ_r，ｙ_r，ｚ_rと姿勢・位置情報算出部２０が算出した値ｘ，ｙ，ｚとの偏差ｄｘ，ｄｙ，ｄｚを算出し、各偏差ｄｘ，ｄｙ，ｄｚがゼロに近づくように、機体のＸ、Ｙ及びＺ軸方向の速度の目標値ｘ_r（ドット），ｙ_r（ドット），ｚ_r（ドット）を決定する処理を行う。さらに、機体１２のヨー軸周りの角度について、目標値ψ_rと姿勢・位置情報算出部２０が算出した値ψとの偏差ｄψを算出し、この偏差ｄψがゼロに近づくように、機体１２のヨー軸周りの角速度の目標値ψ_r（ドット）を決定する処理を行う（第一の目標値決定ステップ）。 The first target value determination unit 22 shown in FIG. 2 is calculated by the attitude / position information calculation unit 20 with _{each target value x r} , y _r , z _r for the positions of the aircraft 12 in the X, Y and Z axis directions. _{The deviation dx, dy, dz from the values x, y, z is calculated, and the target value x r} (dot) of the velocity in the X, Y, and Z axis directions of the aircraft so that each deviation dx, dy, dz approaches zero. ), Y _r (dot), z _r (dot). Further, with respect to the angle around the yaw axis of the aircraft 12, _{the deviation dψ between the target value ψ r} and the value ψ calculated by the attitude / position information calculation unit 20 is calculated, and the aircraft 12 is set so that the deviation dψ approaches zero. _{The process of determining the target value ψ r} (dot) of the angular velocity around the yaw axis is performed (first target value determination step).

偏差と目標値の関係は、図５の中のグラフに示すように、偏差の絶対値が大きいほど目標値の絶対値が大きくなる関係であることが好ましく、例えば関数式を用いて規定することができる。この場合、関数式は、１次関数や２次関数でもよいし、その他の関数でもよい。 As shown in the graph in FIG. 5, the relationship between the deviation and the target value is preferably such that the larger the absolute value of the deviation, the larger the absolute value of the target value. For example, it should be specified by using a function formula. Can be done. In this case, the function expression may be a linear function, a quadratic function, or another function.

このとき、偏差を算出した項目のうち、偏差の絶対値が所定の値（ａ１〜ａ４）以下の項目は、当該偏差をゼロとみなして目標値を決定することが好ましい。このような不感帯を設けることにより、微小な偏差に応答して制御がハンチングするのを防止することができる。 At this time, among the items for which the deviation is calculated, it is preferable to determine the target value by regarding the deviation as zero for the items whose absolute value of the deviation is a predetermined value (a1 to a4) or less. By providing such a dead zone, it is possible to prevent the control from hunting in response to a minute deviation.

また、偏差を算出した項目のうち、偏差の絶対値が所定の値（ｂ１〜ｂ４）を超えた項目は、目標値の絶対値を一定の値に制限し、制御が暴走するのを防止するとよい。なお、目標値の絶対値を一定の値に制限することは、第一の目標値決定部２２に実行させてもよいが、他のブロックの動作を利用して実行することも可能である。例えば、検出部取得部１８が有する増幅回路の出力電圧のサチュレーションを利用し、各検出値の絶対値が一定以上の値にならないように制限すれば、同様の作用効果が得られる。 Further, among the items for which the deviation is calculated, for the item in which the absolute value of the deviation exceeds a predetermined value (b1 to b4), the absolute value of the target value is limited to a certain value to prevent the control from running out of control. good. It should be noted that limiting the absolute value of the target value to a certain value may be executed by the first target value determining unit 22, but it can also be executed by using the operation of another block. For example, if the saturation of the output voltage of the amplifier circuit possessed by the detection unit acquisition unit 18 is used and the absolute value of each detection value is limited so as not to exceed a certain value, the same effect can be obtained.

図２に示す第二の目標値決定部２４は、Ｘ、Ｙ及びＺ軸方向の速度ｘ（ドット），ｙ（ドット），ｚ（ドット）が、第一の目標値決定部２２が決定したＸ、Ｙ及びＺ軸方向の速度の目標値ｘ_r（ドット），ｙ_r（ドット），ｚ_r（ドット）と等しくなるように、機体１２の合計推力の目標値Ｆ_r、ロール及びピッチ軸周りの角度の目標値φ_r，θ_rを決定する処理を行う。具体的には、以下に説明するように、姿勢・位置情報算出部２０が算出したヨー軸周りの角度ψを既知の値とし、定常状態を表した並進運動方程式に基づく演算処理を行う（第二の目標値決定ステップ）。 In the second target value determination unit 24 shown in FIG. 2, the speeds x (dots), y (dots), and z (dots) in the X, Y and Z-axis directions are determined by the first target value determination unit 22. Target value of total thrust of the aircraft 12 F _r , roll and pitch axis _{so as to be equal to the target value x r} (dot), y _r (dot), z _r (dot) of the velocity in the X, Y and Z axis directions. Performs the process of determining the target values φ _r and θ _{r of the surrounding angle.} Specifically, as described below, the angle ψ around the yaw axis calculated by the posture / position information calculation unit 20 is set as a known value, and arithmetic processing is performed based on the translational equation of motion representing the steady state (No. 1). Second target value determination step).

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

ここで、ｇは重力加速度、ｍは機体の質量であり、Ａ_x，Ａ_y，Ａ_zは、それぞれＸ，Ｙ及びＺ軸方向の空気抵抗係数である。また、Ｓ_φ，Ｓ_θ，Ｓ_ψは、それぞれsinθ，sinφ，sinψであり、Ｃ_φ，Ｃ_θ，Ｃ_ψは、それぞれcosθ，cosφ，cosψである。 In general, the translational equation of motion of an aircraft can be expressed as Eq. (2).

Here, g is the gravitational acceleration, m is the mass of the airframe, and A _x , A _y , and A _z are the air resistance coefficients in the X, Y, and Z axis directions, respectively. Further, S _φ , S _θ , and S _ψ are sin θ, sin φ, and sin _{ψ, respectively, and C φ} , C _θ , and C _ψ are cos θ, cos φ, and cos ψ, respectively.

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

Next, in order to consider the steady state, if the accelerations x (two dots), y (two dots), and z (two dots) of the equation (2) are set to zero, the simultaneous equations of the following equation (3) can be obtained.

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

Then, by expanding the three equations of the equation (3), it is possible to derive the equation (4) for obtaining F, the equation (5) for obtaining φ, and the equation (6) for obtaining θ, respectively.

このように、定常状態を想定することによって、Ｆ_，φ，θは非常にシンプルな式で求めることができる。 In this way, by assuming a steady state, F _, φ, and θ can be obtained by a very simple equation.

第二の目標値決定部２４は、姿勢・位置情報算出部２０が算出したヨー軸周りの角度ψ、及び第一の目標値決定部が決定したＸ、Ｙ及びＺ軸方向の速度の目標値ｘ_r（ドット），ｙ_r（ドット），ｚ_r（ドット）を受け、これらを式（４）、（５）、（６）の右辺に代入して左辺のＦ，φ，θを算出し、各算出結果を、機体の合計推力の目標値Ｆ_r、ロール及びピッチ軸周りの角度の目標値φ_r，θ_rに決定する。 The second target value determination unit 24 has an angle ψ around the yaw axis calculated by the posture / position information calculation unit 20, and a target value of the speed in the X, Y, and Z axis directions determined by the first target value determination unit. Receive x _r (dot), y _r (dot), z _r (dot) and substitute these into the right side of equations (4), (5), (6) to calculate F, φ, θ on the left side. , Each calculation result is determined as the target value F _r _{of the total thrust of the aircraft, and the target values φ r} and θ _r of the angles around the roll and pitch axes.

第三の目標値決定部２６は、機体１２のロール及びピッチ軸周りの角度について、第二の目標値決定部２４が算出した目標値φ_r，θ_rと姿勢・位置情報算出部２０が算出した値との偏差ｄφ，ｄθを算出し、この偏差ｄφ，ｄθがゼロに近づくように、機体１２のロール及びピッチ軸周りの角速度の目標値φ_r（ドット），θ_r（ドット）を決定する処理を行う（第三の目標値決定ステップ）。 _{The third target value determination unit 26 calculates the target values φ r} and θ _r calculated by the second target value determination unit 24 and the attitude / position information calculation unit 20 with respect to the roll and the angle around the pitch axis of the machine body 12. _{The deviations dφ and dθ from the values obtained are calculated, and the target values φ r} (dots) and θ _r (dots) of the angular velocity around the roll and pitch axes of the aircraft 12 are determined so that the deviations dφ and dθ approach zero. (Third target value determination step).

偏差と目標値の関係は、図６の中のグラフに示すように、偏差の絶対値が大きいほど目標値の絶対値が大きくなる関係であることが好ましく、例えば関数式を用いて規定することができる。この場合、関数式は、１次関数や２次関数でもよいし、その他の関数でもよい。 As shown in the graph in FIG. 6, the relationship between the deviation and the target value is preferably such that the larger the absolute value of the deviation, the larger the absolute value of the target value. For example, it should be specified by using a function formula. Can be done. In this case, the function expression may be a linear function, a quadratic function, or another function.

このとき、偏差を算出した項目のうち、偏差の絶対値が所定の値（ａ５，ａ６）以下の項目は、当該偏差をゼロとみなして目標値を決定することが好ましい。このような不感帯を設けることにより、微小な偏差応答して制御がハンチングするのを防止することができる。 At this time, among the items for which the deviation is calculated, it is preferable to determine the target value by regarding the deviation as zero for the items whose absolute value of the deviation is a predetermined value (a5, a6) or less. By providing such a dead zone, it is possible to prevent the control from hunting in response to a minute deviation.

また、偏差を算出した項目のうち、偏差の絶対値が所定の値（ｂ５，ｂ６）を超えた項目は、目標値の絶対値を一定の値に制限し、制御が暴走するのを防止するとよい。なお、目標値の絶対値を一定の値に制限することは、第三の目標値決定部２６に実行させてもよいが、他のブロックの動作を利用して実行することも可能である。例えば、検出部取得部１８が有する増幅回路の出力電圧のサチュレーションを利用し、各検出値の絶対値が一定以上の値にならないように制限すれば、同様の作用効果が得られる。 Further, among the items for which the deviation is calculated, for the item in which the absolute value of the deviation exceeds a predetermined value (b5, b6), the absolute value of the target value is limited to a certain value to prevent the control from running out of control. good. It should be noted that limiting the absolute value of the target value to a certain value may be executed by the third target value determining unit 26, but it can also be executed by using the operation of another block. For example, if the saturation of the output voltage of the amplifier circuit possessed by the detection unit acquisition unit 18 is used and the absolute value of each detection value is limited so as not to exceed a certain value, the same effect can be obtained.

図２に示す座標変換部２８は、第一の目標値決定部２２が決定したヨー軸周りの角速度の目標値ψ_r（ドット）、第三の目標値決定部２６が決定したロール及びピッチ軸周りの角速度の目標値φ_r（ドット），θ_r（ドット）を座標変換し、機体１２のＸ、Ｙ及びＺ軸周りの角速度の目標値ω_xr，ω_yr，ω_zrを算出する処理を行う（座標変換ステップ）。 _{The coordinate conversion unit 28 shown in FIG. 2 has a target value ψ r} (dot) of the angular velocity around the yaw axis determined by the first target value determination unit 22, and a roll and pitch axis determined by the third target value determination unit 26. target value of the angular velocity about phi _r (dots), theta _r (dot) coordinate transformation, X aircraft 12, the target value omega _xr of the angular velocity about the Y and Z-axis, omega _yr, the process of calculating the omega _zr Do (coordinate conversion step).

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

In general, the coordinate transformation can be performed using the equation (7). Specifically, the angles φ and θ around the roll and pitch axes calculated by the posture / position information calculation unit 20 are _{substituted into S φ} , S _θ, C _φ and C _θ on the right side as known values, and each target. By substituting the values ψ _r (dot), φ _r (dot), and θ r (dot) into the right side, the target values ω _x r, ω _yr , and ω _z _{r on} the left side are calculated.

第四の目標決定部３０は、第二の目標値決定部２４が決定した機体１２の合計推力の目標値Ｆ_r、及び座標変換部２８が算出した機体１２のＸ、Ｙ及びＺ軸周りの角速度の目標値ω_xr，ω_yr，ω_zrから、各ロータ１４(1)〜１４(4)の各推力の目標値Ｆ_1r，Ｆ_2r，Ｆ_3r，Ｆ_4rを決定する処理を行う。具体的には、以下に説明するように、ジャイロ効果を加えて定常状態を表した回転運動方程式に基づく演算処理を行う（第四の目標値決定ステップ）。 Fourth target determination unit 30, the second body 12 target value F _r of the total thrust, and the coordinate conversion unit 28 calculates the body 12 where the target value determination unit 24 has determined X, around the Y and Z-axis From the target values ω _xr , ω _yr , and ω _{zr of the} angular velocity, the processing is performed to determine _{the target values F 1r} , F _2r , F _3r , and F _4r of each thrust of each rotor 14 (1) to 14 (4). Specifically, as described below, arithmetic processing based on the rotational equation of motion representing the steady state is performed by adding the gyro effect (fourth target value determination step).

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

Generally, the thrust of the rotor is proportional to the square of the angular velocity of the rotor, so the rotor thrust coefficient (proportional coefficient) is k, and the angular velocities of each rotor 14 (1) to 14 (4) are Ω ₁ , Ω ₂ , Ω ₃ , Assuming Ω ₄ , the following equation (8) holds.

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

Further, as shown in FIG. 7, the shaft torque around the 13 axes is τ ₁₃ , the shaft torque around the 24 axes is τ ₂₄ , and the distance from the center T of the machine body 12 to each rotor 14 (1) to 14 (4) ( Assuming that the torque arm) is d, the following equation (9) is established.

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

Next, consider the gyro effect. Since the first and third rotors 14 (1) and 14 (3) have a moment of inertia I around the spin axis orthogonal to the torque axis (13 axes) and rotate at _{angular velocities Ω 1} and Ω _3. It has angular momentums IΩ ₁ and IΩ ₃ . When the shaft torque τ ₁₃ _{is added there, the angular velocities ω 1p} and ω _3p are generated around the presession axis orthogonal to the spin axis and the torque axis (13 axes) due to the gyro effect. Similarly, the second and fourth rotors 14 (2) and 14 (4) have a moment of inertia I around the spin axis orthogonal to the torque axis (24 axis) and rotate at _{angular velocities Ω 2} and Ω _4. Therefore, it has angular momentums IΩ ₂ and IΩ ₄ . When the shaft torque τ ₂₄ _{is added there, the angular velocities ω 2p} and ω _4p are generated around the torsion axis orthogonal to the spin axis and the torque axis (24 axes) due to the gyro effect. The angular velocities ω _1p , ω _2p , ω _3p , and ω _4p around each presession axis can be expressed by the following equation (10).

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

Next, consider the relationship between _{the angular velocities ω x} and ω _y around the X and Y axes calculated by the coordinate conversion unit 28 and the angular velocities ω _1p , ω _2p , ω _3p and ω _{4p around each presession axis.} The relationship between ω _x , ω _y and ω _1p , ω _2p , ω _3p , ω _4p can be expressed by Eq. (11).

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

Then, by substituting the equation (10) into the equation (11) _{to eliminate ω 1p} , ω _2p , ω _3p , ω _4p , and further substituting the equation (9) _{to eliminate τ 13} , τ ₂₄ , X, Equation (12) showing the relationship between _{the angular velocities ω x} , ω _y around the Y and Z axes and the thrusts F ₁ , F ₂ , F ₃ , and F _{4 of each rotor is obtained.}

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

Furthermore, to simplify the calculation, if the angular velocities Ω ₁ , Ω ₂ , Ω ₃ , and Ω ₄ of each rotor in Eq. (12) are approximated to the known values of the standard angular velocities Ω _h during hovering, the following equation is obtained. (13) is obtained.

このように、Ω₁，Ω₂，Ω₃，Ω₄をすべて標準角速度Ω_hに近似することによって、ω_x，ω_yとＦ₁，Ｆ₂，Ｆ₃，Ｆ₄との関係を、非常にシンプルな式で表すことができる。 In this way, by approximating _{Ω 1} , Ω ₂ , Ω ₃ , and Ω ₄ to the standard angular velocity Ω _h _{, the relationship between ω x} , ω _y and F ₁ , F ₂ , F ₃ , and F ₄ is very large. Can be expressed by a simple formula.

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

Next, consider _{the angular velocity ω z} around the Z axis calculated by the coordinate conversion unit 28. The rotation around the Z axis is related to the anti-torque (clockwise / counterclockwise) with respect to the rotation of each rotor, and is caused by the difference between them. Generally, the anti-torque of each rotor is proportional to the square of the angular velocity of the rotor and proportional to the thrust of the rotor. Therefore, if the anti-torque of each rotor 14 (1) to 14 (4) is T ₁ , T ₂ , T ₃ , T ₄ , and the torque conversion coefficient (proportional coefficient between thrust and anti-torque) is μ, the equation (14) ) Is established. In the equation (14), the counterclockwise counter-torque is positive and the clockwise counter-torque is negative.

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

Here, consider a state in which the anti-torques T ₁ , T ₂ , T ₃ , and T ₄ act on the aircraft, and the aircraft 12 rotates counterclockwise _{around the Z axis at an angular velocity ω z.} _{The angular momentums L 1} , L ₂ , L ₃ , and L ₄ of each rotor 14 (1) to 14 (4) are L ₁ = IΩ ₁ , L ₂ = IΩ ₂ , L ₃ = IΩ ₃ , L ₄ = IΩ _4. Therefore, the following equation (15) is established by Euler's equation of rotational motion.

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

Next, in order to consider the steady state, the first term on the left side of equation (15) is set to zero, and equation (14) is substituted _{to eliminate T 1} , T ₂ , T ₃ , and T ₄ , and the following equation is obtained. (16) is obtained.

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

Furthermore, to simplify the calculation, if the angular velocities Ω ₁ , Ω ₂ , Ω ₃ , and Ω ₄ of each rotor in equation (16) are approximated to the known values of the standard angular velocities Ω _h during hovering, the following equation is obtained. (17) is obtained.

このように、Ω₁，Ω₂，Ω₃，Ω₄をすべて標準角速度Ω_hに近似することによって、ω_zとＦ₁，Ｆ₂，Ｆ₃，Ｆ₄との関係を、非常にシンプルな式で表すことができる。 In this way, by approximating _{Ω 1} , Ω ₂ , Ω ₃ , and Ω ₄ to the standard angular velocity Ω _h _{, the relationship between ω z} and F ₁ , F ₂ , F ₃ , and F ₄ is very simple. It can be expressed by an expression.

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

Then, the relations of the equations (13) and (17) and the relation of F = F ₁ + F ₂ + F ₃ + F ₄ are summarized as the equation (18), and the equation (19) is obtained by transforming this. Will be.

第四の目標値決定部３０は、第二の目標値決定部２４が決定した合計推力の目標値Ｆ_r、及び座標変換部２８が算出したＸ、Ｙ及びＺ軸周りの角速度の目標値ω_xr，ω_yr，ω_zrを受け、これらを式（１９）の右辺に代入して左辺のＦ₁，Ｆ₂，Ｆ₃，Ｆ₄を算出し、各算出結果を各ロータ１４(1)〜１４(4)の推力の目標値Ｆ_1r，Ｆ_2r，Ｆ_3r，Ｆ_4rに決定する。 Fourth target value determination unit 30, X target value F _r of the total thrust second target value decision unit 24 decides, and the coordinate conversion unit 28 calculates the target value of the angular velocity about the Y and Z-axis ω _{Receives xr} , ω _yr , and ω _zr , substitutes these into the right side of equation (19), _{calculates F 1} , F ₂ , F ₃ , and F ₄ on the left side, and calculates each calculation result from each rotor 14 (1) to 14 (4) The target values of thrust are determined to be F _1r , F _2r , F _3r , and F _4r .

制御装置１６は、各ロータ１４(1)〜１４(4)の推力が、目標値Ｆ_1r，Ｆ_2r，Ｆ_3r，Ｆ_4rになるように、各ロータ１４(1)〜１４(4)の回転を制御する。 The control device 16 of each rotor 14 (1) to 14 (4) so that the thrust of each rotor 14 (1) to 14 (4) becomes the target values F _1r , F _2r , F _3r , and F _4r . Control rotation.

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

For example, when the thrust of the rotor is controlled by the angular velocity, the following equation (20) can be used. Equation (20) is a modification of the above equation (8).

具体的には、図２に示すように、第五の目標値決定部３２を設けるとよい。第五の目標値決定部３２は、第四の目標値決定部３０が決定した各ロータ１４(1)〜１４(4)の推力の目標値Ｆ_1r，Ｆ_2r，Ｆ_3r，Ｆ_4rを受け、これらを式（２０）の右辺に代入して左辺のΩ₁，Ω₂，Ω₃，Ω₄を算出し、各算出結果を各ロータ１４(1)〜１４(4)の角速度の目標値Ω_1r，Ω_2r，Ω_3r，Ω_4rに決定する。そして、制御装置１６は、各ロータ１４(1)〜１４(4)の角速度がΩ_1r，Ω_2r，Ω_3r，Ω_4rになるように制御する。 Specifically, as shown in FIG. 2, it is preferable to provide a fifth target value determination unit 32. _{The fifth target value determination unit 32 receives the target values F 1r} , F _2r , F _3r , and F _4r of the thrusts of the rotors 14 (1) to 14 (4) determined by the fourth target value determination unit 30. , Substituting these into the right side of equation (20) _{to calculate Ω 1} , Ω ₂ , Ω ₃ , Ω ₄ on the left side, and each calculation result is the target value of the angular velocity of each rotor 14 (1) to 14 (4). Decide on Ω _1r , Ω _2r , Ω _3r , and Ω _4r. Then, the control device 16 controls so that the angular velocities of the rotors 14 (1) to 14 (4) are Ω _1r , Ω _2r , Ω _3r , and Ω _4r .

以上説明したように、マルチコプタ１０及び制御装置１６が実行する制御方法は、定常状態を表した並進運動方程式、及びジャイロ効果を加えて定常状態を表した回転運動方程式を基に、実プロセスを現実的かつ実用的にモデル化して制御の目標値を算出しているので、機体１２の位置及び姿勢を非常に安定に制御することができる。 As described above, the control method executed by the multicopter 10 and the control device 16 realizes the actual process based on the translational equation of motion representing the steady state and the rotational equation of motion expressing the steady state by adding the gyro effect. Since the control target value is calculated by modeling it practically and practically, the position and attitude of the machine body 12 can be controlled very stably.

なお、本発明のマルチコプタ及びその制御方法は、上記実施形態に限定されるものではない。例えば、上述したマルチコプタ１０の制御方法に関する式（２）〜式（２０）は一例であり、各式の一部を変更し、より厳密な計算を行うようにしてもよい。 The multicopter of the present invention and its control method are not limited to the above embodiments. For example, the above-mentioned equations (2) to (20) relating to the control method of the multicopter 10 are examples, and a part of each equation may be changed to perform more rigorous calculation.

その他、上記のマルチコプタ１０はクワッドエックス型であるが、同様の考え方でクワッドプラス型を想定した並進運動方程式及び回転運動方程式を設定すれば、クワッドプラス型においても同様の効果を得ることができる。トライコプタ、ヘキサコプタ、オクトコプタについても同様である。 In addition, the above-mentioned multicopter 10 is a quad-X type, but if the translational equation of motion and the rotational equation of motion assuming the quad-plus type are set in the same way, the same effect can be obtained in the quad-plus type. The same applies to tricopters, hexacopters, and octocopters.

１０マルチコプタ
１２機体
１４(1)〜１４(4) 第一〜第四ロータ
１６制御装置
１８検出値取得部
２０位置・姿勢情報算出部
２２第一の目標値決定部
２４第二の目標値決定部
２６第三の目標値決定部
２８座標変換部
３０第四の目標値決定部
３２第五の目標値決定部 10 Multicopter 12 Aircraft 14 (1) -14 (4) 1st to 4th rotor 16 Control device 18 Detection value acquisition unit 20 Position / attitude information calculation unit 22 1st target value determination unit 24 2nd target value determination unit 26 Third target value determination unit 28 Coordinate conversion unit 30 Fourth target value determination unit 32 Fifth target value determination unit

Claims

In a multicopter including an airframe, a plurality of rotors radially arranged at positions away from the center of the airframe, and a control device for controlling the rotation of each rotor.
The control device is
The X, Y, and Z axes that are orthogonal to each other with the center of the aircraft as the origin are defined, and the detected values of the angular velocities around the X, Y, and Z axes of the aircraft and the acceleration in the X, Y, and Z axis directions of the aircraft are defined. The detected value acquisition unit that acquires the detected value, and
A posture / position information calculation unit that calculates the positions of the aircraft in the X, Y, and Z axis directions and the angles around the roll, pitch, and yaw axes of the aircraft from each detection value acquired by the detection value acquisition unit. ,
With respect to the positions of the aircraft in the X, Y and Z-axis directions, the deviation between each target value and the value calculated by the attitude / position information calculation unit is calculated, and the X of the aircraft is set so that each deviation approaches zero. In addition to determining the target value of the velocity in the Y and Z axis directions,
With respect to the angle around the yaw axis of the aircraft, the deviation between the target value and the value calculated by the attitude / position information calculation unit is calculated, and the target of the angular velocity around the yaw axis of the aircraft is set so that this deviation approaches zero. The first target value determination unit that determines the value, and
From the angle around the yaw axis calculated by the posture / position information calculation unit and the target values of the speeds in the X, Y and Z axis directions determined by the first target value determination unit, the X, Y and Z of the aircraft. A second target value determination unit that determines the target value of the total thrust of the aircraft, the target value of the roll and the angle around the pitch axis, based on the translational equation of motion that expresses the steady state with the acceleration in the axial direction as zero.
With respect to the roll of the machine and the angle around the pitch axis, the deviation between the target value calculated by the second target value determination unit and the value calculated by the attitude / position information calculation unit is calculated, and this deviation approaches zero. As described above, the third target value determining unit for determining the target value of the angular velocity around the roll and pitch axis of the aircraft, and the like.
The posture / position information calculation unit determines the target value of the angular velocity around the yaw axis determined by the first target value determination unit and the target value of the angular velocity around the roll and pitch axes determined by the third target value determination unit. Coordinate conversion unit that calculates the target value of the angular velocity around the X, Y and Z axes of the aircraft by performing coordinate conversion using the roll and the angle around the pitch axis calculated by
The gyro effect was added from the target value of the total thrust of the aircraft determined by the second target value determination unit and the target value of the angular velocity around the X, Y and Z axes of the aircraft calculated by the coordinate conversion unit. The target value of the thrust of each rotor is determined based on the rotational equation of motion that expresses the steady state by approximating the angular velocity of each rotor to the standard angular velocity at the time of hovering, which is a known value. Equipped with a fourth target value determination unit
A multicopter characterized in that the rotation of each rotor is controlled based on the target value of the thrust of each rotor determined by the fourth target value determination unit.

The first and third goal determination units are among the items for which the deviation is calculated.
The multicopter according to claim 1, wherein an item in which the absolute value of the deviation is equal to or less than a predetermined value is regarded as zero, and the target value of the velocity or the angular velocity of the item is determined.

The attitude / position information calculation unit uses a Kalman filter having a function of suppressing a random walk to determine the positions of the aircraft in the X, Y and Z axis directions and the angles around the roll, pitch and yaw axes of the aircraft. The multicopter according to claim 1 or 2 to be calculated.

The attitude / position information calculation unit calculates the positions of the aircraft in the X, Y, and Z axis directions and the angles around the roll, pitch, and yaw axes of the aircraft, and then further performs filtering processing to smooth the values. The multicopter according to any one of claims 1 to 3, wherein the calculation result is.

In a method of controlling a multicopter including an airframe and a plurality of rotors radially arranged at positions away from the center of the airframe.
The X, Y, and Z axes that are orthogonal to each other with the center of the aircraft as the origin are defined, and the detected values of the angular velocities around the X, Y, and Z axes of the aircraft and the acceleration in the X, Y, and Z axis directions of the aircraft are defined. The detection value acquisition step to acquire the detection value and
A posture / position information calculation step for calculating the positions of the aircraft in the X, Y, and Z axis directions and the angles around the roll, pitch, and yaw axes of the aircraft from the detected values acquired in the detection value acquisition step. ,
With respect to the positions of the aircraft in the X, Y and Z-axis directions, the deviation between each target value and the value calculated in the posture / position information calculation step is calculated, and the X of the aircraft is set so that each deviation approaches zero. In addition to determining the target value of the velocity in the Y and Z axis directions,
For the angle around the yaw axis of the aircraft, calculate the deviation between the target value and the value calculated in the posture / position information calculation step, and aim the angular velocity around the yaw axis of the aircraft so that this deviation approaches zero. The first target value determination step to determine the value, and
From the angle around the yaw axis calculated in the posture / position information calculation step and the target values of the speeds in the X, Y and Z axis directions determined in the first target value determination step, the X, Y and Z of the aircraft. A second target value determination step for determining the target value of the total thrust of the aircraft, the target value of the roll and the angle around the pitch axis, based on the translational equation of motion expressing the steady state with the axial acceleration as zero.
With respect to the roll of the aircraft and the angle around the pitch axis, the deviation between the target value calculated in the second target value determination step and the value calculated in the attitude / position information calculation step is calculated, and this deviation approaches zero. As described above, the third target value determination step for determining the target value of the angular velocity around the roll and pitch axis of the aircraft, and
The posture / position information calculation step sets the target value of the angular velocity around the yaw axis determined in the first target value determination step and the target value of the angular velocity around the roll and pitch axes determined in the third target value determination step. In the coordinate conversion step of calculating the target value of the angular velocity around the X, Y and Z axes of the aircraft by performing coordinate conversion using the roll and the angle around the pitch axis calculated in step 1.
The gyro effect was added from the target value of the total thrust of the aircraft determined in the second target value determination step and the target value of the angular velocity around the X, Y and Z axes of the aircraft calculated in the coordinate conversion step. The target value of the thrust of each rotor is determined based on the rotational equation of motion that expresses the steady state by approximating the angular velocity of each rotor to the standard angular velocity at the time of hovering, which is a known value. With a fourth target value determination step
A method for controlling a multicopter, which controls rotation of each rotor based on a target value of thrust of each rotor determined in the fourth target value determination step.

Of the items for which the deviation was calculated in the first and third goal determination steps,
The multicopter control method according to claim 5, wherein for an item in which the absolute value of the deviation is equal to or less than a predetermined value, the deviation is regarded as zero and the target value of the velocity or the angular velocity of the item is determined.

In the attitude / position information calculation step, a Kalman filter having a function of suppressing a random walk is used to determine the positions of the aircraft in the X, Y and Z axis directions and the angles around the roll, pitch and yaw axes of the aircraft. The method for controlling a multicopter according to claim 5 or 6 for calculation.

In the attitude / position information calculation step, the positions in the X, Y, and Z axis directions of the aircraft and the roll, pitch, and angles around the yaw axis of the aircraft are calculated, and then further filtered to smooth the values. The method for controlling a multicopter according to any one of claims 5 to 7, wherein the calculation result is the same.