本發(fā)明涉及一種基于共形幾何代數(shù)的機械臂運動規(guī)劃的形式化分析方法及系統(tǒng)，屬于計算機科學技術領域。

背景技術：

共形幾何代數(shù)(CGA)作為一種先進的幾何表示和計算系統(tǒng)，它為經(jīng)典幾何提供了簡潔、直觀和統(tǒng)一的齊性代數(shù)框架。CGA通過引入兩個額外維度基準原點(e₀)和無窮遠點(e_∞)，使得歐氏空間嵌入到共形空間中，并賦予其閔氏內(nèi)積結構，不僅保留了齊次空間中外積的Grassmann結構、更使得內(nèi)積具備了表征距離、角度等基本度量的明確幾何意義。CGA不僅成功的解決了運用幾何語言完成幾何計算，基于CGA的新方法和新算法層出不窮,還在許多高科技領域中如工程學和計算機科學的幾何問題的解決起了關鍵性的作用。在機器人學中，CGA與以往的代數(shù)不同，它的運算對象不是數(shù)字，而是幾何體。而機器人研究對象是于基本幾何體建立的系統(tǒng)所形成的幾何關系。因此，共形幾何代數(shù)在機器人研究中具有獨特性。

目前利用CGA為數(shù)學工具進行建模和計算分析傳統(tǒng)上使用基于計算機的數(shù)值計算分析和計算機代數(shù)系統(tǒng)(CASs)如Maple、CLUCalc、Gaalop等，然而這兩種方法均不能完全保證結果的正確性和精確性，由于計算的迭代次數(shù)受限于計算機內(nèi)存和浮點數(shù)限制，數(shù)值方法并不能完全保證結果的精確性，而CASs提供的符號方法雖然利用核心算法可以精確推導出符號表達式的解在一定程度上避免了數(shù)值計算解不精確的問題，但是對龐大的符號集進行運算的算法并沒有經(jīng)過驗證，并且在邊界條件的處理上存在短板，所得到的結果仍然可能存在問題。為避免這些問題在系統(tǒng)早期設計時就產(chǎn)生，對CGA理論進行形式化分析是一種理想的解決辦法。近幾十年來，形式化方法在很多領域中都取得了巨大進步，基礎研究的進展加上技術進步的推動，使新方法和新工具不斷出現(xiàn)并逐步完善成為一種成熟的高可靠驗證技術。它的主要思想是根據(jù)數(shù)學理論來證明所設計的系統(tǒng)滿足系統(tǒng)的規(guī)范或具有所期望的性質。與人為筆紙分析和上述傳統(tǒng)方法相比，形式化方法可根據(jù)數(shù)學邏輯的嚴密性提高發(fā)現(xiàn)微小而關鍵的早期設計錯誤的機率。

技術實現(xiàn)要素：

有鑒于此，本發(fā)明的目的在于提供一種基于共形幾何代數(shù)的機械臂運動規(guī)劃的形式化分析方法及系統(tǒng)，主要是在高階邏輯證明工具HOL-Light中建立CGA系統(tǒng)的形式化模型，并基于建立的共形幾何邏輯模型對機器人的運動規(guī)劃進行形式化建模與驗證，以解決現(xiàn)有技術中計算復雜、結果不太精確的問題。

下面將本發(fā)明的技術方案詳述如下：

本發(fā)明一方面提供了一種基于共形幾何代數(shù)的機械臂運動規(guī)劃的形式化分析方法，其包括：

基于共形幾何代數(shù)理論對機器人的基本構件、運動規(guī)劃約束構建對應的幾何模型，再用高階邏輯語言描述所建立的幾何模型，形成機器人的基本幾何邏輯模型系統(tǒng)；

確定機器人的具體結構參數(shù)和運動規(guī)劃參數(shù)；

基于基本幾何邏輯模型系統(tǒng)對具體機器人運動過程的形式化建模，得到具體機器人運動過程的幾何關系邏輯模型；

用邏輯公式描述需要驗證的機器人運動過程的約束或屬性；

將具體機器人運動過程的所述幾何關系邏輯模型和待驗證的運動過程的約束或屬性組成一個邏輯命題；

利用邏輯推理引擎證明所述邏輯命題是否成立，成立表明所述模型滿足所述的約束或者具備所述的屬性，不成立表明所述模型不滿足所述的約束或者不具備所述的屬性。

