陳 琦，王國(guó)輝，張倩穎，施智平，陳善言，關(guān) 永

(高可靠嵌入式系統(tǒng)技術(shù)北京市工程研究中心，首都師范大學(xué) 信息工程學(xué)院，北京 100048)

E-mail：ghwang@cnu.edu.cn

1 引 言

并聯(lián)機(jī)構(gòu)是指含有兩個(gè)或兩個(gè)以上自由度且以并聯(lián)方式驅(qū)動(dòng)的一種閉環(huán)機(jī)構(gòu)，它的動(dòng)平臺(tái)和靜平臺(tái)通過(guò)至少兩個(gè)獨(dú)立的支鏈相連接.常見(jiàn)的并聯(lián)機(jī)構(gòu)包括平面并聯(lián)機(jī)構(gòu)和空間并聯(lián)機(jī)構(gòu).其中具有三自由度的平面并聯(lián)機(jī)構(gòu)在3D打印[1]、數(shù)控機(jī)床[2]等方面有著廣泛的應(yīng)用.它能夠完成靈活、精細(xì)的抓取操作的同時(shí)具有較強(qiáng)的承載力，但這也增加了平面并聯(lián)機(jī)構(gòu)的控制難度.平面并聯(lián)機(jī)構(gòu)控制算法是為了解決工作空間[3]、奇異位置[4]、校對(duì)零位置[5]等問(wèn)題而被學(xué)術(shù)界提出的，這些問(wèn)題的研究基礎(chǔ)是正運(yùn)動(dòng)學(xué)求解.正運(yùn)動(dòng)學(xué)求解[6]是指根據(jù)支鏈的桿長(zhǎng)對(duì)動(dòng)平臺(tái)相對(duì)靜平臺(tái)的位置和姿態(tài)進(jìn)行求解.常用的正運(yùn)動(dòng)學(xué)求解方法是D-H法，但它存在消元求解技巧性強(qiáng)、計(jì)算過(guò)程復(fù)雜等問(wèn)題，極易導(dǎo)致求解過(guò)程錯(cuò)誤.為了解決這一問(wèn)題，學(xué)術(shù)界基于幾何代數(shù)對(duì)平面并聯(lián)機(jī)構(gòu)進(jìn)行了研究.例如：張立先[7]提出了基于幾何代數(shù)的并聯(lián)機(jī)構(gòu)分析，利用旋量和關(guān)節(jié)速度的幾何表示，研究并聯(lián)機(jī)構(gòu)的奇異性，但其表達(dá)式不夠簡(jiǎn)潔.而相比于幾何代數(shù)，共形幾何代數(shù)[8](Conformal Geometric Algebra，CGA)具有幾何概念清楚、物理意義明確、表達(dá)形式簡(jiǎn)單、代數(shù)運(yùn)算方便等優(yōu)點(diǎn)，便于工程應(yīng)用中的幾何解釋、問(wèn)題描述，極大地降低了機(jī)器人運(yùn)動(dòng)學(xué)的研究難度.因此，學(xué)術(shù)界基于CGA對(duì)平面并聯(lián)機(jī)構(gòu)進(jìn)行了研究.例如：倪振松[9]提出了基于CGA的一種平面并聯(lián)機(jī)構(gòu)位置正解分析方法，建立了運(yùn)動(dòng)學(xué)正解模型.它是CGA在平面并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)應(yīng)用的一個(gè)初步嘗試.之后，針對(duì)3RPR平面并聯(lián)機(jī)構(gòu)，張英[10]提出了平面并聯(lián)機(jī)構(gòu)正運(yùn)動(dòng)學(xué)分析的幾何建模和免消元計(jì)算，為平面并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)求解理論提供了一種新思路.以上算法都是基于特定機(jī)構(gòu)進(jìn)行研究，而黃昔光[6]提出的基于CGA的平面并聯(lián)機(jī)構(gòu)位置正解求解方法，是一種一般性求解方法，普遍適用于所有平面并聯(lián)機(jī)構(gòu)，但該方法仍處于數(shù)值驗(yàn)證階段.因此，本文基于CGA對(duì)平面并聯(lián)機(jī)構(gòu)的正向運(yùn)動(dòng)學(xué)求解方法進(jìn)行形式化建模與驗(yàn)證.

