第13章：实时实现挑战

控制频率要求 vs 算法复杂度：
┌────────────┬──────────┬─────────────┐
│ 控制频率    │ 周期时间  │ 可用计算时间 │
├────────────┼──────────┼─────────────┤
│ 100 Hz     │ 10 ms    │ 5-8 ms      │
│ 500 Hz     │ 2 ms     │ 1-1.5 ms    │
│ 1 kHz      │ 1 ms     │ 0.5-0.8 ms  │
│ 2 kHz      │ 0.5 ms   │ 0.2-0.4 ms  │
└────────────┴──────────┴─────────────┘

质量矩阵稀疏模式（7自由度机械臂）：
M = [* * * * . . .]  * 表示非零元素
    [* * * * * . .]  . 表示零元素
    [* * * * * * .]
    [* * * * * * *]
    [. * * * * * *]
    [. . * * * * *]
    [. . . * * * *]

# 伪代码：MPC热启动
X_init[0:N-1] = X_prev[1:N]  # 时间平移
X_init[N-1] = X_prev[N-1]    # 复制最后状态
U_init[0:N-2] = U_prev[1:N-1]  # 控制平移
U_init[N-2] = U_prev[N-2]      # 复制最后控制

误差 vs 迭代次数（典型QP求解器）：
迭代  相对误差   计算时间(μs)  误差减少比
1     1e-1      50           -
2     1e-2      100          10x
3     1e-3      150          10x
4     1e-4      200          10x
5     1e-5      250          10x
10    1e-10     500          1e5x

while (elapsed_time < time_budget && !converged) {
    iterate();
    if (residual < tolerance) converged = true;
}

摩擦力模型查找表（Stribeck效应）：
速度(m/s)  摩擦系数   斜率(导数)
0.00       0.15      -3.0     (静摩擦)
0.01       0.12      -2.0     (过渡区)
0.05       0.10      -0.5
0.10       0.09      -0.1     (动摩擦)
0.50       0.085     -0.01    (粘性区)
...

template<int N>  // N = 表格大小
class LookupTable {
    float values[N];
    float slopes[N];  // 存储导数以提供平滑插值
    float x_min, x_max, dx;

    float interpolate(float x) {
        int idx = (x - x_min) / dx;
        float t = (x - x_min) / dx - idx;
        // Hermite插值，保证C1连续
        return values[idx] * (1 - t) + values[idx+1] * t +
               slopes[idx] * t * (1 - t) * dx;
    }
};

// 避免
double* matrix = malloc(n * n * sizeof(double));

// 推荐
#define MAX_DOF 30
static double matrix[MAX_DOF * MAX_DOF];

内存池布局示例：
┌──────────────────┐
│ 控制器工作区     │ 16KB
├──────────────────┤
│ 动力学缓存       │ 8KB
├──────────────────┤
│ QP求解器空间     │ 32KB
├──────────────────┤
│ 传感器缓冲区     │ 4KB
├──────────────────┤
│ 通信缓冲区       │ 4KB
└──────────────────┘
总计：64KB SRAM

// 32字节对齐（典型缓存行大小）
struct __attribute__((aligned(32))) RobotState {
    double q[7];        // 56字节
    double dq[7];       // 56字节
    double tau[7];      // 56字节
    double padding[3];  // 24字节填充
};  // 总计：192字节 = 6个缓存行

// Q15.16格式的定点乘法
int32_t fixed_mul(int32_t a, int32_t b) {
    int64_t result = ((int64_t)a * b) >> 16;
    return (int32_t)result;
}

// 定点三角函数（查找表+插值）
int32_t fixed_sin(int32_t angle) {
    // angle in Q15.16, 表示弧度
    int index = (angle >> 10) & 0x3FF;  // 10位索引
    int32_t y0 = sin_table[index];
    int32_t y1 = sin_table[index + 1];
    int frac = angle & 0x3FF;
    return y0 + ((y1 - y0) * frac >> 10);
}

