陳卓凡 周坤 秦菲菲 王斌銳

摘要 ：針對(duì)工業(yè)機(jī)器人在逆運(yùn)動(dòng)學(xué)求解過(guò)程中存在的位姿奇異、解不唯一、求解精度低等問(wèn)題，提出了一種改進(jìn)量子粒子群算法。首先，利用D-H參數(shù)法建立機(jī)器人運(yùn)動(dòng)學(xué)模型，以機(jī)械臂末端最小位姿誤差為主要優(yōu)化目標(biāo)，加入運(yùn)動(dòng)前后關(guān)節(jié)角變化最小、行程平穩(wěn)連續(xù)的約束條件，設(shè)計(jì)了目標(biāo)函數(shù)；其次，通過(guò)采用Levy飛行策略改進(jìn)粒子更新方式、非線性地動(dòng)態(tài)調(diào)整收縮膨脹因子、采用變權(quán)重方法計(jì)算最優(yōu)平均位置等方法設(shè)計(jì)了一種改進(jìn)量子粒子群優(yōu)化（IQPSO）算法；然后，模擬單點(diǎn)位姿和連續(xù)軌跡兩種不同的求解情況進(jìn)行三種算法（IQPSO、APSO、QPSO）的仿真對(duì)比實(shí)驗(yàn)，結(jié)果表明IQPSO算法具有收斂速度快、求解精度高等優(yōu)點(diǎn)；最后，將IQPSO算法用于機(jī)械臂本體進(jìn)行實(shí)物驗(yàn)證，實(shí)驗(yàn)結(jié)果表明IQPSO算法求解出的插值點(diǎn)所組成的軌跡連續(xù)且平滑，進(jìn)一步證明了該算法應(yīng)用于實(shí)際運(yùn)動(dòng)控制中的穩(wěn)定性和可行性。

關(guān)鍵詞 ：工業(yè)機(jī)器人；逆運(yùn)動(dòng)學(xué)求解；目標(biāo)函數(shù)；改進(jìn)量子粒子群優(yōu)化算法

中圖分類(lèi)號(hào) ：TP242.2

DOI：10.3969/j.issn.1004-132X.2024.02.014

開(kāi)放科學(xué)（資源服務(wù)）標(biāo)識(shí)碼（OSID）：

Inverse Kinematics Solution of Robots Based on IQPSO Algorithm

CHEN Zhuofan ZHOU Kun QIN Feifei WANG Binrui

School of Mechanical and Electrical Engineering，China Jiliang University，Hangzhou，310018

Abstract ： Aiming at the problems of singular pose， non-unique solution and low solution precision in the inverse kinematics solution processes of general robots， an improved quantum particle swarm optimization algorithm was proposed. Firstly， the robot kinematics model was established by using the D-H parameter method， and the minimum pose errors at the end of the manipulators were the main optimization goal， and the constraints of the minimum joint angle changes before and after the movement and the smooth and continuous stroke were added， and the objective function was designed. Secondly， an IQPSO algorithm was designed by using the Levy flight strategy to improve the particle update method， nonlinear dynamic adjust the shrinkage and expansion factors， and using the variable weight method to calculate the optimal average position. Then， the simulation and comparison experiments of three algorithms（IQPSO，APSO，QPSO） were carried out by simulating two different solutions of single point pose and continuous trajectory. The results show that the IQPSO algorithm has the advantages of fast convergence speed and high solution accuracy; finally， the IQPSO algorithm was used in the body of the robot arm for physical verification. The results show that the trajectory composed of interpolation points obtained by the IQPSO algorithm is continuous and smooth， which further proves the stability and feasibility of the algorithm in practical motion control.

Key words ： industrial robot; inverse kinematics solution; objective function; improved quantum particle swarm optimization（IQPSO） algorithm

0 引言

工業(yè)機(jī)器人作為工業(yè)自動(dòng)化領(lǐng)域的一個(gè)重要組成部分，已經(jīng)被大量應(yīng)用于電子、醫(yī)療、物流、交通制造、軍工等行業(yè) ?［1］ 。工業(yè)機(jī)器人的逆運(yùn)動(dòng)學(xué)求解旨在依據(jù)給定的末端位姿信息計(jì)算出機(jī)械臂各關(guān)節(jié)所對(duì)應(yīng)的角度值。由于描述機(jī)器人結(jié)構(gòu)的數(shù)學(xué)模型具有非線性的特點(diǎn)，因此整個(gè)求解過(guò)程可以看作是求解一個(gè)多變量、強(qiáng)耦合下的高維非線性方程組，求解過(guò)程較為復(fù)雜且可能會(huì)出現(xiàn)多組解 ?［2］ ，對(duì)于逆解如何取優(yōu)也是一個(gè)研究難點(diǎn)。目前有關(guān)逆運(yùn)動(dòng)學(xué)的算法主要分為解析法、數(shù)值法、智能算法等。對(duì)滿(mǎn)足Piper 準(zhǔn)則的機(jī)器人可以使用解析法進(jìn)行求解 ?［3］ 。這種解法利用代數(shù)幾何以及投影的方法可以獲得閉合形式的逆運(yùn)動(dòng)學(xué)解，但是由于計(jì)算過(guò)程繁瑣導(dǎo)致整體運(yùn)算量偏大，且受限于機(jī)器人構(gòu)型，普適性差 ?［4］ 。數(shù)值法通常采用迭代求解，對(duì)于迭代初值選取較為敏感且迭代過(guò)程中雅可比矩陣會(huì)出現(xiàn)奇異導(dǎo)致無(wú)解。智能算法不受構(gòu)型限制，它通過(guò)設(shè)立一個(gè)合適的適應(yīng)函數(shù)將逆解問(wèn)題轉(zhuǎn)化為一個(gè)智能優(yōu)化問(wèn)題。目前應(yīng)用較為廣泛的智能算法包括人工神經(jīng)網(wǎng)絡(luò) ?［5］ 、遺傳算法（GA） ?［6］ 、粒子群優(yōu)化（PSO）算法 ?［7］ 等。神經(jīng)網(wǎng)絡(luò)算法的準(zhǔn)確性更多地取決于網(wǎng)絡(luò)的結(jié)構(gòu)，且需要大量已知樣本進(jìn)行訓(xùn)練；遺傳算法在單個(gè)染色體上使用交叉操作然后相互比較，共享效率較低；粒子群優(yōu)化算法直觀高效，易于實(shí)現(xiàn)，執(zhí)行效率高，可以避免繁瑣的數(shù)學(xué)公式推導(dǎo)，比較適合作為機(jī)器人逆運(yùn)動(dòng)學(xué)的求解方法 ?［7］ 。但是由于PSO算法在運(yùn)算初期需設(shè)定的參數(shù)（慣性權(quán)重 ω，學(xué)習(xí)因子c 1、c 2） 太多，不利于找到最佳的優(yōu)化參數(shù)。近年來(lái)有許多學(xué)者針對(duì)標(biāo)準(zhǔn)PSO算法存在的問(wèn)題提出了改進(jìn)方法，SANCAKTAR等 ?［8］ 提出一種自適應(yīng)的粒子群優(yōu)化算法，并將其用于求解三自由度機(jī)械臂的逆運(yùn)動(dòng)學(xué)問(wèn)題；姜濤等 ?［9］ 提出引入收縮學(xué)習(xí)因子提高算法收斂能力的方法；DU等 ?［10］ 提出了三種關(guān)于慣性權(quán)重的非線性調(diào)整方法，證明了相比較線性調(diào)整慣性權(quán)重，非線性調(diào)整方法有更好的優(yōu)化效果。

