基于Simulink Real-Time的dVRK機(jī)械臂實(shí)時控制研究

翟文正

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

0 前言

當(dāng)前以達(dá)芬奇(da Vinci)為代表的微創(chuàng)手術(shù)機(jī)器人成為國際機(jī)器人領(lǐng)域的前沿和研究熱點(diǎn)。截至2019年，達(dá)芬奇微創(chuàng)手術(shù)機(jī)器人系統(tǒng)已在全世界裝機(jī)超過5 500臺，成功開展720多萬例機(jī)器人手術(shù)，應(yīng)用涵蓋泌尿外科、普外科、心臟外科、肝膽外科、胃腸外科、耳鼻喉科等學(xué)科。

基于達(dá)芬奇手術(shù)機(jī)器人的研究平臺dVRK(da Vinci Research Kit)是一個開源的機(jī)器人機(jī)電一體化平臺，是達(dá)芬奇第一代各部件的集成，并配備了由約翰霍普金斯大學(xué)LCSR計算傳感和機(jī)器人實(shí)驗(yàn)室和伍斯特理工學(xué)院AIM實(shí)驗(yàn)室開發(fā)的各種控制器和軟件。dVRK被全球30多個國家的70多所大學(xué)和研究機(jī)構(gòu)使用，現(xiàn)有研究大多聚焦于路徑規(guī)劃、機(jī)器視覺和多傳感器融合的創(chuàng)新應(yīng)用開發(fā)，很少有基于機(jī)器人控制器的算法驗(yàn)證和實(shí)時性控制研究。

圖1所示為dVRK研究平臺，包括操作手柄、操作臂系統(tǒng)、手術(shù)操作監(jiān)視器和腳踏板。每個機(jī)械臂末端夾持手術(shù)器械，安裝在機(jī)械臂末端的手術(shù)器械采用類人手腕式結(jié)構(gòu),與傳統(tǒng)的手術(shù)器械相比,大大提高了手術(shù)操作的靈活度。

圖1 JHU LCSR實(shí)驗(yàn)室的dVRK研究平臺

機(jī)器人系統(tǒng)的高頻、硬實(shí)時特性，要求系統(tǒng)能夠周期性計算并實(shí)時控制底層各關(guān)節(jié)的位置、速度和扭矩等變量，急需一個帶有簡易編程接口的實(shí)時環(huán)境支持計算機(jī)科學(xué)家算法。Simulink Real-Time是Mathworks推出的一項實(shí)時仿真技術(shù)，是基于PC設(shè)備實(shí)現(xiàn)實(shí)時快速原型和硬件在回路的仿真解決方案，支持用戶基于Simulink模型創(chuàng)建實(shí)時仿真應(yīng)用。本文作者通過對dVRK機(jī)器人機(jī)械臂建模與在環(huán)仿真，實(shí)現(xiàn)對機(jī)器人的實(shí)時控制，提高機(jī)器人控制的實(shí)時性和穩(wěn)定性。

1 dVRK機(jī)器人機(jī)械臂運(yùn)動學(xué)描述

達(dá)芬奇手術(shù)機(jī)器人機(jī)械臂(PSM)有一系列多位置關(guān)節(jié)和可旋轉(zhuǎn)的末端關(guān)節(jié)與套管相連，具有7個自由度，保證可達(dá)到手術(shù)要求的運(yùn)動空間。

根據(jù)D-H法思想，建立PSM各關(guān)節(jié)坐標(biāo)系如圖2所示，其各桿件參數(shù)及關(guān)節(jié)變量如表1所示。由、、和4個參數(shù)描述dVRK機(jī)器人PSM空間姿態(tài)和各臂間的連接關(guān)系，通過推導(dǎo)4個參數(shù)之間關(guān)系，得到PSM各關(guān)節(jié)和相應(yīng)坐標(biāo)系與基坐標(biāo)系的齊次坐標(biāo)變換矩陣，取機(jī)械臂末端關(guān)節(jié)的中點(diǎn)為末端建立D-H運(yùn)動矩陣方程。

