基于六自由度機械臂的艾灸機器人手法控制算法

白善明, 崔娟*, 張志東, 張天生, 鄭永秋, 薛晨陽

(1.中北大學儀器科學與動態測試教育部重點實驗室, 太原 030051; 2.山西省針灸醫院, 太原 030000)

艾灸是中國傳統醫學的重要組成部分,在中華民族的防病治病中發揮了重大作用,廣泛應用于疾病治療、康復保健、延緩衰老等領域。其具有效果明顯、簡便易行、經濟實用等優點,在越來越多的國家得到推廣。但在實際艾灸治療中,由于施灸時間過長、醫生疲憊不堪,既造成施灸不準,影響艾灸手法的重復性,也對醫者手臂手腕形成損傷。以采用機械臂設計的艾灸機器人代替醫生完成長時間的艾灸治療工作,可減輕醫生負擔,降低醫學消耗。

近年來醫療輔助機器人快速發展[1],且具有極高的針對性,包括穿戴式下肢骨骼機器人[2]、中醫脈診機器人[3]、輔助移位機器人[4]、針灸及艾灸機器人等。其中,艾灸機器人的發展由各類艾灸器具、艾灸盒的形式,逐步演化為智能化交互機器人和多姿態艾灸機械臂。唐菊麗等[5]提出了一種新型艾灸盒,基于巧妙的結構設計和電子技術,實現了艾灸設備的溫控、除煙等功能;戴耀南等[6]通過對艾灸機理和機器人運動學的研究,研制了人體脊椎恒溫艾灸機器人;鐘義[7]引入傳感與控制系統設計,提升了艾灸機器人的人機交互體驗;樂文輝等[8]基于D-H參數法建立艾灸機器人模型,通過MATLAB仿真,驗證了艾灸機器人良好的運動學性能;刁吉瑞等[9]集成2R1T并聯機構,設計了五自由度的混聯艾灸輔助機器人,可實現艾灸輔療中的姿態轉換。然而,人體穴位角度多樣,絕大多數艾灸手法都需要以復雜的角度和軌跡來實現?；谏鲜鲅芯堪l現,當前的艾灸機器人所用的機械臂自由度較低,雖然初步實現了艾灸輔療過程中的姿態控制,但仍無法模擬醫生復雜的手臂動作和艾灸手法,特別是無法對于人體側方的穴位實施主動艾灸;除此之外,當前艾灸機器人智能化程度低,在治療過程中控制系統無法根據實時的溫度監測信息對艾灸軌跡進行重新規劃。

因此,現基于六自由度艾灸機械臂,通過機器人操作系統(robot operating system, ROS)[10-11],致力于研制一種可模擬各類復雜艾灸手法的輔助型艾灸機器人。該機器人使用MoveIt平臺對機械臂運動學控制進行快速配置[12],可以實現機械臂末端在任意坐標點的姿態變換。通過引入笛卡爾軌跡規劃和多項式插值方法[13],建立不同艾灸手法所需的末端軌跡曲線求解算法,希望機械臂在任意姿態下快速完成軌跡規劃,以及對各類艾灸手法的精準模擬。同時,在機械臂末端引入溫度監測及反饋系統,達到實時反饋患者表皮溫度變化的效果,并由此調節機械臂軌跡運行的高度、方向及速度等參數,有望真實模擬醫生在艾灸治療過程中根據溫度變化做出的手法起伏,提高艾灸機器人操作過程的安全性、智能性和友好性。

1 機器人控制系統設計

