基于Speedgoat的dVRK机械臂控制系统开发

翟文正

(1.常州信息职业技术学院网络空间安全学院，江苏常州 213164；2.上海大学微电子研究与开发中心，上海 200072)

0 前言

外科手术机器人有效结合了手术医生的经验和机器人定位精确、运行稳定和操作精度高的特性,可协助医生完成精细的手术动作,减少手术中因为疲劳产生的误操作和手部振颤造成的损伤,从而提高手术质量与安全性,缩短治疗时间,降低医疗成本。著名的医疗机器人有DaVinci系统、Zeus系统、伊索机器人Aesop和黎元、妙手等。

达芬奇外科手术机器人系统(Da Vinci)是最具代表性的商业化手术机器人，至今超过5 500台被安装使用。基于外科手术达芬奇机器人的研究平台dVRK(da Vinci Research Kit，达芬奇研究平台)是达芬奇第一代各部件的集成，是一个主从遥操作机器人平台，可通过开源软硬件对机器人各控制层级进行读/写控制，极大地降低了外科手术机器人控制系统设计的学习难度。dVRK在全球30多个国家的70多所大学和研究机构使用，现有研究大多聚焦于路径规划、机器视觉和多传感器融合的创新应用开发，很少有基于机器人控制器的算法验证和实时性控制研究。

机器人控制系统开发需要高频硬实时和周期性计算，以实现对底层关节位置、速度和扭矩的实时控制。Simulink Real-Time是Mathworks公司推出的一款实时仿真技术，为基于PC实现控制系统快速原型开发和硬件在环仿真提供解决方案。基于MATLAB/Simulink的机器人开发能直接从Simulink算法模块中产生优化的、可移植和个性化的代码，使控制系统的快速原型化成为可能，大大提高了算法实现过程的效率和可靠性。本文作者通过进行dVRK机器人主操作臂建模与在环仿真，实现对机器人的实时控制，提高机器人控制的实时性和稳定性。

1 dVRK机器人控制系统描述

图1所示为dVRK机器人机械臂实时控制系统，主要包括宿主机PC、Speedgoat目标机、用于信号采集与控制的FPGA控制器、伺服直流电机及驱动系统。宿主机主要用于控制算法建模和调用RTW(Real-Time Workshop)实现控制算法的自动代码生成，并对目标机代码下载、控制和数据通信。伺服电机的转速由编码器实时采集并上传到仿真系统，控制系统产生的控制指令通过检测与控制模块实时传送给机器人电机驱动电路。

图1 dVRK实时控制系统

1.1 dVRK机器人控制器

达芬奇手术机器人医生控制台是da Vinci系统的控制中心，外科医生通过使用2个主操作臂遥操作患者侧手术臂(PSM)和内窥镜机械手，从而实现对患者的手术。图2所示的dVRK控制板采用Xilinx-Spartan-6xc6slx45 FPGA，配置IEEE-1394a(FireWire)接口和以太网接口，多个控制器和电机驱动板通过dVRK控制板链接成一个FireWire网络，与PC机或Speedgoat目标机通过以太网进行通信。同时，将FPGA连接到I/O设备如编码器和用于输出电机电流的DAC。

图2 dVRK FGPA控制板

1.2 Speedgoat目标机

Speedgoat实时目标机基于MathWorks的实时操作系统Simulink Real-Time，执行由宿主机下载的控制算法代码，负责与外部设备连接，交互控制信息与反馈信息，监控测试软件，在线调整参数，实现电机的实时控制。Speedgoat配置高性能处理器和高效的实时内核，支持多种通信协议和I/O类型，网络传输速率达1 000 MB/s以上。

宿主机以Socket链接的方式发送Speedgoat实时目标机，Speedgoat控制机器人运动，并将机器人位置、速度、加速度等信息实时反馈给上位机。上位机对反馈信息闭环控制，持续不断发送控制信息给机器人控制器，控制机器人实时运动。

2 dVRK控制系统的模型设计

2.1 宿主机与目标机的通信连接

搭建基于Simulink Real-Time的UDP实时控制系统，在宿主机与目标机之间建立网络连接，设置应用程序实时运行环境，实现在上位机、实时目标机对电机的实时控制。如表1所示的网络配置，宿主机通过RJ45网线与目标机Host link接口相连，运行于实时目标机的程序通过TCP/IP接口从宿主机下载到Speedgoat，宿主机将控制指令以一定的控制格式封装为Simulink S函数，通过实时UDP协议传输给目标机，由约定的通信端口接收来自目标机的实时数据反馈。

表1 Speedgoat-dVRK网络配置

2.2 机械臂控制模块

dVRK控制器FPGA通信端口基于IEEE-1394(Firewire)协议，支持2种类型的数据传输：4字节传输和块传输。对于4字节传输请求命令，当带有目的地址段的4字节数据包被发送到FPGA控制器后，FPGA将回应请求读取的数据或执行写入请求，该类型的数据请求可直接访问FPGA所有I/O资源。

如图3所示的Simulink Real Time UDP读取包模型，其构造过程是通过S函数接收来自客户端的4个输入参数：node、tcode、addr和tl，然后根据协议处理后打包发送机器人4个32位命令序列包packet0、packet1、packet2、packet3，具体构造过程如以下函数定义：

图3 Simulink Real Time UDP包请求模型块

void make_1394_header(unsigned long *packet,unsigned short node,unsigned long addr,unsigned int tcode,

unsigned int tl)

