協(xié)作機器人逆運動學(xué)形式化建模與驗證

王 暢，王國輝，施智平，關(guān) 永，張倩穎，邵振洲

(首都師范大學(xué) 信息工程學(xué)院，北京 100048)

1 引 言

隨著人工智能和計算機的快速發(fā)展，工業(yè)機器人被廣泛地應(yīng)用于各個領(lǐng)域中，在電子電氣、汽車制造、物流運輸、食品加工等行業(yè)都有著不同的貢獻.當(dāng)前，人機協(xié)作型機器人成為工業(yè)機器人領(lǐng)域里備受關(guān)注的類型之一.與傳統(tǒng)的工業(yè)機器人相比，協(xié)作型機器人成本低廉、場景多元、操作便捷.作為工業(yè)機器人新興領(lǐng)域，協(xié)作型機器人雖然有許多的優(yōu)點，給人類的生活帶來極大的便利，但是隨之而來的安全問題也越來越受到重視.傳統(tǒng)工業(yè)機器人致死的新聞屢見不鮮，例如：2015年德國大眾汽車制造廠中一個機器人殺死了一名人類工作人員.在安裝和調(diào)制機器人過程中，該工作人員被機器人擊中胸部導(dǎo)致其被碾壓在金屬板上，當(dāng)場死亡.頻發(fā)的安全事故表明，即使傳統(tǒng)的工業(yè)機器人在部署時，還額外安裝了安全圍欄，也無法避免因機器人突然的“暴走”而引發(fā)的災(zāi)難性事故，因此人機協(xié)作機器人的安全性問題成為人們關(guān)注的焦點，也對其安全性提出了更高的要求.

機器人逆運動學(xué)的建模與求解是決定其安全性的必要因素之一.機器人的逆運動學(xué)是指根據(jù)所要到達的目標(biāo)位置來確定機器人各關(guān)節(jié)的運動變量[1].目前關(guān)于機器人模型的方法主要有D-H參數(shù)法[2]和旋量法[3]兩種.其中，D-H參數(shù)法是一種為關(guān)節(jié)鏈中的每一連桿建立坐標(biāo)系的矩陣方法[4].但是對于相鄰關(guān)節(jié)平行的機器人，會出現(xiàn)模型參數(shù)不連續(xù)而導(dǎo)致的奇異性問題[5].相較于傳統(tǒng)的D-H參數(shù)法，旋量法從整體上描述剛體的運動，它無需建立各連桿坐標(biāo)系，只建立慣性基坐標(biāo)系和末端工具坐標(biāo)系，從而避免了用局部坐標(biāo)系描述時所造成的奇異性[6].目前，也有許多學(xué)者基于旋量法對協(xié)作型機器人逆運動學(xué)求解進行研究[7].例如：Igor Dimovsk[8]

等基于旋量法描述了一些常見的Paden-Kahan子問題在機器人逆運動學(xué)求解中的應(yīng)用.然后基于這些常見的子問題擴展出了一些新的子問題.Chen[9]等基于指數(shù)積公式和分類的子問題對機器人逆運動學(xué)求解.但是，這些研究還都停留在使用傳統(tǒng)的測試用例方法對機器人逆運動學(xué)進行驗證.

目前，機器人模型的驗證也有許多不同的方法.常見的有：利用MATLAB對機器人模型進行模擬與仿真，以及用Maple、Mathematica等數(shù)學(xué)軟件驗證機器人的代數(shù)系統(tǒng).然而這些軟件自身的短板和漏洞，常常會導(dǎo)致數(shù)值計算的誤差或符號計算的錯誤，從而無法實現(xiàn)機器人逆運動學(xué)正確的求解[10].例如：計算仿真系統(tǒng)(如MATLAB)因為浮點表達的天然局限無法提供100%精確的結(jié)果[11]；計算機代數(shù)系統(tǒng)(如 Maple、Mathematica等)雖然支持符號運算從而可以得到精確結(jié)果，但是其在邊界條件的處理上存在短板(如x≠0是表達式1/x的邊界條件)，同時計算機代數(shù)系統(tǒng)內(nèi)核中的符號計算算法也尚未得到驗證，可能因其自身存在漏洞而影響其驗證結(jié)果的精確性[12].因此，提出使用形式化的方法對其進行驗證，可以克服上述傳統(tǒng)方法的局限性，從而保證機器人逆運動學(xué)求解模型的可靠性.