智能艾灸機器人的核心為六自由度機械臂,如圖1(a) 所示,其主要完成的工作是靈活調整關節、快速響應移動、長時間穩定的保持某種姿態,進而實現對人體手臂結構的真實模擬。同時,為了便于機械手臂在運行過程中對艾柱的固定,本文研究設計了艾柱夾具并將其安裝在機械臂末端,如圖1(b)所示,艾條豎直固定在夾具的中心位置,將各個非接觸式傳感器固定在夾具邊緣位置,如圖1(c)所示;除此之外,夾具本身的錐形設計還可以結合風機實現對艾煙的高效吸收。特別的,在實際模擬回旋灸手法的過程中,由于一些穴位處所需的施灸半徑較小,這種情況下機械臂的整體運動顯得過于笨重,無法高效便捷的實施艾灸,因此本文研究基于偏心輪設計了一款偏心旋轉夾具,如圖1(d)所示,可將不同規格的艾柱固定在夾具邊緣處,由機械臂末端電機帶動其沿某一方向轉動,從而達到持艾柱在穴位點處走過特定圓形軌跡的效果。

圖1 機械臂結構設計Fig.1 Structural design of mechanical arm

機器人模擬人體手臂進行艾灸治療,需要機械臂具有更高的穩定性和靈活度,因此在艾灸機器人的設計過程中(圖2),選用MBDHT2510伺服電機驅動器和MSMD042G1U伺服電機作為機械臂關節的執行機構。機械臂模擬艾灸手法時,往往需要其在低速狀態下保持平穩運行,而大多數電機由于工作原理的限制,極易出現低頻振動現象,這對于機器人的正常運轉非常不利;相比之下,伺服電機具有共振抑制功能,根據系統內部的頻率解析機能,可檢測出機械結構的共振點,便于系統做出調整,運作十分平穩,即便是在低速時也不會出現振動現象。同時,伺服電機具有內置編碼器,可以實時測量電機磁極位置、伺服電機轉角及其轉速,形成閉環控制系統,使機械臂具有更高的精度和穩定性。

圖2 機械臂控制系統架構圖Fig.2 Structure diagram of manipulator control system

在艾灸治療過程中,為防止高溫燃燒的艾柱燙傷人體,設計溫度監測及反饋系統,需要在機械臂末端配置紅外陣列傳感器,實時監測施灸穴位點的皮膚溫度;本文選用了型號為MXL90614的紅外傳感器,將傳感芯片測得的溫度信息轉化為相對應的電壓模擬量傳輸至機械臂的上位機系統,并據此作出反饋。

將STM32芯片作為機械臂控制系統的主控芯片,通過控制下發至驅動器的模擬量和脈沖頻率,達到控制機械臂關節電機轉速及其轉角的效果,主要內容是通過脈寬調制信號(pulse width modulation, PWM)實現電機轉動控制,輸出高低電平實現電機轉向控制,由驅動器內部提供的雙閉環系統,實現電機方向和速度的突變。由于STM32所能提供的最大控制電壓為3.3 V,不足以滿足驅動器控制引腳的需要,因此加入一個光耦電路進行升壓。選用伺服控制模式為速度控制,將電機的位置信號或直接負載的位置信息反饋給上層的控制系統,從而增加整個系統的定位精度。

2 機械臂軌跡規劃算法

ROS中的MoveIt平臺包含了大量針對機械臂的功能包[14],據此可以快速完成對機械臂運動學解算、碰撞檢測、位置插補等一系列工作,因此在對艾灸手法軌跡進行規劃前,需要完成對機械臂的基本建模和配置: 創建機械臂的URDF模型,其中包含了機械臂的整體結構,尺寸大小以及運動學參數等關鍵信息;據此配置其自碰撞矩陣,MoveIt會隨機選取采樣點生成碰撞參數,檢測到永遠不會產生碰撞的關節桿件;之后便可以為機械臂配置運動學求解器,運動學算法是機械臂各類算法的核心,它包含了正運動學求解和逆運動學求解等算法,MoveIt中默認采用運動學-動力學庫(kinematics-dynamics library, KDL)完成對機械臂的運動學解算;最后定義機器人位姿,生成機械臂配置文件,并其中添加控制器插件,實現MoveIt與下層控制和驅動之間的通信。

2.1 基于艾灸手法的軌跡規劃算法

