devbox
菜鸟追梦旅行
这个屌丝很懒，什么也没留下！
关注作者
热门标签
热门文章
当前位置:   article > 正文

智能车竞赛圆环、避障等元素方案开源_智能车如何找圆弧形拐点

作者：菜鸟追梦旅行 | 2024-04-14 06:20:18

赞

踩

智能车如何找圆弧形拐点

一、圆环

1.基于传统扫线的补线方法

   以右圆环为例

（1）找到近端右拐点，且左边界不丢线

 （2）找到右边中间拐点，定位近端右拐点，拉线

 （3）右边界底部丢线，结束拉线，找远端右拐点

 

 （4）定位远端右拐点，补线进环

 （5）编码器计步大于某阈值，停止补线，进普通循迹状态

  （6）赛道宽度突变，补线出环

 （7）找远端右拐点，补线不入环

 

 （8）结束，清空状态

  1. void Right_Roll_Judge(void)
  2. {
  3.     extern int Distance_L;
  4.     extern int Distance_R;
  5.  
  6.     int erro_x,erro_y;
  7. 	if(XK.roll_flag==0&&XK.roll_situation==0)//初始状态
  8. 	{
  9. 		if((Right_NearInflection_Judge(40,30)==1)&&(Left_Road_Lose(10,60)==1))//找到右拐点且左边界不丢失
  10. 		{
  11. 			XK.roll_situation=1;
  12. 			XK.roll_flag=1;
  13. 			XK.y3=28;
  14. 			erro_x=0;
  15. 			erro_y=0;
  16. 			XK.y2=0;
  17. 			XK.x2=0;
  18. 		}
  19. 	}
  20. 	if(XK.roll_situation==1)//找拐点，定位
  21. 	{
  22. 		Right_NearInflection_Catch(XK.y,60,XK.x,XK.y);//近拐点定位
  23. 		Right_RoundInflection_Catch(XK.y,1,XK.x2,XK.y2);//中间拐点定位
  24. 		if(XK.y>0&&XK.y2>0)//找到两个拐点，准备拉线
  25. 		{
  26. 		    erro_x=0;
  27. 		    erro_y=0;
  28. 		    XK.y1=XK.y2;
  29. 		    XK.x1=XK.x2;
  30. 		    XK.roll_situation=2;
  31. 		}
  32. 	}
  33. 	if(XK.roll_situation==2)//拉线
  34. 	{
  35.  
  36. 	    Right_NearInflection_Catch((XK.y-2),60,XK.x2,XK.y2);
  37. 		erro_y=abs(XK.y2-20);
  38.  
  39. 	    XK.y3=XK.y;
  40. 	    XK.y4=XK.y1+erro_y;
  41. 	    XK.x3=XK.x;
  42. 	    XK.x4=XK.x1;
  43. 		XK.line=1;
  44. 		if(XK.right_line[100]>160)//当右边界底部丢线，结束拉线
  45. 		{
  46. 			XK.line=0;
  47. 			XK.y2=0;
  48. 			XK.roll_situation=3;
  49. 		}
  50. 	}
  51. 	if(XK.roll_situation==3)//找远拐点
  52. 	{
  53. 		Right_FarInflection_Catch(1,40,XK.x2,XK.y2);//远拐点定位
  54. 		if((XK.y2>0)&&(XK.right_line[20]>150))//找到远拐点且右边界前端丢线，准备拉线
  55. 		{
  56.             XK.y1=XK.y2;
  57.             XK.x1=XK.x2;
  58.             Distance_L=0;
  59.             Distance_R=0;
  60.             XK.Encoder_distance=0;
  61. 		    XK.roll_situation=4;
  62. 		}
  63. 	}
  64. 	if(XK.roll_situation==4)//入环补线
  65. 	{
  66.  
  67.         XK.y3=25;
  68.         XK.y4=55;
  69.         XK.x3=110;
  70.         XK.x4=45;
  71.         XK.line=1;
  72.  
  73.         if(XK.Encoder_distance>5)//编码器计步（走一段距离，结束补线）
  74.         {
  75.             XK.y2=0;
  76.             XK.line=0;
  77.             Distance_L=0;
  78.             Distance_R=0;
  79.             XK.Encoder_distance=0;
  80.             XK.roll_situation=6;
  81.         }
  82. 	}
  83. 	if(XK.roll_situation==6)//环内状态
  84. 	{
  85. 	    if((XK.road_width[29]-XK.road_width[30]>50)||XK.road_width[30]>150)//赛道宽度突变
  86. 	    {
  87.             XK.y1=XK.y2;
  88.             XK.x1=XK.x2;
  89.             XK.y2=0;
  90.             Distance_L=0;
  91.             Distance_R=0;
  92.             XK.Encoder_distance=0;
  93. 	        XK.roll_situation=7;
  94. 	    }
  95. 	}
  96. 	if(XK.roll_situation==7)//出环补线
  97. 	{
  98.         XK.y3=25;
  99.         XK.y4=60;
  100.         XK.x3=115;
  101.         XK.x4=30;
  102.         XK.line=1;
  103.         Right_FarInflection_Catch(1,30,XK.x2,XK.y2);找远端拐点
  104.         distence_count();
  105.         if((XK.y2>0)&&XK.Encoder_distance>7)//不入环判断：在走一段距离的情况下，找到远端拐点
  106.         {
  107.             XK.y1=XK.y2;
  108.             XK.x1=XK.x2;
  109.             XK.roll_situation=8;
  110.         }
  111. 	}
  112. 	if(XK.roll_situation==8)//不入环补线
  113. 	{
  114.         XK.y3=100;
  115.         XK.y4=20;
  116.         XK.x3=136;
  117.         XK.x4=113;
  118.         XK.line=1;
  119. 	}
  120. 	if(XK.roll_situation==8)//不入环补线
  121. 	{
  122.         Distance_L=0;
  123.         Distance_R=0;
  124.         XK.Encoder_distance=0;
  125.         XK.y3=100;
  126.         XK.y4=20;
  127.         XK.x3=136;
  128.         XK.x4=113;
  129.         XK.line=1;
  130. 	}
  131.     if((Left_Road_Lose(20,60)==1)&&XK.roll_situation==8)//右边界不丢失，圆环标志位清零
  132.     {
  133.         XK.roll_situation=0;
  134. 		XK.roll_flag=0;
  135.         XK.line=0;
  136.     }
  137.  
  138. }
  139.  
  140. void draw_line(void)//补线处理函数
  141. {
  142.     if(XK.line==1)
  143.     {
  144.  		ips114_add_line(XK.y,XK.x+0,XK.y1,XK.x1+0,0x00);
  145. 		ips114_add_line(XK.y,XK.x+1,XK.y1,XK.x1+1,0x00);
  146. 		ips114_add_line(XK.y,XK.x+2,XK.y1,XK.x1+2,0x00);
  147.  		ips114_add_line(XK.y,XK.x+3,XK.y1,XK.x1+3,0x00);
  148. 		ips114_add_line(XK.y,XK.x-1,XK.y1,XK.x1-1,0x00);
  149. 		ips114_add_line(XK.y,XK.x-2,XK.y1,XK.x1-2,0x00);
  150.     }
  151. }

