设为首页收藏本站
开启辅助访问 切换到宽版

 找回密码
 立即注册

QQ登录

只需一步，快速开始

热门： 51单片机 | 24小时必答区 | 单片机教程 | 单片机DIY制作 | STM32 | Cortex M3 | 模数电子 | 电子DIY制作 | 音响/功放 | 拆机乐园 | Arduino | 嵌入式OS | 程序设计

搜索
»论坛 › 嵌入式/单片机论坛 › MSP430单片机 › 基于MSP430单片机的四旋翼 PID Control种群遗传算法 通 ...
返回列表 发新帖
查看: 4240|回复: 1
打印 上一主题 下一主题
收起左侧

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

[复制链接]
跳转到指定楼层
楼主
ID:415178 发表于 2018-10-25 13:22 | 只看该作者 |只看大图 回帖奖励 |倒序浏览 |阅读模式
基于MSP430单片机的四旋翼 仅供大家参考


PID Control种群遗传算法：
  1. #include "io430x14x.h"
  2. #include "stdlib.h"
  3. #include "sys.h"
  4. #include "Control.h"


  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. }

  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轴

  78.                         i++;
  79.                 }
  80.         }
  81.         ret=65535/(ret/ev_N+max);
  82.         return ret;        //返回适应值
  83. }

  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. }

  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. }

  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. }

  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. }

  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"

  10. uchar RxBuf[32]={0};
  11. uchar TxBuf[32]={0};
  12. uchar EEPROMBuffer[SIZE]={0};

  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};

  20. uchar RX = 0, TX = 0, CALC = 0, Fly = 0, EEPROMWR = 0;
  21. uint ms2 = 0, ms10 = 0, ms500 = 0;

  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. }

  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, 下载次数: 77)


评分

参与人数 1黑币 +50 收起 理由
admin + 50 共享资料的黑币奖励！

查看全部评分

分享到:  QQ好友和群QQ好友和群 QQ空间QQ空间 腾讯微博腾讯微博 腾讯朋友腾讯朋友
收藏收藏4 分享淘帖 顶顶 踩踩
回复

使用道具 举报

沙发
ID:497177 发表于 2020-2-19 14:48 | 只看该作者
非常好
回复

使用道具 举报

返回列表 发新帖
高级模式
B Color Image Link Quote Code Smilies
您需要登录后才可以回帖 登录 | 立即注册

本版积分规则

手机版|小黑屋|51黑电子论坛 |51黑电子论坛6群 QQ 管理员QQ:125739409;技术交流QQ群281945664

Powered by 单片机教程网

快速回复 返回顶部 返回列表