在使用機械臂模擬實施艾灸治療時,需要由醫生通過拖動示教模式獲取多個穴位點在笛卡爾空間中的坐標位置,之后再由機械臂的軌跡規劃算法解算出基于當前坐標所展開的艾灸手法的運行軌跡,即計算出艾灸手法的起始點和終止點,通過定義中間點,對艾灸手法的軌跡曲線進行擬合,得到平滑穩定的運行軌跡,模擬真實艾灸動作。為了解決這一問題,依據正運動學算法,得出艾灸手法軌跡關鍵點坐標的求解方法。

模擬艾灸手法的核心是找到艾條的燃燒點相對于穴位點的運行軌跡,采用WTGAHRS3慣性導航模塊,將其固定在艾條末端,并由醫生實施專業的艾灸動作,進而捕獲到相對于穴位點坐標系的艾灸手法運行軌跡,將完整的軌跡曲線根據速度變化情況分割成若干段不同的圓弧曲線,這些曲線段之間的連接點即為艾灸軌跡的關鍵點,這樣取點也便于機械臂在模擬艾灸過程中對其速度進行精確控制,使其對艾灸手法的模擬更加真實。已知機械臂末端與穴位點之間的位置關系,即可求解出軌跡關鍵點在機械臂夾具末端中心坐標系(tool center point, TCP)中的表示方法。機械臂坐標系表示方法如圖3所示。

由于人體穴位并非全部垂直向上,因此大多數艾灸手法都是以復雜姿態和軌跡呈現的,需要機械臂在當前TCP末端所處的平面上平緩走過特定的圓弧軌跡。笛卡爾軌跡規劃算法至少需要3點才能確定一個三維空間內的圓弧,故各種艾灸手法軌跡的關鍵點通常均選取為3個。只需預先解算出機械臂末端所在的平面,以及此平面上圓弧軌跡中3個關鍵點在基底(Base)坐標系下的表示方法,機械臂便可通過逆運動學方法到達這些位置。

機械臂是由多關節組成的機械結構,每個關節的位置和姿態發生變化都會對其他關節產生影響,在各個關節分別建立坐標系,則關節在空間中的姿態變化可等效為各坐標系的相對變換,即以平移變換、旋轉變換及其組合變換來表述。

圖3 機械臂坐標系Fig.3 Coordinates of the manipulator

圖4中,設坐標系B是由坐標系A經過平移、旋轉后得到的,點P、Q、T是坐標系B的X-Y平面上圓弧軌跡中的3個點,且點P、Q、T相對于坐標系B的坐標是不變的;若把坐標系A視作機械臂的Base坐標系,把坐標系B視作機械臂末端的TCP坐標

圖4 軌跡關鍵點Fig.4 Key points of trajectory

系,那么只需要求解出點P、Q、T在坐標系A下的坐標表示,即可得到機械臂隨機運動后,TCP末端所處平面的圓形軌跡上的3個點在Base坐標系下的表示方法。以點P為例,設P在坐標系B下的表示為(x,y,z),則只經過坐標系平移變換后P點在坐標系A下的表示為

AP=APB+BP

(1)

(2)

式中:AP為P點在坐標系A下的坐標;APB為坐標系B相對于坐標系A的坐標,即為坐標系B對于坐標系A的平移變換。

只經過坐標系旋轉變換P在坐標系A下的表示為

(3)

(4)

由式(4)可知,坐標系的旋轉變換可由旋轉矩陣表示,即B坐標軸上的單位向量在A坐標軸單位向量上的投影。經過坐標系平移、旋轉變換整合后P在坐標系A下的表示為

(5)

(6)

(7)

由于機械臂是多連桿坐標系組合的結構,所以點P經過連續變換后在坐標系A下的表示方法為

(8)

同理,可以求得Q和T點經過隨機變換后在坐標系A下的表示方法。

圖5 軌跡關鍵點的求解方法Fig.5 The solution method of trajectory key point