{

packet[0]=bswap_32((0xFFC0|node)<< 16|(tl & 0x3F)<< 10|(tcode & 0x0F)<< 4);

packet[1]=bswap_32(0xFFFF << 16|((addr &

0x0000FFFF00000000)>> 32));

packet[2]=bswap_32(addr&0xFFFFFFFF);

packet[3]=0;//CRC--should still work without valid CRC

}

客户端将要发送的数据打包成一个数据帧，通过实时以太网传递到服务器端，经Ethernet/FireWire 桥广播到菊花链接的各个FPGA控制器节点。当数据帧通过各个节点时，会根据帧头目标节点号提取属于自己的数据，并将要输出的数据插入到数据帧。

实时块传输常用于读取和发送机器人伺服控制线程中的数据，实现对机器人的电机控制、FPGA PROM读写和Dallas 1-wire加密芯片读写。块传输支持同时对所有通道的多个寄存器进行读写。上位机PC通过执行块读取来获得反馈数据，计算控制信号(电机电流)，然后执行块写入。

3 机器人控制应用设计

dVRK机械臂的实时控制主要由分别运行在宿主机和目标机的Simulink模型实现。在宿主机上设置机械臂各执行关节的期望位置，模型编译下载后，目标机实时计算控制量偏差，并通过Real time UDP传送给dVRK机器人FPGA控制器，最终输出执行器动作的偏移量，以使机械臂定位到期望位置。

宿主机将要发送到目标机的UDP数据打包成帧，通过以太网传递给目标机，目标机将采集的实时数据发回宿主机如此循环往复，完成宿主机-目标机的数据传输。

如图4所示的宿主机模型中，UDP网络配置块Configure UDP首先根据表1配置初始化Ethernet通信环境，包括网络IP地址和通信端口号；块Receive UDP packet接收目标机反馈的控制信号，通过Byte Unpack数据包解析，并通过Data Type Conversition进行类型转换；Byte Pack则进行数据打包并通过Send UDP packet将控制信息从宿主机发送到目标机，利用Ethernet UDP完成整个控制系统数据的传输。

图4 宿主机实时通信模块

目标机Simulink模型主要由块读请求(BRead Request)、块读响应(BRead Feedback)、块写操作(BWrite)和PID控制器(PID Subsystem)模块组成，实现对机器人的电源控制、传感(状态)数据读取、电机位置控制量写入等功能。

目标机实时计算来自上位机设定各电机转速与编码器反馈的转速差值，PID控制器计算控制电量并通过DA转换量传送到电机驱动板，构成电机转速的闭环控制。

4 实验结果及分析

搭建如图5所示的dVRK机器人实时控制平台。在宿主机与目标机之间建立网络连接，设置应用程序实时运行环境，在宿主机Simulink环境中进行算法建模和仿真，通过Real-Time Workshop(RTW)实现控制算法的自动代码生成，并实现对目标机代码下载、控制和数据通信。

图5 dVRK机械臂实时控制实验平台

4.1 机械臂控制

图6所示为目标机实时数据读取界面，显示了目标机模型读取的dVRK机器人主、从机械臂各关节编码器传感数值(Enc)、电势(Pot)、速度(Vel)、反馈电流(Cur)和期望电流值(DAC)，以及包括运行状态(Status)、时间戳(Timestamp)、控制板号在内的调试信息。

图6 目标机实时数据读取

基于dVRK机器人实时控制平台，运行宿主机和目标机上Simulink Real Time模型后读取得到机械臂末端执行器位置数值如表2所示。经过对比可知理论位置值和实际位置值间数据误差相对较小，整个控制系统的运行精度比较高，验证了所建立的机器人控制模型的正确性。

表2 dVRK机械臂末端执行器运行实验数据

4.2 dVRK实时性能测试

环回时间(Round-Trip Time，RTT)是决定UDP传输速率的重要指标，在测量UDP链接的RTT时，首先搭建如图7所示的基于Simulink Real-Time UDP的实时通信测量模型，包括客户机UDP打包发送，服务器UDP接收解包及响应。时间延迟计算块是由服务器UDP接收解包的时间戳与客户机UDP打包发送时刻时间戳的差值，经系数增益块将测量的RTT量化为特定的微秒、纳秒等量级，计数器取值范围1～3 276，则模型中“输出显示”模块的数值即实际网络的往返时延值。

图7 RTT测试模型

设置仿真步长为0.1 ms时，测得各任务周转时间如表3所示：块读请求任务平均周转时间为13 μs，来自机器人的响应约为9 μs，PID子系统平均周转时间约为6 μs，均具有较高的实时性。当选择更短的仿真步长时出现不同情况的系统丢帧，这说明Speedgoat目标机能够完成系统对实时性要求。

表3 任务周转时间测试

5 结论

本文作者针对dVRK外科机器人搭建了基于Simulink Real Time和Speedgoat目标机的实时控制系统，在宿主机上创建控制系统的Simulink模型，并利用RTW和编译器生成可执行代码下载到运行实时内核的目标机。通过对机器人不断发送命令来读取机器人的传感器状态并控制机械臂各关节的执行器到达指定位置，从而完成对机器人的实时控制。结果表明：基于Speedgaot的实时控制平台具有在线调参、实时仿真、目标代码快速原型化等优点，仿真精度高，可靠性好，大大缩短了dVRK控制算法的研制周期。