引言

在之前的两篇文章中，我们分别剖析了 AirSim 的总体架构（五层分层设计）和 AirSim↔PX4 SITL 的双通道通信架构（锁步同步与 MAVLink 路由）。本文更进一步，聚焦四旋翼 SITL 仿真的完整闭环——从 AirSim 初始化一个四旋翼无人机、与 PX4 建立连接、传感器数据注入、到 PX4 解算控制指令并驱动旋翼旋转的端到端流程。所有分析基于 AirSim main 分支源码。

本文重点：不重复前两篇文章的内容。如果你还没读过，建议先看前两篇建立全局认知。

一、仿真闭环总览：一张图看懂数据流

在深入源码之前，先建立全局视图。下图展示了一次 SITL 仿真循环的完整数据链路——从 AirSim 物理引擎生成位姿，到 PX4 飞控解算电机指令，再回到旋翼驱动的闭环：

AirSim↔PX4 SITL闭环架构

简化版 ASCII 流程如下：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
┌─────────────────────────────────────────────────────────────────┐
│                        AirSim (Unreal/Unity)                      │
│                                                                   │
│  ┌──────────────┐     ┌──────────────────┐     ┌──────────────┐  │
│  │ PhysicsBody   │────▶│ SensorCollection  │────▶│ MavLinkMulti │  │
│  │ (位姿/速度)   │     │ IMU/Baro/Mag/GPS  │     │ rotorApi     │  │
│  └──────────────┘     └──────────────────┘     └──────┬───────┘  │
│                                                        │          │
│  ┌──────────────┐     ┌──────────────────┐            │          │
│  │ RotorActuator│◀────│ getActuation()   │            │ HIL_SENSOR│
│  │ (拉力/扭矩)  │     │ 读取 rotor_controls│           │ HIL_GPS   │
│  └──────────────┘     └──────────────────┘            │          │
│                                                        │          │
└────────────────────────────────────────────────────────┼──────────┘
                                                         │
                                              MAVLink UDP 4560
                                                         │
┌────────────────────────────────────────────────────────┼──────────┐
│                     PX4 SITL (px4 进程)                 │          │
│                                                         ▼          │
│  ┌──────────────┐     ┌──────────────────┐     ┌──────────────┐  │
│  │ EKF2 估计器   │◀────│ sensors/vehicle_  │◀────│ simulator     │  │
│  │ (状态融合)    │     │ IMU             │     │ mavlink 接收   │  │
│  └──────┬───────┘     └──────────────────┘     └──────────────┘  │
│         │                                                         │
│  ┌──────▼───────┐     ┌──────────────────┐     ┌──────────────┐  │
│  │ mc_att_control│────▶│ mc_rate_control   │────▶│ mixers        │  │
│  │ (姿态控制)    │     │ (角速度控制)       │     │ (混控器)       │  │
│  └──────────────┘     └──────────────────┘     └──────┬───────┘  │
│                                                        │          │
│                                            HIL_ACTUATOR_CONTROLS  │
│                                            返回到 AirSim           │
└───────────────────────────────────────────────────────────────────┘

核心闭环：AirSim 物理 → 传感器 → MAVLink → PX4 飞控 → 电机指令 → AirSim 旋翼 → 回到物理。

二、启动链路：从 settings.json 到四旋翼诞生

2.1 工厂模式创建无人机

AirSim 启动时，MultiRotorParamsFactory 根据 settings.json 中的 VehicleType 字段决定创建哪种无人机：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// MultiRotorParamsFactory.hpp: 行 21-48
static std::unique_ptr<MultiRotorParams> createConfig(
    const AirSimSettings::VehicleSetting* vehicle_setting,
    std::shared_ptr<const SensorFactory> sensor_factory)
{
    if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypePX4) {
        // 创建 PX4 控制的无人机
        config.reset(new Px4MultiRotorParams(/* ... */));
    }
    else if (vehicle_setting->vehicle_type == AirSimSettings::kVehicleTypeSimpleFlight) {
        // 创建内置 simple_flight 控制的无人机
        config.reset(new SimpleFlightQuadXParams(/* ... */));
    }
    // ArduCopter, ArduCopterSolo 等其他固件...
}