形式化驗證是基于計算機的數(shù)學(xué)分析技術(shù)，用嚴謹?shù)臄?shù)學(xué)邏輯來驗證模型的可靠性.高階邏輯定理證明是形式化驗證中的一種，它是一種交互式驗證方法，主要涉及到基于高階邏輯的系統(tǒng)數(shù)學(xué)建模和基于演繹推理的系統(tǒng)性能驗證，它將系統(tǒng)與規(guī)范都描述成數(shù)學(xué)邏輯的形式，從公理出發(fā)尋求描述，證明數(shù)學(xué)模型的準(zhǔn)確性與可靠性[13].

由于機器人逆運動學(xué)的建模與求解是決定其安全性的必要因素之一.然而，目前使用形式化方法對機器人運動學(xué)進行建模與驗證的案例還屈指可數(shù).2017 年日本國家先進工業(yè)科學(xué)研究院信息技術(shù)研究所Affeldt[14]等人在COQ 定理證明器中，實現(xiàn)了羅德里格斯公式等數(shù)學(xué)理論定理庫的構(gòu)建，完成了SCARA 型串聯(lián)機器人運動學(xué)模型的構(gòu)建與驗證.巴基斯坦伊斯蘭堡國立科技大學(xué)Adnan Rashid[15]等采用旋量理論對細胞注射機器人動力學(xué)分析進行了形式化驗證.2020年首都師范大學(xué)陳琦[16]等采用定理證明的方法實現(xiàn)了平面并聯(lián)機構(gòu)的相關(guān)數(shù)學(xué)理論的高階邏輯表達，驗證了平面并聯(lián)機構(gòu)運動學(xué)分析的正確性和求解方法的可靠性.但是，對機器人逆運動學(xué)形式化建模與驗證的工作還是空白.因此本文在旋量高階邏輯定理證明庫的基礎(chǔ)上，實現(xiàn)指數(shù)積[17]和Paden-Kahan子問題(subprob-R)等數(shù)學(xué)理論的高階邏輯表達，在交互式定理證明器HOL-Light[18]中對6R協(xié)作型機器人逆運動學(xué)建模與求解過程進行形式化驗證.本文的主要貢獻如下：

·基于旋量理論，對剛體運動的運動旋量的指數(shù)形式進行形式化建模；

·對Paden-Kahan子問題(subprob-R)進行形式化建模；

·對6R型協(xié)作機器人逆運動學(xué)的求解過程進行形式化分析與驗證.

2 預(yù)備知識

本節(jié)將簡要介紹定理證明器HOL-Light和工具中常用符號的含義，然后分析6R型協(xié)作機器人的經(jīng)典結(jié)構(gòu).

2.1 HOL-Light基礎(chǔ)

HOL-Light是一種基于LCF理論的定理證明器[19].它的實現(xiàn)語言為元語言(ML)，廣泛地應(yīng)用于理論形式的數(shù)學(xué)證明地構(gòu)造[20].在定理證明器中，必須確保已經(jīng)建立了相關(guān)的數(shù)學(xué)定理庫，然后才可以進行定理證明[21].目前，在定理證明器HOL-Light中已存在大量的定理庫例如旋量、多元分析、實數(shù)、超越等.這些定理庫為本文的后續(xù)工作提供了強大的支持.因此，本文選擇使用定理證明器HOL-Light對機器人逆運動學(xué)進行形式化建模.為了方便對本文其它部分的理解，這里對定理證明器HOL-Light中一些常用的符號和函數(shù)進行了說明，如表1所示.

表1 HOL-Light中的符號與函數(shù)

2.2 人機協(xié)作機器人經(jīng)典結(jié)構(gòu)