然而PSO算法終究不是一個(gè)符合全局收斂準(zhǔn)則的全局收斂算法，其全局搜索能力非常依靠對(duì)粒子速度上限的設(shè)定。針對(duì)此，孫俊 ?［11］ 提出了一種具有量子行為的粒子群優(yōu)化（QPSO） 算法，相比較傳統(tǒng)粒子群算法，該算法取消了粒子的速度屬性，參數(shù)更少，收斂速度更快。TIAN等 ?［12］ 用選定的基準(zhǔn)函數(shù)去測(cè)試QPSO算法性能后也驗(yàn)證了該算法的優(yōu)越性。在分析討論QPSO算法的收斂性時(shí)，SUN等 ?［13］ 分析了算法迭代公式的參數(shù)選擇對(duì)搜索能力的影響，并提出了可以增強(qiáng)全局搜索能力有效避免局部?jī)?yōu)化的方法；TURGUT等 ?［14］ 提出在量子粒子群中引入不同的混沌映射從而達(dá)到提高算法魯棒性的方法；ZHANG等 ?［15］ 在初始化量子位置信息時(shí)引入高斯分布的變異算子以增加種群的多樣性。

然而上述方法的改進(jìn)思路都集中在優(yōu)化種群初始化過(guò)程、預(yù)防算法早熟等方面，對(duì)算法更新準(zhǔn)則中局部吸引子和搜索范圍這兩個(gè)直接影響位置更新速度的因素研究不夠具體。因此本文在該領(lǐng)域先前的研究基礎(chǔ)上針對(duì)上述問(wèn)題提出了一種改進(jìn)的量子粒子群優(yōu)化（IQPSO）算法。首先，對(duì)本文的研究對(duì)象——六軸工業(yè)機(jī)器人機(jī)械臂進(jìn)行運(yùn)動(dòng)學(xué)建模與分析，并根據(jù)優(yōu)化目標(biāo)建立合適的適應(yīng)度函數(shù)；其次，介紹粒子群算法和量子粒子群算法的相關(guān)原理和聯(lián)系，并在QPSO算法基礎(chǔ)上引入Levy飛行策略改進(jìn)局部吸引子更新方式，采用非線性方法動(dòng)態(tài)調(diào)整收縮膨脹因子，采用變權(quán)重方法計(jì)算平均最優(yōu)位置，從而優(yōu)化粒子的搜索范圍；然后，通過(guò)仿真實(shí)驗(yàn)?zāi)M工作空間中任意單點(diǎn)位姿和連續(xù)軌跡兩種不同的求解情況，將本文所述改進(jìn)算法與加入混沌映射和高斯分布的QPSO算法、自適應(yīng)粒子群算法進(jìn)行對(duì)比求解分析，證明IQPSO算法的優(yōu)越性；最后在六軸工業(yè)機(jī)器人平臺(tái)上應(yīng)用IQPSO算法進(jìn)行實(shí)際運(yùn)動(dòng)過(guò)程中的點(diǎn)位求解，驗(yàn)證該算法在真實(shí)工作中的可行性和可靠性。

1 六軸工業(yè)機(jī)器人運(yùn)動(dòng)學(xué)建模與分析

1.1 機(jī)器人運(yùn)動(dòng)學(xué)建模

本文所采用的六自由度新松SR4C機(jī)械臂符合Pieper準(zhǔn)則，根據(jù)表1給出的各關(guān)節(jié)信息，采用改進(jìn)D-H參數(shù)法建立連桿坐標(biāo)系，如圖1所示。

其中，θ i、α i、a i、d i分別表示每一個(gè)連桿的關(guān)節(jié)角、扭轉(zhuǎn)角、連桿長(zhǎng)度以及連桿偏移。根據(jù) SR4C 機(jī)械臂實(shí)際參數(shù)，將各軸的連桿偏移和連桿長(zhǎng)度選取如下：

d 1=330 ?mm ，d 2=0，d 3=0， d 4=310 ?mm ， d 5=0，d 6=70 ?mm ，a 1=40 ?mm ，a 2=315 ?mm ，a 3=70 ?mm ，a 4=0，a 5=0，a 6=0。

因此表示相鄰坐標(biāo)系間位姿變換關(guān)系的齊次矩陣如下：

i－1 ???i ?T =

cos ?θ i － sin ?θ i cos ?α i ?sin ?θ i sin ?α i a i cos ?θ i ?sin ?θ i ?cos ?θ i cos ?α i － cos ?θ i sin ?α i a i sin ?θ i 0 ?sin ?α i ?cos ?α i d i 0 0 0 1 ???（1）

將表1中的 D-H 參數(shù)代入式（1）后，可以得到各個(gè)關(guān)節(jié)的齊次變換矩陣。隨后依據(jù)正運(yùn)動(dòng)學(xué)表達(dá)式，將6個(gè)齊次變換矩陣從左到右相乘就可以得到機(jī)器人末端相對(duì)于基坐標(biāo)系的位姿矩陣 0 T ?6：

0 T ?6= T ?1 T ?2 T ?3 T ?4 T ?5 T ?6=

n x o x a x p x n y o y a y p y n z o z a z p z 0 0 0 1 ?= ??R ??3×3 ??P ?0 ?1 ???（2）

式中， R ??3×3 為末端執(zhí)行器相對(duì)于基坐標(biāo)系的姿態(tài)變換矩陣； P 為末端執(zhí)行器相對(duì)于基坐標(biāo)系的位置變換矢量。

1.2 目標(biāo)函數(shù)的建立

智能優(yōu)化算法旨在將機(jī)器人逆運(yùn)動(dòng)學(xué)求解的問(wèn)題轉(zhuǎn)換為優(yōu)化控制的問(wèn)題來(lái)解決，因此需要設(shè)立一個(gè)合適的優(yōu)化目標(biāo)函數(shù)。給定機(jī)械臂末端執(zhí)行器的期望位姿矩陣為

T ???des ?= ??R ??3×3， des ???P ???des ??0 ?1 ???（3）

將初始關(guān)節(jié)角度θ i代入式（2）后，可以得到當(dāng)前關(guān)節(jié)角所對(duì)應(yīng)的位姿矩陣：

T ???cur ?= ??R ??3×3， cur ???P ???cur ??0 ?1 ???（4）

為了讓機(jī)械臂的末端執(zhí)行器達(dá)到期望位姿，需要使得式（3）和式（4）滿(mǎn)足：

Δ=‖ T ???cur ?－ T ???des ?‖=0 ?（5）

滿(mǎn)足式（5）的關(guān)節(jié)角度即為正確的逆運(yùn)動(dòng)學(xué)解。同時(shí)滿(mǎn)足條件的關(guān)節(jié)角度的邊界條件設(shè)置為

θ ?l ?i≤θ i≤θ ?u ?i ?i=1，2，…，6 ?（6）

式中，θ ?l ?i、θ ?u ?i分別為第i個(gè)關(guān)節(jié)所對(duì)應(yīng)的關(guān)節(jié)上限和關(guān)節(jié)下限。

由于式（5）同時(shí)涉及到了姿態(tài)的9個(gè)變量和位置的3個(gè)變量，方程組的構(gòu)成較為復(fù)雜，因此本文將其分解為一個(gè)關(guān)于姿態(tài)誤差的函數(shù)Δ 1和一個(gè)關(guān)于位置誤差的函數(shù)Δ 2：

Δ 1=‖ Δ ?R ??3×3 ‖=‖ R ??3×3， cur ?－ R ??3×3， des ?‖ ??（7）

Δ 2=‖ Δ ?P ‖=‖ P ???cur ?－ P ???des ?‖ ??（8）

