---
title: ドローンの2Dシミュレーション
tags: 
author: [伊藤恒平](https://www.docswell.com/user/Kouhei_Ito)
site: [Docswell](https://www.docswell.com/)
thumbnail: https://bcdn.docswell.com/page/47ZMVQ23E3.jpg?width=480
description: ドローンの2Dシミュレーション by 伊藤恒平
published: July 17, 22
canonical: https://www.docswell.com/s/Kouhei_Ito/K7D2J5-2022-07-17-163421
---
# Page. 1

![Page Image](https://bcdn.docswell.com/page/47ZMVQ23E3.jpg)

初めてのドローンシミュレーション
2Dマルチコプタシミュレーション⽤⽅程式
Motor
𝑄 = 𝐶# 𝜔&quot;
𝑑𝑖
𝐿 + 𝑅𝑖 + 𝐾𝜔 = 𝑢
𝑑𝑡
𝑑𝜔
J!
+ 𝐷𝜔 + 𝑄 = 𝐾𝑖
𝑑𝑡
初めての⼈はオイラー法を慣れた⾔語で書いたりSimulinkを使い、これら8本の
微分⽅程式を解いて計算（シミュレーション）してみることをお勧めします
※式の形は左右共通
※記号の説明はP.3参照
𝑞
𝜔B
𝜔A
𝜃
インダクタンスを無視
𝑇$ = 𝐶&#039; 𝜔$ &quot; 𝑇% = 𝐶&#039; 𝜔% &quot;
𝑑𝑞
J
= 𝑇$ − 𝑇% 𝑙
𝑑𝑡
𝑑𝑈
𝑀
+ 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶&amp; 𝑈
𝑑𝑡
𝑑𝑊
𝑀
− 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊
𝑑𝑡
𝑈
𝑋
𝑑𝜔
𝐾&quot;
𝐾
J!
+ 𝐷+
𝜔 + 𝐶# 𝜔&quot; = 𝑢
𝑑𝑡
𝑅
𝑅
Copter
𝑥
𝑧
𝑊
𝑌
Kinematics
𝑑𝜃
J
=𝑞
𝑑𝑡
𝑑𝑋
= 𝑈 cos 𝜃 + 𝑊 sin 𝜃
𝑑𝑡
𝑑𝑍
= −𝑈 sin 𝜃 + 𝑊 cos 𝜃
𝑑𝑡
1


# Page. 2

![Page Image](https://bcdn.docswell.com/page/YJ6N56VZJV.jpg)

初めてのドローンシミュレーション
つりあい(Trim)の計算 モータ・機体
変化がない状態、微分が0がつりあいです
Trim状態の状態量は添字0を付けて表します。
定常直線⾶⾏状態を考えた場合
𝑞) = 0
そのほかの状態量は何らかの値を持ちます。（0を含む）
Motor
※式の形は左右共通
Copter
𝑇$ = 𝐶&#039; 𝜔$ &quot; 𝑇% = 𝐶&#039; 𝜔% &quot;
𝑑𝑞
J
= 𝑇$ − 𝑇% 𝑙
𝑑𝑡
𝑑𝑈
𝑀
+ 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶&amp; 𝑈
𝑑𝑡
𝑑𝑊
𝑀
− 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊
𝑑𝑡
𝑇$) − 𝑇%) =0
𝑑𝜔
𝐾&quot;
𝐾
J!
+ 𝐷+
𝜔 + 𝐶# 𝜔&quot; = 𝑢
𝑑𝑡
𝑅
𝑅
𝐾&quot;
𝐾
&quot;
𝐷+
𝜔) + 𝐶# 𝜔) = 𝑢)
𝑅
𝑅
𝑅
𝑢) =
𝐾
𝐾&quot;
𝐷+
𝜔) + 𝐶# 𝜔) &quot;
𝑅
𝜔) = 𝜔$) = 𝜔%)
0 = −𝑀𝑔 sin 𝜃) + 𝐶&amp; 𝑈)
0 = − 𝑇$) + 𝑇%) + 𝑀𝑔 cos 𝜃) + 𝐶( 𝑊)
𝑀𝑔 sin 𝜃)
𝐶&amp;
𝑀𝑔 cos 𝜃) + 𝐶( 𝑊)
𝑇) = 𝑇$) = 𝑇%) =
2
&quot;
𝑇) = 𝐶&#039; 𝜔)
𝑈) =
𝜔) =
𝑀𝑔 cos 𝜃) + 𝐶( 𝑊)
2𝐶&#039;
2