其中，所構建的幾何模型包括：機器人關節(jié)抽象化成點模型、機器人關節(jié)末端可達范圍抽象化為球模型、機器人關節(jié)所在輔助面抽象為面模型、機器人關節(jié)點連接構成的線模型、將機器人基本構件的約束關系抽象化為幾何體求交模型、機器人基本構件旋轉運動抽象化為幾何體純旋轉模型、機器人基本構件平移運動抽象化為幾何體純平移模型、機器人關節(jié)末端到達期望位置抽象化為幾何體剛體運動模型、機器人基本構件度量關系抽象化為幾何體間距離模型、機器人基本構件夾角抽象化為幾何體夾角模型。

其中，所述機器人關節(jié)抽象化成點模型如下表示：

e_∞＝e_-+e₊

其中，小寫s＝s₁e₁+s₂e₂+s₃e₃表示歐氏三維空間中的點，e1-e3是歐式三維空間中的單位正交基，s1-s3為系數(shù)，大寫S為該點從歐氏空間映射到共形空間中的點的表達式；e₀表示原點，e_∞表示無窮遠點，e₊,e_-為共形幾何空間中的第四個基矢量和第五個基矢量。

其中，所述機器人關節(jié)末端可達范圍抽象化為球模型用于將機器人關節(jié)末端可達范圍抽象化為以關節(jié)為球心以兩關節(jié)間的連桿為半徑的球，如下表示：

其中，P、r分別表示球心和半徑，球心用歐氏空間中三維向量表示，e_∞表示無窮遠點。

其中，所述機器人關節(jié)所在輔助面抽象為面模型用于將機器人關節(jié)所在輔助面抽象為面，如下表示：

π＝n+de_∞

其中，n、d分別表示輔助面法向量和輔助面到原點的距離，法向量用歐氏空間中三維向量表示。

其中，所述機器人關節(jié)點連接構成的線模型用于將兩關節(jié)間的連桿抽象為線，如下表示：

L^*＝A∧B∧e_∞

其中，A和B分別表示兩關節(jié)所代表的點。

其中，所述將機器人基本構件的約束關系抽象化為幾何體求交模型如下表示：

o＝o₁∧o₂∧...∧o_n

其中，o_i表示代表第i個機器人基本構件的幾何體，i＝1……n

其中，所述機器人基本構件旋轉運動抽象化為幾何體純旋轉模型如下表示：

其中，R為共形幾何代數(shù)中的旋轉算子，為R的幾何反，L表示的旋轉軸,φ是旋轉角度，o表示旋轉前幾何體的表達形式，o_rotated表示旋轉后幾何體的表達形式。

其中，所述機器人基本構件平移運動抽象化為幾何體純平移模型如下表示：

其中，T為共形幾何代數(shù)中的平移算子，為T的幾何反，其中t＝t₁e₁+t₂e₂+t₃e₃是平移向量，表示平移的方向和長度,o表示平移前幾何體的表達形式，o_translated表示平移后幾何體的表達形式。

其中，所述機器人關節(jié)末端到達期望位置抽象化為幾何體剛體運動模型如下表示：

其中，R,T分別是旋轉算子和平移算子，為M的幾何反，o_{rigid_body_motion}表示經(jīng)過剛體運動后的幾何體表達形式。

其中，所述機器人基本構件度量關系抽象化為幾何體間距離模型如下表示：

其中，a、b表示三維歐氏空間中任意兩點，A、B為共形空間中該兩點的對應表達式，幾何體間距包括點線距離、點球距離、球球距離。

其中，所述機器人基本構件夾角抽象化為幾何體夾角模型用于計算機器人基本構件夾角，如下表示：

其中，o₁、o₂分別表示待求夾角的幾何體，

其中，所述機器人運動過程的形式化建模包括機器人抓取物體運動的形式化建模，具體包括：

步驟31：計算物體的被抓取位置所在的目標圓；

步驟32：計算機械爪所在圓；

步驟33：計算機械爪抓取物體的平移算子，所述平移算子包括平移軸和平移長度；

步驟34：計算機械爪抓取物體的旋轉算子，包括旋轉軸和旋轉角度；

步驟35：計算機械爪新的目標位置。

其中，步驟1中目標圓如下計算得到：

通過物體邊緣的四個特征點x1、x2、x3、x4得到物體的位置方位，其中x1,x2,x3為物體底部邊沿上任意三點，x4為物體頂部邊沿上任意一點，由物體底部三點構成的參考圓通過圓的下述直接表達式得到：

則物體底部參考圓所在的面如下得到：

其中，I_c為共形幾何代數(shù)的偽標量；e_∞表示無窮遠點；

所述目標圓Z_t為底部參考圓沿-π_b方向平移長度后的圓，則相應的平移算子為：

