李壯 方貞琪 王興宇 唐樂(lè) 陳章寶

【摘?? 要】?? 機(jī)器人軌跡規(guī)劃技術(shù)是機(jī)器人學(xué)基礎(chǔ)課程中的核心內(nèi)容，是研究機(jī)器人、機(jī)械臂等系統(tǒng)的重要理論基礎(chǔ)，課程體現(xiàn)了較強(qiáng)的實(shí)踐性，理論知識(shí)涉及三維空間，公式推導(dǎo)過(guò)程較為復(fù)雜、抽象。應(yīng)用MATLAB中強(qiáng)大的數(shù)值計(jì)算和可視化功能，結(jié)合軌跡規(guī)劃中理論公式對(duì)機(jī)器人軌跡進(jìn)行規(guī)劃、分析。通過(guò)數(shù)值仿真對(duì)機(jī)器人笛卡爾空間、關(guān)節(jié)空間的軌跡進(jìn)行規(guī)劃，讓學(xué)生直觀地學(xué)到軌跡規(guī)劃理論，掌握從公式推理到數(shù)值仿真的實(shí)踐過(guò)程，增強(qiáng)學(xué)生對(duì)軌跡規(guī)劃的理解，提高學(xué)生的專業(yè)知識(shí)能力，提升教學(xué)效果。

【關(guān)鍵詞】?? 軌跡規(guī)劃；MATLAB；關(guān)節(jié)空間；笛卡爾空間；數(shù)值仿真

Research on Simulation Teaching of Trajectory

Planning of Basic Course of Robotics

Li Zhuang， Fang Zhenqi， Wang Xingyu， Tang Le， Chen Zhangbao

（Bengbu College， Bengbu 233030， China）

【Abstract】??? Robot trajectory planning technology is the key chapter in the basic course of robotics， and it is an important theoretical basis for the research of robot for robot arm and other systems. The course embodies strong practicality， its theoretical knowledge involves three-dimensional space， the theory is relatively complex and abstract， the formula derivation process also is complex. The powerful numerical calculation and visualization functions in MATLAB are applied to plan and analyze the trajectory of the robot in combination with the theoretical formulas in trajectory planning. Through numerical simulation， the trajectory planning of robot cartesian space and joint space is carried out， so that students can intuitively learn the trajectory planning theory， master the practical process from formula reasoning to numerical simulation， enhance students' understanding of trajectory planning， improve their professional knowledge and ability， and enhance the teaching effect.

【Key words】???? trajectory planning; MATLAB; joint space; cartesian space; numerical simulation

〔中圖分類號(hào)〕 TP242.6????????????? ?????? ????? ???〔文獻(xiàn)標(biāo)識(shí)碼〕? A????? ???????????? 〔文章編號(hào)〕 1674 - 3229（2024）02- 0107 - 07

[收稿日期]?? 2023-11-13

[基金項(xiàng)目]?? 安徽省高等學(xué)校省級(jí)質(zhì)量工程項(xiàng)目“機(jī)器人工程新建專業(yè)質(zhì)量提升項(xiàng)目” （2022xjzlts025）；校級(jí)教研項(xiàng)目“新工科背景下‘機(jī)器人學(xué)基礎(chǔ)課程教學(xué)與實(shí)踐能力培養(yǎng)探索與實(shí)踐”（2023jyxm16）；國(guó)家級(jí)大學(xué)生創(chuàng)新創(chuàng)業(yè)訓(xùn)練計(jì)劃項(xiàng)目“疫情下一種只能送餐送藥機(jī)器人”（20221135007）

[作者簡(jiǎn)介]?? 李壯（1994- ），男，碩士，蚌埠學(xué)院電子與電氣工程學(xué)院助教，研究方向：分布式模型預(yù)測(cè)控制、多智能體協(xié)同。

0???? 問(wèn)題提出