# Page. 3

![Page Image](https://bcdn.docswell.com/page/GJ55Y6D9J4.jpg)

初めてのドローンシミュレーション
つりあい(Trim)の計算 キネマティクス
Kinematics
𝑑𝜃
J
=𝑞
𝑑𝑡
𝑑𝑋
= 𝑈 cos 𝜃 + 𝑊 sin 𝜃
𝑑𝑡
𝑑𝑍
= −𝑈 sin 𝜃 + 𝑊 cos 𝜃
𝑑𝑡
𝑞) = 0
V*) =
𝑈) cos 𝜃) + 𝑊) sin 𝜃)
V+) = − 𝑈) sin 𝜃) + 𝑊) cos 𝜃)
記号
𝑄：プロペラトルク[Nm]
𝐶! ：プロペラトルク係数
𝐿：モータコイルインダクタンス [H]
𝑅：モータコイル抵抗 [H]
𝑖：モータコイル電流 [A]
𝜔&quot; ,𝜔# ：モータ⾓速度 [rad/s]
𝐾：モータトルク定数 [Nm/A][Vs/rad]
𝑖：モータコイル電流 [A]
𝐽$ ：モータ慣性モーメント [kgm2]
𝐷：モータ動粘性抵抗係数 [Nm rad/s]
𝑇&quot; ,𝑇# ：プロペラ推⼒ [N]
𝐶% ：プロペラ推⼒係数
𝐽：機体慣性モーメント [kgm2]
𝑞 ：機体⾓速度(ピッチレート) [rad/s]
𝜃 ：機体姿勢⾓(ピッチ⾓) [rad]
𝑀 ：機体質量 [kg]
𝑙：モータ配置半径 [m]
𝑈 ：機体座標系横速度 [m/s]
𝑊 ：機体座標系縦速度 [m/s]
𝑀 ：機体質量 [kg]
𝐶&amp; , 𝐶&amp; ：空⼒係数
𝑋 , 𝑍 ：慣性座標系位置(横，縦)[m]
3


# Page. 4

![Page Image](https://bcdn.docswell.com/page/9E2NGM857R.jpg)

初めてのドローンシミュレーション
微⼩擾乱法による線形化 モータ
微⼩擾乱法とは、例えば、モータの⾓速度𝜔は
トリム値𝜔) と微⼩変化量∆𝜔の和と考え
𝜔 = 𝜔) + ∆𝜔
として⽅程式を展開し微⼩量を無視し近似する
Motor
※式の形は左右共通
𝑑𝜔
𝐾&quot;
𝐾
&quot;
J!
+ 𝐷+
𝜔 + 𝐶# 𝜔 = 𝑢
𝑑𝑡
𝑅
𝑅
𝑑∆𝜔
𝐾&quot;
J!
+ 𝐷+
(𝜔) + ∆𝜔) + 𝐶# 𝜔) + ∆𝜔
𝑑𝑡
𝑅
&quot;
𝐾
= (𝑢) + ∆𝑢)
𝑅
𝑑∆𝜔
𝐾&quot;
𝐾
J!
+ 𝐷+
(𝜔) + ∆𝜔) + 𝐶# (𝜔)&quot; + 2𝜔) ∆𝜔 + ∆𝜔&quot; ) = (𝑢) + ∆𝑢)
𝑑𝑡
𝑅
𝑅
トリム計算が反映さ
𝑑∆𝜔
𝐾&quot;
𝐾
J!
+ 𝐷+
∆𝜔 + 2𝐶# 𝜔) ∆𝜔 = ∆𝑢
れ簡単になる
𝑑𝑡
𝑅
𝑅
微少量の2乗は⾮常に⼩さ
いとして無視する
𝑑∆𝜔
𝐾&quot;
𝐾
J!
+ 𝐷+
+ 2𝐶# 𝜔) ∆𝜔 = ∆𝑢
𝑑𝑡
𝑅
𝑅
4


# Page. 5

