本發(fā)明涉及agv軌跡控制，尤其涉及基于改進增量支配mpcagv小車軌跡追蹤控制方法。

背景技術：

1、隨著科學技術水平的發(fā)展，越來越多的場景可以通過自動化產(chǎn)品來代替人工進行更高效的作業(yè)。自動引導小車就是其中之一。自動引導小車(automated?guided?vehicle)又稱agv，是一種帶有具備引導功能的光學、視覺導航系統(tǒng)，屬于人工智能機器人。起源于物流行業(yè)，廣泛使用于倉儲物流搬運貨物。因其獨特的優(yōu)勢，現(xiàn)如今應用于其他場景。如醫(yī)院、酒店等。agv的關鍵技術其中包括了感知、決策和控制三部分。軌跡跟蹤作為控制部分核心，是其在搬運貨物時能夠保證精準、穩(wěn)定高效和安全送達目的地的關鍵。因此具有重要的研究價值。在研究軌跡跟蹤時，通常采用pid控制、滑膜控制、自適應控制、模糊控制等傳統(tǒng)控制方法。而mpc控制算法采用在線求解的方式，不僅可以處理軌跡追蹤問題，并且在處理多約束最優(yōu)控制問題也有顯著的優(yōu)勢。mpc是一種基于動態(tài)模型的先進控制策略，通過對系統(tǒng)的動態(tài)模型進行預測生成最優(yōu)控制序列，從而實現(xiàn)對系統(tǒng)的閉環(huán)控制。能夠處理多變量、非線性系統(tǒng)和對系統(tǒng)約束條件處理更靈活。但同時也有些局限性，比如計算復雜度較高導致難以滿足高頻采樣系統(tǒng)的實時性要求、對模型精度要求較高等問題。因此想要使用mpc控制器控制agv軌跡跟蹤需要滿足實時性要求。

技術實現(xiàn)思路

1、針對上述現(xiàn)有技術，本發(fā)明在于提供基于改進增量支配mpcagv小車軌跡追蹤控制方法，主要解決上述背景技術中存在的技術問題。

2、為達到上述目的，本發(fā)明實施例的技術方案是這樣實現(xiàn)的：基于改進增量支配mpcagv小車軌跡追蹤控制方法，所述方法包括下列步驟：

3、構建agv小車運動學模型及進行線性化和離散化分析；

4、根據(jù)運動模型建立傳統(tǒng)mpc控制器；

5、構建增量支配向量和增量支配矩陣，并搭建改進增量支配mpc控制器；

6、在bieo的基礎上，提出pbieo優(yōu)化算法并對增量支配向量中待優(yōu)化的支配關系進行優(yōu)化；

7、利用優(yōu)化后的增量支配mpc控制器對期望軌跡進行追蹤。

8、構建agv小車運動學模型，具體包括：

9、在全局坐標系下，四輪agv的運動學模型為：

10、

11、其中l(wèi)表示前后輪之間的軸距，θ表示前輪轉向角度，(x,y)表示以兩后輪連線的中點為agv參考點，δ表示agv橫擺角,v為后輪速度。

12、將agv運動學模型線性化和離散化，具體包括：

13、設agv控制量為u＝[v,δ]t,狀態(tài)變量為x＝[x,y,θ]t。因此在t時刻，agv狀態(tài)空間方程為：

14、

15、期望的軌跡狀態(tài)方程可寫成：

16、

17、其中xr＝[xr,yr,θr]t,ur＝[vr,δr]t。將公式(2)在期望軌跡點(xr,ur)處進行泰勒展開，并忽略高階項：

18、

19、將公式(4)與公式(3)相減：

20、

21、令公式(5)可化簡為：

22、

23、其中為f對x的雅可比矩陣，化簡計算得同理為f對u的雅可比矩陣，計算得至此已完成對agv運動模型的線性化，下面將進行對于模型的離散化。

24、使用前向差分法對公式(6)離散化，得到k時刻的離散的狀態(tài)方程為：

25、

26、其中i為單位矩陣,t為采樣周期。

27、構建mpc控制器，具體包括：

28、將控制器的輸入量設置成每時刻的輸入增量它與和的關系式為結合公式(7)得：

29、

30、公式(8)可寫成矩陣形式為：

31、

32、其中nu，nx分別表示控制量的數(shù)量和狀態(tài)變量的數(shù)量，在本文中分別為nu＝2,nx＝3。0代表零矩陣，i代表單位矩陣，下標代表矩陣維度。mpc控制agv軌跡追蹤效果最終還是由其位姿展現(xiàn)的(x＝[x,y,θ]t),因此設其中結合公式(9)，得：

33、

34、在mpc控制中存在著預測時域np和控制時域nc，控制時域是小于或等于控制時域?；趍pc原理，由公式(10)可列出在預測時域內(nèi)第k時刻的預測方程為：

35、η(k)＝φ(k)ξ(k)+ψ(k)δu(k)??(11)