與基于計(jì)算機(jī)代數(shù)系統(tǒng)等傳統(tǒng)方法不同，形式化方法是是建立在嚴(yán)格完備的數(shù)學(xué)基礎(chǔ)上，采用精確數(shù)學(xué)語(yǔ)義的推理方法[11].它可以根據(jù)數(shù)學(xué)邏輯的嚴(yán)密性來(lái)提高發(fā)現(xiàn)微小錯(cuò)誤的機(jī)率，具有較強(qiáng)的準(zhǔn)確性和完備性.目前形式化驗(yàn)證技術(shù)主要有以下兩種：模型檢驗(yàn)和定理證明.模型檢驗(yàn)[12]是將系統(tǒng)表達(dá)為有限狀態(tài)機(jī)的自動(dòng)驗(yàn)證技術(shù).定理證明[13]是將研究?jī)?nèi)容建立物理模型，提取屬性性質(zhì)后轉(zhuǎn)化成數(shù)學(xué)模型，根據(jù)語(yǔ)言規(guī)范和運(yùn)算變成邏輯模型.與模型檢驗(yàn)相比，定理證明不僅可以處理大規(guī)模的系統(tǒng)，而且可以對(duì)無(wú)限狀態(tài)系統(tǒng)進(jìn)行推理，擁有更好的靈活性.因此，本文采用定理證明的方法.常見(jiàn)的定理證明器有：ACL2、Mizar、Coq、PVS、HOL等，其中HOL家族中HOL Light[14]是最流行的定理證明器之一，它包含了高效的證明策略和豐富的數(shù)學(xué)定理庫(kù)，如：幾何代數(shù)庫(kù)、共形幾何代數(shù)庫(kù)、實(shí)分析庫(kù)、多維向量庫(kù)等，而且使用了更加簡(jiǎn)單易懂的邏輯核心，能夠準(zhǔn)確檢查定義和定理的形式化表達(dá)的正確性，使系統(tǒng)更加輕便、高效、簡(jiǎn)潔，為我們接下來(lái)的工作提供了重要的保障.本文選擇HOL Light對(duì)CGA進(jìn)行驗(yàn)證.

目前，幾何代數(shù)形式化工作也取得了一些成果，例如：李黎明[11，15]完成了幾何代數(shù)庫(kù)的搭建，形式化證明了幾何代數(shù)結(jié)構(gòu)及基本運(yùn)算，為我的工作奠定了理論基礎(chǔ).馬莎[8，16]完成了共形幾何代數(shù)庫(kù)的搭建，形式化驗(yàn)證了幾何表示和幾何變換，同時(shí)，建立了5-R 串聯(lián)機(jī)器人位置逆解和串聯(lián)機(jī)器人抓取算法的模型，但目前并沒(méi)有對(duì)并聯(lián)機(jī)構(gòu)進(jìn)行形式化建模與驗(yàn)證.

本文以幾何代數(shù)、CGA的高階邏輯表達(dá)為基礎(chǔ)，在HOL Light定理證明器中形式化描述了平面并聯(lián)機(jī)構(gòu)相關(guān)數(shù)學(xué)理論，通過(guò)運(yùn)動(dòng)變換等效模型、點(diǎn)運(yùn)動(dòng)變換表達(dá)式方程、桿長(zhǎng)約束方程、位置正解封閉方程組實(shí)現(xiàn)平面并聯(lián)機(jī)構(gòu)正向運(yùn)動(dòng)學(xué)的高階邏輯模型的建立，形式化分析與驗(yàn)證正向運(yùn)動(dòng)學(xué)的一般性求解算法，驗(yàn)證位置正解方程與一元六次方程的等價(jià)性，從而確保平面并聯(lián)機(jī)構(gòu)運(yùn)動(dòng)學(xué)分析的正確性和分析求解方法的可靠性.

2 基于CGA的剛體運(yùn)動(dòng)的形式化

幾何代數(shù)是一種用于描述和計(jì)算幾何問(wèn)題的代數(shù)語(yǔ)言，實(shí)現(xiàn)了高維幾何計(jì)算與幾何分析的統(tǒng)一[16]，使其成為連接幾何和代數(shù)的統(tǒng)一的描述性語(yǔ)言.幾何代數(shù)包括三種重要的運(yùn)算積：外積、內(nèi)積和幾何積.外積主要用于幾何體的構(gòu)建，內(nèi)積主要用于距離與角度的計(jì)算，幾何積主要用于運(yùn)動(dòng)的描述.表1是幾何代數(shù)的運(yùn)算符號(hào).

表1 幾何代數(shù)的運(yùn)算符號(hào)

內(nèi)積是一個(gè)降維運(yùn)算，它與向量代數(shù)中的標(biāo)準(zhǔn)內(nèi)積不同.幾何代數(shù)的內(nèi)積運(yùn)算對(duì)象既可以是同維度空間對(duì)象也可以是不同維度空間對(duì)象，它能表示空間對(duì)象的距離、角度等幾何表達(dá).Inner是HOL Light中定義的高階邏輯函數(shù)，表示幾何代數(shù)的內(nèi)積運(yùn)算.

外積是一個(gè)升維運(yùn)算.Outer是HOL Light中定義的高階邏輯函數(shù)，表示幾何代數(shù)的外積運(yùn)算.當(dāng)向量a，b線性相關(guān)時(shí)，其外積為零.此外，外積也滿足反對(duì)稱性.

幾何積是連接內(nèi)積和外積的混合運(yùn)算.對(duì)于向量來(lái)說(shuō)，幾何積等于其內(nèi)積和外積之和.

下文是基于CGA的剛體運(yùn)動(dòng)的形式化.空間剛體運(yùn)動(dòng)主要包括旋轉(zhuǎn)和平移.

定義1.旋轉(zhuǎn)算子的旋量可表示為

