這套攝像頭處理算法是可以直接使用的。只需要重寫一下void servo()//舵機(jī)控制函數(shù)。
輸入的參數(shù)是**extern uint8 image_dec[60][80];**數(shù)組中保存的攝像頭傳過來二值化后的數(shù)據(jù)。
輸出是距離賽道的偏差 保存在整形變量：duojijiaodu 中。把這個(gè)變量傳入舵機(jī)打腳的pid算法中即可。
講道理這樣基本就可以用了。如果想跑的更順暢，更快的話需要調(diào)整最下面代碼中的

my_piancha=(int)(mzd2*currentzhongjian_lk - mzd1*B);//0.95,0.32//偏差合成，控制轉(zhuǎn)彎，1.1倍的點(diǎn)+0,9倍的斜率構(gòu)成總的偏差

通過修改兩個(gè)參數(shù)控制曲率和偏差的融合來實(shí)現(xiàn)。
注意：攝像頭循跡不會(huì)走賽道中線，尤其在拐彎時(shí)。比如再拐一個(gè)左彎時(shí)，在入彎時(shí)車模會(huì)貼賽道右側(cè)，出彎時(shí)會(huì)貼賽道左側(cè)。這樣可以保證高速過彎且不會(huì)沖出賽道。
這些代碼是我之前上傳文件的一部分。攝像頭四輪

使用時(shí)，需要依次調(diào)用函數(shù)：
//攝像頭轉(zhuǎn)向控制
lkcongzhongjiansaomiao();
pianchachuli();
servo();

