一種五自由度機(jī)械手控制方法
【專(zhuān)利摘要】本發(fā)明公開(kāi)了一種五自由度機(jī)械手控制方法，屬于機(jī)械設(shè)備控制領(lǐng)域?，F(xiàn)有技術(shù)的控制方法繁瑣，控制精度不高，不能有效控制機(jī)械手的運(yùn)動(dòng)。本發(fā)明對(duì)五自由機(jī)械手，所能出現(xiàn)的所有工作情況，將反解所有出現(xiàn)的可能解一一分析，分類(lèi)成八種情況，將其所得的解與其對(duì)應(yīng)進(jìn)行控制。本發(fā)明控制方法簡(jiǎn)單、控制精度高的五自由度機(jī)械手控制方法。
【專(zhuān)利說(shuō)明】
一種五自由度機(jī)械手控制方法
技術(shù)領(lǐng)域
[0001 ]本發(fā)明涉及一種五自由度機(jī)械手控制方法，屬于機(jī)械設(shè)備控制領(lǐng)域。
【背景技術(shù)】
[0002] 機(jī)械手能模仿人手和臂的某些動(dòng)作功能，可以按固定程序抓取、搬運(yùn)物件或操作 工具的自動(dòng)操作裝置。機(jī)械手是最早出現(xiàn)的工業(yè)機(jī)器人，也是最早出現(xiàn)的現(xiàn)代機(jī)器人，它可 代替人的繁重勞動(dòng)以實(shí)現(xiàn)生產(chǎn)的機(jī)械化和自動(dòng)化，能在有害環(huán)境下操作以保護(hù)人身安全， 因而廣泛應(yīng)用于機(jī)械制造、冶金、電子、輕工和原子能等部門(mén)。它可通過(guò)編程來(lái)完成各種預(yù) 期的作業(yè)任務(wù)，在構(gòu)造和性能上兼有人和機(jī)器各自的優(yōu)點(diǎn)，尤其體現(xiàn)了人的智能和適應(yīng)性。 機(jī)械手作業(yè)的準(zhǔn)確性和各種環(huán)境中完成作業(yè)的能力，在國(guó)民經(jīng)濟(jì)各領(lǐng)域有著廣闊的發(fā)展前 景。但是機(jī)械手的控制方法比較繁瑣，需要預(yù)先獲取機(jī)械臂末端運(yùn)動(dòng)軌跡曲線，根據(jù)軌跡曲 線反算各個(gè)機(jī)械手關(guān)節(jié)的轉(zhuǎn)動(dòng)量，將上述得到的轉(zhuǎn)動(dòng)量下發(fā)至運(yùn)動(dòng)控制卡，通過(guò)伺服驅(qū)動(dòng) 系統(tǒng)驅(qū)動(dòng)各臂轉(zhuǎn)動(dòng)，各連桿聯(lián)動(dòng)完成給定的運(yùn)動(dòng)軌跡。但是現(xiàn)有技術(shù)的控制方法繁瑣，控制 精度不高，不能有效控制機(jī)械手的運(yùn)動(dòng)。針對(duì)目前現(xiàn)有技術(shù)中存在的上述缺陷，實(shí)有必要進(jìn) 行研發(fā)，解決現(xiàn)有技術(shù)中存在的缺陷；

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