(1)

可形式化如下：

|-?tmn.Rotation_CGAtmn=cos(t/& 2)%mbasis{}-

sin(t/& 2)/(norm(multivecmoutermultivecn)%

(multivecmoutermultivecn))

其中，t，m，n是變量.“&”是將自然數(shù)類型(：num)轉(zhuǎn)換為實(shí)數(shù)類型(：real)的運(yùn)算符.“%”表示向量的數(shù)乘.為了保持類型統(tǒng)一，實(shí)數(shù)與mbasis{}相乘可以把實(shí)數(shù)類型轉(zhuǎn)化為向量類型.函數(shù)norm是HOL Light中定義的高階邏輯函數(shù)，輸入變量是N維向量，輸出結(jié)果是N維向量的范數(shù).函數(shù)multivec可將向量轉(zhuǎn)換為對(duì)應(yīng)的多重向量的形式.

定義2.平移算子的旋量可表示為

T=1-(t1e1+t2e2+t3e3)e∞/2

(2)

其中，t=t1e1+t2e+t3e3為平移向量.

可形式化如下：

|-?t.Translation_CGAt=mbasis{}-& 1/& 2%(t＄1%

mbasis{1}+t＄2%mbasis{2}+t＄3%mbasis{3})*null_inf

當(dāng)運(yùn)算對(duì)象為實(shí)數(shù)“*”表示數(shù)乘.當(dāng)運(yùn)算對(duì)象為多重向量時(shí)表示幾何積.其中mbasis{i}表示靜坐標(biāo)的基底ei.t＄1%mbasis{1}+t＄2%mbasis{2}+t＄3%mbasis{3}表示平移向量.

定理1.旋轉(zhuǎn)算子的共軛

旋轉(zhuǎn)算子R的共軛

(3)

可形式化如下：

|-?tmn.reversion(Rotation_CGAtmn)=cos(t/& 2)%mbasis(t/& 2)/(norm(multivecmoutermultivecn)%(multivecmoutermultivecn))

其中，reversion(Rotation_CGAtmn)表示旋轉(zhuǎn)的共軛.通過(guò)旋轉(zhuǎn)定義、共軛、幾何積和外積的基本運(yùn)算性質(zhì)，結(jié)合策略REWRITE_TAC簡(jiǎn)化目標(biāo)，進(jìn)而利用策略COND_CASES_TAC拆分目標(biāo)，最后使用向量基本運(yùn)算性質(zhì)實(shí)現(xiàn)定理1的形式化證明.該定理在剛體運(yùn)動(dòng)方程的共軛(定理4)中將被使用到.

定理2.平移算子的共軛

平移算子T的共軛

(4)

可形式化如下：

|-reversion(Translation_CGAt)=mbasis{}+& 1/& 2%

(t＄1%mbasis{1}+t＄2%mbasis{2}+t＄3%mbasis{3})*null_inf

通過(guò)平移定義、共軛的基本運(yùn)算性質(zhì)，結(jié)合有限集合相關(guān)定理對(duì)不同情況進(jìn)行分類討論，并使用實(shí)數(shù)推導(dǎo)策略完成定理2的形式化證明.

定義3.剛體運(yùn)動(dòng)的運(yùn)動(dòng)算子

在CGA中，旋轉(zhuǎn)(定義1)和平移(定義2)在旋量下的復(fù)合是簡(jiǎn)單的幾何積，所以剛體運(yùn)動(dòng)的運(yùn)動(dòng)算子

M=RT

(5)

可形式化如下：

|-?tmnl.motor_CGAtmnl=Rotation_CGAtmn*

Translation_CGAl

其中，t，m，n，l表示變量.其中，motor_CGAtmnl表示剛體運(yùn)動(dòng)算子.Rotation_CGAtmn表示旋轉(zhuǎn)算子，它包含三個(gè)變量；Translation_CGAl表示平移算子，它包含一個(gè)變量.

定義4.剛體運(yùn)動(dòng)的運(yùn)動(dòng)算子

在CGA中，任意剛體o的剛體運(yùn)動(dòng)變化都可以借助幾何積來(lái)實(shí)現(xiàn)，廣泛適用于CGA中所有剛體的空間運(yùn)動(dòng)變換.它的表達(dá)式為：

(6)

其中，origid_motion為o變化后的剛體；M為規(guī)范化的運(yùn)動(dòng)變換算子.

可形式化如下：

|-?xtmnl.geometry_CGAxtmnl=motor_CGAtmnl*

x*reversion(motor_CGAtmnl)

其中，x表示任意剛體，motor_CGAtmnl表示剛體運(yùn)動(dòng)的運(yùn)動(dòng)算子，reversion(motor_CGAtmnl)表示運(yùn)動(dòng)算子的共軛.

3 平面并聯(lián)機(jī)構(gòu)的形式化建模與驗(yàn)證

