用Python在AirSim中实现LQR控制：让无人机画出完美8字轨迹

当第一次看到无人机在AirSim仿真环境中自动飞出精确的8字轨迹时，那种成就感难以言表。LQR（线性二次调节器）作为经典控制算法，与AirSim这个强大的无人机仿真平台结合，能创造出令人惊艳的自动化飞行效果。本文将带你从零开始，用Python代码实现这一目标，避开数学公式的泥沼，直击可运行的完整解决方案。

1. 环境准备与基础配置

在开始编写LQR控制器之前，我们需要确保AirSim环境正确设置。推荐使用Windows 10/11系统，搭配AirSim的"Blocks"环境，这是最稳定的测试场景。

首先安装必要的Python包：

pip install msgpack-rpc-python numpy scipy airsim

接着配置AirSim的设置文件（ settings.json ），确保无人机初始状态为可控制模式：

{
  "SettingsVersion": 1.2,
  "SimMode": "Multirotor",
  "Vehicles": {
    "Drone1": {
      "VehicleType": "SimpleFlight",
      "AutoCreate": true,
      "X": 0, "Y": 0, "Z": -2
    }
  }
}

测试连接是否成功的Python代码片段：

import airsim

client = airsim.MultirotorClient()
client.confirmConnection()
client.enableApiControl(True)
client.armDisarm(True)

print("无人机已准备就绪！")

常见问题排查：

• 连接失败时检查AirSim是否正在运行
• 确保Python版本为3.6+
• 防火墙可能阻止通信，必要时添加例外

2. LQR控制器实现详解

我们将创建一个完整的LQR控制器类，封装所有核心功能。不同于理论推导，这里聚焦于实际可用的Python实现。

2.1 控制器类结构设计

import numpy as np
from scipy.linalg import solve_continuous_are

class LQRController:
    def __init__(self, Q=None, R=None):
        # 默认权重矩阵
        self.Q = np.diag([10, 10, 5, 1, 1, 1]) if Q is None else Q
        self.R = np.diag([0.1, 0.1, 0.1]) if R is None else R
        
        # 系统矩阵（根据无人机动力学简化）
        self.A = np.zeros((6, 6))
        self.A[:3, 3:] = np.eye(3)
        
        self.B = np.zeros((6, 3))
        self.B[3:, :] = np.eye(3)
        
        # 反馈矩阵K
        self.K = self._compute_gain_matrix()
    
    def _compute_gain_matrix(self):
        """计算LQR增益矩阵"""
        P = solve_continuous_are(self.A, self.B, self.Q, self.R)
        K = np.linalg.inv(self.R) @ self.B.T @ P
        return K
    
    def compute_control(self, state, desired_state):
        """计算控制指令"""
        error = state - desired_state
        u = -self.K @ error
        return u

2.2 状态空间与参数调优

理解状态变量设计对控制器性能至关重要：

状态分量	物理意义	典型权重范围
x,y,z	位置	5-20
vx,vy,vz	速度	0.5-5

调整Q和R矩阵的经验法则：

• 增大Q对角元素：状态收敛更快，但可能超调
• 增大R对角元素：控制输入更平滑，但响应变慢

调试建议流程：

1. 从较小Q和较大R开始
2. 逐步增加Q直到出现振荡
3. 微调R使控制平滑

3. 8字轨迹生成与跟踪

3.1 参数化8字轨迹

我们使用Lissajous曲线生成理想的8字轨迹：

def generate_figure8(t, scale=5, period=20):
    """生成8字轨迹"""
    x = scale * np.sin(2 * np.pi * t / period)
    y = scale * np.sin(4 * np.pi * t / period)
    z = -5  # 固定高度
    
    # 计算期望速度（导数）
    vx = (2 * np.pi * scale / period) * np.cos(2 * np.pi * t / period)
    vy = (4 * np.pi * scale / period) * np.cos(4 * np.pi * t / period)
    vz = 0
    
    return np.array([x, y, z, vx, vy, vz])

3.2 完整控制循环实现

将各个组件集成到主控制循环中：

import time

