協(xié)作機(jī)器人逆運(yùn)動(dòng)學(xué)形式化建模與驗(yàn)證

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

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

1 引 言

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

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

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

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

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

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

·基于旋量理論，對(duì)剛體運(yùn)動(dòng)的運(yùn)動(dòng)旋量的指數(shù)形式進(jìn)行形式化建模；

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

·對(duì)6R型協(xié)作機(jī)器人逆運(yùn)動(dòng)學(xué)的求解過程進(jìn)行形式化分析與驗(yàn)證.

2 預(yù)備知識(shí)

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

2.1 HOL-Light基礎(chǔ)

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

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

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

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

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

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

3 剛體運(yùn)動(dòng)與求解方法的形式化

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

3.1 剛體運(yùn)動(dòng)旋量指數(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中的表達(dá)形式，其中dR為三階矩陣，dx為三維列向量.

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

定理1.運(yùn)動(dòng)旋量的矩陣表達(dá)形式

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

定理1證明了運(yùn)動(dòng)旋量可以表示為速度的齊次矩陣形式.

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

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

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

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

(4)

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

4 6R協(xié)作機(jī)器人逆運(yùn)動(dòng)學(xué)形式化驗(yàn)證

4.1 正向運(yùn)動(dòng)學(xué)形式化建模

在對(duì)機(jī)器人進(jìn)行逆運(yùn)動(dòng)學(xué)求解時(shí)，需要先根據(jù)圖1的坐標(biāo)系對(duì)機(jī)器人進(jìn)行運(yùn)動(dòng)學(xué)建模.

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

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

(5)

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

定義4.初始位姿

|-?gst_initialx=homo_transx(mat1)

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

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

(6)

其正向運(yùn)動(dòng)學(xué)的形式化模型如定義5所示.

定義5.正向運(yùn)動(dòng)學(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 逆運(yùn)動(dòng)學(xué)求解模型驗(yàn)證

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

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

(7)

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

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

|-?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個(gè)關(guān)節(jié)到末端執(zhí)行器的剛體運(yùn)動(dòng)變化過程，這個(gè)運(yùn)動(dòng)變換過程的代數(shù)運(yùn)算可由公式(8)描述.

(8)

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

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

|-?ξ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é)距的單位運(yùn)動(dòng)旋量，驗(yàn)證了gst-1(0)同乘公式(6)兩邊可得到gst(1)的過程.

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

(9)

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

圖4 前4關(guān)節(jié)螺旋運(yùn)動(dòng)

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

(11)

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

定理6.p2點(diǎn)坐標(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))

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

(12)

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

(13)

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

定理7.驗(yàn)證關(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)，運(yùn)用化簡(jiǎn)策略SIMP_TAC實(shí)現(xiàn)定理7的證明，該策略的主要功能是通過已有定理進(jìn)行有條件的上下文重寫來簡(jiǎn)化目標(biāo).

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

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

(14)

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

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

|-?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驗(yàn)證了各個(gè)變量之間的關(guān)系是準(zhǔn)確的.

通過化簡(jiǎ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解析模型的形式化驗(yàn)證過程描述如定理9所示.

定理9.驗(yàn)證關(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)前件中等價(jià)條件的兩側(cè)互換位置，最后使用庫中已有定理化簡(jiǎn)證明定理9.

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

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

(20)

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

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

|-?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與各個(gè)變量之間的關(guān)系可形式化描述為定理10.

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

通過定理10驗(yàn)證了各個(gè)變量之間的關(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的形式化驗(yàn)證過程如定理11所示.

定理11.驗(yàn)證關(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))的最簡(jiǎn)形式的子目標(biāo)，然后結(jié)合重寫策略REWRITE_TAC，化簡(jiǎn)策略SIMP_TAC實(shí)現(xiàn)定理11的證明.

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

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

(25)

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

(26)

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

5 結(jié) 論

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