平面并聯(lián)機(jī)構(gòu)正運(yùn)動(dòng)學(xué)的一般性算法是指根據(jù)桿長(zhǎng)的約束條件，建立1140種平面并聯(lián)機(jī)構(gòu)等效機(jī)構(gòu)模型的普適方法.該算法可以解決實(shí)際生產(chǎn)過(guò)程中所有平面并聯(lián)機(jī)構(gòu)的正運(yùn)動(dòng)學(xué)問(wèn)題.研究?jī)?nèi)容主要分為兩部分：一是對(duì)平面并聯(lián)機(jī)構(gòu)等效模型的位置正解封閉方程組進(jìn)行形式化建模；二是驗(yàn)證平面并聯(lián)機(jī)構(gòu)位置正解方程與一元六次方程的等價(jià)性.具體步驟如下：

S1：為了將具體的實(shí)際問(wèn)題抽象成為CGA問(wèn)題，需要建立運(yùn)動(dòng)變換等效模型，如圖1所示.這一模型可以代表所有平面并聯(lián)機(jī)構(gòu)，通過(guò)這一模型就能夠得到剛體運(yùn)動(dòng)算子方程.

圖1 平面并聯(lián)機(jī)構(gòu)等效模型

S2：基于這個(gè)等效模型，建立點(diǎn)運(yùn)動(dòng)變換后的表達(dá)式方程.接著，通過(guò)剛體運(yùn)動(dòng)算子方程，得到動(dòng)平臺(tái)的點(diǎn)在靜平臺(tái)下的坐標(biāo).

S3：將上述步驟求得的點(diǎn)坐標(biāo)與桿長(zhǎng)結(jié)合得到桿長(zhǎng)約束方程.

S4：通過(guò)步驟2、步驟3的運(yùn)算結(jié)果，建立了平面并聯(lián)機(jī)構(gòu)等效模型的位置正解封閉方程組.

S5：對(duì)封閉方程組的消簡(jiǎn)過(guò)程進(jìn)行形式化驗(yàn)證.也就是驗(yàn)證平面并聯(lián)機(jī)構(gòu)位置正解方程與一元六次方程的等價(jià)性.

3.1 建立運(yùn)動(dòng)變換等效模型

圖1為平面并聯(lián)機(jī)構(gòu)的等效模型，虛線表示連接動(dòng)平臺(tái)、靜平臺(tái)的等效運(yùn)動(dòng)支鏈，其長(zhǎng)度為支鏈的實(shí)際長(zhǎng)度.它是一個(gè)一般性的方法，普遍適用于所有平面并聯(lián)機(jī)構(gòu).

取點(diǎn)P1為靜坐標(biāo)xoy的坐標(biāo)原點(diǎn)，P1P2方向?yàn)閤軸，三個(gè)靜平臺(tái)點(diǎn)坐標(biāo)分別為P1(0，0)，P2(P2x，0)，P3(P3x，P3y)；取P4為動(dòng)坐標(biāo)x1o1y1的原點(diǎn)，P4P5方向?yàn)閯?dòng)坐標(biāo)x1軸，點(diǎn)在動(dòng)坐標(biāo)x1o1y1中的坐標(biāo)為P4(0，0)，P5(P5x，0)，P6(P6x，P6y).設(shè)點(diǎn)P4在靜坐標(biāo)xoy中的坐標(biāo)為(Px，Py)，動(dòng)坐標(biāo)x1o1y1相對(duì)于靜坐標(biāo)xoy的姿態(tài)角為θ.平面并聯(lián)機(jī)構(gòu)正運(yùn)動(dòng)學(xué)的一般性算法就是已知三條等效支鏈的桿長(zhǎng)q1(P1P4)、q2(P2P5)、q3(P3P6)，求點(diǎn)P4在靜坐標(biāo)的坐標(biāo)(Px，Py)及姿態(tài)角θ.

動(dòng)坐標(biāo)到靜坐標(biāo)的運(yùn)動(dòng)變換可以看做三個(gè)點(diǎn)的變換過(guò)程，P4可看作靜坐標(biāo)中的P1沿x軸平移Px，沿y軸平移Py，再繞z軸旋轉(zhuǎn)θ角得到，P5、P6也如此，這樣子就可以得到P4、P5、P6在靜坐標(biāo)下的坐標(biāo).通過(guò)建立運(yùn)動(dòng)變換等效模型，得到剛體運(yùn)動(dòng)算子方程.

定義5.橫向平移

靜坐標(biāo)沿x軸平移Px，即

(7)

可形式化如下：

|-?Px.Translation_CGA_TxPx=mbasis{}-& 1/& 2%Px%mbasis{1}*null_inf

橫向平移表示靜坐標(biāo)只沿著x軸移動(dòng)Px.依據(jù)定義2平移算子的定義，橫向平移中的mbasis{2}、mbasis{3}分別表示y方向與z方向.橫向平移并未向這兩個(gè)方向進(jìn)行平移，所以它們前面的系數(shù)t＄2、t＄3均為零.

定義6.縱向平移

靜坐標(biāo)沿y軸平移Py，即

(8)

可形式化如下：

|-?Py.Translation_CGA_TyPy=mbasis{}-& 1/& 2%

Py%mbasis{2}*null_inf