当用户配置 "VehicleType": "PX4" 时，工厂创建 Px4MultiRotorParams。

2.2 Px4MultiRotorParams：连接信息 + 机架配置

Px4MultiRotorParams 做了两件事：

第一，从 settings.json 提取 MAVLink 连接信息（含锁步开关）：

1
2
3
4
5
6
7
8
// Px4MultiRotorParams.hpp: 行 20-24
Px4MultiRotorParams(const AirSimSettings::MavLinkVehicleSetting& vehicle_setting,
                    std::shared_ptr<const SensorFactory> sensor_factory)
    : sensor_factory_(sensor_factory)
{
    connection_info_ = getConnectionInfo(vehicle_setting);
    // connection_info_ 包含了 UdpPort, ControlPort, lock_step 等全套连接参数
}

第二，根据 Model 字段选择机架类型：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
// Px4MultiRotorParams.hpp: 行 36-58
virtual void setupParams() override
{
    if (connection_info_.model == "Blacksheep") {
        setupFrameBlacksheep(params);
    }
    else if (connection_info_.model == "Flamewheel") {
        setupFrameFlamewheel(params);
    }
    else if (connection_info_.model == "Hexacopter") {
        setupFrameGenericHex(params);
    }
    else // 默认 Generic 四旋翼
        setupFrameGenericQuad(params);
}

然后创建 MavLinkMultirotorApi 实例：

1
2
3
4
5
6
7
8
9
// Px4MultiRotorParams.hpp: 行 28-35
virtual std::unique_ptr<MultirotorApiBase> createMultirotorApi() override
{
    unique_ptr<MultirotorApiBase> api(new MavLinkMultirotorApi());
    auto api_ptr = static_cast<MavLinkMultirotorApi*>(api.get());
    api_ptr->initialize(connection_info_, &getSensors(), true);
    // is_simulation=true → 启用传感器注入模式
    return api;
}

2.3 PX4 进程启动

AirSim 本身不启动 PX4。PX4 SITL 需要外部启动：

1
2
3
4
# PX4Scripts/run_airsim_sitl.sh
export PX4_SIM_MODEL=iris
$BIN_DIR/px4 -i $instance_num $BUILD_DIR \
    -s "etc/init.d-posix/rcS" -t $TEST_DATA

PX4_SIM_MODEL=iris 告诉 PX4 使用 Iris 四旋翼的机架参数。PX4 启动后会创建一个 UDP 端口等待 AirSim 连接。

这是仿真闭环的第一半——AirSim 如何把物理仿真结果「喂」给 PX4。

3.1 仿真 tick 入口

每一帧，MultiRotorPhysicsBody::updateKinematics() 被调用：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
// MultiRotorPhysicsBody.hpp: 行 70-91
virtual void updateKinematics(const Kinematics::State& kinematics) override
{
    PhysicsBody::updateKinematics(kinematics);
    updateSensorsAndController(); // ← 关键入口
}

void updateSensorsAndController()
{
    updateSensors(*params_, getKinematics(), getEnvironment());
    // 1. 更新传感器（IMU/Baro/Mag/GPS 从物理真值 + 噪声模型生成）

    vehicle_api_->update();
    // 2. 更新飞控 → 这里调用 MavLinkMultirotorApi::update()

    for (uint rotor_index = 0; rotor_index < rotors_.size(); ++rotor_index) {
        rotors_.at(rotor_index).setControlSignal(
            vehicle_api_->getActuation(rotor_index));
    }
    // 3. 将 PX4 返回的电机指令传给旋翼执行器
}

3.2 MavLinkMultirotorApi::update() — HIL 传感器发送

