聚豐項目 > 基于RT-THREAD自穩三棱柱
本項目是基于沁恒的CH32V103R8T6開發板進行開發,在RT-thread操作系統下通過對JY61陀螺儀進行數據采樣經過PID控制算法實現裝置自穩。其中JY61內置微處理器,結合動態卡爾曼濾波和姿態解算獲取高精度高穩定性姿態數據,滿足控制需求;同時,無刷電機的高轉速為其動量提供了足夠的保證,調試時逐飛科技的無線串口可以負責連接PC和下位機。
Dawnwang
Dawnwang
團隊成員
王宇 學生
趙洋 學生
巨太平 學生
宋卓 學生
電源穩壓降壓用LMT317 作為LDO芯片,采用直流無刷電機實現高轉速保證動量輪高動量,內部集成TB6612驅動芯片和SY8120IABC穩壓芯片保證動量輪處于正常控制狀態。狀態反饋傳感器采用JY61陀螺儀。主控芯片采用CH32V103R8T6。
/*說明:片內外設使用了定時器和串口,定時器用來正交解碼讀取編碼器的值,也用于pwm輸出。串口1用來調試和調用finSH組件,串口2接收陀螺儀數據 內核使用了線程和信號量以及中斷*/ //發送到VOFA+ void sendVofa(float data_1,float data_2) { unsigned char tail[4]={0x00, 0x00, 0x80, 0x7f}; float fdata[2]; fdata[0]=data_1; fdata[1]=data_2; uart_putbuff(UART_1,(char*)fdata,sizeof(float)*2); uart_putbuff(UART_1,(char*)tail,4); } //數據濾波 #define dt 0.012 //這個單位是s,是采樣周期 /********************************************** * 卡爾曼濾波 ***********************************************/ float KalmanFilter_Elect(float curr_elect_val,float last_elect_val) { static float Q_curr = 1.0;//0.1 //Q增大,動態響應增大,過程噪聲的協方差 static float Q_last = 0.0001; //過程噪聲的協方差,過程噪聲的協方差為一個一行兩列矩陣 static float R_elect = 10.0; //測量噪聲的協方差 即測量偏差 static float Pk[2][2] = { {1, 0}, {0, 1 }}; static float Pdot[4] = {0,0,0,0}; static float q_bias = 0.0; static float elect_err = 0.0; static float PCt_0 = 0.0; static float PCt_1 = 0.0; static float E = 0.0; static float K_0 = 0.0, K_1 = 0.0, t_0 = 0.0, t_1 = 0.0; Pdot[0] = Q_curr - Pk[0][1] - Pk[1][0]; //Pk-先驗估計誤差協方差的微分 Pdot[1] = -Pk[1][1]; Pdot[2] = -Pk[1][1]; Pdot[3] = Q_last; Pk[0][0] += Pdot[0] * dt; //Pk-先驗估計誤差的協方差微分的積分 Pk[0][1] += Pdot[1] * dt; //先驗估計誤差協方差 Pk[1][0] += Pdot[2] * dt; Pk[1][1] += Pdot[3] * dt; elect_err = curr_elect_val - last_elect_val; //偏差 = 測量值 - 預測值,先驗估計 PCt_0 = Pk[0][0]; PCt_1 = Pk[1][0]; E = R_elect + PCt_0; K_0 = PCt_0 / E; K_1 = PCt_1 / E; t_0 = PCt_0; t_1 = 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; curr_elect_val += K_0 * elect_err; //后驗估計 更新最優電磁值 最優電磁值 = 預測值 + 卡爾曼增益*(測量值-預測值) q_bias += K_1 * elect_err; //后驗估計 更新誤差 return curr_elect_val; } //控制算法部分 static void calculatepwm(float actualang,float expectang,int32_t actualsped,int32_t expectsped)//控制算法運行 { //直立環經典pid+fuzzy:pd errbala.err=expectang-actualang;//當前誤差 errbala.derr=gyro;//當次誤差與上一次誤差之間的差值 errbala.err=KalmanFilter_Elect(errbala.err,errbala.lasterr);//卡爾曼濾波 errbala.derr=KalmanFilter_Elect(errbala.derr,errbala.lastderr);//卡爾曼濾波 errbala.lasterr=errbala.err; errbala.lastderr=errbala.derr; errbala.out=kp*errbala.err+kd*errbala.derr;//算出結果 //速度環pi errsped.err=expectsped-actualsped;//當前誤差 errsped.sum+=errsped.err;//誤差積分 if(errsped.sum>1000)//積分限幅 errsped.sum=1000; else if(errsped.sum<-300) errsped.sum=-300; errsped.out=kp1*errsped.err+ki1*errsped.sum;//算出結果 pwmout=errbala.out+errsped.out;// if(pwmout>=9999)pwmout=9999; else if(pwmout<-9999)pwmout=-9999; //這里做最后的輸出控制 if(pwmout>0) { pwm_duty(PWM2_CH2_A1, pwmout); pwm_duty(PWM3_CH1_A6,9000); } else { pwm_duty(PWM2_CH2_A1, -pwmout); pwm_duty(PWM3_CH1_A6,0); } //rt_kprintf("pwmout:%d\r\n",(int32_t)pwmout); } //編碼器值獲取及濾波 encoder =(int32_t)(0.7* timer_quad_get(TIMER_4)+0.3*lastenc);//一階低通濾波去毛刺 //陀螺儀數據獲取 if (ucRxBuffer[0]!=0x55) //數據頭不對,則重新開始尋找0x55數據頭 { ucRxCnt=0; return; } if (ucRxCnt<11) {return;}//數據不滿11個,則返回 else { if(ucRxBuffer[1]==0x53) { int i=-1; while((i++)<11) { ANGLE[i]=ucRxBuffer[i]; } } else if(ucRxBuffer[1]==0x52) { int i=-1; while((i++)<11) { GYRO[i]=ucRxBuffer[i]; } } ucRxCnt=0;//清空緩存區 } roll=((short)(ANGLE[3]<<8|ANGLE[2]))/32768.0*180;//得到角度 gyro=((short)(GYRO[3]<<8|GYRO[2]))/32768.0*2000;//得到角速度
代碼和3D模型見附件
(2.45 MB)下載