同理，定義6也如此.它表示靜坐標(biāo)沿著y軸平移Py，因此只包括mbasis{2}.

定義7.旋轉(zhuǎn)公式

靜坐標(biāo)繞z軸旋轉(zhuǎn)t，即

Rz=cos(t/2)-(e1∧e2)sin(t/2)

(9)

可形式化如下：

|-?t.Rotation_CGA_Rzt=cos(t/& 2)%mbasis{}-

(mbasis{1}outermbasis{2})*sin(t/& 2)%mbasis{}

旋轉(zhuǎn)公式表示靜平臺(tái)沿著z軸旋轉(zhuǎn)t.依據(jù)旋轉(zhuǎn)算子、外積的運(yùn)算性質(zhì)，可得norm(mbasis{1}outermbasis{2})等于1，可以省略norm(multivecmoutermultivecn)不寫(xiě).因此，旋轉(zhuǎn)算子簡(jiǎn)化為上式所示.

定義8.模型的剛體運(yùn)動(dòng)算子

該模型的剛體運(yùn)動(dòng)算子為

M=TxTyRz

(10)

可形式化如下：

|-?PxPyt.M_CGAPxPyt=Translation_CGA_TxPx*

Translation_CGA_TyPy*Rotation_CGA_Rzt

剛體運(yùn)動(dòng)算子就是旋轉(zhuǎn)和平移的幾何積.本文建立的運(yùn)動(dòng)變換等效模型包括兩次平移和一次旋轉(zhuǎn)，即模型的剛體運(yùn)動(dòng)算子表示為它們?nèi)叩膸缀畏e.

定理3.剛體運(yùn)動(dòng)算子方程

剛體運(yùn)動(dòng)算子方程表示如下：

(11)

可形式化如下：

|-?tPxPy.Translation_CGA_TxPx*

Translation_CGA_TyPy*Rotation_CGA_Rxt=

cos(t/& 2)%mbasis{}-

(mbasis{1}*mbasis{2})*sin(t/& 2)%mbasis{}-

(& 1/& 2*Px*cos(t/& 2)+& 1/& 2*Py*

sin(t/& 2))%(mbasis{1}*null_inf)-

(& 1/& 2*Py*cos(t/& 2)-& 1/& 2*

Px*sin(t/& 2))%(mbasis{2}*null_inf)

該式為剛體運(yùn)動(dòng)算子方程.通過(guò)橫縱向平移、旋轉(zhuǎn)公式的定義和幾何積等于外積的性質(zhì)，結(jié)合策略SIMP_TAC化簡(jiǎn)目標(biāo)，繼而通過(guò)幾何積的自動(dòng)證明策略CONV_TACGEOM_ARITH自動(dòng)解算各種積運(yùn)算的混合表達(dá)式實(shí)現(xiàn)剛體運(yùn)動(dòng)算子方程的證明.

定理4.剛體運(yùn)動(dòng)方程的共軛

剛體運(yùn)動(dòng)算子方程的共軛表示如下：

(12)

可形式化如下：

|-Reversion(Translation_CGA_TxPx*

Translation_CGA_TyPy*Rotation_CGA_Rxt)=

cos(t/& 2)%mbasis{}+(mbasis{1}*mbasis{2})*

sin(t/& 2)%mbasis{}+(& 1/& 2*Px*cos(t/& 2)+

& 1/& 2*Py*sin(t/& 2))%(mbasis{1}*null_inf)+

