基于Speedgoat的dVRK機(jī)械臂控制系統(tǒng)開發(fā)

翟文正

(1.常州信息職業(yè)技術(shù)學(xué)院網(wǎng)絡(luò)空間安全學(xué)院，江蘇常州 213164；2.上海大學(xué)微電子研究與開發(fā)中心，上海 200072)

0 前言

外科手術(shù)機(jī)器人有效結(jié)合了手術(shù)醫(yī)生的經(jīng)驗(yàn)和機(jī)器人定位精確、運(yùn)行穩(wěn)定和操作精度高的特性,可協(xié)助醫(yī)生完成精細(xì)的手術(shù)動作,減少手術(shù)中因?yàn)槠诋a(chǎn)生的誤操作和手部振顫造成的損傷,從而提高手術(shù)質(zhì)量與安全性,縮短治療時間,降低醫(yī)療成本。著名的醫(yī)療機(jī)器人有DaVinci系統(tǒng)、Zeus系統(tǒng)、伊索機(jī)器人Aesop和黎元、妙手等。

達(dá)芬奇外科手術(shù)機(jī)器人系統(tǒng)(Da Vinci)是最具代表性的商業(yè)化手術(shù)機(jī)器人，至今超過5 500臺被安裝使用。基于外科手術(shù)達(dá)芬奇機(jī)器人的研究平臺dVRK(da Vinci Research Kit，達(dá)芬奇研究平臺)是達(dá)芬奇第一代各部件的集成，是一個主從遙操作機(jī)器人平臺，可通過開源軟硬件對機(jī)器人各控制層級進(jìn)行讀/寫控制，極大地降低了外科手術(shù)機(jī)器人控制系統(tǒng)設(shè)計(jì)的學(xué)習(xí)難度。dVRK在全球30多個國家的70多所大學(xué)和研究機(jī)構(gòu)使用，現(xiàn)有研究大多聚焦于路徑規(guī)劃、機(jī)器視覺和多傳感器融合的創(chuàng)新應(yīng)用開發(fā)，很少有基于機(jī)器人控制器的算法驗(yàn)證和實(shí)時性控制研究。

機(jī)器人控制系統(tǒng)開發(fā)需要高頻硬實(shí)時和周期性計(jì)算，以實(shí)現(xiàn)對底層關(guān)節(jié)位置、速度和扭矩的實(shí)時控制。Simulink Real-Time是Mathworks公司推出的一款實(shí)時仿真技術(shù)，為基于PC實(shí)現(xiàn)控制系統(tǒng)快速原型開發(fā)和硬件在環(huán)仿真提供解決方案。基于MATLAB/Simulink的機(jī)器人開發(fā)能直接從Simulink算法模塊中產(chǎn)生優(yōu)化的、可移植和個性化的代碼，使控制系統(tǒng)的快速原型化成為可能，大大提高了算法實(shí)現(xiàn)過程的效率和可靠性。本文作者通過進(jìn)行dVRK機(jī)器人主操作臂建模與在環(huán)仿真，實(shí)現(xiàn)對機(jī)器人的實(shí)時控制，提高機(jī)器人控制的實(shí)時性和穩(wěn)定性。

1 dVRK機(jī)器人控制系統(tǒng)描述

圖1所示為dVRK機(jī)器人機(jī)械臂實(shí)時控制系統(tǒng)，主要包括宿主機(jī)PC、Speedgoat目標(biāo)機(jī)、用于信號采集與控制的FPGA控制器、伺服直流電機(jī)及驅(qū)動系統(tǒng)。宿主機(jī)主要用于控制算法建模和調(diào)用RTW(Real-Time Workshop)實(shí)現(xiàn)控制算法的自動代碼生成，并對目標(biāo)機(jī)代碼下載、控制和數(shù)據(jù)通信。伺服電機(jī)的轉(zhuǎn)速由編碼器實(shí)時采集并上傳到仿真系統(tǒng)，控制系統(tǒng)產(chǎn)生的控制指令通過檢測與控制模塊實(shí)時傳送給機(jī)器人電機(jī)驅(qū)動電路。

圖1 dVRK實(shí)時控制系統(tǒng)

1.1 dVRK機(jī)器人控制器