則物體被抓取的部位所形成的目標圓為：

其中，步驟32中所述機械抓所在圓如下計算：

已知機械爪所在圓的圓心P_h、半徑ρ、機械爪上兩點a,b可計算出機械爪所在圓的位置，首先構造機械爪所在球的位置，通過球的標準表達式得到：

其中，S_h為所述機械爪所在的球；

計算機械爪上兩點a,b計算機械爪所在面：

計算機械爪所在球S_h和所在面相交得到機械爪所在圓Z_h：

Z_h＝S_h∧π_h。

其中，所述步驟33中機械爪抓取物體的平移算子包括平移軸和平移長度，具體如下計算：

首先計算目標圓Z_t的圓心：

P_t＝Z_te_∞Z_t

通過直接表達式計算平移軸

其中，P_h為機械爪所在圓的圓心；

計算平移長度d：

其中，步驟34中機械爪抓取物體的旋轉算子包括旋轉軸和旋轉角度，具體如下計算：

計算目標圓Z_t和機械爪所在圓Z_h的兩軸線和

獲取兩軸線確定的面為：

計算旋轉軸

計算旋轉角度：

其中，步驟35中機械爪新的目標位置如下計算：

通過步驟3和步驟4計算得到的平移軸平移長度d、旋轉軸旋轉角度θ，得到機械爪抓取該物體運動的旋轉算子和平移算子：

其中，R和T分別為旋轉算子和平移算子，

機械爪新的目標位置如下計算：

其中，Z'_h為機械爪新的目標位置，Z_h為機械爪所在圓，為R的幾何反，為T的幾何反。

一種基于共形幾何代數(shù)的機械臂運動規(guī)劃的形式化分析系統(tǒng)，其包括：

機器人基本幾何邏輯模型建立模塊，其基于共形幾何代數(shù)理論對機器人的基本構件、運動規(guī)劃約束構建對應的幾何模型，再用高階邏輯語言描述所建立的幾何模型，形成機器人的基本幾何邏輯模型系統(tǒng)；

參數(shù)確定模塊，其確定機器人的具體結構參數(shù)和運動規(guī)劃參數(shù)；

機器人運動過程幾何關系邏輯模型建立模塊，其基于基本幾何邏輯模型系統(tǒng)對具體機器人運動過程的形式化建模，得到具體機器人運動過程的幾何關系邏輯模型；

機器人運動過程驗證模塊，其利用邏輯公式描述需要驗證的機器人運動過程的約束或屬性；

邏輯命題構成模塊，其將具體機器人運動過程的所述幾何關系邏輯模型和待驗證的運動過程的約束或屬性組成一個邏輯命題；

證明模塊，其利用邏輯推理引擎證明所述邏輯命題是否成立，成立表明所述模型滿足所述的約束或者具備所述的屬性，不成立表明所述模型不滿足所述的約束或者不具備所述的屬性。

本發(fā)明優(yōu)點及功效在于：與傳統(tǒng)方法不同，本發(fā)明中形式化驗證根據(jù)系統(tǒng)形式規(guī)范或屬性，使用數(shù)學方法證明系統(tǒng)的正確性，對所驗證的性質而言是精確和完備的。而共形幾何代數(shù)CGA由于可以對點、線、面、圓、球等幾何元素以及這些幾何元素的旋轉和平移進行統(tǒng)一建模和處理，在處理機器人運動學和運動規(guī)劃問題上具有很強的優(yōu)勢。在本發(fā)明中，通過應用共形幾何代數(shù)理論，以幾何對象為運算對象，提高解決問題的維數(shù)來簡化機器人學計算中的耦合，降低計算復雜程度。

【附圖說明】

圖1為本發(fā)明中基于CGA的機器人抓取物體的形式化建模流程圖。

圖2為本發(fā)明中機器人抓取物體算法的驗證流程圖。

圖3為本發(fā)明中目標圓Z_t的計算示意圖。

圖4為本發(fā)明中機械爪圓Z_h的計算示意圖。

圖5為本發(fā)明中旋轉算子和平移算子的求解示意圖。

【具體實施方式】

以下將參照附圖更詳細地描述本發(fā)明的各種實施例。在各個附圖中，相同的元件采用相同或類似的附圖標記來表示。為了清楚起見，附圖中的各個部分沒有按比例繪制。