这是最核心的传感器注入函数：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
// MavLinkMultirotorApi.hpp: 行 126-232
virtual void update() override
{
    // ===== 锁步检查 =====
    if (lock_step_active_) {
        if (last_update_time_ + 1000000 < now) {
            // 1 秒未收到 HIL_ACTUATOR_CONTROLS → 超时，重置锁步
            lock_step_active_ = false;
            lock_step_resets_++;
        }
        else if (!received_actuator_controls_) {
            // 在锁步模式下，还没收到执行器指令 → 跳过本轮传感器发送
            return;
        }
    }

    // ===== 发送 HIL_SENSOR（IMU + 磁力计 + 气压计） =====
    const auto& imu_output = getImuData("");
    const auto& mag_output = getMagnetometerData("");
    const auto& baro_output = getBarometerData("");

    sendHILSensor(
        imu_output.linear_acceleration,   // 三轴加速度 (m/s²)
        imu_output.angular_velocity,       // 三轴角速度 (rad/s)
        mag_output.magnetic_field_body,    // 磁场矢量
        baro_output.pressure * 0.01f,      // 气压 (Pa → mbar)
        baro_output.altitude               // 气压高度
    );

    sendSystemTime();  // 发送系统时间同步

    // ===== 发送 HIL_GPS =====
    const auto& gps_output = getGpsData("");
    if (gps_output.is_valid && gps_output.gnss.time_utc > last_gps_time_) {
        sendHILGps(
            gps_output.gnss.geo_point,    // 经纬度
            gps_velocity,                  // 三维速度
            gps_velocity_xy.norm(),        // 地速
            gps_cog,                       // 航向角
            gps_output.gnss.eph,           // 水平精度
            gps_output.gnss.epv,           // 垂直精度
            gps_output.gnss.fix_type,      // 定位类型
            10                             // 可见卫星数
        );
    }
}

3.3 sendHILSensor 详解——锁步同步的关键

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
// MavLinkMultirotorApi.hpp: 行 1755-1800
void sendHILSensor(const Vector3r& acceleration, const Vector3r& gyro,
                   const Vector3r& mag, float abs_pressure, float pressure_alt)
{
    MavLinkHilSensor hil_sensor;
    hil_sensor.time_usec = last_hil_sensor_time_ = getSimTime();
    // ↑ 时间戳使用 lock_step_active_ 下的冻结时间

    hil_sensor.xacc = acceleration.x();
    hil_sensor.yacc = acceleration.y();
    hil_sensor.zacc = acceleration.z();
    hil_sensor.fields_updated = 0b111;  // 标记加速度字段有效

    hil_sensor.xgyro = gyro.x();
    hil_sensor.ygyro = gyro.y();
    hil_sensor.zgyro = gyro.z();
    hil_sensor.fields_updated |= 0b111000;  // 标记角速度字段有效

    hil_sensor.xmag = mag.x();
    hil_sensor.ymag = mag.y();
    hil_sensor.zmag = mag.z();
    hil_sensor.fields_updated |= 0b111000000;  // 标记磁场字段有效

    hil_sensor.abs_pressure = abs_pressure;
    hil_sensor.pressure_alt = pressure_alt;
    hil_sensor.fields_updated |= 0b1101000000000;  // 标记气压字段有效

    if (hil_node_ != nullptr) {
        hil_node_->sendMessage(hil_sensor);
        received_actuator_controls_ = false;  // ← 重置，等待 PX4 回复

        if (lock_step_active_ && world_ != nullptr) {
            world_->pauseForTime(1);
            // ← 锁步模式下暂停仿真，等待 HIL_ACTUATOR_CONTROLS
        }
    }
}

关键设计：fields_updated 字段是一个位掩码，告诉 PX4 哪些传感器数据是有效的。复位时最高位（bit 31）置 1，PX4 会重新初始化 EKF。

四、锁步同步机制：为什么需要以及如何实现

4.1 问题背景

默认情况下，AirSim 和 PX4 以各自的速率异步运行：

  • AirSim 以渲染帧率（~60-120 Hz）更新物理
  • PX4 以固定的 250 Hz 运行飞控循环

