一類機(jī)器人的示教奇異位形預(yù)測(cè)

陳 皓，徐善軍，任書楠，寧 昕，李聰利

（1.國電富通科技發(fā)展有限責(zé)任公司，北京 100071；2.國網(wǎng)天津市電力公司，天津 300010）

0 引言

隨著科技的發(fā)展，機(jī)器人的使用范圍也日益加大，一些傳統(tǒng)、危險(xiǎn)的人工操作也逐漸被機(jī)器人替代[1]。丹麥機(jī)器人制造商Universal Robots公司的協(xié)作機(jī)器人憑借優(yōu)異的靈活度、安全性和獨(dú)特創(chuàng)新的人機(jī)協(xié)同能力[2]，顛覆了傳統(tǒng)工業(yè)機(jī)器人笨重及昂貴的特征[2]，可以利用有限的空間和資源實(shí)現(xiàn)人機(jī)靈活、安全協(xié)同作業(yè)，因此在現(xiàn)代工業(yè)中具有廣闊的應(yīng)用場(chǎng)景[2～5]。然而在實(shí)際的運(yùn)動(dòng)控制中，由于操作人員缺乏對(duì)機(jī)器人運(yùn)動(dòng)學(xué)[2]，奇異性的理解[2]，經(jīng)常設(shè)置不合理路點(diǎn)[2]，使得機(jī)器人作業(yè)時(shí)出現(xiàn)保護(hù)性停止、找不到解決方案、機(jī)器人不能到達(dá)該位置等警報(bào)[2]，此時(shí)操作人員需要手動(dòng)清除報(bào)警信息并重新上電初始化機(jī)器人。如此往復(fù)增加了調(diào)試的難度，也降低了一線操作人員的使用體驗(yàn)度。

本文針對(duì)該問題，基于現(xiàn)有UR10機(jī)器人本體，考慮了機(jī)械臂的可行空間，建立了六軸機(jī)器人運(yùn)動(dòng)學(xué)模型。目前算法對(duì)多約束、環(huán)境復(fù)雜的條件下的奇異點(diǎn)預(yù)測(cè)不能快速實(shí)時(shí)找到問題的相關(guān)解，本文將尋求其他的算法在該問題上的應(yīng)用，設(shè)計(jì)了示教過程中對(duì)奇異位形的預(yù)測(cè)算法，實(shí)時(shí)計(jì)算未來軌跡，避免了現(xiàn)有UR機(jī)器人在作業(yè)過程中遇到的上述問題。該方法原理具有通用性，可移植性強(qiáng)，成本代價(jià)小。

1 運(yùn)動(dòng)學(xué)分析

機(jī)械臂運(yùn)動(dòng)學(xué)特性的研究主要包括正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)，正運(yùn)動(dòng)學(xué)即已知關(guān)節(jié)空間軸坐標(biāo)求笛卡爾坐標(biāo)系下末端位姿，逆運(yùn)動(dòng)學(xué)即已知機(jī)器人末端在笛卡爾坐標(biāo)系下的位姿，求解關(guān)節(jié)軸位置[8]。該問題為預(yù)測(cè)奇異位形的基礎(chǔ)。

1.1 連桿參數(shù)描述

本文使用的連桿參數(shù)描述機(jī)構(gòu)運(yùn)動(dòng)關(guān)系的規(guī)則為Denavit-Hartenberg參數(shù)[9]，相鄰連桿參數(shù)由α，a，d，θ構(gòu)成，關(guān)節(jié)連桿參數(shù)示意圖如圖1所示。其中，αi表示繞Xi-1，Zi-1到Zi的旋轉(zhuǎn)角度；ai表示沿著Xi-1，Zi-1到Zi的距離；di表示沿著Zi-1，Xi-1到Xi的距離；θi表示繞Zi-1，Xi-1到Xi的旋轉(zhuǎn)角度。

圖1 轉(zhuǎn)動(dòng)關(guān)節(jié)連桿參數(shù)示意圖

1.2 坐標(biāo)系

建立如圖2所示機(jī)器人坐標(biāo)系，并根據(jù)該坐標(biāo)系和1.1中的關(guān)節(jié)連桿參數(shù)描述，確定如表1所示的ur10機(jī)器人標(biāo)準(zhǔn)DH參數(shù)。

圖2 UR10機(jī)器人坐標(biāo)系

表1 UR10機(jī)器人標(biāo)準(zhǔn)D-H參數(shù)

1.3 正運(yùn)動(dòng)學(xué)

1.4 逆運(yùn)動(dòng)學(xué)

