四旋翼无人机动力学与控制(1)-动力学模型

笔者近来开始接触四旋翼无人机，学习过程中写了一份有关无人机动力学与控制的python代码，在此做一些记录和原理解释。

附上仓库地址：四旋翼无人机仿真建模: 本仓库实现了四旋翼无人机的动力学仿真建模，并在此基础上完成了控制器的设计，包括串级PID、LQR、MPC等，实现了位置跟踪和路径规划。

总体框架与思路如图所示，四旋翼无人机的运动模型可以简单的看作是十二个状态变量(位置、速度、角度、角速度各三个变量）随运动而发生改变的过程，运动学方程如下图右半部分所示。

从框架讲起，作为一名飞行器控制专业的学生，四旋翼无人机在我眼中并不算是严格意义上的飞行器，我对其的理解更偏向于robotic，与固定翼相比，四旋翼的动力学模型更简单，我浅显的理解为依靠四个轴上的电机带动螺旋桨提供升力，通过调节电机转速制造升力差引起角度的偏转进而带动无人机运动，因此就引出了四旋翼无人机常见的分级控制策略，分为角速度、姿态、速度和位置由内到外的四个环，详细内容将在下一篇文章展开。

笔者构建的动力学模型分为上图的几个部分，无人机构型为x型，定义电机顺序如上图左上所示，电机作为执行器，近似看作一个一阶延迟环节，控制分配器由升力和力矩在四轴上的分解和合成计算得出。

选取四阶龙格库塔法进行状态更新，由此可以开始编写程序了：

common_function：

# Author：gyx
# Usage: common function
# Last change: 2024-7-20

import numpy as np


def dcm_from_euler(euler_angles):
    """
    计算方向余弦矩阵（DCM）(Earth2Body）
    :param roll: 滚转角，单位为弧度
    :param pitch: 俯仰角，单位为弧度
    :param yaw: 偏航角，单位为弧度
    :return: 方向余弦矩阵（DCM）
    """
    roll, pitch, yaw = euler_angles
    c_roll = np.cos(roll)
    s_roll = np.sin(roll)
    c_pitch = np.cos(pitch)
    s_pitch = np.sin(pitch)
    c_yaw = np.cos(yaw)
    s_yaw = np.sin(yaw)

    # 构建方向余弦矩阵
    dcm = np.array([
        [c_yaw * c_pitch, c_yaw * s_pitch * s_roll - s_yaw * c_roll, c_yaw * s_pitch * c_roll + s_yaw * s_roll],
        [s_yaw * c_pitch, s_yaw * s_pitch * s_roll + c_yaw * c_roll, s_yaw * s_pitch * c_roll - c_yaw * s_roll],
        [-s_pitch, c_pitch * s_roll, c_pitch * c_roll]
    ])
    return dcm


def rotation_matrix(euler_angles):  # （Body2Earth)
    phi, theta, psi = euler_angles
    R_x = np.array([[1, 0, 0],
                    [0, np.cos(phi), -np.sin(phi)],
                    [0, np.sin(phi), np.cos(phi)]])

    R_y = np.array([[np.cos(theta), 0, np.sin(theta)],
                    [0, 1, 0],
                    [-np.sin(theta), 0, np.cos(theta)]])

    R_z = np.array([[np.cos(psi), -np.sin(psi), 0],
                    [np.sin(psi), np.cos(psi), 0],
                    [0, 0, 1]])

    return R_z @ R_y @ R_x


def angle_body2world(euler_angles):
    phi, theta, psi = euler_angles
    Trans_matrix = np.array([[1, np.tan(theta) * np.sin(phi), np.tan(theta) * np.cos(phi)],
                             [0, np.cos(phi), -np.sin(phi)],
                             [0, np.sin(phi) / np.cos(theta), np.cos(phi) / np.cos(theta)]])
    return Trans_matrix


def rk4_step(f, state, u, t, dt):
    k1 = f(state, u, t)
    print([x * 0.5 * dt for x in k1])
    k2 = f(state + [x * 0.5 * dt for x in k1], u, t + 0.5 * dt)
    k3 = f(state + [x * 0.5 * dt for x in k2], u, t + 0.5 * dt)
    k4 = f(state + [x  * dt for x in k3], u, t + dt)

    return state + (dt / 6) * (k1 + 2 * k2 + 2 * k3 + k4)

dynamic：

# Author：gyx
# Usage: build uav dynamics model
# Last change: 2024-7-20

import numpy as np
from common_function import dcm_from_euler, rotation_matrix


class UAVDynamics:
    def __init__(self, params):
        self.params = params
        self.w_current = params.ModelInit_Rads

    def get_angular_velocity_derivative(self, M, J, omega):
        return np.linalg.inv(J) @ (M - np.cross(omega, J @ omega))

    def get_linear_velocity_derivative(self, F, m):
        return np.clip(F / m, -0.4, 0.4)

    def Obtain_force_torque(self, w, R, Cm, Ct, Vb, Cd, wb, Cdm, Jrp):
        M_rctcm = np.array([
            [-np.sin(np.pi / 4) * R * Ct, np.sin(np.pi / 4) * R * Ct, np.sin(np.pi / 4) * R * Ct,
             -np.sin(np.pi / 4) * R * Ct],  # Roll torque
            [np.sin(np.pi / 4) * R * Ct, -np.sin(np.pi / 4) * R * Ct, np.sin(np.pi / 4) * R * Ct,
             -np.sin(np.pi / 4) * R * Ct],  # Pitch torque
            [Cm, Cm, -Cm, -Cm]  # Yaw torque
        ])

        Mp = np.dot(M_rctcm, w ** 2)  # Torque

        Fp = np.array([0, 0, -np.sum(Ct * (w ** 2))])  # Thrust
        Fd = -Cd * Vb * np.abs(Vb) * 0.5  # Aerodynamic force
        Md = -Cdm * wb * np.abs(wb)  # Aerodynamic moment

        # Gyro moment
        Ga = np.zeros(3)
        Ga[0] = Jrp * wb[1] * (w[0] + w[1] - w[2] - w[3])
        Ga[1] = Jrp * wb[0] * (-w[0] - w[1] + w[2] + w[3])

        return Fp, Fd, Mp, Md, Ga

    def motor_dynamics(self, w_current, w_target, T, dt):
        dw = (w_target - w_current) / T
        return w_current + dw * dt

    def state_derivative(self, state, w_target,dt):
        velB, angEuler, rateB = state

        m = self.params.ModelParam_uavMass
        J = self.params.ModelParam_uavJ
        g = self.params.ModelParam_envGravityAcc
        uavType = self.params.ModelParam_uavType
        R = self.params.ModelParam_uavR
        Cm = self.params.ModelParam_rotorCm
        Ct = self.params.ModelParam_rotorCt
        Cd = self.params.ModelParam_uavCd
        Cdm = self.params.ModelParam_uavCCm
        Jrp = self.params.ModelParam_motorJm
        T = self.params.ModelParam_motorT

        self.w_current = self.motor_dynamics(self.w_current, w_target, T, dt)
        Fp, Fd, Mp, Md, Ga = self.Obtain_force_torque(self.w_current, R, Cm, Ct, velB, Cd, rateB, Cdm, Jrp)
        dcm = dcm_from_euler(angEuler)
        total_force = Fp + Fd + np.array([0, 0, m * g]).dot(dcm)
        total_torque = Mp + Md + Ga
        velB_dot = self.get_linear_velocity_derivative(total_force, m)
        rateB_dot = self.get_angular_velocity_derivative(total_torque, J, rateB)

        return velB_dot, rateB_dot

    def update_dynamics(self, state, w_target, dt):
        velB, angEuler, rateB = state

        # Runge-Kutta 4th Order Method
        k1_velB_dot, k1_rateB_dot = self.state_derivative(state, w_target,dt)
        k2_velB_dot, k2_rateB_dot = self.state_derivative(
            (velB + 0.5 * k1_velB_dot * dt, angEuler, rateB + 0.5 * k1_rateB_dot * dt), w_target,dt)
        k3_velB_dot, k3_rateB_dot = self.state_derivative(
            (velB + 0.5 * k2_velB_dot * dt, angEuler, rateB + 0.5 * k2_rateB_dot * dt), w_target,dt)
        k4_velB_dot, k4_rateB_dot = self.state_derivative(
            (velB + k3_velB_dot * dt, angEuler, rateB + k3_rateB_dot * dt), w_target,dt)

        velB = velB + (k1_velB_dot + 2 * k2_velB_dot + 2 * k3_velB_dot + k4_velB_dot) * dt / 6
        rateB = rateB + (k1_rateB_dot + 2 * k2_rateB_dot + 2 * k3_rateB_dot + k4_rateB_dot) * dt / 6

        return velB, angEuler, rateB

Last,放上笔者的个人主页：郭昱鑫的个人主页