本發(fā)明屬于機器人控制領(lǐng)域，尤其涉及一種XYZR四軸鉆孔機器人主動柔順控制裝置及方法。

背景技術(shù)：

目前箱包鉆孔機器人機構(gòu)設(shè)計往往采用傳統(tǒng)機器人機構(gòu)，如龍門式機器人、三軸XYZ直角坐標(biāo)機器人、四軸XYZR鉆孔機器人以及六軸關(guān)節(jié)型機器人等，機器人控制系統(tǒng)往往采用基于運動學(xué)的控制方案，即以打孔位置為控制目標(biāo)進(jìn)行工作，比較適合用于厚板或塊狀等不易變形金屬、塑料板、亞克力板等材料的鉆孔。

現(xiàn)有研究中的主動柔順控制機制依靠六維力傳感器實現(xiàn)力和力矩的檢測，依靠機器視覺傳感器檢測機器人當(dāng)前位姿，綜合上述信息設(shè)計位置/力混合控制器。也有研究靠ETHERCAT主站將機器人驅(qū)動器采集的電機周期性數(shù)據(jù)與設(shè)定的期望值進(jìn)行疊加并進(jìn)行阻抗控制,輸出參考力矩值至機器人驅(qū)動，適用于接觸性作業(yè)應(yīng)用?；蛘咴诠I(yè)機器人上安裝柔性研拋刀具,計算機通過機器人通訊接口與機器人控制器進(jìn)行雙向通訊連接,機器人控制器控制工業(yè)機器人動作,由柔性研拋刀具對固定于工作臺上的工件實施研拋加工，力傳感器將采集的研拋力數(shù)據(jù)做為閉環(huán)反饋信號送至計算機處理，且其關(guān)鍵技術(shù)在于適用多個力傳感器感知環(huán)境作用力，使用繩驅(qū)機器人實現(xiàn)阻抗控制，適用于柔性管臂型環(huán)境作業(yè)。

基于位置控制的鉆孔機器人在給薄片型易變形金屬鉆孔時，采用基于運動學(xué)的控制方案，以打孔位置為控制目標(biāo)進(jìn)行工作，推動鉆頭的力及加速度在控制動態(tài)過程中波動較大、無法保持在合理的特定時變值上，因此會導(dǎo)致鉆孔對象發(fā)生變形、孔形破壞、毛刺過多不合格；目前主動柔順控制方案多適用于表面接觸式并且需要保持特定壓力的工作場合，對于鉆孔等穿破材料表面的工作場合不宜直接適用；目前主動柔順控制方案多采用價格昂貴的力傳感器作為力檢測裝置，成本較高。

技術(shù)實現(xiàn)要素：

為了解決上述問題，本發(fā)明提出了一種XYZR四軸鉆孔機器人主動柔順控制裝置，其控制對象為四路伺服電機模塊、電鉆模塊，所述裝置包括主控模塊、伺服驅(qū)動控制模塊以及人機交互模塊；主控模塊與伺服驅(qū)動控制模塊相互連接，伺服驅(qū)動控制模塊與電鉆模塊相連，主控模塊還與人機交互模塊相互連接；

所述主控模塊用于完成運動學(xué)軌跡規(guī)劃、主動柔順控制決策、專家經(jīng)驗學(xué)習(xí)、加速度-力-電流離線匹配、加速度-力-電流在線匹配、通信管理、IO管理、驅(qū)動數(shù)據(jù)交互；

所述伺服驅(qū)動主控模塊用于實時控制四路伺服電機模塊；

所述電鉆模塊屬于XYZR四周鉆孔機器人的基本部件，包括受控于主控模塊的變頻器和主軸電鉆，用于控制主軸電鉆，完成鉆孔；

所述人機交互模塊用于操作人員進(jìn)行任務(wù)編輯、指令發(fā)送、顯示機器人工作狀態(tài)以及完成與主控模塊通信。

一種XYZR四軸鉆孔機器人主動柔順控制裝置，其特征在于，所述主控模塊包括運動學(xué)軌跡規(guī)劃模塊、主動柔順控制決策模塊、專家經(jīng)驗學(xué)習(xí)模塊、加速度、力、電流離線匹配模塊，加速度、力、電流在線匹配模塊、RS485通信模塊、IO控制管理模塊、電源管理模塊、安全控制模塊、驅(qū)動數(shù)據(jù)交互模塊、工具工件標(biāo)定模塊、PC調(diào)試通信模塊、SD卡讀寫模塊；

