本發(fā)明涉及自動化控制，尤其涉及一種果實采摘機(jī)械系統(tǒng)控制器優(yōu)化方法。

背景技術(shù)：

1、蘋果采摘機(jī)器人是一種廣泛應(yīng)用于農(nóng)業(yè)領(lǐng)域的智能化設(shè)備，旨在替代人工完成蘋果的精準(zhǔn)采摘任務(wù)；這種機(jī)器人通常需要具備快速、準(zhǔn)確的定位能力，以及對復(fù)雜農(nóng)田環(huán)境的適應(yīng)能力。在果實采摘過程中，機(jī)器人必須協(xié)調(diào)機(jī)械臂運動以及視覺識別功能，以實現(xiàn)高效采摘；然而，在實際應(yīng)用中，由于農(nóng)田環(huán)境復(fù)雜多變，采摘過程往往面臨諸多技術(shù)挑戰(zhàn)。

2、pid控制器（比例-積分-微分控制器）作為一種經(jīng)典的控制算法，因其簡單高效的結(jié)構(gòu)被廣泛應(yīng)用于蘋果采摘機(jī)器人中，用于機(jī)械臂的運動控制；然而，傳統(tǒng)的pid控制器在復(fù)雜非線性環(huán)境中難以達(dá)到最優(yōu)控制效果，機(jī)械臂的運動控制精度較低，運動速度較慢，主要體現(xiàn)在以下幾個方面：1、pid控制器的性能嚴(yán)重依賴于比例、積分和微分參數(shù)（kp、ki、kd）的設(shè)定。在復(fù)雜多變的農(nóng)田環(huán)境中，這些參數(shù)往往需要頻繁調(diào)節(jié)以適應(yīng)不同的果實種類和采摘條件，手動調(diào)參既耗時又難以獲得最優(yōu)解；2、蘋果采摘任務(wù)對控制系統(tǒng)的響應(yīng)速度和穩(wěn)定性有較高要求。然而，傳統(tǒng)pid控制器在快速響應(yīng)和抑制振蕩之間難以實現(xiàn)理想的平衡，容易導(dǎo)致過沖、滯后或系統(tǒng)震蕩，影響采摘效率和準(zhǔn)確性；3、農(nóng)田環(huán)境中存在各種動態(tài)干擾因素，如風(fēng)力、枝葉晃動和光照變化，這些因素可能導(dǎo)致機(jī)械臂定位偏差，傳統(tǒng)pid控制器難以實時適應(yīng)這種動態(tài)變化。

3、增長優(yōu)化算法（growth?optimizer,?go）是一種新型元啟發(fā)式算法，旨在解決連續(xù)全局優(yōu)化問題，go算法的設(shè)計靈感來源于個體在社會成長過程中的學(xué)習(xí)和反思機(jī)制，通過模擬個體的成長行為，go算法旨在找到全局最優(yōu)解，go算法分為學(xué)習(xí)階段和反思階段建立個體位置更新數(shù)學(xué)模型，利用go算法對果實采摘機(jī)械系統(tǒng)控制器的pid算法的控制參數(shù)優(yōu)化可以提高控制器的靈敏度和精確度，但是需要對go算法改進(jìn)，解決其易陷入局部最優(yōu)解的問題，才能保證在增長優(yōu)化算法對果實采摘機(jī)械系統(tǒng)控制器的pid算法的控制參數(shù)優(yōu)化過程中的絕對優(yōu)越性。

技術(shù)實現(xiàn)思路

1、本發(fā)明的目的是提供一種果實采摘機(jī)械系統(tǒng)控制器優(yōu)化方法，利用改進(jìn)的增長優(yōu)化算法通過對果實采摘機(jī)械系統(tǒng)控制器的控制算法的控制參數(shù)整定，加強(qiáng)果實采摘機(jī)械系統(tǒng)控制器的控制精度和靈敏度，解決上述背景技術(shù)中蘋果采摘機(jī)器人機(jī)械臂的運動控制精度較低，運動速度較慢的問題，實現(xiàn)果實采摘機(jī)器人采摘精度最優(yōu)魯棒控制。