(& 1/& 2*Py*cos(t/& 2)

該式剛體運(yùn)動(dòng)算子方程的共軛.該定理的證明步驟與定理1、定理2的證明步驟類似.

3.2 建立點(diǎn)運(yùn)動(dòng)變換后的表達(dá)式方程

首先，先定義平面上的點(diǎn)坐標(biāo)，即P1、P2、P3在靜坐標(biāo)下的坐標(biāo)，P4、P5、P6在動(dòng)坐標(biāo)下的坐標(biāo)；接著，定義點(diǎn)變換的表達(dá)式方程，這里會(huì)涉及剛體運(yùn)動(dòng)算子方程、剛體運(yùn)動(dòng)算子方程的共軛.通過(guò)點(diǎn)變換的表達(dá)式方程，可以得到P4、P5、P6在靜坐標(biāo)下的坐標(biāo).

定義9.平面點(diǎn)坐標(biāo)方程

在CGA中，點(diǎn)可以寫(xiě)成

S=s+s2e∞/2+e0

(13)

其中，s=s1e1+s2e2+s3e3，s1、s2、s3為三維歐式空間點(diǎn)的坐標(biāo).

可形式化如下：

|-P.point_planeP=P$1%mbasis{1}+P$2%mbasis{2}+(& 1/& 2*(PdotP)%null_inf+null_zero)

該式為平面點(diǎn)的表達(dá)式方程.S＄1表示x軸上的系數(shù)，即s1；S＄2表示y軸上的系數(shù)，即s2.在平面中，點(diǎn)坐標(biāo)是二維的，只包括x軸和y軸，所以平面中的點(diǎn)只包括mbasis{1}和mbasis{2}.其中，dot是HOLLight中定義的高階邏輯函數(shù)，表示向量的點(diǎn)積.

pow是HOLLight中定義的高階邏輯函數(shù)，輸入變量是兩個(gè)實(shí)數(shù)x、y，輸出結(jié)果是xy，P1、P2、P3在靜坐標(biāo)中的表示如表2所示，P4、P5、P6在動(dòng)坐標(biāo)中的表示如表3所示.

表2 P1、P2、P3在靜坐標(biāo)中的表達(dá)式

定義10.動(dòng)坐標(biāo)的點(diǎn)P4、P5、P6在靜坐標(biāo)xoy的坐標(biāo)

可形式化如下：

|-?pM_CGA.point_transpM_CGA=M_CGA*p*

(reversionM_CGA)

該式是把動(dòng)坐標(biāo)的點(diǎn)轉(zhuǎn)化為靜坐標(biāo)下.運(yùn)用3.1中的剛體運(yùn)動(dòng)算子方程M_CGA可以得到動(dòng)坐標(biāo)的點(diǎn)P4、P5、P6在靜坐標(biāo)下所對(duì)應(yīng)的坐標(biāo).

表3 P4、P5、P6在動(dòng)坐標(biāo)中的表達(dá)式

3.3 建立桿長(zhǎng)約束方程

上面已經(jīng)得到了每個(gè)點(diǎn)在靜坐標(biāo)下所對(duì)應(yīng)的坐標(biāo).接下來(lái)需要根據(jù)各支鏈的桿長(zhǎng)約束方程，建立點(diǎn)與點(diǎn)的約束方程，把點(diǎn)與點(diǎn)建立關(guān)系.

定義11.桿長(zhǎng)約束方程

內(nèi)積與距離的表達(dá)式方程：

P·S=-(s-p)2/2

(14)

可形式化如下：

|-get_square_qab=-& 2%(ainnerb)

根據(jù)2個(gè)點(diǎn)P、S的內(nèi)積與距離的關(guān)系，建立桿長(zhǎng)約束方程，該方程表示2個(gè)點(diǎn)P、S的內(nèi)積與距離的關(guān)系.這里的get_square_qab指的是a與b兩點(diǎn)實(shí)際距離的平方.

定義12.P1、P4約束方程

可形式化如下：

|-?PxPy.-& 2%

(point_plane_P1innerpoint_plane_P4PxPy)=

get_square_qpoint_plane_P1(point_plane_P4PxPy)

定義13.P2、P5約束方程

可形式化如下：

|-?tPxPyP2xP5x.-&2%(point_plane_P2P2x)inner

(point_trans(point_plane_P5P5x)(M_CGAPxPyt))=

get_square_q(point_plane_P2P2x)(point_trans

(point_plane_P5P5x)(M_CGAPxPyt))

定義14.P3、P6約束方程

可形式化如下：

|-?tPxPyP3xP6xP3yP6y.-& 2%

(point_plane_P3P3xP3yinner(point_trans

(point_plane_P6(P6x：real)(P6y：real))

(M_CGA(Px：real)(Py：real)(t：real))))=

get_square_q(point_plane_P3P3xP3y)

(point_trans(point_plane_P6P6xP6y)

(M_CGAPxPyt))

3.4 建立位置正解封閉方程組

結(jié)合點(diǎn)運(yùn)動(dòng)變換后的表達(dá)式方程和桿長(zhǎng)約束方程，建立位置正解封閉方程組.

(Px)2+(Py)2=(q1)2

(15)

(Px)2+(Py)2+(P2x)2+(P5x)2-2(P2xP5x-PxP5x)cosθ+2P5xPysinθ-2PxP2x=(q2)2

(16)

(Px)2+(Py)2+(P3x)2+(P3y)2+(P6x)2+(P6y)2+

2(PyP6y+PxP6x-P3xP6x-P3yP6y)cosθ+

2(PyP6x-P3yP6x-PxP6y+P3xP6y)sinθ-

2PyP3y-2PxP3x=(q3)2

(17)

定理5.P1、P4封閉方程

可形式化如下：

|-?PxPy.Pxpow2%mbasis{}+Pypow2%mbasis{}=

get_square_qpoint_plane_P1(point_plane_P4PxPy)

定理6.P2、P5封閉方程

可形式化如下：

|-?tPxPyP2xP5x.Pxpow2%mbasis{}+Pypow2%mbasis{}+P2xpow2%mbasis{}+P5xpow2%mbasis{}-

& 2%(P2x*P5x-Px*P5x)%cost%mbasis{}+

& 2%P5x%Py%sint%mbasis{}-

& 2%(Px*P2x)%mbasis{}=

get_square_q(point_plane_P2P2x)

(point_trans(point_plane_P5P5x)(M_CGAPxPyt))

定理7.P3、P6封閉方程

可形式化如下：

|-?tPxPyP3P6P3xP6xP3yP6y.Pxpow2%mbasis{}+

Pypow2%mbasis{}+P3xpow2%mbasis{}+

P3ypow2%mbasis{}+P6xpow2%mbasis{}+

P6ypow2 %mbasis{}+& 2%(Py*P6y+

Px*P6x-P3x*P6x-P3y*P6y)%cost%mbasis{}+

& 2% (Py*P6x-P3y*P6x-Px*P6y+P3x*P6y)%

sint%mbasis{}-& 2%(Py*P3y)%mbasis{}-

& 2%(Px*P3x)%mbasis{}=

get_square_q(point_plane_P3P3xP3y)

(point_trans(point_plane_P6P6xP6y)(M_CGAPxPyt))

定理5-定理7是平面并聯(lián)機(jī)構(gòu)等效模型的位置正解封閉方程組，其中t，Px，Py為待求量，其余變量都為已知機(jī)構(gòu)參數(shù).利用約束方程替換封閉方程，繼而使用策略將點(diǎn)的表達(dá)式方程重寫(xiě)，通過(guò)化簡(jiǎn)得到位置正解封閉方程組，實(shí)現(xiàn)封閉方程組到二元一次方程組的變換，進(jìn)而推導(dǎo)出一元六次方程.

3.5 封閉方程組的消簡(jiǎn)過(guò)程的形式化驗(yàn)證

驗(yàn)證平面并聯(lián)機(jī)構(gòu)位置正解方程與一元六次方程的等價(jià)性證明.結(jié)合P1、P4封閉方程、P2、P5封閉方程、P3、P6封閉方程得到Px、Py的二元一次方程組.

APx+BPy+D=0EPx+CPy+G=0

(18)

其中，

A=2P5xcosθ-2P2x

B=2P5xsinθ

C=2P6ycosθ+2P6xsinθ-2P3y

D=(q1)2-(q2)2+(P2x)2+(P5x)2-2P2xP5xcosθ

E=2P6xcosθ-2P6ysinθ-2P3x

G=(q1)2-(q3)2+(P3x)2+(P3y)2+(P6x)2+(P6y)2-2(P3xP6x+P3yP6y) cosθ+2(P3xP6y-P3yP6x) sinθ

為了簡(jiǎn)化后面代碼的形式化描述，A～G的形式化表示如下：

A=& 2*P5x*cost-& 2*P2x∧

B=& 2*P5x*sint∧

C=& 2*P6y*cost+& 2*P6x*sint-& 2*P3y∧

D=get_square_qpoint_plane_P1(point_plane_P4Px

Py)-get_square_q(point_plane_P2P2x)

(point_trans(point_plane_P5P5x)(M_CGAPxPyt))+

P2xpow2%mbasis{}+P5xpow2%mbasis{}-

(& 2*P2x*P5x*cost)%mbasis{}∧

E=& 2*P6x*cost-& 2*P6y*sint-& 2*P3x∧

G=get_square_qpoint_plane_P1

(point_plane_P4PxPy)-get_square_q

(point_plane_P3P3xP3y)

(point_trans(point_plane_P6P6xP6y)(M_CGAPxPyt))+(P3xpow2+P3ypow2+P6xpow2+P6ypow2-& 2*(P3x*P6x+P3y*P6y)*cost+& 2*

(P3x*P6y-P3y*P6x)*sint)%mbasis{}

定理8.關(guān)于Px、Py方程組

可形式化如下：

|-?PxPyABD.

(A*Px)%mbasis{}+(B*Py)%mbasis{}+D=vec0

|-?PxPyCEG.

(E*Px)%mbasis{}+(C*Py)%mbasis{}+G=vec0

其中，vec0表示的是零向量.上式是Px、Py的二元一次方程組.結(jié)合封閉方程的定義square_q1、square_q2、square_q3，得到Px、Py的二元一次方程組.

Px、Py關(guān)于t的表達(dá)式如下：

(19)

定理9.Px關(guān)于t的表達(dá)式

可形式化如下：

|-?ABCDEG.

((A*C-B*E)*Px)%mbasis{}=B%G-C%D

該式為Px關(guān)于t的表達(dá)式方程.對(duì)二元一次方程組進(jìn)行消元，消去Py，得到定理9.利用移項(xiàng)化簡(jiǎn)實(shí)現(xiàn)邏輯推理.

定理10.Py關(guān)于t的表達(dá)式

可形式化如下：

|-?ABCDEG.

((A*C-B*E)*Py)%mbasis{}=E%D-A%G

同理，消去Px，得到Py關(guān)于t的表達(dá)式.該定理的證明步驟與定理9的證明步驟類似.當(dāng)A*C-B*E等于零時(shí)，表示該機(jī)構(gòu)為退化構(gòu)型，而本文討論的是非退化構(gòu)型，也就是本文只討論A*C-B*E不為零的情況.

定理11.一元三次方程

其數(shù)學(xué)描述如下：

r1(cosθ)3+r2sinθ(cosθ)2+r3(cosθ)2+r4(sinθcosθ)+r5cosθ+r6sinθ+r7=0

(20)

可形式化如下：

|-?r1r2r3r4r5r6r7ABDECG.

～(A*C-B*E=& 0)?r1=E1∧r2=E2∧

r3=E3∧r4=E4∧r5=E5∧r6=E6∧r7=E7

?(r1*costpow3+r2*sint*costpow2)%mbasis{}+

costpow2%r3+(sint*cost)%r4+cost%r5+

sint%r6+r7=vec0

r1～r7是與機(jī)構(gòu)參數(shù)相關(guān)的常數(shù)，它是通過(guò)Maple16編程軟件實(shí)現(xiàn)的.系數(shù)特別復(fù)雜因此很難使用傳統(tǒng)的方法進(jìn)行驗(yàn)證.該式是關(guān)于θ的方程，包含sinθ和cosθ.結(jié)合策略把Px關(guān)于t的方程、Py關(guān)于t的方程重寫(xiě)到P1、P4封閉方程square_q1中.在運(yùn)算過(guò)程中，等號(hào)右邊出現(xiàn)了cos4θ和sinθcos3θ，而等式左邊不包含這兩項(xiàng)，因此需要驗(yàn)證cos4θ和sinθcos3θ前面的系數(shù)為0，進(jìn)而消去這兩項(xiàng).

引理1.a與其倒數(shù)相乘等于1(a不等于0)

可形式化如下：

|-?t.inv(& 1+tan(t*inv(& 2))*tan(t*inv(& 2)))

*(& 1+tan(t*inv(& 2))*tan(t*inv(& 2)))=& 1

其中，inv是HOL Light中定義的高階邏輯函數(shù)，函數(shù)的輸入變量是實(shí)數(shù)，輸出結(jié)果是該實(shí)數(shù)的倒數(shù)，且定義0的倒數(shù)仍然為0.當(dāng)a不等于0時(shí)，它與其倒數(shù)相乘等于1.這里a=&1+tan(t*inv(& 2))*tan(t*inv(& 2))，因?yàn)閍不等于零，所以它與其倒數(shù)相乘才等于1.為了證明定理12，需要結(jié)合該引理輔助證明.

