一種平面冗余度機(jī)器人避障及避奇異的路徑規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及機(jī)器人運(yùn)動(dòng)控制領(lǐng)域，尤其涉及一種平面冗余度機(jī)器人避障及避奇異 的路徑規(guī)劃方法。
【背景技術(shù)】
[0002] 隨著電子產(chǎn)品的小型化和精密化，傳統(tǒng)的機(jī)器人自動(dòng)化裝配不能很好的完成一些 復(fù)雜精細(xì)化裝配任務(wù)，而完全由人工完成此類產(chǎn)品的裝配任務(wù)會(huì)使工作人員的勞動(dòng)強(qiáng)度大 大增加，從而影響產(chǎn)品的裝配質(zhì)量。目前代工廠商開始引入平面型機(jī)器人作為高速生產(chǎn)線 的輔助性設(shè)備，協(xié)助工作人員完成電子產(chǎn)品的裝配作業(yè)，降低工作人員的勞動(dòng)強(qiáng)度并且提 升裝配質(zhì)量。在流水線這種狹小空間內(nèi)協(xié)助工人完成裝配任務(wù)，人機(jī)碰撞的概率大大增加。 這不僅對(duì)工人的生命安全存在很大的威脅，而且對(duì)產(chǎn)品的裝配質(zhì)量也會(huì)有一定的影響。
[0003] 平面冗余度機(jī)器人，是指關(guān)節(jié)數(shù)大于2的平面關(guān)節(jié)型機(jī)器人，在不影響末端位姿的 情況下可以通過其自運(yùn)動(dòng)特性實(shí)現(xiàn)多種運(yùn)動(dòng)優(yōu)化目標(biāo)。在人機(jī)協(xié)作裝配過程中，由于裝配 流水線空間狹小，同一流水線上的協(xié)作者和協(xié)作機(jī)器人的工作空間大面積重疊，再加之此 類機(jī)器人的工作速度較大，使得人機(jī)協(xié)作的風(fēng)險(xiǎn)大大增加。再者奇異構(gòu)形附近冗余度機(jī)器 人容易出現(xiàn)關(guān)節(jié)速度非常大，導(dǎo)致機(jī)器人末端執(zhí)行器偏離預(yù)期軌跡，這將會(huì)對(duì)裝配質(zhì)量有 一定的影響。
[0004] 所以用于小型電子產(chǎn)品的人機(jī)協(xié)作機(jī)器人需要一種可以同時(shí)完成避障、避奇異的 路徑規(guī)劃方法，使其在協(xié)作過程中不僅可以完成對(duì)協(xié)作者的回避，也可完成對(duì)其自身奇異 構(gòu)型的回避從而降低人機(jī)協(xié)作的風(fēng)險(xiǎn)并提高裝配質(zhì)量。目前，平面冗余度機(jī)器人運(yùn)動(dòng)控制 領(lǐng)域內(nèi)，尚無有關(guān)同時(shí)完成避障、避奇異的路徑規(guī)劃方法的報(bào)道。

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