2、為了實現(xiàn)上述目的，本發(fā)明提供如下技術(shù)方案：一種果實采摘機(jī)械系統(tǒng)控制器優(yōu)化方法，通過增長優(yōu)化算法進(jìn)行改進(jìn)，利用改進(jìn)的增長優(yōu)化算法對果實采摘機(jī)械系統(tǒng)控制器的pid控制算法的控制參數(shù)優(yōu)化，具體包括以下步驟。

3、s1、將果實和機(jī)械爪位置建立在同一個三維空間，通過視覺傳感器定位果實的三維位置，同時計算實時械臂爪的三維位置。

4、s2、通過果實和實時機(jī)械爪的三維位置計算果實位置與機(jī)械爪位置的誤差坐標(biāo)（δx,?δy,?δz），建立果實與機(jī)械爪三維位置數(shù)據(jù)誤差值。

5、s3、將所述誤差值輸至加強(qiáng)型增量式pid算法模塊進(jìn)行處理，輸出當(dāng)前時刻加強(qiáng)型增量式pid算法輸出量δu(t)；所述加強(qiáng)型增量式pid算法模塊為改進(jìn)的增長優(yōu)化算法模塊和增量式pid算法模塊融合，具體方法為：利用改進(jìn)的增長優(yōu)化算法整定增量式pid算法的控制參數(shù)，得到加強(qiáng)型增量式pid算法，所述加強(qiáng)型增量式pid算法用于果實采摘機(jī)械系統(tǒng)控制器。

6、s4、果實采摘機(jī)械系統(tǒng)控制器將所述δu(t)迭加到上一時刻機(jī)械臂調(diào)控信號u(t-1)輸出當(dāng)前時刻機(jī)械臂調(diào)控信號u(t)；所述當(dāng)前時刻機(jī)械臂調(diào)控信號調(diào)控機(jī)械臂運動；同時將實時械臂爪的三維位置反饋到所述s2，循環(huán)執(zhí)行s2至s4達(dá)到δu(t)為零，完成果實采摘機(jī)械系統(tǒng)控制器的優(yōu)化。

7、優(yōu)選的，果實采摘機(jī)械系統(tǒng)包括果實采摘機(jī)器人的機(jī)械臂和機(jī)械爪兩個部分，機(jī)械爪與機(jī)械臂的末端相接，通過果實采摘機(jī)械臂調(diào)整機(jī)械爪的角度和距離；果實采摘機(jī)械系統(tǒng)控制器包括果實和機(jī)械爪位置數(shù)據(jù)輸入模塊、增量式pid算法模塊、果實與機(jī)械爪三維位置數(shù)據(jù)差值計算模塊、機(jī)械爪實時位置數(shù)據(jù)反饋模塊、機(jī)械臂調(diào)控輸出模塊，其中，機(jī)械臂調(diào)控輸出模塊輸出當(dāng)前時刻機(jī)械臂調(diào)控信號u(t)，所述調(diào)控信號u(t)輸入機(jī)器臂三自由度電調(diào)模塊，其中電調(diào)模塊將調(diào)控信號u(t)處理后分別控制機(jī)械臂三個自由度電機(jī)運動。

8、優(yōu)選的，通過計算果實位置與機(jī)械爪實時位置的誤差（δx,?δy,?δz）確定機(jī)械爪需要調(diào)整的角度和距離，其中，機(jī)械爪與目標(biāo)果實之間的直線距離數(shù)學(xué)模型為：

9、（1）；

10、式（1）中，為橫軸位置誤差，為縱軸位置誤差，為z軸位置誤差；為機(jī)械爪與目標(biāo)果實之間的直線距離，該直線距離是機(jī)械爪沿直線軌跡移動所需調(diào)整的距離；