機(jī)器人的軌跡規(guī)劃是機(jī)器人學(xué)基礎(chǔ)課程中重難點(diǎn)知識(shí)，也是機(jī)器人運(yùn)動(dòng)學(xué)的重要基礎(chǔ)理論，其對(duì)機(jī)器人控制起到重要作用。機(jī)器人的空間軌跡包括位置、速度以及加速度等基本物理量，其包括笛卡爾空間路徑規(guī)劃和關(guān)節(jié)空間路徑規(guī)劃［1-2］。機(jī)器人進(jìn)行軌跡規(guī)劃的主要目的是在于已知機(jī)器人初、末狀態(tài)位置時(shí)，機(jī)器人如何實(shí)現(xiàn)從初始位置平穩(wěn)地達(dá)到終點(diǎn)位置。軌跡規(guī)劃包括多種方式，在軌跡中存在奇異位置，靠近其位形時(shí)，關(guān)節(jié)速度會(huì)趨于無(wú)窮大，應(yīng)避免此類情況出現(xiàn)；軌跡規(guī)劃過(guò)程中，位置、速度以及加速度等變量需要滿足基本約束條件和自由度參數(shù)，因此機(jī)器人的軌跡規(guī)劃過(guò)程就顯得十分重要［3-4］。

針對(duì)軌跡規(guī)劃問(wèn)題，眾多學(xué)者開(kāi)展了不同的研究。文獻(xiàn)［5］根據(jù)不同類型的軌跡規(guī)劃方法展開(kāi)研究；文獻(xiàn)［6］針對(duì)工業(yè)機(jī)器人應(yīng)用不同的軌跡方法進(jìn)行設(shè)計(jì)仿真，總結(jié)相關(guān)特點(diǎn)；文獻(xiàn)［7］介紹了軌跡規(guī)劃中多項(xiàng)式插值方法和笛卡爾空間軌跡規(guī)劃方法，并針對(duì)能量、時(shí)間等參數(shù)進(jìn)行研究分析、歸納。文獻(xiàn)［8］就如何提高實(shí)驗(yàn)操作的可行性和課程教學(xué)提出了相關(guān)措施方法。

機(jī)器人學(xué)基礎(chǔ)課程中詳細(xì)闡述了笛卡爾空間規(guī)劃和關(guān)節(jié)空間規(guī)劃，其中關(guān)節(jié)空間規(guī)劃包括多項(xiàng)式路徑規(guī)劃、拋物線擬合線性插值路徑規(guī)劃等；笛卡爾空間路徑規(guī)劃包括空間直線位置插值、空間圓弧位置插值以及姿態(tài)插值等［9-10］。本文將對(duì)上述兩種方式進(jìn)行理論分析、推導(dǎo)和數(shù)值仿真，最后對(duì)路徑規(guī)劃內(nèi)容進(jìn)行總結(jié)。

機(jī)器人工程專業(yè)涉及專業(yè)知識(shí)較多，尤其是數(shù)學(xué)計(jì)算與推導(dǎo)部分，成為了眾多學(xué)生的難題，單純的理論計(jì)算在一定程度上削弱了學(xué)生的實(shí)踐能力，進(jìn)而影響到專業(yè)的發(fā)展［11］。本文就機(jī)器人學(xué)基礎(chǔ)課程建設(shè)問(wèn)題在課程中融入實(shí)例仿真，同時(shí)結(jié)合工業(yè)機(jī)器人設(shè)備和視覺(jué)搬運(yùn)機(jī)器人開(kāi)展實(shí)踐操作與學(xué)習(xí)，進(jìn)一步提升學(xué)生的專業(yè)素質(zhì)能力和綜合實(shí)踐能力。

1???? 關(guān)節(jié)空間路徑規(guī)劃

關(guān)節(jié)空間是對(duì)機(jī)器人的各個(gè)關(guān)節(jié)進(jìn)行規(guī)劃，規(guī)劃參數(shù)有位置、角度等。關(guān)節(jié)空間規(guī)劃是以關(guān)節(jié)角度的函數(shù)來(lái)描述機(jī)器人的軌跡，不考慮工作空間坐標(biāo)中的路徑形狀，計(jì)算較為簡(jiǎn)單。圖1為機(jī)械手臂軌跡規(guī)劃圖。