實(shí)際工程應(yīng)用中常會(huì)出現(xiàn)多組解的問(wèn)題，在逆解取優(yōu)時(shí)應(yīng)以機(jī)器人各關(guān)節(jié)角θ i變化最小作為取優(yōu)的依據(jù)，此時(shí)運(yùn)動(dòng)前后機(jī)器人姿態(tài)變化最小可保證運(yùn)動(dòng)行程的平穩(wěn)連續(xù)。 假設(shè)在運(yùn)動(dòng)過(guò)程中，當(dāng)前點(diǎn)P ?s 經(jīng)求逆后對(duì)應(yīng)的角度為θ ??s ，i （i= 1，2，…，6），目標(biāo)點(diǎn)P ?r 所對(duì)應(yīng)的角度為θ ??r ，i （i=1， 2，…，6）， 則各關(guān)節(jié)角度的變化函數(shù)為

F（θ）=∑ 6 i=1 |θ ??r ，i －θ ??s ，i | ?（9）

綜上，可以將逆解問(wèn)題轉(zhuǎn)化為一個(gè)以位姿誤差最小、各關(guān)節(jié)運(yùn)動(dòng)前后角度變化最小、運(yùn)行穩(wěn)定的多目標(biāo)約束下的優(yōu)化問(wèn)題，目標(biāo)函數(shù)如下：

min ?φ（θ）= min （Δ 1，Δ 2，F(xiàn)（θ）） ??（10）

式中，F(xiàn)（θ）為運(yùn)行平穩(wěn)目標(biāo)。

由于式（10）涉及到了兩個(gè)優(yōu)化目標(biāo)，包含三個(gè)子項(xiàng)，屬于一個(gè)多目標(biāo)優(yōu)化問(wèn)題，因此本文采用目前使用較為廣泛的線性加權(quán)法，用三個(gè)不同的權(quán)重系數(shù)將式（10）的各項(xiàng)線性組合，轉(zhuǎn)換為綜合優(yōu)化的方式，即

φ（θ）=μΔ 1+λΔ 2+γF（θ） ??（11）

由式（7）可以看出姿態(tài)誤差函數(shù)是正弦余弦函數(shù)的組合，其參數(shù)單位為弧度；由式（8）可以看出位置誤差函數(shù)以歐氏距離差表示，其參數(shù)單位為毫米；由式（9）可以看出關(guān)節(jié)變化函數(shù)以前后角度差表示，其參數(shù)單位為弧度。經(jīng)實(shí)驗(yàn)后發(fā)現(xiàn)，三個(gè)子項(xiàng)并不處于同一量綱，因此需要設(shè)立權(quán)重系數(shù)μ、λ、γ對(duì)各子項(xiàng)進(jìn)行歸一化處理，選取不同的參數(shù)協(xié)調(diào)各子項(xiàng)對(duì)目標(biāo)函數(shù)收斂速度的影響，避免因數(shù)量級(jí)的問(wèn)題而導(dǎo)致某種誤差指標(biāo)變化過(guò)小而被忽略。具體的系數(shù)設(shè)置根據(jù)機(jī)器人本體的參數(shù)值確定。

2 改進(jìn)的量子粒子群優(yōu)化算法

2.1 基本粒子群算法原理

PSO 是一種基于種群搜索的自適應(yīng)進(jìn)化算法，其基本思想在于通過(guò)群體中個(gè)體之間的合作和信息共享來(lái)尋找到最優(yōu)解。假設(shè)在N維空間內(nèi)，由M個(gè)粒子組成的一個(gè)粒子群都以一定的速度在此空間內(nèi)飛行搜索，設(shè)其中一個(gè)粒子m所處位置為 x ?m=（x ?m，1 ，x ?m，2 ，…，x ?m，N ），當(dāng)前粒子m的飛行速度為 v ?m=（v ?m，1 ，v ?m，2 ，…，v ?m，N ）；粒子的個(gè)體最優(yōu)預(yù)測(cè)位置為 p ???best ?=（p ?m，1 ，p ?m.2 ，…，p ?m，N ），粒子種群的全局最優(yōu)預(yù)測(cè)位置為 g ???best ?=（g 1，g 2，…，g N）。標(biāo)準(zhǔn) PSO 算法的表達(dá)式即粒子的飛行速度和位置更新如下：

v ?m，n （t+1）=ωv ?m，n （t）+c 1r 1（p ?m，n （t）－x ?m，n （t））+

c 2r 2（g m（t）－x ?m，n （t）） ?（12）

x ?m，n （t+1）=x ?m，n （t）+v ?m，n （t+1） ??（13）

m=1，2，…，M ?n=1，2，…，N

式中，ω為慣性權(quán)重；c 1、c 2為學(xué)習(xí)因子，也稱(chēng)個(gè)體加速度系數(shù)和群體加速度系數(shù)；r 1、r 2為［0，1］內(nèi)均勻分布的隨機(jī)值。

2.2 具有量子行為的粒子群算法

標(biāo)準(zhǔn)的粒子群優(yōu)化算法可以解決多峰、高維的控制優(yōu)化問(wèn)題，但是當(dāng)變量個(gè)數(shù)增多且維數(shù)變大后，計(jì)算量大大增加且穩(wěn)定性降低，很容易陷入局部最優(yōu) ?［7］ 。對(duì)于求解六軸工業(yè)機(jī)器人的θ i（i=1，2，…，6），該算法并不完全滿(mǎn)足穩(wěn)定性和精度的要求，理論上來(lái)說(shuō)并不能稱(chēng)為一種全局性算法，因此孫俊 ?［11］ 將量子行為引入粒子群算法，提出一種全新的量子粒子群優(yōu)化算法。該算法在進(jìn)行優(yōu)化迭代時(shí)擯棄了式（12）所示的速度更新，轉(zhuǎn)而只更新粒子的位置信息。為保證量子粒子群算法收斂，每一個(gè)粒子必須收斂到自身的局部吸引子（又稱(chēng)勢(shì)阱中心），吸引子的定義如下：

p ?m，n （t+1）=φp ?m，n （t）+（1－φ）g ??best ，n （t） ?（14）

φ∈［0，1］

式中，p ?m，n 為當(dāng)次迭代時(shí)第m個(gè)粒子個(gè)體最優(yōu)位置的第n維，它影響粒子每次迭代過(guò)程的進(jìn)化收斂趨勢(shì)；g ??best ，n 為整個(gè)粒子群體最優(yōu)位置的第n維。

QPSO 算法引入了量子態(tài)粒子行為，用一個(gè)波函數(shù)Ψ（x，t）描述粒子的位置狀態(tài) ?［12］ ，隨后求解薛定諤方程獲得粒子的概率密度函數(shù)Q和概率分布函數(shù)D，確定粒子出現(xiàn)在工作空間中某處的概率：

Q（x ?m，n （t+1））= ?1 L ?m，n （t） ?exp （－2|p ?m，n （t）－

x ?m，n （t+1）|/L ?m，n （t））

D（x ?m，n （t+1））= ?exp （－2|p ?m，n （t）－

x ?m，n （t+1）|/L ?i，j （t）） ??（15）

L ?m，n （t）=2a|W n（t）－x ?m，n （t）| ?（16）

W n（t）= 1 M ∑ M m=1 p ?m，n ??（17）

式中，L ?m，n （t）為粒子間的特征長(zhǎng)度，用來(lái)確定粒子的搜索范圍；W n（t）為 QPSO 算法中定義的全新參數(shù)，表示第t次迭代時(shí)所有粒子的平均最優(yōu)位置；a為收縮膨脹因子。

至此粒子在空間中的具體位置可通過(guò)蒙特卡羅逆變換方法獲得 ?［16］ ，位置更新方程如下：

x ?m，n （t+1）=p ?m，n （t）± 1 2 L ?m，n （t） ln （1/u） ?（18）

其中，u表示0～1均勻分布的隨機(jī)數(shù)，當(dāng)u>0.5 時(shí)“±”取“+”，當(dāng)u<0.5 時(shí)“±”取“－”。