11、更具體地，實驗過程中機(jī)械爪需要調(diào)整的角度和距離即為果實采摘機(jī)械臂需要調(diào)整的角度，機(jī)械臂為三自由度機(jī)械臂，通過機(jī)械臂電調(diào)驅(qū)動三個伺服電機(jī)調(diào)整機(jī)械臂的角度；其中，機(jī)械爪調(diào)整所需的角度在笛卡爾坐標(biāo)系中轉(zhuǎn)換為極坐標(biāo)系表示，定義目標(biāo)果實點的方向角，包括俯仰角和偏航角；其中，俯仰角為垂直方向調(diào)整角度在四象限內(nèi)的分布，數(shù)學(xué)模型為：

12、（2）；

13、偏航角是果實與機(jī)械爪在水平面上的夾角，數(shù)學(xué)模型為：

14、（3）。

15、優(yōu)選地，果實采摘機(jī)械系統(tǒng)控制器的控制算法采用增量式pid算法，對機(jī)械臂調(diào)控信號的增量進(jìn)行調(diào)整，相對于位置式pid，其僅計算當(dāng)前時刻的控制信號增量，基于當(dāng)前誤差的變化量調(diào)整，對計算誤差的累積影響較小，具有較好的魯棒性，數(shù)學(xué)模型為：

16、（4）；

17、式（4）中，為增量式pid算法的比例系數(shù)、為增量式pid算法的積分系數(shù)、為增量式pid算法的微分系數(shù)，、、分別為t、t-1和t-2時刻果實與機(jī)械爪三維位置數(shù)據(jù)誤差值；

18、更具體地，果實位置與機(jī)械爪位置的誤差坐標(biāo)（δx,?δy,?δz），建立果實與機(jī)械爪三維位置數(shù)據(jù)誤差值，數(shù)學(xué)模型為：

19、（5）；

20、更具體地，通過加強(qiáng)型增量式pid算法輸出量δu(t)改變果實采摘機(jī)械系統(tǒng)控制器的輸出機(jī)械臂調(diào)控信號，直到果實與機(jī)械爪三維位置數(shù)據(jù)誤差值趨于零，即加強(qiáng)型增量式pid算法輸出量δu(t)趨于零，果實采摘機(jī)械系統(tǒng)控制器反饋結(jié)束，數(shù)學(xué)模型為：

21、（6）；

22、式中，u(t-1)為上一時刻機(jī)械臂調(diào)控信號，u(t)為當(dāng)前時刻機(jī)械臂調(diào)控信號。

23、優(yōu)選地，機(jī)器臂三自由度電調(diào)模塊是一種現(xiàn)有的控制模塊，可以將單一控制信號轉(zhuǎn)換為多組控制信號，將機(jī)械臂調(diào)控信號轉(zhuǎn)換為一個三維數(shù)組，數(shù)組的第一維數(shù)據(jù)為機(jī)械臂第一自由度的調(diào)控信號增量，調(diào)控偏航角，第二維數(shù)據(jù)為機(jī)械臂第二自由度的調(diào)控信號增量，調(diào)控伸縮長度，第三維數(shù)據(jù)為機(jī)械臂第三自由度的調(diào)控信號增量，調(diào)控俯仰角。

24、優(yōu)選的，利用改進(jìn)的增長優(yōu)化算法對增量式pid算法的比例系數(shù)、積分系數(shù)和微分系數(shù)整定，得到的最佳的控制參數(shù)輸入到公式（4）中得到加強(qiáng)型增量式pid算法，包括：比例系數(shù)、積分系數(shù)和微分系數(shù)；將加強(qiáng)型增量式pid算法用于果實采摘機(jī)械系統(tǒng)控制器的增量式pid算法模塊，提高果實采摘機(jī)械系統(tǒng)控制器對機(jī)械臂的控制精度和靈敏度。

25、優(yōu)選的，增長優(yōu)化算法包括學(xué)習(xí)階段和反思階段兩個階段的數(shù)學(xué)模型，其中，學(xué)習(xí)階段模擬了個體在社會中的學(xué)習(xí)過程，通過從其他個體之間的差距中學(xué)習(xí)來提升自己的知識水平，為增長優(yōu)化算法的全局搜索階段；反思階段模擬了個體在自我反思過程中的行為，通過檢查和彌補(bǔ)自身的不足來提升自己的知識水平，為增長優(yōu)化算法的局部開發(fā)階段；增長優(yōu)化算法通過迭代更新個體位置，找到最佳的個體位置，其迭代過程即為增量式pid算法最佳控制參數(shù)的尋優(yōu)過程，最佳的個體位置即為最佳控制參數(shù)的值。

