笔者近来开始接触四旋翼无人机，学习过程中写了一份有关无人机动力学与控制的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
</div><div data-report-view="{"mod":"1585297308_001","spm":"1001.2101.3001.6548","dest":"https://blog.csdn.net/perfectdisaster/article/details/143431038","extend1":"pc","ab":"new"}"><div></div></div>
</div>