def run_figure8():
    client = airsim.MultirotorClient()
    client.confirmConnection()
    client.enableApiControl(True)
    client.armDisarm(True)
    client.takeoffAsync().join()
    
    lqr = LQRController()
    start_time = time.time()
    
    try:
        while True:
            # 获取当前状态
            pose = client.simGetVehiclePose()
            kinematics = client.getMultirotorState().kinematics_estimated
            
            current_state = np.array([
                pose.position.x_val, 
                pose.position.y_val,
                pose.position.z_val,
                kinematics.linear_velocity.x_val,
                kinematics.linear_velocity.y_val,
                kinematics.linear_velocity.z_val
            ])
            
            # 生成期望状态
            t = time.time() - start_time
            desired_state = generate_figure8(t)
            
            # 计算控制指令
            u = lqr.compute_control(current_state, desired_state)
            
            # 转换为AirSim控制指令
            client.moveByVelocityAsync(
                u[0], u[1], u[2], 0.1  # 0.1秒持续时间
            )
            
            time.sleep(0.05)  # 控制频率约20Hz
            
    except KeyboardInterrupt:
        client.armDisarm(False)
        client.reset()

4. 高级技巧与性能优化

4.1 实时可视化调试

添加以下代码可在AirSim中显示轨迹参考点：

def draw_reference_point(client, position, color_rgba=[1.0, 0.0, 0.0, 1.0]):
    """在仿真环境中绘制参考点"""
    client.simPlotPoints(
        [airsim.Vector3r(position[0], position[1], position[2])],
        color_rgba,
        size=15,
        duration=0.1,
        is_persistent=False
    )

4.2 抗风扰增强

在LQR控制器中添加积分项以消除稳态误差：

class EnhancedLQRController(LQRController):
    def __init__(self, Q=None, R=None, Ki=0.1):
        super().__init__(Q, R)
        self.Ki = Ki
        self.integral_error = np.zeros(6)
    
    def compute_control(self, state, desired_state):
        error = state - desired_state
        self.integral_error += error * 0.1  # 积分时间步
        
        # 基本LQR控制 + 积分项
        u = -self.K @ error - self.Ki * self.integral_error
        return u

4.3 性能指标监控

实现一个简单的性能评估器：

class PerformanceMonitor:
    def __init__(self):
        self.errors = []
    
    def update(self, error):
        self.errors.append(np.linalg.norm(error[:3]))  # 只考虑位置误差
    
    def get_stats(self):
        errors = np.array(self.errors)
        return {
            'max_error': np.max(errors),
            'avg_error': np.mean(errors),
            'rms_error': np.sqrt(np.mean(errors**2))
        }

典型性能指标范围：

指标	优秀值	可接受值	需改进值
最大误差(m)	<0.5	0.5-1.5	>1.5
RMS误差(m)	<0.2	0.2-0.5	>0.5

5. 实战：从简单路径到复杂轨迹

掌握了基础8字轨迹后，可以扩展更复杂的飞行模式。例如，实现高度变化的螺旋8字：

def generate_spiral_figure8(t, scale=5, period=20, height_amp=3):
    """生成带高度变化的8字轨迹"""
    base_z = -10  # 基准高度
    x = scale * np.sin(2 * np.pi * t / period)
    y = scale * np.sin(4 * np.pi * t / period)
    z = base_z + height_amp * np.sin(2 * np.pi * t / (2 * period))
    
    # 速度计算
    vx = (2 * np.pi * scale / period) * np.cos(2 * np.pi * t / period)
    vy = (4 * np.pi * scale / period) * np.cos(4 * np.pi * t / period)
    vz = (2 * np.pi * height_amp / (2 * period)) * np.cos(2 * np.pi * t / (2 * period))
    
    return np.array([x, y, z, vx, vy, vz])

对于更精确的控制，可以考虑添加前馈补偿：

def compute_control_with_feedforward(self, state, desired_state, desired_accel):
    """带前馈的LQR控制"""
    error = state - desired_state
    u = -self.K @ error + desired_accel  # 前馈项
    return u

在项目实践中，我发现将轨迹生成与控制器分离是良好的架构设计。这样可以根据需要切换不同的轨迹生成器，而控制器保持不变。另一个实用技巧是在调试阶段降低仿真速度，使用AirSim的 simSetSimulationMode 函数可以减慢时间，方便仔细观察无人机行为。