26、優(yōu)選的，增長優(yōu)化算法的第i個個體的位置向量包括d個維度，其中d為算法問題維度，增長優(yōu)化算法對增量式pid算法優(yōu)化過程中，針對增量式pid算法的控制參數(shù)整定，得到在當(dāng)前環(huán)境下的最佳控制參數(shù)，其中，控制參數(shù)的個數(shù)記為算法問題維度d，其中，增量式pid算法的控制參數(shù)包括比例參數(shù)、微分參數(shù)和積分參數(shù)；第i個個體的位置向量為，第i個個體的位置向量的第1維值映射增量式pid算法的比例參數(shù)值，第i個個體的位置向量的第2維值映射增量式pid算法的積分參數(shù)值，第i個個體的位置向量的第3維值映射增量式pid算法的微分參數(shù)值。

27、優(yōu)選的，增長優(yōu)化算法學(xué)習(xí)階段通過構(gòu)建四種不同標(biāo)準(zhǔn)個體間的差異，模擬個體之間的協(xié)作學(xué)習(xí)，其中四種不同標(biāo)準(zhǔn)個體包括：當(dāng)前種群的最優(yōu)個體、當(dāng)前種群中的精英個體、當(dāng)前種群中的較差個體和當(dāng)前種群中的隨機(jī)個體，其中，四種不同標(biāo)準(zhǔn)個體間的差異的數(shù)學(xué)模型為：

28、（7）；

29、式（7）中，為全局種群的最優(yōu)個體位置向量的第j維值，為適應(yīng)度值排名前的個體，記為當(dāng)前種群中的精英個體的位置向量的第j維值，為最大種群規(guī)模，為適應(yīng)度值排名后的個體，記為當(dāng)前種群中的較差個體的位置向量的第j維值，和為種群中的隨機(jī)的兩個個體的位置向量的第j維值，為當(dāng)前種群的最優(yōu)個體與精英個體的距離，為當(dāng)前種群中的精英個體與較差個體的距離，為當(dāng)前種群的最優(yōu)個體與較差個體的距離，為種群中的隨機(jī)的兩個個體之間的距離。

30、優(yōu)選的，增長優(yōu)化算法學(xué)習(xí)階段根據(jù)知識獲取量更新第i個個體位置，數(shù)學(xué)模型：

31、（8）；

32、式（8）中，為第t+1次迭代第i個個體的位置向量，為第t次迭代第i個個體的位置向量，為第k個知識獲取量，通過四種不同標(biāo)準(zhǔn)個體間的歐幾里得距離得到。

33、優(yōu)選的，當(dāng)前標(biāo)準(zhǔn)增長優(yōu)化算法主要依賴于歐幾里得距離，目標(biāo)方向單一，影響對果實采摘機(jī)械系統(tǒng)控制的精度；對標(biāo)準(zhǔn)增長優(yōu)化算法的學(xué)習(xí)階段個體位置更新策略改進(jìn)，在學(xué)習(xí)階段，引入模仿高適應(yīng)值個體的多方向探索策略得到改進(jìn)的學(xué)習(xí)階段個體位置更新策略，具體步驟為：

34、步驟一、計算當(dāng)前個體與其他n-1個個體的歐幾里得距離，其中距離小于r的作為當(dāng)前個體的鄰域個體，根據(jù)鄰域個體的位置更新當(dāng)前個體的位置，數(shù)學(xué)模型為：

35、（9）；

36、式（9）中，為當(dāng)前個體鄰域r內(nèi)的個體數(shù)；為第i個個體根據(jù)第y個鄰域個體生成的位置向量，其中y=1,2,...,n；為第t次迭代的鄰域解集內(nèi)的第y個鄰域個體位置向量；為第t次迭代第i個個體的位置向量；