圖2 dVRK機(jī)器人機(jī)械臂坐標(biāo)系

表1 PSM機(jī)械臂D-H參數(shù)

-1=(,-1)(,-1)(,)(,)=

(1)

-1為繞軸旋轉(zhuǎn)的角度，-1為沿軸移動的距離，為繞軸旋轉(zhuǎn)的角度；為沿著軸機(jī)器臂連桿相對于連桿-1的移動距離。以連桿和關(guān)節(jié)屬于剛性體為前提，則、和參數(shù)為固定值，為變量。

(6)

(8)

式中：=cos，=sin，以此類推。

(9)

其中:前3列為dVRK機(jī)械臂在三維空間中末端執(zhí)行器上參考點(diǎn)相對于基坐標(biāo)系的姿態(tài)向量，最右列為相對于基坐標(biāo)系的位置向量，、、分別為末端執(zhí)行器在空間中的、、坐標(biāo)。

式中：

={-[+(+)]+}+[-

(+)]+{-[+(+)]}

={[(+(+)]-}

={-[+(+)]+}-[-

(+)]

={{-[+(+)]+}-

[-(+)]}+{-[+(+

)]+}++

=[-(+)+]+(-)+

[-(+)]

=-[-(+)]

=[-(+)+]-(-)

={[-(+)+]-(-)}+

[-(+)+]++

=[(+(-)]-}+[-+

(-)]+{[+(-)]-}

=-{[+(-)]-}

={[+(-)]-}+[-(-)]

={[(+(-))-]+[-

(-)]}+[(+(-))-

]-(+)

在dVRK PSM中，、、長度分別為0.431 8、0.416 2、0.010 2 m，為0.009 1 m。

2 dVRK機(jī)械臂控制

2.1 dVRK控制架構(gòu)

一個完整的dVRK系統(tǒng)使用8個FPGA控制器及電機(jī)驅(qū)動板，用來控制包括操縱臂和機(jī)械臂在內(nèi)的4個手臂。如圖3所示的控制架構(gòu)含有1個控制計算機(jī)、4個FPGA控制器及電機(jī)驅(qū)動板QLA，用于2個機(jī)器人手臂的控制和通信。

圖3 dVRK控制架構(gòu)

多個FPGA控制器和電機(jī)驅(qū)動板通過IEEE-1394a(FireWire)菊花鏈接成一個FireWire網(wǎng)，以太網(wǎng)到FireWire橋接節(jié)點(diǎn)接收來自主機(jī)PC的以太網(wǎng)數(shù)據(jù)包產(chǎn)并轉(zhuǎn)換為FireWire數(shù)據(jù)包，執(zhí)行FireWire操作，同時將FireWire響應(yīng)的數(shù)據(jù)包轉(zhuǎn)換為PC的以太網(wǎng)數(shù)據(jù)包。上位機(jī)通過以太網(wǎng)向FPGA板發(fā)送/接收數(shù)據(jù)包，然后FPGA通過FireWire網(wǎng)絡(luò)與多個分布式節(jié)點(diǎn)通信。

從PC到機(jī)器人各個控制節(jié)點(diǎn)的數(shù)據(jù)幀包含了4字節(jié)讀/寫、數(shù)據(jù)塊讀/寫，基于廣播的寫/查詢和系統(tǒng)同步等內(nèi)容，數(shù)據(jù)幀包含數(shù)據(jù)包頭和校驗(yàn)部分。

2.2 PSM機(jī)械臂PD關(guān)節(jié)位置跟蹤控制

dVRK機(jī)械臂的關(guān)節(jié)位置控制主要分為關(guān)節(jié)空間控制和笛卡爾空間控制。關(guān)節(jié)空間的控制由逆運(yùn)動學(xué)模塊將笛卡爾任務(wù)軌跡映射到關(guān)節(jié)空間，得到關(guān)節(jié)空間軌跡之后通過關(guān)節(jié)層控制器跟蹤軌跡。

PID控制是最經(jīng)典的控制策略之一，具有算法簡單、適應(yīng)性強(qiáng)、可靠性高和魯棒性好等優(yōu)點(diǎn)。PID控制是把給定值和輸出反饋值的偏差通過比例、微分和積分的線性組合構(gòu)成控制量，對控制對象進(jìn)行控制。PID微分方程為

式中：為比例系數(shù)；為積分時間常數(shù)；為微分時間常數(shù)。

dVRK PSM 7-DOF機(jī)械臂的位置跟蹤控制是一個高度復(fù)雜的多變量、強(qiáng)耦合的非線性系統(tǒng)問題，需要較大的在線計算量，給機(jī)械臂的動態(tài)實(shí)時控制帶來了困難。為簡化控制難度同時能夠滿足特定的作業(yè)現(xiàn)場要求，多采用獨(dú)立的PD(Proportion Differentiation)位置跟蹤控制方案。

圖4所示為機(jī)械臂實(shí)時位置控制模型，目標(biāo)機(jī)實(shí)時計算來自上位機(jī)的各關(guān)節(jié)期望電機(jī)位置與編碼器反饋的位置差值，PD調(diào)節(jié)器模塊自動調(diào)整每個機(jī)械臂驅(qū)動器的最佳PD參數(shù)，PID控制器計算出控制量并通過DA轉(zhuǎn)換傳送到電機(jī)驅(qū)動板，構(gòu)成機(jī)械臂關(guān)節(jié)電機(jī)實(shí)時位置的閉環(huán)控制。

圖4 dVRK機(jī)械臂實(shí)時位置控制

Conv模塊Joint2ActuaTorque通過變換PSM各執(zhí)行器之間的伴隨矩陣得到輸出轉(zhuǎn)矩；Conv模塊NumToAmps通過各執(zhí)行器的特性將轉(zhuǎn)矩映射為電流值；Conv模塊Actua2Joint利用伴隨矩陣將執(zhí)行器空間映射到關(guān)節(jié)空間；Conv模塊Bits2PosSI通過塊讀取命令獲取機(jī)器人的PSM編碼器、電壓、電流等傳感量。

上位機(jī)PC通過以太網(wǎng)UDP設(shè)置每個PSM執(zhí)行關(guān)節(jié)的期望位置，目標(biāo)機(jī)計算出實(shí)際偏差控制量，并通過Simulink Real-Time UDP傳輸給dVRK PSM控制器，最后輸出執(zhí)行器動作的偏移量，使機(jī)械臂到達(dá)期望的位置。

2.3 PD Tuner模型設(shè)計

PID Tuner可對Simulink 模型中的PID控制器進(jìn)行自動調(diào)參，實(shí)現(xiàn)控制性能和健壯性的良好平衡；能自動給出一個平衡了控制性能和魯棒性的初始PID控制設(shè)計參數(shù)。在圖4所示的dVRK機(jī)械臂實(shí)時位置控制PD Simulink模型塊中，設(shè)置塊參數(shù)為基于傳遞函數(shù)的自動調(diào)節(jié)方法，進(jìn)入PID Tuner模型塊分別設(shè)置如圖5所示的輸入?yún)?shù)、增益輸出跟蹤和啟動/停止位。

