四旋翼无人机动力学与控制(2)-飞行器控制

四旋翼无人机
发布日期:   2024-11-04

继上一篇博客搭建完无人机的动力学模型之后，进而可以开始设计控制算法，首先需要了解常见的飞控策略：

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

总体框架如上图，本项目采用常见的分级控制，由内到外分为角速率环、姿态环、速度环和位置环。上一篇中笔者写过，我将无人机的运动浅显的理解为依靠四个轴上的电机带动螺旋桨提供升力，通过调节电机转速制造升力差引起角度的偏转进而带动其运动，因此无人机的角度偏转是无人机在xy平面内运动的前提。换句话说，控制好角度是控制好位置的前提，而采用角速率环和速度环是由于被控量（位置、姿态）和控制量（加速度、角加速度）之间差了两个阶次，只使用一个环无法控制中间变量，容易导致超调，因此加入一个中间环来达到更好的控制效果。

本项目中采用了PID、LQR和MPC三种控制算法，以下是控制部分的代码：

（1）PID：原理不做阐述 

  1. # Author：gyx
  2. # Usage: pid controller
  3. # Last change: 2024-7-20
  4.  
  5. import numpy as np
  6. import matplotlib.pyplot as plt
  7.  
  8.  
  9. class PIDController:
  10.     def __init__(self, Kp, Ki, Kd, integrator_max=None, integrator_min=None,max=None,min=None):
  11.         self.Kp = Kp
  12.         self.Ki = Ki
  13.         self.Kd = Kd
  14.         self.integrator = 0
  15.         self.prev_error = 0
  16.         self.integrator_max = integrator_max
  17.         self.integrator_min = integrator_min
  18.         self.max = max
  19.         self.min = min
  20.         self.error = 0
  21.         self.measurement_history = []
  22.         self.setpoint_history = []
  23.  
  24.     def reset(self):
  25.         self.integrator = 0
  26.         self.prev_error = 0
  27.  
  28.     def update(self, setpoint, measurement, dt):
  29.         self.measurement_history.append(measurement)
  30.         self.setpoint_history.append(setpoint)
  31.         error = setpoint - measurement
  32.         self.error = error
  33.         self.integrator += error * dt
  34.         if self.integrator_max is not None and self.integrator > self.integrator_max:
  35.             self.integrator = self.integrator_max
  36.         if self.integrator_min is not None and self.integrator < self.integrator_min:
  37.             self.integrator = self.integrator_min
  38.  
  39.         derivative = (error - self.prev_error) / dt
  40.         output = self.Kp * error + self.Ki * self.integrator + self.Kd * derivative
  41.         self.prev_error = error
  42.         if self.max is not None and self.min is not None:
  43.             output = np.clip(output,self.min,self.max)
  44.         return output
  45.  
  46.     def plot_error(self,fig,i,title='none'):
  47.         # plt.plot(self.setpoint_history, [i for i in self.setpoint_history], linewidth =2.0, label = r"setpoint",color='b', linestyle='dotted')
  48.         # plt.plot(self.measurement_history, [i for i in self.measurement_history], linewidth =2.0, label = r"setpoint",color='r',linestyle='dashed')
  49.         ax = fig.add_subplot(131 + i)
  50.         ax.plot([i/100 for i in range(len(self.setpoint_history))], self.setpoint_history, label='setpoint')
  51.         ax.plot([i/100 for i in range(len(self.measurement_history))], self.measurement_history,label='measurement')
  52.         ax.set_xlabel('time')
  53.         ax.set_ylabel('value')
  54.         ax.set_title(title)
  55.         ax.legend()
  56.  
  57.  
  58. class CascadePIDController:
  59.     def __init__(self, outer_controller, inner_controller):
  60.         self.outer_controller = outer_controller
  61.         self.inner_controller = inner_controller
  62.  
  63.     def update(self, outer_setpoint, outer_measurement, inner_measurement, dt):
  64.         outer_output = self.outer_controller.update(outer_setpoint, outer_measurement, dt)
  65.         inner_output = self.inner_controller.update(outer_output, inner_measurement, dt)
  66.         return inner_output
  67.  
  68.  
  69. class posion_pos_controller:
  70.     def __init__(self,params):
  71.         self.params = params
  72.  
  73.     def position_controller(self):
  74.         x_position_controller = PIDController(self.params.Kpxp, 0, 0, integrator_max=1,
  75.                                               integrator_min=-1, max=5, min=-5)
  76.         x_velocity_controller = PIDController(self.params.Kvxp, self.params.Kvxi, self.params.Kvxd, integrator_max=1,
  77.                                               integrator_min=-1)
  78.         y_position_controller = PIDController(self.params.Kpyp, 0, 0, integrator_max=1,
  79.                                               integrator_min=-1, max=5, min=-5, )
  80.         y_velocity_controller = PIDController(self.params.Kvyp, self.params.Kvyi, self.params.Kvyd, integrator_max=1,
  81.                                               integrator_min=-1)
  82.  
  83.         height_position_controller = PIDController(self.params.Kpzp, 0, 0, integrator_max=1,
  84.                                                    integrator_min=-1, min=-3, max=3)
  85.         height_velocity_controller = PIDController(self.params.Kvzp, self.params.Kvzi, self.params.Kvzd, integrator_max=1,
  86.                                                    integrator_min=-1, min=-0.4, max=0.4)
  87.         x_controller = CascadePIDController(x_position_controller, x_velocity_controller)
  88.         y_controller = CascadePIDController(y_position_controller, y_velocity_controller)
  89.         height_controller = CascadePIDController(height_position_controller, height_velocity_controller)
  90.         return [x_controller,y_controller,height_controller]
  91.  
  92.     def pos_controller(self):
  93.         DEG2RAD = self.params.DEG2RAD
  94.         roll_angle_controller = PIDController(self.params.Kp_RP_ANGLE, 0, 0, integrator_max=1,
  95.                                               integrator_min=-1, max=DEG2RAD * 35, min=-DEG2RAD * 35)
  96.         roll_rate_controller = PIDController(self.params.Kp_RP_AngleRate, self.params.Ki_RP_AngleRate,
  97.                                              self.params.Kd_RP_AngleRate, integrator_max=1,
  98.                                              integrator_min=-1, max=DEG2RAD * 220, min=-DEG2RAD * 220)
  99.         roll_controller = CascadePIDController(roll_angle_controller, roll_rate_controller)
  100.  
  101.         pitch_angle_controller = PIDController(self.params.Kp_RP_ANGLE, 0, 0, integrator_max=1,
  102.                                                integrator_min=-1, max=DEG2RAD * 35, min=-DEG2RAD * 35)
  103.         pitch_rate_controller = PIDController(self.params.Kp_RP_AngleRate, self.params.Ki_RP_AngleRate,
  104.                                               self.params.Kd_RP_AngleRate, integrator_max=1,
  105.                                               integrator_min=-1, max=DEG2RAD * 220, min=-DEG2RAD * 220)
  106.         pitch_controller = CascadePIDController(pitch_angle_controller, pitch_rate_controller)
  107.         yaw_angle_controller = PIDController(self.params.Kp_RP_ANGLE, 0, 0, integrator_max=1,
  108.                                              integrator_min=-1, max=DEG2RAD * 35, min=-DEG2RAD * 35)
  109.         yaw_rate_controller = PIDController(self.params.Kp_YAW_AngleRate, self.params.Ki_YAW_AngleRate,
  110.                                             self.params.Kd_YAW_AngleRate, integrator_max=1,
  111.                                             integrator_min=-1, max=DEG2RAD * 220, min=-DEG2RAD * 220)
  112.         yaw_controller = CascadePIDController(yaw_angle_controller, yaw_rate_controller)
  113.         return [roll_controller,pitch_controller,yaw_controller]

