燈哥開源-四足機(jī)器人程序算法講解與STM32移植-PA-STABLIZE自穩(wěn)文件講解

1、 燈哥開源四機(jī)器程序算法講解與STM32移植PA_STABLIZE穩(wěn)件講解錄寫在前由于四機(jī)器為8由度的機(jī)器，故穩(wěn)模塊是當(dāng)四機(jī)器所在平前后出現(xiàn)度變化時，體仍然保持平的功能模塊，機(jī)器所在平左右發(fā)度變化時，此模塊并不起作。此外，當(dāng)機(jī)器出現(xiàn)翻到時，機(jī)器會進(jìn)recover模式，進(jìn)起，重新站。本次講解穩(wěn)的原理，并沒有拘泥于8由度的四機(jī)器的穩(wěn)模型進(jìn)講解，是對12由度的四機(jī)器的穩(wěn)模型進(jìn)講解，家可去掉多余由度即可得出推導(dǎo)公式。穩(wěn)原理 因此在實(shí)際程問題中，上式公式的未知量為x，y，z三軸的夾，此夾我們可以通過安裝在四機(jī)器的陀螺儀獲得，便可以得出此時對應(yīng)的端坐標(biāo)的值，即B1A1。代碼實(shí)現(xiàn)穩(wěn)的邏輯代碼主要在件PA_S

2、TABLIZE中，其主要可以分為獲取陀螺儀數(shù)據(jù)，對姿態(tài)進(jìn)判斷決策，和最后的輸出。值得注意的是，對對姿態(tài)進(jìn)判斷決策中分為兩種情況，1.發(fā)輕微傾斜可主調(diào)整 和 2.摔倒后恢復(fù)。獲取陀螺儀數(shù)據(jù)這部分情況較復(fù)雜，在函數(shù)stab的開頭先是調(diào)了PA_IMU件中的get_values函數(shù)對陀螺儀的數(shù)據(jù)進(jìn)獲取，在PA_IMU件中class accel中，封裝了獲取陀螺儀參數(shù)的系列函數(shù)，這些函數(shù)中有對特定數(shù)據(jù)的位操作，有調(diào)了 i2c 協(xié)議通訊的庫件，有通訊的初始化函數(shù)等?？傊ㄟ^這系列操作，我們可以從獲得最原始的數(shù)據(jù)！這些數(shù)值保存在數(shù)組 ay 當(dāng)中。在函數(shù)stab的結(jié)尾，將數(shù)組 ay 的值全部傳PA_IMU.

3、IMUupdate進(jìn)處理，通過對原始數(shù)據(jù)的處理，代碼中已經(jīng)詳細(xì)標(biāo)明。通過IMUupdate的處理，我們獲得到可以直接使的數(shù)據(jù) 數(shù)組 q 。 def IMUupdate(gx,gy,gz,ax,ay,az):K=0.7a=0,0,0,0,0,0,0,0global Kp,Ki,halfT,q0,q1,q2,q3,exInt,eyInt,ezIntif ax!=0 or ay!=0 or az!=0:norm=math.sqrt(ax*ax+ay*ay+az*az);ax=ax/norm; # 單位化vz=q0*q0-q1*q1-q2*q2+q3*q3;#錯誤的領(lǐng)域和向傳感器測量參考向之間的交叉乘

4、積的總和ex= (ay*vz-az*vy );gx=gx+Kp*ex+exInt;gy=gy+Kp*ey+eyInt;gz=gz+Kp*ez+ezInt;q1=q1/norm;q2=q2/norm;q3=q3/norm;Pitch=math.asin (-2*q1*q3+2*q0*q2 )*57.3; #pitch , 轉(zhuǎn)換為度數(shù)if -2*q1*q1-2*q2*q2+1!=0:Roll=math.atan (2*q2*q3+2*q0*q1)/(-2*q1*q1-2*q2*q2+1) )*57.3; #rollva0=Pitcha1=Rollif ay*ay+az*az!=0:a5=gya6=gzreturn a此時，已經(jīng)完成了數(shù)據(jù)的獲取作，緊接著進(jìn)對姿態(tài)進(jìn)判斷決策。先介紹的是當(dāng)平變化較時候，即 q13 或 q1-3對 Pitch 向進(jìn)處理，每個周期按照定的速度（filter_data_p*kp_sta）進(jìn)調(diào)整，調(diào)整完之后的數(shù)值進(jìn)與最值pit_max_ang進(jìn)限幅處理。另外 Roll 向同 Pitch 向的處理思路致我就不再贅述。 其次是介紹翻轉(zhuǎn)時候的處理，當(dāng)感受到度變化過時，即 q2=50 時，機(jī)器將進(jìn) recover 模式。recover模式時，機(jī)器是按照事先寫好的動作，利運(yùn)動學(xué)逆解模式對四腿進(jìn)輸出，完成機(jī)器的重新起的動作。為了理解，可以