一种基于RTX 的移动机器人实时控制平台

刘溢 阳加远 张驰

（中国航天科技集团有限公司第一研究院战术武器事业部 北京市 100076）

移动机器人包括轮式、足式、复合式。轮式移动机器人能够在结构化地形中执行产品自动转运任务，具有导航定位精确和承载能力强的特点，可广泛应用于仓库、码头、工厂等物联场所。足式机器人由于腿部与地面的接触是非连续的，且多条腿之间的协调运动可产生多种步态，所以相比于一般移动机器人，具有更好的机动性和复杂地形的适应性，可以广泛运用于影视娱乐、反恐、军事运输、抢险、救灾等领域。复合式则结合轮式、足式机器人特点，可以切换使用模式，能够广泛适应不同的场景工况。移动机器人自主移动关键环节包括：环境识别与建模、路径规划、实时轨迹跟踪控制。其中环境识别与建模、路径规划是控制目标的建模和解算环节，主要由上位管控机完成，机器人控制器作为下位机则需保证一定的算力对控制目标进行解析和分解。实时轨迹控制是机器人控制器的核心任务，常常需面向模糊控制、滑膜控制、神经网络PID 控制等智能控制算法解算，运算量大、开放性要求高、实时约束强，急需开发一种移动机器人控制平台，拓展性能和开放性的同时，保证控制的实时性。

1 移动机器人控制平台硬件系统搭建

基于工控机的控制系统（PC-Based 系统）被广泛应用于现代控制工程实践中。相比嵌入式系统，PC-Based 系统硬件运算能力强，开放性水平高，兼容性好，便于拓展、维护。PC-Based 系统以可靠性高的工控机(即IPC)为控制器，拓展外接DQA 数据板卡，以实时操作系统为编程环境，开发控制软件，并在系统上实时运行编译的控制程序，实现数据采集和控制，达到控制复杂设备的目的。另外，PCI 板卡具有即插即用的特性，且其商业化产品技术成熟，故选取PCI 板卡作为CPU 外设拓展具有强的工程意义。综上，本平台选型为基于IPC+PCI 板卡的PC-Based 系统，以保障卓越的运算能力和开放性。

移动机器人控制平台完成选型后，可得出机器人实时控制系统总体结构如图1 所示。其中IPC+PCI 板卡的移动机器人控制平台负责参数设定、路径解析、反馈量A/D 转换、实时控制运算及驱动量D/A 转换，是控制系统的核心。中间层由放大器、驱动装置（液压为伺服阀，电动为电机驱动器）及滤波器组成，其中放大器、驱动装置串接实现控制量从电压到物理量的转换，滤波器对反馈回路的高频噪声进行过滤。执行层为分布式执行模块，主要为各自由度作动器及其传感器，以及其他运动状态传感器，负责实现机器人运动和相关数据采集。分层式的控制系统硬件结构层次清晰，便于维护和拓展。

图1： 移动机器人实时控制系统总体结构简图

2 移动机器人控制平台实时系统环境搭建

运算速度快并不是实时系统最突出特性，时间确定性才更核心，即在规定时间内，必须处理完实时事件，称为系统的硬实时性。在移动机器人多自由度并行控制系统中，实时性可以保证多通道控制链路在系统级环境下运行的独立性与抗干扰性，是并行运算控制的基础。

工控机必须运行实时操作系统，才能保证基础环境的实时性。RTX 基于 Windows 操作系统，修改并扩展了整个硬件抽象层，实现了独立的实时内核驱动模式。相较其他实时系统，RTX 最吸引人的地方是其与Windows 的平台融合，在保证实时性的同时，可大幅提高开发、调试效率。通过安装RTX 插件就可以赋予Windows 硬实时功能，而无需专设一台实时PC 及相关通信外设，能在节省成本的同时，减轻系统复杂性。故选择搭载RTX 插件的Windows 为控制平台操作系统。

单纯地创建实时程序，不能满足实时监控的人机交互需求，无法发挥出Window 基础系统环境的优越性。在一个工程中同时创建MFC 和RTX 应用程序，具体流程如下：先在VS 包含文件及库文件夹中加载RTX 的链接库，再分别创建RXT Aplication 和MFC 工程实例，最后在MFC 中包含RTX头文件。

