Arduino環(huán)境下冗余度機(jī)械臂E47算法的實現(xiàn)

李克訥 王溫鑫 胡旭初 馬玉如 賀之祥 袁偉明 葉洪濤

摘? 要：針對目前機(jī)械臂運(yùn)動規(guī)劃與控制系統(tǒng)成本較高，機(jī)器人技術(shù)難以普及等問題，選擇8位單片機(jī)實現(xiàn)機(jī)械臂運(yùn)動規(guī)劃算法;為提高機(jī)械臂執(zhí)行任務(wù)的實時性，提出了一種基于Simulink的機(jī)械臂運(yùn)動規(guī)劃硬件實現(xiàn)模型。首先，對機(jī)械臂進(jìn)行運(yùn)動學(xué)分析，建立數(shù)學(xué)模型;其次，將機(jī)械臂的運(yùn)動規(guī)劃問題轉(zhuǎn)化成二次規(guī)劃問題，同時引入誤差補(bǔ)償函數(shù)以減小機(jī)械臂運(yùn)動過程中產(chǎn)生的位置誤差;在Arduino Mega2560平臺下，利用E47數(shù)值算法對機(jī)械臂運(yùn)動規(guī)劃問題進(jìn)行求解;利用Matlab對機(jī)械臂執(zhí)行圓形和正方形軌跡跟蹤任務(wù)進(jìn)行仿真實驗;最后，通過硬件實現(xiàn)模型對機(jī)械臂執(zhí)行正方形軌跡進(jìn)行實物驗證。結(jié)果表明：8位單片機(jī)對機(jī)械臂運(yùn)動規(guī)劃算法的實現(xiàn)具有可行性和可適用性，在硬件實現(xiàn)模型中具有有效性，為后續(xù)更好地研究機(jī)械臂運(yùn)動規(guī)劃提供了參考。

關(guān)鍵詞：8位單片機(jī);機(jī)械臂運(yùn)動規(guī)劃;E47數(shù)值算法;誤差補(bǔ)償;硬件實現(xiàn)模型

中圖分類號：TP241;TP183? ? ? ? ?DOI：10.16375/j.cnki.cn45-1395/t.2022.02.011

0? ? 引言

一般機(jī)器人運(yùn)動規(guī)劃與控制系統(tǒng)成本較高，不利于開展機(jī)器人實操教學(xué)。與其他控制器相比，8位單片機(jī)價格低，通用性強(qiáng)，運(yùn)算精度能滿足研究者的基本需求，因此，將其應(yīng)用于機(jī)器人運(yùn)動規(guī)劃與控制系統(tǒng)設(shè)計，可以降低學(xué)習(xí)成本，有利于機(jī)器人技術(shù)的普及和發(fā)展。冗余度機(jī)械臂運(yùn)動較為靈活，具有避障、躲避關(guān)節(jié)極限[1-2]等能力，是很多教師在教學(xué)方面的研究對象。在冗余度機(jī)械臂的運(yùn)動規(guī)劃過程中，逆運(yùn)動學(xué)求解是一個不容忽視的問題。目前，求解逆運(yùn)動學(xué)問題的方法很多，其中的偽逆法[3-4]應(yīng)用廣泛，但該方法計算量大，求逆復(fù)雜且存在矩陣奇異的問題。為提高計算效率，學(xué)者們提出將逆運(yùn)動學(xué)問題轉(zhuǎn)化為二次規(guī)劃問題（QP）進(jìn)行求解[5-7]。二次規(guī)劃問題可與線性變分不等式（LVI）進(jìn)行等價轉(zhuǎn)換，然后通過遞歸神經(jīng)網(wǎng)絡(luò)或者數(shù)值算法對LVI進(jìn)行求解[8-10]，最終得到QP的最優(yōu)解。在實際應(yīng)用中機(jī)械臂通過8位單片機(jī)對其電機(jī)或舵機(jī)進(jìn)行控制，但神經(jīng)網(wǎng)絡(luò)求解器需要求解常微分方程，不便于C/C++等進(jìn)行程序的編程。因此，對于實際機(jī)械臂的逆運(yùn)動學(xué)求解，本文采用了文獻(xiàn)[11]所提出的核心方程，即E47數(shù)值算法[12]。該方法在求解機(jī)械臂的逆運(yùn)動學(xué)問題時，能夠降低計算復(fù)雜性，適用于低性能的8位單片機(jī)系統(tǒng)。由于8位單片機(jī)對算法運(yùn)算的實時性不理想，使得機(jī)械臂在執(zhí)行任務(wù)時由于操作時間間隔長，不能連續(xù)完成指定任務(wù)，有可能會因為外界原因?qū)е聶C(jī)械臂末端位置產(chǎn)生誤差，進(jìn)而影響實驗結(jié)果，為此提出了一種基于Simulink的機(jī)械臂運(yùn)動規(guī)劃硬件實現(xiàn)模型，以達(dá)到實時控制機(jī)械臂的目的。