![Page Image](https://bcdn.docswell.com/page/D7YPVX667M.jpg)

初めてのドローンシミュレーション
微⼩擾乱法による線形化 機体⾓速度
Copter
𝑇$ = 𝐶&#039; 𝜔$ &quot; 𝑇% = 𝐶&#039; 𝜔% &quot;
𝑑𝑞
J
= 𝑇$ − 𝑇% 𝑙
𝑑𝑡
𝑑𝑈
𝑀
+ 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶&amp; 𝑈
𝑑𝑡
𝑑𝑊
𝑀
− 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊
𝑑𝑡
推⼒を展開してモータ⾓速
𝑑∆𝑞
&quot;
&quot;
J
= 𝐶&#039; 𝜔$) + ∆𝜔$ − 𝜔%) + ∆𝜔% 𝑙
度で表している
𝑑𝑡
𝑑∆𝑞
微少量の2乗は⾮常に⼩さ
J
= 𝐶&#039; 𝜔$) &quot; + 2𝜔$) ∆𝜔$ + ∆𝜔$ &quot; − 𝜔%) &quot; + 2𝜔%) ∆𝜔% + ∆𝜔% &quot; 𝑙
いとして無視する
𝑑𝑡
トリム計算が反映さ
𝑑∆𝑞
J
= 𝐶&#039; 2𝜔$) ∆𝜔$ − 2𝜔%) ∆𝜔% 𝑙
れ簡単になる
𝑑𝑡
J
𝑑∆𝑞
= 2𝜔) 𝐶&#039; 𝑙 ∆𝜔$ − ∆𝜔%
𝑑𝑡
5


# Page. 6

![Page Image](https://bcdn.docswell.com/page/VEN5M511J8.jpg)

初めてのドローンシミュレーション
微⼩擾乱法による線形化 機体並進速度
Copter
&quot;
&quot;
𝑇$ = 𝐶&#039; 𝜔$ 𝑇% = 𝐶&#039; 𝜔%
𝑑𝑞
J
= 𝑇$ − 𝑇% 𝑙
𝑑𝑡
𝑑𝑈
𝑀
+ 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶&amp; 𝑈
𝑑𝑡
𝑑𝑊
𝑀
− 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊
𝑑𝑡
計算メモ
𝑞) + ∆𝑞 𝑊) + ∆𝑊 = 𝑞) 𝑊) + 𝑞) ∆𝑊 + ∆𝑞𝑊) + ∆𝑞∆𝑊
≈ 𝑊) ∆𝑞
sin/cosは加法定理を
𝑑∆𝑈
𝑀
+ 𝑊) ∆𝑞 = −𝑀𝑔 sin 𝜃) + ∆𝜃 + 𝐶&amp; 𝑈) + ∆𝑈
⽤いて展開
𝑑𝑡
※計算メモ参照
𝑑∆𝑈
sin/cosは近似
𝑀
= −𝑀𝑔 sin 𝜃) cos ∆𝜃 + cos 𝜃) sin ∆𝜃 + 𝐶&amp; 𝑈) + ∆𝑈 − 𝑀𝑊) ∆𝑞
𝑑𝑡
𝑑∆𝑈
𝑀
= −𝑀𝑔 sin 𝜃) + cos 𝜃) I ∆𝜃 + 𝐶&amp; 𝑈) + ∆𝑈 − 𝑀𝑊) ∆𝑞
𝑑𝑡
𝑀
𝑑∆𝑈
= −𝑀𝑔 cos 𝜃) I ∆𝜃 + 𝐶&amp; ∆𝑈 − 𝑀𝑊) ∆𝑞
𝑑𝑡
6


# Page. 7

![Page Image](https://bcdn.docswell.com/page/Y79YNY5YE3.jpg)

初めてのドローンシミュレーション
微⼩擾乱法による線形化 機体並進速度
Copter
&quot;
&quot;
𝑇$ = 𝐶&#039; 𝜔$ 𝑇% = 𝐶&#039; 𝜔%
𝑑𝑞
J
= 𝑇$ − 𝑇% 𝑙
𝑑𝑡
𝑑𝑈
𝑀
+ 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶&amp; 𝑈
𝑑𝑡
𝑑𝑊
𝑀
− 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊
𝑑𝑡
計算メモ
𝑞) + ∆𝑞 U) + ∆𝑈 = 𝑞) 𝑈) + 𝑞) ∆𝑈 + ∆𝑞𝑈) + ∆𝑞∆𝑈
≈ 𝑈) ∆𝑞
𝑑∆𝑊
𝑀
− 𝑈) ∆𝑞 = −𝐶&#039; 𝜔$) &quot; + 2𝜔$) ∆𝜔$ + ∆𝜔$ &quot; − 𝜔%) &quot; + 2𝜔%) ∆𝜔% + ∆𝜔% &quot;
𝑑𝑡
※計算メモ参照
+𝑀𝑔 cos 𝜃) cos ∆𝜃 + sin 𝜃) sin ∆𝜃 + 𝐶( 𝑊) + ∆𝑊
𝑀
𝑀
𝑑∆𝑊
= −𝐶&#039; 𝜔$) &quot; + 2𝜔$) ∆𝜔$ − 𝜔%) &quot; + 2𝜔%) ∆𝜔%
𝑑𝑡
+𝑀𝑔 cos 𝜃) + sin 𝜃) I ∆𝜃 + 𝐶( 𝑊) + ∆𝑊 + 𝑀𝑈) ∆𝑞
𝑑∆𝑊
= −2𝜔) 𝐶&#039; ∆𝜔$ − ∆𝜔% + 𝑀𝑔 sin 𝜃) I ∆𝜃 + 𝐶( ∆𝑊 + 𝑀𝑈) ∆𝑞
𝑑𝑡
7