3 移动机器人多自由度实时并行控制软件设计

3.1 板卡实时驱动的开发

通常桥路芯片定位寄存器即插即配，只要安装了相关的底层驱动，包括地址设定、总线号分配、中断向量初始等在内的初始化工作，由主板程序自动完成。本文所述的板卡实时驱动开发，是在底层驱动安装到RTX 环境基础上，开发PCI 外设操控驱动，实现对PCI 数据偏移寄存器操控，而与设备内部寄存器交互是通过PCI 总线实现，所以所述驱动便是操控设备内部寄存器的程序。

3.1.1 RTX 下PCI 基地址获取

对于设备寄存器的操控，首先要获得设备由主板程序分配的基地址，再根据板卡寄存器结构，编程操控寄存器。PCI 板卡的每个寄存器在I/O 空间的地址都被指定为卡的基地址的偏移。所以先调用RtGetBusDataByOffset()根据设备号、厂商号搜寻设备并返回其基地址，再给予基地址调用RtReadPortUchar()、RtWritePortUchar()等函数读写PCI 板卡寄存器，编制板卡驱动API。

3.1.2 RTX 下PCI 模拟量采集卡寄存器操作

PCI 模拟量采集卡一般集成A/D 数据转换采集、计数/定时功能，如研华的PCI1710、PCI1713 等。本文以研华PCI1710 板卡为例，分析给出PCI 模拟量采集卡寄存器操作方法。

3.1.2.1 A/D 数据寄存器操作

A/D 数据寄存器主要用于对模拟量数据进行A/D 转换寄存。对该寄存器的操作主要涉及通道编号读取和寄存器数字量转模拟量读取。PCI1710 板卡A/D 数据采集分辨率12 位，A/D 数据寄存器为2 个字节容量，其通道编号寄存于A/D 数据寄存器BASE+1 （表示基地址+1 字节，下同）的字节高4 位，A/D 数据则寄存于A/D 数据寄存器BASE+1 的字节低4 位和BASE+0 的字节全8 位。使用RtReadPortUshort() 函数读取特定地址的数据，再根据位定义进行运算可分离得出通道编号和寄存器A/D 数据。

A/D 数据采集转换公式：

读取的数据为寄存器当前数据，为避免单个数据扰动影响，一般通过多个变量将最近的寄存数据存储到PC 中，以便采样取平均值。

3.1.2.2 通道范围设置寄存器操作

通过写入板载RAM 增益码控制每个A/D 通道的输入范围。对于每个通道增益码，可分别进行变更：首先将目标通道编号同时写进BASE+4 和BASE+5 ，然后将增益码写入BASE+2 的位0 ～位2。增益码值与通道范围的对应关系通过通道增益设置表进行规定，可根据实际需求进行写入操作。

3.1.2.3 控制寄存器操作

A/D 中断、触发源设置通过向寄存器BASE+6 和BASE+7 中写入数值实现。本平台的A/D 数据卡用在控制中，因此设置为时钟控制脉冲触发模式，即时钟脉冲触发采样写入FIFO，开始每个控制周期时板卡缓存读空，以免数据溢出缓存。

3.1.2.4 Intel 82C54 可编程计数器操作

PCI1710 板卡采用Intel 82C54 计数/ 定时器芯片，含计数器0、计数器1 及计数器2 共3 个16 位计数器。本平台选用计数器1 & 2 模式，计数器1 和2 结合在一起为32 位计数器，设置模式2，即脉冲触发采样模式。82C54 通过4个地址来进行编程设置，具体编程时根据所需脉冲频率，分别计算出计数器1、计数器2 写入值，再按高低字节分解数值写入寄存器。

3.1.3 RTX 下PCI 模拟量输出卡寄存器操作

PCI 模拟量输出卡一般集成D/A 数据转换输出、通道同步输出控制功能，如研华的PCI1720、PCI1727 等。本文以研华PCI1727 板卡为例，分析给出PCI 模拟量输出卡寄存器操作方法。

3.1.3.1 同步设置及输出控制寄存器操作