（2）LQR： 

  1. # Author：gyx
  2. # Usage: LQR controller
  3. # Last change: 2024-7-20
  4.  
  5. import numpy as np
  6. import matplotlib.pyplot as plt
  7.  
  8.  
  9. class LQRController:
  10.     def __init__(self, A, B, Q, R):
  11.         self.A = A
  12.         self.B = B
  13.         self.Q = Q
  14.         self.R = R
  15.         self.measurement_history = []
  16.         self.setpoint_history = []
  17.  
  18.     def lqr_solve(self):
  19.         n = self.A.shape[0]
  20.         m = self.B.shape[1]
  21.         # Riccati equation using a Lyapunov approach
  22.         Z = np.vstack([
  23.             np.hstack([self.A, -self.B @ np.linalg.inv(self.R) @ self.B.T]),
  24.             np.hstack([-self.Q, -self.A.T])
  25.         ])
  26.         eigvals, eigvecs = np.linalg.eig(Z)
  27.         # Select stable eigenvalues
  28.         stable_indices = np.where(np.real(eigvals) < 0)[0]
  29.         eigvecs_stable = eigvecs[:, stable_indices]
  30.  
  31.         X = eigvecs_stable[:n, :]
  32.         Y = eigvecs_stable[n:, :]
  33.         # Solve P
  34.         P = np.real(Y @ np.linalg.inv(X))
  35.         # Compute gain
  36.         K = np.linalg.inv(self.R) @ self.B.T @ P
  37.  
  38.         eigVals, _ = np.linalg.eig(self.A - self.B @ K)
  39.  
  40.         return K, P, eigVals
  41.  
  42.     def update(self, setpoint, measurement):
  43.         self.measurement_history.append(measurement)
  44.         self.setpoint_history.append(setpoint)
  45.         K, _, _ = self.lqr_solve()
  46.         u = -K @ (measurement - setpoint)
  47.         return u
  48.  
  49.     def plot_error(self, title='none'):
  50.         self.setpoint_history = np.array(self.setpoint_history)
  51.         self.measurement_history = np.array(self.measurement_history)
  52.         fig = plt.figure(figsize=(14, 4))
  53.         subtitle=['x','y','z']
  54.         for i in range(len(self.setpoint_history[0])):
  55.             ax = fig.add_subplot(131+i)
  56.             ax.plot([i / 100 for i in range(len(self.setpoint_history[:,i]))], self.setpoint_history[:,i], label='setpoint')
  57.             ax.plot([i / 100 for i in range(len(self.measurement_history[:,i]))], self.measurement_history[:,i], label='measurement')
  58.             ax.set_xlabel('time')
  59.             ax.set_ylabel('value')
  60.             ax.set_title(subtitle[i]+title)
  61.             ax.legend()
  62.         plt.tight_layout()
  63.         # fig.legend()
  64.  
  65.  
  66.  
  67. if __name__ == '__main__':
  68.     A = np.array([[0., 1.], [-1., -1.]])
  69.     B = np.array([[0.], [1.]])
  70.     Q = np.array([[1., 0.], [0., 1.]])
  71.     R = np.array([[1.]])
  72.  
  73.     controller = LQRController(A, B, Q, R)
  74.     K, P, eigVals = controller.lqr_solve()
  75.     x = np.array([0, 0])
  76.     r = np.array([1, 0])
  77.     u = controller.update(np.array([1, 0]), np.array([0, 0]))
  78.  
  79.     print("LQR gain matrix K:")
  80.     print(K)
  81.     print("\nSolution to Riccati equation P:")
  82.     print(P)
  83.     print("\nClosed loop system eigenvalues:")
  84.     print(eigVals)
  85.     print("Control value:")
  86.     print(u)

