基于Kabsch算法的NURBS曲線的變精度優(yōu)化匹配方法
【專利摘要】基于Kabsch算法的NURBS曲線的變精度優(yōu)化匹配方法，采用“先粗糙，后精細”的變精度的匹配方法，利用NURBS來描述任意曲線，曲線的分裂和組合具有統(tǒng)一的數學形式，首先用相似閾值來衡量兩條軌跡的相似程度，通過降低基元相似度和增加搜索步長在大量軌跡中搜索與期望軌跡含有較多相似特征的軌跡；接著提高相應的匹配精度指標，在滿足相似度的情況下，從搜索出的軌跡中求取使期望參考曲線分割次數最少時的軌跡基元；最后，將所有匹配的基元依次組合起來，此組合曲線與原期望曲線相似。
【專利說明】
基于Kabsch算法的NURBS曲線的變精度優(yōu)化匹配方法 (一)
技術領域
[0001] 本發(fā)明是為了提高曲線優(yōu)化匹配組合效率，使其在迭代學習控制領域得到更好的 應用。 (二)
【背景技術】
[0002] 迭代學習控制（Iterative Learning Control，簡稱ILC)是針對這種精確控制任 務而提出的，ILC系統(tǒng)的運動參考軌跡(或作業(yè)任務)可以由一系列軌跡基元串聯(lián)而成，從以 往作業(yè)中學習積累軌跡基元的控制信號，面臨新的參考軌跡時，進行軌跡基元優(yōu)化匹配、依 次尋找相似軌跡基元，經串聯(lián)合成相似參考軌跡，提取其運動控制信號并進行信號處理、分 析和補償后作為當前作業(yè)的初次迭代控制信號;這可避免直接去尋找整個作業(yè)區(qū)間相似的 參考軌跡，也將改變傳統(tǒng)ILC的初次迭代控制信號為零(或其它預設值），作業(yè)區(qū)間和作業(yè)任 務(或參考軌跡)變化需要重新學習的狀況。通過軌跡基元匹配，獲得接近于期望值的初次 迭代控制信號，將顯著降低初次迭代控制的跟蹤誤差。NURBS已經成為描述和設計幾何圖形 的工業(yè)標準工具。NURBS為自由型曲線曲面的表示提供了一種統(tǒng)一的數學方法，并且其在通 常的幾何變換（如平移、旋轉、平行和透視投影)下是具有不變性，因此，我們用其來描述期 望參考軌跡和曲線庫中的所有軌跡基元。
[0003] Kabsch算法能求解給出最小化兩組對應點集間均方根偏差的最優(yōu)仿射變換算子， 因此可利用此算法建立軌跡匹配相似性描述方法，研究曲線之間的相似性判斷方法，進而 給出軌跡優(yōu)化匹配組合方法。
[0004] 徐建明，臧永燦，朱自立，孫明軒，俞立，中國專利《一種迭代學習控制參考軌跡的 優(yōu)化匹配組合方法》，專利號為CN201510446408.6，【公開日】2015年11月25日，該專利采用的 是統(tǒng)一精度的匹配方式，該方法的靈活性差，效率低，計算數據多，在匹配軌跡數量眾多的 環(huán)境下，不具有實用性?；谝陨戏椒ǖ牟蛔悖緦＠岢隽俗兙鹊钠ヅ浞绞?。 (三)

【發(fā)明內容】

