四足机械狗/人形机器人实时软件设计规则

前言人形机器人软件系统是一个对实时性要求较高的软件系统。它包含低层级运动闭环、运动规划与行为决策、环境感知、人机交互等任务。在这些任务中有些任务对实时性要求较高例如EtherCAT 主站、力矩闭环。有些任务对实时性要求不高例如环境感知、人机交互任务。本文将从PREEMPT_RT Linux、操作系统调度策略、CPU内核隔离、内存锁定、进程亲和性绑定方面进行阐述详细讲解如何构建一个既能满足各任务实时性需求又能满足功能多样性需求的四足机械狗/人形机器人软件系统。一、什么是 PREEMPT_RTPREEMPT_RTReal-Time Preemption俗称 RT 补丁是一套对 Linux 内核进行大规模改造的补丁集。其目标是把标准 Linux 从软实时改造为具备确定性低延迟的实时 Linux让内核几乎全部代码路径支持抢占。PREEMPT_RT 四大核心改造如下。1. 自旋锁改造spinlock → rtmutex可睡眠锁普通 spinlock_t 在 PREEMPT_RT 下不再忙等、不再关闭抢占内部基于rtmutex实时互斥锁实现争锁失败时线程主动让出 CPU自带优先级继承PI缓解优先级反转极短底层同步代码保留 raw_spinlock_t真正不可抢占。2. 中断线程化Threaded IRQs硬件中断只保留极简上半部ack 中断绝大多数中断处理逻辑放到独立内核线程运行。这样做的好处有中断线程拥有优先级可以被更高优先级实时任务抢占不再出现 “中断长时间霸占 CPU” 阻塞实时任务。3. 最大化内核抢占点普通 CONFIG_PREEMPT遇到 spinlock 临界区就不能抢占。PREEMPT_RT几乎所有内核代码都允许抢占只有极少数极短底层代码路径禁止抢占。4. 增强高精度定时器 hrtimer为周期性控制任务伺服、运动控制提供稳定纳秒级定时能力。二、Linux 四种抢占模式对比配置名称实时能力典型最大延迟适用场景PREEMPT_NONE无抢占最差数 ms~ 几十 ms服务器、高性能计算PREEMPT_VOLUNTARY自愿抢占一般~2ms桌面发行版默认PREEMPT可抢占内核低延迟中等几百 μs~1ms普通低延迟嵌入式PREEMPT_RT实时抢占内核最优几十 μs优化后 50μs工业控制、机器人、伺服、音频三、SCHED_FIFO 与 SCHED_RR调度策略SCHED_FIFO 与 SCHED_RR都属于Linux实时调度策略这两种实时策略能不能用要看你的 Linux 内核有没有开启对应的抢占选项PREEMPT_RT或者CONFIG_PREEMPT。1. SCHED_FIFO先进先出(1) SCHED_FIFO规则1) 同一优先级下多个任务排队2) 一个任务拿到 CPU 后持续运行直到以下任意一种情况才让出 CPU任务主动阻塞sleep、等待信号量、I/O任务主动调用 sched_yield() 放弃 CPU有更高优先级任务就绪被抢占;3) 任务不会因为时间片用完被强制切走没有时间片概念。(2) SCHED_FIFO例子优先级90、80两个FIFO任务。90任务就绪直接抢占80任务。同优先级两个 FIFO 任务 A、BA 运行后只要不阻塞B 永远得不到调度。(3) SCHED_FIFO适用场景SCHED_FIFO适用于不希望中途被同优先级任务打断的场景。例如机器人伺服闭环、EtherCAT 主站、周期性硬实时控制等任务。(4) C代码设置SCHED_FIFO#includesched.hvoidset_realtime_fifo(intprio){structsched_paramsp;sp.sched_priorityprio;sched_setscheduler(0,SCHED_FIFO,sp);}2. SCHED_RR轮询实时Round-Robin(1) SCHED_RR规则跨优先级行为和 FIFO 完全一致高优先级随时抢占低优先级同一优先级内部加入时间片机制同优先级多个任务轮流执行任务用完时间片后放到同优先级队列尾部若任务中途阻塞再次唤醒后排在队列末尾。默认时间片由内核参数控制 /proc/sys/kernel/sched_rr_timeslice_ms默认 100ms。(2) 例子同优先级 90的RR 任务 A、BA 运行一段时间时间片耗尽 → 切换 BB 时间片耗尽切回 A如果此时来了优先级 91 任务立刻抢占 A/B。(3) 适用场景1) 场景 1多传感器同步采集系统工业检测平台设备同时采集高速相机图像、激光测距、IMU 数据三类采集线程设置相同实时优先级采集线程都需要周期性抢占普通任务没有任何一路采集拥有绝对最高权限不能允许某一路持续占用 CPU 导致其他传感器丢帧SCHED_RR 依靠时间片轮转让多路采集线程轮流执行均衡分配 CPU防止单个传感器线程长期运行阻塞其余采集任务。2) 场景 2多通道实时音频处理专业声卡、音频工作站多路音频轨道渲染、音效滤波、混音输出全部设置为同一实时优先级如果用 SCHED_FIFO最先被调度到的轨道线程会一直运行其他声道饥饿产生爆音、丢采样使用 SCHED_RR同优先级各路音频线程按时间片轮流执行保证所有声道均衡获得 CPU 时间避免单一路径独占保障多路音频同步输出。四、cpu亲和性CPU 亲和性CPU Affinity就是强制把某个线程 / 进程 “绑定到一个或多个 CPU 核心上运行”限制内核调度器不能随意把它迁移到其他核心。1. 为什么迁移 CPU 会带来延迟线程从 CPU0 切换到 CPU1 时L1/L2 缓存失效新核心缓存为空大量 Cache MissTLB 刷新跨 NUMA 节点内存访问延迟飙升引入不可预测的调度抖动。2. cpu亲和性在Linux中分为两种软亲和性调度器倾向尽量不迁移不强制硬亲和性通过 API 显式掩码绑定强制限制可运行的核心集合。3. CPU 掩码掩码每一位代表一个核心0b0001 → 只能跑在 CPU00b0101 → 可以跑在 CPU0、CPU24. C代码绑定当前线程到 CPU3cpu_set_tmask;CPU_ZERO(mask);CPU_SET(3,mask);sched_setaffinity(0,sizeof(mask),mask);五、内存锁mlock把进程虚拟内存锁定在物理内存禁止内核把这块内存交换swap到磁盘。1. 背景知识Linux 默认采用虚拟内存 按需分页Demand Paging程序启动时只有虚拟地址物理内存不一定立刻分配内存不足时内核会把不常用页面换出到 swap 磁盘程序访问尚未载入物理内存的地址 → 缺页异常Page Fault。内核要从磁盘读取页面耗时毫秒级甚至十几 ms对于微秒级实时闭环是致命延迟尖峰。实时任务最大敌人之一运行中途触发缺页故障。mlock 就是用来杜绝运行时 page fault。2. 内存锁两个系统调用(1) mlock()锁定指定一段内存区域堆、全局数组、共享内存。intmlock(constvoid*addr,size_tlen);mlock 做了两件核心事强制所有虚拟页立刻映射到物理内存主动触发缺页在初始化阶段一次性完成标记页面不可被回收、不可 swap out运行期间不会再产生磁盘 IO 类缺页。(2) mlockall()// MCL_CURRENT锁定当前已经分配的所有内存// MCL_FUTURE锁定将来新分配的内存malloc/mmapintmlockall(intflags);工程实时程序最常用mlockall(MCL_CURRENT | MCL_FUTURE)六、四足机械狗/人形机器人实时软件进程隔离规则以下隔离规则统一用主流工控平台8核CPU举例。1. 底层运动控制层隔离规则底层运动控制代表程序有EtherCAT 主站、力矩闭环、全身 WBC 控制器。这类底层运动控制程序应独占CPU3不与任何其他线程共享核心。调试策略用先进先出SCHED_FIFO调度不要使用SCHE_RR。进程启动第一时间执行 mlockall(MCL_CURRENT | MCL_FUTURE)防止运行时缺页。线程内部禁止调用阻塞IO、printf大量打印、磁盘写操作。禁止调用ROS、GPU推理等耗时不定的库。EtherCAT网卡中断IRQ手动绑定CPU3。不要把视觉、SLAM任何线程放到CPU3一旦CPU3出现计算尖峰容易导致伺服丢周期机器人摔倒。2. 运动规划层隔离规则运动规划层的代表程序有步态规划、平衡控制、状态估计等。这类程序的典型周期为200Hz~500Hz指定其运行在CPU2。优先级低于伺服闭环保证当闭环线程需要CPU时可以立刻抢占规划线程。允许做矩阵运算、QP优化WBC二次规划。规划层计算超时只会影响步态流畅性不会造成电机失控。步态规划可以和状态估计共享CPU2但是尽量拆分为独立线程区分优先级。3. 环境感知层隔离规则环境感知的代表程序有相机、激光雷达、SLAM等。全部不允许这类程序进入隔离实时核可指定这类程序运行在CPU1。图像推理、点云处理耗时抖动极大属于典型“计算时长不可预测任务”。对于这类程序一律使用SCHED_OTHER CFS 调度。如果需要略微降低延迟可少量使用SCHED_RR策略。感知结果通过共享内存或内存零拷贝传递给运动规划程序。避免让感知线程直接调用运动控制接口。4. 人机交互层隔离规则人机交互程序主要有手柄、语音对话、上位机客户端、网页控制端等。对于这类程序可指定在CPU0或CPU4中执行采用普通分时调度即可。因为它们只负责解析指令、输出目标姿态/目标速度给运动规划层。交互模块丢失几帧指令只会造成操控卡顿不影响机器人稳定。此外网络收发、语音解码存在不确定延迟不要将这类程序提升至实时优先级。七、四足机械狗/人形机器人软件进程隔离架构图