基本平衡原理.docVIP

研究方法与步骤 首先介绍车身结构与叙述两轮车平衡直立时的运动情况，接着说明如何使两轮车平衡达到平衡的流程，且说明如何设定角度的微调与控制，最后则是说明如何运用陀螺仪与马达PID的整合输出，实现两轮车车体平衡。 两轮平衡小车图片 要让车体保持平衡，陀螺仪扮演重要角色。我们把车体分成三种情况：1，直立原理维持平衡，2车体前倾，3车体后仰。 车体原始状态 车体倾斜状态 从两张图中，我们设定起始状态车体是直立的，Q1是Ga与Gb直线的中心点，也是Gtotal的中心轴垂直坐落的重心，同时也是陀螺仪应该在的位置。 简单的说一下车体的平衡原理：如果受动某种外力干扰，车体开始倾斜，出现了角度误差 a 。则通过当前的角度误差乘上PID参数的P，对误差进行放大，再加上角速度乘上D的控制量，做为最后的输出来控制输出脉冲（PWM）的占空比来调整电机转速和转向，让车体向S1的方向运动来调整重新达到平衡状态。 平衡车 平衡控制流程图 下面讲一下陀螺仪和重力传感器的采集和标定方法。 很多资料中的转换方法都是经过简化的，刚入门的读都很难明白公式中每个数据的含义，这里我们使用原始公式，一一讲解其中每个数据的意义。 我为了调试程序方便，就使用了STM32F103和NI LabVIEW 组成了一套DAQ采集系统，使用它们直接采集重力传感器和角速度传感器输出的电压信号，把它们转换成对应的物理量进行计算，最终通过数据融合得到角度传感器板的倾角。 1 由单片机STM32F103的AD采集值转换到电压值方法： STM32F103是12位AD，满量程一般为3.3V。所以最小分辨率为3.3 / 4096 = 0.0008V，如果当前单片机的AD值为x.，则当前采样电压为 x / 4096 * 3.3 。 2 由采集到的AD电压值转换成重力和角速度的物理量： g = ( g -1.5 ) / 0.174 ; if (g 1) g = 1; if (g -1) g = -1; Angle = asin(g ) *180/3.141593 ; 上面是重力传感器的标定公式，它把重力传感器采样到的电压值转换成角度值。公式中常量1.5是重力传感器的中点电压，0.174是它的灵敏度，这个在传感器介绍中已经有说明，可以参看前面的传感器基本参数说明部分。在公式g = ( g -1.5 ) / 0.174 ; 计算后就由AD采集到的电压值转换到了重力（加速度）物理量g。再由Angle = asin(g ) *180/3.141593 ;一句把重力值转换为角度值，角度值的范围是从-90度到90度之间，0度时表示车体直立。但其中有个问题，被asin的数值不能大于1或是小于-1，否则会得到无效的数值。所以又加上了一句if (g 1) g = 1; if (g -1) g = -1;对重力值进行限制。 因为重力和加速度其实是一个东西，由重力传感器计算出的角度包含着振动和车体加速度，不能准确的体现出车体的角度值，所以须要使用数据融合算法，取重力传感器的角度积分和陀螺仪传感器的微分融合成的角度才可以准确地体现出角度传感器的角度变化。下面让我们看看角速度的计算公式： GyroOut =- ( Gyro -1.5 ) / 0.002 ; 角速度值等于陀螺仪的电压值减去中点电压1.5V，再除以陀螺仪的灵敏度2mv/度/秒， （即0.002V），便得到了角速度。从AD采样数据标定为角速度是一个线性公式，所以最终可以简化为GyroOut =- ((float)AD_GYRO_X - 1861) / 2.482;，这样还可以避免大量单精度运算带来的误差。 3使用互补滤波C_angle_dot=gyro_m_cf-bias_cf;，如果每次采样时间间隔为20ms,则500次的均值为500*0.02，所以这里减去的是大约10秒的均值偏置。最后一句C_angle=0.98*(C_angle+C_angle_dot*0.02)+0.02*angle_m_cf;则是用于数据融合，把0.98倍的重力传感器的积分的值与0.02的陀螺仪的微分值相加得到一个完整的角度值。一个完整的周期数为1/0.02 =50次，每次执行要0.02秒，所以要1秒钟的周期时间。角度计算的整体公式如下： bias_cf=0.998*bias_cf+0.002*gyro_m_cf; //陀螺仪零飘低通滤波；500次均值； C_angle_dot=gyro_m_cf-bias_cf; C_angle=0.98*(C_angle+C_angle_dot*0.02)+0.02*angle_m_cf; 这里gyro_