綜上所述，本文利用算法移植在8位單片機(jī)系統(tǒng)上對機(jī)械臂運(yùn)動規(guī)劃算法的實現(xiàn)進(jìn)行研究。首先，利用D-H參數(shù)法對機(jī)械臂進(jìn)行數(shù)學(xué)模型的建立與工作空間的求解。其次，以機(jī)械臂運(yùn)動過程中實時產(chǎn)生的位置誤差構(gòu)造誤差補(bǔ)償函數(shù)，并基于E47數(shù)值算法設(shè)計機(jī)械臂的運(yùn)動規(guī)劃方案，減少機(jī)械臂末端執(zhí)行器的位置誤差。然后，利用Arduino單片機(jī)系統(tǒng)對該算法進(jìn)行運(yùn)算，所得結(jié)果通過MATLAB軟件進(jìn)行仿真分析。最后，利用硬件實現(xiàn)模型對六自由度機(jī)械臂執(zhí)行期望任務(wù)進(jìn)行實物驗證，結(jié)果表明，8位單片機(jī)對機(jī)械臂運(yùn)動規(guī)劃算法的實現(xiàn)具有可行性和有效性。

1? ? 機(jī)械臂運(yùn)動學(xué)建模與工作空間的求解

1.1? ?機(jī)械臂運(yùn)動學(xué)建模

目前建立機(jī)器人模型的方法主要有D-H法[13]和旋量法，其中D-H法是一種對機(jī)器人關(guān)節(jié)和連桿進(jìn)行運(yùn)動學(xué)建模的方法，該方法建模簡單，具有通用性，被廣泛用于機(jī)器人的運(yùn)動學(xué)建模。為驗證機(jī)械臂運(yùn)動規(guī)劃算法在8位單片機(jī)上的實現(xiàn)結(jié)果，選用圖1所示的六自由度機(jī)械臂為實驗平臺，并將鉛筆作為末端執(zhí)行器以跟蹤繪制期望軌跡。為便于將末端執(zhí)行器進(jìn)行統(tǒng)一建模，采用標(biāo)準(zhǔn)D-H法建立如圖2所示的六自由度機(jī)械臂坐標(biāo)系。機(jī)械臂D-H參數(shù)如表1所示。

根據(jù)表1中的參數(shù)，采用機(jī)器人正向運(yùn)動學(xué)方程推導(dǎo)出機(jī)械臂末端位姿矩陣。機(jī)械臂末端相對于連桿[i-1]坐標(biāo)系的位置可通過式（1）得到：

[i-1Tn=i-1TiiTi+1…n-1Tn，i=1，2，…，n] ，? ? （1）

式中：[n]為機(jī)械臂的關(guān)節(jié)空間維數(shù)，[i-1Ti]為相鄰連桿的坐標(biāo)變換矩陣。坐標(biāo)系變換通式為：

[i-1Ti=cθi-sθicαisθisαiaicθisθicθicαi-cθisαiaisθi0sαicαidi0001]，? ? ? ?（2）

式中：[cθi=cosθi，sθi=sinθi，cαi=cosαi，sαi=sinαi]。實驗對象為六自由度機(jī)械臂，則[n=6]。

將表1中D-H參數(shù)代入坐標(biāo)變換矩陣[i-1Ti]中，可得到6個相鄰連桿坐標(biāo)系之間的連桿變換矩陣[i-1Ti]，令i=1，由式（1）可得從機(jī)械臂末端到基座的變換位姿矩陣[0T6][14-15]：

[0T6=0T11T2…5T6=nxoxaxpxnyoyaypynzozazpz0001].? ? ? （3）

