智能车制作

标题: 互补滤波可直接用 [打印本页]

作者: 依恋lry 时间: 2012-2-24 23:56
标题: 互补滤波可直接用
#include<SH88F2051.h>
//#include<math.h>
//#include<hp.c>

//#define port_angle 0
//#define port_angle_dot 1
#define pwm_l PWMD
#define pwm_r TH0
//#define pwm_on PWMCON|=0X80;TR0=1
//#define pwmr_on P3|=0x10
//#define pwmr_off P3&=0xef
#define MA P4_0
#define MB P4_1
#define MC P4_2
#define MD P3_7
#define FW MA=1;MB=0;MC=1;MD=0
#define BW MA=0;MB=1;MC=0;MD=1

void INT_INI()
{
EX1=1;
EX0=1;    //开中断
IT0=1;    //边沿触发
IT1=1;
PX0L = 1;    // 设置 INT0为高级中断
PX1L = 1;
}

USART_INI()
{
SCON=0X50;

T2MOD=0X80;    //不对sysclk再分频，51那是直接用ocsclk计数故徐分频
T2CON=0X30;    //收发都以t2计数
RCAP2H=0XFF;
RCAP2L=0XE5; //计算得来，非查表
TR2=1;    //开T2
}

void PWM_INI()
{
PWMCON=0XB1;    //八分频，pwm引脚输出
PWMP=0Xff;       //计数上限
IPL1 |= 0x20;    // 设置 PPWML为高级中断
IEN1  |= 0x20;    // PWM 允许中断

/*
TMOD|=2;    //t0自重载方式
TCON1|=4;    //不分频
TH0=0Xf8;    //e0
TL0=0Xf8;
//ET0=1;    //T0中断允许
PT0L=1;    // 设置 T0定时器为高级中断 */
}

void USART_T( unsigned char dat )
{
SBUF=dat;
while(!TI);
TI=0;

}

ADC_INI()
{
ADT=0; //ad计算时间

ADCH=0XFF;    //选择管脚复用功能为ad
}

void PORT_INI()
{
P1M0 = 0x08;
  P1M1 = 0;
  P3M0 = 0;       // 设置 MA,MB,MC,MD,C1为推挽输出, C2为准双向
  P3M1 = 0xC0;
  P4M0 = 0;
  P4M1 = 0x07;
}
/*
//Kalman滤波，8MHz的处理时间约1.8ms；
//-------------------------------------------------------
static float angle, angle_dot; //外部需要引用的变量
//-------------------------------------------------------
static const float Q_angle=0.001, Q_gyro=0.003, R_angle=0.5, dt=0.3;
//注意：dt的取值为kalman滤波器采样时间;
static float Pk[2][2] = { {1, 0 }, {0, 1 }};

static float Pdot[4] ={0,0,0,0};

static const char C_0 = 1;

static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{
angle+=(gyro_m-q_bias) * dt;

Pdot[0]=Q_angle - Pk[0][1] - Pk[1][0];
Pdot[1]=- Pk[1][1];
Pdot[2]=- Pk[1][1];
Pdot[3]=Q_gyro;

Pk[0][0] += Pdot[0] * dt;
Pk[0][1] += Pdot[1] * dt;
Pk[1][0] += Pdot[2] * dt;
Pk[1][1] += Pdot[3] * dt;

angle_err = angle_m - angle;

PCt_0 = C_0 * Pk[0][0];
PCt_1 = C_0 * Pk[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = C_0 * Pk[0][1];

Pk[0][0] -= K_0 * t_0;
Pk[0][1] -= K_0 * t_1;
Pk[1][0] -= K_1 * t_0;
Pk[1][1] -= K_1 * t_1;

angle += K_0 * angle_err;
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;
///angle_dot = gyro_m;
}
*/


ADC(unsigned char port)
{
float x;

ADCON|=port*2; //选端口
ADCON|=0X80; //开是转化
x++;
x--;
ADCON|=0X01;
while(ADCON&1); //等待完成

x=ADDH*4;
x+=ADDL;

ADCON=0;

return x;
}

//-------------------------------------------------------
//互补滤波
//-------------------------------------------------------
static float angle,angle_dot; //外部需要引用的变量
//-------------------------------------------------------
static float bias_cf;
static const float dt=0.01;
//-------------------------------------------------------
void complement_filter(float angle_m_cf,float gyro_m_cf)
{
bias_cf*=0.0001; //陀螺仪零飘低通滤波；500次均值；0.998
bias_cf+=gyro_m_cf*0.009;    //0.002
angle_dot=gyro_m_cf-bias_cf;
angle=(angle+angle_dot*dt)*0.95+angle_m_cf*0.04;
//加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；0.90 0.05
}
static float acceler,gyro;
void AD_calculate(void)
{

gyro=0.011557*ADC(1)-3.035+105*3.14/180; //后面105*3.14/180是对角速度零点的修正

acceler=0.003052*ADC(0)-1.4375; //计算角度时，只用了加速度传感器的Z轴就够了

acceler=acceler*1.5+14*3.14/180;  //没有使用arcsin（），因为sin在0度附近变化时可以用sin值代替角度值，这样可以减轻单片机的计算负担。
//*1.5是对角度的适当放大，14*3.14/180是对角度零点的修正

complement_filter(acceler,gyro);

}

