⼩型四旋翼飞机的仿真以及实物操作-------Pythonmatplotlib仿真篇（⼀）画出飞
机
下个星期就要参加中国⼯程机加粗样式器⼈⼤赛了，此之前⼀直是玩 类⼈形机器⼈以及ROS⼆轮差速⼩车，和三轮全向⼩车，从未涉及到四轴飞⾏器系列，因此，等我⽐完赛我要开始涉⾜四旋翼系列…
在淘宝上买了⼀些硬件之后，先不急，由于抱着练⼿ matplotlib 的⼼情来Python仿真四旋翼的飞⾏过程，在此之间趁机学习四旋翼飞⾏原理，所需的数学知识…
由于刚刚接触四旋翼，在仿真与实操的情况下，有可能哪⾥说得不对，那么，看过的⼩伙伴如果发现了我的错误，请留⾔告诉我，⼤家⼀起讨论…
⾸先，在桌⾯新建⼀个⽂件夹，名字⾃拟，反正我的叫做: Quadrotor…
然后我们就可以在此⽬录下，新建python脚本了…
我使⽤的编辑器是 Anaconda⾃带的 Spyder， 同时也⽤ Pycharm 和 Python⾃带的ide
打开 Spyder 我们新建 Python脚本，命名为:
然后，就开始编写程序：
既然是 matplotlib 仿真，我们必然是要导⼊ 特定的模块：
import matplotlib.pyplot as plt
导⼊这个模块我们是为了以图形的⽅式来展现我们的四旋翼飞机；
创建⾃定义3D图像；
plt.ion()
fig = plt.figure()spyder python下载
self.ax = fig.add_subplot(111, projection='3d')
plt.ion():
使⽤plt.ion()这个函数，使matplotlib的显⽰模式转换为交互（interactive）模式。即使在脚本中遇到plt.show()，代码还是会继续执⾏。有时候，在plt.show()之前⼀定不要忘了加plt.ioff()，如果不加，界⾯会⼀闪⽽过，并不会停留。那么动态图像是如何画出来的，请看下⾯这段代码：
这是最终的效果图…
from math import cos, sin
还有特殊向量格式的numpy
import numpy as np
def transformation_matrix(self):
x = self.x
y = self.y
z = self.z
roll = ll
pitch = self.pitch
yaw = self.yaw
return np.array(
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],        [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
[-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
])
此后，接着写：在这⾥⾯…
在这⾥我们必须注意了解的是这些函数：
def plot(self):
T = ansformation_matrix()
p1_t = np.matmul(T, self.p1)
p2_t = np.matmul(T, self.p2)
p3_t = np.matmul(T, self.p3)
p4_t = np.matmul(T, self.p4)
plt.cla()
self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]],
[p1_t[1], p2_t[1], p3_t[1], p4_t[1]],
[p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.')
self.ax.plot([p1_t[0], p2_t[0]], [p1_t[1], p2_t[1]],
[p1_t[2], p2_t[2]], 'r-')
self.ax.plot([p3_t[0], p4_t[0]], [p3_t[1], p4_t[1]],
[p3_t[2], p4_t[2]], 'r-')
self.ax.plot(self.x_data, self.y_data, self.z_data, 'b:')
plt.xlim(-5, 5)
plt.ylim(-5, 5)
self.ax.set_zlim(0, 10)
plt.pause(0.001)
然后，补充剩下的初始代码：
class Quadrotor():
def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=1, show_animation=True):        self.p1 = np.array([size / 2, 0, 0, 1]).T
self.p2 = np.array([-size / 2, 0, 0, 1]).T
self.p3 = np.array([0, size / 2, 0, 1]).T
self.p4 = np.array([0, -size / 2, 0, 1]).T
self.x_data = []
self.y_data = []
self.z_data = []
self.show_animation = show_animation
if self.show_animation:
plt.ion()
fig = plt.figure()
self.ax = fig.add_subplot(111, projection='3d')
self.update_pose(x, y, z, roll, pitch, yaw)
def update_pose(self, x, y, z, roll, pitch, yaw):
self.x = x
self.y = y
self.z = z
self.pitch = pitch
self.yaw = yaw
self.x_data.append(x)
self.y_data.append(y)
self.z_data.append(z)
if self.show_animation:
self.plot()
完整代码如下：
from math import cos, sin
import numpy as np
import matplotlib.pyplot as plt
#size=0.25
class Quadrotor():
def __init__(self, x=0, y=0, z=0, roll=0, pitch=0, yaw=0, size=1, show_animation=True):
self.p1 = np.array([size / 2, 0, 0, 1]).T
self.p2 = np.array([-size / 2, 0, 0, 1]).T
self.p3 = np.array([0, size / 2, 0, 1]).T
self.p4 = np.array([0, -size / 2, 0, 1]).T
self.x_data = []
self.y_data = []
self.z_data = []
self.show_animation = show_animation
if self.show_animation:
plt.ion()
fig = plt.figure()
self.ax = fig.add_subplot(111, projection='3d')
self.update_pose(x, y, z, roll, pitch, yaw)
def update_pose(self, x, y, z, roll, pitch, yaw):
self.x = x
self.y = y
self.z = z
self.pitch = pitch
self.yaw = yaw
self.x_data.append(x)
self.y_data.append(y)
self.z_data.append(z)
if self.show_animation:
self.plot()
def transformation_matrix(self):
x = self.x
y = self.y
z = self.z
roll = ll
pitch = self.pitch
yaw = self.yaw
return np.array(
[[cos(yaw) * cos(pitch), -sin(yaw) * cos(roll) + cos(yaw) * sin(pitch) * sin(roll), sin(yaw) * sin(roll) + cos(yaw) * sin(pitch) * cos(roll), x],            [sin(yaw) * cos(pitch), cos(yaw) * cos(roll) + sin(yaw) * sin(pitch) *
sin(roll), -cos(yaw) * sin(roll) + sin(yaw) * sin(pitch) * cos(roll), y],
[-sin(pitch), cos(pitch) * sin(roll), cos(pitch) * cos(yaw), z]
])
def plot(self):
T = ansformation_matrix()
p1_t = np.matmul(T, self.p1)
p2_t = np.matmul(T, self.p2)
p3_t = np.matmul(T, self.p3)
p4_t = np.matmul(T, self.p4)
plt.cla()
self.ax.plot([p1_t[0], p2_t[0], p3_t[0], p4_t[0]],
[p1_t[1], p2_t[1], p3_t[1], p4_t[1]],
[p1_t[2], p2_t[2], p3_t[2], p4_t[2]], 'k.')

版权声明：本站内容均来自互联网，仅供演示用，请勿用于商业和其他非法用途。如果侵犯了您的权益请与我们联系QQ:729038198，我们将在24小时内删除。