一種基于突加度的障礙物躲避運(yùn)動(dòng)規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及冗余度機(jī)械臂運(yùn)動(dòng)規(guī)劃及控制領(lǐng)域，具體涉及一種基于突加度（加速 度的導(dǎo)數(shù)，Jerk)的障礙物躲避運(yùn)動(dòng)規(guī)劃方法。
【背景技術(shù)】
[0002] 冗余度機(jī)械臂是一種所具有的自由度數(shù)量大于執(zhí)行任務(wù)所需最小自由度數(shù)量的 機(jī)械臂，廣泛地應(yīng)用于各種國民經(jīng)濟(jì)生產(chǎn)活動(dòng)中。冗余度解析是操作冗余度機(jī)械臂中的一 個(gè)重要的問題，即是依據(jù)不同的優(yōu)化性能指標(biāo)和二級(jí)任務(wù)，通過冗余度機(jī)械臂的末端位置 和姿態(tài)來確定機(jī)械臂各個(gè)關(guān)節(jié)最優(yōu)角度的問題。冗余度機(jī)械臂的二級(jí)任務(wù)包括障礙物躲 避、關(guān)節(jié)極限角躲避和重復(fù)運(yùn)動(dòng)，通常被描述成等式約束、不等式約束或者雙端約束。障礙 物躲避是冗余度解析問題中的一個(gè)重要的問題。目前有效的障礙物躲避運(yùn)動(dòng)規(guī)劃方法都是 在速度層或者加速度層上進(jìn)行解析的。然而，這不能滿足某些采用突加度控制冗余度機(jī)械 臂的要求，而且不能考慮到突加度極限，從而可能導(dǎo)致機(jī)械臂在運(yùn)動(dòng)過程中超出突加度極 限。

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