同步设置寄存器可读可写。最低位写入0、1 时，则分别设置为单通道独立输出模式、多通道同步输出模式。读取最低位，值为0、1 则分别表示处于独立输出、同步输出模式。

向同步输出控制寄存器写入任意数值，则系统立即同步输出锁存器中的各个通道电压。

3.1.3.2 D/A 输出寄存器操作为确保高、低字节数据写入无冲突，采用双缓冲技术。必须首先将高字节数据（仅低6 位可用）写入到锁存器临时锁存，不立即变更模拟输出。只有发送低字节时，高字节才同步写入D / A 转换器，实现高、低字节的无冲突同步输出。

3.1.4 板卡驱动的嵌入

板卡寄存器操作代码封装嵌入到工程程序中，形成板卡RTX 驱动，便于控制平台程序调用。本平台通过VS 工程框架编写驱动源程序，和MFC 控制程序集成编译，生成RTSS 应用，以类库的形式挂载板卡驱动在RTX 程序中，供实时控制程序使用，如图2 所示。

图2： 板卡驱动挂载图

3.2 实时控制任务的算法实现

划分系统任务为4 个模块：多通道数据采集与输出、多自由度实时并行控制、参数设置及GUI 监视。其中前2 个任务面向实时控制，运行在RTX 内核RTSS 环境中；后2个子任务非实时优先，运行在Win32/Win64 环境中。下文主要针对实时任务算法进行论述。

3.2.1 多通道采集与输出任务

数据采集算法流程如图3 所示。首先启动数据采集板卡，并设置其82C54 计数模式2，周期2ms 脉冲触发多通道同步采样；RTX 定时器设置每12ms 将FIFO 队列读空；FIFO数据组经滤波后进PC 系统共享内存，用于非实时环境下的GUI 程序及RTX 环境下的实时控制。

图3： 数据采集算法流程图

数据输出算法流程如图4 所示，启动板卡并初始化后，等待控制算法给出的控制量，得到控制量后通过锁存确保多通道的一致性实时输出。

图4： 数据输出算法结构图

上述多通道数据同步采集和输出算法保证了多通道一致性与实时性，为移动机器人多自由度轨迹跟踪协调性和控制同步性实现提供关键保障。

3.2.2 多自由度并行控制策略

多自由度并行控制算法流程如图5 所示。在控制主线程下创建RTX 定时器：RtSetThreadPriority()、RtCreate Timer()、RtSetTimerRelative()、RTFCNDCL TimerHandler()函数分别设定主线程优先级为次高、创建定时器并赋予最高优先级、设定定时时间、为定时器中断处理程序。路径规划叠加时钟数据成实时目标，进而利用地图映射、偏差计算程序，将A/D 板卡采集的状态数据转为到各通道控制程序的输入，各通道控制程序可分别搭载神经网络PID、模糊、滑膜变结构等智能控制算法进行并行运算，最后利用D/A 板卡进行同步输出。多通道实时并行控制回路在定时器中断程序中完成，通过不断的周期性高速离散采样与解算，对移动机器人进行闭环控制。RTX 实时环境下PCI 板卡驱动保证了多通道数据流的严格同步性，上述算法实现了移动机器人多通道实时并行控制。

图5： 多自由度并行控制算法流程图

4 试验验证

分别在非实时环境和RTX 环境下运行双通道的神经网络PID 控制程序，结果分别如图6、图7，非实时环境下运行约1 分钟后，控制曲线跟踪效果明显变差，而RTX 环境下运行约3 分钟同样控制程序，控制曲线跟踪效果一直保持稳定。可见，系统能够适应智能控制算法的算力要求，并且RTX 环境下的控制程序具有强实时性，能够保证控制程序优先等级，抗干扰能力强。

图6： 非实时环境下连续控制效果图

图7： RTX 环境下连续控制效果图

5 结论

针对移动机器人实时智能控制需求，搭建了工控机加PCI 数据采集卡的PC-Based 控制平台，在提高控制器运算能力、开放性的同时，确保了系统控制的实时性，为移动机器人智能控制算法的集成应用提供了软硬件条件支持。本控制平台相关技术亦可用于移动机器人半实物仿真及数字孪生场景，为移动机器人的研发、试验和人机融合应用提供重要支撑。