本發(fā)明涉及自動(dòng)控制，尤其涉及一種自移動(dòng)設(shè)備的控制方法、裝置、存儲(chǔ)介質(zhì)及程序產(chǎn)品。
背景技術(shù)：
1、隨著自動(dòng)化技術(shù)和人工智能的發(fā)展，自移動(dòng)設(shè)備(如自動(dòng)駕駛汽車、無人機(jī)、自動(dòng)導(dǎo)引車agv等)的應(yīng)用日益廣泛。這些設(shè)備在物流運(yùn)輸、農(nóng)業(yè)監(jiān)測(cè)、環(huán)境勘探、公共安全以及個(gè)人消費(fèi)電子等多個(gè)領(lǐng)域發(fā)揮著重要作用。軌跡跟蹤作為自移動(dòng)設(shè)備的核心功能之一，是指設(shè)備根據(jù)預(yù)設(shè)的路徑或目標(biāo)點(diǎn)自主調(diào)整其運(yùn)動(dòng)狀態(tài)，以確保能夠精確地沿著指定路線行駛的能力。良好的軌跡跟蹤性能有助于減少能量消耗、縮短任務(wù)完成時(shí)間，提高整體操作效率的同時(shí)，提高安全性，降低碰撞事件發(fā)生的概率。
2、在現(xiàn)有的自移動(dòng)設(shè)備的控制方法中，模型預(yù)測(cè)控制(model?predictive?control，mpc)算法作為一種先進(jìn)的控制策略得到了廣泛應(yīng)用。mpc算法通過基于當(dāng)前狀態(tài)預(yù)測(cè)未來一段時(shí)間內(nèi)的系統(tǒng)行為，并優(yōu)化控制輸入以盡量接近目標(biāo)狀態(tài)，同時(shí)滿足系統(tǒng)的物理約束條件。
3、然而，當(dāng)自移動(dòng)設(shè)備遇到較大的位置偏差或軌跡曲率突然增加時(shí)，固定的控制量及其增量上下限可能導(dǎo)致控制性能受限。由于這些上下限是預(yù)先設(shè)定的，面對(duì)需要快速調(diào)整姿態(tài)的情況，固定的限制可能無法提供足夠的靈活性來響應(yīng)需求，使得速度和角速度達(dá)到預(yù)設(shè)上限而無法進(jìn)一步優(yōu)化，導(dǎo)致軌跡跟蹤的精度和效率較低的問題。
技術(shù)實(shí)現(xiàn)思路
1、鑒于此，本發(fā)明實(shí)施例提供了一種自移動(dòng)設(shè)備的控制方法、裝置、存儲(chǔ)介質(zhì)及程序產(chǎn)品，以消除或改善現(xiàn)有的模型預(yù)測(cè)控制(mpc)軌跡跟蹤技術(shù)中存在的“自移動(dòng)設(shè)備遇到較大的位置偏差或軌跡曲率突然增加時(shí)的控制量受限”而失去控制的缺陷，并提高軌跡跟蹤的控制精度和效率。
2、本發(fā)明的一個(gè)方面提供了一種自移動(dòng)設(shè)備的控制方法，該方法包括以下步驟：
3、控制自移動(dòng)設(shè)備沿參考軌跡移動(dòng)，將自移動(dòng)設(shè)備在移動(dòng)過程中的設(shè)備姿態(tài)信息和設(shè)備控制量分別與當(dāng)前的采樣時(shí)刻對(duì)應(yīng)的參考姿態(tài)信息和參考控制量進(jìn)行對(duì)比，得到對(duì)比結(jié)果；其中，參考軌跡由若干個(gè)包含參考姿態(tài)信息和參考控制量的離散采樣點(diǎn)組成，采樣點(diǎn)與采樣時(shí)刻一一對(duì)應(yīng)；
4、在對(duì)比結(jié)果指示自移動(dòng)設(shè)備偏離參考軌跡的情況下，基于自移動(dòng)設(shè)備對(duì)應(yīng)的控制量范圍和控制量增量范圍的上下限，初始化預(yù)設(shè)果蠅覓食優(yōu)化算法的果蠅覓食范圍；
5、在蠅覓食范圍內(nèi)按照預(yù)設(shè)種群規(guī)模隨機(jī)采樣得到初始種群，并按照預(yù)設(shè)迭代次數(shù)迭代初始種群，得到控制量范圍和控制量增量范圍的優(yōu)化結(jié)果；
6、基于優(yōu)化結(jié)果構(gòu)建不等式約束，并將不等式約束輸入預(yù)設(shè)二次規(guī)劃求解器，求解得到控制量偏差；二次規(guī)劃求解器用于在滿足不等式約束的前提下，最小化控制量的平方和與姿態(tài)信息偏差的平方和之間的加權(quán)總和；
7、將控制量偏差引入?yún)⒖伎刂屏?，得到自移?dòng)設(shè)備移動(dòng)至下一采樣點(diǎn)的控制量輸入，并基于控制量輸入生成預(yù)測(cè)姿態(tài)信息，更新設(shè)備姿態(tài)信息；
8、在自移動(dòng)設(shè)備未移動(dòng)至參考軌跡的終點(diǎn)位置的情況下，執(zhí)行控制自移動(dòng)設(shè)備沿參考軌跡移動(dòng)的步驟。
9、在本發(fā)明的一些實(shí)施例中，參考姿態(tài)信息包括參考橫坐標(biāo)信息、參考縱坐標(biāo)信息和參考角度信息；設(shè)備姿態(tài)信息包括設(shè)備橫坐標(biāo)信息、設(shè)備縱坐標(biāo)信息和設(shè)備角度信息；
10、按照預(yù)設(shè)迭代次數(shù)迭代初始種群，得到控制量范圍和控制量增量范圍的優(yōu)化結(jié)果，包括：
11、基于參考橫坐標(biāo)信息、參考縱坐標(biāo)信息、設(shè)備橫坐標(biāo)信息和設(shè)備縱坐標(biāo)信息，生成橫向跟蹤誤差；
12、基于設(shè)備角度信息和參考角度信息，生成航向角誤差；
13、對(duì)于初始種群中的每個(gè)個(gè)體，確定每個(gè)個(gè)體對(duì)應(yīng)的速度路徑長度和角速度路徑長度；
14、將橫向跟蹤誤差、航向角誤差、速度路徑長度和角速度路徑長度，輸入至基于均方誤差的適應(yīng)度函數(shù)中，得到每個(gè)個(gè)體對(duì)應(yīng)的適應(yīng)度值；
15、將當(dāng)前迭代次數(shù)與預(yù)設(shè)迭代次數(shù)進(jìn)行對(duì)比，在當(dāng)前迭代次數(shù)等于預(yù)設(shè)迭代次數(shù)的情況下，將適應(yīng)度值中的最小值作為目標(biāo)優(yōu)化值，并將目標(biāo)優(yōu)化值對(duì)應(yīng)的個(gè)體，確定為優(yōu)化結(jié)果。
16、在本發(fā)明的一些實(shí)施例中，在當(dāng)前迭代次數(shù)小于預(yù)設(shè)迭代次數(shù)的情況下，方法還包括：
17、基于當(dāng)前迭代次數(shù)和預(yù)設(shè)值范圍，動(dòng)態(tài)調(diào)整搜索半徑；搜索半徑在每次迭代中，從預(yù)設(shè)值范圍的上限逐漸減小至預(yù)設(shè)值范圍的下限；
18、通過調(diào)整后的搜索半徑更新種群中的個(gè)體位置，將每個(gè)個(gè)體根據(jù)調(diào)整后的搜索半徑進(jìn)行移動(dòng)；
19、執(zhí)行確定每個(gè)個(gè)體對(duì)應(yīng)的速度路徑長度和角速度路徑長度的步驟。
20、在本發(fā)明的一些實(shí)施例中，在參考軌跡中，相鄰采樣點(diǎn)間隔的采樣時(shí)間相同；
21、基于控制量輸入生成預(yù)測(cè)姿態(tài)信息，更新設(shè)備姿態(tài)信息，包括：
22、通過將調(diào)整后的控制量和設(shè)備姿態(tài)信息輸入到物理模型函數(shù)中，并使用四階朗格－庫塔法逐步計(jì)算半步和全步的平均斜率，依次得到第一坡度、第二坡度、第三坡度和第四坡度；物理模型函數(shù)是基于四階朗格－庫塔法實(shí)現(xiàn)的，用于將姿態(tài)信息和控制量作為輸入預(yù)測(cè)自移動(dòng)設(shè)備的位姿變化；
23、對(duì)第一坡度、第二坡度、第三坡度和第四坡度進(jìn)行加權(quán)平均，得到第五坡度；
24、使用第五坡度乘以采樣時(shí)間，將結(jié)果與設(shè)備姿態(tài)信息相加，生成預(yù)測(cè)姿態(tài)信息。
25、在本發(fā)明的一些實(shí)施例中，基于優(yōu)化結(jié)果構(gòu)建不等式約束，包括：
26、基于優(yōu)化結(jié)果中的速度和速度增量范圍，構(gòu)建速度約束；
27、基于優(yōu)化結(jié)果中的角速度和角速度增量范圍，構(gòu)建角速度約束。
28、在本發(fā)明的一些實(shí)施例中，預(yù)設(shè)二次規(guī)劃求解器包括預(yù)設(shè)目標(biāo)函數(shù)；在預(yù)設(shè)目標(biāo)函數(shù)中，使用預(yù)設(shè)二階單位矩陣對(duì)應(yīng)控制量偏差的平方和進(jìn)行加權(quán)，使用預(yù)設(shè)三階單位矩陣對(duì)設(shè)備姿態(tài)偏差的平方和進(jìn)行加權(quán)，并將加權(quán)結(jié)果相加。
29、在本發(fā)明的一些實(shí)施例中，控制自移動(dòng)設(shè)備沿參考軌跡移動(dòng)之前，還包括：
30、響應(yīng)于接收到的移動(dòng)指令，初始化自移動(dòng)設(shè)備的當(dāng)前位置；移動(dòng)指令中包括終點(diǎn)位置；
31、構(gòu)建從當(dāng)前位置到終點(diǎn)位置的移動(dòng)路徑，作為參考軌跡。
32、本發(fā)明的另一方面提供了一種自移動(dòng)設(shè)備的控制裝置，包括處理器、存儲(chǔ)器及存儲(chǔ)在存儲(chǔ)器上的計(jì)算機(jī)程序/指令，其特征在于，處理器用于執(zhí)行計(jì)算機(jī)程序/指令，當(dāng)計(jì)算機(jī)程序/指令被執(zhí)行時(shí)該裝置實(shí)現(xiàn)如前的自移動(dòng)設(shè)備的控制方法的步驟。
33、本發(fā)明的另一方面提供了一種計(jì)算機(jī)可讀存儲(chǔ)介質(zhì)，其上存儲(chǔ)有計(jì)算機(jī)程序/指令，其特征在于，該計(jì)算機(jī)程序/指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)如前所述的自移動(dòng)設(shè)備的控制方法的步驟。
34、本發(fā)明的另一方面提供了一種計(jì)算機(jī)程序產(chǎn)品，包括計(jì)算機(jī)程序/指令，其特征在于，該計(jì)算機(jī)程序/指令被處理器執(zhí)行時(shí)實(shí)現(xiàn)如前所述的自移動(dòng)設(shè)備的控制方法的步驟。
35、本發(fā)明的自移動(dòng)設(shè)備的控制方法和裝置，能夠基于預(yù)設(shè)果蠅覓食優(yōu)化算法，利用自移動(dòng)設(shè)備在當(dāng)前時(shí)刻的設(shè)備姿態(tài)信息、速度和角速度信息，對(duì)下一時(shí)刻的狀態(tài)進(jìn)行預(yù)測(cè)，并使用更新后的控制量和控制量增量的上下限指導(dǎo)模型預(yù)測(cè)控制中的二次規(guī)劃求解。在自移動(dòng)設(shè)備遇到較大的位置偏差或軌跡曲率突然增加導(dǎo)致控制量受限的情況下，系統(tǒng)能夠自動(dòng)優(yōu)化這些上下限值，使得設(shè)備可以快速調(diào)整自身姿態(tài)以接近參考軌跡，而不受原有固定上下限的限制，在提高了軌跡跟蹤的精度的同時(shí)，還增強(qiáng)了系統(tǒng)的響應(yīng)速度和適應(yīng)性，確保設(shè)備在復(fù)雜或多變環(huán)境中能夠高效執(zhí)行任務(wù)并迅速糾正偏差；通過動(dòng)態(tài)調(diào)整控制量及增量的上下限，有效解決軌跡跟蹤的精度和效率較低的問題，實(shí)現(xiàn)了更加靈活且精準(zhǔn)的路徑跟隨能力，從而顯著提升了軌跡跟蹤的整體性能和效率。
36、本發(fā)明的附加優(yōu)點(diǎn)、目的，以及特征將在下面的描述中將部分地加以闡述，且將對(duì)于本領(lǐng)域普通技術(shù)人員在研究下文后部分地變得明顯，或者可以根據(jù)本發(fā)明的實(shí)踐而獲知。本發(fā)明的目的和其它優(yōu)點(diǎn)可以通過在說明書以及附圖中具體指出的結(jié)構(gòu)實(shí)現(xiàn)到并獲得。
37、本領(lǐng)域技術(shù)人員將會(huì)理解的是，能夠用本發(fā)明實(shí)現(xiàn)的目的和優(yōu)點(diǎn)不限于以上具體所述，并且根據(jù)以下詳細(xì)說明將更清楚地理解本發(fā)明能夠?qū)崿F(xiàn)的上述和其他目的。