最優(yōu)化基座位姿擾動(dòng)的七自由度空間機(jī)械臂軌跡規(guī)劃方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明屬于航空航天控制領(lǐng)域，更具體地說，涉及最優(yōu)化基座位姿擾動(dòng)的七自由 度空間機(jī)械臂軌跡規(guī)劃方法。
【背景技術(shù)】
[0002] 隨著國家國民經(jīng)濟(jì)與國防工業(yè)技術(shù)的飛速發(fā)展，航天器衛(wèi)星等數(shù)量越來越多，人 們越來越關(guān)注空間機(jī)器人在空間技術(shù)的發(fā)展。但是由于各種原因如衛(wèi)星故障、失效或任務(wù) 結(jié)束而被放棄等，衛(wèi)星被留在空中成為了空中廢品，對軌道資源以及其他航天器安全都會(huì) 產(chǎn)生巨大影響。
[0003] 目前，越來越多的學(xué)者不斷地研究空間機(jī)器人的姿態(tài)穩(wěn)定問題。VAFA等關(guān)于自由 漂浮系統(tǒng)的非完整路徑規(guī)劃提出了自校正運(yùn)動(dòng)法和擾動(dòng)圖法。但自校正運(yùn)動(dòng)法只能調(diào)整基 座姿態(tài)，其最終狀態(tài)改變不了。M.A Torres等提出增強(qiáng)干擾圖（EDM)方法，這種方法減小了 姿態(tài)干擾，但其計(jì)算需占用較大存儲空間，計(jì)算慢且效率太低。P. F Huang在2006年提出 最小基座反作用干擾的最優(yōu)路徑規(guī)劃方法，它是基于遺傳算法的，可以穩(wěn)定控制基座姿態(tài)， 但遺傳算法的實(shí)現(xiàn)較粒子群算法復(fù)雜。戈新生等提出基于遺傳算法的最優(yōu)軌跡規(guī)劃，解決 了機(jī)械臂系統(tǒng)運(yùn)動(dòng)規(guī)劃問題。2011年，史也等提出了量子粒子群優(yōu)化算法進(jìn)行關(guān)節(jié)軌跡優(yōu) 化，控制基座姿態(tài)。以上均不是以七自由度的機(jī)械臂為對象同時(shí)規(guī)劃基座的位置和姿態(tài)，本 發(fā)明以七自由度空間機(jī)械臂為仿真對象，采用量子行為粒子群優(yōu)化算法同時(shí)規(guī)劃基座的位 置和姿態(tài)，使基座的位姿擾動(dòng)最小。

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

