一類機器人的示教奇異位形預測

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

（1.國電富通科技發展有限責任公司，北京 100071；2.國網天津市電力公司，天津 300010）

0 引言

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

本文針對該問題，基于現有UR10機器人本體，考慮了機械臂的可行空間，建立了六軸機器人運動學模型。目前算法對多約束、環境復雜的條件下的奇異點預測不能快速實時找到問題的相關解，本文將尋求其他的算法在該問題上的應用，設計了示教過程中對奇異位形的預測算法，實時計算未來軌跡，避免了現有UR機器人在作業過程中遇到的上述問題。該方法原理具有通用性，可移植性強，成本代價小。

1 運動學分析

機械臂運動學特性的研究主要包括正運動學與逆運動學，正運動學即已知關節空間軸坐標求笛卡爾坐標系下末端位姿，逆運動學即已知機器人末端在笛卡爾坐標系下的位姿，求解關節軸位置[8]。該問題為預測奇異位形的基礎。

1.1 連桿參數描述

本文使用的連桿參數描述機構運動關系的規則為Denavit-Hartenberg參數[9]，相鄰連桿參數由α，a，d，θ構成，關節連桿參數示意圖如圖1所示。其中，αi表示繞Xi-1，Zi-1到Zi的旋轉角度；ai表示沿著Xi-1，Zi-1到Zi的距離；di表示沿著Zi-1，Xi-1到Xi的距離；θi表示繞Zi-1，Xi-1到Xi的旋轉角度。

圖1 轉動關節連桿參數示意圖

1.2 坐標系

建立如圖2所示機器人坐標系，并根據該坐標系和1.1中的關節連桿參數描述，確定如表1所示的ur10機器人標準DH參數。

圖2 UR10機器人坐標系

表1 UR10機器人標準D-H參數

1.3 正運動學

1.4 逆運動學

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

2 示教軌跡預測及奇異分析

協作機器人的示教控制方式包括拖拽示教、J1-J6軸動以及機器人末端工具中心位置（TCP）基于笛坐標系的單向移動。其中，基于軸關節空間的拖拽和軸動不會遇到奇異等問題，不在本文考慮范圍內。TCP的單向移動根據自由度類型分為位置方向移動和姿態旋轉移動；根據參考坐標系分為基坐標系和工具坐標系等。

2.1 位置示教

2.2 姿態示教

2.3 奇異位形及不可達預測分析

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

步驟1：獲得當前i時刻六個關節軸位置信息。

步驟2：判斷當前是否為奇異點，若是，則結束，預測結果給true；若否，則到步驟3）。

步驟3；正解得i時刻TCP位姿。

步驟4：根據當前命令請求（如基于極坐標系X軸正方向），計算i+1時刻TCP位姿。

步驟5：運動學逆解得i+1關節軸角度。

步驟6：是否存在逆解釋，若是，則到步驟7）；若否，則結束，路徑將不可達，預測結果給true。

步驟7：選擇最有效解中最優解。其中有效解表示在軸關節可行范圍內，最優解原則即為i時刻

步驟8：判斷i，i+1關軸差分是否大于閾值，若是，則結束，預測結果給true；若否，則到步驟9）。

步驟9：判斷運動是否結束；若是，則結束，預測結果給false；若否，則i++并到步驟4）。

3 實驗設計與驗證分析

為了驗證運動學算法及奇異位形預測的有效性，本文設計了兩個算例，一個算例驗證了ur10機器人運動學，另一個算例基于ROS設計了奇異位形預測節點，在真實環境中驗證2.3算法在示教過程中對不可達點及奇異形的預測。

3.1 運動學驗證

由該末端ΘT逆解出可能對應關節坐標組合值如表2所示。反之，由表2得到的8組解再重新求得8組的末端姿態Ti，此時對比結果，發現由此可以互相驗證其運動學的正、逆解的正確性。

表2 UR10Θ=[1，1，1，1，1，1]時末端逆解

3.2 奇異點預測驗證

算例二設計了如圖3所示的程序結構，以此驗證對示教軌跡奇異位形及不可達點的預測。check_singularity節點實現了2.3中的預測算法。其中，通過ur_modern_driver的/joint_states獲得UR10的實時關節位置信息，并通過main_control的服務請求消息，得知當前運動指令及運動方向，并將計算結果通過服務響應返回給main_control，若響應內容為預測到奇異位置或將抵達不可達預測點時，main_control通過29999端口向機器人發送停止命令。

圖3 奇異點預測節點與主控節點的話題服務通信

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

圖4 位置示教遇奇異位形實際關節位置與預測對比圖

圖5 位置示教遇奇異位形實際軸關節1速度與預測對比圖

4 結語

由圖4、圖5可見，在不干擾原本示教控制前提下，經過文中設計的方法，可提前預知奇異位形及不可達點等信息，及時有效地避免機器人因嘗試穿越奇異位形，導致個別關節軸的高速旋轉而引起的保護停止。經過改進后的控制操作體驗度大大提升，加快了調試速度，提高了作業效率。