QuadrotorFly四旋翼无人机动力学模型
主要目的是开发一个用于无人机动力学仿真的简单易用、功能相对齐全的仿真环境(也许是水论文环境)。这个仿真是基于python编写的,GPL开源。git的地址在:
https://github.com/linxiaobo110/QuadrotorFly
主要功能(已实现)
模型功能
- 四旋翼基本动力学模型,即电机推力到角速度、速度的动力学模型。
- 电机动力学模型,简化成一阶惯性系统,控制输入为百分比(即0-1的小数)。
- 系统干扰,作用在基本动力学模型上的均值可设定的随机噪声
- 机型选择,‘x’ 型或 ‘+’ 型。见常见机型
- 提供了常见控制器以供参考,现有PD控制器
- 提供了奖励函数以供学习算法使用
- 状态边界检查,超出最大值后finish标志变成True
仿真功能
- 支持随机初始位置、固定初始位置两种启动模式
- 采样时间可设定,状态更新方式采用4阶龙格库塔方法
- GUI动画,目前基于matplotlib搭建,支持多机
环境与依赖
环境
python3 + 你喜欢的编辑器
新手推荐anconda+spyder简单粗暴,入门建议anaconda+jupyter快捷稳定逼格高,想长期学习的建议anaconda+pycharm门槛高功能强大到难以想象。
需要的库
我使用的环境
- win10 + Anaconda (python 3.6)+ Pycharm
使用教程
这些程序在pycharm、spyder下运行通过。jupyter的测试程序是工程目录下的TestInJupyter.ipynb,模型可以运行不过gui动画不会动,暂时还没有修复。使用过程中有什么建议或者问题可以联系我729527658@qq.com.
测试
下载解压(建议使用git克隆)后运行其中的 QuadrotorFlyTest.py文件。
git克隆指令
git clone https://github.com/linxiaobo110/QuadrotorFly.git
成功运行可以看到以下效果:
运行结束后在画面单击可以看整个过程的飞行曲线
最简实现
不画图和记录数据,就是了解整个调用流程。可以在当前目录里新疆一个Test_simple.py(这个代码在Test文件夹里有),然后放入以下代码:
import numpy as np
import QuadrotorFlyModel as Qfm
uavPara = Qfm.QuadParas()
simPara = Qfm.QuadSimOpt()
quad1 = Qfm.QuadModel(uavPara, simPara)
print("Simplest simulation begin!")
for i in range(1000):
ref = np.array([0., 0., 1., 0.])
stateTemp = quad1.observe()
action2, oil = quad1.get_controller_pid(stateTemp, ref)
quad1.step(action2)
print("Simulation finish!")
遇到“No module name ‘QuadrotorFlyGui’ ”或者 “No module name ‘QuadrotorFlyModel’”,见工程环境设置错误。
自定义仿真参数
在工程下新建Test_full.py(这个代码在Test文件夹里有),然后复制以下代码:
import numpy as np
import QuadrotorFlyModel as Qfm
import QuadrotorFlyGui as Qfg
import MemoryStore
import matplotlib.pyplot as plt
D2R = Qfm.D2R
simPara = Qfm.QuadSimOpt(
init_mode=Qfm.SimInitType.rand, init_att=np.array([5., 5., 5.]), init_pos=np.array([1., 1., 1.]),
max_position=8, max_velocity=8, max_attitude=180, max_angular=200,
sysnoise_bound_pos=0, sysnoise_bound_att=0,
actuator_mode=Qfm.ActuatorMode.dynamic
)
uavPara = Qfm.QuadParas(
g=9.8, tim_sample=0.01, structure_type=Qfm.StructureType.quad_plus,
uav_l=0.45, uav_m=1.5, uav_ixx=1.75e-2, uav_iyy=1.75e-2, uav_izz=3.18e-2,
rotor_ct=1.11e-5, rotor_cm=1.49e-7, rotor_i=9.9e-5,
rotor_cr=646, rotor_wb=166, rotor_t=1.36e-2
)
quad1 = Qfm.QuadModel(uavPara, simPara)
gui = Qfg.QuadrotorFlyGui([quad1])
record = MemoryStore.DataRecord()
quad1.reset_states()
record.clear()
print("Simplest simulation begin!")
for i in range(1000):
ref = np.array([0., 0., 1., 0.])
stateTemp = quad1.observe()
action2, oil = quad1.get_controller_pid(stateTemp, ref)
quad1.step(action2)
record.buffer_append((stateTemp, action2))
gui.render()
bs = np.array([_[0] for _ in record.buffer])
ba = np.array([_[1] for _ in record.buffer])
t = range(0, record.count)
fig1 = plt.figure(2)
plt.clf()
plt.subplot(3, 1, 1)
plt.plot(t, bs[t, 6] / D2R, label='roll')
plt.plot(t, bs[t, 7] / D2R, label='pitch')
plt.plot(t, bs[t, 8] / D2R, label='yaw')
plt.ylabel('Attitude $(\circ)$', fontsize=15)
plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05))
plt.subplot(3, 1, 2)
plt.plot(t, bs[t, 0], label='x')
plt.plot(t, bs[t, 1], label='y')
plt.ylabel('Position (m)', fontsize=15)
plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05))
plt.subplot(3, 1, 3)
plt.plot(t, bs[t, 2], label='z')
plt.ylabel('Altitude (m)', fontsize=15)
plt.legend(fontsize=15, bbox_to_anchor=(1, 1.05))
print("Simulation finish!")
在spyder下的运行效果:
如果遇到spyder运行时,图片在终端里显示(就是没有弹出新窗口),见Spyder图形渲染设置。
基础知识
常见四旋翼无人机机型
-
+ 型四旋翼无人机
-
X 型四旋翼无人机
四旋翼基本动力学模型
p
¨
x
=
[
cos
φ
sin
θ
cos
ψ
+
sin
φ
sin
ψ
]
τ
0
m
+
d
1
p
¨
y
=
[
cos
φ
sin
θ
sin
ψ
−
sin
φ
cos
ψ
]
τ
0
m
+
d
2
p
¨
z
=
cos
θ
c
o
s
φ
τ
0
m
−
g
+
d
3
φ
¨
=
φ
˙
ψ
˙
(
J
z
z
−
J
x
x
J
y
y
)
+
J
R
J
y
y
φ
˙
Ω
R
+
L
J
y
y
τ
1
+
d
4
θ
¨
=
θ
˙
ψ
˙
(
J
y
y
−
J
z
z
J
x
x
)
−
J
R
J
x
x
θ
˙
Ω
R
+
L
J
x
x
τ
2
+
d
5
ψ
¨
=
θ
˙
φ
˙
(
J
x
x
−
J
y
y
J
z
z
)
+
1
J
z
z
τ
3
+
d
6
,
\begin{aligned} \ddot{p}_x&=[\cos{\varphi}\sin{\theta}\cos{\psi}+\sin{\varphi}\sin{\psi}]\frac{\tau_0}{m} + d_1\\ \ddot{p}_y&=[\cos{\varphi}\sin{\theta}\sin{\psi}-\sin{\varphi}\cos{\psi}]\frac{\tau_0}{m} + d_2\\ \ddot{p}_z&=\cos{\theta}cos{\varphi}\frac{\tau_0}{m}-g + d_3\\ \ddot{\varphi}&=\dot{\varphi}\dot{\psi}(\frac{J_{zz}-J_{xx}}{J_{yy}}) + \frac{J_R}{J_{yy}}\dot{\varphi}\Omega_R + \frac{L}{J_{yy}}\tau_1 + d_4\\ \ddot{\theta}&=\dot{\theta}\dot{\psi} (\frac{J_{yy}-J_{zz}}{J_{xx}}) - \frac{J_R}{J_{xx}}\dot{\theta}\Omega_R +\frac{L}{J_{xx}}\tau_2 + d_5\\ \ddot{\psi}&=\dot{\theta}\dot{\varphi}(\frac{J_{xx}-J_{yy}}{J_{zz}}) + \frac{1}{J_{zz}}\tau_3 + d_6, \end{aligned}
p¨xp¨yp¨zφ¨θ¨ψ¨=[cosφsinθcosψ+sinφsinψ]mτ0+d1=[cosφsinθsinψ−sinφcosψ]mτ0+d2=cosθcosφmτ0−g+d3=φ˙ψ˙(JyyJzz−Jxx)+JyyJRφ˙ΩR+JyyLτ1+d4=θ˙ψ˙(JxxJyy−Jzz)−JxxJRθ˙ΩR+JxxLτ2+d5=θ˙φ˙(JzzJxx−Jyy)+Jzz1τ3+d6,
其中
p
x
,
p
y
,
p
z
p_x,p_y,p_z
px,py,pz 位置,
φ
,
θ
,
ψ
\varphi,\theta,\psi
φ,θ,ψ是姿态,
τ
0
,
1
,
2
,
3
\tau_{0,1,2,3}
τ0,1,2,3分别是总体推力,绕x轴、y轴,z轴的扭力。
电机动力学模型
ω
˙
=
1
T
(
−
ω
+
C
R
u
+
w
b
)
T
=
C
T
ω
2
M
=
C
M
ω
2
\begin{aligned} \dot{\omega} &=\frac{1}{T}(-\omega+C_Ru+w_b)\\ T &= C_T\omega^2\\ M &= C_M \omega^2 \end{aligned}
ω˙TM=T1(−ω+CRu+wb)=CTω2=CMω2
其中
ω
\omega
ω是电机的转速;
u
u
u是输入给电机的控制信号;
T
,
M
T,M
T,M分别是电机产生的推力和扭力。
动力学中的力与螺旋桨产生的力关系
以十型举例
τ
0
=
T
0
+
T
1
+
T
2
+
T
3
τ
1
=
T
1
−
T
0
τ
2
=
T
3
−
T
2
τ
3
=
−
M
1
−
M
2
+
M
3
+
M
4
\begin{aligned} \tau_0 &= T_0 + T_1 + T_2 + T_3\\ \tau_1 &= T_1 - T_0\\ \tau_2 &= T_3 - T_2\\ \tau_3 &= -M_1 - M_2 + M_3 + M_4 \end{aligned}
τ0τ1τ2τ3=T0+T1+T2+T3=T1−T0=T3−T2=−M1−M2+M3+M4
FAQ
工程环境设置错误
- 现象:
ModuleNotFoundError: No module named ‘QuadrotorFlyGui’ - 原因:
这是因为工程运行的目录不是QuadrotorFly目录,比如在Test里,或者是在一个上级的目录。 - 解决办法
如果上级目录,修改import语句成
import QuadrotorFly.QuadrotorFlyGui
如果是在Test里,或者不确定在哪里,直接修改程序运行目录
- spyder编辑器
选择为每个文件单独指定配置文件
选择指定文件夹
![在这里插入图片描述](https://img-blog.csdnimg.cn/2019050620353539.png?x-oss-process=image/watermark,type_ZmFuZ3poZW5naGVpdGk,shadow_10,text_aHR0cHM6Ly9ibG9nLmNzZG4ubmV0L2xpbnhpYW9ibzExMA==,size_16,color_FFFFFF,t_70
选择QuadrotorFly目录
Spyder图形渲染设置
选择tool->preferences
设置合适的渲染工具,不是inline
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)