当前位置：首页 > news >正文

在AirSim里用Python实现LQR控制：让无人机自动跟踪预设轨迹（附完整代码）

news 2026/6/5 10:18:23

用Python实现AirSim无人机LQR轨迹跟踪：从理论到代码落地

1. 环境准备与基础概念

在开始编写代码之前，我们需要先搭建好开发环境并理解几个核心概念。AirSim是微软开源的无人机/车辆仿真平台，基于Unreal Engine构建，提供了高度逼真的物理模拟和丰富的API接口。而LQR（Linear Quadratic Regulator）则是一种经典的最优控制算法，特别适合像无人机这样的线性系统控制。

1.1 安装必要的软件包

首先确保你已经安装了以下组件：

pip install airsim numpy scipy matplotlib

对于AirSim本身，需要从GitHub克隆并编译：

git clone https://github.com/microsoft/AirSim.git cd AirSim ./setup.sh ./build.sh

提示：编译AirSim需要安装Visual Studio（Windows）或gcc（Linux），并确保系统已安装Unreal Engine。

1.2 理解无人机动力学模型

无人机在空中的运动可以用以下状态空间方程表示：

$$ \begin{aligned} \dot{x} &= Ax + Bu \ y &= Cx + Du \end{aligned} $$

其中：

$x$ 是状态向量（位置、速度等）
$u$ 是控制输入（电机推力）
$A$ 是系统矩阵
$B$ 是输入矩阵

对于简单的二维平面控制，我们可以将状态向量定义为：

x = [position_x, position_y, velocity_x, velocity_y]

2. LQR控制器设计与实现

2.1 LQR算法核心原理

LQR通过最小化以下代价函数来找到最优控制律：

$$ J = \int_0^\infty (x^TQx + u^TRu)dt $$

其中：

$Q$ 是状态权重矩阵
$R$ 是控制输入权重矩阵

求解LQR问题最终归结为求解代数Riccati方程：

from scipy.linalg import solve_continuous_are # 解Riccati方程 P = solve_continuous_are(A, B, Q, R) # 计算反馈矩阵K K = np.linalg.inv(R) @ B.T @ P

2.2 Python实现LQR控制器

下面是一个完整的LQR控制器类实现：

import numpy as np from scipy.linalg import solve_continuous_are class LQRController: def __init__(self, A, B, Q, R): self.A = A self.B = B self.Q = Q self.R = R self.compute_gain_matrix() def compute_gain_matrix(self): """计算LQR反馈矩阵K""" P = solve_continuous_are(self.A, self.B, self.Q, self.R) self.K = np.linalg.inv(self.R) @ self.B.T @ P def compute_control(self, x, x_desired): """计算控制输入""" error = x - x_desired return -self.K @ error

3. AirSim接口与轨迹跟踪

3.1 连接AirSim并获取无人机状态

首先我们需要建立与AirSim的连接并获取无人机状态：

import airsim # 连接到AirSim client = airsim.MultirotorClient() client.confirmConnection() client.enableApiControl(True) client.armDisarm(True) # 获取无人机状态 def get_drone_state(): state = client.getMultirotorState() position = state.kinematics_estimated.position velocity = state.kinematics_estimated.linear_velocity return np.array([ position.x_val, position.y_val, velocity.x_val, velocity.y_val ])

3.2 轨迹生成与跟踪

我们可以定义一个简单的圆形轨迹作为跟踪目标：

def generate_circular_trajectory(radius=10, speed=2, dt=0.05): """生成圆形轨迹""" t = 0 while True: x = radius * np.cos(speed * t) y = radius * np.sin(speed * t) vx = -radius * speed * np.sin(speed * t) vy = radius * speed * np.cos(speed * t) yield np.array([x, y, vx, vy]) t += dt

4. 完整控制循环实现

4.1 主控制循环

将前面所有组件整合成一个完整的控制循环：

def main(): # 初始化 client.takeoffAsync().join() # 定义系统矩阵 (简化模型) A = np.array([ [0, 0, 1, 0], [0, 0, 0, 1], [0, 0, 0, 0], [0, 0, 0, 0] ]) B = np.array([ [0, 0], [0, 0], [1, 0], [0, 1] ]) # 权重矩阵 Q = np.diag([10, 10, 1, 1]) # 更重视位置误差 R = np.diag([0.1, 0.1]) # 控制输入权重 # 创建控制器 controller = LQRController(A, B, Q, R) # 轨迹生成器 trajectory = generate_circular_trajectory() try: while True: # 获取当前状态和期望状态 x = get_drone_state() x_desired = next(trajectory) # 计算控制输入 u = controller.compute_control(x, x_desired) # 转换为AirSim控制指令 client.moveByVelocityAsync( u[0], u[1], 0, 0.1 # 只控制xy平面 ) time.sleep(0.05) except KeyboardInterrupt: client.armDisarm(False) client.reset()

4.2 调试与优化技巧

在实际应用中，你可能需要关注以下几点：

系统辨识：真实的无人机动力学可能更复杂，需要通过实验或系统辨识来获取更准确的A、B矩阵
权重调整：Q和R矩阵的选择对控制效果影响很大，需要根据实际需求调整
状态估计：如果使用真实无人机，可能需要卡尔曼滤波来处理传感器噪声
抗饱和处理：当控制输入达到物理限制时，需要特殊处理避免积分饱和

5. 高级话题与扩展

5.1 三维空间扩展

将我们的二维控制器扩展到三维空间：

# 3D状态向量 x_3d = [pos_x, pos_y, pos_z, vel_x, vel_y, vel_z] # 3D系统矩阵 A_3d = np.block([ [np.zeros((3,3)), np.eye(3)], [np.zeros((3,3)), np.zeros((3,3))] ]) B_3d = np.block([ [np.zeros((3,3))], [np.eye(3)] ])