继上一篇博客搭建完无人机的动力学模型之后，进而可以开始设计控制算法，首先需要了解常见的飞控策略：
还是先附上仓库地址：四旋翼无人机仿真建模: 本仓库实现了四旋翼无人机的动力学仿真建模，并在此基础上完成了控制器的设计，包括串级PID、LQR、MPC等，实现了位置跟踪和路径规划。
总体框架如上图，本项目采用常见的分级控制，由内到外分为角速率环、姿态环、速度环和位置环。上一篇中笔者写过，我将无人机的运动浅显的理解为依靠四个轴上的电机带动螺旋桨提供升力，通过调节电机转速制造升力差引起角度的偏转进而带动其运动，因此无人机的角度偏转是无人机在xy平面内运动的前提。换句话说，控制好角度是控制好位置的前提，而采用角速率环和速度环是由于被控量（位置、姿态）和控制量（加速度、角加速度）之间差了两个阶次，只使用一个环无法控制中间变量，容易导致超调，因此加入一个中间环来达到更好的控制效果。
本项目中采用了PID、LQR和MPC三种控制算法，以下是控制部分的代码：
（1）PID：原理不做阐述
# Author：gyx
# Usage: pid controller
# Last change: 2024-7-20
import numpy as np
import matplotlib.pyplot as plt
class PIDController:
def __init__(self, Kp, Ki, Kd, integrator_max=None, integrator_min=None,max=None,min=None):
self.Kp = Kp
self.Ki = Ki
self.Kd = Kd
self.integrator = 0
self.prev_error = 0
self.integrator_max = integrator_max
self.integrator_min = integrator_min
self.max = max
self.min = min
self.error = 0
self.measurement_history = []
self.setpoint_history = []
def reset(self):
self.integrator = 0
self.prev_error = 0
def update(self, setpoint, measurement, dt):
self.measurement_history.append(measurement)
self.setpoint_history.append(setpoint)
error = setpoint - measurement
self.error = error
self.integrator += error * dt
if self.integrator_max is not None and self.integrator > self.integrator_max:
self.integrator = self.integrator_max
if self.integrator_min is not None and self.integrator < self.integrator_min:
self.integrator = self.integrator_min
derivative = (error - self.prev_error) / dt
output = self.Kp * error + self.Ki * self.integrator + self.Kd * derivative
self.prev_error = error
if self.max is not None and self.min is not None:
output = np.clip(output,self.min,self.max)
return output
def plot_error(self,fig,i,title='none'):
# plt.plot(self.setpoint_history, [i for i in self.setpoint_history], linewidth =2.0, label = r"setpoint",color='b', linestyle='dotted')
# plt.plot(self.measurement_history, [i for i in self.measurement_history], linewidth =2.0, label = r"setpoint",color='r',linestyle='dashed')
ax = fig.add_subplot(131 + i)
ax.plot([i/100 for i in range(len(self.setpoint_history))], self.setpoint_history, label='setpoint')
ax.plot([i/100 for i in range(len(self.measurement_history))], self.measurement_history,label='measurement')
ax.set_xlabel('time')
ax.set_ylabel('value')
ax.set_title(title)
ax.legend()
class CascadePIDController:
def __init__(self, outer_controller, inner_controller):
self.outer_controller = outer_controller
self.inner_controller = inner_controller
def update(self, outer_setpoint, outer_measurement, inner_measurement, dt):
outer_output = self.outer_controller.update(outer_setpoint, outer_measurement, dt)
inner_output = self.inner_controller.update(outer_output, inner_measurement, dt)
return inner_output
class posion_pos_controller:
def __init__(self,params):
self.params = params
def position_controller(self):
x_position_controller = PIDController(self.params.Kpxp, 0, 0, integrator_max=1,
integrator_min=-1, max=5, min=-5)
x_velocity_controller = PIDController(self.params.Kvxp, self.params.Kvxi, self.params.Kvxd, integrator_max=1,
integrator_min=-1)
y_position_controller = PIDController(self.params.Kpyp, 0, 0, integrator_max=1,
integrator_min=-1, max=5, min=-5, )
y_velocity_controller = PIDController(self.params.Kvyp, self.params.Kvyi, self.params.Kvyd, integrator_max=1,
integrator_min=-1)
height_position_controller = PIDController(self.params.Kpzp, 0, 0, integrator_max=1,
integrator_min=-1, min=-3, max=3)
height_velocity_controller = PIDController(self.params.Kvzp, self.params.Kvzi, self.params.Kvzd, integrator_max=1,
integrator_min=-1, min=-0.4, max=0.4)
x_controller = CascadePIDController(x_position_controller, x_velocity_controller)
y_controller = CascadePIDController(y_position_controller, y_velocity_controller)
height_controller = CascadePIDController(height_position_controller, height_velocity_controller)
return [x_controller,y_controller,height_controller]
def pos_controller(self):
DEG2RAD = self.params.DEG2RAD
roll_angle_controller = PIDController(self.params.Kp_RP_ANGLE, 0, 0, integrator_max=1,
integrator_min=-1, max=DEG2RAD * 35, min=-DEG2RAD * 35)
roll_rate_controller = PIDController(self.params.Kp_RP_AngleRate, self.params.Ki_RP_AngleRate,
self.params.Kd_RP_AngleRate, integrator_max=1,
integrator_min=-1, max=DEG2RAD * 220, min=-DEG2RAD * 220)
roll_controller = CascadePIDController(roll_angle_controller, roll_rate_controller)
pitch_angle_controller = PIDController(self.params.Kp_RP_ANGLE, 0, 0, integrator_max=1,
integrator_min=-1, max=DEG2RAD * 35, min=-DEG2RAD * 35)
pitch_rate_controller = PIDController(self.params.Kp_RP_AngleRate, self.params.Ki_RP_AngleRate,
self.params.Kd_RP_AngleRate, integrator_max=1,
integrator_min=-1, max=DEG2RAD * 220, min=-DEG2RAD * 220)
pitch_controller = CascadePIDController(pitch_angle_controller, pitch_rate_controller)
yaw_angle_controller = PIDController(self.params.Kp_RP_ANGLE, 0, 0, integrator_max=1,
integrator_min=-1, max=DEG2RAD * 35, min=-DEG2RAD * 35)
yaw_rate_controller = PIDController(self.params.Kp_YAW_AngleRate, self.params.Ki_YAW_AngleRate,
self.params.Kd_YAW_AngleRate, integrator_max=1,
integrator_min=-1, max=DEG2RAD * 220, min=-DEG2RAD * 220)
yaw_controller = CascadePIDController(yaw_angle_controller, yaw_rate_controller)
return [roll_controller,pitch_controller,yaw_controller]
（2）LQR：
# Author：gyx
# Usage: LQR controller
# Last change: 2024-7-20
import numpy as np
import matplotlib.pyplot as plt
class LQRController:
def __init__(self, A, B, Q, R):
self.A = A
self.B = B
self.Q = Q
self.R = R
self.measurement_history = []
self.setpoint_history = []
def lqr_solve(self):
n = self.A.shape[0]
m = self.B.shape[1]
# Riccati equation using a Lyapunov approach
Z = np.vstack([
np.hstack([self.A, -self.B @ np.linalg.inv(self.R) @ self.B.T]),
np.hstack([-self.Q, -self.A.T])
])
eigvals, eigvecs = np.linalg.eig(Z)
# Select stable eigenvalues
stable_indices = np.where(np.real(eigvals) < 0)[0]
eigvecs_stable = eigvecs[:, stable_indices]
X = eigvecs_stable[:n, :]
Y = eigvecs_stable[n:, :]
# Solve P
P = np.real(Y @ np.linalg.inv(X))
# Compute gain
K = np.linalg.inv(self.R) @ self.B.T @ P
eigVals, _ = np.linalg.eig(self.A - self.B @ K)
return K, P, eigVals
def update(self, setpoint, measurement):
self.measurement_history.append(measurement)
self.setpoint_history.append(setpoint)
K, _, _ = self.lqr_solve()
u = -K @ (measurement - setpoint)
return u
def plot_error(self, title='none'):
self.setpoint_history = np.array(self.setpoint_history)
self.measurement_history = np.array(self.measurement_history)
fig = plt.figure(figsize=(14, 4))
subtitle=['x','y','z']
for i in range(len(self.setpoint_history[0])):
ax = fig.add_subplot(131+i)
ax.plot([i / 100 for i in range(len(self.setpoint_history[:,i]))], self.setpoint_history[:,i], label='setpoint')
ax.plot([i / 100 for i in range(len(self.measurement_history[:,i]))], self.measurement_history[:,i], label='measurement')
ax.set_xlabel('time')
ax.set_ylabel('value')
ax.set_title(subtitle[i]+title)
ax.legend()
plt.tight_layout()
# fig.legend()
if __name__ == '__main__':
A = np.array([[0., 1.], [-1., -1.]])
B = np.array([[0.], [1.]])
Q = np.array([[1., 0.], [0., 1.]])
R = np.array([[1.]])
controller = LQRController(A, B, Q, R)
K, P, eigVals = controller.lqr_solve()
x = np.array([0, 0])
r = np.array([1, 0])
u = controller.update(np.array([1, 0]), np.array([0, 0]))
print("LQR gain matrix K:")
print(K)
print("\nSolution to Riccati equation P:")
print(P)
print("\nClosed loop system eigenvalues:")
print(eigVals)
print("Control value:")
print(u)
（3）MPC：
# Author：gyx
# Usage: MPC controller
# Last change: 2024-7-20
import numpy as np
import matplotlib.pyplot as plt
class MPCController:
def __init__(self, A, B, C, N, Q, R, u_min, u_max, reg=1e-5):
self.A = A
self.B = B
self.C = C
self.N = N
self.Q = Q
self.R = R
self.u_min = u_min
self.u_max = u_max
self.reg = reg # 正则化参数
self.measurement_history = []
self.setpoint_history = []
def mpc_control(self, x, r):
x = x.reshape(-1,1)
r = r.reshape(-1,1)
n_states = self.A.shape[0]
n_controls = self.B.shape[1]
# 构建优化问题
H = np.zeros((self.N * n_controls, self.N * n_controls))
F = np.zeros((self.N * n_controls, 1))
for i in range(self.N):
H[i * n_controls:(i + 1) * n_controls, i * n_controls:(i + 1) * n_controls] = self.R
F[i * n_controls:(i + 1) * n_controls, 0] = 0
G = np.zeros((self.N * n_states, self.N * n_controls))
E = np.zeros((self.N * n_states, n_states))
L = np.zeros((self.N * n_states, 1))
for i in range(self.N):
for j in range(i + 1):
G[i * n_states:(i + 1) * n_states, j * n_controls:(j + 1) * n_controls] \
= np.linalg.matrix_power(self.A,i - j) @ self.B
E[i * n_states:(i + 1) * n_states, :] = np.linalg.matrix_power(self.A, i + 1)
L[i * n_states:(i + 1) * n_states, :] = np.linalg.matrix_power(self.A, i + 1) @ x - r
Q_bar = np.kron(np.eye(self.N), self.Q)
H += G.T @ Q_bar @ G
F += (G.T @ Q_bar @ L).reshape(-1, 1)
# 计算最优控制输入
u_opt = -np.linalg.inv(H) @ F
# 取第一个控制输入
u = u_opt[:n_controls]
return u
def update(self, setpoint, measurement):
self.measurement_history.append(measurement)
self.setpoint_history.append(setpoint)
u = self.mpc_control(np.array(measurement), np.array(setpoint))
# print(u)
return u
def plot_error(self, title='none'):
self.setpoint_history = np.array(self.setpoint_history)
self.measurement_history = np.array(self.measurement_history)
subtitle = ['x', 'y', 'z']
fig = plt.figure(figsize=(14, 4))
for i in range(len(self.setpoint_history[0])):
ax = fig.add_subplot(131+i)
ax.plot([i / 100 for i in range(len(self.setpoint_history[:, i]))], self.setpoint_history[:, i],
label='setpoint')
ax.plot([i / 100 for i in range(len(self.measurement_history[:, i]))], self.measurement_history[:, i],
label='measurement')
ax.set_xlabel('time')
ax.set_ylabel('value')
ax.set_title(subtitle[i]+title)
ax.legend()
plt.tight_layout()
if __name__ == '__main__':
A = np.array([[1, 0,0], [0, 1,0],[0,0,1]])
B = np.array([[0.5,0.6,0.2], [1,0.1,0.4],[0.5,0.5,0.8]])
C = np.eye(3) # 输出矩阵
Q = np.eye(3)*2
R = np.eye(1)
N = 10
u_min = -1
u_max = 1
reg = 1e-5
outer_controller = MPCController(A, B, C, N, Q, R, u_min, u_max, reg)
inner_controller = MPCController(A, B, C, N, Q, R, u_min, u_max, reg)
setpoint = np.array([[1], [1],[1]])
# r = np.ones((1, N)) # 目标为1
measurement = np.array([[0],[0],[0]])
x = np.array([[0], [0],[0]])
x_dot = np.array([[0], [0], [0]])
for _ in range(10):
u = outer_controller.update(setpoint, x)
u_v = inner_controller.update(u,x_dot)
x_dot = A @ x_dot + B @ u_v.reshape(-1, 1)
x = A @ x + B @ x_dot.reshape(-1, 1)
# x = A @ x + B @ u.reshape(-1, 1)
y = C @ x
print("控制输入 u:")
print(u)
print('状态 x:')
print(x)
inner_controller.plot_error()
outer_controller.plot_error()
plt.show()
仿真运行代码：
import numpy as np
import matplotlib.pyplot as plt
from init_control import UAVParameters
from uav_dynamics import UAVDynamics
from common_function import rotation_matrix, angle_body2world
from PIDController import posion_pos_controller
from LQRController import LQRController
from MPCController import MPCController
from square_signal import generate_ramp_signal
class UAVSimulation:
def __init__(self, params, dynamics, position_controller, pose_controller, controller_type):
self.params = params
self.dynamics = dynamics
self.controller_type = controller_type
if controller_type == "PID":
self.x_controller = position_controller[0]
self.y_controller = position_controller[1]
self.height_controller = position_controller[2]
else:
self.position_controller = position_controller[0]
self.velocity_controller = position_controller[1]
self.roll_controller = pose_controller[0]
self.pitch_controller = pose_controller[1]
self.yaw_controller = pose_controller[2]
self.position_history = []
self.setpoint_history = []
def desired_euler_angles(self, eax, eay, psi):
"""
Convert the desired horizontal acceleration to the desired Euler angle
Input:
eax, eay: desired horizontal acceleration
Output:
phi, theta: desired roll and pitch
"""
g = 9.8 # 重力加速度 (m/s^2)
phi = (-np.sin(psi) * eax + np.cos(psi) * eay) / g
theta = (-np.cos(psi) * eax - np.sin(psi) * eay) / g
return phi, theta
def motor_mixer(self, roll, pitch, yaw, thrust):
"""
Control allocation. The quadrotor type is X-configuration,
and the airframe is as follows:
3↓ 1↑
\ /
/ \
2↑ 4↓
Input:
roll, pitch, yaw: attitude controller output.
thrust: total thrust.
Output:
M1, M2, M3, M4: motor speed commands.
"""
idle_PWM = 1000
scale = 1000
M1 = (thrust - roll + pitch + yaw) * scale + idle_PWM
M2 = (thrust + roll - pitch + yaw) * scale + idle_PWM
M3 = (thrust + roll + pitch - yaw) * scale + idle_PWM
M4 = (thrust - roll - pitch - yaw) * scale + idle_PWM
return M1, M2, M3, M4
def pwm_to_rpm(self, pwm):
"""
Convert PWM signal to RPM. This is a simple linear approximation.
Input:
pwm: PWM signal value
Output:
rpm: motor speed in RPM
"""
min_pwm = 1000
max_pwm = 2000
# 限制PWM范围
pwm = np.clip(pwm, min_pwm, max_pwm)
# 线性插值计算RPM
rpm = np.clip((pwm - min_pwm) / (max_pwm - min_pwm), -1, 1)
return rpm
def run(self, square_corners, duration, dt):
DEG2RAD = self.params.DEG2RAD
posE = self.params.ModelInit_PosE.astype(np.float64)
velB = self.params.ModelInit_VelB.astype(np.float64)
velE = self.params.ModelInit_VelB.astype(np.float64)
angEuler = self.params.ModelInit_AngEuler
rateB = self.params.ModelInit_RateB
rateE = self.params.ModelInit_RateB
ModelParam_motorCr = self.params.ModelParam_motorCr
ModelParam_motorWb = self.params.ModelParam_motorWb
state = [velB, angEuler, rateB]
current_corner_index = 0
time_steps = int(duration / dt)
num_steps = time_steps // (len(square_corners) - 1)
# next_corner_time = time_steps // (len(square_corners)-1)
input_signal = generate_ramp_signal(square_corners, num_steps)
for step in range(time_steps):
setpoint = input_signal[step]
eax, eay, altitude_output = None, None, None
print('setpoint:', setpoint)
if self.controller_type == 'PID':
eax = self.x_controller.update(outer_setpoint=setpoint[0], outer_measurement=posE[0],
inner_measurement=velE[0], dt=dt)
eay = self.y_controller.update(outer_setpoint=setpoint[1], outer_measurement=posE[1],
inner_measurement=velE[1], dt=dt)
altitude_output = self.height_controller.update(outer_setpoint=setpoint[2], outer_measurement=posE[2],
inner_measurement=velE[2], dt=dt)
else:
vel_setpoint = self.position_controller.update(setpoint, posE)
# 速度环LQR控制
acc_setpoint = self.velocity_controller.update(vel_setpoint, velE)
if self.controller_type == "MPC":
eax, eay, altitude_output = acc_setpoint[0][0], acc_setpoint[1][0], acc_setpoint[2][0]
elif self.controller_type == "LQR":
eax, eay, altitude_output = acc_setpoint[0], acc_setpoint[1], acc_setpoint[2]
else:
print("unknown controller")
phi, theta = self.desired_euler_angles(eax, eay, angEuler[2])
phi = np.clip(phi, -35 * DEG2RAD, 35 * DEG2RAD)
theta = np.clip(theta, -35 * DEG2RAD, 35 * DEG2RAD)
print('eax,eay:', eax, eay)
print('phi,theta:', phi, theta)
roll_output = np.clip(self.roll_controller.update(outer_setpoint=phi, outer_measurement=angEuler[0],
inner_measurement=rateE[0], dt=dt), -1, 1)
pitch_output = np.clip(self.pitch_controller.update(outer_setpoint=theta, outer_measurement=angEuler[1],
inner_measurement=rateE[1], dt=dt), -1, 1)
yaw_output = np.clip(self.yaw_controller.update(outer_setpoint=(step // num_steps) * 0.5 * 0.0174533,
outer_measurement=angEuler[2],
inner_measurement=rateE[2], dt=dt), -1, 1)
thrust = np.clip(-altitude_output + self.params.THR_HOVER, 0.05, 0.9)
print('roll_output,pitch_output,yaw_output,altitude_output:', roll_output, pitch_output, yaw_output,
altitude_output)
M1, M2, M3, M4 = self.motor_mixer(roll_output, pitch_output, yaw_output, thrust)
print('M1,M2,M3,M4:', M1, M2, M3, M4)
w_target = (np.array(
[self.pwm_to_rpm(M1), self.pwm_to_rpm(M2), self.pwm_to_rpm(M3),
self.pwm_to_rpm(M4)]) * ModelParam_motorCr +
np.array([ModelParam_motorWb, ModelParam_motorWb, ModelParam_motorWb, ModelParam_motorWb]))
print('w_target:', w_target)
state = self.dynamics.update_dynamics(state, w_target, dt)
velB, angEuler, rateB = state
# R = rotation_matrix(angEuler)
R_angle = angle_body2world(angEuler)
R = rotation_matrix(angEuler)
rateE = R_angle @ rateB
angEuler = angEuler + rateE * dt
velE = R @ velB
posE = posE + velE * dt
state = velB, angEuler, rateB
self.position_history.append(posE.copy())
print(
f"Time: {step * dt:.2f}s, Position: {posE}, Velocity_world: {velE}, Angle: {angEuler}, Rate: {rateE}")
# if step % (next_corner_time-1) == 0 and step != 0:
# current_corner_index = (current_corner_index + 1) % len(square_corners)
self.setpoint_history.append(setpoint.copy())
def plot_trajectory(self):
position_history = np.array(self.position_history)
setpoint_history = np.array(self.setpoint_history)
fig = plt.figure()
ax = fig.add_subplot(121, projection='3d')
ax.plot(position_history[:, 0], position_history[:, 1], -position_history[:, 2], label='UAV Trajectory')
ax.set_xlabel('X Position (m)')
ax.set_ylabel('Y Position (m)')
ax.set_zlabel('Z Position (m)')
ax.set_title('Trajectory of UAV ')
ax.legend()
ax = fig.add_subplot(122, projection='3d')
ax.plot(setpoint_history[:, 0], setpoint_history[:, 1], -setpoint_history[:, 2], label='setpoint Trajectory')
ax.set_xlabel('X Position (m)')
ax.set_ylabel('Y Position (m)')
ax.set_zlabel('Z Position (m)')
ax.set_title(' Desired Trajectory ')
ax.legend()
def plot_state_error(self):
if self.controller_type == 'PID':
fig_position = plt.figure(figsize=(14, 4))
self.x_controller.outer_controller.plot_error(fig_position, 0, title='x_position')
self.y_controller.outer_controller.plot_error(fig_position, 1, title='y_position')
self.height_controller.outer_controller.plot_error(fig_position, 2, title='height_position')
fig_velocity = plt.figure(figsize=(14, 4))
self.x_controller.inner_controller.plot_error(fig_velocity, 0, title='x_velocity')
self.y_controller.inner_controller.plot_error(fig_velocity, 1, title='y_velocity')
self.height_controller.inner_controller.plot_error(fig_velocity, 2, title='height_velocity')
else:
self.position_controller.plot_error('_position')
self.velocity_controller.plot_error('_velocity')
fig_angle = plt.figure(figsize=(14, 4))
self.roll_controller.outer_controller.plot_error(fig_angle, 0, title='roll_angle')
self.pitch_controller.outer_controller.plot_error(fig_angle, 1, title='pitch_angle')
self.yaw_controller.outer_controller.plot_error(fig_angle, 2, title='yaw_angle')
plt.tight_layout()
fig_rate = plt.figure(figsize=(14, 4))
self.roll_controller.inner_controller.plot_error(fig_rate, 0, title='roll_rate')
self.pitch_controller.inner_controller.plot_error(fig_rate, 1, title='pitch_rate')
self.yaw_controller.inner_controller.plot_error(fig_rate, 2, title='yaw_rate')
plt.tight_layout()
# plt.show()
if __name__ == '__main__':
params = UAVParameters()
dynamics = UAVDynamics(params)
# controller = CascadePIDController(PIDController(3.0, 0.0, 0.15), PIDController(0.15, 0.0, 0.03))
# controller_type = "PID"
# controller_type = "MPC"
controller_type = "LQR"
position_controller = None
if controller_type == 'PID':
position_controller = posion_pos_controller(params).position_controller()
elif controller_type == 'LQR':
# 定义位置环的状态空间模型
A_pos = np.zeros((3, 3))
B_pos = np.eye(3)
Q_pos = np.eye(3) * 1.65
R_pos = np.eye(3) * 4.0
# 定义速度环的状态空间模型
A_vel = np.zeros((3, 3))
B_vel = np.eye(3)
Q_vel = np.eye(3) * 10.0
R_vel = np.eye(3) * 2.4
pos_controller = LQRController(A_pos, B_pos, Q_pos, R_pos)
vel_controller = LQRController(A_vel, B_vel, Q_vel, R_vel)
position_controller = [pos_controller, vel_controller]
elif controller_type == 'MPC':
# 定义离散位置环的状态空间模型
A_pos = np.eye(3)
B_pos = np.eye(3) * 0.01
Q_pos = np.eye(3) * 3.95
R_pos = np.eye(3) * 0.6
# 定义离散速度环的状态空间模型
# A_vel = np.zeros((3, 3))
A_vel = np.eye(3)
B_vel = np.eye(3) * 0.01
Q_vel = np.eye(3) * 12.0
R_vel = np.eye(3) * 0.6
pos_controller = MPCController(A_pos, B_pos, None, 10, Q_pos, R_pos, -3, 3)
vel_controller = MPCController(A_vel, B_vel, None, 10, Q_vel, R_vel, -1, 1)
position_controller = [pos_controller, vel_controller]
else:
print('ERROR:Unknown Controller')
pose_controller = posion_pos_controller(params).pos_controller()
target_altitude = -5 # 目标高度
square_corners = [
[0, 0, 0],
[0, 0, target_altitude],
[5, 0, target_altitude],
[5, 5, target_altitude],
[0, 5, target_altitude],
[0, 0, target_altitude],
[0, 0, 0],
[0, 0, 0]
]
simulation = UAVSimulation(params, dynamics, position_controller=position_controller,
pose_controller=pose_controller, controller_type=controller_type)
simulation.run(square_corners, 35, 0.01)
simulation.plot_trajectory()
simulation.plot_state_error()
plt.show()
三种控制算法效果如下：
可能由于任务较为简单，三种算法差异不大，同时LQR和MPC计算效率较低。
Last，放上笔者的个人主页:郭昱鑫的个人主页
扫一扫
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
