一種針對虛擬微創(chuàng)血管介入手術(shù)的導絲建模方法
【專利摘要】本發(fā)明公開了一種針對虛擬微創(chuàng)血管手術(shù)的導絲建模方法，該方法包括：步驟1：對導絲進行離散化表示，得到導絲模型；步驟2：對所述導絲模型的參數(shù)進行初始化；步驟3：基于初始化的導絲模型參數(shù)，獲得所述導絲模型的能量值；步驟4：基于所述步驟3得到的能量獲得相應(yīng)的力和力矩；步驟5：利用拉格朗日乘子式法實現(xiàn)所述導絲模型的不可拉伸約束，并計算出由該不可拉伸約束產(chǎn)生的約束力；步驟6：對所述步驟4和步驟5求得的力和力矩進行求和，并結(jié)合所述步驟4和步驟5求得的力和力矩，對所述導絲模型的參數(shù)進行更新；步驟7：循環(huán)調(diào)用步驟3～6來進行導絲的動態(tài)仿真。實驗證明，本發(fā)明導絲模型能夠逼真地、實時地模擬真實導絲的物理形變。
【專利說明】
一種針對虛擬微創(chuàng)血管介入手術(shù)的導絲建模方法
技術(shù)領(lǐng)域
[0001] 本發(fā)明涉及虛擬微創(chuàng)血管介入手術(shù)仿真領(lǐng)域，具體涉及一種針對虛擬微創(chuàng)血管介 入手術(shù)的導絲建模方法。
【背景技術(shù)】
[0002] 心血管疾病是全球致死率最高的疾病，微創(chuàng)血管介入手術(shù)是治療該疾病的主要方 法，具有創(chuàng)傷小、恢復快等優(yōu)勢。但是，微創(chuàng)血管介入手術(shù)流程復雜，需要醫(yī)生具有豐富的手 術(shù)操作經(jīng)驗。目前，傳統(tǒng)手術(shù)培訓主要在手術(shù)室內(nèi)基于活體動物、尸體進行手術(shù)培訓，培訓 者很難隨時隨地進行手術(shù)訓練。虛擬微創(chuàng)血管介入手術(shù)培訓系統(tǒng)為培訓者提供一個沉浸感 很強的虛擬培訓場景，培訓者可隨時隨地與虛擬環(huán)境交互，不斷提高手術(shù)技能，提高醫(yī)生培 訓的效率，縮短培訓周期。
[0003] 導絲建模作為虛擬血管介入手術(shù)培訓系統(tǒng)的核心，一直是虛擬血管介入手術(shù)的研 究重點，當前存在很多建模方法，但都存在實時性和逼真度無法同時滿足的缺點。基于此， 本發(fā)明提出了一種針對虛擬微創(chuàng)血管介入手術(shù)的導絲建模方法，該方法具有很好的實時性 和逼真度。

【發(fā)明內(nèi)容】

