一種雙臂機(jī)器人協(xié)調(diào)操作的路徑規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及雙臂機(jī)器人協(xié)調(diào)操作的路徑規(guī)劃方法，屬于雙臂路徑規(guī)劃技術(shù)領(lǐng)域。
【背景技術(shù)】
[0002] 隨著科技的不斷發(fā)展，機(jī)械臂廣泛應(yīng)用于工廠車間的加工、裝配以及空間站的組 裝、維護(hù)等工作，機(jī)械臂技術(shù)已經(jīng)成為重要研宄方向。近年來，雙臂機(jī)器人已成為機(jī)器人領(lǐng) 域中的研宄熱點(diǎn)之一。協(xié)調(diào)操作路徑規(guī)劃算法是雙臂機(jī)器人控制中的關(guān)鍵技術(shù)，因此，對(duì)雙 臂機(jī)器人協(xié)調(diào)操作路徑規(guī)劃算法進(jìn)行研宄具有重要的意義。
[0003] 目前，雙臂機(jī)器人的主要研宄內(nèi)容為運(yùn)動(dòng)規(guī)劃，協(xié)調(diào)控制算法和操作力（力矩）等 方面。但這些方法的計(jì)算效率都比較低。

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

[0004] 有鑒于此，本發(fā)明提供了一種雙臂機(jī)器人協(xié)調(diào)操作的路徑規(guī)劃方法，能夠?qū)崿F(xiàn)雙 臂的協(xié)調(diào)路徑規(guī)劃。
[0005] 為了達(dá)到上述目的，本發(fā)明的技術(shù)方案為：雙臂末端分別連接一個(gè)剪刀的兩柄，雙 臂機(jī)器人的臂為二連桿結(jié)構(gòu)，二連桿的連接處為一個(gè)關(guān)節(jié)，該方法包括以下步驟：
[0006] 步驟1、將剪刀兩柄分別記為C柄和D柄，以兩柄連接處為原點(diǎn)建立兩柄相連接處 三維坐標(biāo)系為{〇T}，以C柄質(zhì)心為原點(diǎn)建立三維質(zhì)心系{0J，以D柄的質(zhì)心為原點(diǎn)建立三維 質(zhì)心系{〇D}，以左臂末端為原點(diǎn)建立三維坐標(biāo)系{0J，以右臂末端為原點(diǎn)建立三維坐標(biāo)系 {〇K};慣性系為{〇。}。
[0007] 獲取雙臂關(guān)節(jié)的初始關(guān)節(jié)角、剪刀初始夾角以及剪刀終止夾角。
[0008] 同時(shí)設(shè)置操作剪刀的整個(gè)運(yùn)動(dòng)規(guī)劃過程中的總時(shí)間tz、加速時(shí)間ta和時(shí)間間隔 tQ，由Kn=ta/t。求得路徑規(guī)劃的總步數(shù)Kn。
[0009] 步驟2、求解剪刀初始夾角與終止夾角的角度差0n。
[0010] 步驟3、采用帶拋物線過渡的速度梯形法對(duì)角度差0n的變化速度進(jìn)行規(guī)劃，獲得 剪刀左、右柄質(zhì)心角速度的梯形速度曲線《。、《 d;
[0011] 步驟4、利用公￥
【主權(quán)項(xiàng)】
1. 一種雙臂機(jī)器人協(xié)調(diào)操作的路徑規(guī)劃方法，雙臂末端分別連接一個(gè)剪刀的兩柄，所 述雙臂機(jī)器人的臂為二連桿結(jié)構(gòu)，二連桿的連接處為一個(gè)關(guān)節(jié)，其特征在于，包括以下步 驟： 步驟1、將所述剪刀兩柄分別記為C柄和D柄，以兩柄連接處為原點(diǎn)建立兩柄相連接處 三維坐標(biāo)系為{〇τ}，以C柄質(zhì)心為原點(diǎn)建立三維質(zhì)心系{0J，以D柄的質(zhì)心為原點(diǎn)建立三維 質(zhì)心系{〇 D}，以左臂末端為原點(diǎn)建立三維坐標(biāo)系{0J，以右臂末端為原點(diǎn)建立三維坐標(biāo)系 {〇κ};慣性系為{〇。}; 獲取雙臂關(guān)節(jié)的初始關(guān)節(jié)角、剪刀初始夾角以及剪刀終止夾角； 同時(shí)設(shè)置操作剪刀的整個(gè)運(yùn)動(dòng)規(guī)劃過程中的總時(shí)間tz、加速時(shí)間ta和時(shí)間間隔t ^，由 Kn= t a/t。求得路徑規(guī)劃的總步數(shù)K n; 步驟2、求解剪刀初始夾角與終止夾角的角度差θη; 步驟3、采用帶拋物線過渡的速度梯形法對(duì)角度差θ η的變化速度進(jìn)行規(guī)劃，獲得剪刀 左、右柄質(zhì)心角速度的梯形速度曲線ω。、《d; 步驟4、利用公式根據(jù)剪刀柄質(zhì)心的角速度ω。、Od，求得剪刀柄質(zhì)心的 線速度曲線ν。、vd;
其中#和C分別為Pt。和Ptd在慣性系下的表示，Pt。為KU相對(duì)于i〇 T}的相對(duì)位置 矢量，Ptd為{〇D}相對(duì)于{〇τ}的相對(duì)位置矢量：
所述Rra為Ι〇τ} 相對(duì)于IOcJ的姿態(tài)變換矩陣； 步驟5、將上述求解得到的剪刀柄質(zhì)心角速度曲線ω。、和線速度曲線Ve、vd，代入雙 臂協(xié)調(diào)運(yùn)動(dòng)的約束方程
，分別求得左臂末端的線速度曲線V1 和角速度曲線Q1，右臂末端的線速度曲線\和角速度曲線ω ^ 式中，被、以分別為瓔和C的反對(duì)稱矩陣，^和^分別為。和Pkd在慣性系下 的表示，禮>=凡Zc，C =?? ; IV為{0J相對(duì)于{0J的姿態(tài)變換矩陣，Pm為{〇 J相 對(duì)于IOJ的相對(duì)位置矢量，Rrai為{〇κ}相對(duì)于IOJ的姿態(tài)變換矩陣，Pkd為IOJ相對(duì)于{〇 κ} 的相對(duì)位置矢量； 步驟6、針對(duì)路徑規(guī)劃中的當(dāng)前步數(shù)k，求解當(dāng)前步數(shù)對(duì)應(yīng)的時(shí)間tk，利用tk在左臂末端 的線速度曲線V1和角速度曲線ω i，右臂末端的線速度曲線^和角速度曲線中找到當(dāng) 前步數(shù)的左臂末端線速度v1(k)和角速度ω 1(k)、右臂末端線速度'(k)和角速度ω Hk);其中k 的初始值為1 ; 利用公式
揮算獲得路徑規(guī)劃中當(dāng)前步數(shù)k下左臂關(guān)節(jié)角速度A以及 右臂關(guān)節(jié)角速度t Λμ , 分別為左、右臂的雅可比矩陣，尤_ ，名…=[^n，~J ; 步驟7、利用屯⑷、，求解路徑規(guī)劃中下一步數(shù)k+Ι的關(guān)節(jié)角度，下一步數(shù)k+Ι下左 臂關(guān)節(jié)角度^+d) +么⑷％以及右臂關(guān)節(jié)角度4_ =巧a) +IaA ;其中θ ιω為當(dāng)前 步數(shù)k下左臂關(guān)節(jié)角度，Θ Hk)為當(dāng)前步數(shù)k下右臂關(guān)節(jié)角度；其中，其中雙臂初始關(guān)節(jié)角已 知； 步驟8、判斷當(dāng)前步數(shù)k是否達(dá)到總步數(shù)Kn，若已達(dá)到總步數(shù)，則路徑規(guī)劃結(jié)束，若沒有 達(dá)到總步數(shù)則k自增1，返回步驟6。
【專利摘要】本發(fā)明公開了一種雙臂機(jī)器人協(xié)調(diào)操作的路徑規(guī)劃方法，雙臂末端連接一個(gè)剪刀的兩柄，首先獲取雙臂初始關(guān)節(jié)角、剪刀初始夾角以及剪刀終止夾角，并獲取路徑規(guī)劃的總步數(shù)Kn；采用帶拋物線過渡的速度梯形法對(duì)角度差θn規(guī)劃獲得剪刀左、右柄質(zhì)心角速度的梯形速度曲線ωc、ωd；利用速度約束公式，根據(jù)剪刀柄質(zhì)心的角速度ωc、ωd，求得剪刀柄質(zhì)心的線速度曲線vc、vd；將ωc、ωd和vc、vd，代入雙臂協(xié)調(diào)運(yùn)動(dòng)的約束方程，分別求得左臂末端的線速度曲線vl和角速度曲線ωl，右臂末端的線速度曲線vr和角速度曲線ωr；根據(jù)路徑規(guī)劃中的當(dāng)前步數(shù)對(duì)應(yīng)的時(shí)間，獲取左右臂末端的角速度和線速度，并根據(jù)約束公式求解左右臂關(guān)節(jié)角速度；然后求解下一步數(shù)k+1的關(guān)節(jié)角度，由此順次求解步數(shù)1～Kn的關(guān)節(jié)角度。
【IPC分類】G05D1-10
【公開號(hào)】CN104750116
【申請(qǐng)?zhí)枴緾N201510181789
【發(fā)明人】張海濤, 柴玉強(qiáng), 曲漢偉, 王宏, 于鵬
【申請(qǐng)人】中國航天科技集團(tuán)公司第五研究院第五一三研究所
【公開日】2015年7月1日
【申請(qǐng)日】2015年4月16日