因UR10機(jī)器人滿足Pieper準(zhǔn)則，具有封閉解[11]。相較于數(shù)值解，解析解在精度和計(jì)算速度上有明顯優(yōu)勢(shì)，本文設(shè)計(jì)的一類奇異位形實(shí)時(shí)預(yù)測(cè)算法選擇了解析法。根據(jù)式(2)左右同乘01T-1，右乘56T-1，可得分別展開等式的第3行4列，可得到關(guān)于θ1的三角函數(shù)方程；展開第3行3列，得到關(guān)于θ5的函數(shù)方程；展開第3行1列，得到關(guān)于θ6的方程。通過該方法用三角函數(shù)萬能公式先求解θ1，再求解θ5，θ6，再求解θ3，最后求解θ2，θ4，其中令：

2 示教軌跡預(yù)測(cè)及奇異分析

協(xié)作機(jī)器人的示教控制方式包括拖拽示教、J1-J6軸動(dòng)以及機(jī)器人末端工具中心位置（TCP）基于笛坐標(biāo)系的單向移動(dòng)。其中，基于軸關(guān)節(jié)空間的拖拽和軸動(dòng)不會(huì)遇到奇異等問題，不在本文考慮范圍內(nèi)。TCP的單向移動(dòng)根據(jù)自由度類型分為位置方向移動(dòng)和姿態(tài)旋轉(zhuǎn)移動(dòng)；根據(jù)參考坐標(biāo)系分為基坐標(biāo)系和工具坐標(biāo)系等。

2.1 位置示教

2.2 姿態(tài)示教

2.3 奇異位形及不可達(dá)預(yù)測(cè)分析

奇異位形分為工作空間邊界及內(nèi)部兩種，前者使得執(zhí)行末端接近工作空間邊界，后者通常是由于至少兩個(gè)關(guān)節(jié)軸發(fā)生共線。當(dāng)機(jī)器人處于奇異位形，它會(huì)失去一個(gè)或多個(gè)自由度，此時(shí)，無論選擇什么樣的關(guān)節(jié)速度都無法使機(jī)械臂末端運(yùn)動(dòng)；在途經(jīng)奇異位形時(shí)，某些關(guān)節(jié)軸會(huì)嘗試瞬間旋轉(zhuǎn)180°，對(duì)設(shè)備造成損傷。本文利用矢量積法求得雅可比矩陣[12]，并令可求得UR機(jī)器人在奇異位形狀的條件是式中三個(gè)因子對(duì)應(yīng)的物理意義分別是四六軸共線、二三四軸共面、末端位置在一二軸構(gòu)成的平面。實(shí)際示教運(yùn)動(dòng)中為了提前預(yù)知上述位形及不可達(dá)位姿，設(shè)計(jì)了如下算法進(jìn)行預(yù)測(cè)。

步驟1：獲得當(dāng)前i時(shí)刻六個(gè)關(guān)節(jié)軸位置信息。

步驟2：判斷當(dāng)前是否為奇異點(diǎn)，若是，則結(jié)束，預(yù)測(cè)結(jié)果給true；若否，則到步驟3）。

步驟3；正解得i時(shí)刻TCP位姿。

步驟4：根據(jù)當(dāng)前命令請(qǐng)求（如基于極坐標(biāo)系X軸正方向），計(jì)算i+1時(shí)刻TCP位姿。

步驟5：運(yùn)動(dòng)學(xué)逆解得i+1關(guān)節(jié)軸角度。

步驟6：是否存在逆解釋，若是，則到步驟7）；若否，則結(jié)束，路徑將不可達(dá)，預(yù)測(cè)結(jié)果給true。

步驟7：選擇最有效解中最優(yōu)解。其中有效解表示在軸關(guān)節(jié)可行范圍內(nèi)，最優(yōu)解原則即為i時(shí)刻

步驟8：判斷i，i+1關(guān)軸差分是否大于閾值，若是，則結(jié)束，預(yù)測(cè)結(jié)果給true；若否，則到步驟9）。

步驟9：判斷運(yùn)動(dòng)是否結(jié)束；若是，則結(jié)束，預(yù)測(cè)結(jié)果給false；若否，則i++并到步驟4）。

3 實(shí)驗(yàn)設(shè)計(jì)與驗(yàn)證分析

為了驗(yàn)證運(yùn)動(dòng)學(xué)算法及奇異位形預(yù)測(cè)的有效性，本文設(shè)計(jì)了兩個(gè)算例，一個(gè)算例驗(yàn)證了ur10機(jī)器人運(yùn)動(dòng)學(xué)，另一個(gè)算例基于ROS設(shè)計(jì)了奇異位形預(yù)測(cè)節(jié)點(diǎn)，在真實(shí)環(huán)境中驗(yàn)證2.3算法在示教過程中對(duì)不可達(dá)點(diǎn)及奇異形的預(yù)測(cè)。

