本發(fā)明涉及云臺姿態(tài)控制領(lǐng)域，具體涉及一種伺服機器手云臺姿態(tài)自適應(yīng)控制方法。

背景技術(shù)：

1、在實際工作中，伺服機械手主要依靠底盤云臺調(diào)整才能準(zhǔn)確到達(dá)目標(biāo)位置，傳統(tǒng)云臺控制系統(tǒng)在面對復(fù)雜、多變的環(huán)境時，無法快速、精確地做出姿態(tài)調(diào)整，導(dǎo)致伺服機械手抓取魯棒性差；其次，在復(fù)雜工作環(huán)境中，外界干擾（如機械震動、電磁干擾）常常導(dǎo)致云臺姿態(tài)的偏移，傳統(tǒng)的云臺控制系統(tǒng)對這些外界干擾的過濾和補償能力有限，容易出現(xiàn)控制滯后或姿態(tài)調(diào)整不足的情況，進而影響伺服機械手的精確度；再者，傳統(tǒng)的控制方法大多依賴于預(yù)設(shè)控制模型，如標(biāo)準(zhǔn)pid控制算法，缺乏足夠的實時反饋機制，且比例參數(shù)kp、積分參數(shù)ki和微分參數(shù)kd固定，不能實現(xiàn)動態(tài)環(huán)境下的實時自適應(yīng)調(diào)整，機械手需要根據(jù)傳感器反饋數(shù)據(jù)不斷修正其運動軌跡和姿態(tài)，而傳統(tǒng)云臺控制系統(tǒng)在這方面表現(xiàn)得較為遲鈍，在機械手負(fù)載變化時云臺姿態(tài)控制穩(wěn)態(tài)誤差也較大。

2、波搜索算法（wsa）是一種受雷達(dá)技術(shù)啟發(fā)的優(yōu)化方法，旨在解決復(fù)雜的優(yōu)化問題，該算法結(jié)合了確定性優(yōu)化技術(shù)和不確定性優(yōu)化技術(shù)的優(yōu)點，通過模擬雷達(dá)波粒子群的發(fā)射、反射和接收過程，實現(xiàn)了對搜索空間的有效探索和利用；wsa算法采用了一種獨特的初始化方法來生成初始粒子群，有助于增加粒子群的多樣性；結(jié)合了擬合的梯度下降方法，利用問題的解析信息來提高搜索效率和精度；wsa存在的問題是算法中的參數(shù)（如初始步長、步長縮放系數(shù)）對算法性能有較大影響，需要繁瑣調(diào)整以獲得最佳算法尋優(yōu)性能，參數(shù)設(shè)置不當(dāng)導(dǎo)致算法收斂速度變慢或陷入局部最優(yōu)解，同時，雖然算法采用了廣義邊界限制規(guī)則來處理越界粒子，但這種策略在一定程度上減少波粒子群的多樣性，影響搜索性能。

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

1、本發(fā)明針對傳統(tǒng)機械手云臺在機械手負(fù)載變化時存在的控制魯棒性差的問題以及標(biāo)準(zhǔn)波搜索算法（wsa）對pid控制性能缺陷的問題，提供了一種伺服機器手云臺姿態(tài)自適應(yīng)控制方法，通過對波搜索算法（wsa）改進，利用改進的波搜索算法（gwsa）優(yōu)化位置式pid算法的控制參數(shù)得到加強型pid算法，其中，控制參數(shù)包括：比例參數(shù)kp、積分參數(shù)ki和微分參數(shù)kd，通過加強型pid算法對伺服機器手的云臺系統(tǒng)控制，實現(xiàn)伺服機器手的云臺姿態(tài)精確控制。

2、為了實現(xiàn)上述目的，本發(fā)明采用的技術(shù)方案如下：一種伺服機器手云臺姿態(tài)自適應(yīng)控制方法，利用改進的波搜索算法（gwsa）優(yōu)異的尋優(yōu)性能，找到伺服機器手的云臺姿態(tài)控制系統(tǒng)的pid算法最佳的kp、ki、kd控制參數(shù)，具體步驟如下。