專家經(jīng)驗學(xué)習(xí)子模塊，加速度、力、電流離線匹配子模塊，加速度、力、電流在線匹配子模塊，工具工件標(biāo)定子模塊關(guān)系為順序依次串聯(lián)關(guān)系，通過數(shù)據(jù)通信連接，將上述模塊定義為第一類子模塊，第一類子模塊在系統(tǒng)上線運行前只需要運行一次，生成必要的控制數(shù)據(jù)供后續(xù)使用；運動學(xué)軌跡規(guī)劃子模塊、主動柔順控制決策子模塊、IO控制管理子模塊、驅(qū)動數(shù)據(jù)交互子模塊關(guān)系為順序依次串聯(lián)關(guān)系，通過數(shù)據(jù)通信連接，將這些子模塊定義為第二類子模塊，第二類子模塊在系統(tǒng)上線運行時的必須運行模塊，第二類子模塊使用第一類子模塊生成的數(shù)據(jù)，兩者之間通過數(shù)據(jù)通信連接；RS485通信子模塊、PC調(diào)試通信模塊我們定義為第三類子模塊，第三類子模塊用于人機交互操作，與其他類子模塊之間的關(guān)系為并發(fā)同步關(guān)系；電源管理子模塊、安全控制子模塊、SD卡讀寫子模塊我們定義為第四類子模塊，第四類子模塊與其他類子模塊關(guān)系為并發(fā)同步關(guān)系。

主控模塊由基于INTEL X86硬件平臺的WINDOWS系統(tǒng)架構(gòu)工控機或基于INTEL X86硬件平臺的LINUX系統(tǒng)架構(gòu)工控機或基于INTEL X86硬件平臺的VXWORKS系統(tǒng)架構(gòu)工控機或基于ARM架構(gòu)的嵌入式控制平臺構(gòu)成。

所述人機交互模塊包括任務(wù)編輯模塊、狀態(tài)監(jiān)控模塊、指令收發(fā)模塊和串口通信模塊；

人機交互模塊中，任務(wù)編輯子模塊、狀態(tài)監(jiān)控子模塊為并發(fā)同步關(guān)系，兩者分別于指令收發(fā)子模塊、串口通信子模塊通過數(shù)據(jù)線進(jìn)行順序依次串聯(lián)；

人機交互模塊由基于獨立操作系統(tǒng)的示教平臺或基于與主控系統(tǒng)共享操作系統(tǒng)的示教平臺或基于單片機的獨立試教平臺構(gòu)成。

所述伺服驅(qū)動控制模塊包括主控數(shù)據(jù)交互模塊與伺服電機1驅(qū)動控制模塊、伺服電機2驅(qū)動控制模塊、伺服電機3驅(qū)動控制模塊、伺服電機4驅(qū)動控制模塊連接，電機1驅(qū)動控制模塊、伺服電機2驅(qū)動控制模塊、伺服電機3驅(qū)動控制模塊、伺服電機4驅(qū)動控制模塊分別與伺服電機1驅(qū)動放大模塊、伺服電機2驅(qū)動放大模塊、伺服電機3驅(qū)動放大模塊、伺服電機4驅(qū)動放大模塊連接，然后再分別與四路伺服電機模塊連接；四路伺服電機模塊包括伺服電機1、伺服電機2、伺服電機3、伺服電機4，是伺服驅(qū)動模塊的直接控制對象，也是構(gòu)成XYZR四軸鉆孔機器人的基本原動部件；

伺服驅(qū)動控制模塊由多路帶有電流控制、位置控制的完全獨立式伺服驅(qū)動器或多片DSP或FPGA構(gòu)成的相對一體式伺服驅(qū)動器或一片DSP構(gòu)成的絕對一體式伺服驅(qū)動器或一片F(xiàn)PGA構(gòu)成的絕對一體式伺服驅(qū)動器構(gòu)成構(gòu)成。

一種基于權(quán)利要求1所述裝置的XYZR四軸鉆孔機器人主動柔順控制方法，具體為：

步驟1、操作人員通過人機交互模塊與主控模塊進(jìn)行交互，控制機器人完成鉆孔過程專家經(jīng)驗學(xué)習(xí)；

步驟2、通過人機交互模塊命令系統(tǒng)完成加速度-力-電流離線和在線匹配，確定驅(qū)動電流與參考加速度的比例參數(shù)，通過驅(qū)動數(shù)據(jù)交互模塊發(fā)送給伺服驅(qū)動模塊使用；

步驟3、通過人機交互模塊對機器人工作任務(wù)進(jìn)行示教；

步驟4、主控模塊根據(jù)示教任務(wù)完成運動學(xué)軌跡規(guī)劃，生成驅(qū)動數(shù)據(jù)發(fā)送給伺服驅(qū)動控制模塊參考執(zhí)行；

