FORCE檢測與回報執行個體

1. 文檔概述
   1. 介紹

本文大體上分為三個部分，一是闡述gripper夾物判斷的實作方法和實體原理。二是利用gripper夾物判斷和Omega配合後實作的力回報系統的結構和原理。三是現有的問題。

一、gripper夾物判斷的實作方法和實體原理

         先從底層硬體驅動電路的角度出發，對gripper電機進行了動力學分析，得出了電機勻速時，電機速度、PWM占空比和檢測電流的數學關系，并利用此數學模型結合實測結果對該速度電流曲線做标定，然後做一些理論上可以允許的簡化和假設。據此，我們可以從電機速度和驅動PWM占空比的資訊算出器械在空載時的加速度理論值，利用該值和從編碼器獲得的加速度實際值做對比，作為gripper夾物判斷的依據

• 力回報系統的結構和原理

有了夾物判斷的方法後，将其實作在PC端的下位機中，同時且實時地将夾物判斷的結果上傳到PC端的上位機(Omega控制程式)、下發到器械端的單片機(器械Tool控制程式)中。上位機通過夾物判斷的結果和Omega-Gripper的開合角度動态地調整回報到手上的作用力，單片機通過夾物判斷的結果和Omega下發的位置控制指令實時地切換電機控制方式(力控制和位置控制)，整個系統便是一個帶有力回報效果的主從跟随系統。

1. 1. 适用範圍

研發部軟體、硬體驅動相關研發成員。

1. Gripper夾物判斷方法
   1. 電機驅動電路分析

要着手分析電機的力學模型，要先了解電機的驅動晶片的結構。

圖二 直流電機驅動晶片MAX14871資料手冊中的推薦電路

表一 直流電機驅動晶片MAX14871資料手冊中的控制邏輯表

圖二和表一均為器械控制闆所選用的直流電機驅動晶片MAX14871的資料手冊中的内容。不妨設流經電機線圈的電流為，流經電阻上的電流為，該電流可被單片機的ADC采樣得到。我們看到電機的驅動電路為H全橋結構，當晶片的使能腳使能後，若此時PWM控制信号為1(高電位)，電機線圈一端接Vdd另一端接GND，此時=±(符号由DIR腳的電平決定)。相反的，若此時PWM控制信号為0(低電位)，電機線圈兩端都接GND，電機線圈短接，此時電機線圈的電流很大，該電流阻止着電機的進一步轉動，電機處于高阻尼的刹車狀态會馬上停下。而這個過程中的=0，意味着我們得不到此時電機線圈的電流。而在理論上，我們可以知道=，其中為電機轉動引起的反向電動勢，為電機線圈内阻，為線圈電感的複阻抗。我們又知道正比于電機旋轉角速度，比例系數為，即=(取決于電機内部磁場、線圈結構。同樣的，電機産生的輸出力矩與線圈電流也成正比，比例系數也為這個，=，我們在電機的選型手冊中可以找到這個，如圖三，對于我們使用的24V電機，其Torque constant即是 = 16mNm/A，同樣也可以由Speed constant這一項求出:= 60/(2*Speed constatnt)=60/(2*595)=0.0160Nm/A )。

圖三

1. 1. Gripper勻速開合，電機速度與電流的關系

由上節我們知道，當PWM信号為1時，電機有個正向加速力矩=*，此時電機加速。當PWM信号為0時，電機有個反向力矩==（為此時的），此時電機刹車。而PWM控制信号的頻率很快（目前BMR手術系統中為15.69KHz）使得電機不停在加速減速下切換以至于從宏觀上來看，當PWM信号占空比一定時，電機勻速轉動。為了保持電機勻速，由角動量守恒可得：

               ①            其中d為PWM占空比（0≤d≤1）

從電學角度：

                           ②            其中為驅動電壓24V，=

單片機的ADC測到的電流值為在整個PWM周期内的平均值

=                                    ③

由①②③可以得出電機勻速運動時單片機擷取到的電機電流與電機轉速和占空比之間的關系：

                    ④            其中為驅動電壓24V，=

 =                                              ⑤  

将此處的記為。

對于目前15.69KHz的PWM控制頻率，我們取其一階基頻，近似認為==

再由圖三得到電機的線圈電阻和電感分别為，，可以畫出如下曲線

圖四 恒定負載且勻速轉動時電機速度電流理論曲線

圖五 恒定負載且勻速轉動時PWM占空比和速度理論曲線

我們可以得出：随着電機轉速的加快先增大，最大時為，此時電機發熱最大，效率最低。轉速繼續增大，電流開始下降，直到電機産生的反向電動勢和激勵電壓相抵消，電流為0，此時轉速最快。該曲線結果中的電流趨勢和最大值和實際測量結果非常符合。

而PWM占空比和電機轉速呈簡單的線性關系。

而實際中，還要考慮摩擦力的影響，包括電機内部的轉動摩擦力矩，器械傳動時帶來的摩擦力矩等等，把他們一并記為(由于器械加工精度和不同開合角度下力傳導的非線性，摩擦力不僅和角速度有關，還和角位置有關，為了簡化問題，我們假設隻與角速度有關)

①式改寫為

                ❶

結合②③可以推出：

                     ❹

                                        ❺

由于此時的是電機勻速轉動時的電流測量值，将其改記作。并将寫成兩部分的組合=+，其中即等于④式，為非摩擦力部分。為摩擦力部分。

由于對摩擦力的數學模組化比較複雜而且一緻性較差，我們采用實驗的辦法進行标定。

圖五 5mm持針器驅動電機轉速與電流曲線

考慮到實際應用場景中電機轉速很少會高于6000rpm/min，是以我們隻測量了電機轉速在7000rpm/min以下的情形，可以看到，在低速情況下，實際與理論值電流值很接近，摩擦力項很小。當電機轉速提升，實際與理論值差距變大，項影響不可忽視。