由式（18）可知，因子a成為了計(jì)算過(guò)程中唯一需要控制調(diào)整的參數(shù)，而且先前的粒子移動(dòng)不再影響粒子下一次的位置更新，使其具有更高的隨機(jī)性，提高了群體智能程度，理論上可以在搜索空間內(nèi)找到最優(yōu)解。因此本文基于這些特性，改進(jìn)優(yōu)化 QPSO算法中粒子的搜索范圍從而更好地協(xié)調(diào)算法全局和局部的搜索能力，保證算法擁有更好的收斂速度和求解精度，然后運(yùn)用改進(jìn)的QPSO算法對(duì)機(jī)器人進(jìn)行逆運(yùn)動(dòng)學(xué)求解。

2.3 改進(jìn)的量子粒子群優(yōu)化算法

由式（18）可知， QPSO 算法的位置更新過(guò)程中粒子局部吸引子p ?m，n （t）和特征長(zhǎng)度L ?m，n （t）這兩個(gè)關(guān)鍵要素起到了很大的作用，p ?m，n （t）決定了粒子種群在之后迭代過(guò)程中的進(jìn)化方向，L ?m，n （t）用來(lái)確定粒子的搜索范圍，并且由收縮膨脹因子和平均最優(yōu)位置這兩個(gè)因素共同決定。本文基于上述特征，針對(duì) QPSO 算法進(jìn)行三方面的改進(jìn)，使算法可以根據(jù)各粒子運(yùn)行過(guò)程中的實(shí)際情況不斷優(yōu)化調(diào)整搜索幅度和范圍，提高 QPSO 算法的準(zhǔn)確性和魯棒性，加快整體收斂，防止算法陷入局部最優(yōu)，進(jìn)一步協(xié)調(diào)好算法的局部搜索和全局搜索能力。

2.3.1 引入 Levy 飛行策略

由式（14）可以看出，粒子的勢(shì)阱中心由當(dāng)前粒子的個(gè)體最優(yōu)和當(dāng)前群體的歷史最優(yōu)決定。然而當(dāng)算法在迭代過(guò)程中歷史最優(yōu)值g ??best ?陷入局部最優(yōu)時(shí)，由于計(jì)算公式中g(shù) ??best ?與p ?m，n 存在線性組合關(guān)系，p ?m，n 也會(huì)因此陷入局部最優(yōu)導(dǎo)致算法過(guò)早收斂。本文為解決此類(lèi)問(wèn)題引入 Levy 飛行策略進(jìn)行改進(jìn)。據(jù)文獻(xiàn)［17-18］記載，目前針對(duì)許多智能優(yōu)化算法如粒子群優(yōu)化算法、蟻群算法的研究中，都會(huì)選擇利用 Levy 分布確定迭代步長(zhǎng)的隨機(jī)游走過(guò)程，然后通過(guò)粒子較大的跳躍在搜索空間中進(jìn)行更有效的搜索，從而可以在一定程度上擺脫算法的局部最小值，提高搜索能力。因此在勢(shì)阱中心的計(jì)算中引入 Levy 飛行策略，當(dāng)種群進(jìn)化方向陷入局部最優(yōu)時(shí)重新分布粒子位置，改善 QPSO 算法提早收斂的劣勢(shì)。

Levy飛行是一類(lèi)非高斯隨機(jī)過(guò)程，其隨機(jī)游走服從Levy穩(wěn)定分布。當(dāng)將Levy飛行策略引入到量子粒子群算法中時(shí)，需首先定義一個(gè)符合Levy 分布的步長(zhǎng)因子s：

s=rand（size（D））Levy（k）～ u |v| ?1 k ???（19）

σ u=（ Γ（1+k） sin （ ?π k 2 ） Γ（ （1+k）k 2 ）2 ??（k－1） 2 ??） ??1 k ???σ v=1

式中的參數(shù)u和v分別都滿(mǎn)足于正態(tài)分布，即u～ N （0，σ 2 u），v～ N （0，σ 2 v）。函數(shù)rand（size（D）） 表示該粒子種群所有維度下的隨機(jī)數(shù)，表示生成的隨機(jī)向量與滿(mǎn)足 levy 分布中采樣得到的向量進(jìn)行逐元素相乘，最終得到維度為D的步長(zhǎng)因子s。σ u定義式中的Γ是根據(jù)式（19）獲得的D維標(biāo)準(zhǔn)伽馬函數(shù)的值；k為［1，3］的隨機(jī)數(shù)。

將定義好的步長(zhǎng)因子s放入勢(shì)阱中心的計(jì)算公式中，有

p ?m，n （t+1）=s|x ?m，n －（φp ?m，n （t）+

（1－φ）g ??best ，n （t））| ?（20）

至此，將式（14）和式（20）作為粒子局部吸引子的兩種更新方式，可根據(jù)不同情況進(jìn)行選擇。在算法初始化過(guò)程中事先給每個(gè)粒子設(shè)置一個(gè)界限值l ??lim ?并初始化跟蹤值t ??trial ?為0。若每次迭代生成的新粒子對(duì)應(yīng)的適應(yīng)度值并未得到改進(jìn)，則跟蹤值遞增一次；若迭代出的新粒子對(duì)應(yīng)的適應(yīng)度值相比前一次有所改善，則置t ??trial ?=0。當(dāng)出現(xiàn)t ??trial ?>l ??lim ?時(shí)，代表算法已陷入局部最優(yōu)情況， 適應(yīng)度值過(guò)早收斂，此時(shí)采取 Levy 飛行方法，根據(jù)式（20）重新計(jì)算局部吸引子位置進(jìn)行后續(xù)迭代。

2.3.2 非線性調(diào)整收縮膨脹因子

通過(guò)重新設(shè)計(jì)調(diào)整收縮膨脹因子a的計(jì)算公式，可以在每一次迭代過(guò)程中都充分考慮并結(jié)合粒子最優(yōu)位置和平均位置的距離差，達(dá)到動(dòng)態(tài)調(diào)整粒子搜索范圍的效果，提高算法的收斂能力，避免陷入局部最優(yōu)。對(duì)于如何調(diào)整收縮膨脹因子a的問(wèn)題，本文引入了相似性的概念 ?［19］ 。定義粒子m和粒子n之間的相似度如下：

S（m，n）=1－（ d（m，n） d ??max ??） 2 ??（21）

式中，d（m，n）為兩粒子間的歐氏距離差；d ??max ?為空間粒子間的最大距離。

由式（21）可知，當(dāng)d（m，n）→0時(shí)，S（m，n）→ 1；當(dāng)d（m，n）→1時(shí)，S（m，n）→0。距離越近相似度越高，反之相似度越低。

根據(jù)這個(gè)特性就可根據(jù)粒子的局部吸引子位置和粒子群體的全局最優(yōu)位置的距離差來(lái)選擇調(diào)整收縮膨脹因子。設(shè)定當(dāng)前粒子局部吸引子位置p ?m，n 和全局最優(yōu)位置g ??best ，n 間的相似度為S（p ?m，n ，g ??best ，n ），當(dāng)S（p ?m，n ，g ??best ，n ）→0時(shí)，說(shuō)明此時(shí)的距離差最大，該粒子距離群體的全局最優(yōu)位置最遠(yuǎn)，所以在下一次迭代時(shí)需要脫離現(xiàn)在的搜索區(qū)域，加強(qiáng)全局搜索的能力，因此收縮膨脹因子值也隨之增大；當(dāng)S（p ?m，n ，g ??best ，n ）→1時(shí)，該粒子距離全局最優(yōu)最為接近，無(wú)需跳出且只需在當(dāng)前的小區(qū)域內(nèi)加大局部搜索的能力即可搜索到最優(yōu)解，所以收縮膨脹因子值隨之減小。

當(dāng)S（p ?m，n ，g ??best ，n ）（0，1）時(shí)，收縮膨脹因子的更新公式如下：

a m=a ??min ?+（a ??max ?－a ??min ?）·

S（p ?m，n ，g ??best ，n ） ?t ??max ?－t t ??max ?????（22）