圖5 Simulink PID Tune位置控制模型塊

仿真運(yùn)行單關(guān)節(jié)的PD自動調(diào)節(jié)模型塊后，以同樣方法配置其他機(jī)械臂關(guān)節(jié)電機(jī)的PD自動調(diào)節(jié)模式，實(shí)現(xiàn)對dVRK機(jī)械臂的實(shí)時位置控制。

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

3.1 機(jī)械臂模型建立

Robotics Toolbox機(jī)器人應(yīng)用工具箱提供了針對機(jī)器人的包括運(yùn)動學(xué)、動力學(xué)和軌跡規(guī)劃等功能的函數(shù)。根據(jù)D-H參數(shù)表，利用Toolbox應(yīng)用工具箱中l(wèi)ink()和robot()函數(shù)建立如圖6所示的機(jī)械臂三維模型，構(gòu)建 PSM模型命令如下：

圖6 dVRK PSM 7-DOF機(jī)械臂三維模型

L1=Link(0,0，pi/2，pi/2)；%采用標(biāo)準(zhǔn)D-H參數(shù)構(gòu)建各個關(guān)節(jié)：

L2=Link(0,0，-pi/2，-pi/2)；

L3=Link(-0.431 8,0，pi/2，0)；

L4=Link(0.416 2,0，0，0)；