定理12.一元六次方程

其數(shù)學(xué)描述如下：

w6t6+w5t5+w4t4+w3t3+w2t2+w1t+w0=0

(21)

式中，w0=r1+r3+r5+r7w1=2(r2+r4+r6)

w2=-3r1-r3+r5+3r7w3=4(-r2+r6)

w4=3r1-r3-r5+3r7w5=2(r2-r4+r6)

w6=-r1+r3-r5+r7

可形式化如下：

|-?tr1r2r3r4r5r6r7E1E2E3E4E5E6E7

w1w2w3w4w5w6w0.

r1=E1∧r2=E2∧r3=E3∧r4=E4∧

r5=E5∧r6=E6∧r7=E7

?

w1=& 2%(r2%mbasis{}+r4+r6)∧

w2=(-& 3*r1)%mbasis{}-r3+r5+& 3%r7∧

w3=& 4%(-r2%mbasis{}+r6)∧

w4=(& 3*r1)%mbasis{}-r3-r5+& 3%r7∧

w5=& 2 %(r2%mbasis{}-r4+r6)∧

w6=-r1%mbasis{}+r3-r5+r7∧

w0=r1%mbasis{}+r3+r5+r7∧

～(cos(t/& 2)=& 0)

?

tan(t/& 2)pow6%w6+tan(t/& 2)pow5%w5+