这导致一个问题：AirSim 的时间可能比 PX4 快。如果 AirSim 跑完 1 秒物理仿真只需要 0.1 秒真实时间，PX4 只接收到了 25 个传感器包而非 250 个——飞控逻辑的时基被打乱。

4.2 锁步实现

锁步的核心思想：AirSim 等待 PX4 的回复再推进仿真时间。

1
2
3
4
5
6
7
8
9
10
11
12
13
// MavLinkMultirotorApi.hpp: 行 1611-1617（接收 HIL_ACTUATOR_CONTROLS 时）
void handleHilActuatorControls()
{
    received_actuator_controls_ = true;

    if (!lock_step_active_ && lock_step_enabled_) {
        // 锁步尚未激活但已启用
        // 检查：PX4 的回执时间戳必须与上次发送的 HIL_SENSOR 时间戳完全一致
        if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) {
            lock_step_active_ = true;  // ← 时间戳匹配，锁步激活！
        }
    }
}

锁步激活的判断条件非常严格：PX4 返回的 HIL_ACTUATOR_CONTROLS.time_usec 必须等于 AirSim 发送的 HIL_SENSOR.time_usec。这保证了 PX4 确实是在响应这一帧的传感器数据。

4.3 时间管理

锁步模式下，getSimTime() 返回冻结时间：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
// MavLinkMultirotorApi.hpp: 行 106-123
unsigned long long getSimTime()
{
    if (lock_step_active_) {
        if (sim_time_us_ == 0) {
            sim_time_us_ = clock()->nowNanos() / 1000;
        }
        return sim_time_us_;  // ← 锁步下不更新，保持冻结
    }
    else {
        return clock()->nowNanos() / 1000;  // 正常模式：返回真实时间
    }
}

void advanceTime()
{
    sim_time_us_ = clock()->nowNanos() / 1000;  // ← 只在收到控制指令后推进
}

每次传感器发送后，advanceTime() 在收到 HIL_ACTUATOR_CONTROLS 后才被调用，确保仿真时间与 PX4 的计算进度严格同步。

4.4 锁步配置

在 settings.json 中启用：

1
2
3
4
5
6
7
8
9
10
11
12
{
    "Vehicles": {
        "PX4": {
            "VehicleType": "PX4",
            "LockStep": true,
            "SitlIp": "127.0.0.1",
            "SitlPort": 4560,
            "ControlIp": "127.0.0.1",
            "ControlPortLocal": 14540
        }
    }
}

注意：LockStep 不是 settings.json 中直接读取的字段，而是通过 connection_info.lock_step 传递——这个值从 MavLinkConnectionInfo 结构体中读取，在 PX4 配置下默认为 true。

五、执行器回路：PX4 指令如何驱动旋翼

5.1 PX4 侧：从传感器到电机指令

PX4 内部的飞控管线（简化）：

1
2
3
4
5
6
7
HIL_SENSOR → sensors/vehicle_imu → EKF2 (状态估计)
                                          ↓
          mc_pos_control (位置控制) → mc_vel_control (速度控制)
                                          ↓
          mc_att_control (姿态控制) → mc_rate_control (角速度控制)
                                          ↓
          mixer (混控器) → HIL_ACTUATOR_CONTROLS → 发回 AirSim

PX4 通过 HIL_ACTUATOR_CONTROLS 消息将 8 个通道的控制量发回 AirSim。对于四旋翼，前 4 个通道分别对应 4 个电机。