T1_INI()
{
TMOD|=0X10; //方式1,16位
//TCON1=0X00; 12分频
TH1=0Xf8;
TL1=0XFA;
TR1=1;
ET1=1;    //  允许T1中断
}

//-------------------------------------------------------
//PWM输出
//-------------------------------------------------------
/*#define MA P4_0
#define MB P4_1
#define MC P4_2
#define MD P3_7*/

unsigned int PWM_10bit; // 10位 PWM 输出缓冲区
void PWM_output (int PWM_LH)
{
if (PWM_LH<0)
{
FW;    //#define FW MA=1;MB=0;MC=1;MD=0

PWM_LH*=-1;
}
else
{
BW; //#define BW MA=0;MB=1;MC=0;MD=1
}

if (PWM_LH>252)
{
PWM_LH=252;
}

PWM_10bit=4*PWM_LH;  //因为PWM原为8为pwm计算得来，这里重新定义10位pwm输出，所以，乘以4
//10位pwm获得方法参考网友Cortex-M0的http://bbs.21ic.com/frame.php?fr ... .com/iclist-11.html
}

static int speed_real_RH;
static int speed_real_LH;

//-------------------------------------------------------
//计算PWM输出值
//-------------------------------------------------------
static float K_voltage,K_angle,K_angle_dot,K_position,K_position_dot; //放大比例参数；这里是在main（）函数中对它们初始化
static float K_angle_AD,K_angle_dot_AD,K_position_AD,K_position_dot_AD;//辅助放大比例参数
static float position,position_dot;
static float position_dot_filter;
static float PWM;

//-------------------------------------------------------
void PWM_calculate(void)
{

K_angle_AD=ADC(2)*0.009;
K_angle_dot_AD=ADC(3)*0.009;
K_position_AD=ADC(4)*0.009;
K_position_dot_AD=ADC(5)*0.009;

position_dot=(speed_real_LH+speed_real_RH)*0.5;//position_dot是电机转速

position_dot_filter*=0.85; //车轮速度滤波
position_dot_filter+=position_dot*0.15;

position+=position_dot_filter;//position电机转过的角度，由position_dot累加而来

if (position<-768) //防止位置误差过大导致的不稳定
{
position=-768;
}
else if  (position>768)
{
position=768;
}

PWM = K_angle*angle *K_angle_AD + K_angle_dot*angle_dot *K_angle_dot_AD
   +K_position*position *K_position_AD + K_position_dot*position_dot_filter *K_position_dot_AD;//PWM是由四个参数累加而来，如果只要求平衡，可以不要position

PWM_output (PWM);
}



void main()    /////////////////////////////////////////////////////////////////
{
int t=0;
unsigned char i=0,j;

CLKCON=0x2c;  //内部16.6M，两分频后8.3Mhz

USART_INI();    //初始化串口
ADC_INI();    //初始化ad
T1_INI();    //初始化T1
PWM_INI(); //PWM初始化
INT_INI(); //外部中断初始化
PORT_INI(); //端口初始化
EA=1;    //开中断

K_position= 0.8* 0.558;
K_angle=32* 25.6; //正常来讲这些放大比例参数是要按一定的方法算出来的，这里的平衡是调出来的就无所谓算不算了
K_position_dot=1.09* 55.8; //调平衡时如果想不考虑哪个参数的影响，设为0即可
K_angle_dot= 2*25.6;


for (i=1;i<=500;i++) //延时启动PWM，等待卡尔曼滤波器稳定
{
for (j=1;j<=300;j++);

}

while(1)
{

}
}

void INT_L(void) interrupt 0  using 1
{
if(MA==1)
{
  speed_real_LH++;
}
else
speed_real_LH--;
}

void INT_R(void) interrupt 2  using 1
{
if(MA==1)
{
  speed_real_RH++;
}
else
speed_real_RH--;
}