36、其中η(k)＝[χ(k+1|k),χ(k+2|k),…χ(k+nc|k),…,χ(k+np|k)]t為預測輸出矩陣，為狀態(tài)變量系數(shù)矩陣,為輸入矩陣。

37、為輸入系數(shù)矩陣。其中0表示零矩陣，維度為nx×nu。建立關于agv追蹤軌跡的損失函數(shù)：

38、

39、其中第一項式子表示過程誤差，q表示該項的權重系數(shù)。第二項式子表示的是增量變化，r代表其權重。第三項是松弛因子及它的權重，沒有實際意義，它可以防止在求解公式(12)過程中無解的現(xiàn)象。此外對于增量也需要進行設置。在agv搬運貨物過程中，過快的速度或者速度變化會導致貨物運輸不穩(wěn)定，造成事故。因此可以給予其約束：

40、

41、由先前定義可知，因此在k+1時刻的預測輸出為以此類推：

42、

43、寫成矩陣形式：

44、

45、其中該單位矩陣和零矩陣的維度都為nu×nu，將公式(15)簡化為：

46、u＝ut+a_tδu??(16)

47、因此公式(13)又可以寫成：

48、

49、將公式(10)和公式(11)帶入公式(12)化簡成二次規(guī)劃的標準形式，并增添約束條件(17)得：

50、

51、s.t.

52、umin≤ut+a_tδu≤umax

53、δumin≤δu≤δumax

54、增量支配向量矩陣的設計，具體包括：

55、定義增量支配關系參數(shù)為ε，其意義為：

56、

57、由于上節(jié)所建立的代價函數(shù)中所求解的量為2個(nu)，因此i矩陣和0矩陣均為2維的。對于控制域nc，為了使離當前時刻較近的預測時刻的增量計算精確，可設q個不受支配的增量，w個受支配的增量，剩下為e個需要優(yōu)化支配關系的增量。因此控制域nc可用q、w、e表示成：

58、nc＝q+e+w??(20)

59、由公式(22),可設增量支配向量idv：

60、idv＝[ε1,…,εq,…,εq+e,…,εq+e+w]??(21)

61、根據(jù)對于q、w定義及公式(19)，可以得到：

62、

63、而中間的e個變量需要進行進一步優(yōu)化得到。將公式(22)代入公式(21)中，得：

64、idv＝[i,…,i,εq+1,…,εq+e,0,…,0]???(23)

65、根據(jù)idv構建nc階增量支配矩陣idm。首先將idm初始化，設idm為nc階零矩陣。根據(jù)公式(23)，可將前q個對角線上的元素賦值為i。后面的w行中若有值為i即在idv中在[q+1,q+e]區(qū)間里有ε＝i，根據(jù)公式(19)定義該項不受前一增量支配，則在該行的同列元素為i。若有值為0即在idv中在[q+1,q+e]區(qū)間里有ε＝0,根據(jù)定義該項受前一增量支配，所以在那一行中與最靠近這一項不受支配的增量的那一列置i。最后根據(jù)公式(19)定義，將剩下的w項按照q項中ε＝0的方式處理。

66、構建增量支配mpc控制器，具體包括：

67、

68、在bieo的基礎上，提出pbieo并對增量支配向量中待優(yōu)化的向量進行優(yōu)化，具體包括：

69、bieo需要對種群進行初始化。由于ε是一個二維的數(shù)組，其行列式為1或0，而eo一般為一維數(shù)，因此需要對其進行處理：

70、

71、其中為初始化的第i個個體。eye(2)為一個二維的單位矩陣，randi為[0,1]之間的隨機數(shù)，round為取整運算。

72、然后需要定義一個損失函數(shù)，對優(yōu)化目標提出指標，從而引導個體進行更新迭代。本文需要經(jīng)過優(yōu)化得到irm矩陣，在將其帶入到公式(25)中，使得agv小車在追蹤軌跡時能夠滿足實時性和精準性的要求，因此我們可以將每個采樣點的狀態(tài)量的評價誤差和在線求解時長進行綜合分析，建立代價方程：

73、cost＝mean(time)+mean(|x(i)-xr(i)|+|y(i)-yr(i)|+|θ(i)-θr(i)|)??(26)

74、其中time為在每個采樣點，使用“內(nèi)點法”對二次優(yōu)化問題求解所用的時間。mean為取平均值。

75、接著，需要建立平衡優(yōu)化池。將每一次更新迭代后四個迄今為止最好的粒子加上這四個粒子的圓整平均值作為候選的平衡狀態(tài)，每次迭代過程中，用等概率從這五個粒子中選取，作為后續(xù)粒子更新迭代的平衡狀態(tài)方向。這一機制不經(jīng)可使得粒子高品質更新，又可防止粒子陷入局部最優(yōu)，其表達式為：

76、ψeq,pool＝[ψeq,best1,ψeq,best2,ψeq,best3,ψeq,best4,ψeq,ave]??(27)

77、ψeq,ave＝round(mean(ψeq,best1,ψeq,best2,ψeq,best3,ψeq,best4))

