柔性雙臂醫療機器人無傳感器零力控制算法研究

曹 燕，尚萬峰，張瀟云

（西安科技大學機械工程學院，陜西 西安 710054）

1 引言

醫療機器人作為新型的機器人對其交互性，安全性以及操作上的便捷性為主要技術要點而成為近年來學術界以及工程界研究熱點。鑒于醫療機器人的操作者通常為醫師、護理人員以及患者等醫療專業人，但缺少機器人專業知識的群體，醫療機器人對于操作的便捷性的需求已經極為迫切。零力控制是實現直接示教的核心基礎，傳統機器人的示教通常依賴于示教器，需要針對機器人運動所要到達的位置逐條進行編程，對于非專業人員極為繁瑣，導致直接示教成為近年來業內的熱點研究方向。直接示教通常采用伺服接通級直接示教即為伺服電機會按照控制器所下發的力矩或位置指令來執行動作，通常分為伺服工作在CSP 模式下通過末端六維力矩傳感器來實現力位混合控制，以及伺服工作在CST 模式下通過關節力矩傳感器或關節電流來實現關節空間的控制。針對零力控制問題國內外學術界有許多成果，例如文獻1 提出了一種無關節轉矩傳感器的關節轉矩控制人形機器人的穩定控制[1]。文獻2 提出了一種改進的基于電流的無力控制方法，提出了扭矩校準方法和補償模型[2]。上述文獻雖然研究均針對零力控制問題提出了自己的解決方案，但大多數停留在理論以及仿真的階段。

以實際的機器人本體為研究對象，通過分析實時采集的機器人關節運動數據，并根據醫療機器人的實際情況選擇合適的摩擦力模型，區別于被廣泛使用的最小二乘擬合法，采用的切比雪夫擬合法對于機器人的摩擦數據分布規律性差的特點有著更好的效果，算法的有效性最終通過實驗得到驗證。

2 動力學模型

如圖1 所示，為雙臂柔性多關節醫療機器人，包含兩條手臂，每條機械臂具有七個自由度，每個關節均為一體化關節并且不存在隨動。對于本體采用DH 建模，DH 表，如表1 所示。針對機器人本體建立動力學模型時，采用牛頓-歐拉遞推方程，限于機器人控制系統的實時性要求，由于機器人軸數較多，采用拉格朗日方程會增加運算的開銷[3]。

圖1 柔性多關節醫療機器人Fig.1 The Mobile Robot Platform

表1 DH 參數Tab.1 DH Parameter

牛頓-歐拉內推，從基座到末端，計算速度、加速度：

其中，各個關節實際的受力可以通過關節電流獲得，通過重力、摩擦力可以分析得出各個關節受力的情況，最終通過伺服力矩輸出到機器人本體來實現對機器人的直接示教，如圖2 所示。

圖2 零力控制原理框圖Fig.2 Force-Free Control Block Diagram

其中，還需要考慮手臂基座的傾斜，經過標定其零位姿態與水平方向的ZYZ歐拉角為-80.2604°，75.1707°，87.4568°，重力加速度在各軸的分量需要做相應的調整。

3 關節摩擦力分析

相較于采用力矩傳感器的方法，無傳感器零力控制省去了在每個關節增加傳感器的成本以及由于加裝傳感器對于本體柔性的改變，基礎動力學模型相對簡化[4]。但有文獻指出無傳感器模式電流信號噪聲較大，電流信號響應頻率高，容易混雜高頻噪聲，這個問題需要謹慎考慮[5]。

3.1 摩擦力采集

在理想的動力學模型中，認為摩擦力近似為零，在機器人運動過程中，可以實時計算得出當前驅動機器人運動所需要的力矩，同時可以通過伺服的電流來獲取機器人當前的實際輸出力矩，而二者的差值即為當前的實際摩擦力。

式中：Tactual—實際采集的力矩；

Ttheory—動力學模型所輸出的當前理論力矩。

以機器人的關節1 為例，驅動機器人第一個軸在不同速度下做往復運動，采集到的摩擦力的2004 組數據，如圖3 所示。

圖3 機器人關節摩擦力Fig.3 Friction of Joint

從圖3 可見，所采集的摩擦力，摩擦力存在周期性的尖峰，通過分析其周期所對應的關節速度可知：（1）通過關節電流采集力矩信號會伴隨有高頻噪聲；（2）摩擦力與關節角速度相關。為得到有效的力矩信號，在控制系統中加入二階巴特沃斯低通濾波器。

常規的低頻電流數據可得到較為完整的保留，信號的大部分高頻成分可被濾波器濾除。

3.2 摩擦力模型分析

通過濾波后的驅動器信息數據，可以直接獲得效果較好的摩擦力數據，摩擦力的模型可以對大量數據的擬合來完成。目前機器人關節摩擦力的基礎模型主要有三個選擇：（1）庫倫模型+粘性摩擦模型；（2）Stribeck 模型；（3）n次多項式模型。采用模型（1）對于摩擦力因速度而帶來的變化不夠敏感，而導致最終推動關節時易產生過補償而導致機器人不易停止甚至在牽引速度較快時飛車[6]；模型（2）的特征是在高速段的擬合較為準確，在低速段卻會有一定程度的失真；模型（3）的特征是在低速段的擬合較為準確，而高速段會遠離實際數據。

3.3 擬合優度