最后，利用機(jī)器人robotics工具箱在MATLAB上建模，應(yīng)用Link函數(shù)和SerialLink函數(shù)搭建如圖3所示的機(jī)械臂三維模型。

圖3中，機(jī)械臂的初始角度和初始位置與D-H參數(shù)表中的數(shù)據(jù)保持一致。圖3（a）上部分為機(jī)械臂末端的位姿，圖3（a）下部分可用來調(diào)節(jié)左側(cè)的各個關(guān)節(jié)角的大小，并在三維坐標(biāo)中（圖3（b））實時反映其位姿狀態(tài)，從而觀察機(jī)械臂的運(yùn)動軌跡和工作空間。

1.2? ?機(jī)械臂工作空間的求解

機(jī)械臂受到本身物理結(jié)構(gòu)與參數(shù)的約束，其工作空間有限。采用蒙特卡羅的方法[16-17]對機(jī)械臂工作空間進(jìn)行MATLAB仿真，結(jié)果如圖4所示。從圖4（a）可知，利用蒙特卡羅的方法可以較好地獲得一系列反映機(jī)械臂工作空間的隨機(jī)點(diǎn)云圖。為了方便測量和計算，設(shè)置機(jī)械臂末端在空間三維圖中的yoz平面執(zhí)行跟蹤任務(wù)，只需對yoz平面進(jìn)行工作空間分析。從圖4（b）中可得，機(jī)械臂在y軸方向上的近似工作范圍為[y∈（-0.36，0.36） m]，在z軸方向上的近似工作范圍為[z∈（-0.24，0.40） m]。雖然該結(jié)果不能精確地表示出工作空間的邊界，但是也能基本滿足一般軌跡任務(wù)的設(shè)置需要。

2? ? 運(yùn)動規(guī)劃方案

2.1? ?二次規(guī)劃

本文采用的二次型機(jī)械臂運(yùn)動規(guī)劃方案[5-7]? ? ?如下：

最小化：

[θTWθ2+qTθ]? ，? ? ? ? ? ? ? ? ? ? ? ? （4）

約束條件：

[J（θ）θ=r]? ? ，? ? ? ? ? ? ? ? ? ? ? ? （5）

[θ-≤θ≤θ+]? ?，? ? ? ? ? ? ? ? ? ? ? ? （6）

[θ-≤θ≤θ+]? ?，?; ? ? ? ? ? ? ? ? ? ? ? （7）

其中：[W]為系數(shù)矩陣，[q]為系數(shù)向量，[r]為機(jī)械臂在笛卡爾空間中的速度，[θ]和[θ]分別表示機(jī)械臂關(guān)節(jié)空間的位置和速度，[J]為機(jī)械臂的雅可比矩陣，[θ±]和[θ±]分別表示機(jī)械臂關(guān)節(jié)向量和關(guān)節(jié)速度向量的上下極限。

將式（6）轉(zhuǎn)化為[θ]的雙端約束：

[k（θ--θ）≤θ≤k（θ+-θ）] ，? ? ? ? ? ? ?（8）

式中正系數(shù)[k]用于調(diào)節(jié)關(guān)節(jié)速度的可行域。因此，關(guān)節(jié)極限（6）和關(guān)節(jié)速度極限（7）可以統(tǒng)一為一個 約束：

[ζ-≤θ≤ζ+] ，? ? ? ? ? ? ? ? ? ? ? ? ? （9）

其中，[ζ-]和[ζ+]的第[i]個元素的關(guān)節(jié)速度分別定義為：

[ζ-i=max（k（θ-i-θi），θ-i），ζ+i=min（k（θ+i-θi），θ+i）.]? ? ? ? ? ? ?（10）

由于本文不研究特定的機(jī)械臂運(yùn)動規(guī)劃問題，可將系數(shù)向量設(shè)置為[q=0∈Rn]，系數(shù)矩陣[W=I∈Rn×n]，因此，可得簡化的二次規(guī)劃方案：

最小化：

[xTx2]，? ? ? ? ? ? ? ? ? ? ? ? ? ? ? ?（11）

約束條件：

[Cx=d]，? ? ? ? ? ? ? ? ? ? ? ? ? ? ? （12）

[ζ-≤x≤ζ+]，? ? ? ? ? ? ? ? ? ? ? ? ? ?（13）