关于如何在图像数组中补线，基于逐飞的IPS114画线函数更改即可

  1. void ips114_add_line (uint16 x_start, uint16 y_start, uint16 x_end, uint16 y_end, const uint16 color)
  2. {
  3.  
  4.     int16 x_dir = (x_start<x_end ? 1:-1);
  5.     int16 y_dir = (y_start<y_end ? 1:-1);
  6.     float temp_rate = 0;
  7.     float temp_b = 0;
  8.  
  9.     if(x_start != x_end)
  10.     {
  11.         temp_rate = (float)(y_start-y_end)/(float)(x_start-x_end);
  12.         temp_b = (float)y_start-(float)x_start*temp_rate;
  13.     }
  14.     else
  15.     {
  16.         while(y_start != y_end)
  17.         {
  18.             mt9v03x_image_copy[x_start][ y_start]=0x00;//根据需要更改的图像数组
  19.  
  20.             y_start += y_dir;
  21.         }
  22.         return;
  23.     }
  24.     if(abs(y_start-y_end)>abs(x_start-x_end))
  25.     {
  26.         while(y_start != y_end)
  27.         {
  28.             mt9v03x_image_copy[x_start][ y_start]=0x00;//根据需要更改的图像数组
  29.             y_start += y_dir;
  30.             x_start = (int16)(((float)y_start-temp_b)/temp_rate);
  31.         }
  32.     }
  33.     else
  34.     {
  35.         while(x_start != x_end)
  36.         {
  37.             mt9v03x_image_copy[x_start][ y_start]=0x00;//根据需要更改的图像数组
  38.             x_start += x_dir;
  39.             y_start = (int16)((float)x_start*temp_rate+temp_b);
  40.         }
  41.     }
  42. }