37、步驟二、模仿種群中適應(yīng)值小的個體，數(shù)學(xué)模型為：

38、（10）；

39、式（10）中，為第t+1次迭代第i個個體的位置向量，為第i個個體根據(jù)第y個鄰域個體生成的位置向量，其中y=1,2,...,n，和為動態(tài)權(quán)重因子，確保不同來源的信息被合理整合，為全局種群的最優(yōu)個體位置向量，從適應(yīng)值最接近的個體位置向量；

40、步驟三、基于多方向的搜索結(jié)果取最優(yōu)，改進(jìn)單個方向引導(dǎo)的方式，具體方式為：

41、第i個個體生成的n個候選解?的適應(yīng)度值分別為，選擇最優(yōu)候選解更新第i個個體位置向量，數(shù)學(xué)模型為：

42、；其中，為第t+1次迭代的第i個個體的第1個候選解，為第t+1次迭代的第i個個體的第2個候選解，為第t+1次迭代的第i個個體的第n個候選解，為個體位置的適應(yīng)度值，為最小值取值函數(shù)。

43、優(yōu)選的，增長優(yōu)化算法（go）中的衰減因子（af）設(shè)計以當(dāng)前迭代次數(shù)和最大迭代次數(shù)為依據(jù)，逐漸降低探索力度，專注于局部開發(fā)；本發(fā)明提出的方法在標(biāo)準(zhǔn)增長優(yōu)化算法（go）中的衰減因子（af）設(shè)計基礎(chǔ)上，利用種群當(dāng)前迭代個體與上一代個體位置變化的平均幅度動態(tài)調(diào)整衰減因子，在局部開發(fā)階段，通過降低探索力度并專注于局部搜索，能夠更精細(xì)地尋找最優(yōu)解，同時在不同階段自適應(yīng)地調(diào)整策略，避免陷入局部最優(yōu)或過早收斂的問題，提高增長優(yōu)化算法在局部開發(fā)階段對增量式pid算法控制參數(shù)的整定優(yōu)化效率和精度；改進(jìn)的衰減因子數(shù)學(xué)模型為：

44、（11）；

45、式中，為第t次迭代的改進(jìn)的衰減因子值，為最大迭代次數(shù)，為第t次迭代的種群活躍度，數(shù)學(xué)模型為：

46、（12）；

47、式中，為種群初始化時的最大活躍度，為種群最大規(guī)模，為第t次迭代第i個個體第j維的位置值，為第t-1次迭代第i個個體第j維的位置值。

48、優(yōu)選的，利用改進(jìn)的衰減因子，同時引入當(dāng)前種群的最優(yōu)個體位置引導(dǎo)個體位置更新對增長優(yōu)化算法的反思階段的個體位置更新策略數(shù)學(xué)模型改進(jìn)，數(shù)學(xué)模型為：

49、（13）；

50、式中，為第t+1次迭代第i個個體的位置向量，為第t次迭代第i個個體的位置向量，ub和lb為個體位置向量上限和下限，r為0到1內(nèi)的隨機(jī)數(shù)，為第t次迭代的種群的最優(yōu)個體位置向量，p為控制參數(shù)。

51、優(yōu)選的，利用改進(jìn)的學(xué)習(xí)階段和反思階段個體位置更新策略，完成對標(biāo)準(zhǔn)增長優(yōu)化算法的改進(jìn)；利用改進(jìn)的增長優(yōu)化算法對增量式pid算法的比例系數(shù)、積分系數(shù)和微分系數(shù)尋優(yōu)，具體步驟為：

52、s31、將果實采摘機(jī)械系統(tǒng)控制器的增量式pid算法的控制參數(shù)編碼為空間向量u，維度為3，將所述空間向量與改進(jìn)的增長優(yōu)化算法的第i個個體的位置向量建立映射；