tan(t/& 2)pow4%w4+tan(t/& 2)pow3%w3+

tan(t/& 2)pow2%w2+tan(t/& 2)%w1+

w0=vec0

4 總 結(jié)

平面并聯(lián)機(jī)構(gòu)是機(jī)構(gòu)運(yùn)動(dòng)學(xué)研究的一部分，它的分析、求解一直廣受學(xué)者們的關(guān)注.在平面并聯(lián)機(jī)器人的求解過(guò)程中，CGA方法具有幾何概念清楚、物理意義清晰、表達(dá)形式簡(jiǎn)潔的優(yōu)點(diǎn)，極大地降低了機(jī)器人運(yùn)動(dòng)學(xué)研究的難度.本文以幾何代數(shù)、CGA為數(shù)學(xué)基礎(chǔ)，在高階邏輯定理證明器HOL Light中，對(duì)平面并聯(lián)機(jī)構(gòu)建立模型并進(jìn)行形式化驗(yàn)證，從而確保了算法的安全性和可靠性.本文中的形式化工作，不僅能填補(bǔ)驗(yàn)證在并聯(lián)機(jī)構(gòu)中的空白，而且為我們以后的相關(guān)工作奠定了基礎(chǔ)，也為以后算法的驗(yàn)證積累了豐富的經(jīng)驗(yàn).此外，并聯(lián)機(jī)構(gòu)不僅包括平面并聯(lián)機(jī)構(gòu)，還包括空間并聯(lián)機(jī)構(gòu).因此下一步工作將圍繞基于共形幾何代數(shù)理論的空間并聯(lián)機(jī)構(gòu)的形式化建模與驗(yàn)證展開(kāi).