本發明涉及自動控制，尤其涉及一種自移動設備的控制方法、裝置、存儲介質及程序產品。

背景技術：

1、隨著自動化技術和人工智能的發展，自移動設備(如自動駕駛汽車、無人機、自動導引車agv等)的應用日益廣泛。這些設備在物流運輸、農業監測、環境勘探、公共安全以及個人消費電子等多個領域發揮著重要作用。軌跡跟蹤作為自移動設備的核心功能之一，是指設備根據預設的路徑或目標點自主調整其運動狀態，以確保能夠精確地沿著指定路線行駛的能力。良好的軌跡跟蹤性能有助于減少能量消耗、縮短任務完成時間，提高整體操作效率的同時，提高安全性，降低碰撞事件發生的概率。

2、在現有的自移動設備的控制方法中，模型預測控制(model?predictive?control，mpc)算法作為一種先進的控制策略得到了廣泛應用。mpc算法通過基于當前狀態預測未來一段時間內的系統行為，并優化控制輸入以盡量接近目標狀態，同時滿足系統的物理約束條件。

3、然而，當自移動設備遇到較大的位置偏差或軌跡曲率突然增加時，固定的控制量及其增量上下限可能導致控制性能受限。由于這些上下限是預先設定的，面對需要快速調整姿態的情況，固定的限制可能無法提供足夠的靈活性來響應需求，使得速度和角速度達到預設上限而無法進一步優化，導致軌跡跟蹤的精度和效率較低的問題。

技術實現思路

1、鑒于此，本發明實施例提供了一種自移動設備的控制方法、裝置、存儲介質及程序產品，以消除或改善現有的模型預測控制(mpc)軌跡跟蹤技術中存在的“自移動設備遇到較大的位置偏差或軌跡曲率突然增加時的控制量受限”而失去控制的缺陷，并提高軌跡跟蹤的控制精度和效率。

2、本發明的一個方面提供了一種自移動設備的控制方法，該方法包括以下步驟：

3、控制自移動設備沿參考軌跡移動，將自移動設備在移動過程中的設備姿態信息和設備控制量分別與當前的采樣時刻對應的參考姿態信息和參考控制量進行對比，得到對比結果；其中，參考軌跡由若干個包含參考姿態信息和參考控制量的離散采樣點組成，采樣點與采樣時刻一一對應；

4、在對比結果指示自移動設備偏離參考軌跡的情況下，基于自移動設備對應的控制量范圍和控制量增量范圍的上下限，初始化預設果蠅覓食優化算法的果蠅覓食范圍；

5、在蠅覓食范圍內按照預設種群規模隨機采樣得到初始種群，并按照預設迭代次數迭代初始種群，得到控制量范圍和控制量增量范圍的優化結果；

6、基于優化結果構建不等式約束，并將不等式約束輸入預設二次規劃求解器，求解得到控制量偏差；二次規劃求解器用于在滿足不等式約束的前提下，最小化控制量的平方和與姿態信息偏差的平方和之間的加權總和；

7、將控制量偏差引入參考控制量，得到自移動設備移動至下一采樣點的控制量輸入，并基于控制量輸入生成預測姿態信息，更新設備姿態信息；

8、在自移動設備未移動至參考軌跡的終點位置的情況下，執行控制自移動設備沿參考軌跡移動的步驟。

9、在本發明的一些實施例中，參考姿態信息包括參考橫坐標信息、參考縱坐標信息和參考角度信息；設備姿態信息包括設備橫坐標信息、設備縱坐標信息和設備角度信息；

10、按照預設迭代次數迭代初始種群，得到控制量范圍和控制量增量范圍的優化結果，包括：

11、基于參考橫坐標信息、參考縱坐標信息、設備橫坐標信息和設備縱坐標信息，生成橫向跟蹤誤差；

12、基于設備角度信息和參考角度信息，生成航向角誤差；

13、對于初始種群中的每個個體，確定每個個體對應的速度路徑長度和角速度路徑長度；

14、將橫向跟蹤誤差、航向角誤差、速度路徑長度和角速度路徑長度，輸入至基于均方誤差的適應度函數中，得到每個個體對應的適應度值；

15、將當前迭代次數與預設迭代次數進行對比，在當前迭代次數等于預設迭代次數的情況下，將適應度值中的最小值作為目標優化值，并將目標優化值對應的個體，確定為優化結果。