[0005] 本發(fā)明基于如下理論：
[0006] 1、避障的路徑規(guī)劃理論
[0007] 首先計(jì)算障礙物和機(jī)器人連桿之間的最短距離，針對(duì)人機(jī)協(xié)作裝配這類對(duì)路徑規(guī) 劃方法實(shí)時(shí)性要求較高的應(yīng)用場(chǎng)合，提出了一種改進(jìn)的實(shí)時(shí)最小距離計(jì)算法。
[0008] 針對(duì)人機(jī)協(xié)作完成裝配任務(wù)的過程，協(xié)作者可考慮成隨機(jī)的障礙物。平面冗余度 機(jī)器人各關(guān)節(jié)上裝有測(cè)量距離的傳感器，可以實(shí)時(shí)測(cè)量機(jī)器人關(guān)節(jié)和障礙物之間的距離。 在機(jī)器人運(yùn)動(dòng)的平面上，可以將障礙物等效成一個(gè)半徑為R的圓盤，以平面三自由度冗余機(jī) 器人為例，機(jī)器人和障礙物位置示意圖如圖1所示。
[0009] (1)采用本發(fā)明所述方法計(jì)算實(shí)時(shí)最小距離
[0010] 先篩選出不能和障礙物發(fā)生碰撞的桿件
[0011] 本發(fā)明所舉的例子中連桿是由桿件1、桿件2和桿件3構(gòu)成；
[0012] 已知障礙物中心在坐標(biāo)系{〇}中的位置(XtQ，ytQ)，連桿截面為邊長為a的正方形， 障礙物半徑為R，根據(jù)坐標(biāo)系{1}相對(duì)于坐標(biāo)系{〇}的旋轉(zhuǎn)矩陣，得出障礙物中心在坐標(biāo)系 {1}中的位置(Xtl，ytl)，即
[0014] 其中，θχ為關(guān)節(jié)1的轉(zhuǎn)角；
[0015] 若(Xtl，ytl)滿足
[0018] 桿件1為安全桿件，不用計(jì)算其和障礙物距離；
[0019] 若(Xtl，ytl)滿足
[0021] 其中，xA1，yA1為桿件1上A點(diǎn)在坐標(biāo)系{1}中的位置坐標(biāo)，χ Β1為桿件1上B點(diǎn)在坐標(biāo)系 {1}中的位置橫坐標(biāo)；
[0022]則桿件1為可能發(fā)生碰撞的連桿，需計(jì)算其與障礙物之間的距離；
[0023]定義可能發(fā)生碰撞的桿件1上距離障礙物最近的點(diǎn)為標(biāo)志點(diǎn)S，則該連桿上的標(biāo)志 點(diǎn)S在坐標(biāo)系{1}中的坐標(biāo)為(Xtl，0)，標(biāo)志點(diǎn)S和障礙物之間的距離為：
[0024] di= | yAi-yti
[0025] 同理計(jì)算可能發(fā)生碰撞的桿件2和桿件3上的標(biāo)志點(diǎn)和障礙物之間的距離辦和山， 并取dl、d2和d3的最小值，即得到實(shí)時(shí)最小距離dmin。
[0026] (2)采用傳統(tǒng)方法計(jì)算實(shí)時(shí)最小距離
[0027] -般都在基座坐標(biāo)系下完成，障礙物在基座坐標(biāo)系中的坐標(biāo)為(Xt，yt )，桿件1的方 程可以寫成
[0029]其中，XA，yA為桿件1上A點(diǎn)在基座坐標(biāo)系中的坐標(biāo);XB，y B為桿件1上B點(diǎn)在基座坐標(biāo) 系中的坐標(biāo)；
[0030]過障礙物圓心點(diǎn)(Xt，yt)并垂直于桿件1的直線方程為
[0032]將公式(5)和(6)聯(lián)立解出碰撞點(diǎn)(xs，ys)，進(jìn)一步得出碰撞點(diǎn)到障礙物距離cU，
[0034] 同理計(jì)算出桿件2和桿件3上碰撞點(diǎn)到障礙物距離山和出，進(jìn)而計(jì)算出實(shí)時(shí)最小距 1? dmin 〇
[0035] 本發(fā)明采用改進(jìn)的距離計(jì)算方法，與傳統(tǒng)的實(shí)時(shí)最小距離計(jì)算方法相比，提高了 計(jì)算效率，更好地滿足了動(dòng)態(tài)避障對(duì)實(shí)時(shí)性的要求，主要體現(xiàn)在兩個(gè)方面：
[0036]①在計(jì)算桿件距離之前，先篩選出可能發(fā)生碰撞的連桿，只計(jì)算此類桿件和障礙 物的距離，大大減小了計(jì)算量；
[0037]②改進(jìn)的距離計(jì)算方法采用側(cè)坐標(biāo)變換的方法計(jì)算距離，舍棄了傳統(tǒng)方法中解方 程組的繁瑣計(jì)算，大大減少了計(jì)算量，提高了計(jì)算效率。
[0038] (3)避障的路徑規(guī)劃
[0039] 在人機(jī)協(xié)作完成產(chǎn)品裝配任務(wù)時(shí)，人與機(jī)器人的工作空間相互重疊，碰撞發(fā)生的 可能性大大增加，再加之機(jī)器人運(yùn)動(dòng)至奇異位置附近時(shí)，關(guān)節(jié)速度會(huì)突然增大甚至超出關(guān) 節(jié)速度限制，對(duì)協(xié)同工作的人員造成極大的安全隱患。避障避奇異路徑規(guī)劃方法可以在對(duì) 障礙物回避的同時(shí)完成對(duì)奇異構(gòu)型的回避，從而可以消除這些安全隱患，進(jìn)一步提高人機(jī) 協(xié)作的安全性。
[0040] 機(jī)器人運(yùn)動(dòng)過程中的某一時(shí)刻，通過上述判斷危險(xiǎn)桿件的方法，篩選出危險(xiǎn)桿件 并求出桿件上的標(biāo)志點(diǎn)S的位置，且該標(biāo)志點(diǎn)S處的雅可比矩陣Js滿足
[0041] J sq = ts (8)
[0042] 其中，JSeRmXn為最靠近障礙物的標(biāo)志點(diǎn)處的雅可比矩陣，扣iT1為關(guān)節(jié)速度向 量，4為標(biāo)志點(diǎn)的運(yùn)動(dòng)速度向量；
[0043] 根據(jù)梯度投影法，冗余度機(jī)器人逆解為
[0044] p + {l-.rf)H (9)
[0045] 其中，聲為笛卡爾空間中機(jī)器人末端速度，I為單位矩陣，機(jī)器人的雅克 比矩陣，J+￡RnXm為雅可比矩陣的廣義逆，Η為冗余度機(jī)器人零空間避障函數(shù)；
[0046] 冗余度機(jī)器人零空間避障函數(shù)可由下式求得
[0047] Η -[./,((7-.；/)) ](W±) no)
[0048] 其中，*eiTxl為機(jī)器人末端的運(yùn)動(dòng)速度向量；
[0049] 那么基于梯度投影法的冗余度機(jī)器人避障運(yùn)動(dòng)學(xué)逆解為
[0050] q = J+x + (I - l - J+J )Jj+fx:; - J,rkj (11)
[0051 ]由于(I-J+J)的特殊性，引入避障系數(shù)a和避障增益b，公式(11)可寫成
[0052] q ^J+x+a\js ((J- ./+·/))?+ (bxs -JsJ+x) (12)
[0053] 上式中系數(shù)a，b和實(shí)時(shí)最小距離dmin有關(guān)；
[0054] 定義兩個(gè)距離dKcb，當(dāng)dmin〈d2時(shí)為危險(xiǎn)區(qū)域，啟動(dòng)避障策略，當(dāng)cUXcU時(shí)桿件為極 度危險(xiǎn)區(qū)域，開啟避障增益策略；
[0057]對(duì)公式(12)求積分后，換算成關(guān)節(jié)轉(zhuǎn)角。
[0058] 2、避奇異的路徑規(guī)劃理論
[0059] 機(jī)器人在奇異位置附近時(shí)關(guān)節(jié)速度容易超限，故引入DLS法對(duì)其進(jìn)行優(yōu)化；
[0060] 首先對(duì)機(jī)器人雅可比矩陣進(jìn)行奇異值分解：
[0061] J=ABCT (15)
[0062] 其中，矩陣A和矩陣C都是正交矩陣，矩陣B由機(jī)器人雅可比矩陣的奇異值組成的對(duì) 角矩陣：
[0064] 其中0^02，··· ^為機(jī)器人雅克比矩陣的奇異值，且ο: 2 σ2 2…2 om，om是雅可比矩 陣的最小奇異值，可以體現(xiàn)機(jī)器人接近奇異構(gòu)形的程度；
[0065] 針對(duì)冗余度機(jī)器人求逆解一般使用偽逆法，偽逆法的實(shí)質(zhì)就是在保證末端軌跡誤 差最小的如提下求解關(guān)節(jié)速度的最小范數(shù)解，即末端跟蹤精度的優(yōu)先級(jí)尚于關(guān)機(jī)速度的優(yōu) 先級(jí);偽逆法為了保證跟蹤精度最小，而沒有考慮關(guān)節(jié)速度，為了使機(jī)械臂在奇異構(gòu)型附近 的關(guān)節(jié)速度不至于過大，使用DLS法，引入阻尼系數(shù)C，用以調(diào)節(jié)末端跟蹤精度和關(guān)節(jié)速度的 優(yōu)先級(jí)別，即在保證
[0066] |^-,/^~| + (c||4||) (17)
[0067] 最小的前提下，求解關(guān)節(jié)速度的最小范數(shù)解，
[0068] 則機(jī)器人運(yùn)動(dòng)學(xué)逆解可以寫成
[0069] ^ = = 十Γ:/) 1 尸 (18)
[0070] 其中，1 =