[0003] 本發(fā)明所要解決的技術(shù)問題是提供一種基于突加度的障礙物躲避運(yùn)動(dòng)規(guī)劃方法， 該方法具有計(jì)算量小的特點(diǎn)。
[0004] 為解決上述技術(shù)問題，本發(fā)明通過以下技術(shù)方案予以實(shí)現(xiàn)：
[0005] -種基于突加度的障礙物躲避運(yùn)動(dòng)規(guī)劃方法，包括如下步驟：
[0006] 1)設(shè)計(jì)基于突加度的障礙物躲避運(yùn)動(dòng)性能指標(biāo)，生成二次型優(yōu)化冗余度解析方 案；其中基于突加度的障礙物躲避運(yùn)動(dòng)性能指標(biāo)受約束于突加度的雅可比矩陣等式、基于 突加度的障礙物躲避不等式、關(guān)節(jié)角度極限、關(guān)節(jié)速度極限、關(guān)節(jié)加速度極限和關(guān)節(jié)突加度 極限；
[0007] 2)將步驟1)中所生成的基于突加度的障礙物躲避二次型優(yōu)化冗余度解析方案轉(zhuǎn) 化為二次規(guī)劃問題；
[0008] 3)將步驟2)中的二次規(guī)劃問題運(yùn)用二次規(guī)劃求解器進(jìn)行求解；
[0009] 4)將步驟3)的求解結(jié)果傳遞給下位機(jī)控制器驅(qū)動(dòng)機(jī)械臂運(yùn)動(dòng)。
[0010] 基于突加度的障礙物躲避運(yùn)動(dòng)性能指標(biāo)設(shè)計(jì)為：最小化纊2，受約束于基于 突加度的雅可比矩陣等式= ^、基于突加度的障礙物躲避不等式*// + 2人奮+ )'^<〇, 關(guān)節(jié)角度極限θ < Θ +、關(guān)節(jié)速度極限，關(guān)節(jié)加速度極限f ,關(guān)節(jié) 突加度極限# 谷'其中，谷_TF#/2表示優(yōu)化性能指標(biāo)，故表示機(jī)械臂關(guān)節(jié)突加度，上 標(biāo)T表示矩陣和向量的轉(zhuǎn)置，W為單位矩陣；等式約束J/= ξ表示機(jī)械臂末端運(yùn)動(dòng)軌跡，上 表示機(jī)械臂末端執(zhí)行器的雅可比矩陣，匕=>v-2j/- ,·/?表示機(jī)械臂末端執(zhí)行器突 加度向量，je是上的時(shí)間導(dǎo)數(shù)，Je是·^的時(shí)間導(dǎo)數(shù)，Θ表示機(jī)械臂的關(guān)節(jié)角度，g表示機(jī) 械臂的關(guān)節(jié)速度，盧表示機(jī)械臂的關(guān)節(jié)加速度+ 表示基于突加度的障礙 物躲避不等式約束，Jz、之和J2分別定義如下：
[0011] Jz^-T0UM)
[0012] 人(()）
[0013] j2=-f ,JΛΘ)
[0014] 其中，re =[Jtc-X。,少e -少。,ze-ZtJ , (Xc，yc，Zc)和（X。，y。，Z。）分別是判據(jù)點(diǎn) C 和障礙物點(diǎn)0相對(duì)于機(jī)械臂基座的坐標(biāo)，上（Θ)表示判據(jù)點(diǎn)c處的雅可比矩陣，Jc(60是 上（Θ)的時(shí)間導(dǎo)數(shù)，義(60是,人(0)的時(shí)間導(dǎo)數(shù)，向量與矩陣乘法操作?定義如下：
[0015] s ? V = [S1V1, S2V2，…S1VJt
[0016] 其中，列向量s = [S1, S1，…，sjlp行向量\^表示矩陣V的第i行，i = 1，2,…，1 ; Θ *表示關(guān)節(jié)角上下限，6^表示關(guān)節(jié)速度上下限，歹表示關(guān)節(jié)加速度上下限^^表示關(guān)節(jié) 突加度上下限。
[0017] 上述的障礙物躲避二次型優(yōu)化冗余度解析方案轉(zhuǎn)化為二次規(guī)劃問題，設(shè)計(jì)其性能 指標(biāo)為最小化x TWx/2,受約束于Ax = b，Cx < d，ζ X < ζ +，其中，X =谷'，W為單位矩 陣，A = Je，辦=匕，C = Jz，j = -2U· - Λ0，ζ _和ζ +分別表示合成雙端約束的上下 極限，它們的第j個(gè)元素分別定義為：
[0018] Cs. = max + ^ - Θj),kv{0j + δ - 0f -Oj),Oj }
[0019] ζ ： = min [λ'ρ (θj - θ - θj), Α:ν {0：! - δ - θj),k.t (Oj - Oj),Oj }
[0020] 其中，j表示關(guān)節(jié)序號(hào)，j = 1, 2, . . . , η，η為關(guān)節(jié)數(shù)目，〇 > 0和ε >〇為關(guān)節(jié)轉(zhuǎn)換 裕度，關(guān)節(jié)極限轉(zhuǎn)換參數(shù)kp>0, kv>0和ka>0用來調(diào)節(jié)和保證關(guān)節(jié)突加度夠大的可行域。
[0021] 上述的二次規(guī)劃問題運(yùn)用二次規(guī)劃求解器進(jìn)行求解。
[0022] 將上述的二次規(guī)劃求解器求解結(jié)果轉(zhuǎn)化為電機(jī)驅(qū)動(dòng)所需要的控制信號(hào)，從而驅(qū)動(dòng) 各關(guān)節(jié)電機(jī)使機(jī)械臂完成障礙物躲避運(yùn)動(dòng)。
[0023] 與現(xiàn)有技術(shù)相比，本發(fā)明有如下優(yōu)點(diǎn)：現(xiàn)有有效的障礙物躲避運(yùn)動(dòng)規(guī)劃方法都是 在速度層或者加速度層上進(jìn)行解析的，這不能滿足某些采用突加度控制的冗余度機(jī)械臂的 要求，而且不能考慮到突加度極限，從而可能導(dǎo)致機(jī)械臂在運(yùn)動(dòng)過程中超出突加度極限。本 發(fā)明是一種計(jì)算量小的基于突加度的障礙物躲避運(yùn)動(dòng)規(guī)劃方法。
【附圖說明】
[0024] 圖1為本發(fā)明的流程圖。
[0025] 圖2為實(shí)現(xiàn)本發(fā)明的機(jī)械臂模型圖。
【具體實(shí)施方式】
[0026] 下面結(jié)合附圖對(duì)本發(fā)明做進(jìn)一步的描述，但本發(fā)明的實(shí)施方式并不限于此。
[0027] 圖1所示的一種基于突加度的障礙物躲避運(yùn)動(dòng)規(guī)劃方法，首先提出基于突加度的 障礙物躲避運(yùn)動(dòng)性能指標(biāo)與約束，生成二次型優(yōu)化冗余度解析方案；然后將二次型優(yōu)化冗 余度解析方案轉(zhuǎn)化為二次規(guī)劃問題；接著運(yùn)用二次規(guī)劃求解器求解；最后下位機(jī)控制器根 據(jù)求解結(jié)果控制機(jī)械臂的運(yùn)動(dòng)。
[0028] 圖2所示實(shí)現(xiàn)本發(fā)明的機(jī)械臂為一個(gè)七自由度的機(jī)械臂。該機(jī)械臂由七個(gè)連桿組 成，通過關(guān)節(jié)1、關(guān)節(jié)2、關(guān)節(jié)3、關(guān)節(jié)4、關(guān)節(jié)5、關(guān)節(jié)6和關(guān)節(jié)7組成。
[0029] 本發(fā)明基于突加度的障礙物躲避運(yùn)動(dòng)規(guī)劃方法的二次型優(yōu)化冗余度解析方案設(shè) 計(jì)為：
[0030] 最小化！Ρ μ?/] (1)
[0031] 約束條件：·// = >:; (2)
[0032] JB + 2)β + Jβ < 0 (3)
[0033] θ ^ θ + (4)
[0034] θ <θ<θ' (5)
[0035] θ <θ< θ+ (6)
[0036] θ <θ<θ' (7)
[0037] 其中，#Tr#/2表示優(yōu)化性能指標(biāo)，泛表示機(jī)械臂關(guān)節(jié)突加度，上標(biāo)τ表示矩陣和 向量的轉(zhuǎn)置，W為單位矩陣；等式約束J/ = /;:表示機(jī)械臂末端運(yùn)動(dòng)軌跡，表示機(jī)械臂末 端執(zhí)行器的雅可比矩陣，& =F-2j/-F表示機(jī)械臂末端執(zhí)行器突加度向量，Je 是上的時(shí)間導(dǎo)數(shù)，X是Je的時(shí)間導(dǎo)數(shù)，Θ表示機(jī)械臂的關(guān)節(jié)角度，g表示機(jī)械