其中虛線表示機(jī)械手臂末端的軌跡，B、S、W、T、G分別表示基坐標(biāo)系、站坐標(biāo)系、腕部坐標(biāo)系、工具坐標(biāo)系以及目標(biāo)坐標(biāo)系，根據(jù)坐標(biāo)相對(duì)位置關(guān)系確立其次坐標(biāo)變換矩陣，進(jìn)行軌跡規(guī)劃。

在關(guān)節(jié)空間軌跡規(guī)劃中重點(diǎn)考慮初始點(diǎn)和目標(biāo)點(diǎn)的位姿、速度以及加速度，其特點(diǎn)是計(jì)算量小、運(yùn)行速度快等。軌跡規(guī)劃示意圖如圖2所示。

在關(guān)節(jié)空間下軌跡規(guī)劃基本步驟為：

（1）定義工具坐標(biāo)系T相對(duì)目標(biāo)坐標(biāo)系G的初始位置、路徑點(diǎn)和終點(diǎn)，并根據(jù)變換方程依次求解各關(guān)節(jié)手臂的旋轉(zhuǎn)轉(zhuǎn)矩R。

（2）應(yīng)用逆向運(yùn)動(dòng)學(xué)將手臂末端點(diǎn)狀態(tài)轉(zhuǎn)換到空間狀態(tài)。

（3）對(duì)所有節(jié)點(diǎn)進(jìn)行規(guī)劃，并得到相應(yīng)平滑曲線。

（4）應(yīng)用正向動(dòng)力學(xué)將關(guān)節(jié)狀態(tài)轉(zhuǎn)換到手臂末端點(diǎn)狀態(tài)，檢查末端點(diǎn)在笛卡爾坐標(biāo)系下軌跡的可行性。

1.1?? 三次多項(xiàng)式

如果使機(jī)器人在一段軌跡上運(yùn)行平穩(wěn)，且路徑軌跡為平滑曲線，由于三次多項(xiàng)式的函數(shù)是連續(xù)的，則對(duì)應(yīng)的關(guān)節(jié)速度和加速度也滿足連續(xù)性，在一定程度上避免了奇異位置。通常情況下，三次多項(xiàng)式包含4個(gè)約束條件，分別為機(jī)器人初始關(guān)節(jié)角度和終點(diǎn)關(guān)節(jié)角度，初、末速度為0，即滿足：

[θt0=θ0，θtf=θf(wàn)θt0=0，θtf=0]????????????? （1）

因此選擇三次多項(xiàng)式為：

[θt=a0+a1t+a2t2+a3t3]????????? （2）

其中a0、a1、a2、a3為常數(shù)參數(shù)。

由公式（2）可知，在任意時(shí)刻，對(duì)其求解一階導(dǎo)數(shù)和二階導(dǎo)數(shù)，對(duì)應(yīng)軌跡的速度和加速度分別為：

[θt=a1+2a2t+3a3t2θt=2a2+6a3t]??????????? （3）?????????????????????????????? 根據(jù)式（1）～式（3）可得

[θ0=a0θf(wàn)=a0+a1tf+a2tf2+a3tf30=a10=a1+2a2tf+3a3tf2]????????? （4）

進(jìn)而求出參數(shù)a0、a1、a2、a3，其分別為：

[a0=θ0a1=0a2=3t2fθf(wàn)-θ0a3=-2t3fθf(wàn)-θ0]?????????????? （5）

則三次多項(xiàng)式插值函數(shù)為

[θt=θ0+3t2fθf(wàn)-θ0t2+3t2fθf(wàn)-θ0t3]? （6）??????????????????? 結(jié)合理論推導(dǎo)和分析，應(yīng)用MATLAB進(jìn)行數(shù)值仿真，其結(jié)果如圖3所示。

由圖3可知，在建立關(guān)節(jié)函數(shù)后，已知起始、終點(diǎn)位姿后，可確立關(guān)節(jié)空軌跡插補(bǔ)曲線。

1.2??? 高次多項(xiàng)式

