一種基於共形幾何代數的機械臂運動規劃的形式化分析方法及系統與流程
2023-06-27 06:06:16 1
本發明涉及一種基於共形幾何代數的機械臂運動規劃的形式化分析方法及系統，屬於計算機科學技術領域。
背景技術：
共形幾何代數(CGA)作為一種先進的幾何表示和計算系統，它為經典幾何提供了簡潔、直觀和統一的齊性代數框架。CGA通過引入兩個額外維度基準原點(e0)和無窮遠點(e∞)，使得歐氏空間嵌入到共形空間中，並賦予其閔氏內積結構，不僅保留了齊次空間中外積的Grassmann結構、更使得內積具備了表徵距離、角度等基本度量的明確幾何意義。CGA不僅成功的解決了運用幾何語言完成幾何計算，基於CGA的新方法和新算法層出不窮,還在許多高科技領域中如工程學和計算機科學的幾何問題的解決起了關鍵性的作用。在機器人學中，CGA與以往的代數不同，它的運算對象不是數字，而是幾何體。而機器人研究對象是於基本幾何體建立的系統所形成的幾何關係。因此，共形幾何代數在機器人研究中具有獨特性。
目前利用CGA為數學工具進行建模和計算分析傳統上使用基於計算機的數值計算分析和計算機代數系統(CASs)如Maple、CLUCalc、Gaalop等，然而這兩種方法均不能完全保證結果的正確性和精確性，由於計算的迭代次數受限於計算機內存和浮點數限制，數值方法並不能完全保證結果的精確性，而CASs提供的符號方法雖然利用核心算法可以精確推導出符號表達式的解在一定程度上避免了數值計算解不精確的問題，但是對龐大的符號集進行運算的算法並沒有經過驗證，並且在邊界條件的處理上存在短板，所得到的結果仍然可能存在問題。為避免這些問題在系統早期設計時就產生，對CGA理論進行形式化分析是一種理想的解決辦法。近幾十年來，形式化方法在很多領域中都取得了巨大進步，基礎研究的進展加上技術進步的推動，使新方法和新工具不斷出現並逐步完善成為一種成熟的高可靠驗證技術。它的主要思想是根據數學理論來證明所設計的系統滿足系統的規範或具有所期望的性質。與人為筆紙分析和上述傳統方法相比，形式化方法可根據數學邏輯的嚴密性提高發現微小而關鍵的早期設計錯誤的機率。
技術實現要素：
有鑑於此，本發明的目的在於提供一種基於共形幾何代數的機械臂運動規劃的形式化分析方法及系統，主要是在高階邏輯證明工具HOL-Light中建立CGA系統的形式化模型，並基於建立的共形幾何邏輯模型對機器人的運動規划進行形式化建模與驗證，以解決現有技術中計算複雜、結果不太精確的問題。
下面將本發明的技術方案詳述如下：
本發明一方面提供了一種基於共形幾何代數的機械臂運動規劃的形式化分析方法，其包括：
基於共形幾何代數理論對機器人的基本構件、運動規劃約束構建對應的幾何模型，再用高階邏輯語言描述所建立的幾何模型，形成機器人的基本幾何邏輯模型系統；
確定機器人的具體結構參數和運動規劃參數；
基於基本幾何邏輯模型系統對具體機器人運動過程的形式化建模，得到具體機器人運動過程的幾何關係邏輯模型；
用邏輯公式描述需要驗證的機器人運動過程的約束或屬性；
將具體機器人運動過程的所述幾何關係邏輯模型和待驗證的運動過程的約束或屬性組成一個邏輯命題；
利用邏輯推理引擎證明所述邏輯命題是否成立，成立表明所述模型滿足所述的約束或者具備所述的屬性，不成立表明所述模型不滿足所述的約束或者不具備所述的屬性。
其中，所構建的幾何模型包括：機器人關節抽象化成點模型、機器人關節末端可達範圍抽象化為球模型、機器人關節所在輔助面抽象為面模型、機器人關節點連接構成的線模型、將機器人基本構件的約束關係抽象化為幾何體求交模型、機器人基本構件旋轉運動抽象化為幾何體純旋轉模型、機器人基本構件平移運動抽象化為幾何體純平移模型、機器人關節末端到達期望位置抽象化為幾何體剛體運動模型、機器人基本構件度量關係抽象化為幾何體間距離模型、機器人基本構件夾角抽象化為幾何體夾角模型。
其中，所述機器人關節抽象化成點模型如下表示：
e∞＝e-+e+
其中，小寫s＝s1e1+s2e2+s3e3表示歐氏三維空間中的點，e1-e3是歐式三維空間中的單位正交基，s1-s3為係數，大寫S為該點從歐氏空間映射到共形空間中的點的表達式；e0表示原點，e∞表示無窮遠點，e+,e-為共形幾何空間中的第四個基矢量和第五個基矢量。
其中，所述機器人關節末端可達範圍抽象化為球模型用於將機器人關節末端可達範圍抽象化為以關節為球心以兩關節間的連杆為半徑的球，如下表示：
其中，P、r分別表示球心和半徑，球心用歐氏空間中三維向量表示，e∞表示無窮遠點。
其中，所述機器人關節所在輔助面抽象為面模型用於將機器人關節所在輔助面抽象為面，如下表示：
π＝n+de∞
其中，n、d分別表示輔助面法向量和輔助面到原點的距離，法向量用歐氏空間中三維向量表示。
其中，所述機器人關節點連接構成的線模型用於將兩關節間的連杆抽象為線，如下表示：
L*＝A∧B∧e∞
其中，A和B分別表示兩關節所代表的點。
其中，所述將機器人基本構件的約束關係抽象化為幾何體求交模型如下表示：
o＝o1∧o2∧...∧on
其中，oi表示代表第i個機器人基本構件的幾何體，i＝1……n
其中，所述機器人基本構件旋轉運動抽象化為幾何體純旋轉模型如下表示：
其中，R為共形幾何代數中的旋轉算子，為R的幾何反，L表示的旋轉軸,φ是旋轉角度，o表示旋轉前幾何體的表達形式，orotated表示旋轉後幾何體的表達形式。
其中，所述機器人基本構件平移運動抽象化為幾何體純平移模型如下表示：
其中，T為共形幾何代數中的平移算子，為T的幾何反，其中t＝t1e1+t2e2+t3e3是平移向量，表示平移的方向和長度,o表示平移前幾何體的表達形式，otranslated表示平移後幾何體的表達形式。
其中，所述機器人關節末端到達期望位置抽象化為幾何體剛體運動模型如下表示：
其中，R,T分別是旋轉算子和平移算子，為M的幾何反，origid_body_motion表示經過剛體運動後的幾何體表達形式。
其中，所述機器人基本構件度量關係抽象化為幾何體間距離模型如下表示：
其中，a、b表示三維歐氏空間中任意兩點，A、B為共形空間中該兩點的對應表達式，幾何體間距包括點線距離、點球距離、球球距離。
其中，所述機器人基本構件夾角抽象化為幾何體夾角模型用於計算機器人基本構件夾角，如下表示：
其中，o1、o2分別表示待求夾角的幾何體，
其中，所述機器人運動過程的形式化建模包括機器人抓取物體運動的形式化建模，具體包括：
步驟31：計算物體的被抓取位置所在的目標圓；
步驟32：計算機械爪所在圓；
步驟33：計算機械爪抓取物體的平移算子，所述平移算子包括平移軸和平移長度；
步驟34：計算機械爪抓取物體的旋轉算子，包括旋轉軸和旋轉角度；
步驟35：計算機械爪新的目標位置。
其中，步驟1中目標圓如下計算得到：
通過物體邊緣的四個特徵點x1、x2、x3、x4得到物體的位置方位，其中x1,x2,x3為物體底部邊沿上任意三點，x4為物體頂部邊沿上任意一點，由物體底部三點構成的參考圓通過圓的下述直接表達式得到：
則物體底部參考圓所在的面如下得到：
其中，Ic為共形幾何代數的偽標量；e∞表示無窮遠點；
所述目標圓Zt為底部參考圓沿-πb方向平移長度後的圓，則相應的平移算子為：
則物體被抓取的部位所形成的目標圓為：
其中，步驟32中所述機械抓所在圓如下計算：
已知機械爪所在圓的圓心Ph、半徑ρ、機械爪上兩點a,b可計算出機械爪所在圓的位置，首先構造機械爪所在球的位置，通過球的標準表達式得到：
其中，Sh為所述機械爪所在的球；
計算機械爪上兩點a,b計算機械爪所在面：
計算機械爪所在球Sh和所在面相交得到機械爪所在圓Zh：
Zh＝Sh∧πh。
其中，所述步驟33中機械爪抓取物體的平移算子包括平移軸和平移長度，具體如下計算：
首先計算目標圓Zt的圓心：
Pt＝Zte∞Zt
通過直接表達式計算平移軸
其中，Ph為機械爪所在圓的圓心；
計算平移長度d：
其中，步驟34中機械爪抓取物體的旋轉算子包括旋轉軸和旋轉角度，具體如下計算：
計算目標圓Zt和機械爪所在圓Zh的兩軸線和
獲取兩軸線確定的面為：
計算旋轉軸
計算旋轉角度：
其中，步驟35中機械爪新的目標位置如下計算：
通過步驟3和步驟4計算得到的平移軸平移長度d、旋轉軸旋轉角度θ，得到機械爪抓取該物體運動的旋轉算子和平移算子：
其中，R和T分別為旋轉算子和平移算子，
機械爪新的目標位置如下計算：
其中，Z'h為機械爪新的目標位置，Zh為機械爪所在圓，為R的幾何反，為T的幾何反。
一種基於共形幾何代數的機械臂運動規劃的形式化分析系統，其包括：
機器人基本幾何邏輯模型建立模塊，其基於共形幾何代數理論對機器人的基本構件、運動規劃約束構建對應的幾何模型，再用高階邏輯語言描述所建立的幾何模型，形成機器人的基本幾何邏輯模型系統；
參數確定模塊，其確定機器人的具體結構參數和運動規劃參數；
機器人運動過程幾何關係邏輯模型建立模塊，其基於基本幾何邏輯模型系統對具體機器人運動過程的形式化建模，得到具體機器人運動過程的幾何關係邏輯模型；
機器人運動過程驗證模塊，其利用邏輯公式描述需要驗證的機器人運動過程的約束或屬性；
邏輯命題構成模塊，其將具體機器人運動過程的所述幾何關係邏輯模型和待驗證的運動過程的約束或屬性組成一個邏輯命題；
證明模塊，其利用邏輯推理引擎證明所述邏輯命題是否成立，成立表明所述模型滿足所述的約束或者具備所述的屬性，不成立表明所述模型不滿足所述的約束或者不具備所述的屬性。
本發明優點及功效在於：與傳統方法不同，本發明中形式化驗證根據系統形式規範或屬性，使用數學方法證明系統的正確性，對所驗證的性質而言是精確和完備的。而共形幾何代數CGA由於可以對點、線、面、圓、球等幾何元素以及這些幾何元素的旋轉和平移進行統一建模和處理，在處理機器人運動學和運動規劃問題上具有很強的優勢。在本發明中，通過應用共形幾何代數理論，以幾何對象為運算對象，提高解決問題的維數來簡化機器人學計算中的耦合，降低計算複雜程度。
【附圖說明】
圖1為本發明中基於CGA的機器人抓取物體的形式化建模流程圖。
圖2為本發明中機器人抓取物體算法的驗證流程圖。
圖3為本發明中目標圓Zt的計算示意圖。
圖4為本發明中機械爪圓Zh的計算示意圖。
圖5為本發明中旋轉算子和平移算子的求解示意圖。
【具體實施方式】
以下將參照附圖更詳細地描述本發明的各種實施例。在各個附圖中，相同的元件採用相同或類似的附圖標記來表示。為了清楚起見，附圖中的各個部分沒有按比例繪製。
目前發展的共形幾何代數系統，理論上可以推廣到空間的任意維數，主要應用是5維共形空間，由3維歐拉空間和2維閔氏空間構成，共形幾何代數提供了表示幾何體的Grassmann結構、表示幾何變換的統一旋量作用、表示幾何量的括號系統和不變量系統，在幾何數據處理和幾何計算方面表現出顯著優勢。在幾何代數框架下，不同變換表達和運算形式具有統一性，經幾何空間變換後，仍可保持原始幾何對象的幾何意義和幾何特性。機器人研究對象是於基本幾何體建立的系統所形成的幾何關係，而與以往的代數不同，共形幾何代數的運算對象不是數字，正是幾何體，所以共形幾何代數在機器人研究中具有獨特性。共形幾何代數還可將研究機器人的傳統方法如李群李代數、吳方法、Clifford代數、螺旋矢量進行統一構造，運用到機構的分析和綜合中，推動了機構學的發展，形成了具有普遍性的理論方法，降低了複雜的機構學建模和推導計算的複雜度，通過擴展維數將空間的各種幾何元素表達為球體坐標，在複雜多自由度並聯機構建模中可以得到比較簡單的數學模型。
CGA框架下的幾何體表達和構建可分別基於內積和外積進行.基於外積的幾何形體表達主要反映不同層次幾何形體間相互構建關係，而內積形式表達則可通過距離、角度等度量表徵參數構建相應的參數方程.對於任意k階片積A，其基於外積和基於內積構建的參數方程分別為X∧A＝0，X·A＝0.兩者的表達可以通過內、外積間的對偶運算進行相互轉換.表1給出基於內積和基於外積構建的幾何體表達，兩種表示方式被很多文獻分別稱作標準表達(standard representation)和直接表達(direct representation).
表1 CGA基本幾何體表示
Table 1:Representation of the Conformal Geometric Entities
表1中x、n標記為粗體，表示歐氏三維空間中的向量：
x＝x1e1+x2e2+x3e3
另外，表1中為歐氏空間中的標準內積即點積(scalar product)，可以用多維向量庫中的點積函數描述.直接表示法中主要通過外積「∧」連接幾何體上的點{Pi}來構建出高維的幾何體，例如一個球可以用球上的四個點來表達.在標準表示法中外積的含義不同，表示幾何體間的相交，例如一個圓可以由兩個球相交而成。
圖1是本發明提出的基於共形幾何代數CGA方法對機器人抓取物體的形式化建模與驗證流程圖，基於共形幾何代數CGA方法對機器人操作的主要思想是CGA的幾何體運動變換表達，但是本發明的變換幾何對象是參考圓而不是參考點，機械爪和被抓取物體的幾何特徵都用圓來表示。本發明中，使用劍橋大學研發的高階邏輯定理證明器HOL-Light作為形式化工具。HOL-Light是最流行的定理證明器之一，不僅擁有龐大的研究團隊和用戶群，而且包含豐富的數學定理庫如實數分析庫、超越函數庫、積分微分庫等和一系列高效的證明策略。
如圖1所示，本發明提出了一種基於共形幾何代數的機械臂運動規劃的形式化分析方法，其包括：
基於共形幾何代數理論對機器人的基本構件、運動規劃約束構建對應的幾何模型，再用高階邏輯語言描述所建立的幾何模型，形成機器人的基本幾何邏輯模型系統；
確定機器人的具體結構參數和運動規劃參數；
基於基本幾何邏輯模型系統對具體機器人運動過程的形式化建模，得到具體機器人運動過程的幾何關係邏輯模型；
用邏輯公式描述需要驗證的機器人運動過程的約束或屬性；
將具體機器人運動過程的所述幾何關係邏輯模型和待驗證的運動過程的約束或屬性組成一個邏輯命題；
利用邏輯推理引擎證明所述邏輯命題是否成立，成立表明所述模型滿足所述的約束或者具備所述的屬性，不成立表明所述模型不滿足所述的約束或者不具備所述的屬性。
共形幾何代數理論是機器人運動規劃的形式化分析方法的理論基礎，機器人運動規劃的形式化分析方法可以針對具體機器人問題分析的普遍方法，邏輯推理引擎實現對共形幾何代數形式化系統和機器人運動規劃的形式化分析方法的邏輯表達與證明的推理演算。
所述基於基本幾何邏輯模型系統對具體機器人運動規劃的運動規划過程形式化建模具體包括對機器人抓取物體的運動規划過程形式化建模。
優選地，所述步驟二中的具體機器人為n自由度串聯機械臂，末端包括一個T字形機械爪
優選地，所述步驟三中具體運動規劃為機器臂抓取物體的運動規划過程
優選地，所述步驟四中待驗證的運動規劃屬性為機械爪實現物體抓取成功並且抓牢。
所構建的幾何模型包括：機器人關節抽象化成點模型、機器人關節末端可達範圍抽象化為球模型、機器人關節所在輔助面抽象為面模型、機器人關節點連接構成的線模型、將機器人基本構件的約束關係抽象化為幾何體求交模型、機器人基本構件旋轉運動抽象化為幾何體純旋轉模型、機器人基本構件平移運動抽象化為幾何體純平移模型、機器人關節末端到達期望位置抽象化為幾何體剛體運動模型、機器人基本構件度量關係抽象化為幾何體間距離模型、機器人基本構件夾角抽象化為幾何體夾角模型。
下面對上述各個模型進行詳細介紹。其中，幾何體包括點、線、面、圓、球、點對等，用於表示機器人的基本構件。
機器人關節抽象化成點模型：
該模型中將機器人關節抽象化成點，上述數學表達式是共形幾何代數中點的標準表達式。
上述模型表達式中小寫s＝s1e1+s2e2+s3e3表示歐氏三維空間中的點，大寫S為該點從歐氏空間映射到共形空間中的點的表達式.函數point_CGA的輸入變量為歐氏空間中三維向量,其返回值為五維共形空間中的多重矢量，由於共形幾何代數可由幾何代數Cl4,1構造而成，其數據類型定義為real^(4,1)multivector.其中s$n表示多維向量s的第n個元素，mbasis函數表示基本片積函數,dot函數表示歐氏空間中的點積，null_inf和null_zero函數分別表示零矢量e0和e∞：
e∞＝e-+e+， (2)
其中e0表示原點，e∞表示無窮遠點，利用e∞,e0取代e+,e-能夠更緊湊的表示共形空間中的點.本發明將基矢量e+,e-定義為mbasis{4}和mbasis{5}，表示CGA空間中第四個基矢量和第五個基矢量.形式化定義如下：
|-null_zero＝(&1/&2)％(mbasis{5}-mbasis{4})/\null_inf＝(mbasis{5})+(mbasis{4})
該模型中point_CGA函數功能表示從三維歐氏空間到五維共形空間中點的映射關係。
機器人關節末端可達範圍抽象化為球模型：
將機器人關節末端可達範圍抽象化成以關節為球心連杆為半徑的球，上述數學表達式是共形幾何代數中球的標準表達式。函數sphere_CGA的輸入變量p、r分別表示球心和半徑，其中球心用歐氏空間中三維向量表示。
機器人關節所在輔助面抽象為面模型：π＝n+de∞
將機器人關節所在輔助面抽象化為面，上述數學表達式是共形幾何代數中面的標準表達式。函數plane_CGA的輸入變量n、d分別表示法向量和到原點的距離，其中法向量用歐氏空間中三維向量表示。
機器人關節點連接構成的線模型:L*＝A∧B∧e∞
上述數學表達式是共形幾何代數中任意兩點構成的線的表達式，可用於機器人中關節點構成的線或者其他輔助線的表示，其中A、B分別表示共形幾何空間中的兩個點。所述模型用於將兩關節間的連杆抽象為線。上述表達式為共形幾何代數中線的直接表達式，函數line_direct_CGA的輸入變量a、b分別表示線上的兩點，數據類型為real^(4,1)multivector，其中函數outer表示幾何代數中外積運算，外積運算可實現由低維幾何體向高維幾何體的構建，即擴維運算。
將機器人基本構件的約束關係抽象化為幾何體求交模型：o＝o1∧o2∧…∧on
|-MEET o1o2…on＝o1outer o2outer…on
其中，oi表示代表第i個機器人基本構件的幾何體，i＝1……n；
所述模型利用外積實現代表機器人基本構件的幾何體的求交運算。函數MEET的輸入變量表示待求交的任意幾何體，數據類型為real^(4,1)multivector。約束關係包括關節在連杆上(即點在線上)、連杆在關節所在輔助面上(線在面上)等。
機器人基本構件旋轉運動抽象化為幾何體純旋轉模型:
|-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))
所述利用幾何積實現幾何體旋轉變換，R為共形幾何代數中的旋轉算子，為R的幾何反，L表示的旋轉軸,φ是旋轉角度.o表示旋轉前幾何體的表達形式，orotated表示旋轉後幾何體的表達形式.函數rotation_CGA的輸入變量t、l分別表示旋轉副的旋轉角度與旋轉軸，該函數實現了旋轉算子的功能。函數pure_rotationed_CGA的輸入變量x、t、l表示幾何體、旋轉角度和旋轉軸，該函數的返回值為旋轉後的幾何體表示，其中函數reversion表示幾何反。
機器人基本構件平移運動抽象化為幾何體純平移模型：
|-Translation_CGA t＝mbasis{}-(&1/&2)％(t*null_inf)
|-pure_translationed_CGA x t＝(Translation_CGA t)*x*(reversion(Translation_CGA t))
所述模型利用幾何積實現幾何體平移變換，T為共形幾何代數中的平移算子，為T的幾何反，其中t＝t1e1+t2e2+t3e3是平移向量，表示平移的方向和長度.o表示平移前幾何體的表達形式，otranslated表示平移後幾何體的表達形式.函數Translation_CGA的輸入變量t是平移向量，表示平移的方向和長度，該函數實現了平移算子的功能。函數pure_translationed_CGA的輸入變量x、t表示幾何體、平移向量，該函數的返回值為平移後的幾何體表示。
所述機器人關節末端到達期望位置抽象化為幾何體剛體運動模型：
|-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))
該模型利用幾何積實現幾何體剛體變換，其中R,T分別是旋轉算子和平移算子,兩者用幾何積運算連接，為M的幾何反，origid_body_motion表示運動後的幾何體表達形式，函數motor_CGA的輸入變量x、a、l、t分別表示幾何體、旋轉角度、旋轉軸、平移向量，該函數實現了馬達算子的功能。函數rigid_body_motion的返回值為經過剛體運動後的幾何體表示，該表達式表示幾何體是先平移後旋轉，但馬達算子具有結合性，旋轉算子和平移算子是可以交換的。
機器人基本構件度量關係抽象化為幾何體間距離模型：
其中a、b表示三維歐氏空間中任意兩點，A、B為共形空間中該兩點的對應表達式，上述模型為共形幾何代數中點點距離表達式，其中dist(a,b)表示兩點距離，可用幾何代數內積表達.其中點線距離、點球距離、球球距離在共形幾何代數中也有具體的表達式，可用於機器人問題中具體計算時使用.
機器人基本構件夾角抽象化為幾何體夾角模型：
該模型為共形幾何代數中幾何體夾角表達式，函數vector_angles_CGA的輸入變量o1o2分別表示待求夾角的幾何體，可以是線或面等，數據類型為real^(4,1)multivector，其中函數inner表示幾何代數中左縮積運算，$${}表示取0階子空間即標量的大小，函數mult_norm功能為多重矢量的求模運算。該模型可用於機器人基本構件之間夾角的計算。
優選地，所述方法中具體機器人為n自由度串聯機械臂，末端包括一個T字形機械爪；
所述方法中具體運動規劃為機器臂抓取物體的運動規划過程；
所述方法中待驗證的運動規劃屬性為機械爪實現物體抓取成功並且抓牢。
圖2示出了本發明中對機器人抓取物體的運動規划過程形式化建模的流程圖。如圖2所示，其包括：
步驟30：分別提取物體和機械爪的特徵點；
步驟31：計算物體的被抓取位置所在的目標圓；
如圖3和圖4所示，通過物體邊緣的四個特徵點(x1,x2,x3,x4)得到物體的位置方位，其中x1,x2,x3為物體底部邊沿上任意三點，x4為物體頂部邊沿上任意一點。由物體底部三點構成的參考圓可通過圓的直接表達式得到：
則物體底部參考圓所在的面為：
其中,Ic為共形幾何代數的偽標量，通過偽標量是可實現對偶運算，對偶運算實現幾何體兩種表示方法的相互轉換。
由於方便設定物體被抓取的位置是中部，所以目標圓Zt應該是底部參考圓沿-π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)))
其中，物體底部參考圓調用圓的直接表示函數circle_direct_CGA,x1 x2 x3 x4表示物體的四個特徵點，HOL類型均是real^3，表示三維歐氏向量，通過函數point_CGA使特徵點由三維歐氏空間嵌入到五維共形空間中。物體被抓取的部位所形成的目標圓通過純平移函數pure_translationed_CGA實現。
步驟32：計算機械爪所在圓；
如圖5所示，已知機械爪所在圓的圓心ph、半徑ρ、機械爪上兩點a,b可計算出機械爪所在圓的位置，首先構造機械爪所在球的位置，通過球的標準表達式得到：
然後通過圓心ph、機械爪上兩點a,b計算機械爪所在面：
機械爪所在球Sh和所在面πh相交得到機械爪所在圓：
Zh＝Sh∧π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))
其中，輔助球Sh調用球的標準表示函數sphere_CGA實現,(ph:real^3)和(r:real)分別表示輔助球的圓心Ph和半徑r,機械爪所在的輔助面調用面的直接表示函數plane_direct_CGA實現，(a:real^3)和(b:real^3)分別表示機械爪上兩點。對偶算子用DUAL函數。
步驟33：計算機械爪抓取物體的平移算子，所述平移算子包括平移軸和平移長度；
如圖5所示，易知平移長度應該為目標圓Zh和機械爪所在圓Zt的圓心距離，而平移軸是過兩圓心的直線。首先計算目標圓Zh的圓心：
Pt＝Zte∞Zt
由於平移軸由兩圓心確定，可通過直接表達式計算平移軸：
相應地，平移長度為
上述計算過程在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.在由兩圓的軸線確定的面上
所以，先計算目標圓Zt和機械爪所在圓Zh的兩軸線：
由兩軸線確定的面為：
則旋轉軸計算為：
而旋轉角度易知為兩軸線的夾角，由夾角公式得：
上述計算過程在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夾角公式來計算，由函數vector_angles_CGA實現.
步驟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)))
其中，由於機械爪新的目標位置是通過先旋轉後平移得到，所以先將機械爪面所構成的圓zh通過純旋轉函數pure_rotationed_CGA進行旋轉運動，該函數的輸入變量是被旋轉的幾何體zh、旋轉角度θ和旋轉軸，即(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))，再將旋轉後的圓通過純平移函數pure_translationed_CGA完成平移運動，該函數輸入變量是旋轉的幾何體平移向量dlT。
上述代碼主要內容涉及共形幾何代數CGA幾何體表示、幾何體間距離特徵和幾何體運動變換的形式化。機械爪圓Zh一步步靠近物體位置所在目標圓Zt，最終機械爪成功抓取到物體並且抓牢必須滿足一定的幾何約束關係。上述算法中，該約束關係為機械爪面所在圓Zh新的位置最終應該與物體所在目標圓Zt的位置吻合，即在共形幾何代數CGA中表達式應該滿足相等的關係。可以在HOL-light中建立目標驗證這層幾何約束關係：
goal:
本發明中關於基於CGA對機器人的運動規划進行形式化建模與驗證的方法，在HOL-Light中可以加載使用。其中最大難點是定理證明技術需要大量的人機互動，工作量較大，時間耗費較長，建模過程中強調使用者對共形幾何代數CGA理論知識的熟識度，驗證過程中要求嚴密的思維和一定的邏輯推理經驗。
以上所述的具體實施例，對本發明的目的、技術方案和有益效果進行了進一步詳細說明，應理解的是，以上所述僅為本發明的具體實施例而已，並不用於限制本發明，凡在本發明的精神和原則之內，所做的任何修改、等同替換、改進等，均應包含在本發明的保護範圍之內。