基于MSP430单片机的四旋翼 PID Control种群遗传算法 通过卡尔曼滤波实现自稳

基于MSP430单片机的四旋翼 仅供大家参考


PID Control种群遗传算法：

1. #include "io430x14x.h"
   
2. #include "stdlib.h"
   
3. #include "sys.h"
   
4. #include "Control.h"
   
5. 
   
6. 
   
7. #define col_MAX 50                //群体空间大小
   
8. #define var_p 65                //变异概率：65 对应的变异概率约等于0.001，655 为0.01   rand():0-65535
   
9. #define epoch_MAX 200        //进化代数
   
10. void inherit(void)                //遗传进化PID
    
11. {
    
12.         unsigned int colony[col_MAX]={
    
13.         62267,15148,39769,31849,58411,49944,29915,58375 ,53831
    
14.         ,29144,40332,51900,60411,48378,11552,26588,61306,60089
    
15.         ,26887,58565, 3794,23125,53291,  646, 9102,13288,13023
    
16.         ,39570,17838,13029, 1001,48941,29169,61066,30539,27436
    
17.         ,55457,34416,13280,44049,54926, 1287,44647,24869,54512
    
18.         ,32952,46495,28107,19963,12429
    
19.         };
    
20.         unsigned int health[col_MAX];        //对应colony[] ，每个个体的适应值，
    
21.         unsigned int dad,mum,baby1,baby2;
    
22.         unsigned int mini_health,mini_id;
    
23.         unsigned int temp_health;
    
24.         unsigned char i,epoch;        //epoch 遗传代数
    
25.         mini_health = found(&colony[0],&health[0]);        //评估初始个体，并作适应值缩放，找出最小适应值
    
26.         for(epoch=0; epoch < epoch_MAX; epoch++) //开始进化
    
27.         {
    
28.                 i = roulette(&health[0]);//轮盘赌转,返回数组下标
    
29.                 dad = colony[i];
    
30.                 i = roulette(&health[0]);//轮盘赌转,返回数组下标
    
31.                 mum = colony[i];
    
32.                 baby1 = dad&0xff;baby1 |= mum&0xff00;
    
33.                 baby2 = mum&0xff;baby2 |= dad&0xff00;
    
34.                 baby1 = variation(baby1);        //变异？
    
35.                 baby2 = variation(baby2);
    
36.                 temp_health = evaluating_unit(baby1);        //评估个体适应值
    
37.                 if(temp_health > mini_health)
    
38.                 {
    
39.                         mini_id = evaluating(&health[0]);        //取得最差适应值的个体id(数组下标)
    
40.                         colony[mini_id] = baby1;
    
41.                         health[mini_id] = temp_health - mini_health;
    
42.                 }
    
43.                 temp_health = evaluating_unit(baby2);        //评估个体适应值
    
44.                 if(temp_health > mini_health)
    
45.                 {
    
46.                         mini_id = evaluating(&health[0]);        //取得最差个体的适应值，及其id
    
47.                         colony[mini_id] = baby2;
    
48.                         health[mini_id] = temp_health - mini_health;
    
49.                 }
    
50.         }
    
51.         while(1); //结束进化
    
52. }
    
53. 
    
54. #define ev_N 25
    
55. #define aim_value 300
    
56. uint evaluating_unit(uint unit)//评估个体适应值  顺便PWM输出
    
57. {
    
58.         uint ret=0,temp=0,max=0;
    
59.         uchar i=0;
    
60.         int uk=0;                                //PID增量
    
61.         redressal(pid, unit);        //根据个体，对基因进行解码  修改PID三个参数
    
62.         while(i<ev_N)        //PID调整 ev_N.PID调整次数
    
63.         {
    
64.                         if(measured > desired)
    
65.                         {
    
66.                                 temp = measured - desired;
    
67.                                 if(temp > max)
    
68.                                         max=temp;        //最大超调量
    
69.                         }
    
70.                         else
    
71.                                 temp = desired - measured;
    
72.                        
    
73.                         ret+=temp;
    
74.                         uk = (int)(PID_Posi(pid, measured, desired));        //PWM输出
    
75.                         M1 = speed[0] - uk;        //y轴
    
76.                         M3 = speed[0] + uk;        //y轴
    
77. 
    
78.                         i++;
    
79.                 }
    
80.         }
    
