本帖最后由 L184463781 于 2019-7-7 17:17 编辑
谈谈循迹算法的设计与实现林宏伟       2019/7/7    2019全国机器人大赛





                          随着5月的离开，全国机器人比赛也圆满结束了。本人有幸和我的小伙伴们参加了今年的全国机器人大赛，也斩获了不错的佳绩。好的，废话不多说，现在我们来谈谈循迹算法的设计与实现。                          说到循迹，网上有很多现成的算法和模块可以使用，但是可移植性太差，也就是说一套算法无法高效移植到其他比赛场地上去（确实是挺麻烦的）。尤其是在结构设计以及传感器选取上有很大差别。我一般会选取灰度传感器作为循迹模块，因为它对场地的光干扰有很大的‘抵抗性’（也就是不会被光干扰太多）。普通传感器一旦遇到较大光差别时候会大幅度减低自身的灵敏性，这样对黑带识别就存在很大干扰了。而且并且在路线识别过程中灰度传感器对其他颜 {MOD}也有较大的识别性，在RGB {MOD}带上，红外传感器识别紫 {MOD}和黑 {MOD}几乎是一模一样，也就是它会认为紫 {MOD}就是黑 {MOD}（别问为啥，说多了都是泪），所以在存在颜 {MOD}干扰以及路线复杂的比赛场地上建议 建议 建议采用灰度传感器（后期可以解决场地问题！！！）                          现附下图，就此图讲解循迹算法规划：
                          此图的特点就是路线复杂（迷宫式）和颜 {MOD}干扰大。简单讲述规则：小车从绿 {MOD}三角2区出发，到达存放货物的黄蓝红储存区后将货物移到物料相对应的配送区。                          思路如下：                          1.确定路线                          2.灰度传感器调试                          3.车体结构设计                          4.算法实现

                          1.确定路线：经过我们小组的不断测试，最后确定出一条如下的一条路线，选取此路线的原则就是：走长直线、避短转弯、靠近配送区                               
                          2.灰度传感器调试：传感器的调试十分简单，就躺在地上，慢慢调节电位器就行了（我队友调了两个星期。。。。。）                          3.车体结构设计：车体结构设计就靠我另外一位队友了（结构图不方便分享了，毕竟是‘机密’）

                          4.算法实现：此循迹算法经过我蹲在实验室一楼一个月，已经研究出了一套移植性较高的算法。通过路线图，此算法包括：循迹、转弯、标记记忆、脱线等部分（下文详细解说此部分）                               ①循迹最基本的要求就是车可以循迹且不脱离循迹轨迹，由于我们的结构是普通的两轮结构，这也存在了打滑、重心不稳的大问题，也就是说我们的车走直线都有点问题，解决的办法就是程序解决，也就是利用程序将车体打滑的轨迹强行纠正。大致思路就是利用前头3-6路传感器检测，车体偏移时传感器会检测到，检测到后就做出纠正，左偏就右纠正，右偏就左纠正，当然，纠正的程序需要增加一些判断机制，这样可以提高车的稳定度，不容易出现纠正过大导致出线的情况。纠正的车速也需要调节，不易过小，亲测是纠正速度大一点，这样比较有效，过小的话容易出现无法纠正的情况。建议采用是‘同向差速纠正’，最好不要用‘反向差速纠正’，同向纠正就可以保证车体运动轨迹一直保持向前，即使纠正过头也不会出现大幅度转弯。

1. //纠正模式一（正头）                        
   
2.                                 
   
3.                         if(FA4==1&&FA3==0&&FA2==0)        //往右偏        
   
4.                         {
   
5.                                 motor1 = 0;
   
6.                                 motor2 =-5000;
   
7.                                 motor3 =-3700;
   
8.                         }
   
9.                         if( FA4==0&&FA3==0&&FA2==1 )                        //往左偏
   
10.                         {
    
11.                                 motor1 = 0;
    
12.                                 motor2 =-3700;
    
13.                                 motor3 =-5000;
    
14.                         }
    
15.                         if(FA4==0&&FA3==1&&FA2==0)
    
16.                         {
    
17.                                 motor1 = 0;
    
18.                                 motor2 =-3400;
    
19.                                 motor3 =-3400;
    
20.                         }
    
21.                         Set_Pwm(motor1,motor2,motor3);
    
22.                         motor1 = 0;