3、s1、輸入伺服機器手目標(biāo)位置的三維坐標(biāo)點(x1,y1,z1)，將所述三維坐標(biāo)點的橫軸的坐標(biāo)點x1和縱軸的坐標(biāo)點y1組成目標(biāo)位置二維坐標(biāo)點(x1,y1)。

4、s2、根據(jù)目標(biāo)位置二維坐標(biāo)點(x1,y1)與伺服機器手當(dāng)前位置二維坐標(biāo)點(x2,y2)，計算出伺服機器手的目標(biāo)位置與當(dāng)前位置的xy平面姿態(tài)偏角。

5、s3、所述xy平面姿態(tài)偏角輸入云臺姿態(tài)轉(zhuǎn)換數(shù)學(xué)模型，輸出云臺旋轉(zhuǎn)角度誤差值。

6、s4、所述云臺旋轉(zhuǎn)角度誤差值輸入到改進型pid算法，所述改進型pid算法具體為：利用改進波搜索算法整定位置式pid的kp、ki、kd控制參數(shù)得到最佳控制參數(shù)，將所述最佳控制參數(shù)用于位置式pid算法得到改進型pid算法；所述改進波搜索算法包括：

7、s41、建立啟發(fā)式指導(dǎo)策略，具體為：通過引入啟發(fā)式規(guī)則來引導(dǎo)波搜索算法對位置式pid控制參數(shù)尋優(yōu)，改進所述波搜索算法的尋優(yōu)機制；

8、s42、基于波粒子間適應(yīng)度值的方差動態(tài)的正弦策略和波粒子全局最小適應(yīng)度值改進波形大小控制系數(shù)α，利用所述改進的波形大小控制系數(shù)改進波搜索算法局部開發(fā)階段波粒子位置更新數(shù)學(xué)模型。

9、s5、將改進型pid算法用于云臺姿態(tài)控制系統(tǒng)，根據(jù)實時云臺旋轉(zhuǎn)角度誤差值，輸出云臺驅(qū)動值對云臺旋轉(zhuǎn)角度調(diào)整，實現(xiàn)對云臺姿態(tài)的自適應(yīng)精確穩(wěn)定控制。

10、優(yōu)選地，云臺姿態(tài)控制系統(tǒng)包含伺服機器手的云臺和云臺驅(qū)動單元，其中，伺服機器手的云臺為無刷直流電機，云臺驅(qū)動單元包括加強型pid算法和云臺姿態(tài)轉(zhuǎn)換數(shù)學(xué)模型；

11、所述云臺姿態(tài)轉(zhuǎn)換數(shù)學(xué)模型將xy平面姿態(tài)偏角轉(zhuǎn)化為云臺旋轉(zhuǎn)角度誤差值，云臺姿態(tài)控制系統(tǒng)根據(jù)云臺旋轉(zhuǎn)角度誤差值調(diào)整云臺的姿態(tài)達(dá)到目標(biāo)姿態(tài)；其中，云臺驅(qū)動單元的加強型pid算法負(fù)責(zé)計算伺服機器手的云臺驅(qū)動信號值。

12、優(yōu)選地，加強型pid算法數(shù)學(xué)模型為：

13、（8）；

14、式（8）中，為第秒的云臺驅(qū)動值，為運行時間，為第秒的云臺旋轉(zhuǎn)角度誤差值，為最佳比例參數(shù)，為最佳積分參數(shù)，為最佳微分參數(shù)。

15、優(yōu)選地，通過云臺姿態(tài)轉(zhuǎn)換數(shù)學(xué)模型將伺服機械手的xy平面姿態(tài)偏角轉(zhuǎn)換為云臺旋轉(zhuǎn)角度誤差值，其中，云臺姿態(tài)轉(zhuǎn)換數(shù)學(xué)模型為：

16、（9）；

17、式（9）中，為第秒的云臺旋轉(zhuǎn)角度誤差值，,,為姿態(tài)誤差權(quán)重、姿態(tài)誤差變化率權(quán)重和加速度權(quán)重，為第秒的xy平面姿態(tài)偏角，為xy平面姿態(tài)誤差的變化率，為xy平面姿態(tài)誤差的加速度。