本文設(shè)定a ??max ?=0.9，a ??min ?=0.3。將經(jīng)過(guò)式（22）調(diào)整過(guò)的收縮膨脹因子應(yīng)用于式（16），即可達(dá)到非線性動(dòng)態(tài)調(diào)整搜索范圍的目的。

2.3.3 變權(quán)重方法計(jì)算平均最優(yōu)位置

QPSO 算法中引入的平均最優(yōu)位置W n（t）只是將種群內(nèi)所有粒子的個(gè)體歷史最優(yōu)值相加求平均計(jì)算。這樣的計(jì)算方法自動(dòng)認(rèn)定每個(gè)粒子的個(gè)體最優(yōu)對(duì)最終的平均最優(yōu)值所起到的作用一致，如下式所示：

W n（t）=（ 1 M ∑ M m=1 p ?m，1 （t）， 1 M ∑ M m=1 p ?m，2 （t），…，

1 M ∑ M m=1 p ?m，N （t）） ?（23）

然而在實(shí)際計(jì)算時(shí)，每個(gè)粒子的適應(yīng)度值都不盡相同，適應(yīng)度值越大說(shuō)明該粒子在種群中就越重要，將這些粒子的個(gè)體最優(yōu)都統(tǒng)一計(jì)算求取平均最優(yōu)值顯得不夠準(zhǔn)確，所以本文將粒子根據(jù)適應(yīng)度值的大小降序排列，然后采用文獻(xiàn)［20-21］中的方法為排序好的粒子分配不同大小的權(quán)重系數(shù)。整體遵循適應(yīng)度值越大，在整體收斂逼近最優(yōu)解的過(guò)程中起到的作用越大，其權(quán)重系數(shù)也應(yīng)越大的準(zhǔn)則。調(diào)整后的平均最優(yōu)位置計(jì)算如下：

W n（t）=（ 1 M ∑ M m=1 α ?m，1 p ?m，1 （t）， 1 M ∑ M m=1 α ?m，2 p ?m，2 （t），…，

1 M ∑ M m=1 α ?m，N p ?m，N （t）） ?（24）

式中，α ?m，n 為權(quán)重系數(shù)，本文中根據(jù)不同粒子的適應(yīng)度大小將權(quán)重系數(shù)從1.5到0.5線性變化。

2.4 改進(jìn)量子粒子群優(yōu)化算法的求解流程

針對(duì)求解量子粒子群的勢(shì)阱中心p ?m，n 時(shí)可能會(huì)陷入局部最優(yōu)的問(wèn)題，采用 Levy 飛行策略使其跳出局部最小值，避免算法提早收斂；針對(duì)原特征長(zhǎng)度L ?m，n 并未充分考慮全局搜索和局部搜索協(xié)同的問(wèn)題，制定了非線性動(dòng)態(tài)調(diào)整收縮膨脹因子和變權(quán)重方法計(jì)算平均最優(yōu)位置等改進(jìn)方式提高了 QPSO 算法的準(zhǔn)確性和魯棒性。改進(jìn)量子粒子群算法的流程如圖2所示。具體步驟如下：

（1）初始化種群大小、維數(shù)、最大迭代次數(shù)等種群參數(shù)，設(shè)定粒子迭代的起始位置，極限值l ??lim ?和跟蹤值t ??trial ?。

（2）根據(jù)式（11）所示目標(biāo)函數(shù)求取當(dāng)前過(guò)程的適應(yīng)度值，從而確定個(gè)體最優(yōu)p ?m，n 和全局最優(yōu)g ??best ，n 。

（3）進(jìn)入循環(huán)，計(jì)算得出各粒子的局部吸 引子。

（4）利用上文所述的變權(quán)重方法計(jì)算平均最優(yōu)位置W n，并根據(jù)當(dāng)前點(diǎn)與歷史最優(yōu)點(diǎn)的距離差動(dòng)態(tài)調(diào)整收縮膨脹因子，計(jì)算出特征長(zhǎng)度L ?m，n 。

（5）根據(jù) IQPSO 算法的迭代方程得出粒子的最新位置，計(jì)算其適應(yīng)度值判斷是否有所優(yōu)化。

（6）若有優(yōu)化，表明粒子處于正常搜索狀態(tài)，置跟蹤值t ??trial ?為0，跳轉(zhuǎn)到步驟（8）；若沒(méi)有優(yōu)化，則表明粒子可能會(huì)陷入局部最優(yōu)區(qū)域，跟蹤值自增1并轉(zhuǎn)去判斷是否超過(guò)極限值l ??lim ?。

（7）當(dāng)t ??trial ?

（8）通過(guò)分析新粒子的適應(yīng)度值確定個(gè)體最優(yōu)p ?m，n 和全局最優(yōu)g ??best ，n 。

（9）判斷是否滿(mǎn)足終止條件，若滿(mǎn)足則輸出最 ?終的g ??best ?，若不滿(mǎn)足則返回步驟（3）進(jìn)入下一次迭代。

3 仿真驗(yàn)證與分析

3.1 對(duì)單點(diǎn)進(jìn)行改進(jìn)算法的求解驗(yàn)證

按照2.4節(jié)所描述的改進(jìn)QPSO算法流程，對(duì)圖1所示的SR4C機(jī)械臂模型進(jìn)行逆運(yùn)動(dòng)學(xué)求解驗(yàn)證。仿真實(shí)驗(yàn)環(huán)境為MATLAB 2018b，Win10 Intel（R） Core（TM） i5-7300Q CPU @ ?2.50 GHz。 為避免實(shí)驗(yàn)的偶然性，在機(jī)器人工作空間內(nèi)隨機(jī)選定5組期望位姿和初始關(guān)節(jié)位置，并分別用引入收縮因子 ?［9］ 的自適應(yīng)PSO算法（將其記為APSO） 、采用混沌映射和高斯分布的QPSO算法 ?［15］ （將其記為QPSO）、本文所述的改進(jìn)QPSO算法（將其記為IQPSO）進(jìn)行求解，然后對(duì)比分析。5組初始關(guān)節(jié)位置和期望位姿矢量見(jiàn)表2、表3。

為方便統(tǒng)計(jì)，表2、表3中的各數(shù)據(jù)取小數(shù)點(diǎn)后四位。表2中的關(guān)節(jié)角度用弧度值表示；表3中的 RPY 角度ψ ?r 、θ ?r 、φ ?r 表示姿態(tài)矢量，用弧度制表示；p x、p y、p z表示位置矢量。

算法的目標(biāo)函數(shù)采用式（11）所示的形式，對(duì)式（11）中三個(gè)不同的權(quán)重系數(shù)進(jìn)行歸一化處理，用于平衡各子項(xiàng)的數(shù)量級(jí)。根據(jù) SR4C機(jī)器人的D-H 參數(shù)以及參照表2、表3列出的關(guān)節(jié)角、位置、姿態(tài)數(shù)據(jù)，確定姿態(tài)誤差、關(guān)節(jié)角變化函數(shù)的數(shù)量級(jí)在10 0，位置誤差的數(shù)量級(jí)在10 3。為便于計(jì)算，同時(shí)結(jié)合調(diào)試經(jīng)驗(yàn)選取姿態(tài)誤差的權(quán)值系數(shù)μ=1作為基準(zhǔn)；關(guān)節(jié)角度變化的數(shù)量級(jí)單位與姿態(tài)誤差保持一致，選擇 γ=μ=1；位置誤差的單位為毫米，數(shù)量級(jí)為10 3，確定λ=1×10 ?－3 。

設(shè)置IQPSO算法的最大迭代次數(shù)為400，種群粒子數(shù)量為50；APSO算法的參數(shù)設(shè)定和實(shí)現(xiàn)參考文獻(xiàn)［9］。將3種算法分別用于求解表3所列的5組期望位姿，將每種算法所收斂的最終適應(yīng)度值統(tǒng)計(jì)在表4中，為便于記錄將表中數(shù)據(jù)統(tǒng)一保留4位有效數(shù)字。