步驟5、主控模塊根據(jù)伺服驅(qū)動控制模塊反饋的機器人狀態(tài)進(jìn)行主動柔順控制決策，確定使用的位置控制參數(shù)和柔順控制參數(shù)，通過驅(qū)動數(shù)據(jù)交互模塊發(fā)送給伺服驅(qū)動模塊參考執(zhí)行；

步驟6、主控模塊同時完成IO控制管理模塊的管理和決策；

步驟7、伺服驅(qū)動模塊根據(jù)主控指令完成各軸伺服電機的實時控制；

步驟8、電鉆模塊根據(jù)IO模塊的狀態(tài)啟停電鉆，進(jìn)行鉆孔。

所述專家經(jīng)驗學(xué)習(xí)的過程即專家經(jīng)驗學(xué)習(xí)模塊的工作的具體步驟為：

步驟101、將電鉆模塊中的加速度傳感器串口數(shù)據(jù)線連接到系統(tǒng)PC調(diào)試串口，通過人機交互模塊啟動鉆孔過程專家經(jīng)驗學(xué)習(xí)模塊，采集N組高級技術(shù)工人鉆孔過程的鉆孔時間和該時間段內(nèi)加速度幅值數(shù)據(jù)，其中N>100；

步驟102、計算采集的N組數(shù)據(jù)的鉆孔時間的平均值、方差以及這些數(shù)據(jù)的97.5%置信區(qū)間；

步驟103、檢查N組數(shù)據(jù)的鉆孔時間是否在②中所述置信區(qū)間內(nèi)，如果在保留，否則剔除，經(jīng)此過程剩下的數(shù)據(jù)假設(shè)為N₁組；

步驟104、對步驟103中的N₁組數(shù)據(jù)進(jìn)行時間歸一化，鉆孔時間大于平均時間的部分，按照平均時間進(jìn)行數(shù)據(jù)壓縮，鉆孔時間小于平均時間的部分，以平均時間為基準(zhǔn)進(jìn)行數(shù)據(jù)擴展；

步驟105、對步驟104中得出的數(shù)據(jù)進(jìn)行平滑處理，處理方法采用滑動平均法，之后將N₁組數(shù)據(jù)按采樣時間點取平均，得到時間歸一化后的平均鉆孔加速度數(shù)據(jù)；

步驟106、對步驟105中得出的數(shù)據(jù)進(jìn)行差分運算，根據(jù)差分結(jié)果將數(shù)據(jù)分為加加速數(shù)據(jù)、勻加速數(shù)據(jù)和減加速數(shù)據(jù)，三類數(shù)據(jù)對應(yīng)的時間段分別為加加速時間段、勻加速時間段和減加速時間段。

所述步驟2中確定驅(qū)動電流與參考加速度的比例參數(shù)的具體過程為

加速度、力、電流離線匹配模塊和加速度、力、電流在線匹配模塊將打孔過程中需要控制的加速度通過力的關(guān)系轉(zhuǎn)化為對推進(jìn)電流的控制；在系統(tǒng)樣機研發(fā)階段，將電鉆模塊中的加速度傳感器的串口數(shù)據(jù)線連接到系統(tǒng)PC調(diào)試串口，通過人機交互模塊啟動在線加速度、力、電流匹配功能，然后加速度、力、電流離線匹配模塊將實際采集到的加速度值與參考加速度進(jìn)行比較，形成負(fù)反饋，調(diào)節(jié)驅(qū)動電流與參考加速度的比例參數(shù)，完成離線匹配；加速度、力、電流在線匹配模塊在首次實際試鉆孔前，啟用加速度、力、電流離線匹配模塊中的驅(qū)動電流與參考加速度的比例參數(shù)，控制驅(qū)動電流推動鉆機前進(jìn)，同時通過機器人當(dāng)前位姿狀態(tài)檢測鉆機位置進(jìn)行二階差分采集鉆機推進(jìn)的實際加速度，然后將該實際采集到的加速度值與參考加速度進(jìn)行比較，形成負(fù)反饋，更新驅(qū)動電流與參考加速度的比例參數(shù)，完成在線匹配。

所述步驟4的具體過程為：

主控模塊中的運動學(xué)軌跡規(guī)劃模塊為實現(xiàn)機器人位置運動過程即非鉆孔過程的運動時間同步性和運動過程平滑性，將當(dāng)前各軸電機運動時間進(jìn)行統(tǒng)一并通過分段控制實現(xiàn)電機速度平滑運動；

步驟401、將電機所要運動的過程分為7段，即加加速、勻加速、減加速、勻速、加減速、勻減速、減減速，設(shè)為加加速段時間、為勻加速段時間、為減加速段時間、為勻速段時間、為加減速段時間、為勻減速段時間、為減減速段時間，表示電機軸的編號；

