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

翟文正

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

0 前言

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

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

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

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

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

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

1.1 dVRK機器人控制器

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

圖2 dVRK FGPA控制板

1.2 Speedgoat目標(biāo)機

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

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

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

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

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

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

2.2 機械臂控制模塊

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ā)送機器人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ù)幀，通過實時以太網(wǎng)傳遞到服務(wù)器端，經(jīng)Ethernet/FireWire 橋廣播到菊花鏈接的各個FPGA控制器節(jié)點。當(dāng)數(shù)據(jù)幀通過各個節(jié)點時，會根據(jù)幀頭目標(biāo)節(jié)點號提取屬于自己的數(shù)據(jù)，并將要輸出的數(shù)據(jù)插入到數(shù)據(jù)幀。

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

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

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

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

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

圖4 宿主機實時通信模塊

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

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

4 實驗結(jié)果及分析

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

圖5 dVRK機械臂實時控制實驗平臺

4.1 機械臂控制

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

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

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

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

4.2 dVRK實時性能測試

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

圖7 RTT測試模型

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

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

5 結(jié)論

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