由表4可以看出，求解不同的期望位姿，本文所述的 IQPSO 算法的收斂效果基本都比其他兩種方法更好，從中可以看出該算法的求解效率高，同時(shí)也具有較高的求解穩(wěn)定性。

為方便直觀地看出各種算法在迭代過(guò)程中的表現(xiàn)，本文將每個(gè)期望位姿的適應(yīng)度值相加除以測(cè)試的組數(shù)，取對(duì)數(shù)處理后生成的平均適應(yīng)度值的收斂曲線如圖3所示。

由圖3可以看出：IQPSO算法在整體求解過(guò)程中，其收斂速度和求解精度要優(yōu)于QPSO和APSO這兩種改進(jìn)粒子群算法的收斂速度和求解精度，同時(shí)相比較標(biāo)準(zhǔn)QPSO算法而言，IQPSO算法在更早的迭代過(guò)程中就已經(jīng)以一個(gè)相對(duì)較快的加速度進(jìn)行收斂，在迭代超過(guò)40次后再以一個(gè)相對(duì)較慢的速度進(jìn)行局部的仔細(xì)搜索直到收斂到最優(yōu)值。整個(gè)過(guò)程相比較其他算法而言，更好地平衡了粒子的全局搜索能力和局部搜索能力，一定程度上證明了2.3節(jié)所述觀點(diǎn)。

為了更加清晰地評(píng)價(jià)各種算法在求解精度上的差異，統(tǒng)計(jì)各種算法在經(jīng)過(guò)5組期望點(diǎn)求解后的實(shí)際位姿T ??cur ?相對(duì)于期望位姿T ??des ??的誤差，將位置誤差和姿態(tài)誤差的均值（ Δ R+ Δ P）/2作為對(duì)比分析的指標(biāo)，其中位置誤差值 Δ R和姿態(tài)誤差值 Δ P遵循下式：

Δ R= ∑ 3 m=1 ∑ 3 n=1 （r ?mn －r ?mn， des ?） 2 ??（25）

Δ P=

（p x－p ?x， des ?） 2+（p y－p ?y， des ?） 2+（p z－p ?z， des ?） 2 ?（26）

經(jīng)數(shù)據(jù)處理后將誤差的平均值、最大值、最小值統(tǒng)計(jì)在表5中。

由表5可以看出：IQPSO算法在對(duì)單點(diǎn)的求解過(guò)程中所產(chǎn)在的誤差最小，證明了該算法在應(yīng)用于機(jī)器人逆運(yùn)動(dòng)學(xué)求解問(wèn)題時(shí)所體現(xiàn)出的優(yōu)越性。此外分別運(yùn)用這三種算法對(duì)5組期望位姿進(jìn)行逆運(yùn)動(dòng)學(xué)求解時(shí)，IQPSO算法的平均運(yùn)算時(shí)間為0.12791s，QPSO算法的平均運(yùn)算時(shí)間為 0.1506 s， APSO算法的平均運(yùn)算時(shí)間為0.2156 s，從中可以看出IQPSO具有更快的求解速度，耗時(shí)相比較其他算法來(lái)說(shuō)也有一定的優(yōu)勢(shì)，驗(yàn)證了算法整體在迭代過(guò)程中具有更快的處理速度。

3.2 對(duì)連續(xù)軌跡進(jìn)行改進(jìn)算法的求解驗(yàn)證

本節(jié)將算法用于求解連續(xù)軌跡上的各個(gè)插值點(diǎn)，對(duì)比這幾種算法在求解連續(xù)軌跡時(shí)的適應(yīng)度值收斂效果。同時(shí)通過(guò)計(jì)算軌跡上各個(gè)插值點(diǎn)的姿態(tài)誤差和位置誤差，分析比較各種算法在針對(duì)連續(xù)軌跡求解問(wèn)題上所體現(xiàn)出的求解精度優(yōu)劣性。

在機(jī)器人工作空間內(nèi)隨機(jī)選取4個(gè)目標(biāo)點(diǎn)A、B、C、D，然后采用 MATLAB 工具箱中的插值函數(shù)進(jìn)行每?jī)牲c(diǎn)間插值，插值50個(gè)點(diǎn)共形成2條連續(xù)軌跡AB、CD。再將幾種算法分別應(yīng)用于軌跡AB和CD進(jìn)行實(shí)驗(yàn)。圖4是選定的兩條期望軌跡，圖5則是各種算法求解出的各插值點(diǎn)所組成的跟蹤軌跡。兩條軌跡端點(diǎn)A、B、C、D的位姿信息記錄在表6中。

將各種算法對(duì)兩條軌跡上的插值點(diǎn)獨(dú)立重復(fù)求解5次后，計(jì)算并繪制出來(lái)兩條軌跡上的插值點(diǎn)的平均位置誤差和平均姿態(tài)誤差變化的對(duì)比如圖6、圖7所示。

同時(shí)根據(jù)生成的位置誤差和姿態(tài)誤差曲線，分別計(jì)算兩條軌跡共100個(gè)插值點(diǎn)所產(chǎn)生誤差的最大值、最小值、平均值和標(biāo)準(zhǔn)差，其中最大最小值體現(xiàn)了求解算法的性能；平均值體現(xiàn)了算法求解的精度；標(biāo)準(zhǔn)差則體現(xiàn)出算法的穩(wěn)定性和魯棒性。表7統(tǒng)計(jì)了位置誤差的各項(xiàng)指標(biāo)，表8統(tǒng)計(jì)了姿態(tài)誤差的各項(xiàng)指標(biāo)。

分析圖6、圖7得知，幾種算法在對(duì)軌跡上各插值點(diǎn)進(jìn)行求解時(shí)都會(huì)存在一些控制精度上的波動(dòng)，但是整體保持在10 ?－5 數(shù)量級(jí)以下；本文所述IQPSO算法求解出的各插值點(diǎn)相較于期望的各點(diǎn)來(lái)說(shuō)， 位置誤差和姿態(tài)誤差都比QPSO算法和APSO算法求解的誤差小，求解精度有了很大的提高， 這一點(diǎn)在表7、表8的最大最小值比較中也得以體現(xiàn)。同時(shí)IQPSO算法在求解中兼顧了精度要求和穩(wěn)定性要求，誤差的平均值和標(biāo)準(zhǔn)差相比較其他兩種算法也具有明顯的優(yōu)勢(shì)。進(jìn)一步證明了本文2.3節(jié)所述觀點(diǎn)。

4 實(shí)驗(yàn)驗(yàn)證

為驗(yàn)證算法的可行性，將算法應(yīng)用于團(tuán)隊(duì)現(xiàn)有的實(shí)驗(yàn)平臺(tái)，在新松SR4C機(jī)械臂實(shí)體上進(jìn)行運(yùn)動(dòng)學(xué)求解的實(shí)物驗(yàn)證?，F(xiàn)有實(shí)驗(yàn)平臺(tái)由主控器、 4 kg 新松SR4C機(jī)械臂、控制柜（內(nèi)含6個(gè) Panasonic 驅(qū)動(dòng)器）、示教器組成，如圖8所示，機(jī)器人本體有6個(gè)關(guān)節(jié)電機(jī)，通過(guò)動(dòng)力線和編碼器信號(hào)線與控制器內(nèi)的驅(qū)動(dòng)器相連。各個(gè)驅(qū)動(dòng)器之間采用串聯(lián)方式并由第一個(gè)驅(qū)動(dòng)器通過(guò)EtherCAT與主控器建立通信。