目前發(fā)展的共形幾何代數(shù)系統(tǒng)，理論上可以推廣到空間的任意維數(shù)，主要應用是5維共形空間，由3維歐拉空間和2維閔氏空間構成，共形幾何代數(shù)提供了表示幾何體的Grassmann結構、表示幾何變換的統(tǒng)一旋量作用、表示幾何量的括號系統(tǒng)和不變量系統(tǒng)，在幾何數(shù)據(jù)處理和幾何計算方面表現(xiàn)出顯著優(yōu)勢。在幾何代數(shù)框架下，不同變換表達和運算形式具有統(tǒng)一性，經(jīng)幾何空間變換后，仍可保持原始幾何對象的幾何意義和幾何特性。機器人研究對象是于基本幾何體建立的系統(tǒng)所形成的幾何關系，而與以往的代數(shù)不同，共形幾何代數(shù)的運算對象不是數(shù)字，正是幾何體，所以共形幾何代數(shù)在機器人研究中具有獨特性。共形幾何代數(shù)還可將研究機器人的傳統(tǒng)方法如李群李代數(shù)、吳方法、Clifford代數(shù)、螺旋矢量進行統(tǒng)一構造，運用到機構的分析和綜合中，推動了機構學的發(fā)展，形成了具有普遍性的理論方法，降低了復雜的機構學建模和推導計算的復雜度，通過擴展維數(shù)將空間的各種幾何元素表達為球體坐標，在復雜多自由度并聯(lián)機構建模中可以得到比較簡單的數(shù)學模型。

CGA框架下的幾何體表達和構建可分別基于內(nèi)積和外積進行.基于外積的幾何形體表達主要反映不同層次幾何形體間相互構建關系，而內(nèi)積形式表達則可通過距離、角度等度量表征參數(shù)構建相應的參數(shù)方程.對于任意k階片積A，其基于外積和基于內(nèi)積構建的參數(shù)方程分別為X∧A＝0，X·A＝0.兩者的表達可以通過內(nèi)、外積間的對偶運算進行相互轉換.表1給出基于內(nèi)積和基于外積構建的幾何體表達，兩種表示方式被很多文獻分別稱作標準表達(standard representation)和直接表達(direct representation).

表1 CGA基本幾何體表示

Table 1:Representation of the Conformal Geometric Entities

表1中x、n標記為粗體，表示歐氏三維空間中的向量：

x＝x₁e₁+x₂e₂+x₃e₃

另外，表1中為歐氏空間中的標準內(nèi)積即點積(scalar product)，可以用多維向量庫中的點積函數(shù)描述.直接表示法中主要通過外積“∧”連接幾何體上的點{P_i}來構建出高維的幾何體，例如一個球可以用球上的四個點來表達.在標準表示法中外積的含義不同，表示幾何體間的相交，例如一個圓可以由兩個球相交而成。

圖1是本發(fā)明提出的基于共形幾何代數(shù)CGA方法對機器人抓取物體的形式化建模與驗證流程圖，基于共形幾何代數(shù)CGA方法對機器人操作的主要思想是CGA的幾何體運動變換表達，但是本發(fā)明的變換幾何對象是參考圓而不是參考點，機械爪和被抓取物體的幾何特征都用圓來表示。本發(fā)明中，使用劍橋大學研發(fā)的高階邏輯定理證明器HOL-Light作為形式化工具。HOL-Light是最流行的定理證明器之一，不僅擁有龐大的研究團隊和用戶群，而且包含豐富的數(shù)學定理庫如實數(shù)分析庫、超越函數(shù)庫、積分微分庫等和一系列高效的證明策略。

如圖1所示，本發(fā)明提出了一種基于共形幾何代數(shù)的機械臂運動規(guī)劃的形式化分析方法，其包括：

基于共形幾何代數(shù)理論對機器人的基本構件、運動規(guī)劃約束構建對應的幾何模型，再用高階邏輯語言描述所建立的幾何模型，形成機器人的基本幾何邏輯模型系統(tǒng)；

確定機器人的具體結構參數(shù)和運動規(guī)劃參數(shù)；

基于基本幾何邏輯模型系統(tǒng)對具體機器人運動過程的形式化建模，得到具體機器人運動過程的幾何關系邏輯模型；

用邏輯公式描述需要驗證的機器人運動過程的約束或屬性；

將具體機器人運動過程的所述幾何關系邏輯模型和待驗證的運動過程的約束或屬性組成一個邏輯命題；

利用邏輯推理引擎證明所述邏輯命題是否成立，成立表明所述模型滿足所述的約束或者具備所述的屬性，不成立表明所述模型不滿足所述的約束或者不具備所述的屬性。