步驟402、將該運動過程的起點速度v_s、終點速度v_e、最大速度v_m、最大加速度a_m、最大加加速度j_m和總位移S作為規(guī)劃約束條件；

步驟403、規(guī)劃第一軸即X軸變速段運動時間，判斷是否含有均勻變速段，其中加速過程為：

若v_m-v_s> QUOTE ，則t_1-1= QUOTE ，t_1-2= QUOTE ,t_1-3=t_1-1；

若v_m-v_s≤ QUOTE ，則t_1-1= QUOTE ，t_1-2=0,t_1-3=t_1-1，加速過程中的實際最大加速度沒有達(dá)到限定值a_m；

按照求出的時間對應(yīng)各個時間段的加速度進(jìn)行積分，求出各段的相應(yīng)速度，再對各段相應(yīng)速度積分，求出加速段位移S_acc；

步驟404、變速段規(guī)劃中的減速過程利用對稱性得出，減速段位移S_dec=S_acc,t_1-6=t_1-2,t_1-5=t_1-7=t_1-3=t_1-1；

步驟405、判斷運動過程中有無勻速段，判斷準(zhǔn)則為如果S_dec+S_acc<S則含有勻速段，否則不含有勻速段；

步驟406、如果有勻速段，勻速段時間為 QUOTE ,如果沒有勻速段，勻速段時間t_1-4=0；

步驟407、重復(fù)步驟103-106，至完成第二、三、四軸的規(guī)劃；

步驟408、比較各軸的總體運行時間，取最大值為各軸同步運行總時間；

根據(jù)約束方程

求出t₂和t₄；

其中，、和分別為四軸電機同步運動約束后的各軸電機加加速、勻加速和勻速的時間；

若約束方程無解或t₂<0，則認(rèn)為不存在加加速段和減減速段，此時約束方程轉(zhuǎn)化為

根據(jù)該方程求出t₁和t₄，另外根據(jù)對稱性t₇=t₅=t₃=t₁,t₆=t₂=0；、、和分別為四軸電機同步運動約束后的各軸電機減加速、加減速、勻減速和減減速的時間；和是相應(yīng)加加速和減加速階段電機的位移；

步驟409、將上述求得的各個段時間及相應(yīng)段上的位置量和速度量作為驅(qū)動數(shù)據(jù)傳輸至伺服驅(qū)動控制模塊執(zhí)行。

所述步驟5中的主動柔順控制決策即根據(jù)當(dāng)前機器人任務(wù)狀態(tài)和機器人當(dāng)前實際位姿狀態(tài)調(diào)整主動柔順力控制和運動學(xué)位置控制的權(quán)重因子，具體過程為：

主動柔順控制決策模塊的輸入量為人機交互的任務(wù)狀態(tài)和機器人當(dāng)前位姿狀態(tài)，當(dāng)人機交互任務(wù)為鉆孔且機器人當(dāng)前狀態(tài)靠近或已經(jīng)位于鉆孔區(qū)域時，主動柔順控制決策模塊提高柔順控制權(quán)重、降低位置控制權(quán)重，進(jìn)而生成控制參數(shù)調(diào)整指令：使用大柔順控制參數(shù)、小位置控制參數(shù)；當(dāng)人機交互任務(wù)為運動或機器人當(dāng)前狀態(tài)離鉆孔區(qū)域較遠(yuǎn)時，決策模塊降低柔順控制權(quán)重、提高位置控制權(quán)重，進(jìn)而生成控制參數(shù)調(diào)整指令：使用小柔順控制參數(shù)、大位置控制參數(shù)；其中柔順控制參數(shù)和位置控制參數(shù)為事先調(diào)好并保存于系統(tǒng)中的參數(shù)；柔順控制參數(shù)包括大柔順控制參數(shù)和小柔順控制參數(shù)，，位置控制參數(shù)包括大位置控制參數(shù)和小位置控制參數(shù)，。

所述步驟7中的主控指令由運動學(xué)軌跡規(guī)劃模塊和主動柔順控制決策模塊生成，主控指令包括各軸電機參考位置、鉆機推進(jìn)電流和控制參數(shù)調(diào)整指令；