void PWM_10(void) interrupt 12    using 1
{ static unsigned int PWM_10bit_bak;    // 10位 PWM 输出缓冲发送区
  static unsigned char PWM_Counter=0;    // 10位 PWM 输出发送个数计数器
  if(PWM_Counter == 0)
{ PWM_10bit_bak = PWM_10bit & 0x3ff;    // 10位 PWM 发送开始，装入10位 PWM 初值至输出缓冲发送区

}
  if(PWM_10bit_bak >= 26)
{ PWM_10bit_bak -= 26;    // 10位 PWM输出缓冲发送区发送数据>=26, 数据减26暂存
   PWMD = 0xff;    // 10位 PWM 发送一次数据0xff
}
  else if(PWM_10bit_bak == 0)    // 10位 PWM输出缓冲发送区发送数据=0
{ PWMD = 0x00;    // 10位 PWM 发送一次数据0x00
}
  else
{ PWM_10bit_bak = 0;    // 10位 PWM输出缓冲发送区发送数据<26, 数据乘10，发送之
   PWMD = (unsigned char)(PWM_10bit_bak * 10);
}
  if(++PWM_Counter > 40)
{ PWM_Counter = 0;    // 10位 PWM 输出发送个数计数值大于40, 清0
}
  PWMCON &= 0XFD;                // 清除 PWM 周期计数器溢出标志 PWMIF

}

unsigned char i=0;
void T1_isr(void) interrupt 3
{

TH1=0XE4;
TL1=0XFA;

AD_calculate();
PWM_calculate();

i++;
if(i==6)
{
i=0;

USART_T(angle*57.3);
USART_T(angle_dot*57.3);
      USART_T(255);
}
speed_real_RH =0;
speed_real_LH    =0;
}

作者: liu1guo2qiang3 时间: 2012-2-25 00:10
先谢过了，LZ，你又试过么？车子立起来了么？求指导呀、弄了好久了。没什么思路、明天把你的程序试试。

作者: 依恋lry 时间: 2012-2-25 00:13
跑起来了啊稳定后上视频

作者: 断翅at雄鹰 时间: 2012-2-26 09:22
牛

作者: 慕名凤姐而来 时间: 2012-2-26 13:38
请问一下在滤波后调节直立的PID时候，P和D的参数是怎样的啊？摇晃劲儿不小但是一放就趴下用手扶着就摇晃咋整？

作者: 我在线上123 时间: 2012-2-27 17:01

作者: ┼梦中身┮ 时间: 2012-2-27 20:27

作者: 断翅at雄鹰 时间: 2012-2-27 20:38
那么多看着晕呢！！！

作者: 神器 时间: 2012-2-27 21:05
mark一下

作者: ytcx100 时间: 2012-2-28 11:03
好东西。。不过看起来好晕啊~

作者: bingo705 时间: 2012-2-28 20:35

谢谢楼主！！

作者: 441044696 时间: 2012-2-28 23:22
留个记号。。。

作者: wuchao1235 时间: 2012-2-29 22:03

依恋lry 发表于 2012-2-25 00:13
跑起来了啊稳定后上视频

你是用楼主的程序跑起来的？

作者: 涛涛 时间: 2012-3-1 09:19
谢谢啊！！
羡慕嫉妒恨啊！！

作者: linping9656 时间: 2012-3-6 22:44
看不下去啊

作者: 云端暮雪 时间: 2012-3-7 16:04
请教几个问题。
void complement_filter(float angle_m_cf,float gyro_m_cf)
{
      bias_cf*=0.0001;                      //陀螺仪零飘低通滤波；500次均值；0.998
      bias_cf+=gyro_m_cf*0.009;                //0.002
      angle_dot=gyro_m_cf-bias_cf;
      angle=(angle+angle_dot*dt)*0.95+angle_m_cf*0.04;
      //加速度低通滤波；20次均值；按100次每秒计算，低通5Hz；0.90 0.05
}
这个函数中，互补滤波式子的系数，0.95+0.04=0.99，并不等于1，这个好像不合理吧？！
至于那个0.0001和0.009的和是不是一定要等于1，这个也想请教下楼主？

作者: 云端暮雪 时间: 2012-3-7 16:08
void AD_calculate(void)
{

gyro=0.011557*ADC(1)-3.035+105*3.14/180;       //后面105*3.14/180是对角速度零点的修正

acceler=0.003052*ADC(0)-1.4375;             //计算角度时，只用了加速度传感器的Z轴就够了

acceler=acceler*1.5+14*3.14/180;  //没有使用arcsin（），因为sin在0度附近变化时可以用sin值代替角度值，这样可以减轻单片机的计算负担。
                                                                     //*1.5是对角度的适当放大，14*3.14/180是对角度零点的修正

complement_filter(acceler,gyro);

}
这个函数里面，楼主用的单位应该是弧度吧？！
那些转换系数，是通过计算得到的还是调试出来的？
如果是计算出来的，是否提供一下计算过程？
如果是调试出来的，那又有什么好的方法啊？

作者: 云端暮雪 时间: 2012-3-7 16:10
还有，用这个算法在车子震得比较厉害的时候，滤波出的数值好像一直都是0了