與發(fā)展了幾十年的工業(yè)機器人相比，協(xié)作機器人最大的突破在于它可以直接與人類并肩合作，而無需使用安全圍欄進行隔離，這種方式不僅減少了人和機器人之間的距離，大大減少了工位所占的面積，更重要的是可以充分結(jié)合人和機器的優(yōu)勢，彼此取長補短，讓機器人輔助人類去完成高重復(fù)性、高精度的工作，而人類則解決靈活性高、不斷優(yōu)化的工作.例如，在裝配高精度的重型零部件時，人機協(xié)作便凸顯出其將人和機器結(jié)合在一起的優(yōu)勢.正是由于人與協(xié)作機器人的緊密工作關(guān)系，使得它的安全性問題受到格外的重視.

常見的協(xié)作型機器人如：UR、OUR等系列都是關(guān)節(jié)均為旋轉(zhuǎn)關(guān)節(jié)的6R型機器人，其結(jié)構(gòu)圖如圖1所示.其中1、2關(guān)節(jié)，4、5關(guān)節(jié)和5、6關(guān)節(jié)的軸線分別相交于一點，相鄰的2、3、4關(guān)節(jié)相互平行.在進行剛體變換時，它僅進行旋轉(zhuǎn)運動，沒有平移.

圖1 人機協(xié)作機器人旋量坐標(biāo)系

3 剛體運動與求解方法的形式化

在本節(jié)中，首先實現(xiàn)剛體運動旋量指數(shù)形式相關(guān)內(nèi)容的高價邏輯表達.然后基于旋量理論，構(gòu)建Paden-Kahan子問題(subprob-R)的形式化模型.

3.1 剛體運動旋量指數(shù)形式

定義1.算子“∧”

|-?screw_2_matrixξ=lambdaij.ifi<=3∧j<=3then(vec3_2_ssm(FSTξ))$i$jelseifi<=3∧j=4then(SNDξ)$ielse&0

定義2.位置的齊次變換矩陣

|-?homo_transxR=lambdaij.ifi=(dimindex(:N)+1)∧～(j=dmindex(:N)+1)then&0elseifi=(dimindex(:N)+1)∧(j=dmindex(:N)+1)then&1eslseif～(i=(dimindex(:N)+1) )∧(j=dmindex(:N)+1)thenx$jelseR$i$j

定義2中homo_transxR是位置的齊次矩陣在HOL-Light中的表示形式，其中的R為三階矩陣，x為三維列向量.

定義3.速度的齊次矩陣

|-?homo_trans_tangentdxdR=lambdaij.ifi=(dimindex(:N)+1)then&0elseif～(i=dimindex(:N)+1)∧(j=dimindex(:N)+1)thendx$ielsedR$i$j

定義3中的homo_trans_tangentdxdR就是速度的齊次矩陣在HOL-Light中的表達形式，其中dR為三階矩陣，dx為三維列向量.

由定義1可知，運動旋量和速度的齊次矩陣的數(shù)學(xué)含義是相同的，但其描述形式是不同的.因此，需要在定理證明器HOL-Light中對其數(shù)學(xué)含義相同的屬性進行證明，該過程被形式化描述為定理1.

定理1.運動旋量的矩陣表達形式

|-?s.screw_2_matrixξ=homo_trans_tangent(SNDξ)(vec3_2_ssm(FSTξ))

定理1證明了運動旋量可以表示為速度的齊次矩陣形式.

定理2.當(dāng)僅進行平移時的旋量的指數(shù)形式

|-?ξθ.FSTξ=vec0?matrix_exp=(θ%%screw_2_matrixξ)=homo_trans(θ%(SNDξ))(mat1)

定理3.當(dāng)既平移又旋轉(zhuǎn)時的運動旋量的指數(shù)形式

|-?ξθ.norm(FSTξ)=&1?matrix_exp=(θ%%screw_2_matrixξ)=homo_trans((mat1-matrix_exp(θ%%vec3_2_ssm(FSTξ)))**((FSTξ)cross(SNDξ) )+θ%%(vec3_2_ssm(FSTξ)**(SNDξ)))(matrix_exp(θ%%vec3_2_ssm(FSTξ)))