伺服驅(qū)動控制模塊內(nèi)有至少兩套離線調(diào)試完成的位置環(huán)、電流環(huán)參數(shù)和一套速度環(huán)參數(shù)，其中位置環(huán)1、電流環(huán)1參數(shù)適合進(jìn)行精準(zhǔn)的運動學(xué)位置控制，位置環(huán)2、電流環(huán)2參數(shù)適合進(jìn)行鉆機推進(jìn)時的主動柔順力電流控制，速度環(huán)在兩套控制參數(shù)中是一樣的；當(dāng)主控指令中控制參數(shù)調(diào)整指令為使用大柔順控制參數(shù)、小位置控制參數(shù)時，伺服驅(qū)動控制模塊選用位置環(huán)2、速度環(huán)、電流環(huán)2構(gòu)成的控制模塊，此時主控指令中鉆機推進(jìn)電流為主要控制目標(biāo)，運動學(xué)位置控制此時主要起限制保護(hù)作用，防止鉆機推進(jìn)過程產(chǎn)生碰撞等意外發(fā)生；當(dāng)主控指令中控制參數(shù)調(diào)整指令為使用小柔順控制參數(shù)、大位置控制參數(shù)時，伺服驅(qū)動控制模塊選用位置環(huán)1、速度環(huán)、電流環(huán)1構(gòu)成的控制模塊，此時主控指令中的電機參考位置控制為主要控制目標(biāo)，實現(xiàn)系統(tǒng)的快速精確運動。

有益效果

本發(fā)明采用力/位置混合控制架構(gòu)的主動柔順控制機制作為XYZR四軸鉆孔機器人控制方案，在機器人點位運動控制過程中，以位置控制為主，保證運動過程的速度、平滑；在打孔過程中，以檢測位置信息為切換閾值的力控制為主，保證鉆孔過程中機器人推動鉆頭的加速度嚴(yán)格控制在期望值偏差內(nèi)；為避免使用昂貴的力傳感器，在主動柔順控制過程中將加速度控制指標(biāo)轉(zhuǎn)為力控制指標(biāo)、再將力控制指標(biāo)轉(zhuǎn)為電流控制指標(biāo)進(jìn)行設(shè)計，利用機器人控制系統(tǒng)均帶有的電流檢測傳感器作為檢測手段進(jìn)行感知，使用電流環(huán)進(jìn)行控制得到期望電流，驅(qū)動機器人工作，實現(xiàn)主動柔順控制；設(shè)計了一套自動離線/在線匹配加速度、力、電流的流程和裝置，避免了加速度、力、電流之間耗時繁瑣的手動匹配和計算過程；基于鉆孔專家經(jīng)驗學(xué)習(xí)機制，設(shè)計了一套自學(xué)習(xí)專家鉆孔經(jīng)驗的流程和裝置，不需要額外進(jìn)行復(fù)雜的鉆孔過程建模及數(shù)據(jù)辨識。本發(fā)明功能完善、結(jié)構(gòu)清晰、實現(xiàn)簡單、模塊耦合度低；同時實現(xiàn)了系統(tǒng)運動的同步性和平滑性；將主動柔順控制決策模塊單獨進(jìn)行設(shè)計，不再將該部分混合在控制律中，具有清晰明朗、簡單易于實現(xiàn)的優(yōu)點；獲取鉆機推進(jìn)過程中相關(guān)數(shù)據(jù)時避免了復(fù)雜的鉆機推進(jìn)過程建模和無指導(dǎo)性規(guī)律的重復(fù)拼湊實驗，過程簡單、實現(xiàn)方便、節(jié)約時間；避免每臺機器人都需要在線使用昂貴的加速段傳感器和力傳感器，節(jié)約成本，方便用戶使用；系統(tǒng)控制參數(shù)是離線調(diào)試好的，避免在線調(diào)整控制參數(shù)引入的不穩(wěn)定因素和系統(tǒng)宕機過程，運動學(xué)位置控制和主動柔順力控制切換機制合理，結(jié)構(gòu)實現(xiàn)簡單、安全性高、通用性強。

附圖說明

圖1為一種XYZR四軸鉆孔機器人主動柔順控制裝置的組成示意圖。

圖2為主動柔順控制決策模塊的工作過程示意圖。

圖3為伺服驅(qū)動控制模塊的控制框圖。

具體實施方式

下面結(jié)合附圖，對本發(fā)明作詳細(xì)說明。本發(fā)明提出了一種XYZR四軸鉆孔機器人主動柔順控制裝置，如圖1所示，其控制對象為四路伺服電機模塊、電鉆模塊，所述裝置包括主控模塊、伺服驅(qū)動控制模塊以及人機交互模塊；主控模塊與伺服驅(qū)動控制模塊相互連接，伺服驅(qū)動控制模塊與電鉆模塊相連，主控模塊還與人機交互模塊相互連接；

所述主控模塊用于完成運動學(xué)軌跡規(guī)劃、主動柔順控制決策、專家經(jīng)驗學(xué)習(xí)、加速度-力-電流離線匹配、加速度-力-電流在線匹配、通信管理、IO管理、驅(qū)動數(shù)據(jù)交互；

所述伺服驅(qū)動主控模塊用于實時控制四路伺服電機模塊；

