康楊雨軒，鄭　文，李松林，余小平，蔡后祥（成都理工大學(xué)信息科學(xué)與技術(shù)學(xué)院，四川成都610059）

基于Leap Motion的機(jī)械臂交互控制研究

康楊雨軒，鄭文，李松林，余小平，蔡后祥
（成都理工大學(xué)信息科學(xué)與技術(shù)學(xué)院，四川成都610059）

針對(duì)多旋轉(zhuǎn)自由度機(jī)械臂的便捷控制問題，建立了一種聯(lián)接體感設(shè)備Leap Motion與六自由度機(jī)械臂的交互控制系統(tǒng)。在體感輸入和機(jī)械臂運(yùn)動(dòng)響應(yīng)模型中，利用基于幾何方法的機(jī)械臂運(yùn)動(dòng)學(xué)逆解，提出一種增加求解約束，二分搜索末端抓取器最優(yōu)空間位姿的解法。這種解法運(yùn)行效率高，求解過程直觀，能夠在對(duì)數(shù)級(jí)時(shí)間內(nèi)求解。經(jīng)過仿真和實(shí)物實(shí)驗(yàn)驗(yàn)證，該解法具備處理大量體感輸入數(shù)據(jù)，實(shí)現(xiàn)機(jī)械臂實(shí)時(shí)響應(yīng)的能力，為非觸控式的體感機(jī)械臂控制提供了一種可行方案。

Leap Motion；多旋轉(zhuǎn)自由度機(jī)械臂；逆運(yùn)動(dòng)學(xué)；幾何法

0　引言

隨著視頻圖像處理技術(shù)和圖像識(shí)別算法的發(fā)展，出現(xiàn)了各種各樣的體感設(shè)備，其中包括可以識(shí)別人的小臂與手部位置及運(yùn)動(dòng)的體感設(shè)備Leap Motion。由于機(jī)械臂運(yùn)動(dòng)方式和人的手臂運(yùn)動(dòng)較為類似，即通過關(guān)節(jié)扭動(dòng)達(dá)到運(yùn)動(dòng)，操作者通過Leap Motion體感設(shè)備控制機(jī)械臂，其運(yùn)動(dòng)軌跡可以非常直觀地反映給操作者，使控制難度大大降低。因此，本文提出了一種基于Leap Motion體感設(shè)備的機(jī)械臂控制系統(tǒng)，同時(shí)給出適用于該系統(tǒng)的運(yùn)動(dòng)模型算法。

1　交互模型

1.1自由度鏈?zhǔn)綑C(jī)械臂

因?yàn)闄C(jī)械臂末端姿態(tài)與6個(gè)變量相關(guān)，所以，至少需要6個(gè)自由度才能實(shí)現(xiàn)可達(dá)空間范圍內(nèi)靈活運(yùn)動(dòng)。本文采用一種適應(yīng)性廣泛的六自由度機(jī)械臂（簡(jiǎn)稱為6R）。機(jī)械臂坐標(biāo)系和關(guān)節(jié)旋轉(zhuǎn)面如圖1所示。

圖1　六自由度機(jī)械臂坐標(biāo)系Fig 1　Coordinate system of 6 DOFs manipulator

為便于描述末端抓取器在空間中的姿態(tài)和位置，在每個(gè)關(guān)節(jié)上建立一個(gè)坐標(biāo)系并利用坐標(biāo)系之間的關(guān)系以描述末端抓取器的姿態(tài)。使用Denavit-HartenBerg（D-H）法建立坐標(biāo)系，描述6R的姿態(tài)位置情況。這里給出機(jī)械臂末端抓取器的位姿表達(dá)式

1.2Leap Motion體感設(shè)備

Leap Motion是一種利用內(nèi)置雙攝像頭的雙目成像原理重建出手的三維空間運(yùn)動(dòng)信息的體感設(shè)備，內(nèi)部系統(tǒng)采用一套三維右旋直角坐標(biāo)系，將捕獲幀中反映的物理特征信息進(jìn)行歸類。本系統(tǒng)使用了部分類與其內(nèi)在屬性，如表1所示。

表1　部分Leap Motion特征屬性Tab 1　Partial properties of Leap Motion

2　幾何法的機(jī)械臂運(yùn)動(dòng)學(xué)逆解

2.1逆運(yùn)動(dòng)模型分析