5.2 AirSim 侧：接收并转发到旋翼

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
// MavLinkMultirotorApi.hpp: 行 1688-1702
else if (msg.msgid == HilActuatorControlsMessage.msgid) {
    HilActuatorControlsMessage.decode(msg);
    bool isarmed = (HilActuatorControlsMessage.mode & 128) != 0;

    for (int i = 0; i < RotorControlsCount; ++i) {
        if (isarmed) {
            rotor_controls_[i] = HilActuatorControlsMessage.controls[i];
        } else {
            rotor_controls_[i] = 0;  // 未解锁 → 所有电机停转
        }
    }

    if (!lock_step_active_ && lock_step_enabled_) {
        if (last_hil_sensor_time_ == HilActuatorControlsMessage.time_usec) {
            lock_step_active_ = true;
        }
    }
}

5.3 旋翼执行器：从控制信号到力和力矩

RotorActuator 是 AirSim 中最底层的物理驱动单元：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
// RotorActuator.hpp: 行 60-128
void setControlSignal(real_T control_signal)
{
    // 控制信号限制在 [0, 1]
    control_signal_filter_.setInput(
        Utils::clip(control_signal, 0.0f, 1.0f));
}

static void setOutput(Output& output, const RotorParams& params,
                      const FirstOrderFilter<real_T>& control_signal_filter,
                      RotorTurningDirection turning_direction)
{
    output.control_signal_input = control_signal_filter.getInput();
    output.control_signal_filtered = control_signal_filter.getOutput();

    // 转速 ∝ √(control_signal)
    // 依据: http://physics.stackexchange.com/a/32013/14061
    output.speed = sqrt(
        output.control_signal_filtered * params.max_speed_square);

    // 推力 ∝ control_signal（线性近似）
    output.thrust = output.control_signal_filtered * params.max_thrust;

    // 扭矩 ∝ control_signal × 旋转方向
    output.torque_scaler = output.control_signal_filtered
                         * params.max_torque
                         * static_cast<int>(turning_direction);
}

力/力矩施加到物理体：

1
2
3
4
5
6
7
8
// RotorActuator.hpp: 行 110-116
virtual void setWrench(Wrench& wrench) override
{
    Vector3r normal = getNormal();
    // 力和力矩与空气密度成正比
    wrench.force = normal * output_.thrust * air_density_ratio_;
    wrench.torque = normal * output_.torque_scaler * air_density_ratio_;
}

5.4 完整的力/力矩计算链

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
PX4 HIL_ACTUATOR_CONTROLS (0~1)
   ↓
rotor_controls_[i]  (8 通道)
   ↓
setControlSignal(rotor_controls_[i])
   ↓
一阶低通滤波: control_signal_filter_.update()
   ↓
thrust  = control_signal_filtered × max_thrust
torque  = control_signal_filtered × max_torque × direction
speed   = √(control_signal_filtered × max_speed²)
   ↓
wrench.force  = normal × thrust × (ρ/ρ₀)    ← 空气密度修正
wrench.torque = normal × torque × (ρ/ρ₀)
   ↓
PhysicsBody 积分 → 新的位置/姿态/速度

六、四旋翼 QuadX 机架物理参数

MultiRotorParams::setupFrameGenericQuad() 配置标准 X 型四旋翼：

参数 典型值 说明
mass 1.0 kg 总质量
rotor_count 4 电机数量
max_thrust 24.5 N 单电机最大推力（≈2.5 kg）
max_torque 1.117 N·m 单电机最大扭矩
max_speed_square 1.21e6 (rad/s)² 最大转速平方
arm_lengths[4] [0.2275, 0.2275, …] m 各臂长
rotor_z 0.02 m 旋翼平面与重心的 z 偏移
linear_drag_coefficient 0.325 线阻力系数
angular_drag_coefficient 0.325 角阻力系数

电机排列（QuadX，俯视图）：

1
2
3
4
5
6
7
    前(0°)
     ↑
CW[3]│CCW[0]
     │
─────┼─────→ 右(90°)
     │
CCW[2]│CW[1]
  • 电机 0：右前，CCW（逆时针）
  • 电机 1：右后，CW（顺时针）
  • 电机 2：左后，CCW
  • 电机 3：左前，CW

这个排列确保相邻电机转向相反，偏航力矩自动抵消。

七、完整启动与仿真时序