/* 攝像頭循跡程序 我們這一屆攝像頭20CM高度，沒法發(fā)揮出攝像頭的優(yōu)勢(shì)，內(nèi)切一直壓線， 這個(gè)程序在30CM高度的攝像頭中可以跑的3M/s數(shù)組 static int half_width_group【】中保存的是賽道寬的一半，可以在實(shí)際中測(cè)量然后修改這個(gè)數(shù)組中的數(shù)據(jù)。最終偏差是由中線的差值和曲率融合。pianchachuli（）函數(shù)中my_piancha=(int)(mzd2*currentzhongjian_lk - mzd1*B); 這個(gè)表達(dá)式完成融合，可以通過調(diào)整兩個(gè)參數(shù)的大小讓小車轉(zhuǎn)彎更順滑，不壓線servo函數(shù)中是控制舵機(jī)打腳，其中16.5/70是打角的系數(shù)，再乘以上面的my_piancha變量就是實(shí)際舵機(jī)需要打的角度*/ #include "saomiao.h"#include "headfile.h" //白1 黑0#include "math.h" int qianzhan=29; int my_piancha,my_lastzhongjian=40; int jiao, x,y,linshi_x,linshi_right_heixian,linshi_y,ruyuanhuan_flag=0; extern uint8 image_dec[60][80];int [60],right_heixian[60],left_heixian[60],zhidao_flag; int lastpiancha_1,lastpiancha_2,lastpiancha_3,lastpiancha_4,duojijiaodu,flag_l=0,flag_r=0,linshi_left_heixian,B,shaobudaozuo_flag[60],shaobudaoyou_flag[60]; int shizidiyidian,shangshizidiyidian,S_Z; int shizishu,chazhi,yy1,yy2,rushizi; int duandian,duandianshu,duandianshu1,currentzhongjian_lk; int xielv_flag; int qulv_point=0,zhangai_right=0,zhangai_left=0; int dian1,youshi_kuan_flag=0; char s_wan_flag,shizi_flag,yuanhuan_flag=0,guai_flag=0; int qvlv_quanju,qulv_jinduan,qulv_yuandaun; int qulv_jinduan_right=0,qulv_jinduan_left=0,qulv_yuandaun_right=0,qulv_yuandaun_left=0,qvlv_quanju_right=0,qvlv_quanju_left=0; int half_zhi[60]; int yy1_pinjun,yy2_pinjun;static int half_width_group[60]= { 0,0,0,0,0,0,0,0,8, 9,10,11,12,12,13,15,16,17,18, 19,20,21,22,23,24,25,25,26,28, 29,30,30,31,32,33,34,35,35,36, 37,38,38,39,39,40,40,40,40,40, 40,40,40,40,40,40,40,40,40,40 }; int regression(int startline,int endline) //線性回歸方程計(jì)算斜率 {if(endline>56)endline=56;int i;int sumX=0,sumY=0,avrX=0,avrY=0 ;int num=0,B_up1=0,B_up2=0,B_up,B_down;for(i=startline;i<=endline;i++){num++;sumX+=i;sumY+=currentzhongjian[i];}avrX=sumX/num;avrY=sumY/num;B_up=0;B_down=0;for(i=startline;i<=endline;i++){B_up1=(int)(currentzhongjian[i]-avrY);B_up2=i-avrX;B_up+=(int)(10*(B_up1*B_up2));B_up=B_up/100*100;B_down+=(int)(10*((i-avrX)*(i-avrX)));}if(B_down==0) B=0;else B=B_up*16/B_down;//16return B; }int hy; void huaxian(int x1,int y1,int x2,int y2)//將兩個(gè)點(diǎn)之間連成一條線段 {int i,max,a1,a2;a1=x1;a2=x2;if(a1>a2){max=a1;a1=a2;a2=max;} for(i=x1;i>0;i--) {if((x2-x1)!=0){hy=(i-x1)*(y2-y1)/(x2-x1)+y1;currentzhongjian[i]=hy;if(hy<=1||hy>=79)break;} } } void huaxian2(int xxx1,int yyy1,int xxx2,int yyy2)//兩點(diǎn)之間的線段 {int i2,max2,a12,a22;a12=xxx1;a22=xxx2;if(a12>a22){max2=a12;a12=a22;a22=max2;} for(i2=xxx1;i2<59;i2++) {if((xxx2-xxx1)!=0){hy=(i2-xxx1)*(yyy2-yyy1)/(xxx2-xxx1)+yyy1;currentzhongjian[i2]=hy;} } }//*****************************************************舵機(jī)控制函數(shù)**********************************************************// void servo()//舵機(jī)控制函數(shù) { jiao= Servo_Center-(int)((duojijiaodu*16.5/70));//舵機(jī)的pid算法公式，沒有小數(shù)，所以要除以70來實(shí)現(xiàn)小數(shù) if(jiao>=Servo_Left) jiao=Servo_Left; if(jiao<=Servo_Right) jiao=Servo_Right; servo_PWM(jiao); } //***************************************************掃描函數(shù)***************************************************************// void lkcongzhongjiansaomiao() {int kuan_flag=0,lo=0,s_point=0,S_COUNT=0,fuduandian=0,qulv_yuanhuan=0,chushizi_flag=0;memset(shaobudaozuo_flag,0,sizeof(shaobudaozuo_flag));//清零函數(shù)memset(shaobudaoyou_flag,0,sizeof(shaobudaoyou_flag));int zhangai_flag=0,qulv_s_y=0,qulv_s_l=0;shangshizidiyidian=0;left_heixian[59]=0;right_heixian[59]=79; shizishu=0;rushizi=0;xielv_flag=0;shizi_flag=0;my_lastzhongjian=39;if(currentzhongjian[58])currentzhongjian[59]=my_lastzhongjian=currentzhongjian[58];elsecurrentzhongjian[59]=39;for(y=58;y>=0;y--)//掃描完58，整副圖像處理完畢{for(x=my_lastzhongjian;x<=79;x++)//從中間向右掃描{if(image_dec[y][x]==0){right_heixian[y]=x;shaobudaoyou_flag[y]=1; break;}}for(x=my_lastzhongjian;x>=0;x--)//向左掃描{ if(image_dec[y][x]==0){left_heixian[y]=x;shaobudaozuo_flag[y]=1;break;} }if(fuduandian==0){if(y>48)//丟邊補(bǔ)線，加整個(gè)賽道補(bǔ)線{if(shaobudaoyou_flag[y]==0&&shaobudaozuo_flag[y]==1)//掃不到右{ right_heixian[y]=left_heixian[y]+2*half_width_group[y];//y為數(shù)組中的x值}else if(shaobudaoyou_flag[y]==1&&shaobudaozuo_flag[y]==0)//掃不到左{left_heixian[y]=right_heixian[y]-2*half_width_group[y];}else if(shaobudaoyou_flag[y]==0&&shaobudaozuo_flag[y]==0)//都掃不到{left_heixian[y]=0;//如果兩邊都掃不到，直接從中間提取中線right_heixian[y]=79;}}else if(y<=48)//更遠(yuǎn)的補(bǔ)線，{if(shaobudaoyou_flag[y]==0&&shaobudaozuo_flag[y]==1)//掃不到右{ right_heixian[y]=right_heixian[y+1]+(abs)(left_heixian[y]-left_heixian[y+1])-1;//根據(jù)左邊這一點(diǎn)與上一點(diǎn)，//（y+1）為數(shù)組中上一次的x坐標(biāo)，//left_heixian[y]-left_heixian[y+1]算左邊偏移量，減一為后期補(bǔ)償}else if(shaobudaoyou_flag[y]==1&&shaobudaozuo_flag[y]==0)//掃不到左{left_heixian[y]=left_heixian[y+1]-(abs)(right_heixian[y+1]-right_heixian[y])+1;}else if(shaobudaoyou_flag[y]==0&&shaobudaozuo_flag[y]==0)//都掃不到{left_heixian[y]=0;right_heixian[y]=79;}}//*************************************中線處理*********************************************************//currentzhongjian[y]=(int)((right_heixian[y]+left_heixian[y])/2); //提取中間線（左邊加右邊比2）half_zhi[y]=(int)((right_heixian[y]-left_heixian[y])/2); //一半賽道值（沒用，不改）//*******************************************************中線濾波防止中線出現(xiàn)毛刺**************************************************/if(currentzhongjian[y]-currentzhongjian[y+1]>4&&y<42&&B<0) currentzhongjian[y]=currentzhongjian[y+1]+1; if(currentzhongjian[y]-currentzhongjian[y+1]<-4&&y<42&&B>0) currentzhongjian[y]=currentzhongjian[y+1]-1; /******************************************************************曲率計(jì)算**************************************************************/ if(y>30)//曲率近端判斷 {if((currentzhongjian[y]-currentzhongjian[y+1])>0) qulv_jinduan_right++; else if((currentzhongjian[y]-currentzhongjian[y+1])<0) qulv_jinduan_left++; } if(y<25&&y>=7)//曲率遠(yuǎn)端判斷，遠(yuǎn)處的一點(diǎn)就不要了 {if((currentzhongjian[y]-currentzhongjian[y+1])>0) qulv_yuandaun_right++; else if((currentzhongjian[y]-currentzhongjian[y+1])<0) qulv_yuandaun_left++; } if(y<=55&&y>15)//曲率全局判斷 {if((currentzhongjian[y]-currentzhongjian[y+1])>0) qvlv_quanju_right++; else if((currentzhongjian[y]-currentzhongjian[y+1])<0) qvlv_quanju_left++;}if(currentzhongjian[y]>79)//中線的限制幅度 currentzhongjian[y]=79;if(currentzhongjian[y]<0) currentzhongjian[y]=0;my_lastzhongjian=currentzhongjian[y];//保存中間點(diǎn)坐標(biāo) if(y<56)//小s位置判斷，小s彎道作直線沖刺 { if((currentzhongjian[y]-currentzhongjian[y+2])>0) qulv_s_y++; else if((currentzhongjian[y]-currentzhongjian[y+2])<0) qulv_s_l++; if(y>36&&abs(currentzhongjian[y]-currentzhongjian[y+2])>0) qulv_yuanhuan++; //printf("y:%d\n",qulv_s_y); //printf("l:%d\n",qulv_s_l); } if(y<58&&y>=10) S_COUNT+=currentzhongjian[y]; // *******************************************小S斷點(diǎn)搜索*********************************************//if(y>8&&(abs)(right_heixian[y]-currentzhongjian[y])<5||(abs)(left_heixian[y]-currentzhongjian[y])<5)//小S斷點(diǎn)判定s_point=y;// else s_point=0;// *******************************************防止掃到跑道外*********************************************//if(y<36&&((abs)(right_heixian[y]-currentzhongjian[y])<1||(abs)(left_heixian[y]-currentzhongjian[y])<1))//防止掃到跑道外面 {duandian=y;fuduandian=y;if(y>15){duandianshu=y-15;if(duandianshu>35)duandianshu=35;}if(y>qianzhan){duandianshu1=y-qianzhan;if(duandianshu1>25)duandianshu1=25;} } else {duandian=0;duandianshu=0;duandianshu1=0; }}}if(right_heixian[y]-left_heixian[y]<half_width_group[y]&&right_heixian[y+1]-left_heixian[y+1]<half_width_group[y+1])//防止掃到跑道外面break; //} //********************************************************y行結(jié)束標(biāo)志*******************************************************S_Z=S_COUNT/(48);//********************************************************************曲率計(jì)算*******************************************************************qvlv_quanju=abs(qvlv_quanju_right-qvlv_quanju_left);//曲率全局qulv_jinduan=abs(qulv_jinduan_right-qulv_jinduan_left);//曲率近端qulv_yuandaun=abs(qulv_yuandaun_right-qulv_yuandaun_left);//曲率遠(yuǎn)端qvlv_quanju_right=qvlv_quanju_left=qulv_jinduan_right=qulv_jinduan_left=qulv_yuandaun_right=qulv_yuandaun_left=0;// *******************************************************十字處理*********************************************************//if(duandian<16) // *****************************十字{for(int i=58;i>7;i--){ if(i>45&&(shaobudaozuo_flag[i]==1||shaobudaoyou_flag[i]==1))rushizi++;if((i>9&&i<=40)&&shaobudaozuo_flag[i]==0&&shaobudaoyou_flag[i]==0)shizishu++;if(i<45&&(shaobudaozuo_flag[i]==1||shaobudaoyou_flag[i]==1)&&shangshizidiyidian==0)shangshizidiyidian=i;}if(shizishu>2&&rushizi>10)//剛?cè)胧?/span>{shizi_flag=1;//gpio_set(PTE1,1);xielv_flag=1;for(int i=49;i>0;i--){ if(shaobudaozuo_flag[i]==0&&shaobudaoyou_flag[i]==0){shizidiyidian=i;break;} }if(shizidiyidian<34){yy1=shizidiyidian+19;yy2=shizidiyidian+24;}else if(shizidiyidian<40){yy1=shizidiyidian+13;yy2=shizidiyidian+18;}else if(shizidiyidian<45){yy1=shizidiyidian+9;yy2=shizidiyidian+13;}else if(shizidiyidian<49){yy1=shizidiyidian+6;yy2=shizidiyidian+10;}elseif(shizidiyidian<55){yy1=shizidiyidian+2;yy2=shizidiyidian+4;}yy1_pinjun=(currentzhongjian[yy1]+currentzhongjian[yy1+1]+currentzhongjian[yy1-1])/3;yy2_pinjun=(currentzhongjian[yy2]+currentzhongjian[yy2+1]+currentzhongjian[yy2-1])/3;//if(!ruyuanhuan_flag)huaxian(yy1,yy1_pinjun,yy2,yy2_pinjun); //huaxian(yy1,currentzhongjian[yy1],yy2,currentzhongjian[yy2]); } else if(shizishu>4&&rushizi<=10&&shangshizidiyidian>=20)//出十字{chushizi_flag=1;// gpio_set(PTE1,1);xielv_flag=1;shizi_flag=1;//if(!ruyuanhuan_flag)huaxian2(shangshizidiyidian-6,currentzhongjian[shangshizidiyidian-6],58,currentzhongjian[58]);}// gpio_set(PTE1,0);}//********************************************************圓環(huán)檢測(cè)******************************************************* if(shizi_flag&&qulv_yuanhuan<=10&&qulv_jinduan<7&&!chushizi_flag) {signed char kkk=0,black_num=0;for( kkk=(int)(currentzhongjian[58]+currentzhongjian[57]+currentzhongjian[56])/3;kkk<=79;kkk++)//向右掃描黑塊點(diǎn)數(shù){if(!image_dec[8][kkk]||!image_dec[10][kkk]){black_num++;}elsebreak;}for( kkk=(int)(currentzhongjian[58]+currentzhongjian[57]+currentzhongjian[56])/3;kkk>=0;kkk--)//向左掃描黑塊點(diǎn)數(shù){if(!image_dec[8][kkk]||!image_dec[10][kkk]){black_num++;}elsebreak;}if(black_num>36)//檢測(cè)到圓環(huán){yuanhuan_flag++;//gpio_set(PTC18,0);//點(diǎn)亮led提示}else{yuanhuan_flag=0;//gpio_set(PTC18,1);關(guān)閉led提示}} else { yuanhuan_flag=0;//gpio_set(PTC18,1);//關(guān)閉led提示 }if(yuanhuan_flag>3)//連續(xù)檢測(cè)到圓環(huán)三次以上 { guai_flag=1; //gpio_set(PTA14,1); }// ***********************************************************小S彎判定*小S彎判定*****************************************************// if(s_point==0&&qulv_s_y>6&&qulv_s_l>6&&zhidao_flag<20&&!shizi_flag) {s_wan_flag=1;//led(LED0, LED_ON); } else { s_wan_flag=0; //led(LED0, LED_OFF); }} //************************************************圖像處理結(jié)束,下面是偏差處理************************************************/void pianchachuli()//偏差處理 {if(xielv_flag==0)regression(15+duandianshu,43+duandianshu);///斜率計(jì)算dian1=abs((currentzhongjian[52]+currentzhongjian[53]+currentzhongjian[54])/3-39); //lins=currentzhongjian[30]; currentzhongjian_lk=(currentzhongjian[qianzhan+duandianshu1]-39);//點(diǎn)if(B>19) B=19; if(B<-19) B=-19;float mzd1=0.11; //曲率與中線合成的權(quán)值 float mzd2=0.75; my_piancha=(int)(mzd2*currentzhongjian_lk - mzd1*B);//0.95,0.32//偏差合成，控制轉(zhuǎn)彎，1.1倍的點(diǎn)+0,9倍的斜率構(gòu)成總的偏差if(!ruyuanhuan_flag&&ruyuanhuan_flag<10)//拐50ms {ruyuanhuan_flag++; if(my_piancha<-4&&shizi_flag)//右邊進(jìn)的圓環(huán) my_piancha+=13; else if(my_piancha>4&&shizi_flag)//左邊進(jìn)的圓環(huán) my_piancha-=13; } else ruyuanhuan_flag=0;lastpiancha_4=lastpiancha_3; lastpiancha_3=lastpiancha_2; lastpiancha_2=lastpiancha_1; lastpiancha_1=duojijiaodu; duojijiaodu=my_piancha; }

總結(jié)

以上是生活随笔為你收集整理的恩智浦智能车赛。摄像头处理函数。的全部內(nèi)容，希望文章能夠幫你解決所遇到的問題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。