專利名稱：實(shí)現(xiàn)多自由度假肢對隨機(jī)目標(biāo)進(jìn)行空間定位的方法
技術(shù)領(lǐng)域：
本發(fā)明涉及肩離斷型殘疾人假肢，尤其涉及上假肢(指上肢假肢)對其隨機(jī)目標(biāo) 進(jìn)行操作空間定位的方法。該方法也可以用在其它具有類似工作情形的機(jī)器人對隨機(jī)目標(biāo) 的操作空間定位上。
背景技術(shù)：
對用在非結(jié)構(gòu)化環(huán)境中的“肩離斷”型殘疾人上假肢，尤其是可實(shí)現(xiàn)代償功能的多 自由度上假肢來說，如何實(shí)現(xiàn)其對隨機(jī)目標(biāo)的操作空間位置實(shí)時(shí)定位，以能按照要求完成 操作任務(wù)，在以往的多自由度上假肢研究中，尚無對該技術(shù)方法的研究。通過本發(fā)明以獲得 求解假肢各關(guān)節(jié)運(yùn)動(dòng)量的必要條件，實(shí)現(xiàn)多自由度上假肢按任務(wù)要求完成操作任務(wù)，是本 領(lǐng)域研究人員亟需解決的問題。
發(fā)明內(nèi)容
本發(fā)明的目的，是為多自由度上假肢對隨機(jī)目標(biāo)能夠?qū)崟r(shí)操作空間定位，提出一 種實(shí)現(xiàn)方法。為達(dá)上述目的，本發(fā)明總的解決方案是，通過采用分別安裝在佩戴者頭部和假肢 上的兩個(gè)三維姿態(tài)傳感器，對佩戴者頭部的空間姿態(tài)相對于上假肢基礎(chǔ)姿態(tài)的變化情況進(jìn) 行檢測，并采用激光測距傳感器對隨機(jī)目標(biāo)進(jìn)行距離檢測，再通過在不同坐標(biāo)系中連續(xù)的 坐標(biāo)變換推演，計(jì)算出隨機(jī)目標(biāo)相對于多自由度上假肢的操作空間定位值。具體方案如下1、在多自由度上假肢上安裝一個(gè)三維姿態(tài)傳感器，該傳感器用作假肢基礎(chǔ)的標(biāo)準(zhǔn) 姿態(tài)校準(zhǔn)；2、在佩戴者的頭部或帽子上安裝另外一個(gè)三維姿態(tài)傳感器，該三維姿態(tài)傳感器用 來檢測佩戴者頭部左右轉(zhuǎn)動(dòng)或上下俯仰時(shí)，其頭部的空間姿態(tài)相對假肢基礎(chǔ)標(biāo)準(zhǔn)姿態(tài)所產(chǎn) 生的變化情況。3、調(diào)整上述兩個(gè)三維姿態(tài)傳感器的初始狀態(tài)，初始狀態(tài)定為當(dāng)佩戴者頭部端正 平視正前方時(shí)，兩個(gè)三維姿態(tài)傳感器的三維姿態(tài)角相同。4、在佩戴者的頭部或帽子上再安裝一個(gè)激光測距傳感器，激光測距傳感器以某種 位姿固定在佩帶者頭部，通過調(diào)整佩戴者頭部左右轉(zhuǎn)動(dòng)和上下俯仰，使該激光測距傳感器 發(fā)出的可見激光束指向需要操作的隨機(jī)目標(biāo)。5、為佩戴者配備一個(gè)假肢控制器，假肢控制器存儲(chǔ)有計(jì)算隨機(jī)目標(biāo)操作空間坐標(biāo) 值P的計(jì)算程序，P的計(jì)算式為P = 二K · 二K · · M(1)其中，S代表肩部坐標(biāo)系(即假肢的基礎(chǔ)坐標(biāo)系)，N代表頸根部正中坐標(biāo)系，H代 表頭部正中坐標(biāo)系，E代表激光測距傳感器的坐標(biāo)系，M代表激光傳感器測得的操作目標(biāo)坐 標(biāo)值，可由激光傳感器實(shí)時(shí)測得，K代表坐標(biāo)變換矩陣。
它們的關(guān)系是在坐標(biāo)系E中描述操作目標(biāo)的坐標(biāo)值M，M值由激光測距傳感器獲得；在坐標(biāo)系H中描述坐標(biāo)系E的位姿，E相對H的位姿變換取決于激光測距傳感器 在頭部的安裝方位，可標(biāo)定，屬于定值，即公式中的標(biāo)定后屬于定值。在坐標(biāo)系N中描述坐標(biāo)系H的位姿，當(dāng)佩戴者頭部端正平視正前方時(shí)的坐標(biāo)系H 相對坐標(biāo)系N的初始方位可標(biāo)定，屬于定值。它主要取決于佩戴者頸部與頭的長度因素， 當(dāng)佩戴者頭部左右轉(zhuǎn)動(dòng)時(shí)，相當(dāng)于坐標(biāo)系H繞坐標(biāo)系N的Zn軸轉(zhuǎn)動(dòng)，坐標(biāo)系H相對坐標(biāo)系 N的姿態(tài)就發(fā)生變化，通過比較兩個(gè)三維姿態(tài)傳感器該方向之間的姿態(tài)角偏差值，可以推導(dǎo) 求得坐標(biāo)系H相對坐標(biāo)系N的姿態(tài)變化幅度，即公式中的; K通過比較兩個(gè)三維姿態(tài)傳感 器之間的相關(guān)姿態(tài)角偏差值，經(jīng)實(shí)時(shí)換算、推導(dǎo)求出。在坐標(biāo)系S中描述坐標(biāo)系N的位姿，當(dāng)佩戴者頭部端正平視正前方時(shí)的坐標(biāo)系N 相對坐標(biāo)系S的初始方位可標(biāo)定，屬于定值，它主要取決于佩戴者肩部的寬度及其與頸部 中央的前后位置參數(shù)。當(dāng)佩戴者頭部上下俯仰時(shí)，相當(dāng)于坐標(biāo)系N繞坐標(biāo)系S的73軸轉(zhuǎn)動(dòng)， 坐標(biāo)系N相對坐標(biāo)系S的姿態(tài)就發(fā)生變化。通過比較兩個(gè)三維姿態(tài)傳感器該方向之間的姿 態(tài)角偏差值，可以推導(dǎo)求得坐標(biāo)系N相對坐標(biāo)系S的姿態(tài)變化幅度，即公式中的^K通過比 較兩個(gè)三維姿態(tài)傳感器之間的相關(guān)姿態(tài)角偏差值，經(jīng)實(shí)時(shí)換算、推導(dǎo)求出。6、每當(dāng)出現(xiàn)需要操作的隨機(jī)目標(biāo)時(shí)，通過佩戴者自主調(diào)整頭部的姿態(tài)，將激光測 距傳感器發(fā)出的激光束對準(zhǔn)目標(biāo)，并由控制器發(fā)出指令，實(shí)時(shí)測出目標(biāo)與激光測距傳感器 間的距離值Μ，同時(shí)由控制器計(jì)算比較前述兩個(gè)三維姿態(tài)傳感器之間的相關(guān)姿態(tài)角偏差值， 求得Jk值與-κ值，控制器再將實(shí)時(shí)獲得的M值^K與-κ值，連同已經(jīng)標(biāo)定的3Κ代入公式 (1)相乘，就可轉(zhuǎn)換為操作目標(biāo)在假肢基礎(chǔ)坐標(biāo)系中實(shí)時(shí)的空間坐標(biāo)值P。本發(fā)明的優(yōu)點(diǎn)在于發(fā)明了一種為工作在非結(jié)構(gòu)化環(huán)境中的“肩離斷”形殘疾人多自由度上假肢對隨 機(jī)目標(biāo)進(jìn)行實(shí)時(shí)空間定位的方法。定位后可為多自由度上假肢實(shí)時(shí)指明目標(biāo)所在的操作空 間位置，以使其完成指定的操作任務(wù)。
參照附圖1、圖2、圖3對本發(fā)明進(jìn)一步說明。圖1是假肢佩戴者與對隨機(jī)目標(biāo)實(shí)時(shí)進(jìn)行空間定位系統(tǒng)的一種初始狀態(tài)的安裝 關(guān)系圖。其中圖1(a)是主視示意圖，圖1(b)是俯視示意圖。圖2是佩戴者頭部前方有一物體時(shí)，佩戴者頭部端平，正好將激光傳感器發(fā)出的 激光束對準(zhǔn)目標(biāo)進(jìn)行測距的示意圖。圖中僅示出頭部或帽子上安裝的三維姿態(tài)傳感器與假 肢基礎(chǔ)上安裝的三維姿態(tài)傳感器的姿態(tài)角無偏差情況。圖3是佩戴者頭部右下前方有一物體時(shí)，佩戴者調(diào)整頭部姿態(tài)將激光測距傳感器 發(fā)出的激光束對準(zhǔn)目標(biāo)進(jìn)行測距的示意圖。圖中僅示出頭部或帽子上安裝的三維姿態(tài)傳感 器與假肢基礎(chǔ)上安裝的三維姿態(tài)傳感器的姿態(tài)角出現(xiàn)偏差，經(jīng)過換算可以推導(dǎo)出頭部姿態(tài) 相對于假肢基礎(chǔ)的姿態(tài)變化情況。圖例說明1_多自由度假肢，2-三維姿態(tài)傳感器，3-三維姿態(tài)傳感器，4-激光測距 傳感器，E-激光測距傳感器的坐標(biāo)系，H-頭部正中的坐標(biāo)系，N-頸根正中的坐標(biāo)系，S-肩部坐標(biāo)系(即假肢的基礎(chǔ)坐標(biāo)系)。維
具體實(shí)施例方式如圖1-3所示，本發(fā)明技術(shù)方案包括在多自由度上假肢1的基礎(chǔ)上安裝-姿態(tài)傳感器2，在佩戴者的頭部帽子上安裝一個(gè)三維姿態(tài)傳感器3，在佩戴者的頭部帽子上 安裝一個(gè)激光測距傳感器4，為佩戴者配備一個(gè)假肢控制器，假肢控制器存儲(chǔ)有可以實(shí)時(shí)計(jì) 算操作隨機(jī)目標(biāo)空間坐標(biāo)值P的計(jì)算程序。P值的計(jì)算式及其代表符號(hào)之間的關(guān)系如技術(shù) 方案中的公式(1)所述。計(jì)算式中建立的各個(gè)坐標(biāo)系的初始姿態(tài)如圖1中所示。下面以圖 1、圖2、圖3說明兩個(gè)對隨機(jī)目標(biāo)實(shí)時(shí)進(jìn)行空間定位的例子。1、首先如圖1所示設(shè)某坐標(biāo)系繞其它某坐標(biāo)系χ軸的旋轉(zhuǎn)角為θ，繞y軸的旋轉(zhuǎn) 角為β，繞ζ軸的旋轉(zhuǎn)角為Φ，則，N坐標(biāo)系繞S坐標(biāo)系xs、ys、zs軸的旋轉(zhuǎn)角就分別記為 θ3、Φ s，H坐標(biāo)系繞N坐標(biāo)系xn、yn、ζη軸的旋轉(zhuǎn)角就分別記為θη、βη、φη，Ε坐標(biāo)系 繞H坐標(biāo)系xh、yh、zh軸的旋轉(zhuǎn)角就分別記為θ h、β h、Cj5h ;設(shè)N坐標(biāo)系原點(diǎn)離開S坐標(biāo)系原 點(diǎn)三方向上的距離分別記為xs、ys、zs，H坐標(biāo)系原點(diǎn)離開N坐標(biāo)系原點(diǎn)三方向上的距離分別 記為xn、yn> zn, E坐標(biāo)系原點(diǎn)離開H坐標(biāo)系原點(diǎn)三方向上的距離分別記為xh、yh、zh。2、通過對佩戴者本身某些參數(shù)進(jìn)行定義與標(biāo)定，假若可獲得如下參數(shù) N坐標(biāo)系相對S坐標(biāo)系初始時(shí)有以下參數(shù)θ s = (T
β, = o° ，φ, = oc
χ==
20mm, ys = 200mm, zs = 50mm,知道N坐標(biāo)系只可以繞S坐標(biāo)系的ys軸轉(zhuǎn)動(dòng)，用齊次方程表 示，則初始時(shí)有
權(quán)利要求
一種實(shí)現(xiàn)多自由度上假肢對隨機(jī)目標(biāo)進(jìn)行操作空間定位的方法，其特征在于，步驟如下第一、在多自由度上假肢上安裝一個(gè)三維姿態(tài)傳感器，該傳感器用作假肢基礎(chǔ)的標(biāo)準(zhǔn)姿態(tài)校準(zhǔn)；第二、在佩戴者的頭部或帽子上安裝另外一個(gè)三維姿態(tài)傳感器，該三維姿態(tài)傳感器用來檢測佩戴者頭部左右轉(zhuǎn)動(dòng)或上下俯仰時(shí)，其頭部的空間姿態(tài)相對假肢基礎(chǔ)標(biāo)準(zhǔn)姿態(tài)所產(chǎn)生的變化情況；第三、調(diào)整上述兩個(gè)三維姿態(tài)傳感器的初始狀態(tài)，初始狀態(tài)定為當(dāng)佩戴者頭部端正平視正前方時(shí)，兩個(gè)三維姿態(tài)傳感器的三維姿態(tài)角相同；第四、在佩戴者的頭部或帽子上再安裝一個(gè)激光測距傳感器，激光測距傳感器以某種位姿固定在佩帶者頭部，通過調(diào)整佩戴者頭部左右轉(zhuǎn)動(dòng)和上下俯仰，使該激光測距傳感器發(fā)出的可見激光束指向需要操作的隨機(jī)目標(biāo)；第五、為佩戴者配備一個(gè)假肢控制器，假肢控制器存儲(chǔ)有計(jì)算操作隨機(jī)目標(biāo)空間坐標(biāo)值P的計(jì)算程序，P的計(jì)算式為 <mrow><mi>P</mi><mo>=</mo><mmultiscripts> <mi>K</mi> <mprescripts/> <mi>N</mi> <mi>S</mi></mmultiscripts><mo>·</mo><mmultiscripts> <mi>K</mi> <mprescripts/> <mi>H</mi> <mi>N</mi></mmultiscripts><mo>·</mo><mmultiscripts> <mi>K</mi> <mprescripts/> <mi>E</mi> <mi>H</mi></mmultiscripts><mo>·</mo><mi>M</mi><mo>-</mo><mo>-</mo><mo>-</mo><mrow> <mo>(</mo> <mn>1</mn> <mo>)</mo></mrow> </mrow>其中，S代表肩部坐標(biāo)系，N代表頸根部正中坐標(biāo)系，H代表頭部正中坐標(biāo)系，E代表激光測距傳感器的坐標(biāo)系，M代表激光傳感器測得的操作目標(biāo)坐標(biāo)值，可由激光傳感器實(shí)時(shí)測得，K代表坐標(biāo)變換矩陣；它們的關(guān)系是在坐標(biāo)系E中描述操作目標(biāo)的坐標(biāo)值M，M值由激光測距傳感器獲得；在坐標(biāo)系H中描述坐標(biāo)系E的位姿，E相對H的位姿變換取決于激光測距傳感器在頭部的安裝方位，可標(biāo)定，屬于定值，即公式中的標(biāo)定后屬于定值；在坐標(biāo)系N中描述坐標(biāo)系H的位姿，當(dāng)佩戴者頭部端正平視正前方時(shí)的坐標(biāo)系H相對坐標(biāo)系N的初始方位可標(biāo)定，屬于定值；它主要取決于佩戴者頸部與頭的長度因素，當(dāng)佩戴者頭部左右轉(zhuǎn)動(dòng)時(shí)，相當(dāng)于坐標(biāo)系H繞坐標(biāo)系N的zn軸轉(zhuǎn)動(dòng)，坐標(biāo)系H相對坐標(biāo)系N的姿態(tài)就發(fā)生變化，通過比較兩個(gè)三維姿態(tài)傳感器該方向之間的姿態(tài)角偏差值，可以推導(dǎo)求得坐標(biāo)系H相對坐標(biāo)系N的姿態(tài)變化幅度，即公式中的通過比較兩個(gè)三維姿態(tài)傳感器之間的相關(guān)姿態(tài)角偏差值，經(jīng)實(shí)時(shí)換算、推導(dǎo)求出；在坐標(biāo)系S中描述坐標(biāo)系N的位姿，當(dāng)佩戴者頭部端正平視正前方時(shí)的坐標(biāo)系N相對坐標(biāo)系S的初始方位可標(biāo)定，屬于定值，它主要取決于佩戴者肩部的寬度及其與頸部中央的前后位置參數(shù)；當(dāng)佩戴者頭部上下俯仰時(shí)，相當(dāng)于坐標(biāo)系N繞坐標(biāo)系S的ys軸轉(zhuǎn)動(dòng)，坐標(biāo)系N相對坐標(biāo)系S的姿態(tài)就發(fā)生變化；通過比較兩個(gè)三維姿態(tài)傳感器該方向之間的姿態(tài)角偏差值，可以推導(dǎo)求得坐標(biāo)系N相對坐標(biāo)系S的姿態(tài)變化幅度，即公式中的通過比較兩個(gè)三維姿態(tài)傳感器之間的相關(guān)姿態(tài)角偏差值，經(jīng)實(shí)時(shí)換算、推導(dǎo)求出；第六、每當(dāng)出現(xiàn)需要操作的隨機(jī)目標(biāo)時(shí)，通過佩戴者自主調(diào)整頭部的姿態(tài)，將激光測距傳感器發(fā)出的激光束對準(zhǔn)目標(biāo)，并由控制器發(fā)出指令，實(shí)時(shí)測出目標(biāo)與激光測距傳感器間的距離值M，同時(shí)由控制器計(jì)算比較前述兩個(gè)三維姿態(tài)傳感器之間的相關(guān)姿態(tài)角偏差值，求得值與值，控制器再將實(shí)時(shí)獲得的M值、與值，連同已經(jīng)標(biāo)定的代入公式(1)相乘，就可轉(zhuǎn)換為目標(biāo)在假肢基礎(chǔ)坐標(biāo)系中實(shí)時(shí)的操作空間坐標(biāo)值P。FSA00000267958900012.tif,FSA00000267958900021.tif,FSA00000267958900022.tif,FSA00000267958900023.tif,FSA00000267958900024.tif,FSA00000267958900025.tif,FSA00000267958900026.tif,FSA00000267958900027.tif
全文摘要
本發(fā)明公開了一種實(shí)現(xiàn)多自由度上假肢對隨機(jī)目標(biāo)進(jìn)行空間定位的方法，它分別在多自由度上假肢上安裝一個(gè)三維姿態(tài)傳感器；在佩戴者的頭部或帽子上安裝另外一個(gè)三維姿態(tài)傳感器；在佩戴者的頭部或帽子上再安裝一個(gè)激光測距傳感器；為佩戴者配備一個(gè)假肢控制器，每當(dāng)出現(xiàn)需要操作的隨機(jī)目標(biāo)時(shí)，通過佩戴者自主調(diào)整頭部的姿態(tài)，將激光測距傳感器發(fā)出的激光束對準(zhǔn)目標(biāo)，并由控制器發(fā)出指令，實(shí)時(shí)測出目標(biāo)與激光測距傳感器間的距離值，同時(shí)由控制器計(jì)算、轉(zhuǎn)換為目標(biāo)在假肢基礎(chǔ)坐標(biāo)系中實(shí)時(shí)的操作空間坐標(biāo)值。本發(fā)明為工作在非結(jié)構(gòu)化環(huán)境中的肩離斷形殘疾人多自由度上假肢對隨機(jī)目標(biāo)進(jìn)行實(shí)時(shí)操作空間定位的方法。
文檔編號(hào)A61F2/68GK101947150SQ201010280508
公開日2011年1月19日 申請日期2010年9月11日 優(yōu)先權(quán)日2010年9月11日
發(fā)明者孫愛芹, 張志獻(xiàn), 樊東哲, 樊炳輝, 江浩, 王傳江 申請人:山東科技大學(xué)