針對同一組數據選擇不同的函數進行擬合以及選擇不同的擬合方法，在擬合的效果上會有較大的差異，以擬合效果的評判標準—可決系數，可決系數被定義為“1-回歸平方和在總平方和中所占的比率”。實際值與平均值的總誤差中，回歸誤差與剩余誤差是此消彼長的關系。因而回歸誤差從正面測定線性模型的擬合優度，剩余誤差則從反面來判定線性模型的擬合優度[7]。可決系數被定義為：

式中：y—原始縱軸數據；—擬合后的結果；—原始數據的平均值。可決系數是衡量擬合整體效果的標準，其越接近于1 則效果越好，反之效果越差[8]。

3.4 摩擦力辨識

由于柔性雙臂醫療機器人注重安全性的工作特性，其速度通常被限制的較低，較為適用n次多項式模型。采用切比雪夫算法對采集的摩擦力-角速度數據進行擬合，步驟如下：給定n個數據點（xi，yi）i=0，1，L，n-1，首先按照xi升序排列，并從這n個數據點中選取m+1 個不同點u0，u1，L，um構成初始參考點集。設在初始點集上，參考多項式φ（x）在初始點附近的上的取值為：

且φ（ui）的各階差商是h的線性函數，φ（x）為m-1 階多項式，其m階差商為0，由此可求出h，再根據φ（ui）的各階差商，由牛頓差值公式可以求出：

若hh=h則φ（x）即為擬合多項式，若hh＞h，則用達到最大偏差最大值的點xj代替點集｛ui｝中離xj最近且具與φ（xi）-yi符號相同的點，從而構成新的參考點集，用這個參考點重復上述的過程，直到最大逼近誤差等于參考偏差為止。在曲線擬合中大量文獻采用了最小二乘法，其需要提前確定離散點分布情況的階次，即使是相同的離散點所擬合的多項式階次不同所得曲線會有很大差異[9]，并且擬合函數超過三次時所擬合曲線的誤差就會很大并出現病態問題。故最小二乘法應用范圍還是很有限的，離散點規律過于復雜用最小二乘法擬合出來的曲線誤差會非常大。雖然采用切比雪夫算法擬合曲線差值達不到要求仍需重新計算，使得計算繁瑣，但是其借助于牛頓差值，使得擬合曲線各階差值在所規定的范圍內，這樣的指標使得擬合曲線誤差很小所得擬合曲線準確。

4 實驗驗證

實驗部分主要針對三個方面：（1）濾波效果，是否能夠獲得較為理想的摩擦數據；（2）摩擦辨識，擬合后的結果是否能夠達到較高的擬合優度；（3）直接牽引示教過程中，本體運動是否柔順，是否會由于補償過大導致機器人失控飛車。通過濾波器后電流中混雜的高頻噪聲被有效的濾除，如圖4 所示。

圖4 濾波效果Fig.4 Effect of Filtering

對圖3 的力矩曲線進行擬合其可決系數較僅能達到0.4654，不能夠呈現該關節的力矩與關節角速度的關系，改變擬合的階次可決系數的影響不大；而采用圖4 的數據，可決系數可達0.8456 可以較好的還原力矩與關節角速度的函數關系，也可證明濾波的必要性。針對上圖4 中濾波后的力矩曲線原始數據采用五次多項式擬合的結果，可決系數可提升至0.8456，擬合后的標準差為0.01739，擬合效果，如圖5 所示。

圖5 五次擬合結果Fig.5 Fifth Order Fitting Results

從圖5 可見型與濾波后的摩擦力曲線能夠基本吻合。

直接牽引示教的實驗中，按照動力學模型推算重力、慣性力的補償值，并分別按照（3～8）次進行擬合，并按照擬合結果來推算摩擦力，根據方向增加摩擦力補償，采用力矩模式對機器人進行直接示教。操作觸感上，在牽引過程中，初始階段需要一定的推力來開始牽引示教，過程中觸感順滑，且通過手部突然增加推力后，會感受到阻力明顯增大，有明顯的降速制動作用。從3 次擬合增加到5 次擬合的過程中，順滑度有明顯的提升，且在運動到高速段時，5 次擬合結果在機器人停止牽引后可以較快的停止，3 次擬合的停止時間較長，從（6～9）次擬合的結果在速度的邊界處會發生阻力過大或助力過大的情況，在不同的關節有不同的體現，且越高次的擬合運算開銷越大，有降低系統整體實時性的風險，故最終選用5 次切比雪夫擬合。牽引示教實驗現場，如圖6 所示。

圖6 牽引示教Fig.6 Traction Teaching

5 結論

提出了一種基于柔性雙臂醫療機器人的無傳感器零力控制算法，相較于傳統的采用關節力矩傳感器的方案，本方法成本低系統結構簡單，但也提升了直接力矩控制的難度。構建了機器人的動力學模型，機器人的重力、慣性力可以由動力學模型直接給出，通過單關節在不同速度下的往復運動，采集關節電流進而得到了摩擦力-關節角速度的關系數據，針對原始摩擦數據混雜高頻噪聲的情況，通過濾波器有效的濾除了高頻信號，通過對于3～8 次擬合結果在牽引示教中實際效果，最終確定了以5 次擬合結果作為摩擦力的最終模型。相對于3 次擬合，5 次擬合在曲線形狀上可以提供更多的變化，可有效抑制在一定程度上抑制摩擦力補償值隨速度的激增，最后通過牽引實驗，驗證了所述算法對于實際摩擦有著較為準確的跟蹤效果。