小型四旋翼飞机的仿真以及实物操作 ------- Python matplotlib仿真篇（一）画出飞机

下个星期就要参加中国工程机加粗样式器人大赛了，此之前一直是玩 类人形机器人以及ROS二轮差速小车，和三轮全向小车，从未涉及到四轴飞行器系列，因此，等我比完赛我要开始涉足四旋翼系列…
在淘宝上买了一些硬件之后，先不急，由于抱着练手 matplotlib 的心情来Python仿真四旋翼的飞行过程，在此之间趁机学习四旋翼飞行原理，所需的数学知识…
由于刚刚接触四旋翼，在仿真与实操的情况下，有可能哪里说得不对，那么，看过的小伙伴如果发现了我的错误，请留言告诉我，大家一起讨论…

首先，在桌面新建一个文件夹，名字自拟，反正我的叫做: Quadrotor…
然后我们就可以在此目录下，新建python脚本了…
我使用的编辑器是 Anaconda自带的 Spyder， 同时也用 Pycharm 和 Python自带的ide

打开 Spyder 我们新建 Python脚本，命名为:

Quadrotor.py

然后，就开始编写程序：
既然是 matplotlib 仿真，我们必然是要导入 特定的模块：

import matplotlib.pyplot as plt

导入这个模块我们是为了以图形的方式来展现我们的四旋翼飞机；
创建自定义3D图像；

plt.ion()
fig = plt.figure()
self.ax = fig.add_subplot(111, projection='3d')

plt.ion():
使用plt.ion()这个函数，使matplotlib的显示模式转换为交互（interactive）模式。即使在脚本中遇到plt.show()，代码还是会继续执行。有时候，在plt.show()之前一定不要忘了加plt.ioff()，如果不加，界面会一闪而过，并不会停留。那么动态图像是如何画出来的，请看下面这段代码：

这是最终的效果图…

在这里 介绍旋转矩阵：
维基百科解释的旋转矩阵

还有这篇：非常好！
http://blog.miskcoo.com/2016/12/rotation-in-3d-space



这是我建立的旋转矩阵函数：
在此之前，我们先导入标准库：

from math import cos, sin

还有特殊向量格式的numpy

import numpy as np

def transformation_matrix(self):
    x = self.x
    y = self.y
    z = self.z
    roll = self.roll
    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]
         ])

此后，接着写：在这里面…

在这里我们必须注意了解的是这些函数：

1.np.matmul() 两个数组的矩阵乘积。
https://docs.scipy.org/doc/numpy-1.13.0/reference/generated/numpy.matmul.html

2.plt.xlim 获取或设置当前x轴的限制。
https://matplotlib.org/api/_as_gen/matplotlib.pyplot.xlim.html

3.plt.ylim 获取或设置当前y轴的限制。
https://matplotlib.org/api/_as_gen/matplotlib.pyplot.xlim.html

4.plt.pause(0.001) 暂停间隔秒
https://matplotlib.org/api/_as_gen/matplotlib.pyplot.pause.html

5.np.array 创建一个数组
https://docs.scipy.org/doc/numpy/reference/generated/numpy.array.html

6.plt.cla() 清除当前轴
https://matplotlib.org/api/_as_gen/matplotlib.pyplot.cla.html

7.self.ax.plot 绘制y与x作为线和/或标记，也就是绘制图像
https://matplotlib.org/api/_as_gen/matplotlib.axes.Axes.plot.html

8.self.ax.set_zlim(0, 10) 设置z轴视图限制。
https://matplotlib.org/api/_as_gen/matplotlib.axes.Axes.set_xlim.html

def plot(self):
    T = self.transformation_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.roll = roll
        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.roll = roll
        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 = self.roll
        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 = self.transformation_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)

q = Quadrotor(x=-5, y=5, z=5, roll=0,pitch=0, yaw=0, size=1, show_animation=True)
#q.plot()

看过的小伙伴如果发现了我的错误，请留言告诉我哦！，大家一起讨论…