（3）MPC： 

  1. # Author：gyx
  2. # Usage: MPC controller
  3. # Last change: 2024-7-20
  4.  
  5. import numpy as np
  6. import matplotlib.pyplot as plt
  7.  
  8. class MPCController:
  9.     def __init__(self, A, B, C, N, Q, R, u_min, u_max, reg=1e-5):
  10.         self.A = A
  11.         self.B = B
  12.         self.C = C
  13.         self.N = N
  14.         self.Q = Q
  15.         self.R = R
  16.         self.u_min = u_min
  17.         self.u_max = u_max
  18.         self.reg = reg  # 正则化参数
  19.         self.measurement_history = []
  20.         self.setpoint_history = []
  21.  
  22.     def mpc_control(self, x, r):
  23.         x = x.reshape(-1,1)
  24.         r = r.reshape(-1,1)
  25.         n_states = self.A.shape[0]
  26.         n_controls = self.B.shape[1]
  27.         # 构建优化问题
  28.         H = np.zeros((self.N * n_controls, self.N * n_controls))
  29.         F = np.zeros((self.N * n_controls, 1))
  30.         for i in range(self.N):
  31.             H[i * n_controls:(i + 1) * n_controls, i * n_controls:(i + 1) * n_controls] = self.R
  32.             F[i * n_controls:(i + 1) * n_controls, 0] = 0
  33.         G = np.zeros((self.N * n_states, self.N * n_controls))
  34.         E = np.zeros((self.N * n_states, n_states))
  35.         L = np.zeros((self.N * n_states, 1))
  36.         for i in range(self.N):
  37.             for j in range(i + 1):
  38.                 G[i * n_states:(i + 1) * n_states, j * n_controls:(j + 1) * n_controls] \
  39.                     = np.linalg.matrix_power(self.A,i - j) @ self.B
  40.             E[i * n_states:(i + 1) * n_states, :] = np.linalg.matrix_power(self.A, i + 1)
  41.             L[i * n_states:(i + 1) * n_states, :] = np.linalg.matrix_power(self.A, i + 1) @ x - r
  42.  
  43.         Q_bar = np.kron(np.eye(self.N), self.Q)
  44.         H += G.T @ Q_bar @ G
  45.         F += (G.T @ Q_bar @ L).reshape(-1, 1)
  46.  
  47.         # 计算最优控制输入
  48.         u_opt = -np.linalg.inv(H) @ F
  49.  
  50.         # 取第一个控制输入
  51.         u = u_opt[:n_controls]
  52.  
  53.         return u
  54.  
  55.     def update(self, setpoint, measurement):
  56.         self.measurement_history.append(measurement)
  57.         self.setpoint_history.append(setpoint)
  58.         u = self.mpc_control(np.array(measurement), np.array(setpoint))
  59.         # print(u)
  60.         return u
  61.  
  62.     def plot_error(self, title='none'):
  63.         self.setpoint_history = np.array(self.setpoint_history)
  64.         self.measurement_history = np.array(self.measurement_history)
  65.         subtitle = ['x', 'y', 'z']
  66.         fig = plt.figure(figsize=(14, 4))
  67.         for i in range(len(self.setpoint_history[0])):
  68.             ax = fig.add_subplot(131+i)
  69.             ax.plot([i / 100 for i in range(len(self.setpoint_history[:, i]))], self.setpoint_history[:, i],
  70.                     label='setpoint')
  71.             ax.plot([i / 100 for i in range(len(self.measurement_history[:, i]))], self.measurement_history[:, i],
  72.                     label='measurement')
  73.             ax.set_xlabel('time')
  74.             ax.set_ylabel('value')
  75.             ax.set_title(subtitle[i]+title)
  76.             ax.legend()
  77.         plt.tight_layout()
  78.  
  79. if __name__ == '__main__':
  80.     A = np.array([[1, 0,0], [0, 1,0],[0,0,1]])
  81.     B = np.array([[0.5,0.6,0.2], [1,0.1,0.4],[0.5,0.5,0.8]])
  82.     C = np.eye(3)  # 输出矩阵
  83.     Q = np.eye(3)*2
  84.     R = np.eye(1)
  85.     N = 10
  86.     u_min = -1
  87.     u_max = 1
  88.     reg = 1e-5
  89.  
  90.     outer_controller = MPCController(A, B, C, N, Q, R, u_min, u_max, reg)
  91.     inner_controller = MPCController(A, B, C, N, Q, R, u_min, u_max, reg)
  92.     setpoint = np.array([[1], [1],[1]])
  93.     # r = np.ones((1, N))  # 目标为1
  94.     measurement = np.array([[0],[0],[0]])
  95.     x = np.array([[0], [0],[0]])
  96.     x_dot = np.array([[0], [0], [0]])
  97.     for _ in range(10):
  98.         u = outer_controller.update(setpoint, x)
  99.         u_v = inner_controller.update(u,x_dot)
  100.         x_dot = A @ x_dot + B @ u_v.reshape(-1, 1)
  101.         x = A @ x + B @ x_dot.reshape(-1, 1)
  102.         # x = A @ x + B @ u.reshape(-1, 1)
  103.         y = C @ x
  104.         print("控制输入 u:")
  105.         print(u)
  106.         print('状态 x:')
  107.         print(x)
  108.     inner_controller.plot_error()
  109.     outer_controller.plot_error()
  110.     plt.show()