達(dá)芬奇手術(shù)機(jī)器人醫(yī)生控制臺是da Vinci系統(tǒng)的控制中心，外科醫(yī)生通過使用2個主操作臂遙操作患者側(cè)手術(shù)臂(PSM)和內(nèi)窺鏡機(jī)械手，從而實(shí)現(xiàn)對患者的手術(shù)。圖2所示的dVRK控制板采用Xilinx-Spartan-6xc6slx45 FPGA，配置IEEE-1394a(FireWire)接口和以太網(wǎng)接口，多個控制器和電機(jī)驅(qū)動板通過dVRK控制板鏈接成一個FireWire網(wǎng)絡(luò)，與PC機(jī)或Speedgoat目標(biāo)機(jī)通過以太網(wǎng)進(jìn)行通信。同時，將FPGA連接到I/O設(shè)備如編碼器和用于輸出電機(jī)電流的DAC。

圖2 dVRK FGPA控制板

1.2 Speedgoat目標(biāo)機(jī)

Speedgoat實(shí)時目標(biāo)機(jī)基于MathWorks的實(shí)時操作系統(tǒng)Simulink Real-Time，執(zhí)行由宿主機(jī)下載的控制算法代碼，負(fù)責(zé)與外部設(shè)備連接，交互控制信息與反饋信息，監(jiān)控測試軟件，在線調(diào)整參數(shù)，實(shí)現(xiàn)電機(jī)的實(shí)時控制。Speedgoat配置高性能處理器和高效的實(shí)時內(nèi)核，支持多種通信協(xié)議和I/O類型，網(wǎng)絡(luò)傳輸速率達(dá)1 000 MB/s以上。

宿主機(jī)以Socket鏈接的方式發(fā)送Speedgoat實(shí)時目標(biāo)機(jī)，Speedgoat控制機(jī)器人運(yùn)動，并將機(jī)器人位置、速度、加速度等信息實(shí)時反饋給上位機(jī)。上位機(jī)對反饋信息閉環(huán)控制，持續(xù)不斷發(fā)送控制信息給機(jī)器人控制器，控制機(jī)器人實(shí)時運(yùn)動。

2 dVRK控制系統(tǒng)的模型設(shè)計(jì)

2.1 宿主機(jī)與目標(biāo)機(jī)的通信連接

搭建基于Simulink Real-Time的UDP實(shí)時控制系統(tǒng)，在宿主機(jī)與目標(biāo)機(jī)之間建立網(wǎng)絡(luò)連接，設(shè)置應(yīng)用程序?qū)崟r運(yùn)行環(huán)境，實(shí)現(xiàn)在上位機(jī)、實(shí)時目標(biāo)機(jī)對電機(jī)的實(shí)時控制。如表1所示的網(wǎng)絡(luò)配置，宿主機(jī)通過RJ45網(wǎng)線與目標(biāo)機(jī)Host link接口相連，運(yùn)行于實(shí)時目標(biāo)機(jī)的程序通過TCP/IP接口從宿主機(jī)下載到Speedgoat，宿主機(jī)將控制指令以一定的控制格式封裝為Simulink S函數(shù)，通過實(shí)時UDP協(xié)議傳輸給目標(biāo)機(jī)，由約定的通信端口接收來自目標(biāo)機(jī)的實(shí)時數(shù)據(jù)反饋。

表1 Speedgoat-dVRK網(wǎng)絡(luò)配置

2.2 機(jī)械臂控制模塊

dVRK控制器FPGA通信端口基于IEEE-1394(Firewire)協(xié)議，支持2種類型的數(shù)據(jù)傳輸：4字節(jié)傳輸和塊傳輸。對于4字節(jié)傳輸請求命令，當(dāng)帶有目的地址段的4字節(jié)數(shù)據(jù)包被發(fā)送到FPGA控制器后，F(xiàn)PGA將回應(yīng)請求讀取的數(shù)據(jù)或執(zhí)行寫入請求，該類型的數(shù)據(jù)請求可直接訪問FPGA所有I/O資源。

如圖3所示的Simulink Real Time UDP讀取包模型，其構(gòu)造過程是通過S函數(shù)接收來自客戶端的4個輸入?yún)?shù)：node、tcode、addr和tl，然后根據(jù)協(xié)議處理后打包發(fā)送機(jī)器人4個32位命令序列包packet0、packet1、packet2、packet3，具體構(gòu)造過程如以下函數(shù)定義：