18、優(yōu)選地，對波搜索算法改進，其中，引入啟發(fā)式指導(dǎo)策略，其一目的為了增加波搜索算法的波粒子群的多樣性，從而為改進后的波搜索算法對位置式pid控制參數(shù)尋優(yōu)提供更多的最佳控制參數(shù)候選解；其二目的是加速波搜索算法的局部收斂速度，從而提高改進后的波搜索算法對位置式pid控制參數(shù)尋優(yōu)得到的位置式pid的kp、ki、kd參數(shù)精度，進一步的，改進波形大小控制系數(shù)α，其一目的為了平衡波搜索算法的全局搜索和局部收斂階段的平衡性，從而避免改進后的波搜索算法算法在尋優(yōu)過程中陷入局部最優(yōu)值，提高位置式pid的kp、ki、kd參數(shù)精度，最后目的實現(xiàn)對云臺的姿態(tài)精確穩(wěn)定控制。

19、優(yōu)選地，設(shè)計啟發(fā)式指導(dǎo)策略，通過引入啟發(fā)式規(guī)則來引導(dǎo)波搜索算法搜索過程，提高波搜索算法的尋優(yōu)收斂速度，所述啟發(fā)式規(guī)則所使用的引導(dǎo)函數(shù)h根據(jù)波粒子隨機位置縮小波搜索算法搜索空間，數(shù)學(xué)模型為:

20、（1）；

21、式（1）中，為隨機生成的第i個波粒子個體第j維的位置值，即隨機候選解值，對隨機候選解值進行調(diào)整的啟發(fā)式引導(dǎo)函數(shù)，其中，為引導(dǎo)強度系數(shù)，控制調(diào)整的幅度，為啟發(fā)式信息函數(shù)，所述啟發(fā)式信息函數(shù)用于表示從隨機候選解值到局部區(qū)域范圍r的中心的調(diào)整量。

22、優(yōu)選地，局部區(qū)域范圍r用一個高斯分布來表示，局部區(qū)域范圍r的中心即第t次迭代的波搜索算法個體最佳位置表示為，協(xié)方差矩陣為σ，建立數(shù)學(xué)模型定義啟發(fā)式信息函數(shù)為：

23、（2）；

24、式（2）中，為第t次迭代的波搜索算法個體最佳位置，為轉(zhuǎn)置操作。

25、優(yōu)選地，所述啟發(fā)式指導(dǎo)策略用于波搜索算法初始化階段和局部開發(fā)階段，具體方法為：

26、s311、在每次迭代初始化波搜索算法種群位置時，在波搜索算法搜索空間內(nèi)利用引導(dǎo)函數(shù)h(x)生成候選解集，數(shù)學(xué)模型為：

27、（3）；

28、式（3）中，為候選集中第i個波粒子個體的位置，為隨機生成的第i個波粒子個體第j維的位置值，即隨機候選解值，對隨機候選解值進行調(diào)整的啟發(fā)式引導(dǎo)函數(shù)；

29、s312、在局部開發(fā)階段，利用引導(dǎo)函數(shù)h(x)更新候選解集，所述候選解集為波粒子種群位置，數(shù)學(xué)模型為：

30、（4）；

31、式（4）中，為第t+1次迭代第i個波粒子第j維的位置值，為第t次迭代第i個波粒子第j維的位置值，為最大迭代次數(shù)，為對解值進行調(diào)整的啟發(fā)式引導(dǎo)函數(shù)；為全局的波粒子個體最佳位置。

32、優(yōu)選地，基于波粒子間適應(yīng)度值的方差動態(tài)的正弦策略和波粒子全局最小適應(yīng)度值改進波形大小控制系數(shù)α，改進后的波形大小控制系數(shù)α(t)數(shù)學(xué)模型為：

33、（5）；

34、式（5）中，為改進后的波形大小控制系數(shù)初始值，為適應(yīng)度值的方差，為適應(yīng)度值的平均值，為適應(yīng)度值，為波粒子全局最小適應(yīng)度值，為第t次迭代的最小適應(yīng)度值，其中，數(shù)學(xué)模型為：

35、（6）；

36、式（6）中，n為波粒子種群規(guī)模，為第i個波粒子位置的適應(yīng)度值。