任务优先级分配（四足机器人示例）：
┌──────────────┬──────┬────────┬──────────┬───────────┐
│ 任务          │ 周期 │ 优先级 │ 执行时间 │ CPU占用率 │
├──────────────┼──────┼────────┼──────────┼───────────┤
│ 传感器采集    │ 1ms  │ 最高(1)│ 0.1ms    │ 10%       │
│ 状态估计      │ 1ms  │ 高(2)  │ 0.2ms    │ 20%       │
│ 控制器计算    │ 1ms  │ 高(3)  │ 0.4ms    │ 40%       │
│ 通信处理      │ 5ms  │ 中(4)  │ 0.5ms    │ 10%       │
│ 轨迹规划      │ 10ms │ 低(5)  │ 2ms      │ 20%       │
└──────────────┴──────┴────────┴──────────┴───────────┘
                                      总计：100%

// 使用事件标志避免阻塞
struct TaskSync {
    std::atomic<bool> sensor_ready{false};
    std::atomic<bool> state_ready{false};

    void sensor_task() {
        read_sensors();
        sensor_ready.store(true, std::memory_order_release);
    }

    void state_task() {
        while (!sensor_ready.load(std::memory_order_acquire))
            std::this_thread::yield();
        estimate_state();
        state_ready.store(true, std::memory_order_release);
    }
};

// 中断服务程序（ISR）
void __attribute__((interrupt)) timer_isr() {
    // 最小化ISR中的工作
    sensor_timestamp = get_system_time();
    set_flag(SENSOR_READY);  // 通知主循环
    // 实际处理在主循环中进行
}

控制循环时序分析：
         0μs                500μs              1000μs
传感器   |===|
状态估计      |======|
控制计算             |===========|
电机输出                         |==|
         ├───────────────────────────┤
                    1ms周期

四核任务分配优化示例：
┌───────────────────────────────────────────────────┐
│ 核心0：传感器融合 + 状态估计 (L1缓存共享)     │
│ 核心1：动力学计算 + 接触检测 (L2缓存共享)      │
│ 核心2：QP求解器主循环 (内存密集)              │
│ 核心3：轨迹规划 + 通信处理 (相对独立)        │
└───────────────────────────────────────────────────┘

// 将任务绑定到特定核心
void set_cpu_affinity(std::thread& t, int cpu_id) {
    cpu_set_t cpuset;
    CPU_ZERO(&cpuset);
    CPU_SET(cpu_id, &cpuset);
    pthread_setaffinity_np(t.native_handle(), 
                          sizeof(cpu_set_t), &cpuset);
}

// 使用AVX2指令集的矩阵乘法（256位宽向量）
void matrix_mult_avx(float* C, float* A, float* B, int n) {
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j += 8) {  // 8个float并行
            __m256 sum = _mm256_setzero_ps();
            for (int k = 0; k < n; k++) {
                __m256 a = _mm256_broadcast_ss(&A[i*n + k]);
                __m256 b = _mm256_loadu_ps(&B[k*n + j]);
                sum = _mm256_fmadd_ps(a, b, sum);  // FMA: a*b+sum
            }
            _mm256_storeu_ps(&C[i*n + j], sum);
        }
    }
}

// AVX-512进一步提升（512位宽，16个float）
void matrix_mult_avx512(float* C, float* A, float* B, int n) {
    for (int i = 0; i < n; i++) {
        for (int j = 0; j < n; j += 16) {  // 16个float并行
            __m512 sum = _mm512_setzero_ps();
            for (int k = 0; k < n; k++) {
                __m512 a = _mm512_set1_ps(A[i*n + k]);
                __m512 b = _mm512_loadu_ps(&B[k*n + j]);
                sum = _mm512_fmadd_ps(a, b, sum);
            }
            _mm512_storeu_ps(&C[i*n + j], sum);
        }
    }
}

指令集   宽度   float并行度  理论加速  实际加速
SSE     128bit    4          4x       2-3x
AVX     256bit    8          8x       4-6x
AVX-512 512bit    16         16x      8-12x
ARM NEON 128bit   4          4x       2-3x

