（2-3-2）位置控制算法：无人机运动控制系统（2）

2.3.3  动力学模型

文件decmk/model/copter.py实现了无人机（quadcopter）动力学模型的线性和非线性版本。通过定义Mixer类，将力和力矩命令转换为旋翼执行器命令，并实现了线性和非线性动力学环境模型（Copter_linear和Copter_nonlinear）。这两个模型包括位置、速度、姿态和角速度等状态变量，其中非线性版本使用四元数来表示姿态。通过对动力学方程的数学建模，可以模拟和分析无人机在复杂机动中的控制行为。

class Mixer:
    '''
    接收力和力矩指令并将其转化为执行器指令。
    执行器指令是指每个旋翼产生的力。
    '''
    def __init__(self, d, c):
        self.d = d
        self.c = c
        # 四旋翼
        self.B = np.array([[1, 1, 1, 1],
                           [0, -d, 0, d],
                           [d, 0, -d, 0],
                           [-c, c, -c, c]])
        # 六旋翼
        # self.B = np.array([[1, 1, 1, 1, 1, 1],
        #                    [-d, d, d/2, -d/2, -d/2, d/2],
        #                    [0, 0, d*np.sqrt(3)/2, -d*np.sqrt(3)/2,
        #                     d*np.sqrt(3)/2, -d*np.sqrt(3)/2],
        #                    [c, -c, c, -c, -c, c]])
        self.Binv = np.linalg.pinv(self.B)
    def inverse(self, rotors):
        return self.B.dot(rotors)

    def __call__(self, forces):
        return self.Binv.dot(forces)

class Copter_linear(BaseEnv):
    '''
    Tayoung Lee, Melvin Leok and N. Harris McClmroch,
    "Control of Complex MAneuvers for a Quadrotor UAV Using Geometric
    Method on SE(3)", 2010
    '''
    J = np.diag((0.0820, 0.0845, 0.1377))  # kg-m^2
    Jinv = np.linalg.inv(J)
    m = 4.34  # kg
    d = 0.315  # m
    c = 8.004e-4  # m
    g = 9.81  # m/s^2
    rotor_max = m * g
    rotor_min = 0

    def __init__(self,
                 pos=np.zeros((3, 1)),
                 vel=np.zeros((3, 1)),
                 angle=np.zeros((3, 1)),
                 omega=np.zeros((3, 1))):
        # angle = [phi, theta, psi].T
        # omega = [dot_phi, dot_theta, dot_psi].T
        super().__init__()
        self.pos = BaseSystem(pos)
        self.vel = BaseSystem(vel)
        self.angle = BaseSystem(angle)
        self.omega = BaseSystem(omega)

        self.mixer = Mixer(d=self.d, c=self.c)

    def deriv(self, pos, vel, angle, omega, rotors):
        F, M1, M2, M3 = self.mixer.inverse(rotors)

        M = np.vstack((M1, M2, M3))

        m, g, J = self.m, self.g, self.J
        e3 = np.vstack((0, 0, 1))

        dpos = vel
        dcm = angle2dcm(*angle[::-1])
        dvel = g*e3 - F*dcm.T.dot(e3)/m
        dangle = omega
        domega = self.Jinv.dot(M - np.cross(omega, J.dot(omega), axis=0))

        return dpos, dvel, dangle, domega

    def set_dot(self, t, rotors):
        states = self.observe_list()
        dots = self.deriv(*states, rotors)
        self.pos.dot, self.vel.dot, self.angle.dot, self.omega.dot = dots

class Copter_nonlinear(BaseEnv):
    '''
    Tayoung Lee, Melvin Leok and N. Harris McClmroch,
    "Control of Complex MAneuvers for a Quadrotor UAV Using Geometric
    Method on SE(3)", 2010
    '''
    J = np.diag((0.0820, 0.0845, 0.1377))  # kg-m^2
    Jinv = np.linalg.inv(J)
    m = 4.34  # kg
    d = 0.315  # m
    c = 8.004e-4  # m
    g = 9.81  # m/s^2
    rotor_max = m * g
    rotor_min = 0
    def __init__(self,
                 pos=np.zeros((3, 1)),
                 vel=np.zeros((3, 1)),
                 quat=np.vstack((1, 0, 0, 0)),
                 omega=np.zeros((3, 1))):
        super().__init__()
        self.pos = BaseSystem(pos)
        self.vel = BaseSystem(vel)
        self.quat = BaseSystem(quat)
        self.omega = BaseSystem(omega)

        self.mixer = Mixer(d=self.d, c=self.c)

    def deriv(self, pos, vel, quat, omega, rotors):
        F, M1, M2, M3 = self.mixer.inverse(rotors)
        M = np.vstack((M1, M2, M3))
        m, g, J = self.m, self.g, self.J
        e3 = np.vstack((0, 0, 1))
        dpos = vel
        dcm = quat2dcm(quat)
        dvel = g*e3 - F*dcm.T.dot(e3)/m
        # DCM integration (Note: dcm; I to B) [1]
        _w = np.ravel(omega)
        # unit quaternion integration [4]
        dquat = 0.5 * np.array([[0., -_w[0], -_w[1], -_w[2]],
                                [_w[0], 0., _w[2], -_w[1]],
                                [_w[1], -_w[2], 0., _w[0]],
                                [_w[2], _w[1], -_w[0], 0.]]).dot(quat)
        eps = 1 - (quat[0]**2+quat[1]**2+quat[2]**2+quat[3]**2)
        k = 1
        dquat = dquat + k*eps*quat
        domega = self.Jinv.dot(M - np.cross(omega, J.dot(omega), axis=0))
        return dpos, dvel, dquat, domega

    def set_dot(self, t, rotors):
        states = self.observe_list()
        dots = self.deriv(*states, rotors)
        self.pos.dot, self.vel.dot, self.quat.dot, self.omega.dot = dots