53、s32、設(shè)置改進(jìn)的增長優(yōu)化算法的最大迭代次數(shù)t，問題維度d，種群最大規(guī)模n，個體位置向量上限ub和下限lb，隨機(jī)初始化每個個體的位置向量值；

54、s33、利用目標(biāo)函數(shù)計算當(dāng)前每個個體的位置的適應(yīng)度值，保留當(dāng)前最小適應(yīng)度值對應(yīng)的個體位置，將其作為第t次迭代的種群的最優(yōu)個體位置向量；按照公式（14）更新全局種群的最優(yōu)個體位置向量；

55、（14）；

56、式中，為全局種群的最優(yōu)個體位置向量，為全局種群的最優(yōu)個體位置向量的適應(yīng)度值；

57、s34、若當(dāng)前迭代次數(shù)t滿足t>t，則將當(dāng)前全局種群的最優(yōu)個體位置向量輸出解析賦值于為果實采摘機(jī)械系統(tǒng)控制器的增量式pid算法的控制參數(shù)；否則執(zhí)行改進(jìn)的增長優(yōu)化算法的個體位置更新數(shù)學(xué)模型；

58、s35、利用改進(jìn)的學(xué)習(xí)階段個體位置更新策略對n個個體的位置更新，計算第t次迭代的改進(jìn)的衰減因子值，根據(jù)所述衰減因子值建立反思階段的個體位置更新策略更新n個個體的位置；

59、s36、將更新后的個體位置向量限制在個體位置向量上限ub和下限lb范圍內(nèi)；迭代次數(shù)自加一，返回執(zhí)行s33。

60、優(yōu)選的，適應(yīng)度值的作用是評價解的優(yōu)越性，適應(yīng)度值的大小反映了當(dāng)前果實采摘機(jī)械系統(tǒng)控制器的控制精度，即果實采摘機(jī)械系統(tǒng)控制器的增量式pid算法的比例系數(shù)、積分系數(shù)和微分系數(shù)精度，通過將改進(jìn)的增長優(yōu)化算法的個體位置解析為增量式pid算法的比例系數(shù)、積分系數(shù)和微分系數(shù)用于果實采摘機(jī)械系統(tǒng)控制器，通過目標(biāo)函數(shù)計算果實采摘機(jī)械系統(tǒng)控制器在當(dāng)前比例系數(shù)、積分系數(shù)和微分系數(shù)下的控制誤差大小得到適應(yīng)度值，適應(yīng)度值越小則果實采摘機(jī)械系統(tǒng)控制器的當(dāng)前控制誤差越小，反之控制誤差越大；目標(biāo)函數(shù)數(shù)學(xué)模型為：

61、（15）；

62、式中，為目標(biāo)函數(shù)，e(t)為第t時刻果實與機(jī)械爪三維位置數(shù)據(jù)誤差值。

63、相比現(xiàn)有技術(shù)，本發(fā)明的技術(shù)方案具有如下優(yōu)點和有益效果：本發(fā)明利用改進(jìn)的增長優(yōu)化算法對果實采摘機(jī)械系統(tǒng)控制器的pid控制算法的控制參數(shù)進(jìn)行優(yōu)化，顯著提高了控制器的控制精度和靈敏度；通過精確調(diào)整機(jī)械臂的運動，實現(xiàn)了對果實的精準(zhǔn)采摘，減少了定位偏差和采摘過程中的誤差，改進(jìn)的增長優(yōu)化算法通過全局搜索和局部開發(fā)兩個階段，能夠更精細(xì)地尋找最優(yōu)解，避免了陷入局部最優(yōu)或過早收斂的問題，因此，本發(fā)明的方法在不同果實種類和采摘條件下均能保持較好的控制性能，增強(qiáng)了系統(tǒng)的適應(yīng)性和魯棒性；傳統(tǒng)的pid控制器需要頻繁調(diào)節(jié)比例、積分和微分參數(shù)以適應(yīng)不同的采摘條件，這不僅耗時而且難以獲得最優(yōu)解，而本發(fā)明的方法通過自動優(yōu)化控制參數(shù)，降低了人工調(diào)參的成本和難度。