圖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

}

客戶端將要發(fā)送的數(shù)據(jù)打包成一個數(shù)據(jù)幀，通過實(shí)時以太網(wǎng)傳遞到服務(wù)器端，經(jīng)Ethernet/FireWire 橋廣播到菊花鏈接的各個FPGA控制器節(jié)點(diǎn)。當(dāng)數(shù)據(jù)幀通過各個節(jié)點(diǎn)時，會根據(jù)幀頭目標(biāo)節(jié)點(diǎn)號提取屬于自己的數(shù)據(jù)，并將要輸出的數(shù)據(jù)插入到數(shù)據(jù)幀。

實(shí)時塊傳輸常用于讀取和發(fā)送機(jī)器人伺服控制線程中的數(shù)據(jù)，實(shí)現(xiàn)對機(jī)器人的電機(jī)控制、FPGA PROM讀寫和Dallas 1-wire加密芯片讀寫。塊傳輸支持同時對所有通道的多個寄存器進(jìn)行讀寫。上位機(jī)PC通過執(zhí)行塊讀取來獲得反饋數(shù)據(jù)，計(jì)算控制信號(電機(jī)電流)，然后執(zhí)行塊寫入。

3 機(jī)器人控制應(yīng)用設(shè)計(jì)

dVRK機(jī)械臂的實(shí)時控制主要由分別運(yùn)行在宿主機(jī)和目標(biāo)機(jī)的Simulink模型實(shí)現(xiàn)。在宿主機(jī)上設(shè)置機(jī)械臂各執(zhí)行關(guān)節(jié)的期望位置，模型編譯下載后，目標(biāo)機(jī)實(shí)時計(jì)算控制量偏差，并通過Real time UDP傳送給dVRK機(jī)器人FPGA控制器，最終輸出執(zhí)行器動作的偏移量，以使機(jī)械臂定位到期望位置。

宿主機(jī)將要發(fā)送到目標(biāo)機(jī)的UDP數(shù)據(jù)打包成幀，通過以太網(wǎng)傳遞給目標(biāo)機(jī)，目標(biāo)機(jī)將采集的實(shí)時數(shù)據(jù)發(fā)回宿主機(jī)如此循環(huán)往復(fù)，完成宿主機(jī)-目標(biāo)機(jī)的數(shù)據(jù)傳輸。

如圖4所示的宿主機(jī)模型中，UDP網(wǎng)絡(luò)配置塊Configure UDP首先根據(jù)表1配置初始化Ethernet通信環(huán)境，包括網(wǎng)絡(luò)IP地址和通信端口號；塊Receive UDP packet接收目標(biāo)機(jī)反饋的控制信號，通過Byte Unpack數(shù)據(jù)包解析，并通過Data Type Conversition進(jìn)行類型轉(zhuǎn)換；Byte Pack則進(jìn)行數(shù)據(jù)打包并通過Send UDP packet將控制信息從宿主機(jī)發(fā)送到目標(biāo)機(jī)，利用Ethernet UDP完成整個控制系統(tǒng)數(shù)據(jù)的傳輸。

圖4 宿主機(jī)實(shí)時通信模塊

目標(biāo)機(jī)Simulink模型主要由塊讀請求(BRead Request)、塊讀響應(yīng)(BRead Feedback)、塊寫操作(BWrite)和PID控制器(PID Subsystem)模塊組成，實(shí)現(xiàn)對機(jī)器人的電源控制、傳感(狀態(tài))數(shù)據(jù)讀取、電機(jī)位置控制量寫入等功能。

目標(biāo)機(jī)實(shí)時計(jì)算來自上位機(jī)設(shè)定各電機(jī)轉(zhuǎn)速與編碼器反饋的轉(zhuǎn)速差值，PID控制器計(jì)算控制電量并通過DA轉(zhuǎn)換量傳送到電機(jī)驅(qū)動板，構(gòu)成電機(jī)轉(zhuǎn)速的閉環(huán)控制。

4 實(shí)驗(yàn)結(jié)果及分析