总结一次完整的 SITL 启动和运行流程：

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
Step 1: 启动 PX4 SITL
  $ ./run_airsim_sitl.sh
  → PX4 在 UDP 4560 监听

Step 2: 启动 AirSim (Unreal/Unity)
  → 读取 settings.json
  → MultiRotorParamsFactory::createConfig("PX4")
  → Px4MultiRotorParams::createMultirotorApi()
  → MavLinkMultirotorApi::initialize()
      → openAllConnections()
      → 连接到 UDP 4560 (数据通道)
      → 连接到 UDP 14580 (控制通道)

Step 3: 心跳握手
  AirSim → HEARTBEAT → PX4
  PX4   → HEARTBEAT → AirSim (got_first_heartbeat_ = true)

Step 4: 进入仿真循环（每帧）
  for each frame:
    ① PhysicsBody 更新位姿
    ② 传感器从物理真值生成读数
    ③ MavLinkMultirotorApi::update()
       - 发送 HIL_SENSOR (accel, gyro, mag, baro)
       - 发送 HIL_GPS (位置, 速度)
       - 发送 SYSTEM_TIME
       - wait for HIL_ACTUATOR_CONTROLS (锁步)
    ④ PX4 接收传感器 → EKF → 控制 → 混控
    ⑤ PX4 发送 HIL_ACTUATOR_CONTROLS
    ⑥ rotor_controls_[0..3] 更新
    ⑦ RotorActuator::setControlSignal()
    ⑧ 旋翼力/力矩施加到 PhysicsBody
    ⑨ 回到 ①

八、与 simple_flight 模式的对比

AirSim 还内置了一个轻量飞控 simple_flight：

特性 PX4 SITL simple_flight
飞控来源 外部 PX4 进程 AirSim 内置 C++
控制算法 EKF + 级联 PID（生产级） 基础角度/速度 PID
通信方式 MAVLink UDP 内存直连（无序列化）
仿真速率 锁步同步可选 帧率同步
传感器噪声 AirSim 注入到 PX4 AirSim 内部闭环
适用场景 算法验证、真实飞控测试 快速原型、视觉算法测试

simple_flight 通过 SimpleFlightQuadXParams 创建，不需要外部 PX4，适合快速验证。

九、实践建议

9.1 调试传感器数据

启用 MAVLink 日志可记录所有通信：

1
2
3
4
5
6
7
{
    "Vehicles": {
        "PX4": {
            "Logs": "~/Documents/AirSim/logs"
        }
    }
}

日志会保存完整的 MAVLink 二进制流，可用 mavlogdump.py 解析。

9.2 修改机架参数

如需定制四旋翼（如更大的无人机），修改 settings.json 中的 Model 字段，或在 Px4MultiRotorParams 中添加新的机架函数。

9.3 锁步调试

若仿真出现时间异常，检查：

  1. lock_step_resets_ 计数器——频繁重置说明 PX4 发送超时
  2. PX4 的 HIL_MODE 是否已设置为启用
  3. HIL_ACTUATOR_CONTROLS.time_usec 是否匹配发送的 HIL_SENSOR.time_usec

小结

本文从 AirSim 源码出发，完整梳理了 PX4 SITL 四旋翼仿真的闭环链路：

  1. 启动：settings.json → MultiRotorParamsFactory → Px4MultiRotorParams → MavLinkMultirotorApi
  2. 传感器注入：MavLinkMultirotorApi::update() → sendHILSensor() / sendHILGps() → PX4
  3. 锁步同步：lock_step_active_ 冻结仿真时间，等待 PX4 的 HIL_ACTUATOR_CONTROLS
  4. 执行器回路：PX4 返回电机控制量 → rotor_controls_[] → RotorActuator → 物理力/力矩
  5. 闭环：物理仿真产生新的位姿，下一帧传感器读取，循环往复

理解这个闭环对于自定义仿真场景、调试飞控算法、以及评估 Sim-to-Real 差距至关重要。

参考：