其中：待求解的決策向量[x=θ∈Rn]，系數(shù)向量[d=r∈Rm]，[C=J（θ）∈Rm×n]。

2.2? ?E47算法求解器

針對2.1提出的二次規(guī)劃問題（即式（11）—式（13）），擬將二次型問題轉(zhuǎn)化為線性變分不等式（LVI），再轉(zhuǎn)化為分段線性方程，最后應(yīng)用E47算法求解器進(jìn)行求解。

首先，將二次規(guī)劃問題轉(zhuǎn)換為線性變分不等式，即找到一個原對偶決策向量[y?∈Ω=y|y-≤y≤y+?Rn+m]，使得[?y∈Ω]都滿足：

[（y-y?）T（Hy?+z）≥0]，? ? ? ? ? ? ? ? （14）

式中：[H=W-CTC0]，[z=0-d]，原對偶決策向量[y]及其上下極限分別定義為：

[y=xu]，[y+=x++∞]，[y-=x--∞]，

式中：[∞]表示一個無窮大的向量，[u]表示等式約束（10）所對應(yīng)的對偶決策向量。

上述結(jié)論可以表示為如下定理：

定理[8]：當(dāng)至少存在一個最優(yōu)解[x?]時，二次規(guī)劃問題（式（11）—式（13））可以轉(zhuǎn)化為LVI問題（式（14））。

其次，LVI問題和分段線性方程問題之間的等價，是將不等式（14）轉(zhuǎn)換成等式（15）：

[PΩ（y-（Hy+z））-y=0]，? ? ? ? ? ? ? ?（15）

式中：[PΩ（?）]為分段線性投影算子。[PΩ（y）]從空間[Rn+m]到集合[Ω]的投影結(jié)果定義為：

[PΩ（yi）=y-i，yi，y+i，? yi<y-i;? ? ? ? ? ?y-i<yi<y+i;? y+i>yi.]

為實現(xiàn)具體的求解算法，在上述等價轉(zhuǎn)化定理的基礎(chǔ)上，定義向量誤差函數(shù)[11]如下：

[e（y）=y-PΩ（y-（Hy+z））] .? ? ? ? ? （16）

因此，求解QP問題（式（11）—式（13））等價于尋求誤差函數(shù)（式（16））的[0]點(diǎn)。將尋找線性變分不等式的[0]點(diǎn)定義如下：

[g（y）=HTe（y）+（Hy+z）]? ?，? ? ? ? ? ? ? （17）

可得第[k]次迭代的結(jié)果為：

[yk+1=PΩ（yk-ρ（yk）g（yk））]? ，? ? ? ? ? ? （18）

[ρ（y）=e（y）22（HT+I）e（y）22]? .? ? ? ? ? ? ? ? ?（19）

在算法實現(xiàn)中，為了保證算法的求解效率，在算法程序中設(shè)置了2個結(jié)束算法流程的控制條件，即求解精度與迭代次數(shù)（均可根據(jù)實際情況自行設(shè)置）。在求解過程中，如果求解得到的數(shù)值精度小于預(yù)先設(shè)置的求解精度，算法流程結(jié)束?？筛鶕?jù)求解時間的需要來設(shè)置迭代次數(shù)，如果迭代次數(shù)達(dá)到預(yù)先設(shè)置的次數(shù)，那么算法流程強(qiáng)制結(jié)束。最后一次迭代過程中計算生成的[y]的前[n]個元素便構(gòu)成了二次規(guī)劃問題（式（11）—式（13））的解，其中式（16）—式（19）構(gòu)成了E47數(shù)值算法。

由于機(jī)械臂在執(zhí)行任務(wù)時，其末端執(zhí)行器存在位置誤差，因此，可將機(jī)械臂末端執(zhí)行器的實時偏差定義為：

[w（t）=rd（t）-r（t）] ，? ? ? ? ? ? ? ? ? （20）

式中：[w（t）∈Rm]為機(jī)械臂末端的實際軌跡與期望軌跡之間的實時誤差，[rd（t）∈Rm]為機(jī)械臂期望的末端運(yùn)動軌跡，[r（t）∈Rm]為機(jī)械臂實際的末端運(yùn)動軌跡。為了更好地使位置誤差最終收斂到0，可設(shè)計誤差補(bǔ)償函數(shù)：