[0004] 鑒于已有生產(chǎn)方法存在的缺陷，本發(fā)明提供最優(yōu)化基座位姿擾動(dòng)的七自由度空間 機(jī)械臂軌跡規(guī)劃方法，該方法是規(guī)劃七自由度空間機(jī)械臂基座的位置和姿態(tài)，利用量子行 為粒子群優(yōu)化算法（QPS0算法）搜索關(guān)節(jié)運(yùn)動(dòng)的最優(yōu)軌跡，達(dá)到機(jī)械臂運(yùn)動(dòng)對基座位姿產(chǎn) 生的擾動(dòng)最小的目的。
[0005] 為實(shí)現(xiàn)上述目的，本發(fā)明所采用的技術(shù)方案是：
[0006] 包括以下步驟：（1)對七自由度空間機(jī)械臂建模，定義機(jī)械臂的連桿長度、連桿以 及基座的質(zhì)量等幾何參數(shù)，利用廣義雅可比矩陣建立七自由度機(jī)械臂的運(yùn)動(dòng)學(xué)方程。（2) 利用四元數(shù)描述機(jī)械臂的基座姿態(tài)，采用正弦函數(shù)五階多項(xiàng)式參數(shù)化空間機(jī)械臂的關(guān)節(jié)軌 跡，根據(jù)空間機(jī)械臂基座的位置和姿態(tài)建立系統(tǒng)狀態(tài)方程，以上均要滿足關(guān)節(jié)角、角速度和 角加速度的約束條件。（3)根據(jù)基座位置和姿態(tài)控制精度指標(biāo)和機(jī)械臂的運(yùn)動(dòng)約束建立適 應(yīng)度函數(shù)。（4)采用量子行為粒子群優(yōu)化算法優(yōu)化目標(biāo)函數(shù)，求解出適應(yīng)度函數(shù)中的最優(yōu) 解，代入?yún)?shù)化后的關(guān)節(jié)軌跡方程，實(shí)現(xiàn)對基座位置和姿態(tài)的最優(yōu)化。
[0007] 另外，目標(biāo)函數(shù)就是適應(yīng)度函數(shù)，定義的適應(yīng)度函數(shù)為：
[0008] LlN 丄UOldSUUU A yJ^ rVJ Z/5 }J^
[0009] 式中，b指基座，q，p分別表示基座姿態(tài)，基座位置。δ qb是基座姿態(tài)的四元數(shù)誤 差，Spb是基座的位置誤差，分別是關(guān)節(jié)速度和角加速度的約束。J q，Jp分別是姿態(tài) 誤差和位置誤差的權(quán)重系數(shù)，分別是關(guān)節(jié)角速度、角加速度限制的權(quán)重系數(shù)，它們均 由精度要求所決定，此處
[0010] 利用QPSO算法搜索空間機(jī)械臂關(guān)節(jié)運(yùn)動(dòng)的最優(yōu)軌跡包括以下步驟：
[0011] 步驟1 :置時(shí)間t = 0,在問題空間中初始化M個(gè)粒子的當(dāng)前位置X1 (0)，（粒子i =1，2,... M)，并置個(gè)體最好位置P1 (0) = X1 (0)。
[0012] 步驟2 :根據(jù)下式計(jì)算粒子的平均最好位置（j指空間維數(shù)，j = 1，2, ... 7):
[0013]
[0014] 步驟3 :根據(jù)適應(yīng)度函數(shù)計(jì)算每個(gè)粒子的適應(yīng)度值F1 (a)，并更新粒子的個(gè)體
[0015] 最好位置，即若 FK1 ⑴]< FEP1 (t-Ι)]，則置 P1U) = X1U);否則，
[0016] P1 (t) = P1(H)0
[0017] 步驟4 :更新全局最優(yōu)位置G⑴，即將P1U)的適應(yīng)值與全局最好位置G (t-Ι)的 適應(yīng)值比較，若 FEPjt)] <F[G(t-l)]，則置 G(t) =P1U);否則 G(t) =G(t-l)。
[0018] 步驟5 :根據(jù)下式計(jì)算得到隨機(jī)點(diǎn)的位置：
[0019]
[0020] 式中，w/t)服從（0, 1)上的均勻分布。
[0021] 步驟6 :根據(jù)下式更新每個(gè)粒子的新位置：
[0022]
[0023] 式中，U1, Jt)服從（0, 1)上的均勻分布，α稱為QPSO的收縮擴(kuò)張系數(shù)，它是算法 除群體規(guī)模和迭代次數(shù)以外的唯一控制參數(shù)。α按如下方式動(dòng)態(tài)變化：
[0024]
[0025] 參數(shù)α隨著迭代線性地從m遞減到η，通常取m = 1，η = 0. 5。N表示最大迭代 次數(shù)。
[0026] 步驟7 :若算法達(dá)到迭代次數(shù)，算法結(jié)束；否則，置t = t+Ι，返回步驟2。
[0027] 本發(fā)明同時(shí)規(guī)劃了七自由度空間機(jī)械臂基座的位置和姿態(tài)，最優(yōu)化基座位姿，使 其受到的擾動(dòng)最小，采用QPSO算法可以較快地找到全局最優(yōu)解，收斂速度快，所需相關(guān)參 數(shù)少，規(guī)劃的關(guān)節(jié)路徑滿足關(guān)節(jié)角、角速度及角加速度的范圍且規(guī)劃的關(guān)節(jié)路徑平滑，適合 機(jī)械臂的控制，本發(fā)明提高了計(jì)算效率和求解精度。
【附圖說明】
[0028] 圖1基座位置變化曲線；
[0029] 圖2基座姿態(tài)變化曲線；
[0030] 圖3關(guān)節(jié)角的運(yùn)動(dòng)軌跡曲線；
[0031] 圖4關(guān)節(jié)角速度曲線；
[0032] 圖5關(guān)節(jié)角加速度曲線；
[0033] 圖6本發(fā)明的技術(shù)方案簡圖。
【具體實(shí)施方式】
[0034] 本發(fā)明的實(shí)施例是在以本發(fā)明技術(shù)方案為前提下進(jìn)行實(shí)施的，給出了詳細(xì)的實(shí)施 方式和具體的操作過程，針對七自由度空間機(jī)械臂進(jìn)行研究，使機(jī)械臂的基座擾動(dòng)最小。但 本發(fā)明的保護(hù)范圍不限于下述實(shí)施例。
[0035] 實(shí)施例1
[0036] 步驟1 :定義如表一^h自由度空間機(jī)械臂的D-H參數(shù)和表二的機(jī)械臂各連桿的質(zhì) 量參數(shù)；
[0037] 表一系統(tǒng)的D-H參數(shù)
[0038]
[0039] 表二空間機(jī)械臂的質(zhì)量特性
[0040] CN 105138000 A ^ 4/8 貝
[0041] 根據(jù)以上參數(shù)建立七自由度空間機(jī)械臂的運(yùn)動(dòng)學(xué)模型。七自由度即空間機(jī)械臂中 具有七個(gè)獨(dú)立位置變量。本發(fā)明將雅可比矩陣擴(kuò)展為廣義雅可比矩陣來描述空間機(jī)械臂的 系統(tǒng)狀態(tài)。該空間機(jī)械臂的雅可比矩陣為：
[0042]
[0043] 其中：V。，ω。分別為機(jī)械臂初始的線速度和角速度，I b為基座的慣性矩陣，I bni為基 座與機(jī)械臂之間的耦合慣性矩陣，辦為關(guān)節(jié)角組成的關(guān)節(jié)角矩陣，Jvb為關(guān)于的雅可比矩 陣部分，JMb為關(guān)于ω。的雅可比矩陣部分。
[0044] 該自由漂浮模式下空間機(jī)械臂的廣義雅可比公式為：
[0045]
[0046] 其中：分別為空間機(jī)械臂末端執(zhí)行器的線速度和角速度，Jni為機(jī)械臂的雅 可比矩陣，J b為基座的雅可比矩陣，I b為基座的慣性矩陣，I bni為基座與機(jī)械臂之間的耦合慣 性矩陣，命為機(jī)械臂關(guān)節(jié)角組成的關(guān)節(jié)角矩陣，r為廣義雅可比的符號，Ψ 基座的姿態(tài)， Hi1為機(jī)械臂每個(gè)連桿的質(zhì)量，I1為慣性矩陣。步驟2 :采用四元數(shù)法描述空間機(jī)械臂的基座 姿態(tài)，根據(jù)機(jī)械臂基座的位置和姿態(tài)建立系統(tǒng)狀態(tài)方程，使用正弦函數(shù)五階多項(xiàng)式對七自 由度空間機(jī)械臂的關(guān)節(jié)軌跡參數(shù)化，同時(shí)滿足關(guān)節(jié)角、角速度和角加速度的限制條件；
[0047] 步驟3 :定義適應(yīng)度函數(shù)；
[0048] 步驟4 :定義基座位置和姿態(tài)的初始狀態(tài)和期望狀態(tài)以及定義關(guān)節(jié)角、角速度、角 加速度范圍；
[0049] 步驟5 :利用QPSO算法對建立的七自由度空間機(jī)械臂的適應(yīng)度函數(shù)求解，進(jìn)行仿 真實(shí)驗(yàn)；
[0050] 所述QPSO算法步驟如下：
[0051] 步驟1 :置時(shí)間t = 0,在問題空間中初始化M個(gè)粒子的當(dāng)前位置X1(O),(粒子i =1，2,... Μ)，并置個(gè)體最好位置P1 (O) = X1 (O)。
[0052] 步驟2 :根據(jù)下式計(jì)算粒子的平均最好位置（j指空間維數(shù)，j = 1，2, ... 7):
[0053]
[0054] 步驟3 :根據(jù)適應(yīng)度函數(shù)計(jì)算每個(gè)粒子的適應(yīng)度值F1 (a)，并更新粒子的個(gè)體最好 位置，即若 FK1 ⑴]< FEP1 (t-i)]，則置 P1U) =X1U);否則，P1U) =P1U-Ih
[0055] 步驟4 :更新全局最優(yōu)位置G (t)，即將P1U)的適應(yīng)值與全局最好位置G (t-Ι)的 適應(yīng)值比較，若 FEPjt)] <F[G(t-l)]，則置 G(t) =P1U);否則 G(t) =G(t-l)。
[0056] 步驟5 :根據(jù)下式計(jì)算得到隨機(jī)點(diǎn)的位置：
[0057]
[0058] 式中，爐/⑴服從（0, 1)上的均勻分布。
[0059] 步驟6 :根據(jù)下式更新每個(gè)粒子的新位置：
[0060]
[0061] 式中，U1, j⑴服從（0, 1)上的均勻分布，α稱為QPSO的收縮擴(kuò)張系數(shù)，它是算法 除群體規(guī)模和迭代次數(shù)以外的唯一控制參數(shù)。α按如下方式動(dòng)態(tài)變化：
[0062]
[0063] 參數(shù)α隨著迭代線性地從m遞減到η，通常取m = 1，η = 0. 5。N表示最大迭代 次數(shù)。
[0064] 步驟7 :若算法達(dá)到迭代次數(shù)Ν，算法結(jié)束；否則，置t = t+Ι，返回步驟2。
[0065] 圖1至圖5的曲線均在程序中設(shè)置得到各自的變化曲線。圖1位置曲線是在程序 中畫出三個(gè)坐標(biāo)軸X，y和z的變化曲線。
[0066] 實(shí)施例2
[0067] 首先，四元數(shù)描述機(jī)械臂的基座姿態(tài)，四