# GCC/Clang
-O3 -march=native -mavx2 -mfma -ftree-vectorize

# Intel Compiler
-O3 -xHost -qopt-report=5

// CUDA核函数：优化的矩阵向量乘法（使用共享内存）
__global__ void matvec_kernel_optimized(float* y, float* A, float* x, 
                                        int m, int n) {
    extern __shared__ float s_x[];  // 共享内存存储x向量

    int tid = threadIdx.x;
    int row = blockIdx.x * blockDim.x + tid;

    // 协作加载x到共享内存
    for (int i = tid; i < n; i += blockDim.x) {
        s_x[i] = x[i];
    }
    __syncthreads();

    if (row < m) {
        float sum = 0.0f;
        // 从共享内存读取，避免全局内存访问
        for (int j = 0; j < n; j++) {
            sum += A[row * n + j] * s_x[j];
        }
        y[row] = sum;
    }
}

内存类型      带宽(GB/s)  延迟(cycles)  大小
寄存器        8000+       0            32KB/SM
共享内存      4000+       ~30          96KB/SM
L1缓存        2000+       ~100         128KB/SM
L2缓存        1000+       ~200         6MB
全局内存      900         ~400         16-48GB

运算类型        CPU时间   GPU时间   加速比  带宽利用率
质量矩阵求逆    5.2ms    0.3ms    17x    65%
QP梯度计算      2.1ms    0.15ms   14x    70%
约束投影        1.8ms    0.12ms   15x    60%
批量MPC(x10)   120ms    3.5ms    34x    85%

// 批量动力学前向积分
__global__ void batch_dynamics(State* states, Control* controls, 
                               int batch_size, float dt) {
    int idx = blockIdx.x * blockDim.x + threadIdx.x;
    if (idx < batch_size) {
        // 每个线程处理一个轨迹点
        State s = states[idx];
        Control u = controls[idx];
        // 动力学积分
        s.x += dt * s.vx;
        s.y += dt * s.vy;
        s.vx += dt * (u.fx / mass);
        s.vy += dt * (u.fy / mass - gravity);
        states[idx + 1] = s;
    }
}

// Verilog示例：定点PID控制器
module pid_controller #(
    parameter WIDTH = 32,
    parameter FRAC = 16
)(
    input clk,
    input signed [WIDTH-1:0] error,
    output signed [WIDTH-1:0] control
);
    // 流水线级1：比例项
    reg signed [WIDTH-1:0] p_term;
    always @(posedge clk)
        p_term <= (Kp * error) >>> FRAC;

    // 流水线级2：积分项
    reg signed [WIDTH-1:0] integral;
    reg signed [WIDTH-1:0] i_term;
    always @(posedge clk) begin
        integral <= integral + error;
        i_term <= (Ki * integral) >>> FRAC;
    end

    // 流水线级3：微分项
    reg signed [WIDTH-1:0] prev_error;
    reg signed [WIDTH-1:0] d_term;
    always @(posedge clk) begin
        d_term <= (Kd * (error - prev_error)) >>> FRAC;
        prev_error <= error;
    end

    // 流水线级4：输出
    always @(posedge clk)
        control <= p_term + i_term + d_term;
endmodule

时钟周期  1   2   3   4   5   6   7   8
样本1    P1  I1  D1  S1
样本2        P2  I2  D2  S2
样本3            P3  I3  D3  S3
样本4                P4  I4  D4  S4

P=比例 I=积分 D=微分 S=求和
延迟：4周期，吞吐量：1样本/周期

TPU风格的脉动阵列：
     B0  B1  B2  B3
    ┌──┬──┬──┬──┐
A0──┤  │  │  │  │
    ├──┼──┼──┼──┤
A1──┤  │  │  │  │
    ├──┼──┼──┼──┤
A2──┤  │  │  │  │
    ├──┼──┼──┼──┤
A3──┤  │  │  │  │
    └──┴──┴──┴──┘
     C0  C1  C2  C3