搭建如圖5所示的dVRK機(jī)器人實(shí)時控制平臺。在宿主機(jī)與目標(biāo)機(jī)之間建立網(wǎng)絡(luò)連接，設(shè)置應(yīng)用程序?qū)崟r運(yùn)行環(huán)境，在宿主機(jī)Simulink環(huán)境中進(jìn)行算法建模和仿真，通過Real-Time Workshop(RTW)實(shí)現(xiàn)控制算法的自動代碼生成，并實(shí)現(xiàn)對目標(biāo)機(jī)代碼下載、控制和數(shù)據(jù)通信。

圖5 dVRK機(jī)械臂實(shí)時控制實(shí)驗(yàn)平臺

4.1 機(jī)械臂控制

圖6所示為目標(biāo)機(jī)實(shí)時數(shù)據(jù)讀取界面，顯示了目標(biāo)機(jī)模型讀取的dVRK機(jī)器人主、從機(jī)械臂各關(guān)節(jié)編碼器傳感數(shù)值(Enc)、電勢(Pot)、速度(Vel)、反饋電流(Cur)和期望電流值(DAC)，以及包括運(yùn)行狀態(tài)(Status)、時間戳(Timestamp)、控制板號在內(nèi)的調(diào)試信息。

圖6 目標(biāo)機(jī)實(shí)時數(shù)據(jù)讀取

基于dVRK機(jī)器人實(shí)時控制平臺，運(yùn)行宿主機(jī)和目標(biāo)機(jī)上Simulink Real Time模型后讀取得到機(jī)械臂末端執(zhí)行器位置數(shù)值如表2所示。經(jīng)過對比可知理論位置值和實(shí)際位置值間數(shù)據(jù)誤差相對較小，整個控制系統(tǒng)的運(yùn)行精度比較高，驗(yàn)證了所建立的機(jī)器人控制模型的正確性。

表2 dVRK機(jī)械臂末端執(zhí)行器運(yùn)行實(shí)驗(yàn)數(shù)據(jù)

4.2 dVRK實(shí)時性能測試

環(huán)回時間(Round-Trip Time，RTT)是決定UDP傳輸速率的重要指標(biāo)，在測量UDP鏈接的RTT時，首先搭建如圖7所示的基于Simulink Real-Time UDP的實(shí)時通信測量模型，包括客戶機(jī)UDP打包發(fā)送，服務(wù)器UDP接收解包及響應(yīng)。時間延遲計(jì)算塊是由服務(wù)器UDP接收解包的時間戳與客戶機(jī)UDP打包發(fā)送時刻時間戳的差值，經(jīng)系數(shù)增益塊將測量的RTT量化為特定的微秒、納秒等量級，計(jì)數(shù)器取值范圍1～3 276，則模型中“輸出顯示”模塊的數(shù)值即實(shí)際網(wǎng)絡(luò)的往返時延值。

圖7 RTT測試模型

設(shè)置仿真步長為0.1 ms時，測得各任務(wù)周轉(zhuǎn)時間如表3所示：塊讀請求任務(wù)平均周轉(zhuǎn)時間為13 μs，來自機(jī)器人的響應(yīng)約為9 μs，PID子系統(tǒng)平均周轉(zhuǎn)時間約為6 μs，均具有較高的實(shí)時性。當(dāng)選擇更短的仿真步長時出現(xiàn)不同情況的系統(tǒng)丟幀，這說明Speedgoat目標(biāo)機(jī)能夠完成系統(tǒng)對實(shí)時性要求。

表3 任務(wù)周轉(zhuǎn)時間測試

5 結(jié)論

本文作者針對dVRK外科機(jī)器人搭建了基于Simulink Real Time和Speedgoat目標(biāo)機(jī)的實(shí)時控制系統(tǒng)，在宿主機(jī)上創(chuàng)建控制系統(tǒng)的Simulink模型，并利用RTW和編譯器生成可執(zhí)行代碼下載到運(yùn)行實(shí)時內(nèi)核的目標(biāo)機(jī)。通過對機(jī)器人不斷發(fā)送命令來讀取機(jī)器人的傳感器狀態(tài)并控制機(jī)械臂各關(guān)節(jié)的執(zhí)行器到達(dá)指定位置，從而完成對機(jī)器人的實(shí)時控制。結(jié)果表明：基于Speedgaot的實(shí)時控制平臺具有在線調(diào)參、實(shí)時仿真、目標(biāo)代碼快速原型化等優(yōu)點(diǎn)，仿真精度高，可靠性好，大大縮短了dVRK控制算法的研制周期。