共形幾何代數(shù)理論是機器人運動規(guī)劃的形式化分析方法的理論基礎，機器人運動規(guī)劃的形式化分析方法可以針對具體機器人問題分析的普遍方法，邏輯推理引擎實現(xiàn)對共形幾何代數(shù)形式化系統(tǒng)和機器人運動規(guī)劃的形式化分析方法的邏輯表達與證明的推理演算。

所述基于基本幾何邏輯模型系統(tǒng)對具體機器人運動規(guī)劃的運動規(guī)劃過程形式化建模具體包括對機器人抓取物體的運動規(guī)劃過程形式化建模。

優(yōu)選地，所述步驟二中的具體機器人為n自由度串聯(lián)機械臂，末端包括一個T字形機械爪

優(yōu)選地，所述步驟三中具體運動規(guī)劃為機器臂抓取物體的運動規(guī)劃過程

優(yōu)選地，所述步驟四中待驗證的運動規(guī)劃屬性為機械爪實現(xiàn)物體抓取成功并且抓牢。

所構建的幾何模型包括：機器人關節(jié)抽象化成點模型、機器人關節(jié)末端可達范圍抽象化為球模型、機器人關節(jié)所在輔助面抽象為面模型、機器人關節(jié)點連接構成的線模型、將機器人基本構件的約束關系抽象化為幾何體求交模型、機器人基本構件旋轉運動抽象化為幾何體純旋轉模型、機器人基本構件平移運動抽象化為幾何體純平移模型、機器人關節(jié)末端到達期望位置抽象化為幾何體剛體運動模型、機器人基本構件度量關系抽象化為幾何體間距離模型、機器人基本構件夾角抽象化為幾何體夾角模型。

下面對上述各個模型進行詳細介紹。其中，幾何體包括點、線、面、圓、球、點對等，用于表示機器人的基本構件。

機器人關節(jié)抽象化成點模型：

該模型中將機器人關節(jié)抽象化成點，上述數(shù)學表達式是共形幾何代數(shù)中點的標準表達式。

上述模型表達式中小寫s＝s₁e₁+s₂e₂+s₃e₃表示歐氏三維空間中的點，大寫S為該點從歐氏空間映射到共形空間中的點的表達式.函數(shù)point_CGA的輸入變量為歐氏空間中三維向量,其返回值為五維共形空間中的多重矢量，由于共形幾何代數(shù)可由幾何代數(shù)Cl_4,1構造而成，其數(shù)據(jù)類型定義為real^(4,1)multivector.其中s$n表示多維向量s的第n個元素，mbasis函數(shù)表示基本片積函數(shù),dot函數(shù)表示歐氏空間中的點積，null_inf和null_zero函數(shù)分別表示零矢量e₀和e_∞：

e_∞＝e_-+e₊， (2)

其中e₀表示原點，e_∞表示無窮遠點，利用e_∞,e₀取代e₊,e_-能夠更緊湊的表示共形空間中的點.本發(fā)明將基矢量e₊,e_-定義為mbasis{4}和mbasis{5}，表示CGA空間中第四個基矢量和第五個基矢量.形式化定義如下：

|-null_zero＝(&1/&2)％(mbasis{5}-mbasis{4})/\null_inf＝(mbasis{5})+(mbasis{4})

該模型中point_CGA函數(shù)功能表示從三維歐氏空間到五維共形空間中點的映射關系。

機器人關節(jié)末端可達范圍抽象化為球模型：

將機器人關節(jié)末端可達范圍抽象化成以關節(jié)為球心連桿為半徑的球，上述數(shù)學表達式是共形幾何代數(shù)中球的標準表達式。函數(shù)sphere_CGA的輸入變量p、r分別表示球心和半徑，其中球心用歐氏空間中三維向量表示。

機器人關節(jié)所在輔助面抽象為面模型：π＝n+de_∞

將機器人關節(jié)所在輔助面抽象化為面，上述數(shù)學表達式是共形幾何代數(shù)中面的標準表達式。函數(shù)plane_CGA的輸入變量n、d分別表示法向量和到原點的距離，其中法向量用歐氏空間中三維向量表示。

機器人關節(jié)點連接構成的線模型:L^*＝A∧B∧e_∞

上述數(shù)學表達式是共形幾何代數(shù)中任意兩點構成的線的表達式，可用于機器人中關節(jié)點構成的線或者其他輔助線的表示，其中A、B分別表示共形幾何空間中的兩個點。所述模型用于將兩關節(jié)間的連桿抽象為線。上述表達式為共形幾何代數(shù)中線的直接表達式，函數(shù)line_direct_CGA的輸入變量a、b分別表示線上的兩點，數(shù)據(jù)類型為real^(4,1)multivector，其中函數(shù)outer表示幾何代數(shù)中外積運算，外積運算可實現(xiàn)由低維幾何體向高維幾何體的構建，即擴維運算。