[w（t）=-λw（t）]，? ? ? ? ? ? ? ? ? ? ?（21）

式中：[λ]為誤差補(bǔ)償系數(shù)，為一個標(biāo)量系數(shù)。根據(jù)誤差補(bǔ)償?shù)男枰x取不同的[λ]值[18]。

引入誤差補(bǔ)償函數(shù)以后，機(jī)械臂在執(zhí)行任務(wù)過程中的實際末端執(zhí)行器速度可表示為：

[r（t）=rd（t）+w（t）]? .? ? ? ? ? ? ? ? ? （22）

綜上，加入誤差補(bǔ)償函數(shù)的E47算法求解器的求解控制框圖如圖5所示。

3? ? 仿真研究

由于Arduino單片機(jī)系統(tǒng)的程序開發(fā)條件較為便利[19-20]，因此，可在Arduino單片機(jī)系統(tǒng)上對機(jī)械臂執(zhí)行圓形和正方形軌跡跟蹤方案分別求解，并通過MATLAB軟件對所得結(jié)果進(jìn)行仿真分析。由于產(chǎn)生的軌跡數(shù)據(jù)點(diǎn)較多且實物為舵機(jī)驅(qū)動的簡易六自由度機(jī)械臂，因此，可選用以8位單片機(jī)為核心的Arduino Mega 2560開發(fā)板作為主控制器，其配置容量為256 kB的Flash[19]，數(shù)據(jù)存儲量相對充足，滿足本實驗的需要。而且開發(fā)板具有專門的PWM通道[20]，便于機(jī)械臂系統(tǒng)中6個舵機(jī)的連接控制。

3.1? ?圓形軌跡跟蹤

設(shè)機(jī)械臂末端執(zhí)行器從指定初始位置執(zhí)行半徑為0.03 m的圓形軌跡跟蹤任務(wù)，其期望圓形軌跡參數(shù)方程如下：

[rx（t）=ix，ry（t）=cos（desird_p）+iy-r，rz（t）=-rsin（desird_p）+iz.]? ? ? ? ?（23）

式中：[ix]、[iy]、[iz]分別代表機(jī)械臂末端的初始位置[（0.253，0，0.215）m]，[desird_p=aa?T?（t-T?sin（2π·]

[t/T）/（2π））/（2π）]，其中長度參數(shù)[aa=0.398]，任務(wù)周期[T=10 s]。初始的關(guān)節(jié)角[θ（0）=0，π/2，0，0，0，0Trad]，各個關(guān)節(jié)角度的上下極限分別為：

[θ+=π/2，147π/180，52π/180，π，-π/2，πTrad]，

[θ-=-π/2，36π/180，-53π/180，0，-π/2，0Trad].

設(shè)置求解精度為[10-6 m]，且迭代次數(shù)達(dá)1 000次時強(qiáng)制退出。仿真結(jié)果如圖6所示。

通過Arduino Mega2560對機(jī)械臂運(yùn)動規(guī)劃算法進(jìn)行運(yùn)算，得到的各個關(guān)節(jié)角速度數(shù)據(jù)所構(gòu)成的曲線如圖6（a）所示，曲線連續(xù)、較光滑且平穩(wěn)，沒有出現(xiàn)明顯毛糙的現(xiàn)象，說明Arduino Mega2560平臺運(yùn)算結(jié)果數(shù)據(jù)精度較高，利于機(jī)械臂的平穩(wěn)控制。圖6（b）展示了機(jī)械臂的末端運(yùn)動軌跡，從圖中可以看出，機(jī)械臂末端執(zhí)行器運(yùn)動軌跡能很好地跟蹤期望軌跡。將沿x軸、y軸和z軸方向上的位置誤差分別定義為[ex]、[ey]和[ez]，具體的末端位置誤差隨時間變化情況如圖6（c）所示。從圖中可以看到，最大絕對值位置誤差沿x方向為[ex=1.208×10-4 m]，沿y方向為[ey=1.590×10-4 m]，沿z方向為[ez=1.906×10-4 m]，該誤差精度達(dá)到了毫? ? ?米級。