每个单元：Cij += Ai * Bj

传感器输入 ──> 状态估计 ──> WBC优化 ──> 电机指令
   1kHz         1kHz         1kHz        1kHz
    ↑                          ↓
IMU/编码器                 扭矩指令

class WBC_1kHz {
private:
    // 预分配的内存
    Eigen::Matrix<double, 18, 18> H_;
    Eigen::Matrix<double, 18, 1> g_;
    Eigen::Matrix<double, 50, 18> A_;
    Eigen::Matrix<double, 50, 1> b_;

    // QP求解器（使用qpOASES）
    qpOASES::QProblem qp_solver_;

public:
    void update(const RobotState& state, 
               const ControlCommand& cmd) {
        // 1. 构建QP矩阵 (0.15ms)
        buildQPMatrices(state, cmd);

        // 2. 求解QP (0.3ms)
        qp_solver_.hotstart(H_.data(), g_.data(), 
                          A_.data(), nullptr, nullptr,
                          b_.data(), nullptr);

        // 3. 提取解 (0.05ms)
        extractSolution();
    }
};

1kHz控制循环时间分解：
┌─────────────────┬────────┬─────────┐
│ 任务             │ 时间   │ 占比    │
├─────────────────┼────────┼─────────┤
│ 传感器读取       │ 0.05ms │ 5%      │
│ 状态估计         │ 0.15ms │ 15%     │
│ 参考轨迹更新     │ 0.10ms │ 10%     │
│ QP矩阵构建       │ 0.15ms │ 15%     │
│ QP求解          │ 0.30ms │ 30%     │
│ 关节映射        │ 0.10ms │ 10%     │
│ 电机指令发送     │ 0.05ms │ 5%      │
│ 安全检查        │ 0.05ms │ 5%      │
│ 预留余量        │ 0.05ms │ 5%      │
├─────────────────┼────────┼─────────┤
│ 总计            │ 1.00ms │ 100%    │
└─────────────────┴────────┴─────────┘

// 利用对称性，只计算上三角
for (int i = 0; i < 18; ++i) {
    for (int j = i; j < 18; ++j) {
        H_(i,j) = computeHessian(i, j);
        if (i != j) H_(j,i) = H_(i,j);  // 对称
    }
}

// 结构体数组 -> 数组结构体
struct LegData {
    double J[3][3];     // 雅可比
    double f[3];        // 接触力
    bool contact;       // 接触状态
} __attribute__((aligned(64)));  // 缓存行对齐

LegData legs[4];

// 使用Intel Intrinsics加速向量运算
void computeAcceleration(double* acc, const double* M_inv, 
                         const double* forces, int n) {
    __m256d sum = _mm256_setzero_pd();
    for (int i = 0; i < n; i += 4) {
        __m256d m = _mm256_load_pd(&M_inv[i]);
        __m256d f = _mm256_load_pd(&forces[i]);
        sum = _mm256_fmadd_pd(m, f, sum);
    }
    _mm256_store_pd(acc, sum);
}

控制周期统计（100,000个样本）：
最小值：0.72ms
平均值：0.85ms
中位数：0.84ms
最大值：0.98ms
标准差：0.04ms
99%分位：0.95ms

抖动分析：
< 10μs：85%
10-50μs：12%
50-100μs：2.5%
> 100μs：0.5%

CPU使用率（Intel i7-9700K @ 4.9GHz）：
核心0：85%（主控制循环）
核心1：45%（传感器处理）
核心2：30%（轨迹规划）
核心3：20%（通信）
核心4-7：<10%（系统任务）

内存使用：
堆栈：8KB
堆：64KB（预分配）
代码段：256KB
总计：<512KB

跟踪误差（trot步态，1.5m/s）：
         RMS误差   最大误差
位置X    12mm     35mm
位置Y    8mm      22mm
位置Z    5mm      15mm
姿态Roll  1.2°    3.5°
姿态Pitch 1.5°    4.0°
姿态Yaw   2.0°    5.5°