81.         ret=65535/(ret/ev_N+max);
    
82.         return ret;        //返回适应值
    
83. }
    
84. 
    
85. void redressal(float* pid,uint colon)
    
86. {
    
87.         pid[0]=(float)((colon&0xFC00)>>10)*5.0/63;        //[0:0.079:5]
    
88.         pid[1]=(float)((colon&0x3E0)>>5)*0.1/31.0;        //[0:0.0032:0.1]
    
89.         pid[2]=(float)(colon&0x1F)/31.0;                        //[0:0.032:1]
    
90. }
    
91. 
    
92. uint found(uint colony,uint health)        //计算初始群体适应值，并找出最小值
    
93. {
    
94.         uchar i;
    
95.         uint mini=0xff;
    
96.         for(i=0;i<col_MAX;i++)
    
97.         {
    
98.                 *(health+i) = evaluating_unit(*(colony+i));        //评估个体适应值
    
99.                 if(*(health+i)<mini)
    
100.                         mini=*(health+i);
     
101.         }
     
102.         for(i=0;i<col_MAX;i++) //适应值缩放
     
103.                 *(health+i)-=mini;
     
104.         return mini;
     
105. }
     
106. 
     
107. uchar roulette(uint health)
     
108. {
     
109.         uchar i;
     
110.         uint temp=0;
     
111.         i=(uchar)(rand()%col_MAX); //0~(col_MAX-1) 随机选择起点
     
112.         while(1)
     
113.         {
     
114.                 if((col_MAX-1)==i) //实现首尾相接
     
115.                         i=0;
     
116.                
     
117.                 temp+=*(health+i); //累加适应值
     
118.                 if (temp>1200)
     
119.                         return i;
     
120.                 i++;
     
121.         }
     
122. }
     
123. 
     
124. uchar evaluating(uint health)        //取得最小适应值的个体id(数组下标)
     
125. {
     
126.         uchar i,id;
     
127.         uint mini=0xffff;
     
128.         for(i=0;i<col_MAX;i++)
     
129.         {
     
130.                 if(*(health+i)<mini)
     
131.                 {
     
132.                         mini=*(health+i);
     
133.                         id=i;
     
134.                 }
     
135.         }
     
136.         return id;
     
137. }
     
138. 
     
139. uint variation(uint baby)        //对基因进行变异
     
140. {
     
141.         uchar i;
     
142.         for(i=0;i<16;i++)
     
143.         {
     
144.                 if(rand()<var_p)        //rand()0-65535，变异概率：65 对应的变异概率约等于0.001，655 为0.01
     
145.                 {
     
146.                         if(0==(baby&(1<<i)))
     
147.                                 baby|=(1<<i);
     
148.                         else
     
149.                                 baby &= ~(1<<i);
     
150.                 }
     
151.         }
     
152.         return baby;
     
153. }
     

复制代码

单片机源程序如下:

1. #include "io430x14x.h"
   
2. #include "sys.h"
   
3. #include "24L01.h"
   
4. #include "MPU6050.h"
   
5. #include "HMC5883L.h"
   
6. #include "EEPROM.h"
   
7. #include "Control.h"
   
8. #include "math.h"
   
9. 
   
10. uchar RxBuf[32]={0};
    
11. uchar TxBuf[32]={0};
    
12. uchar EEPROMBuffer[SIZE]={0};
    
13. 
    
14. int speed[5] = {1300,0,0,0,0};//M0,M1-M4
    
15. int desire[4]={0,0,0,0};//HMC_YAW,ROLL,PITCH,YAW
    
16. float PID_Roll[6]={0}, PID_Pitch[6]={0}, PID_Yaw[6]={0};
    
17. float roll = 0, pitch = 0, yaw = 0;
    
18. int gyro[3]={0}, accel[3]={0}, magne[3]={0};
    
19. 
    
20. uchar RX = 0, TX = 0, CALC = 0, Fly = 0, EEPROMWR = 0;
    
21. uint ms2 = 0, ms10 = 0, ms500 = 0;
    
22. 
    
23. /********************************Main*********************************/
    
24. void main(void)
    
