实验 5 自主移动机器人避障与运动控制实验
一、实验目的
通过驱动轮式机器人行驶和障碍物检测，掌握机器人常见传感器红外光电和超声波传感器的原理，掌握机器人运动结构的组成及执行机构如直流电机，舵机的用法。
二、实验内容
1. 树莓派的基本配置
熟悉linux下常用指令，如将树莓派配置为路由器模式，SSID为“USTC_GroupX”,其中 X 为组号，比如第 1 组为 USTC_Group01，密码为 12345678，将该配置添加到开机自启动项 rc.local 中；
2. GPIO 编程练习
选择shell编程，wiringPi或Python3三种方式之一控制小车前部探照灯亮灭；
3. 电机驱动控制
轮式机器人的基本行驶，编程实现小车直线前行、后退和左右转弯行驶；
4. 红外光电传感器的应用
能监测到车前方不小于50mm的障碍物并停车、后退若干距离后转向行驶；
5. 舵机和超声波传感器的应用
能监测到车前方不小于100mm的障碍物并停车，在左、右和前3个方向判断有无障碍物后，按照向量直方图法，选择开阔方向继续行驶，并通过指定赛道。
三、实验设备
基于ARM11内核处理器的树莓派主板，4 轮全驱移动机器人底盘，linux 操作系统
四、实验原理
1. 红外避障原理
红外传感器具有一对红外发射与接收管，发射管发射出一定频率的红外线，当检测方向遇到障碍物时，发射出去的红外线反射回来被接收管接收。它常用于安装在小车上，判断前方是否有障碍物。可通过红外传感器模块上的电位器设置红外避障的距离。
红外传感器避障的基本原理是利用物体的反射性质，在一定范围内，如果没有障碍物，发射出去的红外线，因为传播的距离越来越远而逐渐减弱，最后消失。如果有障碍物，红外线遇到障碍物，会被被反射到达传感器接收头。我们本次实验采用的是两路红外传感器分别连接在树莓派主控板上的wiringPi编码的26，0口上。我们通过检测两端口的电平来判断障碍物的情况，并做出相应的避障动作。
图1 树莓派主控板
图2 2路红外避障模块
2. 超声波避障原理
超声波模块是利用超声波特性检测距离的传感器。其带有两个超声波探头，分别用作发射和接收超声波。其测量的范围是3-450cm。
图3 超声波模块
图4 超声波发射和接收示意图
图5 超声波模块引脚
该模块的工作原理：
先使用树莓派的wiringPi编码引脚31向TRIG脚输入至少10us的高电平信号,触发超声波模块的测距功能，如下图所示：
图6 超声波模块发射触发信号
测距功能触发后，模块将自动发出8个40kHz的超声波脉冲，并自动检测是否有信号返回，这一步由模块内部自动完成。
图7 超声波脉冲
一旦检测到有回波信号则ECHO引脚会输出高电平。高电平持续的时间就是超声波从发射到返回的时间。此时可以使用时间函数计算出echo引脚高电平的持续时间，即可计算出距被测物体的实际距离。公式: 距离=高电平时间*声速(340M/S)/2。
五、实验代码分析
首先对引脚进行定义，定义电机引脚、按键引脚、超声波引脚、LED灯的引脚、舵机的引脚以及红外传感器的引脚，并初始化舵机的位置向前。
1 //定义引脚 2 int Left_motor_go = 28; //左电机前进AIN2连接Raspberry的wiringPi编码28口 3 int Left_motor_back = 29; //左电机后退AIN1连接Raspberry的wiringPi编码29口 4 int Right_motor_go = 24; //右电机前进BIN2连接Raspberry的wiringPi编码24口 5 int Right_motor_back = 25;//右电机后退BIN1连接Raspberry的wiringPi编码25口 6 int Left_motor_pwm = 27; //左电机控速PWMA连接Raspberry的wiringPi编码27口 7 int Right_motor_pwm = 23; //右电机控速PWMB连接Raspberry的wiringPi编码23口 8 //按键 9 int key = 10; //定义按键为Raspberry的wiringPi编码10口 10 //超声波 11 int EchoPin = 30; //定义回声脚为连接Raspberry的wiringPi编码30口 12 int TrigPin = 31; //定义触发脚为连接Raspberry的wiringPi编码31口 13 //定义引脚 14 int LED_R = 3; //LED_R接在Raspberry上的wiringPi编码3口 15 int LED_G = 2; //LED_G接在Raspberry上的wiringPi编码2口 16 int LED_B = 5; //LED_B接在Raspberry上的wiringPi编码5口 17 //定义舵机引脚 18 int ServoPin = 4; 19 //初始化舵机位置向前 20 int ServoPos = 90; 21 22 const int AvoidSensorLeft = 26; //定义左边避障的红外传感器引脚为wiringPi编码26口 23 const int AvoidSensorRight = 0; //定义右边避障的红外传感器引脚为wiringPi编码0口 24 int LeftSensorValue ; //定义变量来保存红外传感器采集的数据大小 25 int RightSensorValue ;
接下来定义脉冲函数，用来模拟产生PWM值。
1 void servo_pulse(int ServoPin, int myangle) 2 { 3 int PulseWidth; //定义脉宽变量 4 PulseWidth = (myangle * 11) + 0; //将角度转化为500-2480 的脉宽值 5 digitalWrite(ServoPin, HIGH); //将舵机接口电平置高 6 delayMicroseconds(PulseWidth); //延时脉宽值的微秒数 7 digitalWrite(ServoPin, LOW); //将舵机接口电平置低 8 delay(20 - PulseWidth / 1000); //延时周期内剩余时间 9 return; 10 } 11 void servo_appointed_detection(int pos) 12 { 13 int i = 0; 14 for (i = 0; i <= 15; i++) //产生PWM个数，等效延时以保证能转到响应角度 15 { 16 servo_pulse(ServoPin, pos);//模拟产生PWM 17 } 18 }
下面这个是超声波避障函数，控制舵机进行转向，利用超声波测距来实现小车避障行驶。LED灯会根据小车的状态显示相应的颜色。具体的实现过程为：定义三个变量LeftDistance， RightDistance，FrontDistance分别表示左方、右方和前方的距离值。设计避障的方式，当左右和前方的距离都小于30的时候，LED灯显示亮红色，此时小车前方已经没有路了，进行掉头。如果是左侧的距离大于右侧的距离的时候，LED灯显示亮蓝色，小车原地左转。如果右侧的距离大于左侧的距离的时候，LED灯显示亮红色，小车原地右转。
1 void servo_color_carstate() 2 { 3 float distance; 4 //定义舵机位置变量和小车前方,左侧,右侧距离 5 int iServoPos = 0; 6 int LeftDistance = 0; //左方距离值变量LeftDistance 7 int RightDistance = 0; //右方距离值变量RightDistance 8 int FrontDistance = 0; //前方距离值变量FrontDistance 9 corlor_led(ON, OFF, OFF);//开红灯 10 back(80); //避免突然停止,刹不住车 11 brake(); 12 13 //舵机旋转到0度,即右侧,测距 14 servo_appointed_detection(0); 15 delay(500); 16 distance = Distance_test(); //测距 17 RightDistance = distance; //所测的右侧距离赋给变量RightDistance 18 19 //舵机旋转到180度,即左侧,测距 20 servo_appointed_detection(180); 21 delay(500); 22 distance = Distance_test(); //测距 23 LeftDistance = distance;//所测的左侧距离赋给变量LeftDistance 24 25 //舵机旋转到90度,即左侧,测距 26 servo_appointed_detection(90); 27 delay(500); 28 distance = Distance_test(); 29 FrontDistance = distance;//所测前侧距离赋给变量FrontDistance 30 31 if (LeftDistance < 30 && RightDistance < 30 && FrontDistance < 30 ) 32 { 33 //亮品红色,掉头 34 corlor_led(ON, OFF, ON); 35 spin_left(700); 36 } 37 else if ( LeftDistance >= RightDistance) //当发现左侧距离大于右侧，原地左转 38 { 39 //亮蓝色 40 corlor_led(OFF, OFF, ON); 41 spin_left(350); 42 } 43 else if (LeftDistance < RightDistance ) //当发现右侧距离大于左侧，原地右转 44 { 45 //亮品红色,向右转 46 corlor_led(ON, OFF, ON); 47 spin_right(350); 48 } 49 }
接下来是对小车的行驶状态的一些定义，包括前进、停止、向左转、向右转、后退等功能，该部分代码省略。
另外还定义了一个按键函数。
1 void key_scan() 2 { 3 while (digitalRead(key)); //当按键没有被按下一直循环 4 while (!digitalRead(key)) //当按键被按下时 5 { 6 delay(10); //延时10ms 7 if (digitalRead(key) == LOW)//第二次判断按键是否被按下 8 { 9 delay(100); 10 while (!digitalRead(key)); //判断按键是否被松开 11 } 12 } 13 return; 14 }
主函数代码如下。首先对wiringPi进行初始化，并初始化电机驱动IO口为输出方式。对LED灯、按键、超声波模块、舵机和红外传感器进行初始化。调用按键扫描函数，如果检测到按键启动则开始进行避障运动。选用红外避障加超声波避障的模式，在红外避障模式下，如果两侧都没有障碍物的时候，小车前进；如果检测到右边有障碍物，小车原地向左转；如果检测到左边有障碍物，小车原地向右转。在本次实验中，我们可以调整红外避障模块上的电位器来设置红外避障的距离，以达到比较好的实验效果。如果两侧检测到有障碍物的话，我们调用超声波模块来进行避障，超声波避障函数在上面已经介绍过了。
1 void main() 2 { 3 float distance; 4 5 //wiringPi初始化 6 wiringPiSetup(); 7 8 //初始化电机驱动IO口为输出方式 9 pinMode(Left_motor_go, OUTPUT); 10 pinMode(Left_motor_back, OUTPUT); 11 pinMode(Right_motor_go, OUTPUT); 12 pinMode(Right_motor_back, OUTPUT); 13 14 //创建两个软件控制的PWM脚 15 softPwmCreate(Left_motor_pwm,0,255); 16 softPwmCreate(Right_motor_pwm,0,255); 17 18 //初始化RGB三色LED的IO口为输出方式 19 pinMode(LED_R, OUTPUT); 20 pinMode(LED_G, OUTPUT); 21 pinMode(LED_B, OUTPUT); 22 23 //定义按键接口为输出接口 24 pinMode(key, INPUT); 25 26 //初始化超声波引脚 27 pinMode(EchoPin, INPUT); //定义超声波输入脚 28 pinMode(TrigPin, OUTPUT); //定义超声波输出脚 29 30 //舵机初始化为向前 31 servo_appointed_detection(ServoPos); 32 //舵机初始化为输出模式 33 pinMode(ServoPin, OUTPUT); 34 35 //定义左右传感器为输入接口 36 pinMode(AvoidSensorLeft, INPUT); 37 pinMode(AvoidSensorRight, INPUT); 38 39 //调用按键扫描函数 40 key_scan(); 41 42 while(1) 43 { 44 //遇到障碍物,红外避障模块的指示灯亮,端口电平为LOW 45 //未遇到障碍物,红外避障模块的指示灯灭,端口电平为HIGH 46 LeftSensorValue = digitalRead(AvoidSensorLeft); 47 RightSensorValue = digitalRead(AvoidSensorRight); 48 49 if (LeftSensorValue == HIGH && RightSensorValue == HIGH) 50 { 51 run(90,90); //当两侧均未检测到障碍物时调用前进函数 52 } 53 else if (LeftSensorValue == HIGH && RightSensorValue == LOW) 54 { 55 spin_left(45); //右边探测到有障碍物，有信号返回，原地向左转 56 } 57 else if (RightSensorValue == HIGH && LeftSensorValue == LOW) 58 { 59 spin_right(45);//左边探测到有障碍物，有信号返回，原地向右转 60 } 61 else if(RightSensorValue == LOW && LeftSensorValue == LOW) 62 { 63 servo_color_carstate(); //调用超声波避障函数 64 } 65 } 66 return; 67 }
图8 避障主函数流程图
六、实验步骤与结果
（1）启动小车，可以通过WiFi来连接小车，在linux系统下进入树莓派小车的文件里面，通过修改rc.local文件实现小车的自启动。我们小组是第8小组，将树莓派配置为路由器模式，SSID 为“Group08”。
（2）执行小车自带的七彩探照灯的程序，通过wiringPi的方式来控制小车前部探照灯的亮灭。
（3）编程实现小车直线前行、后退和左右转弯行驶。
（4）通过修改小车自带的红外加超声波的避障函数，实现小车行进过程中的自主避障，能够自动检测障碍物并避开障碍物前行，并实现小车通过指定赛道。
图9 小车在指定赛道中运动
七、实验总结
在本次实验中主要采用红外避障模块加超声波避障模块来实现小车的自主避障。在实验的开始，我们首先是通过蓝牙连接来控制小车运动，确保小车是能够正常运行的。刚开始拿到小车的时候电池没电，给电池充电之后才能够启动，在实验的过程中小车电池电量消耗也挺快。首先通过观看树莓派智能小车官网提供的红外避障和超声波避障实验，了解小车避障实验的基本原理。
然后运行树莓派小车自带的红外加超声波避障程序，但是效果并不是很好。于是决定自己对代码进行改写。之前小车自带的代码设计思想比我们的要复杂一些，但是改写之后的代码运行效果更好。在这个过程中参考了同学的代码，对一些参数进行调节，进行测试。最终实现一个较好的效果。不过也还有一些不足之处，小车在赛道中运动时候有时候会靠在赛道边缘前行，很可能是红外避障实现的效果不是很好，还有待改进。