簡介：採用粒子群優化演算法進行機械人軌跡規劃，以3-5-3多項式軌跡形式進行軌跡規劃，以最短時間為優化指標進行尋優。
3-5-3軌跡軌跡的形式
其中，j為關節編號，如六自由度編號為1-6。已知關節j的4個插值點(弧度值)：
。3-5-3軌跡係數矩陣的求解
2. 3-5-3軌跡係數矩陣的求解
約束優化條件：
其中，
為適應度函式
3. 約束條件轉換
是需要優化的未知量，係數矩陣a是待求解的位置數，若把a當作自變數則需要優化14個維度的資料，計算量大，為此直接將t作為自變數，降低搜尋維度為3維。
4.粒子群優化
(1)初始化種群規模npon=50，迭代次數nger=50，待求解向量維度ndim=3，隨機初始化粒子位置向量theta和粒子速度向量vtheta
(2)給定粒子位置向量的取值範圍thetalim，給定粒子速度的取值範圍vthetalim
(3)給定慣性權重weight，自我學習因子clearn1和群體學習因子clearn2
(4)初始化個體的最佳歷史位置為thetahistory = theta，種群最佳歷史位置thetabest = zeros(1, ndim)，個體最佳適應度indtbest = inf+zeros(npon, 1)，種群歷史最佳適應度tbest = inf
注意：由於是求最小值indtbest、tbest應當初始化為乙個較大的值，而不應該初始化為0。
(5)呼叫適應度函式fitnesstrac，計算係數矩陣a,關節速度在各段曲線上的極大值和極小值矩陣v，若a滿秩且v絕對值的最大值小於給定的最大關節速度說明當前時間t1、t2、t3有效，否則說明當前值無效，flag返回false。
(6)若flag為true 說明當前時間
有效，進行記錄並於個體的歷史最佳適應度indtbest進行比較，若indtbest大於當前計算的適應度fxbest則更新indtbest，否則不更新；若flag 為false 說明當前值無效，此時直接將當前適應度fxbest賦值為無窮大。
(7)獲得當前種群中最佳適應度min(indtbest)，若其小於種群歷史最佳適應度tbest，則更新tbest。
(8)採用粒子群公式進行速度vtheta的更新；
(9)判斷每個粒子的速度和位置是否在安全範圍之內，若不在則拉回到安全邊界值。
qq_37469992的部落格_yy小苗(高階划水工程師)_csdn部落格-設計模式gof,matlab,c++領域博主blog.csdn.net
matlab中粒子群演算法基礎（ ）
演算法描述 n個粒子在q維 變數個數 空間進行搜尋,每個粒子表示為xi xi1,xi2,xi3,xi4,xi5.對應的速度vi vi1,vi2,vi3,vi4,vi5.搜尋時考慮兩個因素 1自己歷史最大值2全部粒子最優值 速度由3個因素改變 1慣性權重，2自身權重3種群權重 可以通過plotobje...
粒子群優化的matlab實現
在youtube上依葫蘆畫瓢寫了乙個matlab的粒子群演算法來實現多變數函式的優化，程式最終能輸出迭代後的最優解，並以半對數座標的形式展示出每一代的最優解。對於不同的問題只需修改costfunction 待優化的函式 nvar 待優化函式的變數個數 varmin和varmax 待優化函式變數的上下...
粒子群演算法的matlab實現
2018.7.19 粒子群演算法function xm,fv pso fitness,n,c1,c2,w,m,d 本函式用粒子群演算法實現優化 給定初始化條件 c1 學習因子1 c2 學習因子2 w 慣性權重 m 最大迭代次數 d 搜尋空間維數 n 初始化群體個體數目 xm,fv pso fitne...