為進(jìn)一步提高誤差精度以獲得更好的跟蹤效果，可在機(jī)械臂運(yùn)動規(guī)劃方案中引入誤差補(bǔ)償函數(shù)（即式（21）—式（22）），取[λ=30]，數(shù)據(jù)仿真結(jié)果如圖7所示。從圖7（a）中可以明顯地看到，機(jī)械臂末端跟蹤軌跡與期望軌跡較好地重合在一起。由圖7（b）可以發(fā)現(xiàn)，最大絕對值位置誤差沿x方向為[6.025×10-6 m]，沿y方向為[3.871×10-5 m]，沿z方向為[3.325×10-5 m]，誤差精度相比沒有加入誤差補(bǔ)償函數(shù)時得到了提高。以上分析結(jié)果說明了8位單片機(jī)對機(jī)械臂運(yùn)動規(guī)劃算法的實現(xiàn)是可行的。

3.2? ?正方形軌跡跟蹤

為進(jìn)一步驗證在8位單片機(jī)上實現(xiàn)機(jī)械臂運(yùn)動規(guī)劃算法，對機(jī)械臂執(zhí)行的邊長為0.05 m的正方形軌跡跟蹤任務(wù)進(jìn)行仿真分析。該任務(wù)執(zhí)行周期[T=8] s，求解迭代次數(shù)最高為200次，求解精度、機(jī)械臂初始關(guān)節(jié)角、機(jī)械臂末端初始位置以及關(guān)節(jié)角的上下極限均與上例跟蹤圓形軌跡相同。

圖8是未加入誤差補(bǔ)償時，8位單片機(jī)對機(jī)械臂執(zhí)行正方形軌跡跟蹤任務(wù)進(jìn)行求解的數(shù)據(jù)仿真結(jié)果。從圖8（a）機(jī)械臂末端運(yùn)動軌跡中可知，機(jī)械臂在執(zhí)行整個任務(wù)過程中產(chǎn)生了一定的誤差，沒能完全跟蹤期望軌跡。從圖8（b）中可知，機(jī)械臂最大絕對值誤差在x方向為[6.580×10-4 m]，在y方向為[9.899×10-4 m]，在z方向為[9.911×10-4 m]。并且從圖中可明顯地看到，機(jī)械臂末端在[t=8] s時，雖然x軸方向上的位置誤差沒有收斂到0，但誤差精度仍為毫米級。

為使機(jī)械臂更好地跟蹤期望軌跡，可考慮加入誤差補(bǔ)償函數(shù)，使其整體的跟蹤誤差減小。仿真結(jié)果如圖9所示。

從圖9（a）中可以看到，加入誤差補(bǔ)償函數(shù)后，機(jī)械臂末端跟蹤軌跡與期望軌跡較吻合。從圖9（b）可以看到，在x、y、z軸方向上的誤差均減小了，誤差精度得到了提高。以上分析結(jié)果說明8位單片機(jī)對機(jī)械臂運(yùn)動規(guī)劃算法進(jìn)行運(yùn)算時能達(dá)到較高的精度需求。從圖9（c）可以看出，機(jī)械臂能較好地跟蹤正方形軌跡。通過以上仿真結(jié)果分析可知，8位單片機(jī)對機(jī)械臂運(yùn)動規(guī)劃算法進(jìn)行運(yùn)算所得的數(shù)據(jù)曲線較平滑，誤差精度較高，進(jìn)一步說明了8位單片機(jī)對機(jī)械臂運(yùn)動規(guī)劃算法的實現(xiàn)具有較好的可行性和實用性。

3.3? ?構(gòu)造硬件實現(xiàn)模型及實物驗證

由于8位單片機(jī)對算法運(yùn)算的實時性不理想，在實際執(zhí)行任務(wù)時會由于執(zhí)行任務(wù)不連續(xù)而導(dǎo)致機(jī)械臂末端位置存在誤差，影響實驗結(jié)果，因此，構(gòu)造了基于Simulink的機(jī)械臂運(yùn)動規(guī)劃硬件實現(xiàn)模型，如圖10所示。該模型分為4個部分，data模塊用于放置由Arduino Mega2560單片機(jī)對機(jī)械臂運(yùn)動規(guī)劃算法運(yùn)算得到的6個關(guān)節(jié)角度運(yùn)動數(shù)據(jù)，數(shù)據(jù)讀取模塊用于讀取單位時間內(nèi)的角度數(shù)據(jù)，將角度數(shù)據(jù)傳遞給舵機(jī)，驅(qū)動機(jī)械臂進(jìn)行軌跡跟蹤任務(wù)。圖11為對應(yīng)3.2小節(jié)機(jī)械臂跟蹤邊長為0.05 m的正方形仿真的實物實驗情況。圖11中，“起點(diǎn)”表示機(jī)械臂初始的末端位置，箭頭表示機(jī)械臂對正方形軌跡的跟蹤方向。圖12展示了實際機(jī)械臂末端在整個周期內(nèi)執(zhí)行的完整路徑。從圖中能夠清晰地看到，畫出的正方形邊長為0.05 m，與期望軌跡較為吻合。因此，該實驗表明了E47數(shù)值算法在實際應(yīng)用中的可行性以及所構(gòu)造的硬件實現(xiàn)模型的有效性。