3.1 運(yùn)動(dòng)學(xué)驗(yàn)證

由該末端ΘT逆解出可能對(duì)應(yīng)關(guān)節(jié)坐標(biāo)組合值如表2所示。反之，由表2得到的8組解再重新求得8組的末端姿態(tài)Ti，此時(shí)對(duì)比結(jié)果，發(fā)現(xiàn)由此可以互相驗(yàn)證其運(yùn)動(dòng)學(xué)的正、逆解的正確性。

表2 UR10Θ=[1，1，1，1，1，1]時(shí)末端逆解

3.2 奇異點(diǎn)預(yù)測(cè)驗(yàn)證

算例二設(shè)計(jì)了如圖3所示的程序結(jié)構(gòu)，以此驗(yàn)證對(duì)示教軌跡奇異位形及不可達(dá)點(diǎn)的預(yù)測(cè)。check_singularity節(jié)點(diǎn)實(shí)現(xiàn)了2.3中的預(yù)測(cè)算法。其中，通過ur_modern_driver的/joint_states獲得UR10的實(shí)時(shí)關(guān)節(jié)位置信息，并通過main_control的服務(wù)請(qǐng)求消息，得知當(dāng)前運(yùn)動(dòng)指令及運(yùn)動(dòng)方向，并將計(jì)算結(jié)果通過服務(wù)響應(yīng)返回給main_control，若響應(yīng)內(nèi)容為預(yù)測(cè)到奇異位置或?qū)⒌诌_(dá)不可達(dá)預(yù)測(cè)點(diǎn)時(shí)，main_control通過29999端口向機(jī)器人發(fā)送停止命令。

圖3 奇異點(diǎn)預(yù)測(cè)節(jié)點(diǎn)與主控節(jié)點(diǎn)的話題服務(wù)通信

相關(guān)的設(shè)置參數(shù)如下：位移前瞻POS_MAX_PROSPECT=40mm；位移步長POS_MIN_STEP=5mm；旋轉(zhuǎn)角前瞻ORI_MAX_PROSPECT=10×M_PI/180rad；旋轉(zhuǎn)步長ORI_MIN_STEP=2*M_PI/180rad；奇異判斷閾值SINGULARITY_THRESHOLD=0.4rad；起點(diǎn)Θ=[-83.8852，-48.272，-122.884，-100.159，91.5561，6.0538]；運(yùn)動(dòng)方向IS_POSITIVE=TRUE；運(yùn)動(dòng)自由度JOG_INDEX=2（Y軸）；參考坐標(biāo)系REF_COORD=BASE（基坐標(biāo)）；速度比VEL_OVERRIDE=100%；運(yùn)動(dòng)過程機(jī)器人末端位置將穿過在一、二軸構(gòu)成的平面即奇異位形，實(shí)際的關(guān)節(jié)位置跟蹤和預(yù)測(cè)對(duì)比圖如圖4所示，關(guān)節(jié)軸1速度對(duì)比如圖5所示。根據(jù)圖4可看出預(yù)測(cè)算法對(duì)軸關(guān)節(jié)未來趨勢(shì)的準(zhǔn)確預(yù)計(jì)；圖5中T=2.691s，實(shí)際檢測(cè)關(guān)節(jié)速度vel=20.15degree/s2，預(yù)測(cè)關(guān)節(jié)速度遠(yuǎn)遠(yuǎn)大于電機(jī)可執(zhí)行范圍，即預(yù)測(cè)到穿越奇異位形，此時(shí)check_singularity節(jié)點(diǎn)終止預(yù)測(cè)，main_control收到異常，并向機(jī)器人發(fā)送停止腳本，機(jī)器人在T=2.91s安全停止，未觸發(fā)保護(hù)性停止等斷電行為。

圖4 位置示教遇奇異位形實(shí)際關(guān)節(jié)位置與預(yù)測(cè)對(duì)比圖

圖5 位置示教遇奇異位形實(shí)際軸關(guān)節(jié)1速度與預(yù)測(cè)對(duì)比圖

4 結(jié)語

由圖4、圖5可見，在不干擾原本示教控制前提下，經(jīng)過文中設(shè)計(jì)的方法，可提前預(yù)知奇異位形及不可達(dá)點(diǎn)等信息，及時(shí)有效地避免機(jī)器人因嘗試穿越奇異位形，導(dǎo)致個(gè)別關(guān)節(jié)軸的高速旋轉(zhuǎn)而引起的保護(hù)停止。經(jīng)過改進(jìn)后的控制操作體驗(yàn)度大大提升，加快了調(diào)試速度，提高了作業(yè)效率。