3.2 Paden-Kahan子問題(subprob-R)

基于Kahan[22]的研究，Paden[23]提出將幾何求解方法應(yīng)用于機器人逆運動學(xué)中.一般情況下，運動學(xué)求逆解的子問題是指涉及的運動旋量個數(shù)不超過3，且具有明確的幾何意義和數(shù)值穩(wěn)定性[1].根據(jù)子問題中運動旋量的個數(shù)與性質(zhì)可以將其分為11種子問題，例如：描述剛體旋轉(zhuǎn)的subprob-R、描述剛體平移的subprob-TT以及描述剛體旋轉(zhuǎn)和平移相結(jié)合的subprob-RT(或subprob-TR)等，這些統(tǒng)稱為Paden-Kahan子問題[8].其主要思想是將復(fù)雜的機器人運動學(xué)逆解分解為若干個具有明確幾何意義的逆解子問題，然后逐一解決，即將復(fù)雜的運動分解為幾個連續(xù)的簡單運動[24].在用旋量法對機器人求逆解的過程中，經(jīng)常會遇到這些子問題，現(xiàn)在越來越多的人關(guān)注到這個問題并對其進行研究.

圖2 子問題(subprob-R)

定理4.Paden-Kahan子問題(subprob-R)的解

|-?θωruu′vv′pqξ.--(pi/&2)<θ∧θ<(pi/&2)∧ξ=(ω,rcrossω)①

u=p-r∧v=q-r∧normω=&1∧②

u′=u-(vec3_vtrans(FSTξ)**u)∧③

v′=v-(vec3_vtrans(FSTξ)**v)∧④

matrix_exp(θ%%screw_2_matrixξ)**(homo_point(mk_pointr))=(homo_point(mk_pointr))∧⑤

matrix_exp(θ%%screw_2_matrixξ)**(homo_point(mk_pointp))=(homo_point(mk_pointq))∧～(u′=0)?⑥

θ=atn((ωdot(u′crossv′ ))/(u′dotv′))

圖3 子問題(subprob-R)投影

(1)

(2)

將公式(2)帶入子目標(biāo)后，可得公式(3).

(3)

然后，再引入一個子目標(biāo)ωωTu′=0，證明其成立.由于u′和u存在關(guān)系式：u′=u-ωωTu.利用重寫策略將該關(guān)系式寫入子目標(biāo)中，可得ωωTu′=ωωTωωTu′.通過使用化簡策略可將公式(3)化簡為公示(4).

(4)

在定理證明器中通過再次使用重寫策略將公式(4)重寫入公式(1)，經(jīng)過化簡策略即可證明子目標(biāo)成立.最終證明Paden-Kahan子問題(subprob-R)是成立的.

4 6R協(xié)作機器人逆運動學(xué)形式化驗證

4.1 正向運動學(xué)形式化建模

在對機器人進行逆運動學(xué)求解時，需要先根據(jù)圖1的坐標(biāo)系對機器人進行運動學(xué)建模.

各關(guān)節(jié)單位矢量ωi，軸上一點ri和運動旋量分別構(gòu)建如下[5]：

圖1中機器人對應(yīng)的初始位姿可由公式(5)描述.

(5)

公式(5)中的初始位姿可被形式化定義為定義4.

定義4.初始位姿

|-?gst_initialx=homo_transx(mat1)

在定義4中，mat1表示的是三階單位矩陣，x為三維列向量.

6自由度串聯(lián)機器人正向運動學(xué)的數(shù)學(xué)公式如公式(6)所示.

(6)

其正向運動學(xué)的形式化模型如定義5所示.

定義5.正向運動學(xué)

|-?gstξ1ξ2ξ3ξ4ξ5ξ6θ1θ2θ3θ4θ5θ6x=matrix_exp(θ1%%screw_2_matrixξ1)**matrix_exp(θ2%%screw_2_matrixξ2)**matrix_exp(θ3%%screw_2_matrixξ3)**matrix_exp(θ4%%screw_2_matrixξ4)**matrix_exp(θ5%%screw_2_matrixξ5)**matrix_exp(θ6%%screw_2_matrixξ6)**(gst_initialx)