[0003] 針對(duì)現(xiàn)有技術(shù)的缺陷，本發(fā)明的目的在于提供一種控制方法簡(jiǎn)單、控制精度高的 五自由度機(jī)械手控制方法。
[0004] 為實(shí)現(xiàn)上述目的，本發(fā)明的技術(shù)方案為：
[0005] -種五自由度機(jī)械手控制方法，包括如下步驟：
[0006] S1.獲取機(jī)械臂末端運(yùn)動(dòng)軌跡曲線，根據(jù)曲線特點(diǎn)以及要求上選取N個(gè)末端位置坐 標(biāo)P(Px，Py，Pz)和接近矢量a(ax，ay，az)，方位矢量o(ox，oy，oz)，法向矢量n(nx，ny，nz).
[0007] S2.將獲取的N個(gè)點(diǎn)進(jìn)行運(yùn)動(dòng)學(xué)反解的計(jì)算，得到的每一個(gè)軌跡坐標(biāo)點(diǎn)相對(duì)應(yīng)的各 個(gè)驅(qū)動(dòng)關(guān)節(jié)所對(duì)應(yīng)的一個(gè)角度值，獲得機(jī)械手的運(yùn)動(dòng)軌跡與各關(guān)節(jié)驅(qū)動(dòng)角度之間的一個(gè)關(guān) 系，五個(gè)關(guān)節(jié)角度分別是h%;
[0008] S3.由于反解存在多解的情況，故而需要對(duì)反解的得到的各個(gè)解進(jìn)行分析和判斷， 進(jìn)而選取合理的解，但是取解的情況需要考慮到實(shí)際的工作情況和人為的需求；
[0009] 根據(jù)基座的運(yùn)動(dòng)范圍，將機(jī)械手分為左臂與右臂，左臂的運(yùn)動(dòng)范圍是:0°~180°， 右臂的運(yùn)動(dòng)范圍是:_180°~0°;其次，根據(jù)小臂的運(yùn)動(dòng)范圍將機(jī)械臂分為上肘0 3<9〇°與下 肘03多90° ;最后，對(duì)于最后兩個(gè)決定末端姿態(tài)的驅(qū)動(dòng)關(guān)節(jié)將其分為翻轉(zhuǎn)-180°~0°與非翻 轉(zhuǎn)0° ~180° ;
[0010] 因此，可將反解的多解情況分為八組然后選擇其合理的一組，分別是：左臂上肘翻 轉(zhuǎn)、左臂上肘非翻、左臂下肘翻轉(zhuǎn)、左臂下肘非翻、右臂上肘翻轉(zhuǎn)、右臂上肘非翻、右臂下肘 翻轉(zhuǎn)、右臂下肘非翻；
[0011] S4.將上述得到的轉(zhuǎn)動(dòng)角度下發(fā)至運(yùn)動(dòng)控制卡，通過(guò)伺服驅(qū)動(dòng)系統(tǒng)驅(qū)動(dòng)各臂轉(zhuǎn)動(dòng)， 通過(guò)傳感器檢測(cè)并反饋轉(zhuǎn)動(dòng)信號(hào)，各連桿聯(lián)動(dòng)完成給定的運(yùn)動(dòng)軌跡。
[0012] 進(jìn)一步地，左臂上肘翻轉(zhuǎn)、左臂上肘非翻、左臂下肘翻轉(zhuǎn)、左臂下肘非翻、右臂上肘 翻轉(zhuǎn)、右臂上肘非翻、右臂下肘翻轉(zhuǎn)、右臂下肘非翻，
[0013] 每一組的情況都是通過(guò)坐標(biāo)的旋轉(zhuǎn)、平移得到的齊次變換矩陣：
[0015] 進(jìn)一步地，
[0016] a)首先解出0:，可用逆變換丨廠1 (佚)左乘方程兩邊來(lái)求出:
[0018]即可通過(guò)上式解出該位置坐標(biāo)下p(px，py，pz)的關(guān)節(jié)角度1;
[0019] b)再通過(guò)上面的轉(zhuǎn)換矩陣，可得關(guān)節(jié)角度3的求解方程：
[0021] 其中12，d4分別表示的是大臂和小臂的長(zhǎng)度，px、py、pz表示的是在對(duì)應(yīng)角度下的 位置坐標(biāo)；
[0022] c)通過(guò)在齊次變換矩陣方程兩邊左乘逆變換，'可求得：
[0024]代入之前求解得到的關(guān)節(jié)1和關(guān)節(jié)3的轉(zhuǎn)動(dòng)角度，通過(guò)上式解得關(guān)節(jié)2和關(guān)節(jié)3的轉(zhuǎn) 動(dòng)角度之和；
[0025] d)根據(jù)02 = 023_03,得到相對(duì)應(yīng)的坐標(biāo)下的關(guān)節(jié)2的轉(zhuǎn)動(dòng)角度值；
[0026] e)在步驟c得到的矩陣中，解得：
[0028] 求得的在相應(yīng)坐標(biāo)下的關(guān)節(jié)4的轉(zhuǎn)動(dòng)角度；
[0029] f)在齊次變換矩陣兩邊同時(shí)乘以，-1，得到關(guān)節(jié)5的轉(zhuǎn)動(dòng)角度：
[0031] 求得的在相應(yīng)坐標(biāo)下的關(guān)節(jié)5的轉(zhuǎn)動(dòng)角度。
[0032] 進(jìn)一步地，
[0033] ①左臂
[0034]關(guān)節(jié)1的角度值：
[0036] 對(duì)于關(guān)節(jié)1角度的解，通過(guò)對(duì)以上01、01+3i、01-3i三個(gè)方程進(jìn)行判斷，如在左臂的運(yùn) 動(dòng)范圍0°~180°，就將此值賦給01;
[0037] ②右臂 [0038]關(guān)節(jié)1的角度值：
[0040] 對(duì)于關(guān)節(jié)1角度的解，通過(guò)對(duì)以上01、01+31、01-31三個(gè)方程進(jìn)行判斷，如在左臂的運(yùn) 動(dòng)范圍-180°~0°，就將此值賦給01;
[0041 ] ③上肘
[0042] 關(guān)節(jié)3的角度倌：
[0044]對(duì)于關(guān)節(jié)3角度的解，通過(guò)對(duì)上述兩個(gè)方程03、03+3i的判斷，如若解得在上肘范圍 小于90度，就將此值賦給03;
[0045] ④下肘
[0046] 關(guān)節(jié)3的角度值：
[0048]對(duì)于關(guān)節(jié)3角度的解，通過(guò)對(duì)上述兩個(gè)方程03、03+jt的判斷，如若解得在上肘范圍 大于90度，就將此值賦給03;
[0049] ⑤翻轉(zhuǎn)
[0050]關(guān)節(jié)4,5的角度值：
[0053] 對(duì)于關(guān)節(jié)4,5角度的解，通過(guò)分別對(duì)以上04、04+3t、04-jt和以上05、05+3t、05-jt三個(gè) 方程的判斷，如若解得在翻轉(zhuǎn)的運(yùn)動(dòng)范圍-180°~0°，就將此值賦給關(guān)節(jié)4、5;
[0054] ⑥非翻轉(zhuǎn)
[0055]關(guān)節(jié)4,5的角度值：
[0058] 對(duì)于關(guān)節(jié)4,5角度的解，通過(guò)分別對(duì)以上04、04+3t、04-jt和以上05、05+3t、05-jt三個(gè) 方程的判斷，如若解得在翻轉(zhuǎn)的運(yùn)動(dòng)范圍0°~180°，就將此值賦給關(guān)節(jié)4、5。
[0059]進(jìn)一步地，將各個(gè)關(guān)節(jié)的角度值發(fā)送至運(yùn)動(dòng)控制卡，利用DSP高效的數(shù)據(jù)處理能力 生成直流伺服電機(jī)的驅(qū)動(dòng)信號(hào)，控制直流伺服電機(jī)的速度和轉(zhuǎn)動(dòng)角度;傳感器檢測(cè)到各個(gè) 關(guān)節(jié)轉(zhuǎn)動(dòng)的角度，反饋給運(yùn)動(dòng)控制卡，從而精確的控制運(yùn)動(dòng)軌跡。
[0060] 與現(xiàn)有技術(shù)相比，本發(fā)明具有以下有益效果：
[0061] 本發(fā)明對(duì)五自由機(jī)械手，所能出現(xiàn)的所有工作情況，將反解所有出現(xiàn)的可能解一 一分析，分類(lèi)成八種情況，將其所得的解與其對(duì)應(yīng)進(jìn)行控制。本發(fā)明控制方法簡(jiǎn)單、控制精 度高的五自由度機(jī)械手控制方法。
【附圖說(shuō)明】
[0062] 圖1為本發(fā)明的五自由度機(jī)械手連桿參數(shù)；
[0063] 圖4為本發(fā)明的機(jī)械手各關(guān)節(jié)角度示意圖；
[0064]圖2為本發(fā)明的機(jī)械手控制步驟；
[0065] 圖3為本發(fā)明的機(jī)械手各關(guān)節(jié)控制方法。
【具體實(shí)施方式】
[0066] 為了使本發(fā)明的目的、技術(shù)方案及優(yōu)點(diǎn)更加清楚明白，以下結(jié)合附圖及實(shí)施例，對(duì) 本發(fā)明進(jìn)行進(jìn)一步詳細(xì)說(shuō)明；應(yīng)當(dāng)理解，此處所描述的具體實(shí)施例僅僅用以解釋本發(fā)明，并 不用于限定本發(fā)明；
[0067] 相反，本發(fā)明涵蓋任何由權(quán)利要求定義的在本發(fā)明的精髓和范圍上做的替代、修 改、等效方法以及方案;進(jìn)一步，為了使公眾對(duì)本發(fā)明有更好的了解，在下文對(duì)本發(fā)明的細(xì) 節(jié)描述中，詳盡描述了一些特定的細(xì)節(jié)部分;對(duì)本領(lǐng)域技術(shù)人員來(lái)說(shuō)沒(méi)有這些細(xì)節(jié)部分的 描述也完全理解本發(fā)明；
[0068] -種五自由度機(jī)械手控制方法，包括如下步驟：
[0069] S1.獲取機(jī)械臂末端運(yùn)動(dòng)軌跡曲線，根據(jù)曲線特點(diǎn)以及要求上選取N個(gè)末端位置坐 標(biāo)P(Px，Py，Pz)和接近矢量a(ax，ay，az)，方位矢量o(ox，oy，oz)，法向矢量n(nx，ny，nz).
[0070] S2.將獲取的N個(gè)點(diǎn)進(jìn)行運(yùn)動(dòng)學(xué)反解的計(jì)算，得到的每一個(gè)軌跡坐標(biāo)點(diǎn)相對(duì)應(yīng)的各 個(gè)驅(qū)動(dòng)關(guān)節(jié)所對(duì)應(yīng)的一個(gè)角度值，獲得機(jī)械手的運(yùn)動(dòng)軌跡與各關(guān)節(jié)驅(qū)動(dòng)角度之間的一個(gè)關(guān) 系，五個(gè)關(guān)節(jié)角度分別是H 03,04,05;
[0071] S3.由于反解存在多解的情況，故而需要對(duì)反解的得到的各個(gè)解進(jìn)行分析和判斷， 進(jìn)而選取合理的解，但是取解的情況需要考慮到實(shí)際的工作情況和人為的需求；
[0072]根據(jù)基座的運(yùn)動(dòng)范圍，將機(jī)械手分為左臂與右臂，左臂的運(yùn)動(dòng)范圍是:0°~180°， 右臂的運(yùn)動(dòng)范圍是:_180°~0°;其次，根據(jù)小臂的運(yùn)動(dòng)范圍將機(jī)械臂分為上肘03<9〇°與下 肘03多90° ;最后，對(duì)于最后兩個(gè)決定末端姿態(tài)的驅(qū)動(dòng)關(guān)節(jié)將其分為翻轉(zhuǎn)-180°~0°與非翻 轉(zhuǎn)0° ~180° ;
[0073]因此，可將反解的多解情況分為八組然后選擇其合理的一組，分別是：左臂上肘翻 轉(zhuǎn)、左臂上肘非翻、左臂下肘翻轉(zhuǎn)、左臂下肘非翻、右臂上肘翻轉(zhuǎn)、右臂上肘非翻、右臂下肘 翻轉(zhuǎn)、右臂下肘非翻。
[0074]左臂上肘翻轉(zhuǎn)、左臂上肘非翻、左臂下肘翻轉(zhuǎn)、左臂下肘非翻、右臂上肘翻轉(zhuǎn)、右臂 上肘非翻、右臂下肘翻轉(zhuǎn)、右臂下肘非翻，每一組的情況都是通過(guò)坐標(biāo)的旋轉(zhuǎn)、平移得到的 齊次變換矩陣： fix. Ox (Jx j)x n n , riv Or ax py
[0075] °5t=\t1^ =.....' m 〇z ch pz 0 0 0 1 - 」〇.
[0076] a)首先解出，可用逆變換y-1 (ft)左乘方程兩邊來(lái)求出0::
[0078]即可通過(guò)上式解出該位置坐標(biāo)下p(px，py，pz)的關(guān)節(jié)角度1;
[0079] b)再通過(guò)上面的轉(zhuǎn)換矩陣，可得關(guān)節(jié)角度3的求解方程：
[0081] 其中12，d4分別表示的是大臂和小臂的長(zhǎng)度，px、py、pz表示的是在對(duì)應(yīng)角度下的 位置坐標(biāo)；
[0082] c)通過(guò)在齊次變換矩陣方程兩邊左乘逆變換可求得：
[0084]代入之前求解得到的關(guān)節(jié)1和關(guān)節(jié)3的轉(zhuǎn)動(dòng)角度，通過(guò)上式解得關(guān)節(jié)2和關(guān)節(jié)3的轉(zhuǎn) 動(dòng)角度之和；
[0085] d)根據(jù)02 = 023-03,得到相對(duì)應(yīng)的坐標(biāo)下的關(guān)節(jié)2的轉(zhuǎn)動(dòng)角度值；
[0086] e)在步驟c得到的矩陣中，解得：
[0088] 求得的在相應(yīng)坐標(biāo)下的關(guān)節(jié)4的轉(zhuǎn)動(dòng)角度；
[0089] f)在齊次變換矩陣兩邊同時(shí)乘以〗1^4導(dǎo)到關(guān)節(jié)5的轉(zhuǎn)動(dòng)角度：
[0091] 求得的在相應(yīng)坐標(biāo)下的關(guān)節(jié)5的轉(zhuǎn)動(dòng)角度。
[0092] ①左臂
[0093]關(guān)節(jié)1的角度值：
[0095] 對(duì)于關(guān)節(jié)1角度的解，通過(guò)對(duì)以上01、01+31、01-JT三個(gè)方程進(jìn)行判斷，如在左臂的運(yùn) 動(dòng)范圍0°~180°，就將此值賦給01;
[0096] ②右臂 [0097]關(guān)節(jié)1的角度值：
[0099] 對(duì)于關(guān)節(jié)1角度的解，通過(guò)對(duì)以上0U01+101-3I三個(gè)方程進(jìn)行判斷，如在左臂的運(yùn) 動(dòng)范圍-180°~0°，就將此值賦給01;
[0100] ③上肘
[0101] 關(guān)節(jié)3的角度值：
[0103] 對(duì)于關(guān)節(jié)3角度的解，通過(guò)對(duì)上述兩個(gè)方程03、03+3!的判斷，如若解得在上肘范圍 小于90度，就將此值賦給03;
[0104] ④下肘
[0105] 關(guān)節(jié)3的角度值：
[0107]對(duì)于關(guān)節(jié)3角度的解，通過(guò)對(duì)上述兩個(gè)方程03、03+jt的判斷，如若解得在上肘范圍 大于90度，就將此值賦給03;
[0108] ⑤翻轉(zhuǎn)
[0109] 關(guān)節(jié)4,5的角度值：
[0112] 對(duì)于關(guān)節(jié)4,5角度的解，通過(guò)分別對(duì)以上04、04+3t、04-jt和以上05、05+3t、05-jt三個(gè) 方程的判斷，如若解得在翻轉(zhuǎn)的運(yùn)動(dòng)范圍-180°~0°，就將此值賦給關(guān)節(jié)4、5;
[0113]⑥非翻轉(zhuǎn) [0114]關(guān)節(jié)4,5的角度值：
[0117] 對(duì)于關(guān)節(jié)4,5角度的解，通過(guò)分別對(duì)以上04、04+3t、04-jt和以上05、05+3t、05-jt三個(gè) 方程的判斷，如若解得在翻轉(zhuǎn)的運(yùn)動(dòng)范圍0°~180°，就將此值賦給關(guān)節(jié)4、5。
[0118] S4.將上述得到的轉(zhuǎn)動(dòng)角度下發(fā)至運(yùn)動(dòng)控制卡，通過(guò)伺服驅(qū)動(dòng)系統(tǒng)驅(qū)動(dòng)各臂轉(zhuǎn)動(dòng)， 通過(guò)傳感器檢測(cè)并反饋轉(zhuǎn)動(dòng)信號(hào)，各連桿聯(lián)動(dòng)完成給定的運(yùn)動(dòng)軌跡。
[0119]將各個(gè)關(guān)節(jié)的角度值發(fā)送至運(yùn)動(dòng)控制卡，利用DSP高效的數(shù)據(jù)處理能力生成直流 伺服電機(jī)的驅(qū)動(dòng)信號(hào)，控制直流伺服電機(jī)的速度和轉(zhuǎn)動(dòng)角度;傳感器檢測(cè)到各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng) 的角度，反饋給運(yùn)動(dòng)控制卡，從而精確的控制運(yùn)動(dòng)軌跡。
[0120]以上所述僅為本發(fā)明的較佳實(shí)施例而已，并不用以限制本發(fā)明，凡在本發(fā)明的精 神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明的保護(hù)范圍之內(nèi)。
【主權(quán)項(xiàng)】
1. 一種五自由度機(jī)械手控制方法，其特征在于，包括如下步驟：51. 獲取機(jī)械臂末端運(yùn)動(dòng)軌跡曲線，根據(jù)曲線特點(diǎn)以及要求上選取N個(gè)末端位置坐標(biāo)P (Px，Py，Pz)和接近矢量a(ax，ay，az)，方位矢量o(ox，oy，oz)，法向矢量n(nx，ny，nz).52. 將獲取的N個(gè)點(diǎn)進(jìn)行運(yùn)動(dòng)學(xué)反解的計(jì)算，得到的每一個(gè)軌跡坐標(biāo)點(diǎn)相對(duì)應(yīng)的各個(gè)驅(qū) 動(dòng)關(guān)節(jié)所對(duì)應(yīng)的一個(gè)角度值，獲得機(jī)械手的運(yùn)動(dòng)軌跡與各關(guān)節(jié)驅(qū)動(dòng)角度之間的一個(gè)關(guān)系， 五個(gè)關(guān)節(jié)角度分別是θι，，θ 3，θ4, θ5;53. 由于反解存在多解的情況，故而需要對(duì)反解的得到的各個(gè)解進(jìn)行分析和判斷，進(jìn)而 選取合理的解，但是取解的情況需要考慮到實(shí)際的工作情況和人為的需求； 根據(jù)基座的運(yùn)動(dòng)范圍，將機(jī)械手分為左臂與右臂，左臂的運(yùn)動(dòng)范圍是：〇°~180°，右臂 的運(yùn)動(dòng)范圍是:-180°~0°;其次，根據(jù)小臂的運(yùn)動(dòng)范圍將機(jī)械臂分為上肘θ3<90°與下肘θ3 多90° ;最后，對(duì)于最后兩個(gè)決定末端姿態(tài)的驅(qū)動(dòng)關(guān)節(jié)將其分為翻轉(zhuǎn)-180°~0°與非翻轉(zhuǎn)0° ~180° ; 因此，可將反解的多解情況分為八組然后選擇其合理的一組，分別是：左臂上肘翻轉(zhuǎn)、 左臂上肘非翻、左臂下肘翻轉(zhuǎn)、左臂下肘非翻、右臂上肘翻轉(zhuǎn)、右臂上肘非翻、右臂下肘翻 轉(zhuǎn)、右臂下肘非翻；54. 將上述得到的轉(zhuǎn)動(dòng)角度下發(fā)至運(yùn)動(dòng)控制卡，通過(guò)伺服驅(qū)動(dòng)系統(tǒng)驅(qū)動(dòng)各臂轉(zhuǎn)動(dòng)，通過(guò) 傳感器檢測(cè)并反饋轉(zhuǎn)動(dòng)信號(hào)，各連桿聯(lián)動(dòng)完成給定的運(yùn)動(dòng)軌跡。2. 如權(quán)利要求1所述的一種五自由度機(jī)械手控制方法，其特征在于，左臂上肘翻轉(zhuǎn)、左 臂上肘非翻、左臂下肘翻轉(zhuǎn)、左臂下肘非翻、右臂上肘翻轉(zhuǎn)、右臂上肘非翻、右臂下肘翻轉(zhuǎn)、 右臂下肘非翻， 每一組的情況都是通過(guò)坐標(biāo)的旋轉(zhuǎn)、平移得到的齊次變換矩陣：3. 如權(quán)利要求2所述的一種五自由度機(jī)械手控制方法，其特征在于， a) 首先解出Q1，可用逆變換y-1 0!)左乘方程兩邊來(lái)求出Q1:即可通過(guò)上式解出該位置坐標(biāo)下P (px，py，pz)的關(guān)節(jié)角度1; b) 苒誦忖h而的轉(zhuǎn)換矩陣，可得關(guān)節(jié)角庠3的求解方程：其中12，d4分別表示的是大臂和小臂的長(zhǎng)度，pX、py、pz表示的是在對(duì)應(yīng)角度下的位置 坐標(biāo)； c) 通過(guò)在齊次變換矩陣方程兩邊左乘逆變換?Τ1，可求得：代入之前求解得到的關(guān)節(jié)1和關(guān)節(jié)3的轉(zhuǎn)動(dòng)角度，通過(guò)上式解得關(guān)節(jié)2和關(guān)節(jié)3的轉(zhuǎn)動(dòng)角 度之和； d) 根據(jù)θ2 = θ23-θ3,得到相對(duì)應(yīng)的坐標(biāo)下的關(guān)節(jié)2的轉(zhuǎn)動(dòng)角度值； e) 在步驟c得到的矩陣中，解得：求得的在相應(yīng)坐標(biāo)下的關(guān)節(jié)4的轉(zhuǎn)動(dòng)角度； f) 在齊次變換矩陣兩邊同時(shí)乘以泣^，得到關(guān)節(jié)5的轉(zhuǎn)動(dòng)角度：求得的在相應(yīng)坐標(biāo)下的關(guān)節(jié)5的轉(zhuǎn)動(dòng)角度。4.如權(quán)利要求3所述的一種五自由度機(jī)械手控制方法，其特征在于， ① 左臂 關(guān)節(jié)1的角度值：對(duì)于關(guān)節(jié)1角度的解，通過(guò)對(duì)以上ΘΚΘΙ+^ΘΙ-π三個(gè)方程進(jìn)行判斷，如在左臂的運(yùn)動(dòng)范 圍0°~180°，就將此值賦給Θ1; ② 右臂 關(guān)節(jié)1的角度值：對(duì)于關(guān)節(jié)1角度的解，通過(guò)對(duì)以上ΘΚΘΙ+^θ?-π三個(gè)方程進(jìn)行判斷，如在左臂的運(yùn)動(dòng)范 圍-180°~0°，就將此值賦給Θ1; ③ 上肘 光書(shū)'Qfth任古.對(duì)十夫節(jié)3角度的觶，通過(guò)對(duì)上述購(gòu)個(gè)萬(wàn)程Θ3、θ3+3Τ的判斷，如若解得在上肘范圍小于 90度，就將此值賦給Θ 3; ④ 下肘 關(guān)節(jié)3的角度值：對(duì)于關(guān)節(jié)3角度的解，通過(guò)對(duì)上述兩個(gè)方程Θ3、θ3+π的判斷，如若解得在上肘范圍大于 90度，就將此值賦給Θ 3; ⑤ 翻轉(zhuǎn) 關(guān)節(jié)4,5的角度值：對(duì)于關(guān)節(jié)4,5角度的解，通過(guò)分別對(duì)以上04、04+31、04-31和以上05、05+31、05-31三個(gè)方程 的判折，如若解得在翻轉(zhuǎn)的運(yùn)動(dòng)范圍-180°~0°，就將此值賦給關(guān)節(jié)4、5; ⑥非翻轉(zhuǎn) 關(guān)節(jié)4,5的角度值：對(duì)于關(guān)節(jié)4,5角度的解，通過(guò)分別對(duì)以上04、04+31、04-31和以上05、05+31、05-31三個(gè)方程 的判斷，如若解得在翻轉(zhuǎn)的運(yùn)動(dòng)范圍0°~180°，就將此值賦給關(guān)節(jié)4、5。5.如權(quán)利要求1-4任一所述的一種五自由度機(jī)械手控制方法，其特征在于，將各個(gè)關(guān)節(jié) 的角度值發(fā)送至運(yùn)動(dòng)控制卡，利用DSP高效的數(shù)據(jù)處理能力生成直流伺服電機(jī)的驅(qū)動(dòng)信號(hào)， 控制直流伺服電機(jī)的速度和轉(zhuǎn)動(dòng)角度;傳感器檢測(cè)到各個(gè)關(guān)節(jié)轉(zhuǎn)動(dòng)的角度，反饋給運(yùn)動(dòng)控 制卡，從而精確的控制運(yùn)動(dòng)軌跡。
【文檔編號(hào)】B25J9/16GK106003041SQ201610443388
【公開(kāi)日】2016年10月12日
【申請(qǐng)日】2016年6月17日
【發(fā)明人】楊亮亮, 劉權(quán)慶, 時(shí)軍, 歐陽(yáng)博, 應(yīng)思齊, 胡鑫杰
【申請(qǐng)人】浙江理工大學(xué)