所述電鉆模塊屬于XYZR四周鉆孔機器人的基本部件，包括受控于主控模塊的變頻器和主軸電鉆，用于控制主軸電鉆，完成鉆孔；

所述人機交互模塊用于操作人員進(jìn)行任務(wù)編輯、指令發(fā)送、顯示機器人工作狀態(tài)以及完成與主控模塊通信。

所述主控模塊包括運動學(xué)軌跡規(guī)劃模塊、主動柔順控制決策模塊、專家經(jīng)驗學(xué)習(xí)模塊、加速度、力、電流離線匹配模塊，加速度、力、電流在線匹配模塊、RS485通信模塊、IO控制管理模塊、電源管理模塊、安全控制模塊、驅(qū)動數(shù)據(jù)交互模塊、工具工件標(biāo)定模塊、PC調(diào)試通信模塊、SD卡讀寫模塊；

主控模塊由基于INTEL X86硬件平臺的WINDOWS系統(tǒng)架構(gòu)工控機或基于INTEL X86硬件平臺的LINUX系統(tǒng)架構(gòu)工控機或基于INTEL X86硬件平臺的VXWORKS系統(tǒng)架構(gòu)工控機或基于ARM架構(gòu)的嵌入式控制平臺構(gòu)成。

所述人機交互模塊包括任務(wù)編輯模塊、狀態(tài)監(jiān)控模塊、指令收發(fā)模塊和串口通信模塊；

人機交互模塊由基于獨立操作系統(tǒng)的示教平臺或基于與主控系統(tǒng)共享操作系統(tǒng)的示教平臺或基于單片機的獨立試教平臺構(gòu)成。

所述伺服驅(qū)動控制模塊包括主控數(shù)據(jù)交互模塊與伺服電機1驅(qū)動控制模塊、伺服電機2驅(qū)動控制模塊、伺服電機3驅(qū)動控制模塊、伺服電機4驅(qū)動控制模塊連接，電機1驅(qū)動控制模塊、伺服電機2驅(qū)動控制模塊、伺服電機3驅(qū)動控制模塊、伺服電機4驅(qū)動控制模塊分別與伺服電機1驅(qū)動放大模塊、伺服電機2驅(qū)動放大模塊、伺服電機3驅(qū)動放大模塊、伺服電機4驅(qū)動放大模塊連接，然后再分別與四路伺服電機模塊連接；四路伺服電機模塊包括伺服電機1、伺服電機2、伺服電機3、伺服電機4，是伺服驅(qū)動模塊的直接控制對象，也是構(gòu)成XYZR四軸鉆孔機器人的基本原動部件；

伺服驅(qū)動控制模塊由多路帶有電流控制、位置控制的完全獨立式伺服驅(qū)動器或多片DSP或FPGA構(gòu)成的相對一體式伺服驅(qū)動器或一片DSP構(gòu)成的絕對一體式伺服驅(qū)動器或一片F(xiàn)PGA構(gòu)成的絕對一體式伺服驅(qū)動器構(gòu)成構(gòu)成。

基于上述裝置的XYZR四軸鉆孔機器人主動柔順控制方法，其具體過程為：

步驟1、操作人員通過人機交互模塊與主控模塊進(jìn)行交互，控制機器人完成鉆孔過程專家經(jīng)驗學(xué)習(xí)；

步驟101、將電鉆模塊中的加速度傳感器串口數(shù)據(jù)線連接到系統(tǒng)PC調(diào)試串口，通過人機交互模塊啟動鉆孔過程專家經(jīng)驗學(xué)習(xí)模塊，采集N組高級技術(shù)工人鉆孔過程的鉆孔時間和該時間段內(nèi)加速度幅值數(shù)據(jù)，其中N>100；

步驟102、計算采集的N組數(shù)據(jù)的鉆孔時間的平均值、方差以及這些數(shù)據(jù)的97.5%置信區(qū)間；

步驟103、檢查N組數(shù)據(jù)的鉆孔時間是否在②中所述置信區(qū)間內(nèi)，如果在保留，否則剔除，經(jīng)此過程剩下的數(shù)據(jù)假設(shè)為N₁組；

步驟104、對步驟103中的N₁組數(shù)據(jù)進(jìn)行時間歸一化，鉆孔時間大于平均時間的部分，按照平均時間進(jìn)行數(shù)據(jù)壓縮，鉆孔時間小于平均時間的部分，以平均時間為基準(zhǔn)進(jìn)行數(shù)據(jù)擴展；

步驟105、對步驟104中得出的數(shù)據(jù)進(jìn)行平滑處理，處理方法采用滑動平均法，之后將N₁組數(shù)據(jù)按采樣時間點取平均，得到時間歸一化后的平均鉆孔加速度數(shù)據(jù)；