4.2 逆運動學(xué)求解模型驗證

4.2.1 驗證關(guān)節(jié)角θ1

第1個關(guān)節(jié)到末端執(zhí)行器的剛體運動變化過程可由公式(7)描述.

(7)

該過程形式化模型如定義6所示.

定義6.從第1個關(guān)節(jié)到末端執(zhí)行器的剛體變換的運動過程

|-?gst1ξ1ξ2ξ3ξ4ξ5ξ6θ1θ2θ3θ4θ5θ6=matrix_exp(θ1%%screw_2_matrixξ1)**matrix_exp(θ2%%screw_2_matrixξ2)**matrix_exp(θ3%%screw_2_matrixξ3)**matrix_exp(θ4%%screw_2_matrixξ4)**matrix_exp(θ5%%screw_2_matrixξ5)**matrix_exp(θ6%%screw_2_matrixξ6)

根據(jù)定義6，可得到第1個關(guān)節(jié)到末端執(zhí)行器的剛體運動變化過程，這個運動變換過程的代數(shù)運算可由公式(8)描述.

(8)

根據(jù)公式(7)與公式(8)相等的屬性.該剛體的運動過程可以被形式化建模為定理5.

定理5.從第1個關(guān)節(jié)到末端執(zhí)行器的剛體變換的運動過程的代數(shù)運算

|-?ξ1ξ2ξ3ξ4ξ5ξ6θ1θ2θ3θ4θ5θ6x

gstξ1ξ2ξ3ξ4ξ5ξ6θ1θ2θ3θ4θ5θ6x**(matrix_inv(gst_initialx))=gst1ξ1ξ2ξ3ξ4ξ5ξ6θ1θ2θ3θ4θ5θ6

在定理5中，假設(shè)ξ1，ξ2，ξ3和ξ4是具有零節(jié)距的單位運動旋量，驗證了gst-1(0)同乘公式(6)兩邊可得到gst(1)的過程.

若設(shè)機器人第5、6關(guān)節(jié)軸線相交于點p56，用該點同時右乘公式(8)兩邊，并根據(jù)位置不變原則，可得：

(9)

公式(9)表示點p56分別繞著前4個關(guān)節(jié)軸線做旋轉(zhuǎn)運動，最終到達目標(biāo)位置p1，其幾何表示如圖4所示.

圖4 前4關(guān)節(jié)螺旋運動

從圖4可知，當(dāng)機器人位于初始位姿時，點p56分別圍繞第4、3、2關(guān)節(jié)軸線轉(zhuǎn)動角度θ4、θ3、θ2到達p2點位置.最后該點再圍繞第1關(guān)節(jié)軸線ξ1轉(zhuǎn)動角度θ1到達目標(biāo)位置p1.求解角度θ1的關(guān)鍵是求出點p2的坐標(biāo)，那么該點可通過已知的p56和p1兩點來求得.則根據(jù)圖4可得到公式(10).

(10)

由于各變量的值分別為p1=(px,py,pz)，ω1=(0,0,1)，ω2=(1,0,0)，o1=(0,0,pz)和p56=(d4,0,a2+a3+d5).結(jié)合公式(10)即可求得點p2的坐標(biāo)如公式(11)所示.

(11)

推導(dǎo)點p2坐標(biāo)的過程可形式化描述為定理6.

定理6.p2點坐標(biāo)

|-?o1ω1ω2p1p56p2pxpypza2a3d4.

(p2-p56)dotw2=&0∧(p2-01)dotω1=&0∧norm(p2-01)=norm(p1-o1) ∧p1=vector[px;py;pz]∧ω1=vector[&0;&0;&1]∧ω2=vector[&1;&0;&0]∧o1=vector[&0;&0;pz]∧p56=vector[d4;&0;a2+a3+d4]?(p2$1=d4∧p2$3=pz∧p2$2pow2=(pxpow2+pypow2-d4pow2))