至此可以得到TCP末端所在平面上圓弧軌跡中多個關鍵點的坐標位置,即艾灸手法所需要的起始點、終點或中間點。系統在這些點之間形成的軌跡是隨機曲線,不僅無法完成對艾灸手法的模擬,甚至可能觸碰并燙傷患者。采用笛卡爾空間軌跡規劃對中間路徑的擬合,可生成特定的直線、圓弧及其組合曲線; 運用坐標系變換的方法,計算出軌跡運行的方向及旋轉角度,得到各插補點在坐標系中的值,從而真實模擬當前穴位下艾灸治療手法的運動軌跡(圖5)。

由ROS中的MoveIt系統完成機械臂路徑規劃后,MoveIt會根據控制指令,發布一個名為joint_path_command的話題消息,其中包含了運動學求解器插件對機械臂路徑規劃后的所有位點信息,展示了機械臂將會以何種姿態和軌跡運動,即笛卡爾軌跡規劃后產生的所有插補點。然而MoveIt解算輸出的軌跡規劃數據僅有對于機械臂各個關節角度的插值數據,沒有對于其角速度和角加速度的插值求解,這無疑增大了機械臂末端的跟蹤誤差,導致機械臂在運行過程中產生較強的抖動。為了解決這一問題,在MoveIt輸出角度插值的基礎上,加入一個數據處理環,通過五次多項式插值算法對各個角度的插值進行細分,從而得到連續的角速度和角加速度插值,即把機器人的關節變量變換成關于時間的函數,然后對角速度角加速度進行約束,由此便確保了機械臂在運行過程中的角速度是連續可導的,增強了機械臂在運行過程中的平穩性。圖6中展示了3種艾灸手法的仿真結果,圖7為機械臂軌跡規劃算法的邏輯框圖。

2.2 溫度檢測及反饋系統

在機械臂模擬人體手臂實施艾灸治療的過程中,燃燒的艾柱離人體表皮的距離非常近,極易發生燙傷,對患者造成損失。本文研究引入溫度監測及反饋系統,通過機械臂末端的溫度傳感器,對艾灸部位的皮膚溫度進行實時監測,并將反饋數據上傳到控制系統進行運算,求解出溫度變化的范圍和速度,據此對艾灸機器人的手法軌跡進行重新規劃,改變其運行的方向、速度和位置高度等參數,獲得并運行全新的艾灸手法軌跡,從而確保艾灸過程中,患者穴位點的艾灸溫度可以一直保持在最佳范圍內,同時也真實模擬了醫生在艾灸治療過程中手法上的起伏效果。當溫度發生驟變,突然接近或高于人體耐受溫度的閾值,反饋信號便會回傳給控制系統,其控制機械臂快速抬升并中斷運行程序,有效防止燃燒的艾柱燙傷人體,提高艾灸機器人的安全性。

圖6 基于Moveit完成機械臂軌跡規劃Fig.6 Trajectory planning of the manipulator based on Moveit

圖7 機械臂軌跡規劃算法邏輯圖Fig.7 Logic diagram of trajectory planning algorithm for manipulator

3 系統驗證

為驗證艾灸機器人手法控制算法的可行性和精確度,設計了上位機系統和圖形化操作界面,選取10位志愿者進行100次測試,對艾灸機器人進行各項測試,結果均符合要求,測試方法及測試結果如下。

3.1 測試方法

