基于Simulink Real-Time的dVRK機械臂實時控制研究

翟文正

(1.常州信息職業技術學院網絡空間安全學院，江蘇常州 213164；2.上海大學微電子研究與開發中心，上海 200072)

0 前言

當前以達芬奇(da Vinci)為代表的微創手術機器人成為國際機器人領域的前沿和研究熱點。截至2019年，達芬奇微創手術機器人系統已在全世界裝機超過5 500臺，成功開展720多萬例機器人手術，應用涵蓋泌尿外科、普外科、心臟外科、肝膽外科、胃腸外科、耳鼻喉科等學科。

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

圖1所示為dVRK研究平臺，包括操作手柄、操作臂系統、手術操作監視器和腳踏板。每個機械臂末端夾持手術器械，安裝在機械臂末端的手術器械采用類人手腕式結構,與傳統的手術器械相比,大大提高了手術操作的靈活度。

圖1 JHU LCSR實驗室的dVRK研究平臺

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

1 dVRK機器人機械臂運動學描述

達芬奇手術機器人機械臂(PSM)有一系列多位置關節和可旋轉的末端關節與套管相連，具有7個自由度，保證可達到手術要求的運動空間。

根據D-H法思想，建立PSM各關節坐標系如圖2所示，其各桿件參數及關節變量如表1所示。由、、和4個參數描述dVRK機器人PSM空間姿態和各臂間的連接關系，通過推導4個參數之間關系，得到PSM各關節和相應坐標系與基坐標系的齊次坐標變換矩陣，取機械臂末端關節的中點為末端建立D-H運動矩陣方程。

圖2 dVRK機器人機械臂坐標系

表1 PSM機械臂D-H參數

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

(1)

-1為繞軸旋轉的角度，-1為沿軸移動的距離，為繞軸旋轉的角度；為沿著軸機器臂連桿相對于連桿-1的移動距離。以連桿和關節屬于剛性體為前提，則、和參數為固定值，為變量。

(6)

(8)

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

(9)

其中:前3列為dVRK機械臂在三維空間中末端執行器上參考點相對于基坐標系的姿態向量，最右列為相對于基坐標系的位置向量，、、分別為末端執行器在空間中的、、坐標。

式中：

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

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

={[(+(+)]-}

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

(+)]

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

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

)]+}++

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

[-(+)]

=-[-(+)]

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

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

[-(+)+]++

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

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

=-{[+(-)]-}

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

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

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

]-(+)

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

2 dVRK機械臂控制

2.1 dVRK控制架構

一個完整的dVRK系統使用8個FPGA控制器及電機驅動板，用來控制包括操縱臂和機械臂在內的4個手臂。如圖3所示的控制架構含有1個控制計算機、4個FPGA控制器及電機驅動板QLA，用于2個機器人手臂的控制和通信。

圖3 dVRK控制架構

多個FPGA控制器和電機驅動板通過IEEE-1394a(FireWire)菊花鏈接成一個FireWire網，以太網到FireWire橋接節點接收來自主機PC的以太網數據包產并轉換為FireWire數據包，執行FireWire操作，同時將FireWire響應的數據包轉換為PC的以太網數據包。上位機通過以太網向FPGA板發送/接收數據包，然后FPGA通過FireWire網絡與多個分布式節點通信。

從PC到機器人各個控制節點的數據幀包含了4字節讀/寫、數據塊讀/寫，基于廣播的寫/查詢和系統同步等內容，數據幀包含數據包頭和校驗部分。

2.2 PSM機械臂PD關節位置跟蹤控制

dVRK機械臂的關節位置控制主要分為關節空間控制和笛卡爾空間控制。關節空間的控制由逆運動學模塊將笛卡爾任務軌跡映射到關節空間，得到關節空間軌跡之后通過關節層控制器跟蹤軌跡。

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

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

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

圖4所示為機械臂實時位置控制模型，目標機實時計算來自上位機的各關節期望電機位置與編碼器反饋的位置差值，PD調節器模塊自動調整每個機械臂驅動器的最佳PD參數，PID控制器計算出控制量并通過DA轉換傳送到電機驅動板，構成機械臂關節電機實時位置的閉環控制。

圖4 dVRK機械臂實時位置控制

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

上位機PC通過以太網UDP設置每個PSM執行關節的期望位置，目標機計算出實際偏差控制量，并通過Simulink Real-Time UDP傳輸給dVRK PSM控制器，最后輸出執行器動作的偏移量，使機械臂到達期望的位置。

2.3 PD Tuner模型設計

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

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

仿真運行單關節的PD自動調節模型塊后，以同樣方法配置其他機械臂關節電機的PD自動調節模式，實現對dVRK機械臂的實時位置控制。

3 實驗結果及分析

3.1 機械臂模型建立

Robotics Toolbox機器人應用工具箱提供了針對機器人的包括運動學、動力學和軌跡規劃等功能的函數。根據D-H參數表，利用Toolbox應用工具箱中link()和robot()函數建立如圖6所示的機械臂三維模型，構建 PSM模型命令如下：

圖6 dVRK PSM 7-DOF機械臂三維模型

L1=Link(0,0，pi/2，pi/2)；%采用標準D-H參數構建各個關節：

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′)；%構建機械臂

robot.teach()；%用于調節關節量，實時演示機械臂空間位姿

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

3.2 機械臂控制

基于dVRK機器人實時控制平臺，在宿主機與目標機之間建立網絡連接，設置應用程序實時運行環境，在宿主機Simulink環境中進行算法建模和仿真，Real-Time Workshop(RTW)實現控制算法的自動代碼生成，并對目標機進行代碼下載、控制和數據通信。運行宿主機和目標機上Simulink Real-Time模型后讀取到dVRK機器人機械臂PSM各關節編碼器傳感數值、電勢、速度、反饋電流和期望電流值，以及包括運行狀態、時間戳等的調試信息。

如圖7所示的目標機實時數據顯示界面，首行顯示了目標機程序當前運行時間為49 s，采樣頻率為5 kHz，9個Scope信號窗口正確顯示了目標機Simulink模型實時運行中各伺服電機的設定轉速、編碼器讀數、PID偏差調節量、DA轉換值等用于電機實時控制的監測信號。

圖7 目標機實時數據顯示

將電機控制響應數值導出到Workspace并可視化后得到如圖8所示的dVRK PSM PID響應曲線。可知：P增益為30、I增益為1、D增益為1.5；系統響應曲線總體上與輸入控制信號吻合，機器人操作臂關節電機的偏差電流能在較短時間內準確跟蹤期望值，無超調，穩態過程存在最大幅值為1%的波動，完全滿足控制要求。

圖8 dVRK PSM PID響應曲線

4 結論

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