將機器人基本構件的約束關系抽象化為幾何體求交模型：o＝o₁∧o₂∧…∧o_n

|-MEET o₁o₂…on＝o₁outer o₂outer…o_n

其中，o_i表示代表第i個機器人基本構件的幾何體，i＝1……n；

所述模型利用外積實現(xiàn)代表機器人基本構件的幾何體的求交運算。函數(shù)MEET的輸入變量表示待求交的任意幾何體，數(shù)據(jù)類型為real^(4,1)multivector。約束關系包括關節(jié)在連桿上(即點在線上)、連桿在關節(jié)所在輔助面上(線在面上)等。

機器人基本構件旋轉運動抽象化為幾何體純旋轉模型:

|-rotation_CGA t l＝cos(t/&2)％mbasis{}-l*(sin(t/&2)％mbasis{}

|-pure_rotationed_CGA x t l＝(rotation_CGA t l)*x*(reversion(rotation_CGA t l))

所述利用幾何積實現(xiàn)幾何體旋轉變換，R為共形幾何代數(shù)中的旋轉算子，為R的幾何反，L表示的旋轉軸,φ是旋轉角度.o表示旋轉前幾何體的表達形式，orotated表示旋轉后幾何體的表達形式.函數(shù)rotation_CGA的輸入變量t、l分別表示旋轉副的旋轉角度與旋轉軸，該函數(shù)實現(xiàn)了旋轉算子的功能。函數(shù)pure_rotationed_CGA的輸入變量x、t、l表示幾何體、旋轉角度和旋轉軸，該函數(shù)的返回值為旋轉后的幾何體表示，其中函數(shù)reversion表示幾何反。

機器人基本構件平移運動抽象化為幾何體純平移模型：

|-Translation_CGA t＝mbasis{}-(&1/&2)％(t*null_inf)

|-pure_translationed_CGA x t＝(Translation_CGA t)*x*(reversion(Translation_CGA t))

所述模型利用幾何積實現(xiàn)幾何體平移變換，T為共形幾何代數(shù)中的平移算子，為T的幾何反，其中t＝t₁e₁+t₂e₂+t₃e₃是平移向量，表示平移的方向和長度.o表示平移前幾何體的表達形式，o_translated表示平移后幾何體的表達形式.函數(shù)Translation_CGA的輸入變量t是平移向量，表示平移的方向和長度，該函數(shù)實現(xiàn)了平移算子的功能。函數(shù)pure_translationed_CGA的輸入變量x、t表示幾何體、平移向量，該函數(shù)的返回值為平移后的幾何體表示。

所述機器人關節(jié)末端到達期望位置抽象化為幾何體剛體運動模型：

|-motor_CGA a l t＝rotation_CGA a l*Translation_CGA t

|-rigid_body_motion x a l t＝(motor_CGA a l t)*x*(reversion(motor_CGA a l t))

該模型利用幾何積實現(xiàn)幾何體剛體變換，其中R,T分別是旋轉算子和平移算子,兩者用幾何積運算連接，為M的幾何反，o_{rigid_body_motion}表示運動后的幾何體表達形式，函數(shù)motor_CGA的輸入變量x、a、l、t分別表示幾何體、旋轉角度、旋轉軸、平移向量，該函數(shù)實現(xiàn)了馬達算子的功能。函數(shù)rigid_body_motion的返回值為經(jīng)過剛體運動后的幾何體表示，該表達式表示幾何體是先平移后旋轉，但馬達算子具有結合性，旋轉算子和平移算子是可以交換的。

機器人基本構件度量關系抽象化為幾何體間距離模型：

其中a、b表示三維歐氏空間中任意兩點，A、B為共形空間中該兩點的對應表達式，上述模型為共形幾何代數(shù)中點點距離表達式，其中dist(a,b)表示兩點距離，可用幾何代數(shù)內(nèi)積表達.其中點線距離、點球距離、球球距離在共形幾何代數(shù)中也有具體的表達式，可用于機器人問題中具體計算時使用.

機器人基本構件夾角抽象化為幾何體夾角模型：

該模型為共形幾何代數(shù)中幾何體夾角表達式，函數(shù)vector_angles_CGA的輸入變量o₁o₂分別表示待求夾角的幾何體，可以是線或面等，數(shù)據(jù)類型為real^(4,1)multivector，其中函數(shù)inner表示幾何代數(shù)中左縮積運算，$${}表示取0階子空間即標量的大小，函數(shù)mult_norm功能為多重矢量的求模運算。該模型可用于機器人基本構件之間夾角的計算。

優(yōu)選地，所述方法中具體機器人為n自由度串聯(lián)機械臂，末端包括一個T字形機械爪；

所述方法中具體運動規(guī)劃為機器臂抓取物體的運動規(guī)劃過程；

所述方法中待驗證的運動規(guī)劃屬性為機械爪實現(xiàn)物體抓取成功并且抓牢。

圖2示出了本發(fā)明中對機器人抓取物體的運動規(guī)劃過程形式化建模的流程圖。如圖2所示，其包括：

步驟30：分別提取物體和機械爪的特征點；

步驟31：計算物體的被抓取位置所在的目標圓；

如圖3和圖4所示，通過物體邊緣的四個特征點(x1,x2,x3,x4)得到物體的位置方位，其中x1,x2,x3為物體底部邊沿上任意三點，x4為物體頂部邊沿上任意一點。由物體底部三點構成的參考圓可通過圓的直接表達式得到：

則物體底部參考圓所在的面為：

其中,Ic為共形幾何代數(shù)的偽標量，通過偽標量是可實現(xiàn)對偶運算，對偶運算實現(xiàn)幾何體兩種表示方法的相互轉換。

由于方便設定物體被抓取的位置是中部，所以目標圓Z_t應該是底部參考圓沿-π_b方向平移長度后的圓，則相應的平移算子為：

則物體被抓取的部位所形成的目標圓為：

其中，上述計算過程在HOL-Light中進行形式化：

let dual_circle_zb x1 x2 x3＝(circle_direct_CGA(point_CGA x1)(point_CGA x2)(point_CGA x3))

let plane_b x1 x2 x3＝((dual_circle_zb x1 x2 x3)outer null_inf)*pseudo

let circle_zt x1 x2 x3 x4＝pure_translationed_CGA(--DUAL(dual_circle_zb x1 x2 x3))(--(&1/&2)％((plane_b x1 x2 x3)inner(point_CGA x4)))

其中，物體底部參考圓調用圓的直接表示函數(shù)circle_direct_CGA,x1 x2 x3 x4表示物體的四個特征點，HOL類型均是real^3，表示三維歐氏向量，通過函數(shù)point_CGA使特征點由三維歐氏空間嵌入到五維共形空間中。物體被抓取的部位所形成的目標圓通過純平移函數(shù)pure_translationed_CGA實現(xiàn)。

步驟32：計算機械爪所在圓；

如圖5所示，已知機械爪所在圓的圓心ph、半徑ρ、機械爪上兩點a,b可計算出機械爪所在圓的位置，首先構造機械爪所在球的位置，通過球的標準表達式得到：

然后通過圓心ph、機械爪上兩點a,b計算機械爪所在面：

機械爪所在球Sh和所在面πh相交得到機械爪所在圓：

Z_h＝S_h∧π_h

上述計算過程在HOL-Light中進行如下形式化：

let sphere_Sh ph r＝sphere_CGA ph r

let dual_plane_pih ph a b＝plane_direct_CGA(point_CGA ph)(point_CGA a)(point_CGA b)

let circle_zh ph r a b＝(sphere_Sh ph r)outer(--DUAL(dual_plane_pih ph a b))

其中，輔助球S_h調用球的標準表示函數(shù)sphere_CGA實現(xiàn),(ph:real^3)和(r:real)分別表示輔助球的圓心Ph和半徑r,機械爪所在的輔助面調用面的直接表示函數(shù)plane_direct_CGA實現(xiàn)，(a:real^3)和(b:real^3)分別表示機械爪上兩點。對偶算子用DUAL函數(shù)。

步驟33：計算機械爪抓取物體的平移算子，所述平移算子包括平移軸和平移長度；

如圖5所示，易知平移長度應該為目標圓Zh和機械爪所在圓Zt的圓心距離，而平移軸是過兩圓心的直線。首先計算目標圓Zh的圓心：

P_t＝Z_te_∞Z_t

由于平移軸由兩圓心確定，可通過直接表達式計算平移軸：

相應地，平移長度為

上述計算過程在HOL-Light中進行形式化：

let center_point_pt x1 x2 x3 x4＝(circle_zt x1 x2 x3 x4)*null_inf*(circle_zt x1 x2 x3 x4)

let dual_translation_axis ph x1 x2 x3 x4＝line_direct_CGA(point_CGA ph)(center_point_pt x1 x2 x3 x4)

let distance ph x1 x2 x3 x4＝--sqrt(&2*((point_CGA ph)inner(center_point_pt x1 x2 x3 x4))$${})

步驟34：計算機械爪抓取物體的旋轉算子，包括旋轉軸和旋轉角度；

如圖5，機械爪抓取物體的旋轉軸滿足幾點約束：

1.過機械爪所在圓的圓心ph

2.在由兩圓的軸線確定的面上

所以，先計算目標圓Z_t和機械爪所在圓Z_h的兩軸線：

由兩軸線確定的面為：

則旋轉軸計算為：

而旋轉角度易知為兩軸線的夾角，由夾角公式得：

上述計算過程在HOL-Light中進行形式化：

let dual_lh ph r a b＝(circle_zh ph r a b)outer null_inf

let dual_lt x1 x2 x3 x4＝(circle_zt x1 x2 x3 x4)outer null_inf

let dual_plane_th x1 x2 x3 x4 ph r a b＝(dual_lt x1 x2 x3 x4)outer((dual_lh ph r a b)*(null_zero outer null_inf))

let dual_rotation_axis x1 x2 x3 x4 ph r a b＝(point_CGA ph)outer(--DUAL(dual_plane_th x1 x2 x3 x4ph r a b))outer null_inf

let rotation_angle x1 x2 x3 x4 ph r a b＝vector_angles_CGA(dual_lh ph r a b)(dual_lt x1 x2 x3 x4)

其中，旋轉角度θ的求解則使用CGA夾角公式來計算，由函數(shù)vector_angles_CGA實現(xiàn).

步驟35：計算機械爪新的目標位置；

通過步驟33和步驟34計算得到的平移軸平移長度d、旋轉軸旋轉角度θ，可以通過機器人幾何邏輯模型中定義6和定義7得到機械爪抓取該物體運動的旋轉算子和平移算子：

其中，R和T分別為旋轉算子和平移算子，

機械爪新的目標位置可以先旋轉后平移計算得到，利用定義8中提供的剛體算子可計算為：

最終可以得到機械爪新的目標位置，在HOL-Light中進行形式化：

let circle_zh_new x1 x2 x3 x4 ph r a b＝pure_translationed_CGA

(pure_rotationed_CGA(circle_zh ph r a b)(rotation_angle x1 x2 x3 x4 ph r a b)(--DUAL(dual_rotation_axis x1 x2 x3 x4 ph r a b)))

((distance ph r a b x1 x2 x3 x4)％(--DUAL(dual_translation_axis ph r a b x1 x2 x3 x4)))

其中，由于機械爪新的目標位置是通過先旋轉后平移得到，所以先將機械爪面所構成的圓z_h通過純旋轉函數(shù)pure_rotationed_CGA進行旋轉運動，該函數(shù)的輸入變量是被旋轉的幾何體z_h、旋轉角度θ和旋轉軸，即(circle_zh ph r a b)、(rotation_angle x1 x2 x3 x4 ph r a b)和(--DUAL(dual_rotation_axis x1 x2 x3 x4 ph r a b))，再將旋轉后的圓通過純平移函數(shù)pure_translationed_CGA完成平移運動，該函數(shù)輸入變量是旋轉的幾何體平移向量dl_T。

上述代碼主要內(nèi)容涉及共形幾何代數(shù)CGA幾何體表示、幾何體間距離特征和幾何體運動變換的形式化。機械爪圓Z_h一步步靠近物體位置所在目標圓Z_t，最終機械爪成功抓取到物體并且抓牢必須滿足一定的幾何約束關系。上述算法中，該約束關系為機械爪面所在圓Z_h新的位置最終應該與物體所在目標圓Z_t的位置吻合，即在共形幾何代數(shù)CGA中表達式應該滿足相等的關系?？梢栽贖OL-light中建立目標驗證這層幾何約束關系：

goal:

本發(fā)明中關于基于CGA對機器人的運動規(guī)劃進行形式化建模與驗證的方法，在HOL-Light中可以加載使用。其中最大難點是定理證明技術需要大量的人機交互，工作量較大，時間耗費較長，建模過程中強調使用者對共形幾何代數(shù)CGA理論知識的熟識度，驗證過程中要求嚴密的思維和一定的邏輯推理經(jīng)驗。

以上所述的具體實施例，對本發(fā)明的目的、技術方案和有益效果進行了進一步詳細說明，應理解的是，以上所述僅為本發(fā)明的具體實施例而已，并不用于限制本發(fā)明，凡在本發(fā)明的精神和原則之內(nèi)，所做的任何修改、等同替換、改進等，均應包含在本發(fā)明的保護范圍之內(nèi)。