步驟106、對步驟105中得出的數(shù)據(jù)進(jìn)行差分運算，根據(jù)差分結(jié)果將數(shù)據(jù)分為加加速數(shù)據(jù)、勻加速數(shù)據(jù)和減加速數(shù)據(jù)，三類數(shù)據(jù)對應(yīng)的時間段分別為加加速時間段、勻加速時間段和減加速時間段。

步驟2、通過人機交互模塊命令系統(tǒng)完成加速度-力-電流離線和在線匹配，確定驅(qū)動電流與參考加速度的比例參數(shù)，通過驅(qū)動數(shù)據(jù)交互模塊發(fā)送給伺服驅(qū)動模塊使用；

加速度、力、電流離線匹配模塊和加速度、力、電流在線匹配模塊將打孔過程中需要控制的加速度通過力的關(guān)系轉(zhuǎn)化為對推進(jìn)電流的控制；在系統(tǒng)樣機研發(fā)階段，將電鉆模塊中的加速度傳感器的串口數(shù)據(jù)線連接到系統(tǒng)PC調(diào)試串口，通過人機交互模塊啟動在線加速度、力、電流匹配功能，然后加速度、力、電流離線匹配模塊將實際采集到的加速度值與參考加速度進(jìn)行比較，形成負(fù)反饋，調(diào)節(jié)驅(qū)動電流與參考加速度的比例參數(shù)，完成離線匹配；加速度、力、電流在線匹配模塊在首次實際試鉆孔前，啟用加速度、力、電流離線匹配模塊中的驅(qū)動電流與參考加速度的比例參數(shù)，控制驅(qū)動電流推動鉆機前進(jìn)，同時通過機器人當(dāng)前位姿狀態(tài)檢測鉆機位置進(jìn)行二階差分采集鉆機推進(jìn)的實際加速度，然后將該實際采集到的加速度值與參考加速度進(jìn)行比較，形成負(fù)反饋，更新驅(qū)動電流與參考加速度的比例參數(shù)，完成在線匹配。

步驟3、通過人機交互模塊對機器人工作任務(wù)進(jìn)行示教；

步驟4、主控模塊根據(jù)示教任務(wù)完成運動學(xué)軌跡規(guī)劃，生成驅(qū)動數(shù)據(jù)發(fā)送給伺服驅(qū)動控制模塊參考執(zhí)行；

所述步驟4的具體過程為：

主控模塊中的運動學(xué)軌跡規(guī)劃模塊為實現(xiàn)機器人位置運動過程即非鉆孔過程的運動時間同步性和運動過程平滑性，將當(dāng)前各軸電機運動時間進(jìn)行統(tǒng)一并通過分段控制實現(xiàn)電機速度平滑運動；

步驟401、將電機所要運動的過程分為7段，即加加速、勻加速、減加速、勻速、加減速、勻減速、減減速，設(shè)為加加速段時間、為勻加速段時間、為減加速段時間、為勻速段時間、為加減速段時間、為勻減速段時間、為減減速段時間，表示電機軸的編號；

步驟402、將該運動過程的起點速度v_s、終點速度v_e、最大速度v_m、最大加速度a_m、最大加加速度j_m和總位移S作為規(guī)劃約束條件；

步驟403、規(guī)劃第一軸即X軸變速段運動時間，判斷是否含有均勻變速段，其中加速過程為：

若v_m-v_s> QUOTE ，則t_1-1= QUOTE ，t_1-2= QUOTE ,t_1-3=t_1-1；

若v_m-v_s≤ QUOTE ，則t_1-1= QUOTE ，t_1-2=0,t_1-3=t_1-1，則加速過程中的實際最大加速度沒有達(dá)到限定值a_m；

按照求出的時間對應(yīng)各個時間段的加速度進(jìn)行積分，求出各段的相應(yīng)速度，再對各段相應(yīng)速度積分，求出加速段位移S_acc；

步驟404、變速段規(guī)劃中的減速過程利用對稱性得出，減速段位移S_dec=S_acc,t_1-6=t_1-2,t_1-5=t_1-7=t_1-3=t_1-1；

步驟405、判斷運動過程中有無勻速段，判斷準(zhǔn)則為如果S_dec+S_acc<S則含有勻速段，否則不含有勻速段；

步驟406、如果有勻速段，勻速段時間為 QUOTE ,如果沒有勻速段，勻速段時間t_1-4=0；

步驟407、重復(fù)步驟103-106，至完成第二、三、四軸的規(guī)劃；

步驟408、比較各軸的總體運行時間，取最大值為各軸同步運行總時間；

根據(jù)約束方程

求出t₂和t₄；

