本帖最后由 okwh 于 2017-11-11 23:38 编辑
自平衡小车－5 结构、调试、平衡
话说到了最后阶段，我还是有些懵懂。 如图是我的小车的结构： 螺丝固定用一个废光盘作骨架平台，螺丝固定电池盒，没有三个18650电池并列，经升压模块升至10V,送电机驱动做电机电源; 用长虹的移动充电宝eleCloud做板子电源通过USB接头提供。如何固定呢？没办法，我只好用橡皮筋把其他的固定，MPU6050用橡皮筋固定在顶部中央。在连接轮子的底盘固定一长条硬纸板用于翻车时防撞坏。 总之，我这个 似乎算是橡皮筋捆绑结构。
试验结果： 先用MPU6050测定平衡点数据做为控制目标，PWM基于据他人经验把加速度、角速度在历史、当前、相互之间以各种权重合并为PID需要的两个变量，然后试验PID控制参数。PID参数按经验先调P参数，至观察到摆动，调整D参数至震动不大。或者说 这象一个阻尼过程，使得小车在偏离平衡位置时，通过轮子加速度产生一个力使其回复平衡位置。单车体、轮子全是动态，加速度、角速度等测定有各种干扰，调整好很有难度。据说这时机械专业的倒置单摆问题，是个变动的多力矩综合体系，要实现它需要精确的控制整体质量及其分布、重心位置及其变动范围、轮子加速及其产生的作用、失控条件等等。
视频：观察平衡 https://v.youku.com/v_show/id_XMzEzNTc1OTY1Ng==.html?spm=a2h3j.8428770.3416059.1
最终自平衡最长时间约20秒，且不总是能成功，且还没视频下来，所以只能算办成功。经常问题是，摆动调整很快放大到向其中一侧偏转过大，轮子加速也调整不过来；而在摆动中，轮子启动、旋转、加速减速转动的阻力动力都是不规则的变化的，这还不说地面的摩察力也不固定，最终导致极易失衡。
视频： 原地平衡代码 ＋ 一个轮子PWM多一个固定的值，实现了小车旋转平衡，犹如一个陀螺。这倒是不会倒了！ https://v.youku.com/v_show/id_XMzEzNTc1NTM4MA==.html?spm=a2h3j.8428770.3416059.1 就我的装置，个人感觉电源部分太重了，轮子的起停变速带来的失衡后果大于对平衡的贡献，结果体系成了正反馈失衡体系了，而不是负反馈平衡体系。
估计原因： 1) 这个电机起停变速过于迟滞，也许需要大功率的电机；2) 当然我无法试验更大电机， 但我的装置似乎太重了，极易倾倒，或者说通过电机加速可调的变动范围太小。那么可能减轻重量应该可以。 3) 如果降低重心到轮轴甚至其下，那就近乎不倒翁结构了，应该就容易多了。 改进意见：换电池、降重量、降重心。
代码： 这方面好在很多，并无新奇之处。 以下问题存在： DMP初始化失败。 单个加速度数据获取失败！， MPU6050设备ID读回的是0x72不是0x68。
计算MPU6050数据： 按模块安装方向测量 确定： 只需要 Acc_Y, gyro_X,其他变化不大忽略。 - void MP6050_DataTrans(void)
- {
- static float gyro_y_last,acc_x_last;//低通滤波用参数
- short intacc_x, intgyro_y , ay,az;
- // MP6050_GetData(MPU_ACCEL_YOUTH_REG,&intacc_x);//读取y轴加速度 只需要y
- MPU_Get_Accelerometer(&ay,&intacc_x,&az);
- acc_x = 0.7*acc_x_last+0.3*intacc_x; //对加速度进行低通再滤波
- acc_x_last=acc_x;
- acc_x=acc_x-acc_x_errer;//减去零点漂移
- angle_ax=acc_x/16384;//加速底转换成角度(弧度) g unit +-2g
- angle_ax=angle_ax*180/3.14159;//弧度转换成角度 ????
-
- MP6050_GetData(MPU_GYRO_XOUTH_REG,&intgyro_y);//读取x轴角速度 只需要X
- gyro_y = 0.7*gyro_y_last+0.3*intgyro_y; //对角速度进行低通滤波
- gyro_y_last=gyro_y;
- gyro_y=gyro_y-gyro_y_errer; //减去零点漂移
- gyro_y=gyro_y /16.384; //由陀螺仪值计算角速度 +- 2000 du/s
- angle_gy=angle_gy+gyro_y*0.004; //角速度转换成角度 4ms 测一次 ???? +gyro_y*0.004 每使用？？
- // 最后合并
- // angle=FILTER_NUMBER*(angle+angle_gy*0.004)+(1-FILTER_NUMBER)*angle_ax;//一阶互补滤波,4ms一次 最后合并
- angle=FILTER_NUMBER*(angle+gyro_y*0.004)+(1-FILTER_NUMBER)*angle_ax;//一阶互补滤波,4ms一次 最后合并
-
- // printf("%d, %d\r\n",intacc_x,intgyro_y);
- }
在中断中 获得MPU6050测量数据，PD计算PWM, 更新PWM: - void ctimer_match1_callback(uint32_t flags)
- {
- ms1++;
- if((ms1>= 3 )|(ms1== 1) ) // 2*4ms PID计算一次 6050 250Hz 4ms 一次采样
- {
- ms1=1;
- MP6050_DataTrans();
- // PWM = (int)(Kp*angle + Kd*gyro_y); //角速度和角度PD
- PWM = (int)(Kp*angle+ Kd*gyro_y); //角速度和角度PD *fabs(angle)/8
- // PWM = (int)(Kp*angle); //角速度和角度PD *fabs(angle)/8
- // printf("%f, %f, %f, %d\r\n", angle,angle_ax, angle_gy, PWM);
- // printf("%d, %d, %d, %d\r\n", angle, angle_gy,gyro_y, PWM);
- }
- // return;
- //中断一次 变速一次 变速量（加速度）可调 自动加减速 /3
- // printf("%d\r\n", PWM);
- if(PWM >0)
- sp = LOWSP + fabs(1.0*PWM/(ms1+1.5)); //10 step only + 左右轮不同
- else
- sp = LOWSP + fabs(1.0*PWM/(ms1+1)); //10 step only + +0.2
- if (sp>upsp) sp = upsp;//pwm上限
- if(PWM <0)
- {
- SCTIMER_StopTimer(SCT0, kSCTIMER_Counter_L); // 修改SCTIMER_UpdatePwmDutycycle，提出StopTimer StartTimer
- SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_6, sp/10 , event6); // 右轮 前进 +ad
- SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_7, 1, event7);
- SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_5, sp/10, event5); // 左轮 前进 +ad
- SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_4, 1, event4);
- SCTIMER_StartTimer(SCT0, kSCTIMER_Counter_L);
- GPIO->B[0][25] =0; GPIO->B[0][26] =1; //指示等，可看到在平衡点附近闪烁
- }
- else
- {
- SCTIMER_StopTimer(SCT0, kSCTIMER_Counter_L);
- SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_7, sp/10, event7);
- SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_6, 1, event6); // 右轮 后退 +16 -ad +ad
- SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_4, sp/10, event4);
- SCTIMER_UpdatePwmDutycycle(SCT0, kSCTIMER_Out_5, 1, event5); // 左轮 后退
- SCTIMER_StartTimer(SCT0, kSCTIMER_Counter_L);
- GPIO->B[0][25] =1; GPIO->B[0][26] =0;
- }
|
/3 
发表于 2017-11-5 14:23:52
沉默卡
照妖镜
楼主
