目录

写在前面

自稳原理

代码实现

写在前面

由于四足机器人为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.7a=[0,0,0,0,0,0,0,0]global 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; #单位化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; #rollva[0]=Pitcha[1]=Rollif ay*ay+az*az!=0:a[2]=-math.atan(ax/math.sqrt(ay*ay+az*az))*57.2957795if ax*ax+az*az!=0:a[3]=math.atan(ay/math.sqrt(ax*ax+az*az))*57.2957795a[4]=gxa[5]=gya[6]=gza[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) 进行赋值，从而达到本模块输出的目的了。 

本文来自互联网用户投稿，文章观点仅代表作者本人，不代表本站立场，不承担相关法律责任。如若转载，请注明出处。 如若内容造成侵权/违法违规/事实不符，请点击【内容举报】进行投诉反馈！