機(jī)械臂運(yùn)動(dòng)學(xué)的正解相對(duì)簡(jiǎn)單。逆運(yùn)動(dòng)學(xué)的求解模型是已知機(jī)械臂各關(guān)節(jié)間連桿長(zhǎng)度L，根據(jù)抓取器末端的位姿求各關(guān)節(jié)旋轉(zhuǎn)角θ1，θ2，θ3，θ4，θ5。機(jī)械臂逆運(yùn)動(dòng)問題難以建立通用算法。幾何法相較于其他方法形式簡(jiǎn)單，計(jì)算量小，更適用于數(shù)據(jù)變化迅速的控制系統(tǒng)。

由于采用的機(jī)械臂構(gòu)型原因，肩關(guān)節(jié)，肘關(guān)節(jié)和腕關(guān)節(jié)運(yùn)動(dòng)在平面ONJ4T內(nèi)，腰關(guān)節(jié)和其他關(guān)節(jié)之間具有獨(dú)立性，如圖2所示。

圖2　抓取器位姿與關(guān)節(jié)角度對(duì)應(yīng)關(guān)系Fig 2　Relationship between pose of grabber and angle of joints

式中

式（2）是欠定方程組，含有無窮多組解。若采用解析幾何法，最終仍需通過迭代方式進(jìn)行數(shù)值計(jì)算。另外，要求機(jī)械臂符合工程力學(xué)，解析幾何法難以滿足這一要求，因此采用分步幾何法求解。

2.2二分平移求解

1.政府。政策供給在產(chǎn)業(yè)發(fā)展初期的導(dǎo)向性非常明顯，合理引導(dǎo)產(chǎn)業(yè)發(fā)展、功能結(jié)構(gòu)布局、宣傳造勢(shì)都需要政府的大力支持。廣西全區(qū)要加快落實(shí)扶持健身休閑產(chǎn)業(yè)發(fā)展的各項(xiàng)土地、稅收、資金等政策優(yōu)惠，推進(jìn)健身休閑項(xiàng)目的立項(xiàng)和建設(shè)。降低大型體育場(chǎng)館、各類健身休閑場(chǎng)所等企業(yè)稅費(fèi)成本，加大對(duì)符合條件的場(chǎng)館財(cái)政補(bǔ)助力度。重視區(qū)域聯(lián)動(dòng)宣傳造勢(shì)，推進(jìn)休閑健身平臺(tái)的建設(shè)，采取多樣化的宣傳方式，對(duì)全區(qū)健身休閑動(dòng)態(tài)與發(fā)展規(guī)劃實(shí)時(shí)推送，給市場(chǎng)提供靈活的信號(hào)。同時(shí)加強(qiáng)產(chǎn)業(yè)專題培訓(xùn)，增強(qiáng)對(duì)健身休閑企業(yè)和社會(huì)組織的服務(wù)和監(jiān)管。

首先討論兩圓相離的情況。圓心距為

當(dāng)d＞L2+L4，兩圓相離。如圖3所示。勾繪的實(shí)線為一種可行情況?！螼J1＞180°，角∠X2J4＜0°（均為逆時(shí)針角），不符合對(duì)應(yīng)關(guān)節(jié)旋轉(zhuǎn)角度范圍。僅考慮J2=X2， J3=X3?？梢钥闯?，當(dāng)l=l1，即l經(jīng)過點(diǎn)J1時(shí)，最短，解存在下邊界；當(dāng)l=l2，即l與OJ1相切于點(diǎn)S'，與OJ4相交于，T'2時(shí)，最長(zhǎng)，解存在上邊界。

圖3　平移求解關(guān)節(jié)位置Fig 3　Translation solves position of joints

當(dāng)l‖l1即l斜率l固定時(shí)，以l的縱截距b為自變量，為因變量的函數(shù)存在以下屬性：1）值域存在下邊界和上邊界；2）隨著自變量增大，單調(diào)遞增。

滿足二分求值條件。此時(shí)下邊界為全局下邊界。對(duì)關(guān)節(jié)3和關(guān)節(jié)4位置進(jìn)行二分求解。二分函數(shù)f（b）為

式中

其次，兩圓相切與相離情況完全相同，定義域和值域上下邊界不變。兩圓相交時(shí)，X2和X3解的下邊界更新為兩圓交點(diǎn)。對(duì)于不同的位置關(guān)系，二分平移下邊界為全局下邊界，上邊界均為局部上邊界。引入下一步求解。

2.3二分旋轉(zhuǎn)求解

圖4　旋轉(zhuǎn)求解關(guān)節(jié)位置Fig 4　Rotating solves potion of joints