78、指數(shù)項可隨著迭代周期自適應條件全局和局部搜索能力，其公式為：

79、

80、公式(29)中，為hadamard積。a1和a2分別為該次迭代的勘探能力和挖掘能力，一般都為常數(shù)。r和λ均為2維隨機向量；時間t隨著迭代次數(shù)的增加而減少。iter和maxiter分別當前迭代次數(shù)和最大迭代次數(shù)。

81、設置質量生成項，可防止在尋優(yōu)過程中在某個維度上陷入局部最優(yōu)，表達式為：

82、

83、其中，r1和r2均為[0,1]中的隨機數(shù)。ones(2)為全“1”矩陣，其維度為2維。ψeq,pool為本次迭代過程中在平衡池中選取的候選解，ψ為當前解。gcp為生成速率參數(shù)，其主要作用決定生成項參與更新過程的概率。gp為生成概率，其作用是決定有多少粒子使用生成項來進行更新，一般取值為0.5。

84、最后制定更新規(guī)則。提出了一個更新bieo粒子的方法，本文將使用這種方法進行更新。

85、

86、α＝gp×s1??(33)

87、

88、公式(35)中，為第i個個體的第k個維度。s1為[0,1]之間隨機數(shù)且大于0.5。s2為[0,1]之間隨機數(shù)任意數(shù)。更新過程由公式(32)和公式(33)計算出每一個粒子的變化量δψ，再將其帶入到公式(34)v型函數(shù)中計算出每個個體的每個維度的更新概率。最后在根據(jù)公式(35)中的更新規(guī)則更新個體情況，一直迭代更新，直到找到最優(yōu)解。

89、a1和a2分別體現(xiàn)了更新迭代過程中的勘探能力和挖掘能力。由公式(28)、(29)可知，當a1較大時，勘探能力比較強，對應的挖掘能力就較弱，此時這種情況適用于初期需要大范圍的搜索(全局搜索)。而當a2較大時，挖掘能力比較強，對應的勘探能力就較弱，此時適用于后期小范圍的搜索(局部搜索)。因此本文將公式(38)和公式(39)原本為固定值的a1和a2換成可根據(jù)更新迭代的次數(shù)進行自我調(diào)節(jié)的值：

90、

91、對于pbieo而言，同樣需要進行初始化。首先需要對初始化后的粒子分組。一般設置種群的數(shù)量為整十，因此，本文將組別分為5組。然后對每組中的粒子的適應度進行排序，找出每組適應度強的前4個粒子構建平衡優(yōu)化池和每組的最優(yōu)粒子。同時也將全局最優(yōu)解和全局適應度最強的前4個粒子找出，并建立全局平衡優(yōu)化池。之后每一個組單獨進行更新迭代，當?shù)揭欢ù螖?shù)時，將會執(zhí)行兩種不同的策略進行組之間的交流。本文設置的組與組之間交流的迭代次數(shù)為10的倍數(shù)。第一種策略的交流方式如下公式所示：

92、

93、第一種交流策略為當當前迭代次數(shù)不足總迭代次數(shù)的2/5時，同時也滿足當前迭代次數(shù)為10的倍數(shù)時，隨機從全局平衡優(yōu)化池里選取一個粒子ψeq，并與全局最優(yōu)粒子bestp進行結合。根據(jù)公式(38)，將每組中適應度較差的粒子進行突變(適應度較差的粒子為適應度較差的后一半粒子)。通過這種組之間交流方式，在勘探階段可以篩選出適應度較差粒子并且使這些粒子快速靠近最優(yōu)解。進一步提高了算法的收斂速度。

94、當?shù)螖?shù)超過總迭代次數(shù)的2/5時，且同時迭代次數(shù)為10的倍數(shù)時，此時將采用第二種交流策略進行更新。將適應度較差的后一半粒子被相鄰組中的最優(yōu)粒子的平均值的圓整數(shù)所替代。第二種策略表達為：

95、

96、將每組中適應度較差的粒子全部替換。這一策略加強了種群之間的聯(lián)系，提高了算法中后期的挖掘能力，同時也有效地避免了算法過早收斂。除了這些特殊迭代次數(shù)時刻，需要運用到這兩種策略，其余時刻，各組的粒子按照上節(jié)所說的方式進行更新迭代。

97、本發(fā)明的有益效果在于：本技術所提出的基于改進增量支配mpcagv小車軌跡追蹤控制方法，為了滿足mpc控制agv追蹤軌跡時實時性要求，構建了agv小車運動學模型，同時用泰勒公式和前向歐拉對運動學模型進行線性化和離散化。根據(jù)線性化和離散化的運動模型進行mpc控制器的設計。通過定義一種增量支配關系向量和增量支配矩陣減少mpc在線求解的輸入量，減少運算時間，來滿足實時性要求。通過pbieo算法對增量支配關系向量中待優(yōu)化的支配關系進行優(yōu)化，從而達到最優(yōu)的控制效果。