复制代码                               ②ok,第二部分就是标记路线了，所谓标记路线就是对一些路口进行标记，达到记忆的效果。经过我趴地研究，用局部路口标记的方式和特殊路口标记效率最大。所谓局部标记就是省略一些路口，不进行标记，特殊标记就是对T型，倒T等路口进行标记。标记如下图所示：


                          大家看标记图和路线图，可以发现我把一些路口省略掉了。附上从绿 {MOD}区域出发到达标记1的代码。从代码上可以发现我用了二次检测的原理，也就是吧、判断了两次，嗯，这样可以提高系统的稳定性和判断的严谨性。首先看第一行 FC14 == 0 && FA5 == 1 && FA2 == 0 && FA3 == 0 && FA4 == 0 && Flag == 0，这个是运动到标记1位置，然后FA5 == 1 && FA3 == 0 && FC14 == 0二次判断，然后控制电机差速转向，注意time = 1这个语句，它起到很大作用，也就是保证转弯时车不会脱线，做过差速转弯的循迹小车的同学应该都知道，差速转弯十分不稳定，很容易脱线，基本要靠delay函数延迟（但是不好）加入第二个标记位判断可以保证车体脱线时硬生生“掰正”回来，这就是双标记位的使用，十分有效

1. ////顶角转弯直走1    黄 {MOD}圈
   
2.                         if(FC14 == 0 && FA5 == 1 && FA2 == 0 && FA3 == 0 && FA4 == 0 && Flag == 0)
   
3.                         {
   
4.                                 delay_ms(10);
   
5.                                 if(FA5 == 1 && FA3 == 0 && FC14 == 0)
   
6.                                 {
   
7.                                         motor1 = 0;
   
8.                                         motor2 = 4800;
   
9.                                         motor3 = -4500;
   
10.                                         Set_Pwm(motor1,motor2,motor3);
    
11. 
    
12.                                         time = 1;
    
13.                                 }
    
14.                         }       

复制代码                               ③从黄 {MOD}区域到红 {MOD}区域，标记位2，从黄区-->红区，会经过几个路口，我采用忽略标记，不进行标记，大家仔细看程序会发现Flag和 time两个标记位在不断转换，它的作用就是上面提到的对差速转弯的打滑车进行强行掰正，这个是我们小组经过不断努力（掉了好多头发研究出来 的循迹模式-->硬掰模式）大家主要看深蓝 {MOD}的代码那里，其他代码是机械臂抓物块以及停止的，可以不看，贴出来是给大家更加直观地知道整个执行流程以及两个标记位的互换。一般红外循迹小车都是利用delay函数来差速转弯，这个虽然可以，但是一旦车打滑就会出现转弯过大或者过小的情况，这样会影响后续的标记，十分麻烦（亲测不可行），但是利用两个标记位切换的话就可以实现转弯即使过大过小都可以硬生生掰会循迹路线，100%稳定且可行。

1. //黄 {MOD}口停                       
   
2.                         if(FC14 == 0 && FA5 == 1 && FA3 == 1 && time == 1)
   
3.                         {
   
4. <font color="red">                                delay_ms(20);
   
5.                                 if(FC14 == 0 && FA5 == 1)
   
6.                                 {
   
7. <font color="magenta">                                        Flag = 30;</font>
   
8.                                         motor1 = 0;
   
9.                                         motor2 = 0;
   
10.                                         motor3 = 0;
    
11.                                         Set_Pwm(motor1,motor2,motor3);
    
12.                                        
    
13.                                         //0号盘下去
    
14.                                         //舵机归零
    
15. 
    
16.                                                                                
    
17.                                         motor2 = -3700;
    
18.                                   motor3 = -2000;
    
19.                                         Set_Pwm(motor1,motor2,motor3);
    
20.                                         delay_ms(200);
    
21.                                         </font><font color="black">time = 2;</font>
    
22.                                 }                                                               
    
23.                 }
    
24.                        
    
25.                
    
26. //蓝 {MOD}口停
    
27.                 if(FC14 == 1  && FA5 == 0 && time == 2)
    
28.                         {
    
29.                                 delay_ms(20);
    
30.                                 if(FC14 == 1&&FA5 == 0)
    
31.                                 {
    
32. <font color="magenta">                                        Flag = 31;</font>
    
33.                                         motor1 = 0;
    
34.                                         motor2 = 0;
    
35.                                         motor3 = 0;
    
36.                                         Set_Pwm(motor1,motor2,motor3);
    
37.                                        
    
38. 
    
39.                                         motor2 = -3700;
    
40.                                   motor3 = -2000;
    
41.                                         Set_Pwm(motor1,motor2,motor3);
    
42.                                         delay_ms(100);
    
43.                                         time = 3;
    
44.                                 }
    
45.                         }
    
46.                        
    
47. <font color="navy">//红 {MOD}转弯               
    
48.                                 if(FC14 == 0 && FA5 == 1 && FA2 == 0 && FA3 == 0 && FA4 == 0 && Flag == 31)
    