當(dāng)l相切于OJ1時(shí)，以l的斜率k為自變量為因變量的函數(shù)同樣滿足二分求值條件。此時(shí)上邊界為全局上邊界，下邊界與二分平移求解上邊界重合。二分函數(shù)為

式中

其他變量求解表達(dá)式同式（6）。兩圓相切、相交與兩圓相離情況完全相同。對(duì)于不同的兩圓位置關(guān)系，二分旋轉(zhuǎn)上邊界均為全局上邊界；下邊界均為局部下邊界。兩步求解取并，得到幾何法機(jī)械臂運(yùn)動(dòng)的逆解所有情況。求解二分函數(shù)得到關(guān)節(jié)3，4真實(shí)坐標(biāo)。此時(shí)各關(guān)節(jié)旋轉(zhuǎn)角度可求

3　實(shí)驗(yàn)結(jié)果

為了驗(yàn)證算法的實(shí)際運(yùn)行效果以及與體感機(jī)械臂控制系統(tǒng)的耦合度，進(jìn)行了實(shí)物和仿真實(shí)驗(yàn)。如圖5和圖6，在實(shí)物和仿真實(shí)驗(yàn)中，取6R的各連桿長(zhǎng)度為L(zhǎng)1=0.095 m，L2=L3=0.10 m，L4=0.155 m。仿真和實(shí)物實(shí)驗(yàn)均需要完成的任務(wù)是對(duì)于已知6R末端抓取器位姿，計(jì)算機(jī)械臂實(shí)際位姿與理論位姿誤差

式中p為抓取器末端位置向量的理論值，p*為位置向量的實(shí)際值。算法通過增加約束達(dá)到簡(jiǎn)化求解的目的，因此，除末端抓取器外其他關(guān)節(jié)位姿的理論和實(shí)際位姿誤差不納入該計(jì)算公式內(nèi)。

另外，為了驗(yàn)證算法的運(yùn)算高效性，仿真實(shí)驗(yàn)還需完成的任務(wù)是計(jì)算幾何法的運(yùn)動(dòng)學(xué)逆解方法執(zhí)行效率，通過輸入Leap Motion采集的樣本數(shù)據(jù)，測(cè)試算法的運(yùn)行速度。

圖5　實(shí)物機(jī)械臂位姿Fig 5　Position and gesture of manipulator

圖6　抓取器位置跟蹤與運(yùn)動(dòng)展示Fig 6　Position tracking and motion show of grabber

對(duì)于理論旋轉(zhuǎn)角度為（40°，30°，60°，80°，90°）和對(duì)應(yīng)的末端抓取器的位置為（-11.37，2.90，-13.55）cm的樣本數(shù)據(jù)，模型得到的實(shí)際角度為（40°，12°，90°，56°，90°），末端抓取器的位置為（-11.40，2.90，-13.59）cm。實(shí)際位置與理論位置之間誤差均小于0.4 mm。誤差來源是本文所使用算法的求解方式導(dǎo)致實(shí)際旋轉(zhuǎn)角度與理論角度不同。在實(shí)物實(shí)驗(yàn)中，6R末端抓取器位姿與手部姿態(tài)極為相近，且在兩項(xiàng)實(shí)驗(yàn)中末端抓取器軌跡變化十分平滑，這說明了本文算法核心的幾何平移和幾何旋轉(zhuǎn)理論的正確性。

在運(yùn)算效率仿真實(shí)驗(yàn)中，輸入階梯分布的手部姿態(tài)樣本數(shù)據(jù)，運(yùn)算結(jié)果如圖7所示?？梢钥闯鏊惴ㄟ\(yùn)行時(shí)間與樣本集大小近似為線性關(guān)系。實(shí)驗(yàn)平臺(tái)為1.7 GHz CPU，8 G內(nèi)存計(jì)算機(jī)，算法測(cè)試程序?yàn)镃++，運(yùn)行于VS2010平臺(tái)。當(dāng)樣本大小為10 000時(shí)，運(yùn)行時(shí)間在100 ms以內(nèi)。這是由于逆運(yùn)動(dòng)模型的幾何平移和幾何旋轉(zhuǎn)方法均采用二分算法，計(jì)算復(fù)雜度為，屬于對(duì)數(shù)級(jí)，運(yùn)算量小。

圖7　算法運(yùn)行時(shí)間Fig 7　Runtime of algorithm

4　結(jié)論