16、在本發明的一些實施例中，在當前迭代次數小于預設迭代次數的情況下，方法還包括：

17、基于當前迭代次數和預設值范圍，動態調整搜索半徑；搜索半徑在每次迭代中，從預設值范圍的上限逐漸減小至預設值范圍的下限；

18、通過調整后的搜索半徑更新種群中的個體位置，將每個個體根據調整后的搜索半徑進行移動；

19、執行確定每個個體對應的速度路徑長度和角速度路徑長度的步驟。

20、在本發明的一些實施例中，在參考軌跡中，相鄰采樣點間隔的采樣時間相同；

21、基于控制量輸入生成預測姿態信息，更新設備姿態信息，包括：

22、通過將調整后的控制量和設備姿態信息輸入到物理模型函數中，并使用四階朗格－庫塔法逐步計算半步和全步的平均斜率，依次得到第一坡度、第二坡度、第三坡度和第四坡度；物理模型函數是基于四階朗格－庫塔法實現的，用于將姿態信息和控制量作為輸入預測自移動設備的位姿變化；

23、對第一坡度、第二坡度、第三坡度和第四坡度進行加權平均，得到第五坡度；

24、使用第五坡度乘以采樣時間，將結果與設備姿態信息相加，生成預測姿態信息。

25、在本發明的一些實施例中，基于優化結果構建不等式約束，包括：

26、基于優化結果中的速度和速度增量范圍，構建速度約束；

27、基于優化結果中的角速度和角速度增量范圍，構建角速度約束。

28、在本發明的一些實施例中，預設二次規劃求解器包括預設目標函數；在預設目標函數中，使用預設二階單位矩陣對應控制量偏差的平方和進行加權，使用預設三階單位矩陣對設備姿態偏差的平方和進行加權，并將加權結果相加。

29、在本發明的一些實施例中，控制自移動設備沿參考軌跡移動之前，還包括：

30、響應于接收到的移動指令，初始化自移動設備的當前位置；移動指令中包括終點位置；

31、構建從當前位置到終點位置的移動路徑，作為參考軌跡。

32、本發明的另一方面提供了一種自移動設備的控制裝置，包括處理器、存儲器及存儲在存儲器上的計算機程序/指令，其特征在于，處理器用于執行計算機程序/指令，當計算機程序/指令被執行時該裝置實現如前的自移動設備的控制方法的步驟。

33、本發明的另一方面提供了一種計算機可讀存儲介質，其上存儲有計算機程序/指令，其特征在于，該計算機程序/指令被處理器執行時實現如前所述的自移動設備的控制方法的步驟。

34、本發明的另一方面提供了一種計算機程序產品，包括計算機程序/指令，其特征在于，該計算機程序/指令被處理器執行時實現如前所述的自移動設備的控制方法的步驟。

35、本發明的自移動設備的控制方法和裝置，能夠基于預設果蠅覓食優化算法，利用自移動設備在當前時刻的設備姿態信息、速度和角速度信息，對下一時刻的狀態進行預測，并使用更新后的控制量和控制量增量的上下限指導模型預測控制中的二次規劃求解。在自移動設備遇到較大的位置偏差或軌跡曲率突然增加導致控制量受限的情況下，系統能夠自動優化這些上下限值，使得設備可以快速調整自身姿態以接近參考軌跡，而不受原有固定上下限的限制，在提高了軌跡跟蹤的精度的同時，還增強了系統的響應速度和適應性，確保設備在復雜或多變環境中能夠高效執行任務并迅速糾正偏差；通過動態調整控制量及增量的上下限，有效解決軌跡跟蹤的精度和效率較低的問題，實現了更加靈活且精準的路徑跟隨能力，從而顯著提升了軌跡跟蹤的整體性能和效率。

36、本發明的附加優點、目的，以及特征將在下面的描述中將部分地加以闡述，且將對于本領域普通技術人員在研究下文后部分地變得明顯，或者可以根據本發明的實踐而獲知。本發明的目的和其它優點可以通過在說明書以及附圖中具體指出的結構實現到并獲得。

37、本領域技術人員將會理解的是，能夠用本發明實現的目的和優點不限于以上具體所述，并且根據以下詳細說明將更清楚地理解本發明能夠實現的上述和其他目的。