點p2在第1關(guān)節(jié)中的螺旋運動可表示為公式(12).

(12)

根據(jù)已知的各個變量的坐標(biāo)值，通過應(yīng)用Paden-Kahan子問題(subprob-R)，可得θ1的值為公式(13).

(13)

根據(jù)Paden-Kahan子問題(subprob-R)求解θ1值的過程可形式化描述定理7.

定理7.驗證關(guān)節(jié)角度θ1

|-?p1p2ω1r1o1pxpypzs1d4ξ1θ1.

p1=vector[px;py;pz]∧p2=vector[d4;b1;pz]∧ω1=vector[&0;&0;&1]∧ξ1=(ω1;vec0)∧o1=vector[&0;&0;pz]∧～(d4=0)∧--(pi/&2)<θ1∧θ1<(pi/&2)∧norm(p2-01)=norm(p1-o1)∧matrix_exp(θ1%%screw_2_matrixξ1)**(homo_point(mk_pointp2))=(homo_point(mk_pointp1))?θ1=atn((--(px*s1)+d4*py)/(d4*px+py*s1))

其中，atn表示的是反正切函數(shù)arctan.其證明過程如下，首先使用特殊化策略引入Paden-Kahan子問題(subprob-R)，然后結(jié)合旋量指數(shù)形式定理(定理3)，運用化簡策略SIMP_TAC實現(xiàn)定理7的證明，該策略的主要功能是通過已有定理進行有條件的上下文重寫來簡化目標(biāo).

4.2.2 驗證關(guān)節(jié)角θ5和θ6

第2個關(guān)節(jié)到末端執(zhí)行器的剛體運動變化過程可由公式(14)描述.

(14)

該過程可被形式形式化描述為定義7.

定義7.從第2個關(guān)節(jié)到末端執(zhí)行器的剛體變換的運動過程

|-?gst2ξ2ξ3ξ4ξ5ξ6θ2θ3θ4θ5θ6=matrix_exp(θ2%%screw_2_matrixξ2)**matrix_exp(θ3%%screw_2_matrixξ3)**matrix_exp(θ4%%screw_2_matrixξ4)**matrix_exp(θ5%%screw_2_matrixξ5)**matrix_exp(θ6%%screw_2_matrixξ6)

在公式(6)中，可令gst(θ)如公式(15)所示.

(15)

根據(jù)公式(14)可得如公式(16)所示的關(guān)節(jié)角θ5和θ6與其他變量之間關(guān)系，其形式化模型如定理8所示.

(16)

定理8.關(guān)節(jié)角θ5和θ6與其他變量之間關(guān)系形式化模型

|- ?ξ1ξ2ξ3ξ4ξ5ξ6xθ1θ2θ3θ4θ5θ6d4d5a2a3R11R21R31R12R22R32R13R23R33t1t2t3.

