用于具有三角形遠(yuǎn)心點(diǎn)機(jī)構(gòu)的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)數(shù)值解法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明設(shè)及一種串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)數(shù)值解法，具體設(shè)及一種用于具有Ξ角形遠(yuǎn) 屯、點(diǎn)機(jī)構(gòu)的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)數(shù)值解法，屬于微創(chuàng)醫(yī)療器械領(lǐng)域。
【背景技術(shù)】
[0002] 微創(chuàng)手術(shù)與傳統(tǒng)的開(kāi)口手術(shù)相比具有手術(shù)切口小、術(shù)后瘤痕小、恢復(fù)時(shí)間快等優(yōu) 點(diǎn)，因此微創(chuàng)手術(shù)被廣泛地應(yīng)用于臨床手術(shù)。采用主從控制形式的微創(chuàng)手術(shù)機(jī)器人提高了 微創(chuàng)手術(shù)的精度和應(yīng)用范圍，故而有廣闊的市場(chǎng)前景。微創(chuàng)手術(shù)機(jī)器人的機(jī)械臂通常采用 遠(yuǎn)屯、定點(diǎn)運(yùn)動(dòng)機(jī)構(gòu)來(lái)實(shí)現(xiàn)手術(shù)器械在人體內(nèi)的定位。Ξ角形遠(yuǎn)屯、機(jī)構(gòu)依靠關(guān)節(jié)軸線相交于 定點(diǎn)的方式來(lái)實(shí)現(xiàn)機(jī)構(gòu)的遠(yuǎn)屯、運(yùn)動(dòng)。前兩個(gè)關(guān)節(jié)是轉(zhuǎn)動(dòng)關(guān)節(jié)，軸線相交于遠(yuǎn)屯、點(diǎn)，最后一個(gè) 關(guān)節(jié)是移動(dòng)關(guān)節(jié)，軸線通過(guò)遠(yuǎn)屯、點(diǎn)。在遠(yuǎn)屯、機(jī)構(gòu)后面串聯(lián)腕部Ξ個(gè)關(guān)節(jié)，實(shí)現(xiàn)手術(shù)器械的運(yùn) 動(dòng)。在遠(yuǎn)屯、點(diǎn)機(jī)構(gòu)前面可串聯(lián)數(shù)個(gè)被動(dòng)關(guān)節(jié)，用于手術(shù)前調(diào)整遠(yuǎn)屯、點(diǎn)的位置，手術(shù)中鎖死。 機(jī)械臂逆運(yùn)動(dòng)學(xué)求解是根據(jù)機(jī)械臂在空間內(nèi)的末端位置姿態(tài)矩陣，求解各個(gè)關(guān)節(jié)的位移 量。逆運(yùn)動(dòng)學(xué)求解是實(shí)現(xiàn)機(jī)械臂定位控制和軌跡跟蹤控制的基礎(chǔ)，且逆運(yùn)動(dòng)學(xué)算法直接影 響機(jī)械臂的運(yùn)動(dòng)控制器的性能。逆運(yùn)動(dòng)學(xué)方程是非線性超越方程，其求解過(guò)程非常復(fù)雜，而 且可能存在多解、無(wú)解或者奇異等問(wèn)題。對(duì)于六自由度串聯(lián)機(jī)械臂，僅當(dāng)其結(jié)構(gòu)滿(mǎn)足準(zhǔn) 則一-機(jī)械臂腕部Ξ個(gè)相鄰的關(guān)節(jié)軸相交于一點(diǎn)，或者機(jī)械臂腕部Ξ個(gè)相鄰的關(guān)節(jié)的軸線 相互平行，此時(shí)Ξ個(gè)相鄰關(guān)節(jié)的位置和姿態(tài)是解禪的，可W求出機(jī)械臂逆運(yùn)動(dòng)學(xué)的封閉解。 但一般情況下，考慮到使用性能和設(shè)計(jì)制造和裝配過(guò)程中的困難和誤差，機(jī)械臂都不滿(mǎn)足 運(yùn)一準(zhǔn)則。故而需要采用數(shù)值解法完成機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解。一般采用的數(shù)值解法的基 本原理是根據(jù)機(jī)械臂當(dāng)前的關(guān)節(jié)角度求解正運(yùn)動(dòng)學(xué)的計(jì)算結(jié)果，通過(guò)比較其與目標(biāo)位置姿 態(tài)值的差值，對(duì)關(guān)節(jié)角度進(jìn)行修正，反復(fù)試探并控制定位誤差計(jì)算關(guān)節(jié)角度最優(yōu)值。運(yùn)樣的 方法計(jì)算量大，不利于控制系統(tǒng)的實(shí)時(shí)性。

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