计算时间 vs 自由度：
DOF   QP规模   求解时间   可达频率
12    18×50   0.3ms     2000Hz
18    24×80   0.6ms     1000Hz
30    36×140  1.5ms     500Hz
50    56×240  4.0ms     200Hz

分层控制架构：
高层规划(10Hz) ─> 轨迹生成(100Hz) ─> WBC(1kHz) ─> 电机控制(10kHz)
     ↑                ↑                   ↑              ↑
  任务指令        参考轨迹           关节指令       电流控制

分布式控制节点：
┌────────────┐  高速总线  ┌────────────┐
│ 上身控制器  │ ←────────→ │ 下身控制器  │
│ (20 DOF)   │            │ (12 DOF)   │
└────────────┘            └────────────┘
      ↑                          ↑
┌────────────┐            ┌────────────┐
│ 左臂控制器  │            │ 右臂控制器  │
│ (7 DOF)    │            │ (7 DOF)    │
└────────────┘            └────────────┘

通信延迟：<100μs（EtherCAT）
同步精度：<1μs（IEEE 1588）

void control_loop() {
    Eigen::MatrixXd H = Eigen::MatrixXd::Zero(n, n);  // 动态分配！
    std::vector<double> forces;  // 可能触发重分配！
    forces.reserve(12);  // reserve不等于resize
}

// 编译时确定大小
Eigen::Matrix<double, 18, 18> H;
std::array<double, 12> forces;  // 栈上分配

if (contact_force == 0.0) {  // 危险！
    // 假设无接触
}

const double EPSILON = 1e-6;
if (std::abs(contact_force) < EPSILON) {
    // 安全的零值判断
}

// 列优先访问行主序矩阵
for (int j = 0; j < n; j++)
    for (int i = 0; i < n; i++)
        sum += matrix[i][j];  // 缓存未命中！

// 行优先访问
for (int i = 0; i < n; i++)
    for (int j = 0; j < n; j++)
        sum += matrix[i][j];  // 连续内存访问

// 使用优先级继承互斥锁
pthread_mutexattr_t attr;
pthread_mutexattr_setprotocol(&attr, PTHREAD_PRIO_INHERIT);
pthread_mutex_init(&mutex, &attr);

// 矩阵求逆without正则化
Eigen::MatrixXd M_inv = M.inverse();  // 接近奇异时崩溃

// 添加正则化项
const double reg = 1e-6;
Eigen::MatrixXd M_reg = M + reg * Eigen::MatrixXd::Identity(n, n);
Eigen::MatrixXd M_inv = M_reg.inverse();
// 或使用伪逆
Eigen::MatrixXd M_pinv = M.completeOrthogonalDecomposition().pseudoInverse();

if (qp_iterations > 10) {
    // 热启动可能失败，切换冷启动
    qp_solver.reset();
    qp_solver.init(...);
}

void control_callback() {
    auto start = high_resolution_clock::now();

    // 控制计算...

    auto end = high_resolution_clock::now();
    auto duration = duration_cast<microseconds>(end - start);

    if (duration.count() > 900) {  // 90%周期时间
        // 记录警告
        missed_deadlines++;
        // 降级到安全模式
        if (missed_deadlines > 10) {
            enter_safe_mode();
        }
    }
}

// Q15.16乘法可能溢出
int32_t result = (a * b) >> 16;  // 如果a*b > 2^31则溢出

int32_t safe_fixed_mul(int32_t a, int32_t b) {
    int64_t temp = ((int64_t)a * b) >> 16;
    // 饱和处理
    if (temp > INT32_MAX) return INT32_MAX;
    if (temp < INT32_MIN) return INT32_MIN;
    return (int32_t)temp;
}

// 版本A
for(i=0; i<100; i++)
  for(j=0; j<100; j++)
    for(k=0; k<100; k++)
      C[i][j] += A[i][k] * B[k][j];

// 版本B  
for(i=0; i<100; i++)
  for(k=0; k<100; k++)
    for(j=0; j<100; j++)
      C[i][j] += A[i][k] * B[k][j];