49.                                 {
    
50.                                         delay_ms(10);
    
51.                                         if(FA5 == 1 && FA3 == 0 && FC14 == 0)
    
52.                                         {
    
53.                                                 motor1 = 0;
    
54.                                                 motor2 = 4800;
    
55.                                                 motor3 = -4500;
    
56.                                                 Set_Pwm(motor1,motor2,motor3);
    
57. 
    
58.                                                 time = 4;
    
59.                                         }
    
60.                                 }        </font>
    
61.                                
    
62.                                        
    
63. //红 {MOD}角停止       
    
64.                         if(FC14 == 0 && FA5 == 1 && (FA3 == 1 || FA2 == 1 || FA4 == 1) && time == 4)
    
65.                         {
    
66.                                 delay_ms(10);
    
67.                                 if(FC14 == 0 && FA5 == 1)
    
68.                                 {
    
69. //                                                Flag = 33;
    
70.                                                 motor1 = 0;
    
71.                                                 motor2 = 0;
    
72.                                                 motor3 = 0;
    
73.                                                 Set_Pwm(motor1,motor2,motor3);
    
74.                                                 delay_ms(300);
    
75.                        
    
76.                                         Flag = 33;
    
77. //                                                time =  6;                               
    
78.                         }       
    
79.                 }

复制代码                              ④标记4--->标记9思路和上面一模一样，仅仅区别在一些标记延迟和标记判断上，区别不大，主要是2个标记位的切换需要技巧切换，不然车就会“迷路”，或者一直转圈圈，这里主要说下“脱线”的方法，如果觉得整段路一直循迹很无聊。那咋们就可以考虑下“脱线”，脱线，顾名思义就是脱离循迹路线一段时间后继续回到循迹路线，继续循迹。这个应用在多段短路线转弯口十分有用，因为多路的短转弯口如果还是照以前乖乖循迹就真的有点笨笨了，这样标记起来麻烦又容易出现问题，那还不如考虑脱线到稳定直线或者长转弯口再继续循迹（以前就是照旧没有脱线，然后出现很多问题，差不多我们小组都奔溃，辛亏用了脱线的技巧）现在咋们来看标记16->标记17这段路，考虑是短转弯，我们就直接脱线了，因为前面标记中已经进行了很多次双标记切换，最后为了保证标记正确我直接选择脱线。脱线的算法思路就是到达转弯口先小转一点角度后直线冲出线外，然后利用双标记位进行脱线回正，这个大家在制作循迹加小车过程中都会接触到的，什么标记位，循迹的相关概念。

1. //回家
   
2.                                 if(FC14 == 1 && FA5 == 0 && FA2 == 0 && FA3 == 0 && FA4 == 0 && Flag == 44)
   
3.                                 {
   
4.                                         delay_ms(10);
   
5.                                         if(FC14 == 1 && FA5 == 0)
   
6.                                         {
   
7.                                                         time = 14;//清除标记位
   
8.                                                         motor2 = -5500;
   
9.                                                         motor3 =  2000;
   
10.                                                         Set_Pwm(motor1,motor2,motor3);
    
11.                                                         delay_ms(400);
    
12.                                                         motor2 = -3700;
    
13.                                                         motor3 = -3500;
    
14.                                                         Set_Pwm(motor1,motor2,motor3);       
    
15.                                                         delay_ms(300);
    
16.                                                         Flag = 45;
    
17.                                         }
    
18.                                 }
    
19.                                 //硬掰回来
    
20.                         if(FA5 == 1 && FA2 == 0 && FA3 == 0 && FA4 == 0 && Flag == 45)
    
21.                         {
    
22.                                         delay_ms(10);
    
23.                                         if(FA5 == 1 && FA3 == 0)
    
24.                                         {
    
25.                                                         motor2 =  4800;
    
26.                                                         motor3 = -4500;
    
27.                                                         Set_Pwm(motor1,motor2,motor3);
    
28.                                                         time = 15;
    
29. 
    
30.                                         }
    
31.                         }

复制代码                         END
                          结语：感谢数据结构算法的陈老师叫我去参加机器人比赛，让我有机会接触其他学校优秀的小伙伴；感谢我小组的成员，设计结构的锋哥以及调了几个星期灵敏度的姐，还要感谢同个比赛项目的其他小组成员，我永远记得咋们那时候一起熬夜，一大早一起出去吃早餐还有一起请假一周去调试（@那个改结构改到奔溃的啊仓，还有那个熬2夜但是最后没有上场的东哥，还有熬夜帮忙打印的一堆小师弟，和鼓舞我熬夜的锦哥，哈哈哈）这是我在大学一段难以忘怀的经历，谢谢大家