L5=Link(0,0，-pi/2，-pi/2)；

L6=Link(0,0.009 1，-pi/2，-pi/2)；

L7=Link(0.010 2,0，0，0)；

robot=SerialLink([L1,L2,L3,L4,L5,L6,L7],′name′,′dVRK-robotRSM′)；%構(gòu)建機(jī)械臂

robot.teach()；%用于調(diào)節(jié)關(guān)節(jié)量，實(shí)時演示機(jī)械臂空間位姿

通過代入隨機(jī)參數(shù)的方法，根據(jù)式(9)和模型仿真分別得到機(jī)械臂末端執(zhí)行器的理論位置和實(shí)際位置。經(jīng)過對比可知理論位置和實(shí)際位置誤差相對較小，證明了所建立的運(yùn)動學(xué)模型的正確性。

3.2 機(jī)械臂控制

基于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)控制算法的自動代碼生成，并對目標(biāo)機(jī)進(jìn)行代碼下載、控制和數(shù)據(jù)通信。運(yùn)行宿主機(jī)和目標(biāo)機(jī)上Simulink Real-Time模型后讀取到dVRK機(jī)器人機(jī)械臂PSM各關(guān)節(jié)編碼器傳感數(shù)值、電勢、速度、反饋電流和期望電流值，以及包括運(yùn)行狀態(tài)、時間戳等的調(diào)試信息。

如圖7所示的目標(biāo)機(jī)實(shí)時數(shù)據(jù)顯示界面，首行顯示了目標(biāo)機(jī)程序當(dāng)前運(yùn)行時間為49 s，采樣頻率為5 kHz，9個Scope信號窗口正確顯示了目標(biāo)機(jī)Simulink模型實(shí)時運(yùn)行中各伺服電機(jī)的設(shè)定轉(zhuǎn)速、編碼器讀數(shù)、PID偏差調(diào)節(jié)量、DA轉(zhuǎn)換值等用于電機(jī)實(shí)時控制的監(jiān)測信號。

圖7 目標(biāo)機(jī)實(shí)時數(shù)據(jù)顯示

將電機(jī)控制響應(yīng)數(shù)值導(dǎo)出到Workspace并可視化后得到如圖8所示的dVRK PSM PID響應(yīng)曲線。可知：P增益為30、I增益為1、D增益為1.5；系統(tǒng)響應(yīng)曲線總體上與輸入控制信號吻合，機(jī)器人操作臂關(guān)節(jié)電機(jī)的偏差電流能在較短時間內(nèi)準(zhǔn)確跟蹤期望值，無超調(diào)，穩(wěn)態(tài)過程存在最大幅值為1%的波動，完全滿足控制要求。

圖8 dVRK PSM PID響應(yīng)曲線

4 結(jié)論

本文作者針對dVRK外科機(jī)器人機(jī)械臂，利用D-H法進(jìn)行關(guān)節(jié)坐標(biāo)系建模，并計算其齊次變換矩陣。通過MATLAB/Simulink建立了機(jī)械臂各關(guān)節(jié)的PID位置控制半實(shí)物仿真模型，實(shí)現(xiàn)對機(jī)械臂末端的實(shí)時定點(diǎn)控制。結(jié)果表明：利用文中建立的基于Simulink Real-Time的dVRK控制模型可準(zhǔn)確采集機(jī)器人機(jī)械臂各關(guān)節(jié)位置、速度和加速度等變量，同時驗(yàn)證了機(jī)械臂末端定點(diǎn)控制的PID算法的正確性，為進(jìn)一步實(shí)現(xiàn)機(jī)器人的實(shí)時控制提供了參考。