gstξ1ξ2ξ3ξ4ξ5ξ6θ1θ2θ3θ4θ5θ6x=homo_trans(vector[t1;t2;t3])(vector[vector[R11;R12;R13];vector[R21;R22;R23];

vector[R31;R32;R33])∧x=vector[d4;&0;a2+a3+d5]∧ξ1=(vector[&0;&0;&1],vector[&0;&0;&0] )∧ξ2=(vector[&1;&0;&0],vector[&0;&0;&0] )∧ξ3=(vector[&1;&0;&0],vector[&0;a2;&0] )∧ξ4=(vector[&1;&0;&0],vector[&0;a2+a3;&0])∧ξ5=(vector[&0;&0;&1],vector[&0;--d4;&0] )∧ξ6=(vector[&1;&0;&0],vector[&0;a2+a3+d5;&0] )?cosθ5=R11*cosθ1+R21*sinθ1∧sinθ5*sinθ6=R13*cosθ1+R23*sinθ1∧--(sinθ5*cosθ6)=R12*cosθ1+R22*sinθ1∧sinθ5*sin (θ2+θ3+θ4)=R31∧sinθ5*cos(θ2+θ3+θ4)=R21*cosθ1-R11*sinθ1

定理8驗證了各個變量之間的關(guān)系是準(zhǔn)確的.

通過化簡公式(16)可得公式(17)-(19)，即為關(guān)節(jié)角θ5，θ6和θ2+θ3+θ4的解析模型.

(17)

θ6=arctan((s3/sinθ5)/(-s4/sinθ5))(sinθ5≠0)

(18)

θ2+θ3+θ4=arctan((R31/sinθ5)/(s5/sinθ5))(sinθ5≠0)

(19)

式中，s2=R11*cosθ1+R21*sinθ1，s3=R13*cosθ1+R23*sinθ1，s4=R12*cosθ1+R22*sinθ1，s5=R21*cosθ1-R11*sinθ1.

根據(jù)變量之間的已知關(guān)系，關(guān)節(jié)角θ5，θ6和θ2+θ3+θ4解析模型的形式化驗證過程描述如定理9所示.

定理9.驗證關(guān)節(jié)角θ5和θ6

|-?θ1θ2θ3θ4θ5θ6s2s3s4s5R11R21R31R12R22R13R23.

～(sinθ5=&0)∧--(pi/&2)<θ6∧θ6<(pi/&2)∧--(pi/&2)<(θ2+θ3+θ4)∧(θ2+θ3+θ4)<(pi/&2)∧s2=R11*cosθ1+R21*sinθ1∧s3=R13*cosθ1+R23*sinθ1∧s4=R12*cosθ1+R22*sinθ1∧s5=R21*cosθ1-R11*sinθ1∧cosθ5=R11*cosθ1+R21*sinθ1∧sinθ5*sinθ6=R13*cosθ1+R23*sinθ1∧--(sinθ5*cosθ6)=R12*cosθ1+R22*sinθ1∧sinθ5*sin(θ2+θ3+θ4)=R31∧sinθ5*cos(θ2+θ3+θ4)=R21*cosθ1-R11*sinθ1?(&0<θ5∧θ5<(pi/&2)?θ5=atn((sqrt(&1-s2pow2))/b2))∧(--(pi/&2)<θ5∧θ5<&0?θ5=atn(--(sqrt(&1-s2pow2))/s2))∧θ6=atn((s3/(sinθ5))/(--(s4)/sinθ5))∧(θ2+θ3+θ4)=atn((R31/sinθ5)/(s5/(sinθ5)))

根據(jù)已知的變量之間的關(guān)系，首先使用策略UNDISCH_TAC把假設(shè)列表中條件改寫為目標(biāo)的前件，然后結(jié)合策略DISCH_THEN(MP_TACoSYM)使得目標(biāo)前件中等價條件的兩側(cè)互換位置，最后使用庫中已有定理化簡證明定理9.

4.2.3 驗證關(guān)節(jié)角θ2，θ3和θ4

第3個關(guān)節(jié)到末端執(zhí)行器的剛體運動變化過程可由公式(20)描述.

(20)

該過程的形式化描述如定義8所示.

定義8.從第3個關(guān)節(jié)到末端執(zhí)行器的剛體變換的運動過程

|-?gst3ξ2ξ3ξ4θ2θ3θ4=matrix_exp(θ2%%screw_2_matrixξ2)**matrix_exp(θ3%%screw_2_matrixξ3)**matrix_exp(θ4%%screw_2_matrixξ4)

(21)

根據(jù)公式(20)和公式(21)，可以得到公式(22).

(22)

式中t7=t5-(θ2+θ3)sin(θ2+θ3+θ4)，t8=t6+(a2+a3)cos(θ2+θ3+θ4).結(jié)合公式(21)和公式(22)即可求得θ2和θ2+θ3的解.關(guān)節(jié)角θ2，θ3和θ4與各個變量之間的關(guān)系可形式化描述為定理10.

定理10.各個變量之間幾何關(guān)系形式化模型

通過定理10驗證了各個變量之間的關(guān)系是準(zhǔn)確的，然后可求得θ2和θ2+θ3的解如公式(23)和公式(24)所示.

(23)

(24)

其中，A=2a2t7，B=2a2t8，C=a32-a22-t72-t82，s6=a2sinθ2+t7，s7=t8-a2cosθ2.

關(guān)節(jié)角θ2和θ2+θ3的形式化驗證過程如定理11所示.

定理11.驗證關(guān)節(jié)角θ2和θV2+θ3

|-?θ2θ3a2a3t7t8s6s7ABC.

--(pi/&2)<θ2∧θ2<(pi/&2)∧--(pi/&2)<θ2+θ3∧θ2+θ3<(pi/&2)∧--a3*sin (θ2+θ3)-a2*sinθ2=t7∧a3*cos (θ2+θ3)+a2*cosθ2=t8∧A=&2*a2*t7∧B=&2*a2*t8∧C=(a3)pow2-(a2)pow2-(t7)pow2-(t8)pow2∧s6=a2*sinθ2+t7∧s7=t8-a2*cosθ2∧～(a2=&0)∧～(t7=&0)∧～(t7pow2+t8pow2=&0)∧--(pi/&2)<(atn(t8/t7)+atn((t7*sinθ2-t8*cosθ2)/(t7*cosθ2-t8*sinθ2))))∧(atn(t8/t7)+atn((t7*sinθ2-t8*cosθ2)/(t7*cosθ2-t8*sinθ2))))<(pi/&2)?(&0<(&2*t7*a2*cosθ2+&2*t8*a2*sinθ2)?(θ2=atn(B/A)+atn(C/(sqrt((Apow2)+(Bpow2)-(Cpow2)))))∧((&2*t7*a2*cosθ2+&2*t8*a2*sinθ2)<&0?(θ2=atn(B/A)+atn(C/(--(sqrt((Apow2)+(Bpow2)-(Cpow2)))))∧θ2+θ3=atn((--(s6)/a3)/(s7/a3))

首先引入變量C和sqrt((Apow2)+(Bpow2)-(Cpow2))的最簡形式的子目標(biāo)，然后結(jié)合重寫策略REWRITE_TAC，化簡策略SIMP_TAC實現(xiàn)定理11的證明.

通過公式(19)、公式(23)和公式(24)，結(jié)合公式(25)和公式(26)即可得到θ3和θ4的解.在HOL-Light中，可以直接利用重寫策略和實數(shù)自動證明策略完成θ3和θ4的求解過程的形式化驗證.

θ3=(θ2+θ3)-θ2

(25)

θ4=(θ2+θ3+θ4)-(θ2+θ3)

(26)

4.2小節(jié)完成了對6R型協(xié)作機器人逆運動學(xué)各個關(guān)節(jié)角求解模型的形式化驗證，從而確保6R協(xié)作機器人逆運動學(xué)求解的安全性和可靠性.

5 結(jié) 論

本文以旋量理論和指數(shù)積公式為基礎(chǔ)，通過解析、幾何、矩陣理論和Paden-Kahan子問題(subprob-R)對6自由度協(xié)作型機器人求逆解過程進行形式化驗證.在高階邏輯定理證明器HOL-Light中，首先基于已有的旋量庫，對于缺少的剛體運動旋量的指數(shù)形式進行了補充.然后實現(xiàn)了指數(shù)積公式和Paden-Kahan子問題(subprob-R)相關(guān)理論的高階邏輯化.最后，結(jié)合旋量理論、指數(shù)積公式和Paden-Kahan子問題(subprob-R)等相關(guān)定理對協(xié)作型機器人逆運動學(xué)求解模型進行形式化分析與驗證.從而確保基于旋量理論的機器人逆運動學(xué)求解模型與求解過程的完備性與可靠性，保證機器人工作過程中的安全性.本文的研究內(nèi)容有效的確保了機器人逆運動學(xué)建模和求解過程的可靠性，從而保證了機器人系統(tǒng)的安全性.同時也促進了機器人產(chǎn)業(yè)更快更好的發(fā)展，為未來協(xié)作型機器人逆運動學(xué)的形式化分析奠定了基礎(chǔ).