[0003] 本發(fā)明為解決現(xiàn)有微創(chuàng)手術(shù)機(jī)械臂控制系統(tǒng)精度較低、實(shí)時(shí)性較差的問(wèn)題，進(jìn)而 提出具有=角形遠(yuǎn)屯、點(diǎn)機(jī)構(gòu)的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)數(shù)值解法。
[0004] 本發(fā)明為解決上述問(wèn)題采取的技術(shù)方案是:本發(fā)明的具體步驟如下：
[0005] 步驟一、假設(shè)機(jī)械臂末端點(diǎn)與腕部起點(diǎn)〇4重合，即忽略手腕處Ξ個(gè)關(guān)節(jié)，運(yùn)是假設(shè) Α；
[0006] 步驟二、根據(jù)假設(shè)Α，輸入量末端點(diǎn)位姿矩陣的位置分量與0〇坐標(biāo)系下的遠(yuǎn)屯、點(diǎn)位 置矢量相減即為〇0坐標(biāo)系下的第Ξ關(guān)節(jié)的位移量Q3的空間矢量，求出第Ξ關(guān)節(jié)位移量Q3的 中間計(jì)算值，命名為qi3;
[0007] 步驟Ξ、根據(jù)假設(shè)A，由移動(dòng)第Ξ關(guān)節(jié)的軸線方向可W唯一地確定第一關(guān)節(jié)位移量 qi和第二關(guān)節(jié)位移量92的大小，依次求出第一關(guān)節(jié)中間計(jì)算值qii和第二關(guān)節(jié)中間計(jì)算值 qi2；
[0008] 步驟四、在不忽略第四關(guān)節(jié)、第五關(guān)節(jié)、第六關(guān)節(jié)的情況下，利用輸入量末端點(diǎn)位 姿矩陣的姿態(tài)信息和根據(jù)假設(shè)A求出的第一關(guān)節(jié)中間計(jì)算值qii、第二關(guān)節(jié)中間計(jì)算值qi2、 第Ξ關(guān)節(jié)中間計(jì)算值qi3依次求出第四關(guān)節(jié)中間計(jì)算值qi4、第五關(guān)節(jié)中間計(jì)算值qi5、第六關(guān) 節(jié)中間計(jì)算值qi6;
[0009] 步驟五、假設(shè)第四關(guān)節(jié)真實(shí)值q〇4、第五關(guān)節(jié)真實(shí)值q〇5、第六關(guān)節(jié)真實(shí)值q〇6與步驟 四中計(jì)算出的第四關(guān)節(jié)中間計(jì)算值qi4、第五關(guān)節(jié)中間計(jì)算值qi5、第六關(guān)節(jié)中間計(jì)算值qi6相 等，運(yùn)是假設(shè)B;
[0010] 步驟六、在第二關(guān)節(jié)坐標(biāo)系化下，根據(jù)假設(shè)B，用第Ξ關(guān)節(jié)中間計(jì)算值qi3、第四關(guān)節(jié) 中間計(jì)算值qi4、第五關(guān)節(jié)中間計(jì)算值qi5、第六關(guān)節(jié)中間計(jì)算值qi6求出第Ξ關(guān)節(jié)新計(jì)算值 Q23；
[OOW 步驟屯、通過(guò)第；關(guān)節(jié)新計(jì)算值Q23和第四關(guān)節(jié)中間計(jì)算值qi4、第五關(guān)節(jié)中間計(jì)算 值qi5、第六關(guān)節(jié)中間計(jì)算值qi6依次求出第一關(guān)節(jié)新計(jì)算值Q21和第二關(guān)節(jié)新計(jì)算值q22;
[0012] 步驟八、通過(guò)第一關(guān)節(jié)新計(jì)算值Q21、第二關(guān)節(jié)新計(jì)算值Q22、第Ξ關(guān)節(jié)新計(jì)算值Q23 依次求出第四關(guān)節(jié)新計(jì)算值Q24、第五關(guān)節(jié)新計(jì)算值q25、第六關(guān)節(jié)新計(jì)算值Q26 ;
[0013] 步驟九、用第一關(guān)節(jié)新計(jì)算值Q21、第二關(guān)節(jié)新計(jì)算值Q22、第Ξ關(guān)節(jié)新計(jì)算值Q23、第 四關(guān)節(jié)新計(jì)算值024、第五關(guān)節(jié)新計(jì)算值025、第六關(guān)節(jié)新計(jì)算值Q26替代第一關(guān)節(jié)中間計(jì)算值 qii、第二關(guān)節(jié)中間計(jì)算值qi2、第Ξ關(guān)節(jié)中間計(jì)算值qi3、第四關(guān)節(jié)中間計(jì)算值qi4、第五關(guān)節(jié)中 間計(jì)算值qi5、第六關(guān)節(jié)中間計(jì)算值qi6，重新進(jìn)入循環(huán)；
[0014] 步驟十、到達(dá)事先設(shè)定好的循環(huán)次數(shù)，推出循環(huán)，輸出新的一組六個(gè)關(guān)節(jié)的位移。
[0015] 本發(fā)明的有益效果是：1、本發(fā)明求出的六個(gè)關(guān)節(jié)的位移量經(jīng)過(guò)幾次循環(huán)快速逼近 真值，經(jīng)過(guò)機(jī)械臂正運(yùn)動(dòng)學(xué)計(jì)算得到的位置姿態(tài)矩陣的位置分量與輸入的末端點(diǎn)位置姿態(tài) 矩陣的位置分量間的誤差呈指數(shù)速度減小;2、位置姿態(tài)分離求解，先用位置信息，再用姿態(tài) 信息，每一次循環(huán)求出的一組解，可保證再正解出來(lái)的姿態(tài)矩陣沒(méi)有誤差，只有位置矢量有 誤差。即在忽略其他誤差的情況下，運(yùn)用到機(jī)械臂上，末端位置有誤差，姿態(tài)無(wú)誤差;3、求解 過(guò)程在關(guān)節(jié)空間內(nèi)完成，計(jì)算量小。
【附圖說(shuō)明】
[0016] 圖1是本發(fā)明的算法流程圖，圖2是具有Ξ角形遠(yuǎn)屯、機(jī)構(gòu)的六自由度串聯(lián)機(jī)械臂的 坐標(biāo)關(guān)系不意圖，圖3是由qi3、qi4、qi日、qi6求Q23的至間關(guān)系圖，圖2和圖3中A1是第一關(guān)節(jié)軸 線，A2是第二關(guān)節(jié)軸線，A3是第Ξ關(guān)節(jié)軸線，A4是第四關(guān)節(jié)軸線，A5是第五關(guān)節(jié)軸線，A6是第 六關(guān)節(jié)軸線，qi是第一關(guān)節(jié)位移量，Q2是第二關(guān)節(jié)位移量，Q3是第Ξ關(guān)節(jié)位移量，Q4是第四關(guān) 節(jié)位移量，qs是第五關(guān)節(jié)位移量，qs是第六關(guān)節(jié)位移量，qoi第一關(guān)節(jié)真實(shí)值，q〇2是第二關(guān)節(jié) 真實(shí)值，q〇3是第Ξ關(guān)節(jié)真實(shí)值，q〇4是第四關(guān)節(jié)真實(shí)值，q〇5是第五關(guān)節(jié)真實(shí)值，q〇6是第六關(guān)節(jié) 真實(shí)值，qii是第一關(guān)節(jié)中間計(jì)算值，qi2是第二關(guān)節(jié)中間計(jì)算值，qi3是第立關(guān)節(jié)中間計(jì)算值， qi4是第四關(guān)節(jié)中間計(jì)算值，qi日是第五關(guān)節(jié)中間計(jì)算值，qi6是第六關(guān)節(jié)中間計(jì)算值，Q21是第 一關(guān)節(jié)新計(jì)算值，Q22是第二關(guān)節(jié)新計(jì)算值，Q23是跌Ξ關(guān)節(jié)新計(jì)算值，Q24是第四關(guān)節(jié)新計(jì)算 值，Q25是第五關(guān)節(jié)新計(jì)算值，Q26是第六關(guān)節(jié)新計(jì)算值，〇0是參考坐標(biāo)系，〇1是第一關(guān)節(jié)坐標(biāo) 系，〇2是第二關(guān)節(jié)坐標(biāo)系，〇3是第Ξ關(guān)節(jié)坐標(biāo)系，〇4是第四關(guān)節(jié)坐標(biāo)系，〇5是第五關(guān)節(jié)坐標(biāo) 系，〇6是第六關(guān)節(jié)坐標(biāo)系，山第一關(guān)節(jié)桿件偏移量，d3是第Ξ關(guān)節(jié)桿件偏移量，LS是第五關(guān)節(jié) 桿件長(zhǎng)度，L6是第六關(guān)節(jié)桿件長(zhǎng)度，αι是第一關(guān)節(jié)桿件扭轉(zhuǎn)角，02是第二關(guān)節(jié)桿件扭轉(zhuǎn)角。
【具體實(shí)施方式】
[0017]【具體實(shí)施方式】一：結(jié)合圖1至圖3說(shuō)明本實(shí)施方式，本實(shí)施方式所述用于具有Ξ角 形遠(yuǎn)屯、點(diǎn)機(jī)構(gòu)的串聯(lián)機(jī)械臂逆運(yùn)動(dòng)學(xué)數(shù)值解法的具體步驟如下：
[001引步驟一、假設(shè)機(jī)械臂末端點(diǎn)與腕部起點(diǎn)化重合，即忽略手腕處Ξ個(gè)關(guān)節(jié)，運(yùn)是假設(shè) Α；
[0019] 步驟二、根據(jù)假設(shè)Α，輸入量末端點(diǎn)位姿矩陣的位置分量與0〇坐標(biāo)系下的遠(yuǎn)屯、點(diǎn)位 置矢量相減即為〇0坐標(biāo)系下的第Ξ關(guān)節(jié)的位移量Q3的空間矢量，求出第Ξ關(guān)節(jié)位移量Q3的 中間計(jì)算值，命名為qi3;
[0020] 步驟Ξ、根據(jù)假設(shè)A，由移動(dòng)第Ξ關(guān)節(jié)的軸線方向可W唯一地確定第一關(guān)節(jié)位移量 qi和第二關(guān)節(jié)位移量Q2的大小，依次求出第一關(guān)節(jié)中間計(jì)算值qii和第二關(guān)節(jié)中間計(jì)算值 qi2；
[0021] 步驟四、在不忽略第四關(guān)節(jié)、第五關(guān)節(jié)、第六關(guān)節(jié)的情況下，利用輸入量末端點(diǎn)位 姿矩陣的姿態(tài)信息和根據(jù)假設(shè)A求出的第一關(guān)節(jié)中間計(jì)算值qii、第二關(guān)節(jié)中間計(jì)算值qi2、 第Ξ關(guān)節(jié)中間計(jì)算值qi3依次求出第四關(guān)節(jié)中間計(jì)算值qi4、第五關(guān)節(jié)中間計(jì)算值qi5、第六關(guān) 節(jié)中間計(jì)算值qi6;
[0022] 步驟五、假設(shè)第四關(guān)節(jié)真實(shí)值q〇4、第五關(guān)節(jié)真實(shí)值q〇5、第六關(guān)節(jié)真實(shí)值q〇6與步驟 四中計(jì)算出的第四關(guān)節(jié)中間計(jì)算值qi4、第五關(guān)節(jié)中間計(jì)算值qi5、第六關(guān)節(jié)中間計(jì)算值qi6相 等，運(yùn)是假設(shè)