[灯哥开源—四足机器人]程序算法讲解与STM32移植——PA_STABLIZE自稳文件讲解

目录

写在前面

自稳原理

代码实现

---

写在前面

由于四足机器人为8自由度的机器人，故自稳模块是当四足机器人所在平面前后出现角度变化时，身体仍然保持水平的功能模块，而机器人所在平面左右发生角度变化时，此模块并不起作用。此外，当机器人出现翻到时，机器人会进入recover模式，进行起身，重新站立。本次讲解自稳的原理，并没有拘泥于8自由度的四足机器人的自稳模型进行讲解，而是对12自由度的四足机器人的自稳模型进行讲解，大家可自行去掉多余自由度即可得出推导公式。

自稳原理

        因此在实际工程问题中，上式公式的未知量为x，y，z三轴的夹角，此夹角我们可以通过安装在四足机器人的陀螺仪获得，便可以得出此时对应的足端坐标的值，即B1A1。

代码实现

        自稳的逻辑代码主要在文件PA_STABLIZE中，其主要可以分为获取陀螺仪数据，对姿态进行判断决策，和最后的输出。值得注意的是，对对姿态进行判断决策中分为两种情况，1.发生轻微倾斜可自主调整 和 2.摔倒后自己恢复。

         获取陀螺仪数据这一部分情况比较复杂，在函数stab的开头首先是调用了PA_IMU文件中的get_values函数对陀螺仪的数据进行获取，在PA_IMU文件中class accel中，封装了获取陀螺仪参数的一系列函数，这些函数中有对特定数据的位操作，有调用了 i2c 协议通讯的库文件，有通讯的初始化函数等。总而言之，通过这一系列操作，我们可以从获得最原始的数据！这些数值保存在数组 ay 当中。

        而在函数stab的结尾，将数组 ay 的值全部传入PA_IMU.IMUupdate进行处理，通过对原始数据的处理，代码中已经详细标明。通过IMUupdate的处理，我们获得到可以直接使用的数据 数组 q 。

def IMUupdate(gx,gy,gz,ax,ay,az):
    K=0.7
    a=[0,0,0,0,0,0,0,0]
    global Kp,Ki,halfT,q0,q1,q2,q3,exInt,eyInt,ezInt
    if ax!=0 or ay!=0 or az!=0: 
        norm=math.sqrt(ax*ax+ay*ay+az*az);
    ax=ax/norm; #单位化
    ay=ay/norm;
    az=az/norm;
    #估计方向的重力
    vx=2* (q1*q3-q0*q2 );
    vy=2* (q0*q1+q2*q3 );
    vz=q0*q0-q1*q1-q2*q2+q3*q3;
    #错误的领域和方向传感器测量参考方向之间的交叉乘积的总和
    ex= (ay*vz-az*vy );
    ey= (az*vx-ax*vz );
    ez= (ax*vy-ay*vx );
    #积分误差比例积分增益
    exInt=exInt+ex*Ki;
    eyInt=eyInt+ey*Ki;
    ezInt=ezInt+ez*Ki;
    #调整后的陀螺仪测量
    gx=gx+Kp*ex+exInt;
    gy=gy+Kp*ey+eyInt;
    gz=gz+Kp*ez+ezInt;
    #整合四元数率和正常化
    q0=q0+ (-q1*gx-q2*gy-q3*gz )*halfT;
    q1=q1+ (q0*gx+q2*gz-q3*gy )*halfT;
    q2=q2+ (q0*gy-q1*gz+q3*gx )*halfT;
    q3=q3+ (q0*gz+q1*gy-q2*gx )*halfT;
    #正常化四元
    norm=math.sqrt(q0*q0+q1*q1+q2*q2+q3*q3 );
    q0=q0/norm;
    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; #rollv
    a[0]=Pitch
    a[1]=Roll
    if ay*ay+az*az!=0:
        a[2]=-math.atan(ax/math.sqrt(ay*ay+az*az))*57.2957795
    if ax*ax+az*az!=0:
        a[3]=math.atan(ay/math.sqrt(ax*ax+az*az))*57.2957795
    a[4]=gx
    a[5]=gy
    a[6]=gz
    a[0]=-K*Pitch-(1-K)*a[2]
    a[1]=K*Roll+(1-K)*a[3]
    return a

        此时，已经完成了数据的获取工作，紧接着进行对姿态进行判断决策。首先介绍的是当水平面变化较大时候，即 q[1]>3 或 q[1]<-3 对 Pitch 方向进行处理，每个周期按照一定的速度（filter_data_p*kp_sta）进行调整，调整完之后的数值进行与最大值pit_max_ang进行限幅处理。另外 Roll 方向同 Pitch 方向的处理思路一致我就不再赘述。

         其次是介绍翻转时候的处理，当感受到角度变化过大时，即 q[2]<=-50 或 q[2]>=50 时，机器人将进入 recover 模式。recover模式时，机器人是按照事先写好的动作，利用运动学逆解模式对四腿进行输出，完成机器人的重新起立的动作。为了理解，可以点击这里看灯哥的这个视频。

         最后就是对上面所作的所有工作，通过 padog.gesture(Sta_Pitch,Sta_Roll,0) 进行赋值，从而达到本模块输出的目的了。