[0005] 本發(fā)明要克服現有技術的上述缺點，提供一種應用性能好、效率高、靈活性高，同 時大大減少了數據計算量，在軌跡任務較多的環(huán)境下優(yōu)勢明顯的基于Kabsch算法的NURBS 曲線的變精度優(yōu)化匹配方法。
[0006] 在大量軌跡的工業(yè)生產中提高軌跡的匹配效率，傳統(tǒng)的軌跡匹配方式已不滿足工 業(yè)中的實際需求，傳統(tǒng)的統(tǒng)一精度匹配方法靈活性差、效率低、計算數據多。本發(fā)明采用變 精度的匹配方式，提出"先粗糙，后精細"的匹配思想，克服了傳統(tǒng)方法應用性能差、效率低 的缺點，靈活性高，同時大大減少了數據計算量，在軌跡任務較多的環(huán)境下該發(fā)明優(yōu)勢更明 顯。該發(fā)明任務是快速從大量的軌跡中搜索出一條與設定的參考軌跡相似的組合軌跡。該 發(fā)明為迭代學習控制（ILC)中初次迭代控制信號的提取提供了方法支持。
[0007] 軌跡匹配需要綜合考慮軌跡相似閾值、軌跡基元相似性指標以及軌跡位姿特征， 軌跡基元匹配是一個需要權衡考慮各種因素的多目標優(yōu)化理論問題。目標是如何快速從軌 跡庫中眾多軌跡中尋找一些含有與期望軌跡具有較多相似特征的軌跡，再從選擇出的幾條 軌跡中提取一條軌跡基元拼接數量少、相似程度高的相似組合軌跡。
[0008] 本發(fā)明利用Kabsch算法計算基元的相似性、最優(yōu)平移向量T和旋轉矩陣R。首先利 用NURBS來描述任意軌跡和基元;其次，用軌跡的相似閾值來衡量一條軌跡與期望軌跡的相 似程度，從眾多軌跡中選擇出一些含有與期望軌跡較多的相似特征（滿足相似閾值）的軌 跡;接著利用Kabsch算法描述基元的相似性，在滿足基元相似度的情況下，求取使期望參考 軌跡分割次數最少時的軌跡基元;最后，將所有匹配的基元依次組合起來，得到與設定的期 望軌跡相似的組合曲線。
[0009] 在用Kabsch算法計算兩基元相似性時，需要用節(jié)點插入的方式將軌跡分割，u為 NURBS曲線的節(jié)點矢量參數，我們用曲線的分割參數< 和〃丨（0I )宋分割曲線庫 CurVeBank2中的第j條曲線㈨))，(?/)和("〖)則分別是分割參數《〖和<對 應的兩個分割點，分割出的曲線段1,(4(?/>叭(〃1))作為臨時基元。我們將一條期望NURBS曲 線表示為S(Cs(0)，Cs(l))，則起點Cs(0)與節(jié)點列〇<#<1)對應的點幻之間的曲線段記為 S(C s(0)，C：s(;7))。通過Kabsch算法計算出這兩個曲線段的最小均方根誤差 lrm(相似度￡)、最 優(yōu)平移向量T和旋轉矩陣R。
[0010] 同時在衡量整條軌跡的相似性時，我們引入了相似閾值的概念，以下給出軌跡相 似閾值的定義。
[0011]定義:給定一個x>0，初始化一個變量T，期望軌跡上的一片段與某一基元滿足相似 性條件（lrm〈e)時，則做T+1運算，如果期望軌跡與一條軌跡的相似度很高時，則該條軌跡存 在多個與期望軌跡片段滿足相似度e的基元，那么T的值就相對較大，驗證了以T的大小來衡 量兩整條軌跡的相似程度的合理性。設定合理的閾值X，當T>x時，稱期望軌跡與軌跡庫中選 出的該條軌跡相似，稱x為軌跡相似閾值。
[0012]本發(fā)明的優(yōu)化匹配方法通過"先粗糙，后精細"兩步進行，首先通過降低基元相似 度e和增加搜索步長等條件，快速從CurveBankl軌跡庫中眾多軌跡搜索出滿足軌跡相似閾 值的軌跡，該部分算法流程如附圖1所示;接著提高相應的匹配精度指標，用傳統(tǒng)的方式搜 索出的軌跡中提取相似組合軌跡。本發(fā)明原則是選擇合適的相似閾值X，在軌跡庫中一次選 擇出適當數量的軌跡，同時在二次選擇軌跡基元時，在滿足合適基元相似度e的情況下，期 望曲線的分割段數最少。
[0013]步驟1，對于確定的N條曲線的曲線庫CurveBankl和已知的期望曲線S(Cs(0)，Cs (1 ))，其"粗糙"匹配算法步驟如下：
[0014] Stepl:從軌跡庫CurveBankl中取一條軌跡(p，U，P)，其中p為樣條曲線的階次，U為 節(jié)點矢量，P為控制點，初始化參數T = 0;
[0015] Step2:初始化搜索步長v = l和分割參數[=1;
[0016] Step3:判斷^CSi (0)' (h⑶是否滿足相似度條件lrms<e，若 滿足跳入4)，若不滿足，則令v = v/2, m_ = _?-v_，跳入Step5;
[0017] Step4:判斷是否滿足S = 1，若滿足則進行T = T+1，跳入6)，若不滿足，則令v = v/2， M' = M.+.v，T = T+]^^AStep5;
[0018] Step5:判斷搜索步長v的大小，是否滿足abs(V)〈e。若滿足，將期望軌跡未匹配部 分進行歸一化，跳入St印2，若不滿足則跳入St印3。
[0019] Step6:將T的大小與設定的相似閾值進行比較，判斷是否滿足T>x，若滿足則將該 軌跡保存到軌跡庫CurveBank2中，若不滿足則不輸出該軌跡。跳入Stepl;
[0020] 通過以上匹配算法從曲線庫CurveBankl中搜索出n條(n<<N)含有較多與期望軌 跡相似特征的軌跡，并將其放入軌跡庫CurveBank2中。
[0021] 步驟2,接著提高相應的匹配精度指標對含有n條軌跡的軌跡庫CurveBank2的軌跡 進行"精細"匹配，該部分采用傳統(tǒng)的匹配方式，已有相關文獻說明，不再贅述。
[0022] 經過上述優(yōu)化匹配算法后，分割點分別為(\ (")、（\ ((>)......與各個期望曲 線段匹配的各基元分別為Eh、El2、……Elm。由第1部分可得基元E1」與對應的期望曲線段 ⑶之間最優(yōu)的旋轉矩陣R n平移向量T」和基元E 1」的質心％，在 \(C:s((〇.Cs.(i))(戶/2…叫心=1)曲線段上取的點序列為cv，在基元 Elj上取的點序列為 Q相對其質心氣的點序列記作\V，En為叫經過旋轉平移得到，那么與整條期 望曲線匹配的組合曲線為只〇 5(0),(^1)'1^￡7；，至此，得到一條與期望軌跡相似的組合軌 跡。
[0023] 本發(fā)明的優(yōu)點是:采用"先粗糙，后精細"的變精度的匹配方法，相對傳統(tǒng)的統(tǒng)一精 度的匹配方法大大提高了軌跡的匹配效率，減少了大量不必要的計算，在軌跡眾多的工業(yè) 生產中更具有實用性;充分地利用了以往參考軌跡信息，匹配當前迭代學習控制參考軌跡， 算法靈活穩(wěn)定可靠，靈活性表現在可以設定基元相似度和軌跡相似閾值，在相似要求和匹 配效率兩方面權衡選擇合適的相似度和相似閾值。
【附圖說明】
[0024] 圖1為粗糙匹配的算法流程圖，其中S為可變的最小搜索步長，x為可變的軌跡相似 閾值，CurveBankl存放的是大量以往的參考軌跡，Curvebank2存放的是與期望軌跡滿足相 似閾值的軌跡。
[0025] 圖2為精細匹配的算法流程圖。其中，v為可變搜索步長，S為最小搜索步長，m為匹 配完后期望曲線的分割段數"為期望曲線\叭 (0)，&(1))的分割參數，E1 〃謙示曲線庫 CurVeBank3中任意的臨時基元。將所有與期望曲線優(yōu)化匹配的曲線段依次存儲在 Cur VeBank3中，所有E1，組成了優(yōu)化匹配基元庫。平移旋轉后的基元E1，對應的基元Elj之 間最優(yōu)的旋轉矩陣和平移向量分別為Rj和Tj。
[0026]圖3表示事先設定好的期望軌跡。
[0027] 圖4表示從旋轉平移之后的軌跡基元。
[0028] 圖5表示最后匹配成功的與期望軌跡相似的組合軌跡。
【具體實施方式】
[0029]以下結合附圖和實施例對本發(fā)明的技術方案作進一步描述。
[0030] 實施例
[0031]假設ILC的參考軌跡(或期望曲線）如圖2中的實線表示，控制多邊形由虛線表示， 其控制頂點為（〇,〇,〇)、（1，-2,0)、（3，-2,1)、（4,3，1)、（5,0,2)、（7，-1，2)和(9,0,3)，權值 均為 1，節(jié)點矢量 1]=[0,0,0,0,0.2,0.4,0.7，1，1，1，1]。令此期望曲線為3(&(0)，&(1))，那 么&(0) = (0,2,0)，(：5(1) = (9,0,2)。假設曲線庫(：1^6匕&業(yè)1中有50條順觀描述的曲線，且 所有控制點對應的權值均為1，曲線庫中所有曲線的參數如表1所示(P為控制多邊形頂點坐 標矩陣，U為節(jié)點矢量），曲線庫中所有曲線如圖3所示。
[0032]表1三維曲線庫中各曲線的控制參數
[0034]根據圖1的"粗糙"匹配算法流程圖，設定基元匹配精度e=0.1，搜索步長d = 0.1， 期望軌跡的終止搜索步長8 = 0.05,軌跡的相似閾值設定為x = 5。按照粗糙匹配的方法，可 以快速的從CurveBankl中提取出和期望軌跡滿足閾值條件的軌跡。經過一次篩選，從50條 軌跡中選擇出了 7條軌跡保存到CurveBank2中，其軌跡控制參數如表2所示。
[0035]表2.滿足相似度閾值的的軌跡

[0038]根據圖2的優(yōu)化匹配算法結構流程圖和前面所述的組合方法，對CurVeBank2(表2) 中的軌跡進行精細匹配。設定匹配精度e = 〇. 025，搜索步長d = 0.05，期望軌跡終止搜索步 長5 = 0.025，此時可以精確的從CurveBank2軌跡中截取一些軌跡基元，其基元控制參數如 表3所示。
[0039]表3.平移旋轉后的基元控制參數

[0042] 根據精細匹配部分的組合方法，得到5((75(0)，(：5(1^1；:￡7丨:，即組合曲線與期望軌跡 相似。
[0043] 最后我們用本發(fā)明所述方法與傳統(tǒng)的匹配方法進行對比，在5條、25條、50條軌跡 的環(huán)境下其匹配成功用時時間對比如表4所示。
[0044] 表4.本發(fā)明所述方法與傳統(tǒng)方法時間對比
[0047]本發(fā)明的優(yōu)點是:采用"先粗糙，后精細"的變精度的匹配方法，相對傳統(tǒng)的統(tǒng)一精 度的匹配方法大大提高了軌跡的匹配效率，減少了大量不必要的計算，在軌跡眾多的工業(yè) 生產中更具有實用性;充分地利用了以往參考軌跡信息，匹配當前迭代學習控制參考軌跡， 算法靈活穩(wěn)定可靠，靈活性表現在可以設定基元相似度和軌跡相似閾值，在相似要求和匹 配效率兩方面權衡選擇合適的相似度和相似閾值。
【主權項】
1. 基于Kabsch算法的NURBS曲線的變精度優(yōu)化匹配方法，采用"先粗糙，后精細"的兩步 匹配方法，該方法包括對于已知的參考軌跡或期望軌跡兄(0)，Q (?).. A:⑴)和軌跡庫 CurveBankl、CurveBank2、CurveBank3，其中，，C^ (n)為NURBS軌跡的節(jié)點矢量參 數，軌跡庫CurveBankl 中含有N條NURBS軌跡A(cs,(⑴，Q A,⑴），A (CSi (0)，CSi (1))為 軌跡庫中第一條軌跡，j = 1，......，N，軌跡庫CurveBank2中含有從CurveBankl中粗糙匹配出 的n條NURBS軌跡，分割參數_wf和w彳分割CurveBank2的第j條軌跡形成臨時基元El j，基元即軌 跡上的片段;軌跡庫CurveBank3用來存放將臨時基元Elj經旋轉平移后的軌跡基元El〃j參 數； 步驟1，對于確定的N條曲線的曲線庫CurveBankl和已知的期望曲線兄(心(0)，C& (h)...C'Si； (1))， 其"粗糙"匹配算法步驟如下： Stepl:從軌跡庫CurveBankl中取一條軌跡(〇)，Cs, (?).XS, (1)),初始化參數T = 0，用t 的大小來衡量軌跡的相似程度； Step2:初始化搜索步長v= 1和分割參數J = 1; Step3:判斷《(4 (〇),CSi _與(Ci;⑷，Q ^ 入4)，若不滿足，則令v = v/2，〃 = " - _v，跳入Step5; Step4:判斷是否滿足= l,若滿足則進行T = T+1，跳入6)，若不滿足，則令v = v/2， 忑二.5 + v .，T = T+1 跳入 Step5; Step5:判斷搜索步長v的大小，是否滿足abs(V)〈e，0表示終止搜索步長;若滿足，將期 望軌跡未匹配部分進行歸一化，跳入St印2，若不滿足則跳入St印3; Step6:將T的大小與設定的相似閾值x進行比較，判斷是否滿足T>x，若滿足則將該軌跡 保存到軌跡庫CurveBank2中，若不滿足則不輸出該軌跡;跳入Stepl; 通過以上匹配算法從曲線庫CurveBankl中搜索出n條含有較多與期望軌跡相似特征的 軌跡，即含有較多與期望軌跡相似的片段，并將其放入軌跡庫CurveBank2中，n〈〈N; 步驟2,接著提高相應的匹配精度指標對含有n條軌跡的軌跡庫CurveBank2的軌跡進行 "精細"匹配，得到m條軌跡基元，該部分已有相關文獻說明，不再贅述； 該部分得到匹配基元Elpj = l，……，m，El、為匹配基元Eh在其質心坐標系{j}下的表 示，對其進行旋轉和平移變換后保存到曲線庫CurVeBank3中；組合旋轉平移后的匹配基元 E1〃 j，j = 1，……，m，獲得ILC參考軌跡的S (Cs (0)，Cs (1))的相似參考軌跡； 通過以上"粗糙，精細"兩個階段的匹配，得出一條滿足匹配進度的組合軌跡2. 根據權利要求書1中所述的基于Kabsch算法的NURBS曲線的變精度優(yōu)化匹配方法，其 特征在于:步驟1中的Step6中的軌跡相似閾值的獲取方法是： 初始化一個變量T，期望軌跡上的一片段與某一基元滿足相似性條件lrm〈e時，則做T+1 運算，e是對兩個基元利用Kabsch算法計算得到，如果期望軌跡與一條軌跡的相似度很高 時，則該條軌跡存在多個與期望軌跡片段滿足相似度e的基元，那么T的值就相對較大，驗證 了以T的大小來衡量兩整條軌跡的相似程度的合理性;設定合理的閾值X，當T>x時，稱期望 軌跡與軌跡庫中選出的該條軌跡相似，稱X為軌跡相似閾值。
【文檔編號】G05B13/02GK106054593SQ201610369151
【公開日】2016年10月26日
【申請日】2016年5月27日
【發(fā)明人】徐建明, 王耀東, 孔令新, 張貴軍, 邢科新
【申請人】浙江工業(yè)大學