无人机第一届报告项目二

1、项目 2 循迹飞行一、程序概述根据头识别路线标志，沿路线飞行。流程图如图 1 所示。二、算法设计1.目标识别程序利用 opencv 计算机视觉库来进行目标识别。识别流程如图 2 所示。图 2 目标识别流程图2.自旋控制程序根据正前方连点连线的斜率控制偏航角度。流程图如图 3 所示。图 3 自旋控制流程图3. 横滚控制程序根据识别到的点的水平偏差控制飞行器的横滚角，使飞行器保持在标志上方。流程如图 4 所示。图 4 横滚控制流程图4. 速度控制程序飞行器的速度数据，控制俯仰角使飞行器匀速前进。流程如图 5 所示。图 5 速度控制流程图5飞行器状态机飞行器根据水平偏差建立状态机，防止水平丢失目标之

2、后串道。状态机如图 6 所示。图 6 飞行器状态机程序代码#include ardrone/ardrone.h#include math.h#define #define #define #define#defineLEFT 1RIGHT MID20PO_NUM10/特征点数目DIR_FILT_NUM 10/方向滤波数目#define FILTER_PO_NUM 10 /平移偏差滤波个数#define I_NUM 10 /不完全积分个数/shift s#define HANDLE0e#define #define #define#defineLEFT_IN 1LEFT_OUT 2RIGHT_I

3、N 3RIGHT_OUT 4/ARDrone ardrone;lowerB=0; lowerG=0;lowerR=70;upperB=50;upperG=50; upperR=255;p_dir=250; /300 d_dir = 0; p_shift_para=23;/23 i_shift_para=0;d_shift_para=70;/70velocity_altitude_=330; /300= 120; /目标高度alt_p=0;/高度alt_d=0; /高度velocity_p = 16;/速度velocity_d = 9;/速度clock_t start,end;double du

4、r;ShiftData;CvScalar color_yel=CV_RGB(255,255,3); CvScalar color_red = CV_RGB(255,0,0); CvScalar color_white = CV_RGB(255,255,255);CvScalar color_blue = CV_RGB(0,0,255);data400;double doubledoublearray_delta_y400; array_vy400;array_velocity_y400;CvPo CvPoCvSizepoPO_NUM;,popo_2,po_mid,po_mid;image_si

5、ze;double double double double double double doublesvelocity_x,velocity_y,velocity_z; test_double = 0.01;delta_mid_x = 0;vx = 0.0, vy = 0.0, vz = 0.0, vr = 0.0,vr_old =0;delta_shift_arrayFILTER_PO I_arr_shiftI_NUM;dir_arrDIR_FILT_NUM;e=HANDLE; /飞行器丢线状态_NUM;/Thisfunction threshold the HSV image and c

6、reate a binary imageIplImage* GetThresholdedImage(IplImage* imgHSV)IplImage* imgThresh=cvCreateImage(cvGetSize(imgHSV),IPL_DEPTH_8U, 1); cvInRangeS(imgHSV, cvScalar(lowerB,lowerG,lowerR), cvScalar(upperB,upperG,upperR),imgThresh);return imgThresh;/This function create two windows and 6 trackbars for

7、 the Ball window void setwindowSettings()cvNamedWindow(tracking);cvCreateTrackbar(LowerB, tracking, &lowerB, 256, NULL);cvCreateTrackbar(UpperB, tracking, &upperB, 256, NULL);cvCreateTrackbar(LowerG, tracking, &lowerG, 256, NULL);cvCreateTrackbar(UpperG, tracking, &upperG, 256, NULL);cvCreateTrackba

8、r(LowerR, tracking, &lowerR, 256, NULL);cvCreateTrackbar(UpperR, tracking, &upperR, 256, NULL);cvCreateTrackbar(p_dir, tracking, &p_dir, 500, NULL); cvCreateTrackbar(ds_dir, tracking, &d_dir, 500, NULL); cvCreateTrackbar(p_shift, tracking,&p_shift_para, 500, NULL); cvCreateTrackbar(i_shift, tracking

9、,&i_shift_para, 500, NULL);cvCreateTrackbar(D参?数y, tracking, &d_shift_para, 500, NULL);cvCreateTrackbar(speed mm/s, tracking, &velocity_, 500, NULL);cvCreateTrackbar(高?度p, tracking, &alt_p, 500, NULL);cvCreateTrackbar(高?度d, tracking,&alt_d, 500, NULL);cvCreateTrackbar(目?标高?度, tracking, &altitude_, 2

10、00, NULL);cvCreateTrackbar(速度p, tracking, &velocity_p, 500, NULL);cvCreateTrackbar(速度d, tracking, &velocity_d, 500, NULL);double refresh_filter(double delta_mid_x)double sum=0;for(i=0;i(FILTER_PO_NUM-1);i+)delta_shift_arrayi=delta_shift_arrayi+1;sum+=delta_shift_arrayi;delta_shift_arrayFILTER_PO_NUM