進(jìn)行實(shí)物實(shí)驗(yàn)時(shí)，首先需要在示教器上輸入示教點(diǎn)，編寫(xiě)示教運(yùn)動(dòng)指令；然后主控器通過(guò)TCP/IP通信接收到示教指令并進(jìn)行解析；再根據(jù)解析結(jié)果調(diào)用插補(bǔ)運(yùn)算部分中的規(guī)劃函數(shù)和逆運(yùn)動(dòng)學(xué)求解算法，生成組成軌跡的各個(gè)插補(bǔ)點(diǎn)以及對(duì)應(yīng)的關(guān)節(jié)角數(shù)據(jù)，控制機(jī)械臂各關(guān)節(jié)電機(jī)完成相應(yīng)運(yùn)動(dòng)。

為充分驗(yàn)證IQPSO算法在機(jī)械臂實(shí)際工作時(shí)的效果，本文分別針對(duì)同一平面下的定姿態(tài)軌跡和曲面變姿態(tài)軌跡兩種不同情況進(jìn)行實(shí)物實(shí)驗(yàn)，分析驗(yàn)證算法的合理性和實(shí)用性。

4.1 平面軌跡實(shí)驗(yàn)

本節(jié)實(shí)驗(yàn)選定一條雙扭線軌跡作為實(shí)驗(yàn)對(duì)象。首先在示教器上輸入相應(yīng)的點(diǎn)位信息和運(yùn)動(dòng)指令，示教指令如圖9所示。為記錄下機(jī)械臂運(yùn)行的軌跡，自制了一個(gè)連接件將噴槍固定在機(jī)械臂末端，在白紙上進(jìn)行噴繪實(shí)驗(yàn)。然后主控器接收并解析圖9所示的用戶(hù)程序，根據(jù)“MOVL”的解析結(jié)果調(diào)用所對(duì)應(yīng)的速度規(guī)劃算法生成對(duì)應(yīng)的插補(bǔ)點(diǎn)，再利用本文所研究的IQPSO算法對(duì)各個(gè)插補(bǔ)點(diǎn)進(jìn)行逆運(yùn)動(dòng)學(xué)求解，生成每個(gè)點(diǎn)位所需的關(guān)節(jié)角數(shù)據(jù)，最后點(diǎn)位數(shù)據(jù)經(jīng)由主控器發(fā)送給各從站驅(qū)動(dòng)器，在完成關(guān)節(jié)數(shù)據(jù)轉(zhuǎn)換為編碼器值后，各從站驅(qū)動(dòng)器控制本體六個(gè)電機(jī)完成相應(yīng)的軌跡運(yùn)動(dòng)。利用圖9所示用戶(hù)示教程序所繪制的軌跡如圖10所示。

然后按照?qǐng)D11所示先在機(jī)械臂末端安裝靶球并采用Leica-AT403型號(hào)的激光跟蹤儀對(duì)機(jī)械臂所走軌跡進(jìn)行跟蹤測(cè)試，

跟蹤軌跡如圖12所示。軌跡上各點(diǎn)對(duì)應(yīng)關(guān)節(jié)角變化如圖13所示。

從圖10、圖12中可以看出，機(jī)械臂末端根據(jù)IQPSO算法解算出的點(diǎn)位所組成的軌跡段連續(xù)平滑且并未出現(xiàn)明顯的突變情況，說(shuō)明算法在計(jì)算各插值點(diǎn)過(guò)程中未出現(xiàn)無(wú)解或超出關(guān)節(jié)限位的情況，證明了算法在實(shí)際應(yīng)用時(shí)的可靠性。

從圖13中可以看出，各關(guān)節(jié)角的變化曲線都呈現(xiàn)平滑且連續(xù)的特征，證明了利用IQPSO算法求解的各插值點(diǎn)所對(duì)應(yīng)的關(guān)節(jié)角滿(mǎn)足運(yùn)動(dòng)平穩(wěn)性要求。

4.2 曲面軌跡實(shí)驗(yàn)

本節(jié)實(shí)驗(yàn)中同時(shí)改變機(jī)械臂的姿態(tài)和位置信息，讓末端執(zhí)行器在一個(gè)平滑半球的曲面上運(yùn)行，以此驗(yàn)證IQPSO算法在求解曲面軌跡上各個(gè)變姿態(tài)點(diǎn)位時(shí)的效果。在操作臺(tái)上固定一個(gè)半球面，讓機(jī)械臂在半球面上進(jìn)行變姿態(tài)的曲面畫(huà)圓作業(yè)。

繪制變姿態(tài)圓時(shí)，示教調(diào)用步驟與4.1節(jié)所述一致，實(shí)際運(yùn)行的軌跡如圖14所示。激光跟蹤儀跟蹤軌跡如圖15所示。軌跡上各點(diǎn)對(duì)應(yīng)關(guān)節(jié)角變化如圖16所示。

從圖14、圖15中可以看出，雖在曲面上畫(huà)圓導(dǎo)致各插值點(diǎn)的姿態(tài)信息一直在變化，但是也并未出現(xiàn)因各種奇異位姿而導(dǎo)致執(zhí)行中斷或是軌跡上的突變問(wèn)題，整體軌跡和平面上運(yùn)行時(shí)一樣都呈現(xiàn)連續(xù)且平滑的曲線，因此可以看出運(yùn)用IQPSO算法對(duì)不同軌跡上的插值點(diǎn)進(jìn)行求解具有較為優(yōu)良的穩(wěn)定性和求解精度。

圖16所示的關(guān)節(jié)角變化曲線和圖13所示的變化曲線都一樣保持了平滑連續(xù)無(wú)奇變的情況，再一次證明了利用IQPSO算法進(jìn)行逆運(yùn)動(dòng)學(xué)求解可以達(dá)到運(yùn)動(dòng)控制過(guò)程中的運(yùn)動(dòng)平穩(wěn)性原則，也與前文目標(biāo)函數(shù)構(gòu)建過(guò)程中的約束條件相 符合。

5 結(jié)語(yǔ)

本文首先根據(jù)機(jī)器人的正運(yùn)動(dòng)學(xué)模型，以位姿誤差最小為優(yōu)化目標(biāo)，同時(shí)結(jié)合運(yùn)動(dòng)平穩(wěn)性原則建立了優(yōu)化目標(biāo)函數(shù)，將運(yùn)動(dòng)學(xué)求解問(wèn)題轉(zhuǎn)換為優(yōu)化控制問(wèn)題進(jìn)行分析；然后在量子粒子群算法的基礎(chǔ)上引入Levy飛行策略改進(jìn)粒子更新方式，非線性地動(dòng)態(tài)調(diào)整收縮膨脹因子，采用變權(quán)重方法計(jì)算最優(yōu)平均位置，從而設(shè)計(jì)了一種改進(jìn)的量子粒子群（IQPSO）算法。之后模擬單點(diǎn)位姿和連續(xù)軌跡兩種不同的求解情況進(jìn)行不同算法的仿真對(duì)比實(shí)驗(yàn)。通過(guò)適應(yīng)度收斂曲線、位姿誤差變化圖等仿真結(jié)果證明了IQPSO算法在進(jìn)行逆解運(yùn)算時(shí)具有更快的迭代速度，更高的精度和更好的穩(wěn)定性。最后為了證明該算法在實(shí)際運(yùn)動(dòng)時(shí)的可行性，將IQPSO算法編寫(xiě)進(jìn)控制器中進(jìn)行機(jī)械臂本體的實(shí)物實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明，機(jī)械臂末端實(shí)際運(yùn)行軌跡具有連續(xù)且平滑的特性，并沒(méi)有出現(xiàn)明顯突變或因奇異問(wèn)題導(dǎo)致運(yùn)動(dòng)中斷的現(xiàn)象，證明了該算法滿(mǎn)足機(jī)器人實(shí)際示教運(yùn)動(dòng)時(shí)的穩(wěn)定性 準(zhǔn)則。