在路徑規(guī)劃過(guò)程中，當(dāng)考慮約束條件較多時(shí)，則可以應(yīng)用高階多項(xiàng)式作為路徑段進(jìn)行規(guī)劃；如考慮約束條件包括路徑段的起始點(diǎn)和終點(diǎn)位置、速度以及加速度時(shí)，則可采用高階多項(xiàng)式插值函數(shù)進(jìn)行軌跡規(guī)劃，其表達(dá)式為：

[θt=a0+a1t+a2t2+a3t3+a4t4+a5t5]? （7）?????????????????????? 其中a0、a1、a2、a3、a4、a5為常數(shù)參數(shù)；對(duì)式（7）分別求解多階導(dǎo)數(shù)：

[a0=θ0θf(wàn)=a0+a1t+a2t2+a3t3+a4t4+a5t5θ0=a1θf(wàn)=a1+2a2tf+3a3tf2+4a4tf3+5a5tf4θ0=2a2θf(wàn)=2a2+6a3tf+12a4tf2+20a5tf3]? （8）???????????????????? 式（8）為軌跡規(guī)劃的約束條件。通過(guò)求解，高次多項(xiàng)式參數(shù)分別為：

[a0=θ0a1=θ0a2=θ02a3=20θf(wàn)-20θ0-8θf(wàn)+12θ0tf-3θ0-θf(wàn)t2f2t3fa4=30θf(wàn)-30θ0+14θf(wàn)+16θ0tf+3θ0-2θf(wàn)t2f2t4fa5=12θf(wàn)-12θ0-6θf(wàn)+6θ0tf-θ0-θf(wàn)t2f2t5f]?????????????? （9）

在MATLAB中添加Robotics Toobox中的tpoly函數(shù)，對(duì)上述內(nèi)容進(jìn)行仿真分析，結(jié)果如圖4所示。圖（a）為機(jī)器人關(guān)節(jié)移動(dòng)位移，圖（b）為機(jī)器人關(guān)節(jié)速度變化，圖（c）為機(jī)器人關(guān)節(jié)加速度變化曲線。

由圖4可知，在考慮多約束條件時(shí)，機(jī)器人關(guān)節(jié)角度、角速度以及加速度變化滿足對(duì)應(yīng)數(shù)值關(guān)系，軌跡規(guī)劃過(guò)程中未出現(xiàn)奇異位形。

2???? 笛卡爾空間軌跡規(guī)劃

在關(guān)節(jié)空間中進(jìn)行軌跡規(guī)劃時(shí)，要使機(jī)械臂到達(dá)期望的位姿，但其無(wú)法保證末端執(zhí)行器在笛卡爾空間中路徑的線性。因此對(duì)機(jī)器人操作器末端規(guī)劃要求精確時(shí)，則應(yīng)當(dāng)考慮笛卡爾空間路徑規(guī)劃。其規(guī)劃步驟為：

（1）定義工具坐標(biāo)系T相對(duì)目標(biāo)坐標(biāo)系G的初始位置、路徑點(diǎn)和終點(diǎn)，應(yīng)用各節(jié)點(diǎn)位置和旋轉(zhuǎn)列寫(xiě)出各個(gè)節(jié)點(diǎn)位姿。

（2）對(duì)各位置點(diǎn)坐標(biāo)選取曲線進(jìn)行擬合，進(jìn)而對(duì)手臂末端點(diǎn)狀態(tài)進(jìn)行規(guī)劃。

（3）將規(guī)劃好的手臂末端點(diǎn)狀態(tài)的軌跡轉(zhuǎn)換到關(guān)節(jié)空間。

（4）檢查節(jié)點(diǎn)狀態(tài)在關(guān)節(jié)空間下的軌跡是否具備可行性。

2.1?? 直線軌跡規(guī)劃

直線軌跡規(guī)劃是指已知始末兩端位置和姿態(tài)時(shí)，求解軌跡中間點(diǎn)的姿態(tài)和位置，主要包括兩種方法，分別為對(duì)中間位置的分量進(jìn)行直接插補(bǔ)和利用驅(qū)動(dòng)函數(shù)進(jìn)行插補(bǔ)。下面介紹對(duì)中間位置的分量進(jìn)行直接插補(bǔ)。

