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

曹 燕，尚萬峰，張瀟云

（西安科技大學(xué)機(jī)械工程學(xué)院，陜西 西安 710054）

1 引言

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

以實(shí)際的機(jī)器人本體為研究對象，通過分析實(shí)時(shí)采集的機(jī)器人關(guān)節(jié)運(yùn)動(dòng)數(shù)據(jù)，并根據(jù)醫(yī)療機(jī)器人的實(shí)際情況選擇合適的摩擦力模型，區(qū)別于被廣泛使用的最小二乘擬合法，采用的切比雪夫擬合法對于機(jī)器人的摩擦數(shù)據(jù)分布規(guī)律性差的特點(diǎn)有著更好的效果，算法的有效性最終通過實(shí)驗(yàn)得到驗(yàn)證。

2 動(dòng)力學(xué)模型

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

圖1 柔性多關(guān)節(jié)醫(yī)療機(jī)器人Fig.1 The Mobile Robot Platform

表1 DH 參數(shù)Tab.1 DH Parameter

牛頓-歐拉內(nèi)推，從基座到末端，計(jì)算速度、加速度：

其中，各個(gè)關(guān)節(jié)實(shí)際的受力可以通過關(guān)節(jié)電流獲得，通過重力、摩擦力可以分析得出各個(gè)關(guān)節(jié)受力的情況，最終通過伺服力矩輸出到機(jī)器人本體來實(shí)現(xiàn)對機(jī)器人的直接示教，如圖2 所示。

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

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

3 關(guān)節(jié)摩擦力分析

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

3.1 摩擦力采集

在理想的動(dòng)力學(xué)模型中，認(rèn)為摩擦力近似為零，在機(jī)器人運(yùn)動(dòng)過程中，可以實(shí)時(shí)計(jì)算得出當(dāng)前驅(qū)動(dòng)機(jī)器人運(yùn)動(dòng)所需要的力矩，同時(shí)可以通過伺服的電流來獲取機(jī)器人當(dāng)前的實(shí)際輸出力矩，而二者的差值即為當(dāng)前的實(shí)際摩擦力。

式中：Tactual—實(shí)際采集的力矩；

Ttheory—?jiǎng)恿W(xué)模型所輸出的當(dāng)前理論力矩。

以機(jī)器人的關(guān)節(jié)1 為例，驅(qū)動(dòng)機(jī)器人第一個(gè)軸在不同速度下做往復(fù)運(yùn)動(dòng)，采集到的摩擦力的2004 組數(shù)據(jù)，如圖3 所示。

圖3 機(jī)器人關(guān)節(jié)摩擦力Fig.3 Friction of Joint

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

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

3.2 摩擦力模型分析

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

3.3 擬合優(yōu)度

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

式中：y—原始縱軸數(shù)據(jù)；—擬合后的結(jié)果；—原始數(shù)據(jù)的平均值。可決系數(shù)是衡量擬合整體效果的標(biāo)準(zhǔn)，其越接近于1 則效果越好，反之效果越差[8]。

3.4 摩擦力辨識

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

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

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

4 實(shí)驗(yàn)驗(yàn)證

實(shí)驗(yàn)部分主要針對三個(gè)方面：（1）濾波效果，是否能夠獲得較為理想的摩擦數(shù)據(jù)；（2）摩擦辨識，擬合后的結(jié)果是否能夠達(dá)到較高的擬合優(yōu)度；（3）直接牽引示教過程中，本體運(yùn)動(dòng)是否柔順，是否會(huì)由于補(bǔ)償過大導(dǎo)致機(jī)器人失控飛車。通過濾波器后電流中混雜的高頻噪聲被有效的濾除，如圖4 所示。

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

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

圖5 五次擬合結(jié)果Fig.5 Fifth Order Fitting Results

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

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

圖6 牽引示教Fig.6 Traction Teaching

5 結(jié)論

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