仿真运行代码： 

  1. import numpy as np
  2. import matplotlib.pyplot as plt
  3. from init_control import UAVParameters
  4. from uav_dynamics import UAVDynamics
  5. from common_function import rotation_matrix, angle_body2world
  6. from PIDController import posion_pos_controller
  7. from LQRController import LQRController
  8. from MPCController import MPCController
  9. from square_signal import generate_ramp_signal
  10.  
  11. class UAVSimulation:
  12.     def __init__(self, params, dynamics, position_controller, pose_controller, controller_type):
  13.         self.params = params
  14.         self.dynamics = dynamics
  15.         self.controller_type = controller_type
  16.         if controller_type == "PID":
  17.             self.x_controller = position_controller[0]
  18.             self.y_controller = position_controller[1]
  19.             self.height_controller = position_controller[2]
  20.         else:
  21.             self.position_controller = position_controller[0]
  22.             self.velocity_controller = position_controller[1]
  23.         self.roll_controller = pose_controller[0]
  24.         self.pitch_controller = pose_controller[1]
  25.         self.yaw_controller = pose_controller[2]
  26.         self.position_history = []
  27.         self.setpoint_history = []
  28.  
  29.     def desired_euler_angles(self, eax, eay, psi):
  30.         """
  31.         Convert the desired horizontal acceleration to the desired Euler angle
  32.         Input:
  33.           eax, eay: desired horizontal acceleration
  34.         Output:
  35.           phi, theta: desired roll and pitch
  36.         """
  37.         g = 9.8  # 重力加速度 (m/s^2)
  38.  
  39.         phi = (-np.sin(psi) * eax + np.cos(psi) * eay) / g
  40.         theta = (-np.cos(psi) * eax - np.sin(psi) * eay) / g
  41.  
  42.         return phi, theta
  43.  
  44.     def motor_mixer(self, roll, pitch, yaw, thrust):
  45.         """
  46.         Control allocation. The quadrotor type is X-configuration,
  47.         and the airframe is as follows:
  48.         3↓   1↑
  49.           \ /
  50.           / \
  51.         2↑   4↓
  53.         Input:
  54.           roll, pitch, yaw: attitude controller output.
  55.           thrust: total thrust.
  57.         Output:
  58.           M1, M2, M3, M4: motor speed commands.
  59.         """
  60.         idle_PWM = 1000
  61.         scale = 1000
  62.  
  63.         M1 = (thrust - roll + pitch + yaw) * scale + idle_PWM
  64.         M2 = (thrust + roll - pitch + yaw) * scale + idle_PWM
  65.         M3 = (thrust + roll + pitch - yaw) * scale + idle_PWM
  66.         M4 = (thrust - roll - pitch - yaw) * scale + idle_PWM
  67.  
  68.         return M1, M2, M3, M4
  69.  
  70.     def pwm_to_rpm(self, pwm):
  71.         """
  72.         Convert PWM signal to RPM. This is a simple linear approximation.
  73.         Input:
  74.           pwm: PWM signal value
  76.         Output:
  77.           rpm: motor speed in RPM
  78.         """
  79.         min_pwm = 1000
  80.         max_pwm = 2000
  81.  
  82.         # 限制PWM范围
  83.         pwm = np.clip(pwm, min_pwm, max_pwm)
  84.  
  85.         # 线性插值计算RPM
  86.         rpm = np.clip((pwm - min_pwm) / (max_pwm - min_pwm), -1, 1)
  87.         return rpm
  88.  
  89.     def run(self, square_corners, duration, dt):
  90.         DEG2RAD = self.params.DEG2RAD
  91.         posE = self.params.ModelInit_PosE.astype(np.float64)
  92.         velB = self.params.ModelInit_VelB.astype(np.float64)
  93.         velE = self.params.ModelInit_VelB.astype(np.float64)
  94.         angEuler = self.params.ModelInit_AngEuler
  95.         rateB = self.params.ModelInit_RateB
  96.         rateE = self.params.ModelInit_RateB
  97.         ModelParam_motorCr = self.params.ModelParam_motorCr
  98.         ModelParam_motorWb = self.params.ModelParam_motorWb
  99.  
  100.         state = [velB, angEuler, rateB]
  101.         current_corner_index = 0
  102.         time_steps = int(duration / dt)
  103.         num_steps = time_steps // (len(square_corners) - 1)
  104.         # next_corner_time = time_steps  // (len(square_corners)-1)
  105.         input_signal = generate_ramp_signal(square_corners, num_steps)
  106.  
  107.         for step in range(time_steps):
  108.             setpoint = input_signal[step]
  109.             eax, eay, altitude_output = None, None, None
  110.             print('setpoint:', setpoint)
  111.             if self.controller_type == 'PID':
  112.                 eax = self.x_controller.update(outer_setpoint=setpoint[0], outer_measurement=posE[0],
  113.                                                inner_measurement=velE[0], dt=dt)
  114.                 eay = self.y_controller.update(outer_setpoint=setpoint[1], outer_measurement=posE[1],
  115.                                                inner_measurement=velE[1], dt=dt)
  116.                 altitude_output = self.height_controller.update(outer_setpoint=setpoint[2], outer_measurement=posE[2],
  117.                                                                 inner_measurement=velE[2], dt=dt)
  118.             else:
  119.                 vel_setpoint = self.position_controller.update(setpoint, posE)
  120.                 # 速度环LQR控制
  121.                 acc_setpoint = self.velocity_controller.update(vel_setpoint, velE)
  122.                 if self.controller_type == "MPC":
  123.                     eax, eay, altitude_output = acc_setpoint[0][0], acc_setpoint[1][0], acc_setpoint[2][0]
  124.                 elif self.controller_type == "LQR":
  125.                     eax, eay, altitude_output = acc_setpoint[0], acc_setpoint[1], acc_setpoint[2]
  126.                 else:
  127.                     print("unknown controller")
  128.             phi, theta = self.desired_euler_angles(eax, eay, angEuler[2])
  129.             phi = np.clip(phi, -35 * DEG2RAD, 35 * DEG2RAD)
  130.             theta = np.clip(theta, -35 * DEG2RAD, 35 * DEG2RAD)
  131.             print('eax,eay:', eax, eay)
  132.             print('phi,theta:', phi, theta)
  133.             roll_output = np.clip(self.roll_controller.update(outer_setpoint=phi, outer_measurement=angEuler[0],
  134.                                                               inner_measurement=rateE[0], dt=dt), -1, 1)
  135.             pitch_output = np.clip(self.pitch_controller.update(outer_setpoint=theta, outer_measurement=angEuler[1],
  136.                                                                 inner_measurement=rateE[1], dt=dt), -1, 1)
  137.             yaw_output = np.clip(self.yaw_controller.update(outer_setpoint=(step // num_steps) * 0.5 * 0.0174533,
  138.                                                             outer_measurement=angEuler[2],
  139.                                                             inner_measurement=rateE[2], dt=dt), -1, 1)
  140.             thrust = np.clip(-altitude_output + self.params.THR_HOVER, 0.05, 0.9)
  141.             print('roll_output,pitch_output,yaw_output,altitude_output:', roll_output, pitch_output, yaw_output,
  142.                   altitude_output)
  143.             M1, M2, M3, M4 = self.motor_mixer(roll_output, pitch_output, yaw_output, thrust)
  144.             print('M1,M2,M3,M4:', M1, M2, M3, M4)
  145.  
  146.             w_target = (np.array(
  147.                 [self.pwm_to_rpm(M1), self.pwm_to_rpm(M2), self.pwm_to_rpm(M3),
  148.                  self.pwm_to_rpm(M4)]) * ModelParam_motorCr +
  149.                         np.array([ModelParam_motorWb, ModelParam_motorWb, ModelParam_motorWb, ModelParam_motorWb]))
  150.             print('w_target:', w_target)
  151.             state = self.dynamics.update_dynamics(state, w_target, dt)
  152.             velB, angEuler, rateB = state
  153.             # R = rotation_matrix(angEuler)
  154.             R_angle = angle_body2world(angEuler)
  155.             R = rotation_matrix(angEuler)
  156.             rateE = R_angle @ rateB
  157.             angEuler = angEuler + rateE * dt
  158.             velE = R @ velB
  159.             posE = posE + velE * dt
  160.  
  161.             state = velB, angEuler, rateB
  162.             self.position_history.append(posE.copy())
  163.             print(
  164.                 f"Time: {step * dt:.2f}s, Position: {posE}, Velocity_world: {velE}, Angle: {angEuler}, Rate: {rateE}")
  165.             # if step % (next_corner_time-1) == 0 and step != 0:
  166.             #     current_corner_index = (current_corner_index + 1) % len(square_corners)
  167.             self.setpoint_history.append(setpoint.copy())
  168.  
  169.     def plot_trajectory(self):
  170.         position_history = np.array(self.position_history)
  171.         setpoint_history = np.array(self.setpoint_history)
  172.         fig = plt.figure()
  173.         ax = fig.add_subplot(121, projection='3d')
  174.         ax.plot(position_history[:, 0], position_history[:, 1], -position_history[:, 2], label='UAV Trajectory')
  175.         ax.set_xlabel('X Position (m)')
  176.         ax.set_ylabel('Y Position (m)')
  177.         ax.set_zlabel('Z Position (m)')
  178.         ax.set_title('Trajectory of UAV ')
  179.         ax.legend()
  180.  
  181.         ax = fig.add_subplot(122, projection='3d')
  182.         ax.plot(setpoint_history[:, 0], setpoint_history[:, 1], -setpoint_history[:, 2], label='setpoint Trajectory')
  183.         ax.set_xlabel('X Position (m)')
  184.         ax.set_ylabel('Y Position (m)')
  185.         ax.set_zlabel('Z Position (m)')
  186.         ax.set_title(' Desired Trajectory ')
  187.         ax.legend()
  188.  
  189.     def plot_state_error(self):
  190.         if self.controller_type == 'PID':
  191.             fig_position = plt.figure(figsize=(14, 4))
  192.             self.x_controller.outer_controller.plot_error(fig_position, 0, title='x_position')
  193.             self.y_controller.outer_controller.plot_error(fig_position, 1, title='y_position')
  194.             self.height_controller.outer_controller.plot_error(fig_position, 2, title='height_position')
  195.             fig_velocity = plt.figure(figsize=(14, 4))
  196.             self.x_controller.inner_controller.plot_error(fig_velocity, 0, title='x_velocity')
  197.             self.y_controller.inner_controller.plot_error(fig_velocity, 1, title='y_velocity')
  198.             self.height_controller.inner_controller.plot_error(fig_velocity, 2, title='height_velocity')
  199.         else:
  200.             self.position_controller.plot_error('_position')
  201.             self.velocity_controller.plot_error('_velocity')
  202.  
  203.         fig_angle = plt.figure(figsize=(14, 4))
  204.  
  205.         self.roll_controller.outer_controller.plot_error(fig_angle, 0, title='roll_angle')
  206.         self.pitch_controller.outer_controller.plot_error(fig_angle, 1, title='pitch_angle')
  207.         self.yaw_controller.outer_controller.plot_error(fig_angle, 2, title='yaw_angle')
  208.         plt.tight_layout()
  209.         fig_rate = plt.figure(figsize=(14, 4))
  210.  
  211.         self.roll_controller.inner_controller.plot_error(fig_rate, 0, title='roll_rate')
  212.         self.pitch_controller.inner_controller.plot_error(fig_rate, 1, title='pitch_rate')
  213.         self.yaw_controller.inner_controller.plot_error(fig_rate, 2, title='yaw_rate')
  214.         plt.tight_layout()
  215.         # plt.show()
  216.  
  217.  
  218. if __name__ == '__main__':
  219.     params = UAVParameters()
  220.     dynamics = UAVDynamics(params)
  221.     # controller = CascadePIDController(PIDController(3.0, 0.0, 0.15), PIDController(0.15, 0.0, 0.03))
  222.     # controller_type = "PID"
  223.     # controller_type = "MPC"
  224.     controller_type = "LQR"
  225.     position_controller = None
  226.     if controller_type == 'PID':
  227.         position_controller = posion_pos_controller(params).position_controller()
  228.     elif controller_type == 'LQR':
  229.         # 定义位置环的状态空间模型
  230.         A_pos = np.zeros((3, 3))
  231.         B_pos = np.eye(3)
  232.  
  233.         Q_pos = np.eye(3) * 1.65
  234.         R_pos = np.eye(3) * 4.0
  235.  
  236.         # 定义速度环的状态空间模型
  237.         A_vel = np.zeros((3, 3))
  238.         B_vel = np.eye(3)
  239.  
  240.         Q_vel = np.eye(3) * 10.0
  241.         R_vel = np.eye(3) * 2.4
  242.         pos_controller = LQRController(A_pos, B_pos, Q_pos, R_pos)
  243.         vel_controller = LQRController(A_vel, B_vel, Q_vel, R_vel)
  244.         position_controller = [pos_controller, vel_controller]
  245.     elif controller_type == 'MPC':
  246.         # 定义离散位置环的状态空间模型
  247.         A_pos = np.eye(3)
  248.         B_pos = np.eye(3) * 0.01
  249.  
  250.         Q_pos = np.eye(3) * 3.95
  251.         R_pos = np.eye(3) * 0.6
  252.  
  253.         # 定义离散速度环的状态空间模型
  254.         # A_vel = np.zeros((3, 3))
  255.         A_vel = np.eye(3)
  256.         B_vel = np.eye(3) * 0.01
  257.  
  258.         Q_vel = np.eye(3) * 12.0
  259.         R_vel = np.eye(3) * 0.6
  260.         pos_controller = MPCController(A_pos, B_pos, None, 10, Q_pos, R_pos, -3, 3)
  261.         vel_controller = MPCController(A_vel, B_vel, None, 10, Q_vel, R_vel, -1, 1)
  262.         position_controller = [pos_controller, vel_controller]
  263.     else:
  264.         print('ERROR:Unknown Controller')
  265.  
  266.     pose_controller = posion_pos_controller(params).pos_controller()
  267.     target_altitude = -5  # 目标高度
  268.     square_corners = [
  269.         [0, 0, 0],
  270.         [0, 0, target_altitude],
  271.         [5, 0, target_altitude],
  272.         [5, 5, target_altitude],
  273.         [0, 5, target_altitude],
  274.         [0, 0, target_altitude],
  275.         [0, 0, 0],
  276.         [0, 0, 0]
  277.     ]
  278.     simulation = UAVSimulation(params, dynamics, position_controller=position_controller,
  279.                                pose_controller=pose_controller, controller_type=controller_type)
  280.     simulation.run(square_corners, 35, 0.01)
  281.     simulation.plot_trajectory()
  282.     simulation.plot_state_error()
  283.     plt.show()

三种控制算法效果如下：

可能由于任务较为简单，三种算法差异不大，同时LQR和MPC计算效率较低。 

            </div><div data-report-view="{&quot;mod&quot;:&quot;1585297308_001&quot;,&quot;spm&quot;:&quot;1001.2101.3001.6548&quot;,&quot;dest&quot;:&quot;https://blog.csdn.net/perfectdisaster/article/details/143432386&quot;,&quot;extend1&quot;:&quot;pc&quot;,&quot;ab&quot;:&quot;new&quot;}"><div></div></div>
    </div>
文章作者: 郭昱鑫
文章链接: http://ggggggggx.work.gd/2024/11/04/quad02/
版权声明: 本博客所有文章除特別声明外，均采用 CC BY 4.0 许可协议。转载请注明来源 郭昱鑫 !
四旋翼无人机
赏
  目录