在空間當(dāng)中存在任意兩點(diǎn)，分別為起始時(shí)A（x0，y0，z0），終點(diǎn)B（xf，yf，zf），其中直線插補(bǔ)次數(shù)為N，插補(bǔ)點(diǎn)的位姿和位置可求得為：

[xi+1=xi+i+1Δxyi+1=yi+i+1Δyzi+1=zi+i+1Δz??? i=0，1，…，N]???? （10）

其中xi、yi、zi為插補(bǔ)點(diǎn)位置，[Δx]、[Δy]、[Δz]為位置增量，其解為：

[Δx=xf-x0N+1Δy=yf-y0N+1Δz=zf-z0N+1]??????????? （11）

由圖5、圖6可知，對(duì)機(jī)械臂末端執(zhí)行器軌跡進(jìn)行直線插補(bǔ)規(guī)劃，其路徑直觀，滿足線性關(guān)系，關(guān)節(jié)角度變化在對(duì)應(yīng)范圍內(nèi)。

2.2?? 圓弧軌跡規(guī)劃

通常圓弧軌跡規(guī)劃包括平面圓弧和空間圓弧，以平面圓弧軌跡規(guī)劃為例。在二維平面中存在任意一點(diǎn)，并作為圓心o，其半徑大小為r、圓心角為b、插補(bǔ)次數(shù)為N，在此圓弧上存在多個(gè)位置，可通過(guò)旋轉(zhuǎn)關(guān)系求解圓弧上點(diǎn)的坐標(biāo)，考慮起始相角為g，其示意圖如圖7所示。

[xA=x0+rsinγyA=y0+rcosγzA=z0 ]??????????? （12）

由圖7可知，其位置關(guān)系滿足：

[xC=x0+rsinγ+βyC=y0+rcosγ+βzC=z0 ]?????? （13）

[Δβ=β/N+1]???????????? （14）

則圓弧上任意一點(diǎn)坐標(biāo)pi（xi，yi，zi）為

[xi=x0+rsinγ+iΔβyi=y0+rcosγ+iΔβzi=z0??? i=0，1，2，…，N+1]??? （15）

根據(jù)上述公式推導(dǎo)和圓弧軌跡規(guī)劃原理，應(yīng)用MATLAB進(jìn)行數(shù)值仿真，其結(jié)果如圖8所示。

如圖8、圖9所示，應(yīng)用圓弧插補(bǔ)方式進(jìn)行軌跡規(guī)劃，插補(bǔ)值在圓弧上，末端執(zhí)行器規(guī)劃過(guò)程中不會(huì)累積偏差，精確度高，位置變化曲線滿足相應(yīng)關(guān)系。

3???? 應(yīng)用案例

為了更好地展示機(jī)械臂末端執(zhí)行器軌跡規(guī)劃過(guò)程，本案例應(yīng)用實(shí)物機(jī)械臂設(shè)備一套，如圖10（a）所示，主要包括視覺(jué)設(shè)備一套、氣泵以及轉(zhuǎn)臺(tái)設(shè)備等。如圖11所示，首先獲取物件位置，即物件坐標(biāo)系，并確立執(zhí)行器末端坐標(biāo)，包括其位置和姿態(tài)，進(jìn)一步對(duì)機(jī)械臂4個(gè)關(guān)節(jié)進(jìn)行控制，設(shè)置相應(yīng)關(guān)節(jié)角。由圖12可知，機(jī)械臂在規(guī)劃好的軌跡上運(yùn)動(dòng)，可實(shí)時(shí)反饋關(guān)節(jié)位置和姿態(tài)。其過(guò)程示意圖分別如圖10（b）、10（c）、10（d）所示，機(jī)械臂末端執(zhí)行器獲取目標(biāo)點(diǎn)，并獲取物件，按照對(duì)應(yīng)軌跡進(jìn)行關(guān)節(jié)轉(zhuǎn)動(dòng)，最后將物件放置目標(biāo)位置，完成軌跡規(guī)劃和物件抓取的任務(wù)。圖13表示關(guān)節(jié)規(guī)劃參數(shù)的設(shè)置。