37、優(yōu)選地，基于適應(yīng)度方差的動態(tài)調(diào)整使得改進的波搜索算法能夠根據(jù)波粒子種群的多樣性自適應(yīng)調(diào)整波形大小控制系數(shù)α(t)，當(dāng)波粒子種群表現(xiàn)出高多樣性時，α(t)增大，從而增加改進的波搜索算法在迭代前期搜索范圍和探索不同區(qū)域的能力；同時，波形大小控制系數(shù)α(t)能夠根據(jù)改進的波搜索算法的收斂速度動態(tài)調(diào)整，從而在搜索后期細(xì)化搜索范圍，提高解的精度，當(dāng)算法快速接近最優(yōu)解時，減小α(t)可以減少改進的波搜索算法尋優(yōu)過程中過沖和振蕩，幫助改進的波搜索算法穩(wěn)定地收斂到全局最優(yōu)解；最后，引入隨機擾動元素有助于算法在面對復(fù)雜的搜索空間和多個局部最優(yōu)時保持一定的隨機性和靈活性，隨機擾動使得改進的波搜索算法在收斂到某個局部最優(yōu)解時，可以提高改進的波搜索算法跳出并繼續(xù)探索其他的優(yōu)解區(qū)域。

38、優(yōu)先地，所述利用改進的波形大小控制系數(shù)改進波搜索算法局部開發(fā)階段的波粒子位置更新數(shù)學(xué)模型，具體為局部開發(fā)階段的發(fā)射雷達(dá)波數(shù)學(xué)模型；

39、（7）；

40、式（7）中，為第t次迭代第i個波粒子的新位置，為當(dāng)前最佳波粒子位置，為當(dāng)前次佳波粒子位置，為一個列向量，元素服從正態(tài)分布，為第t次迭代的改進后的波形大小控制系數(shù)值。

41、優(yōu)選地，利用改進的波搜索算法整定位置式pid的kp、ki、kd參數(shù)得到最佳控制參數(shù)，其中，?位置式pid的kp、ki、kd參數(shù)作為一個空間向量與改進的波搜索算法的波粒子位置映射，具體步驟為：

42、p1、初始化改進的波搜索算法的參數(shù)，包括：波粒子種群規(guī)模n、問題維度d、搜索空間上界ub 、搜索空間下界lb以及最大迭代次數(shù)tmax以及改進的波形大小控制系數(shù)；

43、p2、將位置式pid算法的kp、ki、kd比例系數(shù)編碼為n組三維空間向量，所述空間向量與波粒子位置實數(shù)映射；

44、p3、隨機初始化改進的波搜索算法種群初始位置，利用公式（3）數(shù)學(xué)模型在波搜索算法搜索空間內(nèi)利用引導(dǎo)函數(shù)h(x)生成候選解，其中，搜索空間上界和下界為三維空間向量的取值范圍；

45、p4、判斷當(dāng)前迭代次數(shù)t等于最大迭代次數(shù)tmax，若是，則輸出最小適應(yīng)度值對應(yīng)的波粒子位置并解析成位置式pid算法最佳的kp、ki、kd比例系數(shù)值；否則，返回執(zhí)行p5；

46、p5、建立改進的波搜索算法位置更新數(shù)學(xué)模型，更新波粒子位置，包括：

47、p51、通過引入隨機數(shù)r1和當(dāng)前迭代次數(shù)建立全局搜索階段位置更新數(shù)學(xué)模型，生成波粒子新的位置；

48、p52、模擬雷達(dá)波的發(fā)射過程，按照公式（7）建立數(shù)學(xué)模型生成新的波粒子位置，探索潛在的最優(yōu)解區(qū)域；

49、p53、模擬雷達(dá)波的反射和接收過程，建立局部開發(fā)階段數(shù)學(xué)模型對波粒子種群位置進行更新，通過多次迭代更新波粒子位置，逐步逼近全局最優(yōu)解，所述全局最優(yōu)解為波粒子種群中適應(yīng)度值最小的波粒子位置；

50、p54、利用數(shù)學(xué)模型公式（4）對當(dāng)前波粒子種群位置進行進一步的更新；