4? ? 總結(jié)

目前機(jī)器人運(yùn)動規(guī)劃與控制系統(tǒng)成本較高，機(jī)器人實驗裝置難以普及，學(xué)生缺少實踐動手機(jī)會。為解決這一問題，通過8位單片機(jī)對E47數(shù)值算法進(jìn)行了實現(xiàn)，并對機(jī)械臂執(zhí)行圓形和正方形軌跡跟蹤的運(yùn)動規(guī)劃進(jìn)行了仿真。仿真結(jié)果表明，8位單片機(jī)對機(jī)械臂運(yùn)動規(guī)劃算法的實現(xiàn)具有可實現(xiàn)性和可適用性。為提高機(jī)械臂執(zhí)行任務(wù)的實時性，構(gòu)造了基于Simulink的硬件實現(xiàn)模型，從實驗結(jié)果中可以看出所構(gòu)造的機(jī)械臂硬件實現(xiàn)模型有效，說明本文所提出的方案能夠降低機(jī)械臂控制系統(tǒng)的成本，滿足研究者對機(jī)械臂運(yùn)動規(guī)劃學(xué)習(xí)的需要，為后續(xù)更好地研究機(jī)械臂運(yùn)動奠定了一定的基礎(chǔ)。

參考文獻(xiàn)

[1]? ? ?HU T J，WANG T S，LI J F，et al.Obstacle avoidance for redundant manipulators utilizing a backward quadratic search algorithm[J].International Journal of Advanced Robotic Systems，2016，13（3）：109-116.

[2]? ? ?ZHANG Y N，GUO D S，CAI B H，et al.Remedy scheme and theoretical analysis of joint-angle drift phenomenon for redundant robot manipulators[J].Robotics and Computer-Integrated Manufacturing，2011，27（4）：860-869.

[3]? ? ?HUANG S H，PENG Y G，WEI W，et al.Clamping weighted least-norm method for the manipulator kinematic control：avoiding joint limits[C]//Proceedings of the 33rd Chinese Control Conference. IEEE，2014：8309-8314.

[4]? ? ?LIAO B L，LIANG P Y，YANG X.Pseudoinverse-based optimization scheme for motion control of redundant robot manipulators[J].Information and Control，2013，42（5）：645-651，656.

[5]? ? ?ZHANG Y N，LI K N.Bi-criteria velocity minimization of robot manipulators using LVI-based primal-dual neural network and illustrated via PUMA560 robot arm[J].Robotica，2009，28（4）：525-537.

[6]? ? ?LI K N，ZHANG Y N.Fault-tolerant motion planning and control of redundant manipulator[J].Control Engineering Practice， 2012，20（3）：282-292.

[7]? ? ?張雨濃，符剛，尹江平.基于雙判據(jù)優(yōu)化方法的機(jī)器人逆運(yùn)動學(xué)求解[J].大連海事大學(xué)學(xué)報，2007，33（3）：1-5.

[8]? ? ?謝清，張雨濃，余曉填，等.面向冗余度機(jī)械臂QP問題求解的E47和94LVI數(shù)值算法[J].計算機(jī)工程與科學(xué)，2015，37（7）：1405-1411.

[9]? ? ?ZHANG Y N.On the LVI-based primal-dual neural network for solving online linear and quadratic programming problems[C]//Proceeding of American Control Conference，2005：1351-1356.

[10]? ?ZHANG Y N，WANG J.A dual neural network for constrained joint torque optimization of kinematically redundant manipulators[J].IEEE Transactions on Systems，Man，and Cybernetics-Part B：Cybernetics，2002，32（5）：654-662.