11、-1=delta_mid_x;sum+=delta_shift_arrayFILTER_PO_NUM-1;return sum/FILTER_PO_NUM;void IRefresh(double I_arr,double delta,num)for(i=0;i(num-1);i+)I_arri=I_arri+1;I_arrnum-1=delta;double get_avr(double I_arr,num)double sum=0;for(i=0;i1) /非丢线if(delta_mid0)se = LEFT_IN;else if(delta_mid1)if(delta_mid0)se =

12、 LEFT_IN;else if(delta_mid1)if(delta_mid0)se = LEFT_IN;else if(delta_mid1)if(delta_mid0)se = LEFT_IN;prf(状态：LEFT_OUT n);break;case RIGHT_OUT:if(po_counter1)if(delta_mid0)se = RIGHT_IN;prf(状态：RIGHT_OUT n);break; default:break;/冒泡排序,po_counter 个点/按横坐标升序排列 void po_sort(poi,j;po_num;_counter)CvPopotmp;_

13、num=po_counter;for(j=0;j(po_num-1);j+)for(i=0;ipoi+1.x)tmp = poi;popoi=poi+1;i+1 = tmp;/计算平均斜率 double get_average_k(double k; double k_avr; double sum=0;po_counter)if(po_counter 1)for(i=0;iwidth,image-height);popo_mid.x = image_size.width/2;_mid.y = image_size.height/2;/ Take off / Landing if (key =

14、 ) if (ardrone.onGround() ardrone.takeoff();elseardrone.landing();/ Movedouble vr_test = 0.8; vx=vy=vz=vr=0;if (key = 0 x260000) vx = 2.0; if (key = 0 x280000) vx = -2.0; if (key = 0 x250000) vy = 2.0;if (key = 0 x270000) vy = -2.0;if (key = l)if (key = k)if (key = q)if (key = a) if(!auto_flag)vr =

15、-2.0; /youvr = 2.0;vz = 1.0;vz = -1.0;/zuoardrone.move3D(vx, vy, vz, vr);/ Change camerasicmode = 0;if (key = c) ardrostart = clock();tCamera(+mode%4);IplImage* imgThresh = GetThresholdedImage(image);cvSmooth(imgThresh, imgThresh, CV_GAUSSIAN,3,3); /smooth the binary image using ernelcvShowImage(tra

16、cking, imgThresh);Gaussi/提取图像轮/排序po_sort(po_counter);/找前面两点if(popo po_counter1)_= po0;2 = popo_counter-1;/求中间点po_mid.y = (po_.y+po_2.y)/2;_mid.y)/podelta_mid_raw = (double)(po_mid.y-po_mid.y;prf(delta_mid %f n,delta_mid);/丢线状态判断shift_se_judge(delta_mid_raw,po_counter);/求斜率和平移偏移量if(se=LEFT_IN | se=RI

17、GHT_IN)if(po_2.y-po_.y)!=0) /防除数为零delta_x=float(po_2.y-po_.y)/(po_2.x-po_.x);delta_x = atan(delta_x);velocity_raw = velocity_;delta_mid = delta_mid_raw;/偏移量赋值elsedelta_x=0;velocity_raw = 0;/斜率降噪IRefresh(dir_arr,delta_x,DIR_FILT_NUM); delta_x_avr = get_avr(dir_arr,DIR_FILT_NUM); if(abs(delta_x-delta_

18、x_avr)0.2)delta_x = delta_x_avr;/prf(斜率被?滤?波了?！?！?！?=);/get auto key if (key = b)if (key = n)auto_flag = 1;auto_flag = 0;/move autovx=vy=vz=vr=0;/高度控制altitude = ardrone.getAltitude();vz = altitude_control(altitude,altitude_,&alt_delta_old);/前向速度控制ardrone.getVelocity(&velocity_x,&velocity_y,NULL);vy = velocity_control(velocity_y,velocity_ if(vy1) vy=1;if(vy-1) vy=-1;_raw,&velocity_delta_