作者: 诺飘冰 时间: 2012-3-7 20:25
请问LZ，#include<SH88F2051.h>与
#include <MC9S12XS128.h>对互补滤波设置能通用的不？

作者: zhang12345 时间: 2012-3-9 19:42

作者: 赛车小子 时间: 2012-3-10 14:44
谢谢，

作者: 木飞右 时间: 2012-3-13 19:35
为什么不回答别人的问题呢？

作者: 依恋lry 时间: 2012-3-15 18:18

wuchao1235 发表于 2012-2-29 22:03
你是用楼主的程序跑起来的？

这个程序是一个源程序，我是在这基础上弄出来的，当中有许多改动的地方，需要自己慢慢调试

作者: 依恋lry 时间: 2012-3-15 18:20

木飞右发表于 2012-3-13 19:35
为什么不回答别人的问题呢？

最近在弄32位的，如果有兴趣可以交流进群，或是人人主页

作者: 依恋lry 时间: 2012-3-15 18:22

云端暮雪发表于 2012-3-7 16:04
请教几个问题。
void complement_filter(float angle_m_cf,float gyro_m_cf)
{

这个具体的要看你设置的参数，参数不同出来的效果就会有很大的区别

作者: quanwenbin 时间: 2012-3-30 13:07

作者: 莫忆 时间: 2012-4-7 16:45
楼主你的卡尔曼滤波用的参数就是你里面写的？还是有经过自己调的？若果是自己调的那可不可以交流一下调试的方法？静止的能稳定站立多长时间？非常感谢喽。。。。。。。。。。

作者: 依恋lry 时间: 2012-4-7 23:11

莫忆发表于 2012-4-7 16:45
楼主你的卡尔曼滤波用的参数就是你里面写的？还是有经过自己调的？若果是自己调的那可不可以交流一下调试 ...

这只是一个参考具体的是自己调出来的需要的话可以给你看一下站立的情况给我邮箱给你发过去

作者: 莫忆 时间: 2012-4-8 16:08
810440683@qq.com非常感谢喽。。。。。。我现在也有点明白参数的调节了不过有一点不明白的是在陀螺仪的加速度转化最终是化成rad/s还是度每秒在进行PD调节的我看有的是用rad/s 有的是度每秒算的我现在车还没站起来所以我不知道不同的算法会对电机有多大的影响楼主有什么看法？

作者: 依恋lry 时间: 2012-4-8 22:25

莫忆发表于 2012-4-8 16:08
非常感谢喽。。。。。。我现在也有点明白参数的调节了不过有一点不明白的是在陀螺仪的加速度转化最终是化 ...

用rad/s 控制电机正反转值越大转速越大

作者: 莫忆 时间: 2012-4-8 22:48
非常感谢楼主了。。。。。。。。

作者: 莫忆 时间: 2012-4-8 22:50
对了楼主我们一直用的都是32位机有什么问题我们可以一起讨论

作者: linping9656 时间: 2012-4-9 23:25
这个好复杂。不过清华方案还是挺好用的

作者: tongyonghua123 时间: 2012-4-15 10:33
好东西

作者: 痞子 时间: 2012-4-16 13:32
这么多

作者: Tonywuke 时间: 2012-11-21 16:18
求教，互补滤波和融合滤波是一回事吗？

作者: 离。。。。 时间: 2012-12-18 20:58
肿么看着像卡尔曼

作者: AOE 时间: 2012-12-29 02:15

{:soso_e134:}

作者: lsgaoyu 时间: 2013-1-27 21:35

作者: xin99 时间: 2013-3-23 13:52

作者: wendaoxyp 时间: 2013-3-29 19:48
嘿嘿找到了

作者: verm 时间: 2013-4-7 12:20

作者: lxnick 时间: 2013-4-13 11:51
这段程序功能是什么啊？请楼主指导一下

作者: tangyinyin123 时间: 2013-4-24 20:02

作者: 江城子 时间: 2013-4-27 09:25
bias_cf*=0.0001; //陀螺仪零飘低通滤波；500次均值；0.998
bias_cf+=gyro_m_cf*0.009;

求解能不能具体解释一下你后面的参数（0.0001、0.009）代表什么意思，怎么得到的

作者: 残忆视觉 时间: 2013-11-10 16:27
楼主，好人啊，可算找到好东西了，学习

作者: Ηellｏ→Luck 时间: 2014-10-19 21:32
大神。。互补滤波那能注释清楚些不。。。

表示没看懂

作者: luhan2014 时间: 2014-10-20 22:04

作者: anyangyuxin 时间: 2015-1-17 09:09
:lo，我也学习一下

作者: 小小贤 时间: 2015-4-6 21:35
留个记号

作者: cbbandbbc 时间: 2017-2-26 16:07
那要是电磁信号的话，程序该怎么改呢？

欢迎光临智能车制作 (http://dns.znczz.com/)