在使用其圖形化界面操作艾灸機器人進行系統測試的過程中,首先建立上位機系統與下層控制器之間的連接;之后使用機器人的拖動示教模式,通過手動拖拽機械臂的方法,對多個穴位點的坐標進行捕捉,系統會自動根據這些穴位的坐標位置進行解算,得到真實模擬艾灸手法的運動軌跡;在圖形化界面中為不同患者制定個性化的診療方案,包括選取不同位置和角度下的穴位點,設定艾灸手法、施灸時間、溫度閾值等。記錄艾灸機器人正常工作的次數,以及是否能夠按照預設的時間進行艾灸;特別的,對于溫度監測及其反饋系統,人為改變穴位點的皮膚溫度,記錄機器人正常反應的次數;除此之外,為了驗證機械臂能否有效的模擬艾灸手法,控制器會以每秒兩次的頻率向上位機系統發送其運行狀態和機械臂末端的空間坐標。根據這些信息對機械臂在運行過程中的軌跡曲線進行擬合,并與醫生艾灸過程中的真實的軌跡曲線進行對比,從而判斷艾灸手法控制系統的準確性。

3.2 測試結果

通過對10位不同志愿者進行的測試,得到表1中的測試結果,其中艾灸機器人的工作狀態出現了1次失敗,原因是中途停止并變更診療方案后,再次運行系統程序,先前設定機械臂回歸原點的指令重復執行,對程序進行修改之后再未出現過類似現象。通過表格中的測試結果可知系統正確執行率較高,說明艾灸手法控制系統可正常運行,驗證了其可行性。

表1 測試結果Table 1 Test results

通過艾灸手法控制算法,本文研究實現了控制機械臂順利完成對各類復雜艾灸手法的真實模擬,包括擺尾灸、回旋灸和雀啄灸等。在艾灸機器人的運行過程中,捕獲在不同時刻下所對應的機械臂的運行姿態,結果如圖8所示。

圖8 機械臂運行測試Fig.8 Operation test of mechanical arm

使用系統回傳的狀態參數,對機械臂的運行曲線進行擬合,與先前捕捉到的醫生實際艾灸過程中的軌跡曲線進行比對。真實艾灸過程中各類艾灸手法都是基于特定平面實施的,為了簡化計算,可將三維空間內的艾灸軌跡曲線轉化為二維平面內的軌跡曲線進行對比,結果圖9所示。

圖9 艾灸手法軌跡對比Fig.9 Comparison of moxibustion technique trajectory

對真實艾灸軌跡進行采集可得到大量的離散數據,運用最小二乘法可將真實曲線進行擬合z并求解出其對應的多項式表達式,在機械臂的運行軌跡中隨機選取100個點,將這些點代入表達式中得到的解,與機械臂運行過程中采樣點的實際值做差,即為誤差。通過計算可得,各類艾灸手法中,每個點的誤差累加后得到的誤差和均不大于1 mm,說明機械臂的運行軌跡和真實的艾灸軌跡高度一致,其艾灸手法控制算法具有較高的精確度。

此外,使用偏心旋轉夾具在小范圍內實施回旋灸時,艾柱末端可以基于穴位點平穩、準確地走過標準的圓形軌跡,與機械臂整體控制模擬回旋灸不同,這種特制的夾具優勢更為顯著,所能完成的圓形軌跡半徑更小、曲率更標準且其運行狀態更平穩,耗能更小。

4 結論

基于六自由度機械臂,提出了一種模擬艾灸手法的控制算法設計與實現。根據人體穴位點坐標,建立人體坐標與機械臂坐標的變換關系,捕獲軌跡關鍵點,通過運動學求解方法和插值數據處理環,實現機械臂在全局坐標系的姿態變化,從而針對不同穴位艾灸需求,執行不同的軌跡曲線,實現對于多種艾灸手法的真實模擬;引入溫度監測與反饋系統,提升了艾灸機器人的安全性、智能性及友好性。最后通過試驗,驗證了機械臂在模擬艾灸治療過程中的可行性、精確度以及安全性。與現有的艾灸機器人相比,具有更高的自由度、穩定性和智能性,對艾灸手法的模擬更加真實,可實現更為復雜的治療方案,有望解放艾灸醫生雙手,應用于醫生輔助艾灸治療。同時,該研究具有友好的人機交互及智能化設計,因此在家庭或個人的自助式艾灸理療市場也具有廣闊前景。