25. {
    
26.         uchar Flag_TX = 0;
    
27.         Init_Sys(); //系统功能初始化
    
28.         _EINT();    //开启中断功能
    
29.        
    
30.         HMC5883_Init();
    
31.         MPU6050_Init();
    
32.         NRF24L01_Init();
    
33.         while(AT24CXX_Check());//检测不到24c16
    
34.        
    
35.         AT24CXX_Read(20,EEPROMBuffer,SIZE);
    
36.         Init_datas(PID_Roll, PID_Pitch, PID_Yaw, desire, speed, EEPROMBuffer);
    
37.        
    
38. /********************************Start*********************************/
    
39.         while(1)
    
40.         {
    
41.                 if(RX)
    
42.                 {
    
43.                         NRF24L01_RX_Mode();
    
44.                         if(NRF24L01_RxPacket(RxBuf)==RX_OK)   //判断是否收到数据
    
45.                         {
    
46.                                 if(RxBuf[0]==0xAA)//有数据更新
    
47.                                 {
    
48.                                         Data_Update(PID_Roll, PID_Pitch, PID_Yaw, desire, speed, RxBuf);
    
49.                                         Fly = RxBuf[START_STOP];//Stop?
    
50.                                 }
    
51.                         }
    
52.                 }
    
53.                 if(TX)
    
54.                 {
    
55.                         TX = 0;
    
56.                         if(Flag_TX == 0)
    
57.                         {
    
58.                                 Flag_TX = 1;
    
59.                                 Data_Send_Status(roll, pitch, yaw, desire, speed, TxBuf);
    
60.                         }
    
61.                         else if(Flag_TX == 1)
    
62.                         {
    
63.                                 Flag_TX = 0;
    
64. //                                Data_Send_Senser(accel, gyro, magne, TxBuf);
    
65.                                 Data_Send_PID(PID_Roll, PID_Pitch, PID_Yaw, TxBuf);
    
66.                         }
    
67.                         NRF24L01_TxPacket(TxBuf);
    
68.                 }
    
69.                 if(CALC)
    
70.                 {
    
71.                         CALC = 0;
    
72.                         MPU6050_Read(accel, gyro);
    
73.                         AHRSupdate(accel, gyro, magne);
    
74.                         yaw = get_angle(magne, roll, pitch);
    
75.                         if(Fly)
    
76.                         {
    
77.                                 EEPROMWR = 1;
    
78.                                 MotoCtrl(PID_Roll, PID_Pitch, PID_Yaw, desire, speed, roll, pitch, yaw, gyro, 1);
    
79.                         }
    
80.                         else
    
81.                         {
    
82.                                 M1=900;M2=900;M3=900;M4=900;
    
83.                                 if(EEPROMWR)
    
84.                                 {
    
85.                                         EEPROMWR = 0;
    
86.                                         AT24CXX_BufferWrite(PID_Roll, PID_Pitch, PID_Yaw, desire, speed, EEPROMBuffer);
    
87.                                 }
    
88.                         }
    
89.                 }
    
90.         }
    
91. }
    
92. 
    
93. //***********************************************************************
    
94. //TIMERA中断服务程序
    
95. //定时时间：1ms
    
96. //***********************************************************************
    
97. #pragma vector = TIMERA1_VECTOR
    
98. __interrupt void Timer_A1(void)
    
99. {
    
100.         if(TAIV==2)//定时溢出标志位
     
101.         {
     
102.                 ms2++;
     
103.                 ms10++;
     
104. //                ms500++;
     
105.                
     
106.                 if(ms2 == 2)        {CALC = 1;                        ms2  = 0;}
     
107.                
     
108.                 if(ms10 == 4)        {TX = 1;                                         }
     
109.                 if(ms10 == 8)        {TX = 1;                                         }
     
110.                 if(ms10 == 12)        {TX = 1;                                         }
     
111.                 if(ms10 == 16)        {TX = 1;                                         }
     
112.                 if(ms10 == 20)        {RX = 1;                        ms10 = 0;}
     
113.                
     
114. //                if(ms500 == 5000)
     
115. //                {
     
116. //                        ms500 = 0;
     
117. //                }
     
118.                
     
119.         }
     
120. }
     

复制代码

所有资料51hei提供下载:
四旋翼飞行器，采用MSP430F149，通过卡尔曼滤波实现自稳。其中包括陀螺仪MPU6505、地.rar (180.89 KB, 下载次数: 75)

2018-10-25 13:22 上传

点击文件名下载附件

非常好