-
标准实用文案
智能车内部资料—直立控制篇
注：不
要将思维局限于以下的资料，任何方案都要进行辨证，以下资料仅限于参考。
关于智能车的直立控制，最早源于第七届电磁车，在看本篇之前，请参看《电磁组直立行车参考设
计方案
2.0
》和《电磁组直立车模调试指南》
，有不懂之处，需要多看几次技术文档，并且利用好网络的资源，
百度、智
能车论坛、
CSDN
（需要淘宝购买积分）
、
PUDN
这几个网站是制作智能车的
好助手，可以搜集的几乎所有做车
过程中遇到的问题及资料，值得一提的是，要时刻与智
能车论坛保持联系，不懂的问题可以在上面发问，只要不
是太幼稚的问题，都会得到很多
人的解答，这样可以认识很多人，要和论坛的一些大神保持联系，定期询问进度
（加
QQ
，要电话号码…）
,
< br>在做车过程中，发现东北赛区的同学比较热情，乐于分享。好了，废话不多说了。
一：
AD
采集
关于直立控制，需要用到陀螺仪和加速度计，通过与网友的交
流，发现使用模拟模块比数字模块的要多，很
多网友反映
IIC
通信占用时间比较长，而直立控制对时间要求比较苛刻，因此在本届比赛中我们使用的是
模拟模
块（学弟学妹们可以尝试比较一下优缺点，毕竟那个
en
c-03
的陀螺仪确实很渣）
，事实上
AD
读取花费的时间
也
不
少
，
AD_ACC_Z=ad_ave(ADC1,
AD13,ADC_12bit,6);//
加
速
度
AD_GYRO
=ad_ave(ADC1,AD15,ADC_12bit,4);
//
陀螺仪
以分别读取
6
次和
4
次来说，
大约
44us
，
若分别
读取
10
和
10
次，
大约
80us
，
若分别读取
3
和
3
次，
大约
28us
（关于
时间测量，在后面的文档中将会提及）
。
二：滤波融合
AD
读取完成后，需要对两者（即加速度计和陀螺仪）的信号进行滤波、融合，关于
滤波方法大致分为清华
方案、互补滤波、卡尔曼滤波，以上
3<
/p>
种方案共
4
种方法经过筛选，最终选择了
清华方案，以下将介绍这几种
方法。
1
、
清华方
案，具体原理看《电磁组直立行车参考设计方案
2.0
》
。
void
AngleCalculate(void)
文档
标准实用文案
{
g_fGravityAngle=((AD_ACC_Z-
GRAVITY_OFFSET)*GRAVITY_ANGLE_RATIO);
g_fGyroscopeAngleSpeed = (AD_GYRO -
GYROSCOPE_OFFSET) *GYROSCOPE_ANGLE_RATIO;
g_fCarAngle =
fGyroscopeAngleIntegral;
fDeltaValue = (g_fGravityAngle -
g_fCarAngle)/GRAVITY_ADJUST_TIME_CONSTANT;
fGyroscopeAngleIntegral+=(g
_fGyroscopeAngleSpeed+fDeltaValue)
/GYROSCOPE_ANGLE_SIGMA_FREQUENCY;
}
其中，
g_fGravityAn
gle
为加速度计倾角；
AD_ACC_Z
为加速计的
Z
轴的
AD
值；
GRAVITY_OFFSET
为加速度
计
Z
轴的偏移量，为定值，通过改变该值的
大小调整直立时的平衡位置；
GRAVITY_ANGLE_RATIO
为加速度计
Z
轴归一化比例系数，
具体测量方法是将加速计模块平放于桌面上，读取
AD
值，
记为
A
值，然后将模块反过来
再读取一次
AD
值，记为
B
值，然后用
180/(B-A)
,
结果就是
GRAVITY_ANGLE_RATIO
（为正值）
，由于测量时
会存在误差，后期可以稍
作调整，也可不用调整；
AD_GYRO
为陀螺仪的
AD
值；
GYROSCOPE_OFFSET
为陀
螺
仪
的
零
偏
，
由
于
陀
螺
仪
存
在
温
飘
，
因
此
这
个
值
每
次
开<
/p>
机
需
要
自
动
采
集
零
偏
，
GYROSCOPE_OFFSET=ad_
ave(ADC1,AD15,ADC_12bit,200)
；
GYROSCOPE_ANGLE_RATIO
为陀螺仪归一化
系数，需要
根据滤波图像进行整定
；
GRAVITY_ADJUST_TIME_CONSTANT
为加速度计时间补偿系数
，一般在
1~4
之间，增大可使融合后的曲线减小加速度计的比
重；
GYROSCOPE_ANGLE_SIGMA_FREQUENCY
为陀螺
仪积分时间，数值为
1000/
（控制周期，一般为
5ms
）
，因此该值为
1000/5=
200
；其余补充内容可以参看程序
以及清华方案技术文档。
2
、
卡尔曼
滤波（矩阵）
，跟随性强，车子调的比较硬，但不懂原理难以整定参数，而且运行比较占
用时间，以
下几个参数都需要整定。
float Q_angle=0.001;
//
增大跟随好，噪点多
文档
标准实用文案
float
Q_gyro=0.003;//
增大噪点多，跟随不及时
float R_angle=0.5;
//
增大可去除噪点，但会跟随不及时
float dt=0.005;
//
增大消除相位差
float P[2][2] = {{ 1, 0 },{ 0, 1 }};
float Pdot[4] ={0,0,0,0};
float C_0 = 1;
float q_bias,
angle_err,PCt_0,PCt_1,E,t_0,t_1;
float
K_0;//
含有卡尔曼增益的另外一个函数，用于计算最优估计值
float K_1;//
含有卡尔曼增益的函数，用于
计算最优估计值的偏差
float
angle_m,gyro_m;
void Kalman_Filter()
{
g_fGravityAngle
= ((AD_ACC_Z
-GRAVITY_OFFSET)*GRAVITY_ANGLE_RATIO);
//
g_fGyroscopeAngleSpeed
=
(AD_GYRO
+GYROSCOPE_OFFSET)
*GYROSCOPE_kalman_RATIO6050;
g_fGyroscopeAngleSpeed =(AD_GYRO
-GYROSCOPE_OFFSET)* GYROSCOPE_kalman_RATIO;
angle_m=g_fGravityAngle;
gyro_m=g_fGyroscopeAngleSpeed;
//
角度更新
angle+=(gyro_m-q_bias) *
dt;//
先验估计
//
派生协方差矩阵计算
Pdot[0]=Q_angle - P[0][1] - P[1][0];//
Pk-' 0,0
先验估计误差协方差的微分
文档
标准实用文案
Pdot[1]=- P[1][1];//0,1
Pdot[2]=- P[1][1];//1,0
Pdo
t[3]=Q_gyro;//1,1Pdot=A*P+P*A
导
+Q
//
协方差矩阵更新
P[0][0] += Pdot[0] * dt;// Pk-
先验估计误差协方差微分的积分
=
先验估计误差协方差
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;
//
计算测量角度和估计角度的偏差
angle_err = angle_m -
angle;//zk-
先验估计
PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];
//
误差估计计算
E = R_angle + C_0 *
PCt_0;//
估计偏差
//<
/p>
卡尔曼增益，
E
越大，
< br>K
越小
K_0
= PCt_0 / E;//Kk
K_1 = PCt_1 /
E;
//
后验估计，更新协方差阵
t_0 = PCt_0;
t_1 = C_0 *
P[0][1];
文档
标准实用文案
P[0][0] -= K_0 *
t_0;//
后验估计误差协方差
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;//P
（
K|K
）
E
越大，
P
越大
//
更新状态估计
angle += K_0 *
angle_err;//
后验估计
q_bias+= K_1 *
angle_err;//
后验估计
angle_dot = gyro_m-
q_bias;//
输出值（后验估计）的微分
=
角速度
g_fCarAngle=angle;
g_fGyroscopeAngleSpeed= angle_dot;
}
3
、
<
/p>
卡尔曼滤波（非矩阵）
，此种方案很少使用。
float Kg = 0,gyroscope_rate =
0,accelerometer_angle=0;
void
kalman_update(void)
{
float Q =1,R = 300;
static float
RealData = 0,RealData_P = 0;
float NowData = 0,NowData_P
= 0;
float Acc_x = 0,Gyro=0,Acc_z=0;
//
需要修改测试下面的三个值
1400 1360 1390-----
Acc_x =
Acc_X_7260-2000;//x
< br>轴水平的输出值
此时车状态是水平放在桌子上
Acc_z =
AAngleAccele[3]
- 2057.0;//
加速度传感器的
z
轴
此时车状态是竖直放在桌子上
文档
标准实用文案
Gyro
=
(AAngleAccele[2]- GYROSCOPE_OFFSET);
//
陀螺仪直立的输出中立值
此时车状态是竖直放
在桌子上
//--
如果是硬件积分出角度的板子，那么就可以这样用完全可以
acc_x
< br>这个引脚
//
也就是
ad1
，把他接到传感器的
J
引脚，直
接当做角度使用。中间完全跳过卡尔曼滤波
accelerometer_angle =
atan2f(-Acc_x,Acc_z);
gyroscope_rate
=
Gyro*0.00028;//0.0023
0.0070
//
参考电压
3.3v
12
位
ADC
放大
9.1
倍
enc-03
0.67mv/deg./sec.
//RealData
：
上
次
得
到
的
角
度
+
陀
螺
仪
得
到
的
角
速
度<
/p>
*
陀
螺
仪
积
分
时
间
//(3300/4096)/(0.67*9.1)*(3.14/180) =
0.0023
NowData = RealData +
gyroscope_rate*0.0001;
//0.0001//1.
预估计
X(k|k-1) = A(k,k-1)*X(k-1|k-1)
+ B(k)*u(k)
NowData_P
=
sqrt(Q*Q+RealData_P*RealData_P);
//2.
计
算
预
估
计
协<
/p>
方
差
矩
阵
P(k|k-1)
=
A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
Kg = sqrt(NowData_P*NowData_P/(NowData_
P*NowData_P+R*R));
//3.
计算卡尔曼增益矩阵
K(k) =
P(k|k-1)*H(k)' /
(H(k)*P(k|k-1)*H(k)' + R(k))
RealData
=
NowData
+
Kg*(accelerometer_angle
-
NowData);
//4.
更
新
估
计
X(k|k)
=
X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
RealData_P
=
sqrt((1-Kg)*NowData_P*NowData_P);
//5.
计
算
更
新
后
估
计
协
防
差
矩
< br>阵
P(k|k)
=
（
I-K(k)*H(k)
）
< br>*P(k|k-1)
QingJiao = RealData - 0.04; //1.57
是
90
度的值
0.04
}
文档
标准实用文案
4
、
互补滤波，效果不太好用。
void
Datehandle()
{
float Q =0.99,R
= 0.01;
//
加速度计陀螺仪权重
float tau=0.005,dtc=0.005;
Q=tau/(tau+dtc);//0.9375
//
定义
前进为有单片机的方向
往前倾数减小
Acc=AAngleAccele[3]-GRAVITY_OFFSET
//-90z
轴
1
为垂直修正量
纠正垂直时的不确定
Gyr=(-xout+GYROSCOPE_OFFSET) *0.0625
//
陀螺仪的值
Acc_jiao=Acc*0.123;
//z
轴转过的角度
0.134
b-a=c
180/c=0.167
Gyr_jiao=Gyr*0.18;
//
陀螺仪的角度
0.052
zenm tiaode
real_angle = Q*(real_angle
+ Gyr_jiao*dtc) + (1-Q)*(Acc_jiao);
// 0.01
采样周期
0.00956
GyrAccCra=real_angle;
}
滤波
图像，
黄色
为融合后的曲线，
蓝色
为陀螺仪的曲线，
红色
为加速度计的曲线，
上位机为
虚拟示波器
，需
要配置协议，
具体协议参看程序。下图为官方的滤波方案，也就是本套程序所采用的方案。
文档
标准实用文案
三：输出调用
s32
AngleControl(void)
{
fValue = g_fCarAngle* ANGLE_CONTROL_P
+g_fGyroscopeAngleSpeed * ANGLE_CONTROL_D;
//
角
度控制系数
if(fValue >
ANGLE_CONTROL_OUT_MAX)
fValue = ANGLE_CONTROL_OUT_MAX;//4000
else if(fValue < ANGLE_CONTROL_OUT_MIN)
fValue = ANGLE_CONTROL_OUT_MIN;
g_fAngleControlOut = fValue;
return g_fAngleControlOut
}
#define AANGPERIODFAV
(5)
//
平滑输出
s32 AAangPWMOut(s32 NewAangPWM ,s32
LastAangPWM,s32 PeriodCount)
{
s32
AangPWMfav,AangOUT
文档
标准实用文案
AangPWMfav = NewAangPWM - LastAangPWM
AangOUT = AangPWMfav
*(PeriodCount+1)/AANGPERIODFAV + LastAangPWM
return AangOUT
}
参数整定：
先增大
ANGLE_CONTROL_P
，不要一点一点的加，看不出效果，等到小车
有立起来的效果时，再逐
渐增大，慢慢的会来回摇晃，这时整定
ANGLE_CONTROL_D
，增大
D
可以使小车平稳很多，过大会造成小车
震动，就是打齿声比较严重，通过几次来回整
定
P
和
D
，得
到一组参数，可以使小车立的比较硬，如果小车
总
是朝某一方向
跑，需要整定
GRAVITY_OFFSET
加速度计的零偏<
/p>
，直到小车在平衡位置来回摆动。
注意
：
若摆动车子，虽有正反转，但发现轮子方向旋转与实际相反，需要
将电机线对调
一下。若整定好加速度计
的零偏后，重新上电
仍然不确定性的朝某个方向跑，要检查加速度计的安装位置，要确保小车处于平衡状态下，
传感器要竖直安装
！
四：直立控制时序
设定一个
1ms
的定时器，每一
ms
执行一次
run
函数。
void run()
{
TimeCount++
SpeedPeriodCoun
t++;//
计算电机
PWM
TurnPeriodCount ++
AAngPeriodCount ++
MotorTurnPWM
= TurnPWMOut(TurnPWMOUT,Las
tTurnPWMOUT,TurnPeriodCount)
MotorSpeedPWM
SpeedPWMOut(g_SpeedControlOutNew
,g_SpeedControlOutOld,SpeedPeriodCount);
=
文档