本文針對(duì)體感設(shè)備和六旋轉(zhuǎn)自由度鏈?zhǔn)綑C(jī)械臂，提出了一種特別優(yōu)化的幾何運(yùn)動(dòng)學(xué)逆解方法，并通過計(jì)算機(jī)仿真驗(yàn)證了該算法的有效性和可靠性，算法計(jì)算復(fù)雜度低，求解連續(xù)運(yùn)動(dòng)實(shí)際效果平滑。一方面，由于采用二分算法，本文所使用的方法系統(tǒng)響應(yīng)實(shí)時(shí)性強(qiáng)，在處理大量數(shù)據(jù)或高精度計(jì)算時(shí)均表現(xiàn)顯著。另一方面，相較于矩陣變換法等其他方法，幾何法具有很強(qiáng)的直觀性，可描述性好，便于算法的修改和優(yōu)化。

同時(shí)，本文作者使用實(shí)物機(jī)械臂和體感設(shè)備實(shí)現(xiàn)了該控制系統(tǒng)。系統(tǒng)的輸入輸出響應(yīng)速度快，運(yùn)動(dòng)精度高，驗(yàn)證了這一控制系統(tǒng)是切實(shí)可行的。系統(tǒng)的實(shí)現(xiàn)對(duì)基于機(jī)械臂的交互模型研究和應(yīng)用都具有重要意義。

［1］Primrose E J F.On the input-output equation of the general 7R mechanism［J］.Mechanism and Machine Theory，1986，21（6）：509-510.

［2］Assal S F M，Watanabe K，Izumi K.Neural network learning from hint for the inverse kinematics problem of redundant arm subject to joint limits［C］∥IEEE/RSJ International Conference on Intelligence Robots and System，Piscataway，NJ，USA：IEEE，2005：1477-1482.

［3］Nguyen V T.Enhancing touchless interaction with the leap motion using a haptic glove［D］.Kuopio/Joensuu：Kuopio University of Eastern Finland，2014.

［4］Paul R P，Shimano B E，Mayer G.Kinematic control equations for simple manipulations［J］.IEEE Trans on SMC，1981，11（6）：449-455.

［5］Fu K S，Gonzalez R C，Lee C S G.Robotics control senses vision and intelligent［M］.New York：Mc Graw-Hill，1987.

［6］馬江.六自由度機(jī)械臂控制系統(tǒng)設(shè)計(jì)與運(yùn)動(dòng)學(xué)仿真［D］.北京：北京工業(yè)大學(xué)，2009.

［7］楊淞.一種六自由度機(jī)械臂的運(yùn)動(dòng)控制系統(tǒng)設(shè)計(jì)［D］.上海：上海交通大學(xué)，2014.

［8］陳鵬，劉璐，余飛，等.一種仿人機(jī)械臂的運(yùn)動(dòng)學(xué)逆解的幾何求解方法［J］.機(jī)器人，2012（2）：211-216.

［9］朱曉龍，頓向明.一種五自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的幾何法［J］.機(jī)械與電子，2014（5）：76-80.

［10］任崇軒.五自由度機(jī)械臂運(yùn)動(dòng)和控制仿真分析［D］.廣州：華南理工大學(xué)，2012.

Research on manipulator interactive control based on Leap Motion

KANG Yang-yu-xuan，ZHENG Wen，LI Song-lin，YU Xiao-ping，CAI Hou-xiang
（College of Information Science and Technology，Chengdu University of Technology，Chengdu 610059，China）

Aiming at problem of degree of freedom（DOFs）convenient control of multiple rotating DOFs mechanical arm，an interactive control system connecting somatosensory device，Leap Motion with six DOFs mechanical arm is built.In model for somatosensory input and manipulator motion response，mechanical arm kinematics inverse solution based on geometry is used，propose a solution that add solving constraints and binary search solution of the optimal spatial pose of terminal grabber.This method has high operating efficiency and has intuitive solving process and can solve in logarithmic degree time.The simulation and physical experiment prove that the solution is capable to process large amount of data of somatosensory and implement function of manipulator reaction in realtime，which provides a feasible scheme for untouched somatosensory manipulator control.

Leap Motion；multiple rotating degree of freedom（DOFs）manipulator；inverse kinematics；geometrical method

TP241

A

1000—9787（2016）06—0034—04

10.13873/J.1000—9787（2016）06—0034—04

2016—04—11

康楊雨軒（1994-），男，河南新鄉(xiāng)人，本科，主要研究方向?yàn)橹悄軝C(jī)器人。