# Page. 8

![Page Image](https://bcdn.docswell.com/page/G786K6YKJD.jpg)

初めてのドローンシミュレーション
微⼩擾乱法による線形化 ピッチ⾓
Kinematics
𝑑𝜃
J
=𝑞
𝑑𝑡
𝑑𝑋
= 𝑈 cos 𝜃 + 𝑊 sin 𝜃
𝑑𝑡
𝑑𝑍
= −𝑈 sin 𝜃 + 𝑊 cos 𝜃
𝑑𝑡
J
𝑑∆𝜃
= ∆𝑞
𝑑𝑡
8


# Page. 9

![Page Image](https://bcdn.docswell.com/page/L7L9Z94PER.jpg)

初めてのドローンシミュレーション
微⼩擾乱法による線形化 ⽔平位置
Kinematics
𝑑𝜃
J
=𝑞
𝑑𝑡
𝑑𝑋
= 𝑈 cos 𝜃 + 𝑊 sin 𝜃
𝑑𝑡
𝑑𝑍
= −𝑈 sin 𝜃 + 𝑊 cos 𝜃
𝑑𝑡
𝑑∆𝑋
𝑉*) +
=
𝑑𝑡
𝑑∆𝑋
𝑉*) +
=
𝑑𝑡
𝑑∆𝑋
𝑉*) +
=
𝑑𝑡
𝑑∆𝑋
𝑉*) +
=
𝑑𝑡
メモ
∆𝑋や∆𝑍に関しては微少量ではなく初期状態からの変化
量と捉えた⽅が良い
計算メモ
𝑑𝑋 𝑑(𝑋) + ∆𝑋) 𝑑𝑋) 𝑑∆𝑋
𝑑∆𝑋
=
=
+
= 𝑉*) +
𝑑𝑡
𝑑𝑡
𝑑𝑡
𝑑𝑡
𝑑𝑡
𝑈) + ∆𝑈 cos(𝜃) + ∆𝜃) + (𝑊) + ∆𝑊) sin(𝜃) + ∆𝜃)
𝑈) + ∆𝑈 cos 𝜃) cos ∆𝜃 + sin 𝜃) sin ∆𝜃 + (𝑊) + ∆𝑊) sin 𝜃) cos ∆𝜃 + cos 𝜃) sin ∆𝜃
𝑈) + ∆𝑈 cos 𝜃) + sin 𝜃) I ∆𝜃 + (𝑊) + ∆𝑊) sin 𝜃) + cos 𝜃) I ∆𝜃
𝑈) cos 𝜃) + 𝑈) sin 𝜃) I ∆𝜃 + cos 𝜃) I ∆𝑈 + (𝑊) sin 𝜃) + 𝑊) cos 𝜃) I ∆𝜃 + sin 𝜃) I ∆𝑊)
𝑑∆𝑋
= cos 𝜃) I ∆𝑈 + sin 𝜃) I ∆𝑊 + 𝑈) sin 𝜃) + 𝑊) cos 𝜃) ∆𝜃
𝑑𝑡
9


# Page. 10