1. 1. gripper夾物判斷

由于實際的操作器械過程中，電機不可能處于勻速狀态，始終有個加速度，記為。即不可能永遠等于，他們的內插補點反映了電機加速度，由電機的動力學關系可知：

      ⑥  

其中為考慮到不同占空比下的的修正值：

為加入器械後電機的等效轉動慣量（如同，為了簡化問題，我們假定其為常量，并由實驗測量标定得出）。

由此，我們可以根據電機的轉速、測量得到的電機電流、PWM控制信号的占空比這三個已知量算出電機在這一時刻的加速度，與通過編碼器獲得的電機在這一時刻的實際加速度做比較，作為器械夾物判斷的依據。

  ⑦

如果大于某個門限，如>=0.5，就意味着電機實際加速度還沒有達到計算得出的加速度值的一半，認為器械前端gripper受到了外物阻擋，使得整體的電機等效轉動慣量變大了。

綜上，給出了一種實時性較高且較靈敏的判斷gripper是否夾到物體的方法。

1. 1. Gripper夾物判斷方法的實驗驗證

我們使用二自由度5mm持針器做了實際測試。實際使用中，我們隻考慮gripper閉合時有沒有碰到物體而受阻，不考慮gripper張開時的情況。我們将的門限設定為60%，并且隻有當理論值大于某個門限（否則，容易産生誤判）時，才進行夾物判斷。實驗結果表明，夾物判斷算法運作良好，可以非常靈敏地檢測到一個很小的外力，空載時在器械的整個自由行程範圍内不會産生夾物誤判。夾物檢測的結果有50~100ms的判斷延時。

1. 力回報系統的結構
   1. 總體結構

圖六為目前實作的力回報系統的總體結構

圖六 力回報系統的總體結構

夾物檢測的運算在PC端的下位機中實作，它将判斷結果同時上傳給上位機、下發給單片機。

1. 1. 單片機控制電機部分邏輯

圖七 驅動闆MCU控制邏輯

單片機把和電機有關的電流、PWM占空比、電機速度(加速度)等資訊上傳給下位機。下位機根據此資訊結合夾物檢測算法判斷gripper有無夾物并回傳給單片機。

單片機根據gripper位置指令和夾物判斷的結果判斷電機控制子產品應該工作在哪個模式。

如果gripper位置指令在gripper行程範圍之内并且沒有檢測到夾到物體，單片機對電機進行位置控制。如果位置指令小于gripper的零位，或者檢測到gripper夾到物體，進入電流環控制電機，電流值的設定取決于從上位機下發的Omega_Force。

1. 1. 上位機邏輯

圖八 上位機邏輯

圖八中的SpringGate是上位機為Omega設定彈性力的參考原點，隻有當Omega的Grip自由度的開合角度OmegaGripperPosition小于SpringGate時，上位機才會為Omega的Grip自由度設定一個正比于他們內插補點的彈簧力。

當檢測到gripper夾到物體時，SpringGate會被設定成目前Omega的Grip自由度的開合角度。此後，若操縱者繼續往内壓Gripper，操縱者的手上會感受到一個反作用力，會有壓到彈簧的感覺。并且，此時的反作用力大小還會下發給單片機，單片機以此為依據設定電流環的電流值。這樣，可以把操作者手上的力和電機進入電流模式下的輸出轉矩對應起來。

1. 遇到的問題

1. 如何讓Omega施加給操作者手上的反作用力OmegaGripperForce正比于器械前端gripper的夾持力。

目前讓Omega産生力是通過Omega SDK中提供的API ，SetGripperForce（...）（機關牛頓）實作的，其準确度有待驗證。這個力的值經過線性放大對應電機的設定電流值，電流值的設定精度取決于電流環的性能。這個電流值使得器械前端産生夾持力，我們認為當器械夾到物體靜止後，電流值和末端夾持力成正比。

整個過程中，在理論上都是線性的，即操作者施加了一個力F1後，經過放大，在器械前端産生一個夾持力F2，F2=kF1，K為常數。具體是否線性，線性度如何，需要工裝去測量。

1. 實驗中發現，當gripper夾到物體後omega繼續向内擠壓，器械前端夾持力會增大。但是，當操作者慢慢松開OmegaGripper，即将手上力慢慢撤掉的過程中（但不完全撤掉），器械前端的夾持力還是會保持在之前的最大值，并不會和手上的力一起慢慢變小。直到操作者将手上的力完全撤掉，電機進入位置模式，器械前端的夾持力才會消失。

這可以說是一個問題，也可以說是一種特性。這是由于電機驅動晶片在不工作時（PWM信号為低時）使得電機處線上圈短接的刹車模式下。即使不給電機驅動電流，電機也會維持不轉，導緻gripper因夾到物體導緻的彈性形變不能恢複進而力也依舊保持着。對于持針器而言，這個特性可以使醫生的手上不必全程保持最大的力就能達到最大夾持力的效果，是個優點。而對于其他器械而言，這個特性會使得醫生發現自己用力過大後難以通過減小手上的力使器械端的力變小，是個缺點。

1. 夾物判斷有延遲。

目前夾物判斷的算法有50ms左右的延遲，導緻的問題在操作者閉合OmegaGripper的過程中，如果操作速度太快，即使器械gripper夾到了物體，已經被卡住，而上位機的SpringGate無法捕捉到真正夾到物體時的器械角度，使得操作者誤以為gripper閉合的過程中并沒有夾到物體。

解決的辦法是減小夾物判斷門限，使其變得更靈敏，但是如果把夾物檢測設定得太靈敏，有可能會在器械空載的自由行程中觸發夾物誤判，影響操作者使用感受。