其中，、和分別為四軸電機同步運動約束后的各軸電機加加速、勻加速和勻速的時間；

若約束方程無解或t₂<0，則認(rèn)為不存在加加速段和減減速段，此時約束方程轉(zhuǎn)化為

根據(jù)該方程求出t₁和t₄，另外根據(jù)對稱性t₇=t₅=t₃=t₁,t₆=t₂=0；、、和分別為四軸電機同步運動約束后的各軸電機減加速、加減速、勻減速和減減速的時間；和是相應(yīng)加加速和減加速階段電機的位移；

步驟409、將上述求得的各個段時間及相應(yīng)段上的位置量和速度量作為驅(qū)動數(shù)據(jù)傳輸至伺服驅(qū)動控制模塊執(zhí)行。

步驟5、主控模塊根據(jù)伺服驅(qū)動控制模塊反饋的機器人狀態(tài)進(jìn)行主動柔順控制決策，確定使用的位置控制參數(shù)和柔順控制參數(shù)，通過驅(qū)動數(shù)據(jù)交互模塊發(fā)送給伺服驅(qū)動模塊參考執(zhí)行；

主動柔順控制決策即根據(jù)當(dāng)前機器人任務(wù)狀態(tài)和機器人當(dāng)前實際位姿狀態(tài)調(diào)整主動柔順力控制和運動學(xué)位置控制的權(quán)重因子，具體過程如圖2所示：

主動柔順控制決策模塊的輸入量為人機交互的任務(wù)狀態(tài)和機器人當(dāng)前位姿狀態(tài)，當(dāng)人機交互任務(wù)為鉆孔且機器人當(dāng)前狀態(tài)靠近或已經(jīng)位于鉆孔區(qū)域時，主動柔順控制決策模塊提高柔順控制權(quán)重、降低位置控制權(quán)重，進(jìn)而生成控制參數(shù)調(diào)整指令：使用大柔順控制參數(shù)、小位置控制參數(shù)；當(dāng)人機交互任務(wù)為運動或機器人當(dāng)前狀態(tài)離鉆孔區(qū)域較遠(yuǎn)時，決策模塊降低柔順控制權(quán)重、提高位置控制權(quán)重，進(jìn)而生成控制參數(shù)調(diào)整指令：使用小柔順控制參數(shù)、大位置控制參數(shù)；其中柔順控制參數(shù)和位置控制參數(shù)為事先調(diào)好并保存于系統(tǒng)中的參數(shù)；柔順控制參數(shù)包括大柔順控制參數(shù)和小柔順控制參數(shù)，，位置控制參數(shù)包括大位置控制參數(shù)和小位置控制參數(shù)，。

步驟6、主控模塊同時完成IO控制管理模塊的管理和決策；

步驟7、伺服驅(qū)動模塊根據(jù)主控指令完成各軸伺服電機的實時控制；主控指令由運動學(xué)軌跡規(guī)劃模塊和主動柔順控制決策模塊生成，主控指令包括各軸電機參考位置、鉆機推進(jìn)電流和控制參數(shù)調(diào)整指令；如圖3所示，伺服驅(qū)動控制模塊內(nèi)有至少兩套離線調(diào)試完成的位置環(huán)、電流環(huán)參數(shù)和一套速度環(huán)參數(shù)，其中位置環(huán)1、電流環(huán)1參數(shù)適合進(jìn)行精準(zhǔn)的運動學(xué)位置控制，位置環(huán)2、電流環(huán)2參數(shù)適合進(jìn)行鉆機推進(jìn)時的主動柔順力電流控制，速度環(huán)在兩套控制參數(shù)中是一樣的；當(dāng)主控指令中控制參數(shù)調(diào)整指令為使用大柔順控制參數(shù)、小位置控制參數(shù)時，伺服驅(qū)動控制模塊選用位置環(huán)2、速度環(huán)、電流環(huán)2構(gòu)成的控制模塊，此時主控指令中鉆機推進(jìn)電流為主要控制目標(biāo)，運動學(xué)位置控制此時主要起限制保護(hù)作用，防止鉆機推進(jìn)過程產(chǎn)生碰撞等意外發(fā)生；當(dāng)主控指令中控制參數(shù)調(diào)整指令為使用小柔順控制參數(shù)、大位置控制參數(shù)時，伺服驅(qū)動控制模塊選用位置環(huán)1、速度環(huán)、電流環(huán)1構(gòu)成的控制模塊，此時主控指令中的電機參考位置控制為主要控制目標(biāo)，實現(xiàn)系統(tǒng)的快速精確運動。

步驟8、電鉆模塊根據(jù)IO模塊的狀態(tài)啟停電鉆，進(jìn)行鉆孔。