51、p6、利用目標(biāo)函數(shù)計算每個波粒子位置的適應(yīng)度值，保留從初始化到第t次迭代最小適應(yīng)度值對應(yīng)的波粒子位置，當(dāng)前迭代次數(shù)t執(zhí)行t=t+1，返回執(zhí)行步驟p3。

52、優(yōu)選地，確保云臺在調(diào)整姿態(tài)過程中的平穩(wěn)性，避免突然的運動和抖動，同時確保云臺能準(zhǔn)確達(dá)到目標(biāo)的姿態(tài)角度，設(shè)計目標(biāo)函數(shù)j，其數(shù)學(xué)模型為：

53、（10）；

54、式（10）中，為運行時間，為第秒的云臺實際姿態(tài)角度，為第秒的云臺目標(biāo)姿態(tài)角度，為云臺實際姿態(tài)角度的平均值。

55、優(yōu)選地，適應(yīng)度值是評價每組pid控制參數(shù)控制效果的指標(biāo)，適應(yīng)度值由目標(biāo)函數(shù)計算得到，具體為：將每個波粒子位置解析為一組三維空間向量，即每個空間向量包括kp、ki、kd值，將所述kp、ki、kd值輸入位置式pid控制算法中，用于整個云臺姿態(tài)控制系統(tǒng)，根據(jù)所述姿態(tài)控制系統(tǒng)輸出的實時姿態(tài)響應(yīng)計算目標(biāo)函數(shù)值，記為適應(yīng)度函數(shù)值，所述姿態(tài)響應(yīng)為第秒的實際姿態(tài)角度。

56、優(yōu)選地，改進的波搜索算法（gwsa）在全局搜索階段主要以提高算法全局探索信息，提高波粒子群的多樣性為目的，通過引入隨機數(shù)和當(dāng)前迭代次數(shù)建立全局搜索階段位置更新數(shù)學(xué)模型，數(shù)學(xué)模型為：

57、（11）；

58、式（11）中，為第t次迭代第i個波粒子的新位置，為第i個波粒子最小值位置，為第i個波粒子最大值位置，為0到1的隨機數(shù)。

59、優(yōu)選地，改進的波搜索算法（gwsa）在局部開發(fā)階段分為雷達(dá)波的反射和雷達(dá)波的接收過程，其中，雷達(dá)波的反射模擬部分，按照公式（12）建立粒子遇到障礙物反射回最優(yōu)位置建立波粒子位置數(shù)學(xué)模型；

60、（12）；

61、式（12）中，為第t次迭代第i個波粒子的新位置，β為反射強度因子，為0到1的隨機數(shù)，為模擬反射電磁波的總波粒子數(shù)，為當(dāng)前最佳波粒子位置，為從最小值到最大值重新排列后的位置矩陣。

62、優(yōu)選地，按照公式（13），雷達(dá)波的接收過程模擬雷達(dá)接收電磁波建立波粒子位置數(shù)學(xué)模型，對波粒子位置進行修正和處理；

63、（13）；

64、式（12）中，為第t次迭代第i個波粒子的新位置，為第t次迭代第i個波粒子的更新前的位置，為服從正態(tài)分布的隨機數(shù)，為接收強度因子，為當(dāng)前最佳波粒子位置，為模擬接收到的電磁波的總波粒子數(shù)，、、和為0到1內(nèi)的隨機數(shù)，為接收權(quán)重因子。

65、綜上所述，由于采用了上述技術(shù)方案，本發(fā)明的有益效果是：本發(fā)明通過改進的波形大小控制系數(shù)α和引入的啟發(fā)式指導(dǎo)策略，提升了標(biāo)準(zhǔn)波搜索算法在不同階段的尋優(yōu)精度和速度，利用改進的波搜索算法優(yōu)化位置式pid的kp、ki、kd控制參數(shù)得到的改進型pid算法，提高了伺服機器手的云臺姿態(tài)控制系統(tǒng)的響應(yīng)速度和精度，進而能夠精確控制云臺的姿態(tài)角度，使其在機械手負(fù)載變化時，云臺控制魯棒性達(dá)到最優(yōu)，同時，改進型pid算法能夠更好地適應(yīng)環(huán)境變化，在復(fù)雜工作環(huán)境中，云臺的姿態(tài)控制能保持穩(wěn)定。