[0004] 本發(fā)明實施的主要目的在于提供一個針對虛擬微創(chuàng)血管介入手術(shù)的導絲建模方 法，該方法基于彈性棒理論和拉格朗日乘子式法對導絲建模，能夠同時滿足實時性和逼真 度的需求。
[0005] 為實現(xiàn)上述目的，本發(fā)明提供了以下技術(shù)方案：
[0006] -種針對虛擬微創(chuàng)血管介入手術(shù)的導絲建模方法，所述方法包括以下步驟：
[0007] 步驟1:對導絲進行離散化表示，得到導絲模型；
[0008] 步驟2:對所述導絲模型的參數(shù)進行初始化；
[0009]步驟3:基于初始化的導絲模型參數(shù)，獲得所述導絲模型的能量值；
[0010]步驟4:基于所述步驟3得到的能量獲得相應(yīng)的力和力矩；
[0011] 步驟5:利用拉格朗日乘子式法實現(xiàn)所述導絲模型的不可拉伸約束，并計算出由該 不可拉伸約束產(chǎn)生的約束力；
[0012] 步驟6:對所述步驟4和步驟5求得的力和力矩進行求和，并結(jié)合所述步驟4和步驟5 求得的力和力矩，對所述導絲模型的參數(shù)進行更新；
[0013] 步驟7:循環(huán)調(diào)用步驟3~6來進行導絲的動態(tài)仿真。
[0014] 可選地，所述導絲模型通過N個空間控制節(jié)點和N-1個四元數(shù)來表示，其中，N個空 間控制節(jié)點由對導絲離散化得到，N-1個四元數(shù)表示N個空間控制節(jié)點形成的N-1個中心線 段的右手正交坐標系。
[0015]可選地，所述空間控制節(jié)點的數(shù)量大于等于3,相鄰空間控制節(jié)點之間的距離相同 或不同。
[0016] 可選地，所述步驟2中，需要進行初始化的導絲模型參數(shù)包括導絲的空間控制節(jié)點 坐標、四元數(shù)坐標、楊氏模量、剪切模量、導絲質(zhì)量、導絲半徑、導絲長度、導絲固有彎曲扭轉(zhuǎn) 屬性、重力大小和能量公式常量參數(shù)中的一種或多種。
[0017] 可選地，導絲模型頭部的楊氏模量小于導絲模型導絲體部分的楊氏模量。
[0018] 可選地，所述步驟3中，所述能量值包括彎曲勢能￡#、內(nèi)摩擦耗能、粘滯力耗能 %和向量值約束補償能Ce。
[0019] 可選地，所述步驟4中，首先將每個能量分別分解為最小單元，然后對每個最小單 元分別計算力和力矩，然后對每個能量對應(yīng)的最小單元力和力矩分別進行求和得到每個能 量對應(yīng)的力和力矩，最后將所有能量的力和力矩分別進行求和得到所有能量的合力以及力 矩之和。
[0020] 可選地，彎曲勢能的最小單元為兩個相鄰四元數(shù)向量；內(nèi)摩擦耗能的最小單元為 兩個相鄰空間控制節(jié)點；粘滯力耗能的小單元為兩個相鄰四元數(shù)向量；向量值約束補償能 的最小單元為兩個空間控制節(jié)點和空間控制節(jié)點間的一個四元數(shù)向量。
[0021] 可選地，所述步驟5進一步包括以下步驟：
[0022] 步驟51，通過下式得到拉格朗日乘子式系數(shù)λ:
[0024] 其中，W是空間控制節(jié)點的質(zhì)量矩陣的逆，J是不可拉伸約束向量的雅克比矩陣，/ 表示雅克比矩陣的時間導數(shù)，％為空間控制節(jié)點的向量集合，衫ρ表示％的時間導數(shù)，f代表 能量產(chǎn)生的力與導絲所受外力之和，kdPkd分別代表彈簧和阻尼系數(shù)，C為不可拉伸約束的 向量形式，.亡表示約束的時間導數(shù)；
[0025] 步驟52,通過下式得到由不可拉伸約束產(chǎn)生的約束力Fif:
[0027]可選地，所述步驟6中，基于以下公式對導絲模型的參數(shù)進行更新：
[0033]其中，h代表時間步長，代表t時刻空間控制節(jié)點i的速度，nu表示空間控制節(jié)點 的質(zhì)量，產(chǎn)表示t時刻導絲的受力，0表示t時刻的空間控制節(jié)點的位置，<4代表四元數(shù)j在 時亥葉時的角速度，I代表慣性張量，%#表示七+1!時刻的非單位四元數(shù)，//f表示t時刻的四 元數(shù)矩陣，0表示代表t時刻的單位四元數(shù)，II.qUI表示的絕對值，代表t時刻的 四元數(shù)j的臨時力矩，其可以通過下式獲得：
[0035] 其中，?表示力矩的中間變量，hJ"表示四元數(shù)矩陣的轉(zhuǎn)置，T j表示求得的力矩之 和。
[0036] 與現(xiàn)有技術(shù)相比，本發(fā)明具有以下優(yōu)點：
[0037] 1、本發(fā)明基于彈性棒理論對導絲進行建模，能夠有效地模擬導絲的真實物理形 變，從而保證了手術(shù)器械模型的逼真度。
[0038] 2、本發(fā)明基于拉格朗日乘子式法保證了導絲模型不可拉伸的物理特性，進一步提 高了導絲模型的穩(wěn)定性和逼真度。
[0039] 3、本發(fā)明通過導絲能量直接獲得相應(yīng)力和力矩，用于后面的導絲狀態(tài)迭代，提高 了模型的計算速度，保證了模型的實時性。
【附圖說明】
[0040] 圖1為根據(jù)本發(fā)明一實施例的針對微創(chuàng)血管介入手術(shù)的導絲建模方法的流程圖。
[0041] 圖2為導絲的離散化表示。
[0042] 圖3(a)_(b)為導絲模型在推拉操作下的誤差變化情況。
[0043] 圖4(a)_(d)為導絲模型在動脈弓的仿真效果。
[0044] 圖5為導絲模型在冠脈分支中的仿真效果。
【具體實施方式】
[0045] 為使本發(fā)明的目的、技術(shù)方案和優(yōu)點更加清楚明白，以下結(jié)合具體實施例，并參照 附圖，對本發(fā)明進一步詳細說明。
[0046] 圖1為根據(jù)本發(fā)明一實施例的針對微創(chuàng)血管介入手術(shù)的導絲建模方法的流程圖， 如圖1所示，所述方法包括以下步驟：
[0047]步驟1:為了能夠讓計算機對導絲進行仿真，需要首先對導絲進行離散化表示，得 到導絲模型；
[0048]如圖2所示，導絲可被離散成一系列的空間控制節(jié)點，如圖2中所示的節(jié)點Gi，所述 空間控制節(jié)點主要存儲該節(jié)點的X，y，z坐標，用來表示該空間控制節(jié)點在三維世界中的具 體位置。
[0049]在所述導絲模型中，將導絲離散成N個空間控制節(jié)點，這N個空間控制節(jié)點構(gòu)成了 N-1個中心線段，如圖2中的線段GwGi，為了實現(xiàn)導絲模型的彎曲和扭轉(zhuǎn)操作，對每一個中 心線段附一個右手正交坐標系，如圖2中的（Cl( j)，C2( j)，C3( j))，其中，Cl( j)、C2( j)、C3( j) 分別表示中心線段j的x，y，z軸?；谠撟鴺讼担梢詫崿F(xiàn)導絲的彎曲和扭轉(zhuǎn)效果。在所述導 絲模型中，空間控制節(jié)點和右手正交坐標系通過向量值約芽
實現(xiàn)關(guān)聯(lián)，其 中，G'表示導絲的切向量，所述向量值約束用來保證右手正交坐標系的C3軸與導絲的切向 量平行。由于真實導絲具有不可拉伸的物理特性，所以本發(fā)明中實現(xiàn)的導絲模型也具有不 可拉伸屬性，于是，就可以假設(shè)在模型的迭代過程中，導絲長度不可拉伸，這樣就可以將導 絲的切向量表示成
_這里，G' i為第i個切向量的離散化表示，li表示中心線段i 的長度。在本發(fā)明一實施例中，對于所述導絲模型，為了提高該導絲模型的計算速度，通過 四元數(shù)Q(j) = (qi (j)，q2 (j)，q3 (j)，q4(j))來表示右手正交坐標系，所述四元數(shù)與（ci (j)，C2 (j)，c3( j))之間的轉(zhuǎn)換關(guān)系如下所示：
[0053]其中，91〇)^2〇)，陽〇)^4(」）分別代表第」個四元數(shù)的四個分量數(shù)值。于是，整 個導絲模型可以通過N個空間控制節(jié)點和N-1個四元數(shù)來表示。
[0054]步驟2:對所述導絲模型的參數(shù)進行初始化，以使所述導絲模型能夠更加逼真地模 擬真實導絲；
[0055] 在得到導絲模型之后，需要對所述導絲模型的參數(shù)進行初始化。在該導絲模型中， 需要初始化的參數(shù)包括導絲的空間控制節(jié)點坐標、四元數(shù)坐標、楊氏模量、剪切模量、導絲 質(zhì)量、導絲半徑、導絲長度、導絲固有彎曲扭轉(zhuǎn)屬性、重力大小和能量公式中的常量參數(shù)。其 中，導絲的空間控制節(jié)點和四元數(shù)信息用來初始化導絲模型在空間中的位置和初始狀態(tài)； 楊氏模量用來表示導絲用來抵制彎曲的能力大小，楊氏模量越大，導絲越能夠提供較大的 恢復力?？紤]到真實導絲分為導絲頭部和導絲體，兩個部分具有不同的楊氏模量，因此在對 導絲模型楊氏模量進行配置的過程中，導絲模型頭部的楊氏模量應(yīng)該小于導絲模型導絲體 部分的楊氏模量，這樣導絲能夠更容易的彎曲，也能夠更容易模擬真實導絲的形變;剪切模 量用來表示導絲抵制扭轉(zhuǎn)的能力大小，剪切模量越大，導絲在發(fā)生扭轉(zhuǎn)時，提供的扭轉(zhuǎn)恢復 力越大；導絲固有彎曲扭轉(zhuǎn)屬性用來控制導絲在穩(wěn)定狀況下，導絲所表現(xiàn)出來的彎曲扭轉(zhuǎn) 效果，可以通過該參數(shù)改變導絲的外在彎曲形狀。
[0056] 所述導絲模型中，所述空間控制節(jié)點的數(shù)量必須大于等于3,另外，相鄰空間控制 節(jié)點之間的距離可以相同也可以不同。
[0057]步驟3:基于初始化的導絲模型參數(shù)，獲得所述導絲模型的能量值，該步驟中，需要 求取的能量值主要包括彎曲勢能￡#、內(nèi)摩擦耗能β|、粘滯力耗能和向量值約束補償能 Ce；
[0058]其中，彎曲勢能用來表示導絲模型的彎曲扭轉(zhuǎn)程度，可以通過下式獲得：
[0060] 在這個公式中，L代表導絲的長度，Sk代表導絲的彎曲扭轉(zhuǎn)應(yīng)變，81和8 2代表導絲在 切面方向的彎曲應(yīng)變，S3代表導絲在軸向的扭轉(zhuǎn)應(yīng)變大小，玲代表導絲的固有彎曲和扭轉(zhuǎn) 屬性，分別對應(yīng)導絲在切面的兩個彎曲固有應(yīng)變和軸向的扭轉(zhuǎn)固有應(yīng)變，Kkk表示剛度張量 的主對角線元素。在本實施例的導絲模型中，假設(shè)導絲模型具有統(tǒng)一的橫截面，于是導絲模 型的剛度張量就可以忽略非對角線元素，其剛度張量K可以通過其對角線元素 Kn、K22、K33來
表示，
，其中Ε為步驟1中提到的楊氏模量，G為剪切 模量，r為導絲半徑。
[0061] 內(nèi)摩擦耗能用來維持導絲模型的穩(wěn)定性，其主要用來模擬空間控制節(jié)點間的摩 擦力，可以通過下式獲得：
[0063] 在這里，γν代表內(nèi)摩擦系數(shù)，Vrel代表空間控制節(jié)點相對平移速度。
[0064] 粘滯力耗能￡^.同樣是用來維持導絲模型的穩(wěn)定性，其主要用來模擬四元數(shù)間的粘 滯力，其可以通過下式獲得：
[0066]其中，代表粘滯力系數(shù)，ω〇代表角速度，ω%表示相對角速度。
[0067]向量值約束補償能用來維持向量值約束，在本實施例導絲模型中，基于補償法 來實現(xiàn)該約束，可以通過下式獲得：
[0069]其中，κ代表補償系數(shù)。
[0070]步驟4:基于所述步驟3得到的能量獲得相應(yīng)的力和力矩，具體地，先將得到的每個 能量進行分解，對分解的部分能量分別進行力和力矩的求解，然后再將每個能量對應(yīng)的力 和力矩進行求和統(tǒng)計；
[0071]在該實施例導絲模型中，力和力矩可以通過下式獲得：
[0073]其中，η代表導絲空間控制節(jié)點和四元數(shù)向量的坐標集合，辦表示η的時間導數(shù)， rin代表由這些能量產(chǎn)生的力和力矩。考慮到微分運算為線性運算，所以在對該公式進行求 解的過程中，可以分別對每個能量進行求解，然后再將其結(jié)果進行相加。在這里，r ιη包含力 和力矩Tin。
[0074] 具體地，在通過上式進行力和力矩求解的過程中，對上述四個能量分別計算相應(yīng) 的力和力矩，計算過程中，將其它的能量賦值為零，并且在計算每個能量相應(yīng)的力和力矩的 過程中，將每個能量拆分成導絲模型的最小單元，對每個最小單元計算出來相應(yīng)的力和力 矩之后，再將其分別加到能量相應(yīng)的全局力和力矩中，最后將所有能量的力和力矩分別進 行求和得到最終能量合力以及合力矩。其中，所述導絲模型的最小單元針對不同的能量有 不同的最小單元。其中，對于彎曲勢能，其最小單元為兩個相鄰四元數(shù)向量;對于內(nèi)摩擦耗 能，其最小單元為兩個相鄰空間控制節(jié)點;對于粘滯力耗能，其最小單元為兩個相鄰四元數(shù) 向量;對于向量值約束補償能，其最小單元為兩個空間控制節(jié)點和空間控制節(jié)點間的一個 四元數(shù)向量。
[0075] 步驟5:利用拉格朗日乘子式法實現(xiàn)所述導絲模型的不可拉伸約束，并計算出由該 不可拉伸約束產(chǎn)生的約束力；
[0076]真實的導絲具有不可拉伸性，為了實現(xiàn)該物理特性，本發(fā)明基于拉格朗日乘子式 法來進行實現(xiàn)，導絲的不可拉伸性是指導絲的長度不會隨著操作而改變，轉(zhuǎn)化到導絲模型 中就是指相鄰的空間控制節(jié)點之間的距離保持不變。
[0077]在本實施例導絲模型中，將不可拉伸約束表示成向量形式C，在該導絲模型中，由 不可拉伸約束產(chǎn)生的內(nèi)力Fit可以表示成JTA，其中，λ是乘子式系數(shù)，J是不可拉伸約束向 量的雅克比矩陣，可以通過|，％為空間控制節(jié)點的向量集合。在這里，λ可以通過 下式獲得：