if __name__ == "__main__":
    system = Copter_nonlinear()
    system.set_dot(t=0, rotors=np.zeros((4, 1)))
    print(repr(system))

2.3.4  基于非线性飞行器模型的飞行控制仿真测试

在本项目的“test”目录中有多个测试文件，用于测试各个功能模块的正确性和稳定性。这些测试文件通过使用PyTest进行自动化测试，确保项目中的各个模块在不同情况下都能正常工作，并提供了一个可靠的测试基础，以便持续集成和验证代码的正确性。主要包含如下所示的测试功能：

1. Fault Injection Testing：在test_fault_*.py文件中，对故障注入模块进行测试，验证系统在故障情况下的行为和控制算法的鲁棒性。
2. Adaptive ISMC Testing：在test_AdaptiveISMC_*.py文件中，对Adaptive ISMC控制算法进行测试，包括线性和非线性模型的性能验证。
3. ISMC Testing：在test_ISMC_*.py文件中，对ISMC控制算法进行测试，涵盖线性和非线性系统，验证算法在不同条件下的表现。
4. LQR Testing：在test_lqr.py文件中，对线性二次型调节器（LQR）进行测试，评估其对线性系统的控制性能。

文件test/fault_lqr.py实现了一个无人机的非线性动力学模型，包含了故障注入和LQR控制器。通过仿真环境和控制器的协同作用，记录了飞行器在参考轨迹下的状态变量和控制输入，最后绘制了相应的时域图，展示了系统在故障和控制下的动态响应过程。

class Env(BaseEnv):
    def __init__(self):
        super().__init__(max_t=10, dt=0.01)

        # Define faults
        self.actuator_faults = [
            LoE(time=5, index=0, level=0.1),
            # LoE(time=14, index=3, level=0.3)
        ]

        # Define agents
        self.plant = Copter_nonlinear()
        self.n = self.plant.mixer.B.shape[1]
        self.fdi = FDI(numact=self.n)
        self.controller = LQRController(self.plant.Jinv,
                                        self.plant.m,
                                        self.plant.g)

    def step(self):
        *_, done = self.update()
        return done

    def get_ref(self, t, x):
        pos_des = np.vstack((1, -1, -2))
        vel_des = np.vstack((0, 0, 0))
        quat_des = np.vstack((1, 0, 0, 0))
        omega_des = np.zeros((3, 1))
        ref = np.vstack((pos_des, vel_des, quat_des, omega_des))

        return ref

    def _get_derivs(self, t, x, effectiveness):
        ref = self.get_ref(t, x)

        forces = self.controller.get_FM(x, ref)

        # Controller
        Bf = self.plant.mixer.B * effectiveness
        L = np.diag(effectiveness)
        rotors_cmd = np.linalg.pinv(Bf.dot(L)).dot(forces)
        rotors = np.clip(rotors_cmd, 0, self.plant.rotor_max)

        return rotors_cmd, rotors, forces, ref

    def set_dot(self, t):
        x = self.plant.state
        effectiveness = np.array([1.] * self.n)
        for act_fault in self.actuator_faults:
            effectiveness = act_fault.get_effectiveness(t, effectiveness)

        rotors_cmd, rotors, forces, ref = self._get_derivs(t, x, effectiveness)

        self.plant.set_dot(t, rotors)

    def logger_callback(self, t, **kwargs):
        x = self.plant.state
        effectiveness = np.array([1.] * self.n)
        for act_fault in self.actuator_faults:
            effectiveness = act_fault.get_effectiveness(t, effectiveness)
        rotors_cmd, rotors, forces, ref = self._get_derivs(t, x, effectiveness)
        return dict(t=t, **self.observe_dict(), forces=forces, rotors=rotors,
                    rotors_cmd=rotors_cmd, ref=ref)