2.电磁圆环，摄像头辅助判断
  1. void elec_roll_judge(void)
  2. {
  3. // ********* 摄像头区分左右环（这一段可单独放在图像处理函数里） ***********
  4.     if(XK.roll_flag==1&&XK.roll_situation==1)
  5.     {
  6.         XK.rolldirr=rroll_dir_judge(10,50,65);
  7.         XK.rolldirl=lroll_dir_judge(10,50,65);
  8.     }
  9. // *****************************************************************
  10.     extern float AD[6];
  11.     if(XK.roll_flag==0&&XK.roll_situation==0)
  12.     {
  13.         if(AD[1]>3000&&AD[0]>3000)//中间电感大于一定阈值
  14.         {
  15.             XK.roll_flag=1;
  16.             XK.roll_situation=1;
  17.             Distance_L=0;
  18.             Distance_R=0;
  19.             XK.Encoder_distance=0;
  20.         }
  21.     }
  22.         if(XK.roll_flag==1&&XK.roll_situation==1)
  23.         {
  24.  
  25.             if(XK.rolldirr==1)//右圆环状态
  26.             {
  27.                 XK.roll_flagr=1;
  28.                 XK.roll_flagl=0;
  29.                 XK.roll_situation=2;
  30.                 Distance_L=0;
  31.                 Distance_R=0;
  32.                 XK.Encoder_distance=0;
  33.             }
  34.             else if(XK.rolldirl==1)//左圆环状态
  35.             {
  36.                 XK.roll_flagr=0;
  37.                 XK.roll_flagl=1;
  38.                 XK.roll_situation=2;
  39.                 Distance_L=0;
  40.                 Distance_R=0;
  41.                 XK.Encoder_distance=0;
  42.             }
  43.             if(XK.Encoder_distance>15)//计步大于某值，跳出圆环
  44.              {
  45.                 XK.roll_situation=0;
  46.                 XK.roll_flag=0;
  47.                 XK.roll_flagl=0;
  48.                 XK.roll_flagr=0;
  49.                 Distance_L=0;
  50.                 Distance_R=0;
  51.                 XK.Encoder_distance=0;
  52.              }
  53.         }
  54.         if(XK.roll_flag==1&&XK.roll_situation==2)
  55.         {
  56.             if(XK.roll_flagl==1&&XK.roll_flagr==0)//右圆环状态
  57.             {
  58.                 if(AD[3]>3300)//右斜电感大于阈值，舵机打角
  59.                 {
  60.                     XK.roll_situation=3;
  61.                     Distance_L=0;
  62.                     Distance_R=0;
  63.                     XK.Encoder_distance=0;
  64.                 }
  65.             }
  66.  
  67.             if(XK.roll_flagl==0&&XK.roll_flagr==1)//左圆环状态
  68.             {
  69.                 if(AD[4]>3400)//左斜电感大于阈值，舵机打角
  70.                 {
  71.                     XK.roll_situation=3;
  72.                     Distance_L=0;
  73.                     Distance_R=0;
  74.                     XK.Encoder_distance=0;
  75.                 }
  76.             }
  77.         }
  78.         if(XK.roll_flag==1&&XK.roll_situation==3)//环内状态
  79.         {
  80.             if(XK.Encoder_distance>9)//计步
  81.             {
  82.                 XK.roll_situation=5;
  83.                 Distance_L=0;
  84.                 Distance_R=0;
  85.                 XK.Encoder_distance=0;
  86.             }
  87.         }
  88.         if(XK.roll_flag==1&&XK.roll_situation==5)
  89.         {
  90.             if(XK.roll_flagl==1&&XK.roll_flagr==0)//右圆环出环判断
  91.             {
  92.                 if(AD[5]>1700)//右侧电感大于某阈值，右打角
  93.                 {
  94.                     XK.roll_situation=6;
  95.                     Distance_L=0;
  96.                     Distance_R=0;
  97.                     XK.Encoder_distance=0;
  98.                 }
  99.             }
  100.             if(XK.roll_flagl==0&&XK.roll_flagr==1)//左圆环出环判断
  101.             {
  102.                 if(AD[2]>2800)//左侧电感大于某阈值，左打角
  103.                 {
  104.                     XK.roll_situation=6;
  105.                     Distance_L=0;
  106.                     Distance_R=0;
  107.                     XK.Encoder_distance=0;
  108.                 }
  109.             }
  110.         }
  111.    
  112.         if(XK.roll_flag==1&&XK.roll_situation==6)//计步大于某阈值，结束圆环，标志位清零
  113.         {
  114.             if(XK.Encoder_distance>3)
  115.             {
  116.                 XK.roll_situation=0;
  117.                 XK.roll_flag=0;
  118.                 XK.roll_flagr=0;
  119.                 XK.roll_flagl=0;
  120.                 Distance_L=0;
  121.                 Distance_R=0;
  122.                 XK.Encoder_distance=0;
  123.             }
  124.         }
  125. }

