灯哥开源-四足机器人程序算法讲解与STM32移植-PA-STABLIZE自稳文件讲解

1、 灯哥开源四机器程序算法讲解与STM32移植PA_STABLIZE稳件讲解录写在前由于四机器为8由度的机器，故稳模块是当四机器所在平前后出现度变化时，体仍然保持平的功能模块，机器所在平左右发度变化时，此模块并不起作。此外，当机器出现翻到时，机器会进recover模式，进起，重新站。本次讲解稳的原理，并没有拘泥于8由度的四机器的稳模型进讲解，是对12由度的四机器的稳模型进讲解，家可去掉多余由度即可得出推导公式。稳原理 因此在实际程问题中，上式公式的未知量为x，y，z三轴的夹，此夹我们可以通过安装在四机器的陀螺仪获得，便可以得出此时对应的端坐标的值，即B1A1。代码实现稳的逻辑代码主要在件PA_S

2、TABLIZE中，其主要可以分为获取陀螺仪数据，对姿态进判断决策，和最后的输出。值得注意的是，对对姿态进判断决策中分为两种情况，1.发轻微倾斜可主调整 和 2.摔倒后恢复。获取陀螺仪数据这部分情况较复杂，在函数stab的开头先是调了PA_IMU件中的get_values函数对陀螺仪的数据进获取，在PA_IMU件中class accel中，封装了获取陀螺仪参数的系列函数，这些函数中有对特定数据的位操作，有调了 i2c 协议通讯的库件，有通讯的初始化函数等。总之，通过这系列操作，我们可以从获得最原始的数据！这些数值保存在数组 ay 当中。在函数stab的结尾，将数组 ay 的值全部传PA_IMU.

3、IMUupdate进处理，通过对原始数据的处理，代码中已经详细标明。通过IMUupdate的处理，我们获得到可以直接使的数据 数组 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;#错误的领域和向传感器测量参考向之间的交叉乘

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 , 转换为度数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此时，已经完成了数据的获取作，紧接着进对姿态进判断决策。先介绍的是当平变化较时候，即 q13 或 q1-3对 Pitch 向进处理，每个周期按照定的速度（filter_data_p*kp_sta）进调整，调整完之后的数值进与最值pit_max_ang进限幅处理。另外 Roll 向同 Pitch 向的处理思路致我就不再赘述。 其次是介绍翻转时候的处理，当感受到度变化过时，即 q2=50 时，机器将进 recover 模式。recover模式时，机器是按照事先写好的动作，利运动学逆解模式对四腿进输出，完成机器的重新起的动作。为了理解，可以