[11]? ?HE B S.A new method for a class of linear variational linequalities[J].Mathematical Programming，1994，66（2）：137-144.

[12]? ?ZHANG Y N，F(xiàn)U S B，ZHANG Z J，et al.On the LVI-based numerical method （E47 algorithm） for solving quadratic programming problems[C]//Proceeding of the IEEE International Conference on Automation and Logistics，2011：125-130.

[13]? ?劉海燕，蘇宇，游慶如，等.HSR-JR605工業(yè)機(jī)械臂的運(yùn)動仿真研究[J].廣西科技大學(xué)學(xué)報，2020，31（3）：22-27.

[14]? ?毛志賢，韋建軍，王春寶，等.新型四臂扶持式康復(fù)機(jī)器人設(shè)計[J].廣西科技大學(xué)學(xué)報，2020，31（3）：1-7.

[15]? ?臧慶凱，李春貴，鐘宛余.基于RBF和BP網(wǎng)絡(luò)的機(jī)器人逆運(yùn)動學(xué)求解[J].廣西工學(xué)院學(xué)報，2012，23（1）：28-33.

[16]? ?孫野，殷鳳龍，王香麗，等.六自由度機(jī)械臂運(yùn)動學(xué)及工作空間分析[J].機(jī)床與液壓，2015，43（3）：76-81.

[17]? ?王春，韓秋實.六自由度串聯(lián)機(jī)械臂運(yùn)動學(xué)及其工作空間研究[J].組合機(jī)床與自動化加工技術(shù)，2020，62（6）：32-36.

[18]? ?李克訥，楊津，徐劍琴，等.機(jī)械臂初始位置誤差的容錯運(yùn)動規(guī)劃[J].哈爾濱理工大學(xué)學(xué)報，2020，25（1）：93-99.

[19]? ?宋振鵬.一種家庭服務(wù)型機(jī)器人移動平臺開發(fā)[D].成都：西南交通大學(xué)，2016.

[20]? ?聶超，宋志丹，孫曉宇，等.基于Mega2560的物聯(lián)型風(fēng)機(jī)主軸部件加熱裝置[J].船舶工程，2020，42（S2）：286-291.

Implementation of E47 algorithm of redundant manipulator in

Arduino environment

LI Kene1， WANG Wenxin1， HU Xuchu2， MA Yuru1， HE Zhixiang1， YUAN Weiming1， YE Hongtao3，4

（1.School of Electrical， Electronic and Computer Science， Guangxi University of Science and Technology，

Liuzhou 545616， China; 2.Guangxi Liuzhou Special Transformer Co.，Ltd.，Liuzhou 545006， China;

3.Guangxi Key Laboratory for Automatic Detecting Technology and Instruments， Guilin University of Electronic Technology， Guilin 541004， China; 4.Guangxi Key Laboratory of Automobile Components and Vehicle

Technology （Guangxi University of Science and Technology）， Liuzhou 545006， China）

Abstract： The high cost of the motion planning and control system of the manipulator decreases? ? ? practical opportunities for the related educatees， which makes it difficult to popularize the robot? ? ? ?technology. In this paper， an 8-bit microcontroller was used to implement the manipulator motion? ? planning algorithm and a hardware implementation model of manipulator motion planning based on Simulink was proposed to improve the real-time performance of manipulator. Firstly， the kinematic analysis of the manipulator was given out， and the mathematical model was established. Secondly， the motion planning scheme of the manipulator was transformed into a quadratic programming problem， and the error compensation function was introduced to reduce the position-error generated during the manipulator motion. Then， in the Arduino Mega2560 platform， the E47 algorithm was used to resolve the manipulator motion planning scheme; the Matlab was used to perform the circular and square path tracking tasks of the manipulator. Finally， the hardware implementation model was used to verify the square trajectory of the manipulator. The results indicate the feasibility and applicability to realize the manipulator motion planning algorithm using the 8-bit microcontroller， and the validity of the hardware implementation model， which provides a basis for the study of the motion planning of the manipulator.

Key words： 8-bit microcontrolle; manipulator motion planning; E47 algorithm; error compensation; hardware implementation model

（責(zé)任編輯：黎? ?婭）