二、避障

 这里需要用到编码器计行驶圈数

  1. #define ENCORDER_PRCISION   1024.f  //512线、1024线等
  2. #define ENCORDER_D      19.2f   //编码器齿轮直径   齿数30
  3. #define WHEEL_D         64.0f   //车轮直径
  4. #define WHEEL_GEAR_D            41.0f   //车轮直连齿轮直径    齿数  68
  5. #define GET_DISTANCE_M(val)    ((((val/ENCORDER_PRCISION)*ENCORDER_D*PI )*WHEEL_D)/WHEEL_GEAR_D/1000)
  6.  
  7.  
  8. void distence_count(void)
  9. {
  10.  
  11.     Distance_L = Distance_L + XK.speedl;
  12.     Distance_R = Distance_R + XK.speedr;
  13.     XK.Encoder_distance=GET_DISTANCE_M((Distance_L + Distance_R) / 2);
  14.  
  15. }

  1. void barrier (void)
  2. {
  3.     extern int Distance_L;//左轮距
  4.     extern int Distance_R;//右轮距
  5.  
  6. 	if(dl1a_distance_mm<XK.dis1&&XK.barrier_judge==0)//避障预判断
  7. 	{
  8. 	    Gyro_qin0();//陀螺仪数值清零
  9. 		XK.barrier_in=1;//避障状态
  10. 		XK.barrier_judge=1;//避障进入标志位
  11. 		XK.Encoder_distance=0;//测距距离
  12.  
  13. 	}
  14. 	if(XK.barrier_judge==1)
  15. 	{
  16. 	    switch (XK.barrier_in)
  17. 	    {
  18.         case 1:
  19.         {
  20.             if(dl1a_distance_mm<=XK.dis2)//行进距离
  21.             {
  22.                 if((mt9v03x_image[2][94]+mt9v03x_image[3][94]+mt9v03x_image[3] 
  23.                 [93]+mt9v03x_image[3][95])/4<55)//tof测距小于某阈值且图像灰度像素小于某阈值                                                                        
  24.                                                                         （区分坡道与避障）
  25.                 {
  26.                 Distance_L=0;
  27.                 Distance_R=0;
  28.                 XK.Encoder_distance=0;
  29.                 XK.barrier_in=2;
  30.                 Gyro_qin0();
  31.                 wireless_uart_send_string("6");//与后车通信
  32.                 }
  33.                 else//未达条件跳出状态
  34.                 {
  35.                     Distance_L=0;
  36.                     Distance_R=0;
  37.                     XK.time=0;
  38.                     XK.Encoder_distance=0;
  39.                     XK.barrier_in=0;
  40.                     XK.barrier_judge=0;
  41.                     Gyro_qin0();
  42.                 }
  43.             }
  44.         }
  45.         break;
  46. 	    case 2://打角避障
  47. 	    {
  48.  
  49. 	        if(XK.dash_dir==1)//右边避障
  50. 	        {
  51. 	          if(eulerAngle.yaw<-3.1)//右航向角
  52. 	          {
  53. 	            Distance_L=0;
  54. 	            Distance_R=0;
  55. 	            XK.time=0;
  56. 	            XK.Encoder_distance=0;
  57. 	            XK.barrier_in=3;
  58. 	            Gyro_qin0();//陀螺仪数值清零
  59. 	          }
  60. 	        }
  61.             if(XK.dash_dir==0)//左边避障
  62.             {
  63.               if(eulerAngle.yaw>1.6)//左航向角
  64.               {
  65.                 Distance_L=0;
  66.                 Distance_R=0;
  67.                 XK.time=0;
  68.                 XK.Encoder_distance=0;
  69.                 XK.barrier_in=3;
  70.                 Gyro_qin0();//陀螺仪数值清零
  71.  
  72.               }
  73.             }
  74. 	    }
  75. 	    break;
  76.         case 3://直行
  77.         {
  78.             distence_count();//编码器测距
  79.             if(XK.Encoder_distance>3)//行进距离
  80.             {
  81.                 Distance_L=0;
  82.                 Distance_R=0;
  83.                 XK.time=0;
  84.                 XK.Encoder_distance=0;
  85.                 XK.barrier_in=4;
  86.                 Gyro_qin0();
  87.             }
  88.         }
  89.         break;
  90.         case 4://打角
  91.         {
  92.            
  93.             if(XK.dash_dir==1)//左边避障
  94.             {
  95.               if(eulerAngle.yaw>3.6)//左航向角
  96.               {
  97.                 Distance_L=0;
  98.                 Distance_R=0;
  99.                 XK.time=0;
  100.                 XK.Encoder_distance=0;
  101.                 XK.barrier_in=5;
  102.                 Gyro_qin0();
  103.               }
  104.             }
  105.             if(XK.dash_dir==0)//右边避障
  106.             {
  107.               if(eulerAngle.yaw<-6.0)//右航向角
  108.               {
  109.                 Distance_L=0;
  110.                 Distance_R=0;
  111.                 XK.time=0;
  112.                 XK.Encoder_distance=0;
  113.                 XK.barrier_in=5;
  114.                 Gyro_qin0();
  115.               }
  116.             }
  117.         }
  118.         break;
  119.         case 5://直行
  120.         {
  121.             distence_count();//编码器测距
  122.             if(XK.Encoder_distance>1)//行进距离
  123.             {
  124.                 Distance_L=0;
  125.                 Distance_R=0;
  126.                 XK.time=0;
  127.                 XK.Encoder_distance=0;
  128.                 XK.barrier_in=6;
  129.                 Gyro_qin0();
  130.             }
  131.         }
  132.         break;
  133.         case 6://打角
  134.         {
  135.            
  136.             if(XK.dash_dir==1)//左边避障
  137.             {
  138.               if(eulerAngle.yaw>5.0)//左航向角
  139.               {
  140.                 Distance_L=0;
  141.                 Distance_R=0;
  142.                 XK.time=0;
  143.                 XK.Encoder_distance=0;
  144.                 XK.barrier_in=7;
  145.                 Gyro_qin0();
  146.               }
  147.             }
  148.             if(XK.dash_dir==0)//右边避障
  149.             {
  150.               if(eulerAngle.yaw<-5.9)//右航向角
  151.               {
  152.                 Distance_L=0;
  153.                 Distance_R=0;
  154.                 XK.time=0;
  155.                 XK.Encoder_distance=0;
  156.                 XK.barrier_in=7;
  157.                 Gyro_qin0();
  158.               }
  159.             }
  160.         }
  161.         break;
  162.         case 7://直行，回赛道
  163.         {
  164.             extern float AD[6];
  165.             distence_count();//编码器测距
  166.             if(XK.dash_dir==1)
  167.             {
  168.               if((mt9v03x_image[110][140]+mt9v03x_image[110][141]+mt9v03x_image[110] 
  169.                   [139])/3>100)//图像灰度像素大于某阈值（白色赛道）
  170.  
  171.               {
  172.                 Distance_L=0;
  173.                 Distance_R=0;
  174.                 XK.time=0;
  175.                 XK.Encoder_distance=0;
  176.                 XK.barrier_in=0;
  177.                 XK.barrier_judge=0;
  178.                
  179.               }
  180.             }
  181.             if(XK.dash_dir==0)
  182.             {
  183.               if((mt9v03x_image[110][48]+mt9v03x_image[110][49]+mt9v03x_image[110] 
  184.               [50])/3>100)//图像灰度像素大于某阈值（白色赛道）
  185.  
  186.               {
  187.                 Distance_L=0;
  188.                 Distance_R=0;
  189.                 XK.time=0;
  190.                 XK.Encoder_distance=0;
  191.                 XK.barrier_in=0;
  192.                 XK.barrier_judge=0;
  193.                
  194.               }
  195.             }
  196.         }
  197.         break; 
  198. 	  }
  199. 	}
  200. }

本文旨在本校智能车实验室教学，若有错漏，欢迎指正

声明：本文内容由网友自发贡献，不代表【wpsshop博客】立场，版权归原作者所有，本站不承担相应法律责任。如您发现有侵权的内容，请联系我们。转载请注明出处：https://www.wpsshop.cn/w/菜鸟追梦旅行/article/detail/420585
推荐阅读
相关标签

Copyright © 2003-2013 www.wpsshop.cn 版权所有，并保留所有权利。

  

闽ICP备14008679号