[0079] 其中，W是空間控制節(jié)點的質(zhì)量矩陣的逆，j表示雅克比矩陣的時間導數(shù)，表示％ 的時間導數(shù)，f代表1^5與導絲所受外力之和，k4Pk d分別代表彈簧和阻尼系數(shù)，亡表示約束 的時間導數(shù)。通過求解該方程可以獲得乘子式系數(shù)λ，之后可以通過/λ獲得由不可拉伸約 束產(chǎn)生的約束力Fg e。
[0080] 步驟6:對所述步驟4和步驟5求得的力和力矩進行求和，并結(jié)合所述步驟4和步驟5 求得的力和力矩，通過歐拉顯式方法對所述導絲模型的參數(shù)進行更新；
[0081] 通過步驟4和步驟5,可以獲得所述導絲模型迭代所需要的全局合力F和合力矩T， 其中合力矩T即為步驟4中求得的Τιη，合力F包括f和Ft，其中所述導絲模型的參數(shù)更新主 要包括導絲的空間控制節(jié)點和四元數(shù)的信息更新?；谝韵鹿剑梢詫Ыz模型的參數(shù) 進行更新：

[0087]其中，h代表時間步長，代表t時刻空間控制節(jié)點i的速度，nu表示空間控制節(jié)點 的質(zhì)量，產(chǎn)表示t時刻導絲的受力，即t時刻求得的合力F，Gf:表示t時刻的空間控制節(jié)點的位 置，代表四元數(shù)j在時刻t時的角速度，I代表慣性張量，泛表示t+h時刻的非單位四元 數(shù)，$表示t時刻的四元數(shù)矩陣，表示代表t時刻的單位四元數(shù)，|| ||表示的絕 對值，代表t時刻的四元數(shù)j的臨時力矩，其可以通過以下式子獲得：
[0089] 其中，A表示力矩的中間變量，i/f表示四元數(shù)矩陣的轉(zhuǎn)置，L表示求得的力矩和。
[0090] 步驟7:循環(huán)調(diào)用步驟3~6來進行導絲的動態(tài)仿真。
[0091] 如圖3(a)_(b)所示，為本發(fā)明導絲模型在不同時間步長情況下，進行推拉操作后 最大誤差的變化情況，圖3(a)為本發(fā)明導絲模型在時間步長為lms的情況下，進行推拉操作 后最大誤差的變化情況，圖3(b)為本發(fā)明導絲模型在時間步長為0.1ms的情況下，進行推拉 操作后最大誤差的變化情況。其中，每次迭代過程中，推拉操作的距離為1_，誤差為2mm左 右，并且該誤差能夠在0.5秒以內(nèi)減少到0.01mm以下。由圖3(a)-(b)可知，本發(fā)明的導絲模 型具有良好的穩(wěn)定性。
[0092] 圖4(a)_(d)和圖5展示了本發(fā)明導絲模型在動脈弓和冠脈分支中的形變效果，其 中，圖4(a)顯示導絲即將進入動脈弓，形變較小，為67mm，圖4(b)顯示導絲開始進入動脈弓， 形變變大，為174mm，圖4(c)顯示導絲進入動脈弓，開始產(chǎn)生較大形變，為277mm，圖4(d)為導 絲完全進入動脈弓，產(chǎn)生180度大形變，為352mm。由圖4(a)-(d)和圖5可知，本發(fā)明導絲模型 具有很好的穩(wěn)定性以及逼真的形變效果。
[0093] 以上所述的具體實施例，對本發(fā)明的目的、技術(shù)方案和有益效果進行了進一步詳 細說明，所應(yīng)理解的是，以上所述僅為本發(fā)明的具體實施例而已，并不用于限制本發(fā)明，凡 在本發(fā)明的精神和原則之內(nèi)，所做的任何修改、等同替換、改進等，均應(yīng)包含在本發(fā)明的保 護范圍之內(nèi)。
【主權(quán)項】
1. 一種針對虛擬微創(chuàng)血管手術(shù)的導絲建模方法，其特征在于，所述方法包括W下步驟： 步驟1:對導絲進行離散化表示，得到導絲模型； 步驟2:對所述導絲模型的參數(shù)進行初始化； 步驟3:基于初始化的導絲模型參數(shù)，獲得所述導絲模型的能量值； 步驟4:基于所述步驟3得到的能量獲得相應(yīng)的力和力矩； 步驟5:利用拉格朗日乘子式法實現(xiàn)所述導絲模型的不可拉伸約束，并計算出由該不可 拉伸約束產(chǎn)生的約束力； 步驟6:對所述步驟4和步驟5求得的力和力矩進行求和，并結(jié)合所述步驟4和步驟5求得 的力和力矩，對所述導絲模型的參數(shù)進行更新； 步驟7:循環(huán)調(diào)用步驟3~6來進行導絲的動態(tài)仿真。2. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述導絲模型通過N個空間控制節(jié)點和N-1 個四元數(shù)來表示，其中，N個空間控制節(jié)點由對導絲離散化得到，N-1個四元數(shù)表示N個空間 控制節(jié)點形成的N-1個中屯、線段的右手正交坐標系。3. 根據(jù)權(quán)利要求2所述的方法，其特征在于，所述空間控制節(jié)點的數(shù)量大于等于3,相鄰 空間控制節(jié)點之間的距離相同或不同。4. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟2中，需要進行初始化的導絲模型 參數(shù)包括導絲的空間控制節(jié)點坐標、四元數(shù)坐標、楊氏模量、剪切模量、導絲質(zhì)量、導絲半 徑、導絲長度、導絲固有彎曲扭轉(zhuǎn)屬性、重力大小和能量公式常量參數(shù)中的一種或多種。5. 根據(jù)權(quán)利要求4所述的方法，其特征在于，導絲模型頭部的楊氏模量小于導絲模型導 絲體部分的楊氏模量。6. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟3中，所述能量值包括彎曲勢能 反韓、內(nèi)摩擦耗能％、粘滯力耗能C訂日向量值約束補償能Ce。7. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟4中，首先將每個能量分別分解為 最小單元，然后對每個最小單元分別計算力和力矩，然后對每個能量對應(yīng)的最小單元力和 力矩分別進行求和得到每個能量對應(yīng)的力和力矩，最后將所有能量的力和力矩分別進行求 和得到所有能量的合力W及力矩之和。8. 根據(jù)權(quán)利要求7所述的方法，其特征在于，彎曲勢能的最小單元為兩個相鄰四元數(shù)向 量；內(nèi)摩擦耗能的最小單元為兩個相鄰空間控制節(jié)點；粘滯力耗能的小單元為兩個相鄰四 元數(shù)向量;向量值約束補償能的最小單元為兩個空間控制節(jié)點和空間控制節(jié)點間的一個四 元數(shù)向量。9. 根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟5進一步包括W下步驟： 步驟51，通過下式得到拉格朗日乘子式系數(shù)λ:其中，W是空間控制節(jié)點的質(zhì)量矩陣的逆，J是不可拉伸約束向量的雅克比矩陣，/表示 雅克比矩陣的時間導數(shù)，％為空間控制節(jié)點的向量集合，嗦Ρ表示％的時間導數(shù)，f代表能量 產(chǎn)生的力與導絲所受外力之和，ks和kd分別代表彈黃和阻尼系數(shù)，C為不可拉伸約束的向量 形式，€表示約束的時間導數(shù)； 步驟52,通過下式得到由不可拉伸約束產(chǎn)生的約束力Flije;10.根據(jù)權(quán)利要求1所述的方法，其特征在于，所述步驟6中，基于W下公式對導絲模型 的參數(shù)進行更新：其中，h代表時間步長，代表t時刻空間控制節(jié)點i的速度，nil表示空間控制節(jié)點的質(zhì) 量，F(xiàn)嗦示t時刻導絲的受力，Gf表示t時刻的空間控制節(jié)點的位置，代表四元數(shù)j在時刻 t時的角速度，I代表慣性張量，Qi + h表示t+h時刻的非單位四元數(shù)，婦I表示t時刻的四元數(shù) 矩陣，0表示代表t時刻的單位四元數(shù)，I暢+i表示巧+'，.的絕對值，巧代表t時刻的四元 數(shù)j的臨時力矩，其可W通過下式獲得：其中，表示力矩的中間變量，表示四元數(shù)矩陣的轉(zhuǎn)置，Tj表示求得的力矩之和。
【文檔編號】G06F19/00GK106096265SQ201610404708
【公開日】2016年11月9日
【申請日】2016年6月8日
【發(fā)明人】謝曉亮, 侯增廣, 高占杰, 邊桂彬, 奉振球, 郝劍龍, 王莉, 周小虎, 程笑冉
【申請人】中國科學院自動化研究所