4???? 結(jié)語(yǔ)

本文通過(guò)理論推導(dǎo)和應(yīng)用MATLAB數(shù)值仿真計(jì)算分析了關(guān)節(jié)空間規(guī)劃和笛卡爾空間規(guī)劃兩種方式，兩類規(guī)劃方法適用于不同情況。將課程知識(shí)與仿真實(shí)例相結(jié)合，并依托學(xué)院相關(guān)視覺(jué)機(jī)械臂開(kāi)展實(shí)際的軌跡規(guī)劃操作，使得學(xué)生深刻理解專業(yè)知識(shí)，掌握軌跡規(guī)劃的概念與實(shí)際工程意義，培養(yǎng)學(xué)生問(wèn)題分析能力和解決問(wèn)題能力，進(jìn)一步激發(fā)機(jī)器人工程專業(yè)學(xué)生學(xué)習(xí)的主觀能動(dòng)性，同時(shí)提升了課堂效率和教學(xué)效果。

［參考文獻(xiàn)］

［1］ 皇甫小峰，鄒光明，劉源泂，等.基于分段插補(bǔ)的焊接軌跡規(guī)劃研究［J］.農(nóng)業(yè)裝備與車輛工程，2023（3）：1-8

［2］ 盧凌霄，董乾鵬，張?zhí)鞓?lè)，等.機(jī)器人運(yùn)動(dòng)學(xué)與運(yùn)動(dòng)規(guī)劃算法綜述［J］.印刷與數(shù)字媒體技術(shù)研究，2023（5）：1-16.

［3］ 胡凌燕，李建華，陳江南，等.機(jī)器人控制基礎(chǔ)與實(shí)踐教程［M］.北京：高等教育出版社，2022.

［4］ 張家琪，馬躍，張智濤，等.一種高速并聯(lián)機(jī)器人的運(yùn)動(dòng)學(xué)建模與軌跡規(guī)劃仿真［J］.機(jī)械設(shè)計(jì)，2023，40（9）：43-48.

［5］ 李尉，鄧朝暉，葛吉民，等.機(jī)器人關(guān)節(jié)空間軌跡規(guī)劃研究進(jìn)展［J］.機(jī)械設(shè)計(jì)與制造工程，2022，51（10）：15-23.

［6］ 范祺，蔡玉強(qiáng)，賈思楠.多種方法的工業(yè)機(jī)器人軌跡規(guī)劃與仿真［J］.華北理工大學(xué)學(xué)報(bào)（自然科學(xué)版），2022，44（4）：60-67.

［7］ 郭萍.工業(yè)機(jī)器人軌跡規(guī)劃研究［J］.中國(guó)新技術(shù)新產(chǎn)品，2022（19）：42-45.

［8］ 楊金鐸，王林波，曾惜，等.自動(dòng)化機(jī)器人軌跡跟蹤與路徑規(guī)劃技術(shù)研究［J］.自動(dòng)化儀表，2022，43（7）：40-45.

［9］ 盧青，郭建國(guó).“新工科”背景下的機(jī)器人軌跡規(guī)劃實(shí)驗(yàn)教學(xué)系統(tǒng)設(shè)計(jì)［J］.實(shí)驗(yàn)室研究與探索，2022，41（5）：147-150+201.

［10］ 董理，楊東，鹿建森.工業(yè)機(jī)器人軌跡規(guī)劃方法綜述［J］.控制工程，2022，29（12）：2365-2374.

［11］ 羅少軒，喬愛(ài)民，王艷春.工業(yè)機(jī)器人關(guān)節(jié)復(fù)合力測(cè)量實(shí)驗(yàn)裝置設(shè)計(jì)及其教學(xué)應(yīng)用［J］.廊坊師范學(xué)院學(xué)報(bào)（自然科學(xué)版），2023，23（2）：108-111.