該算法基于新松自研SR4C機(jī)器人進(jìn)行仿真和實(shí)物驗(yàn)證，理論上可以應(yīng)用于其他的諸如ABB、FANUC、KUKA等品牌的6軸工業(yè)機(jī)器人模型的運(yùn)動(dòng)學(xué)求解，因此后續(xù)工作將根據(jù)實(shí)際情況建立不同的機(jī)器人運(yùn)動(dòng)學(xué)模型利用該算法進(jìn)行逆解求解。

參考文獻(xiàn) ：

［1］ ?李冠琦， 武建德， 李瑞琴. 基于旋量理論的7自由機(jī)械臂運(yùn)動(dòng)學(xué)建模與分析［J］. 機(jī)械制造與自動(dòng)化， 2022， 51（1）：105-107.

LI Guanqi， WU Jiande， LI Ruiqin. Kinematics Modeling and Analysis of Seven-degree-freedom Robotic Arm Based on SpinorTheory［J］. Machine Building & Automation， 2022， 51（1）：105-107.

［2］ ?尹寧寧， 潘松峰， 楊彥平， 等. 一類(lèi)串行關(guān)節(jié)式機(jī)器人逆運(yùn)動(dòng)學(xué)建模與仿真研究［J］. 青島大學(xué)學(xué)報(bào)（工程技術(shù)版）， 2016， 31（4）：79-83.

YIN Ningning， PAN Songfeng， YANG Yanping， et al. A Kind of Serial Articulated Robot Inverse Kinematics Modeling［J］. Journal of Qingdao University（Engineering & Technology Edition）， 2016， 31（4）：79-83.

［3］ ?BEKEY ?G A. Springer Handbook of Robotics（B.Sic-iliano and O. Khatib; 2008）［Book Review］［J］. IEEE Robotics & Automation Magazine， 2008， 15（3）：110-110.

［4］ ?房立金， 高瑞. 一般6R機(jī)器人逆運(yùn)動(dòng)學(xué)算法的改進(jìn)［J］. 機(jī)械科學(xué)與技術(shù)， 2018， 37（9）：1325-1330.

FANG Lijin，GAO Rui. lmproving Inverse Kinematics Algorithm for General 6-DOF Robots［J］. Mechanical Science and Technology for Aerospace Engineering， 2018， 37（9）：1325-1330.

［5］ ?ZAPLANA ?I， BASANEZ L. A Novel Closed-form Solution for the Inverse Kinematics of Redundant Manipulators through Workspace Analysis［J］. Mechanism and Machine Theory， 2018， 121：829-843.

［6］ ?KHALEEL ?H Z. Inverse Kinematics Solution for Redundant Robot Manipulator Using Combination of GA and NN［J］. Al-Khawarizmi Engineering Journal， 2018， 14（1）：136-144.

［7］ ?LIU ?Yiyang， XI Jiali， BAI Hongfei， et al. A General Robot Inverse Kinematics Solution Method Based on Improved PSO Algorithm［J］. IEEE Access， 2021， 9：32341-32350.

［8］ ?SANCAKTAR ?I， TUNA B， ULUTAS M. Inverse Kine-matics Application on Medical Robot Using ?Adapted ?PSO Method［J］. Engineering Science and Technology， an International Journal， 2018， 21（5）：1006-1010.

［9］ ?姜濤， 曹琦. 基于改進(jìn)粒子群算法的機(jī)械臂逆運(yùn)動(dòng)學(xué)求解［J］. 長(zhǎng)春理工大學(xué)學(xué)報(bào)（自然科學(xué)版）， 2023， 46（1）：59-64.

JIANG Tao， CAO Qi. Inverse Kinematics Solution of Manipulator Based on Improved Particle Swarm Optimization Algorithm［J］. Journal of Changchun University of Science and Technology（Natural Science Edition） ， 2023， 46（1）：59-64.

［10］ ?DU ?Shuxing， CUI Jiashan. An Improved Strategy of PSO for Solving Multimodal and Higher Dimensional Complicated Optimization Problems［C］∥2015 Inter-national Conference on Computational Intelligence and Communication Networks（CICN）. Jabalpur：IEEE， 2015：1229-1234.

［11］ ?孫俊. 量子行為粒子群優(yōu)化算法研究［D］.無(wú)錫：江南大學(xué)，2009.

SUN Jun. Study on Drive System of the Full-face Rock Tunnel Boring Machine［D］. Wuxi：Jiangnan University， 2009.

［12］ ?TIAN ?Na， LAI C H. Parallel Quantum-behaved Particle Swarm Optimization［J］. International Journal of Machine Learning and Cybernetics， 2014， 5（2）：309-318.

［13］ ?SUN ?J， FANG W， WU X ， et al. Quantum-behaved Particle Swarm Optimization：Analysis of Individual Particle Behavior and Parameter Selection［J］. Evolutionary Computation， 2012， 20（3）：349-393.

［14］ ?TURGUT ?O E， TURGUT M S， COBAN M T. Chaotic Quantum Behaved Particle Swarm Optimization Algorithm for Solving Nonlinear System of Equations［J］. Computers & Mathematics with Applications， 2014， 68（4）：508-530.

［15］ ?ZHANG ?Feng. Intelligent Task Allocation Method Based on Improved QPSO in Multi-agent System［J］. Journal of Ambient Intelligence and Humanized Computing， 2020， 11（2）：655-662.

［16］ ?何光， 盧小麗， 李高西. 基于雙策略協(xié)同進(jìn)化的QPSO算法及其應(yīng)用［J］. 計(jì)算機(jī)應(yīng)用研究， 2023， 40（2）：418-423.

HE Guang，LU Xiaoli，LI Gaoxi. Double Strategies Coevolutionary Quantum-behaved Particle Swarm Optimization Algorithm and Its Application［J］. Application Research of Computers， 2023， 40（2）：418-423.

［17］ ?LIU ?Yahui， CAO Buyang. A Novel Ant Colony Opti-mization Algorithm with Levy Flight［J］. IEEE Access， 2020， 8：67205-67213.

［18］ ?HAKLI ?H， UGUZ H. A Novel Particle Swarm Opti-mization Algorithm with Levy Flight［J］. Applied Soft Computing， 2014， 23：333-345.

［19］ ?ALKHRAISAT ?H， RASHAIDEH H. Dynamic Inertia Weight Particle Swarm Optimization for Solving Nonogram Puzzles［J］. International Journal of Advanced Computer Science and Applications（IJACSA）， 2016， 7（10） ：277-280.

［20］ ?XI ?Maolong， SUN Jun， XU Wenbo. An Improved Quantum-behaved Particle Swarm Optimization Algorithm with Weighted Mean Best Position［J］. Applied Mathematics and Computation， 2008， 205（2）：751-759.

［21］ ?程鑫，周書(shū)敏，匡森. 自適應(yīng)混合策略的量子行為粒子群優(yōu)化算法［C］∥第23屆中國(guó)系統(tǒng)仿真技術(shù)及其應(yīng)用學(xué)術(shù)年會(huì)會(huì)議論文集.合肥：合肥工業(yè)大學(xué)出版社， 2022：257-261.

CHENG Xin，ZHOU Shumin，KUANG Sen.Quantum-behaved Particle Swarm Optimization Algorithm Based on Adaptive Mixed Strategy［C］∥23rd Proceedings of the China Annual Academic Conference on System Simulation Technology and Its Applications. Hefei：Hefei University of Technology Press， 2022：257-261.

（ 編輯 王艷麗 ）

作者簡(jiǎn)介 ：

陳卓凡 ，男，1999年生，碩士研究生。研究方向?yàn)楣I(yè)機(jī)器人運(yùn)動(dòng)控制。E-mail：958002697@qq.com。

王斌銳 （通信作者），男，1978年生，教授、博士研究生導(dǎo)師。研究方向?yàn)榉律鷻C(jī)器人智能控制。E-mail：wangbrpaper@163.com。