![Page Image](https://bcdn.docswell.com/page/4EMZVZK27W.jpg)

初めてのドローンシミュレーション
微⼩擾乱法による線形化 垂直位置
Kinematics
𝑑𝜃
J
=𝑞
𝑑𝑡
𝑑𝑋
= 𝑈 cos 𝜃 + 𝑊 sin 𝜃
𝑑𝑡
𝑑𝑍
= −𝑈 sin 𝜃 + 𝑊 cos 𝜃
𝑑𝑡
メモ
∆𝑋や∆𝑍に関しては微少量ではなく初期状態からの変化
量と捉えた⽅が良い
𝑑∆𝑍
= − 𝑈) + ∆𝑈 sin(𝜃) + ∆𝜃) + (𝑊) + ∆𝑊) cos(𝜃) + ∆𝜃)
𝑑𝑡
𝑑∆𝑍
= − 𝑈) + ∆𝑈 sin 𝜃) cos ∆𝜃 + cos 𝜃) sin ∆𝜃 + (𝑊) + ∆𝑊) cos 𝜃) cos ∆𝜃 + sin 𝜃) sin ∆𝜃
𝑑𝑡
𝑑∆𝑍
= − 𝑈) + ∆𝑈 sin 𝜃) + cos 𝜃) I ∆𝜃 + (𝑊) + ∆𝑊) cos 𝜃) + sin 𝜃) I ∆𝜃
𝑑𝑡
𝑑∆𝑍
= − 𝑈) sin 𝜃) + 𝑈) cos 𝜃) I ∆𝜃 + sin 𝜃) I ∆𝑈 + (𝑊) cos 𝜃) + 𝑊) sin 𝜃) I ∆𝜃 + cos 𝜃) I ∆𝑊)
𝑑𝑡
𝑑∆𝑍
= − sin 𝜃) I ∆𝑈 + cos 𝜃) I ∆𝑊 + − 𝑈) cos 𝜃) + 𝑊) sin 𝜃) ∆𝜃
𝑑𝑡
10


# Page. 11

![Page Image](https://bcdn.docswell.com/page/PER626X5J9.jpg)

初めてのドローンシミュレーション
微⼩擾乱法による線形化 まとめ
線形化前
Motor
※式の形は左右共通
&quot;
𝑑𝜔
𝐾
J!
+ 𝐷+
𝑑𝑡
𝑅
𝜔 + 𝐶#
𝜔&quot;
𝐾
= 𝑢
𝑅
Copter
𝑇$ = 𝐶&#039; 𝜔$ &quot; 𝑇% = 𝐶&#039; 𝜔% &quot;
𝑑𝑞
J
= 𝑇$ − 𝑇% 𝑙
𝑑𝑡
𝑑𝑈
𝑀
+ 𝑞𝑊 = −𝑀𝑔 sin 𝜃 + 𝐶&amp; 𝑈
𝑑𝑡
𝑑𝑊
𝑀
− 𝑞𝑈 = − 𝑇$ + 𝑇% + 𝑀𝑔 cos 𝜃 + 𝐶( 𝑊
𝑑𝑡
Kinematics
𝑑𝜃
J
=𝑞
𝑑𝑡
𝑑𝑋
= 𝑈 cos 𝜃 + 𝑊 sin 𝜃
𝑑𝑡
𝑑𝑍
= −𝑈 sin 𝜃 + 𝑊 cos 𝜃
𝑑𝑡
線形化された微分⽅程式から状態空間表現でドローンモデルを表し現代制御理
論等での解析や設計を多なうことができる
線形化後
Motor
&quot;
※式の形は左右共通
𝑑∆𝜔
𝐾
𝐾
J!
+ 𝐷+
+ 2𝐶# 𝜔) ∆𝜔 = ∆𝑢
𝑑𝑡
𝑅
𝑅
Copter
𝑑∆𝑞
J
= 2𝜔) 𝐶&#039; 𝑙 ∆𝜔$ − ∆𝜔%
𝑑𝑡
𝑑∆𝑈
𝑀
= −𝑀𝑔 cos 𝜃) I ∆𝜃 + 𝐶&amp; ∆𝑈
𝑑𝑡
𝑑∆𝑊
𝑀
= −2𝜔) 𝐶&#039; ∆𝜔$ − ∆𝜔% + 𝑀𝑔 sin 𝜃) I ∆𝜃 + 𝐶( ∆𝑊
𝑑𝑡
Kinematics
𝑑∆𝜃
J
= ∆𝑞
𝑑𝑡
𝑑∆𝑋
= cos 𝜃) I ∆𝑈 + sin 𝜃) I ∆𝑊 + 𝑈) sin 𝜃) + 𝑊) cos 𝜃) ∆𝜃
𝑑𝑡
𝑑∆𝑍
= − sin 𝜃) I ∆𝑈 + cos 𝜃) I ∆𝑊 + − 𝑈) cos 𝜃) + 𝑊) sin 𝜃) ∆𝜃
𝑑𝑡
11