def run():
    env = Env()
    env.logger = fym.Logger(path="data.h5")
    env.reset()

    while True:
        env.render()
        done = env.step()
        if done:
            break

    env.close()

def exp1():
    run()

def exp1_plot():
    data = fym.logging.load("data.h5")

    fig = plt.figure()
    fig.suptitle("[LQR] state variables")
    ax1 = fig.add_subplot(4, 1, 1)
    ax2 = fig.add_subplot(4, 1, 2, sharex=ax1)
    ax3 = fig.add_subplot(4, 1, 3, sharex=ax1)
    ax4 = fig.add_subplot(4, 1, 4, sharex=ax1)

    ax1.plot(data['t'], data['plant']['pos'].squeeze(), label="plant")
    ax1.plot(data["t"], data["ref"][:, 0, 0], "r--", label="x (cmd)")
    ax1.plot(data["t"], data["ref"][:, 1, 0], "r--", label="y (cmd)")
    ax1.plot(data["t"], data["ref"][:, 2, 0], "r--", label="z (cmd)")
    ax2.plot(data['t'], data['plant']['vel'].squeeze())
    ax3.plot(data['t'], data['plant']['quat'].squeeze())
    ax4.plot(data['t'], data['plant']['omega'].squeeze())

    ax1.set_ylabel('Position [m]')
    ax1.legend([r'$x$', r'$y$', r'$z$'])
    ax1.grid(True)

    ax2.set_ylabel('Velocity [m/s]')
    ax2.legend([r'$u$', r'$v$', r'$w$'])
    ax2.grid(True)

    ax3.set_ylabel('Quaternian')
    ax3.legend([r'$p0$', r'$p1$', r'$p2$', r'$p3$'])
    ax3.grid(True)

    ax4.set_ylabel('Omega [rad/s]')
    ax4.legend([r'$p$', r'$q$', r'$r$'])
    ax4.set_xlabel('Time [sec]')
    ax4.grid(True)

    plt.tight_layout()

    fig2 = plt.figure()
    fig2.suptitle("[LQR] rotor input")
    ax1 = fig2.add_subplot(4, 1, 1)
    ax2 = fig2.add_subplot(4, 1, 2, sharex=ax1)
    ax3 = fig2.add_subplot(4, 1, 3, sharex=ax1)
    ax4 = fig2.add_subplot(4, 1, 4, sharex=ax1)

    ax1.plot(data['t'], data['rotors'].squeeze()[:, 0])
    ax2.plot(data['t'], data['rotors'].squeeze()[:, 1])
    ax3.plot(data['t'], data['rotors'].squeeze()[:, 2])
    ax4.plot(data['t'], data['rotors'].squeeze()[:, 3])

    ax1.set_ylabel('rotor1')
    ax1.grid(True)
    ax2.set_ylabel('rotor2')
    ax2.grid(True)
    ax3.set_ylabel('rotor3')
    ax3.grid(True)
    ax4.set_ylabel('rotor4')
    ax4.grid(True)
    ax4.set_xlabel('Time [sec]')

    plt.tight_layout()

    plt.show()


if __name__ == "__main__":
    exp1()
    exp1_plot()

对上述代码的具体说明如下所示：

1. Env.__init__(self)：初始化飞行器仿真环境，包括定义故障和代理（飞行器、FDI、LQR控制器）。
2. Env.step(self)：执行仿真环境的时间步进，更新系统状态并返回仿真是否结束。
3. Env.get_ref(self, t, x)：获取参考轨迹，包括位置、速度、四元数和角速度。
4. Env._get_derivs(self, t, x, effectiveness)：计算系统状态导数和控制输入，考虑了故障注入和LQR控制器。
5. Env.set_dot(self, t)：更新系统状态的时间导数，考虑了故障注入和LQR控制器。
6. Env.logger_callback(self, t, **kwargs)：记录仿真过程中的关键信息，包括时间、状态变量、控制输入和参考轨迹。
7. run()：运行仿真环境，记录仿真数据到文件。
8. exp1()：运行仿真实验。
9. exp1_plot()：绘制仿真结果的时域图，包括飞行器状态变量和控制输入。

执行后会绘制两幅时域图，这些图形提供了对仿真实验的详细视觉分析，如图2-8所示。第一个图中显示了无人机的位置，参考轨迹中的x、y、z坐标以及飞行器实际位置。第二幅图显示飞行器的速度，分别是u、v、w分量。此外，还有一幅图显示每个电机的输入（rotor input）。

图2-8  绘制的两幅时域图

未完待续