【S072】基于多传感器的自主移动巡检车
作品说明 |
作者:刘总天 刘鑫宇 勇胜浩 候栋舰
单位:北京工业大学
指导老师:康存锋 郑学科
1. 作品简介
1.1 作品介绍
本项目基于多传感器融合技术,采用 Arduino 控制器设计了一款全自主移动巡检车, 实现了自动循迹、翻越障碍、颜色识别、烟雾报警等功能。
首先,循迹策略方面采用三个灰度传感器实现对车身与黑线的相对位置判断以及记录横置黑线数目的功能。基于场地灯光情况,将传感器返回的模拟量进行阈值划分,最终转化为数字量以实现判断 黑线功能;由于控制器本身仅有两个连接电机的引脚,设计了一拖三线电路,分别控制左右侧电机,实现机器人六驱行驶,再配合电机pid算法,根据调试需要预设 P(比例参数)、I(积分参数)、D(微分参数)的初始值,结合灰度传感器所判别的6种车身相对黑线的位置关系, 利用有限状态机分别执行不同的车身修正策略, 并通过PWM 方波调节占空比,实现电机不同转速运行,三者共同作用,使机器人沿黑线稳定并快速行驶。
其次,颜色识别方面将舵机与颜色传感器通过机构连接,实现舵机控制颜色传感器转动至颜色识别区域进行颜色获取与判断。同时在获取颜色为红色以后,烟雾报警器如果再识别到烟雾的话开启报警功能。
最后,结构方面采用被动适应性底盘,仿照类月球车底盘设计两前轮与车身底盘间采用铰链连接以适应复杂路面,且前轮为履带式大直径轮,有效提升上坡及穿越障碍的成功率。
1.2 整体方案设计思路
在场景设计上,我们构造了窄桥台阶和火花识别,爬窄桥和台阶是在巡检过程中必不可少的一部分。
作品说明
虚拟场景图
本项目制作的基于多传感器的自主移动巡检车,在启动后可以自动行驶并通过二种障碍物:楼梯、窄桥。在行进途中,合理利用黑色的引导线进行相应的运动与调整。之后通过舵机带动的颜色传感器和针,先进行颜色识别,可应对可能出现的地面不平整、光线的变化等问题。车体整体结构如下图所示:
车体结构图
车体实物图
相关电子元件:本小车包括Basra主控板、Bigfish 扩展版、颜色传感器、伺服电机、灰度传感器、烟雾报警装置等等。
控制系统架构图如下所示:
控制系统架构图
2. 机械结构的设计思路及创新点
2.1 履带式大前轮设计
轮胎材料的选择与结构方面:
由于使用普通橡胶轮胎翻越台阶存在动力不足的情况,所以我们采用履带和橡胶轮胎相结合,利用履带适应性强、大齿轮高度可高于台阶高度一倍的特点,同时利用橡胶轮胎摩擦力大的特点,克服履带摩擦力小,易打滑的缺点,以确保动力充足,使得在前轮上台阶之后轻松地将后半部分结构抬起从而实现平稳上台阶。
当前轮遇到台阶时,由于台阶的阻挡和转速的降低,前轮轮胎表面的履带在平地上受到的摩擦力较小,自主移动全地形车即可进行矫正保证直线上台阶和窄桥。由于在翻越台阶障碍时,车身须实现利用摩擦力使轮胎垂直上升,而轮径过小会导致车轮在登上台阶平面前无法有效借助到台阶平面上的支持力, 且车轮与台阶侧面的摩擦力无法克服重力使小车上升,而止步于此,通过增大车轮半径可以有效防止这一现象产生, 如下图所示:
履带式大前轮
并且相比于橡胶轮胎,履带式轮胎上的横纹更深, 对于发泡 eva 类较软材质可以在上坡时嵌入其内,更进一步增加车轮抓地力、摩擦力,有助于上台阶, 且由于台阶边框处为小半径圆角,当前轮于台阶圆角发生无滑滚动时速度方向既有垂直向上分量,也有前进分量, 二者均对小车上台阶有利,如下图所示:
大轮径上坡时车轮与台阶受力图
2.2 类月球车底盘
2.2.1 结构设计
我们为此次大赛设计出了一种铰链悬挂机构,类似于月球车的构造,后面四个轮子的连接采用的是月球车模型的原理--被动铰链式悬挂底盘。可简单理解为底盘因地形的变化使底盘结构被动变形适应地形的变化,这样可以尽量保证小车的每一个轮都不会悬空,这样就要求每个轮之间要相对活动,所以我们在螺栓外面加了光滑的套筒,再结合杠杆结构来实现此项功能。如下图所示,可以在崎岖路面上行驶时最大限度保持车轮与地面接触,如此可以保证车体驱动力充足,有利于驶过复杂路面。
类月球车底盘
而本底盘在过窄桥、上斜坡时也同样能使小车紧贴地面行驶,如图下图所示,当前轮登上台阶时,后两轮仍有效提供动力。
(a)前轮上第一节台阶
(b)前轮上第二节台阶
(c)前轮下第二节台阶
(d)前轮下第一节台阶
全地形车过台阶
小车底盘机构简图
如此设计可有效避免非被动适应性底盘在前轮登上台阶时中轮空转现象,从而保证车体前进的稳定性,如下图所示:
非被动适应性底盘中轮空转
经过多次实验,我们发现在两大前轮轮距较小,其后四个小轮轮距较大时,对车轮微扰导致的车身倾斜有机械修正作用,从而防止车轮微小偏离,在缺少修正情况下出现大幅驶离黑线位置偏航的结果。分析得出,在两大前轮发生偏向时,后四轮由于仍处于原行驶方向而对前轮产生使其转回原行驶线路的力矩,而这一现象随着减小后四轮轮距越发不明显,故我们最终设计采用如此车身结构以保证其稳定性。最终的轮距配比如下图所示:
轮距配比
2.2.2 Admas仿真
我们还利用Admas对类月球车结构底盘进行了仿真模拟,如下图所示,测算运动中各连杆之间的角度变化值,从而测算出中轮距离台阶的高度,进而我们可发现中轮是可以始终贴紧台阶行驶的。
仿真模拟图
通过观察下图所示的数据,我们看可以发现,连杆 2 和 3 同连杆 1 之前夹角大小的最大值与最小值之差都大约等于60度。因为后轮始终是位于最低处,通过测算可以得出前轮与最低处的高度差在0-7cm之间,中轮与最低处的高度差也是在0-5cm之间。比赛使用的台阶第一节5cm,最高处10cm,我们的小车先是前轮和中轮上到第一节台阶,之后前轮上到最高处,后轮上至第一节台阶。
这样的上台阶方式,可以让全地形车的前轮或者后轮同中轮在同一节台阶上,因为中轮所在位置是车体的质心所在位置,本设计保证中轮始终接触地面,同时有辅助支撑的轮子,从而使得稳定的到达每一节台阶,不易发生侧翻。
连杆 1 和连杆 2 之间的角度随时间变化曲线
连杆 1 和连杆 3 之间的角度随时间变化曲线
2.2.3 车体平衡设计
全地形车的重心在不同的位置会影响左右两侧车轮受到的阻力,从而使得转速不同。所以我们尽量将车上传感器、伺服马达等零件的安装对称分布,如下图所示:
装配图
左视图
右视图
所有的零件以及控制面板的中心线基本与车体的中心线重合,同时质量较大的电池与伺服马达前后对称,以实现质量尽量分布均匀,质心在车的中央,如下图所示:
重心相对位置图
2.3 颜色识别和烟雾报警
在构思颜色识别时我们希望更贴近本次大赛的主题,即该部分所模拟的是检测火花情景。而实际上火花出现的实现也会伴有烟雾,火花不会整齐的排列在车的左侧,以确定的间距明晰的特点出现,想到这一点,我们力求尽可能使识别颜色模块识别的范围或视角的角度更大,于是采用将传感器安装在舵机的特定结构上,舵机带动传感器定轴转动,理论上实现 360°无死角判别颜色的效果,如此虽牺牲了识别时间,但是换来了对真实情况的模拟,同时为节省模块、实现最大化利用现有条件的理念,我们设计舵机带动颜色模块识别的同时,也在另一端增加了烟雾报警传感器,实现一个舵机完成两项任务的最大化利用效果,具体机构如下图所示:
颜色烟雾识别机构
2.4 关键零件
颜色烟雾识别装置示意图
前轮(驱动轮)示意图
中轮(驱动轮)示意图
后轮(驱动轮)示意图
类月球车底盘
3. 示例程序
#include <Servo.h> #include <FlexiTimer2.h> #include <Wire.h> #include <LiquidCrystal_I2C.h>
LiquidCrystal_I2C mylcd(0x27,16,2);
//颜色识别 #define S0 3 #define S1 4
#define S2 7//S2和S3的组合决定让红、绿、蓝,哪种光线通过滤波器 #define S3 8 #define OUT 2//颜色传感器输出信号连接到Arduino中断引脚,并引发脉冲信号中断 #define LED 13//控制颜色传感器是否点亮LED
int g_count = 0;// 计算与反射光强相对应颜色传感器输出信号的脉冲数 int g_array[3];// 数组用于存储在1s内输出信号的脉冲数 int g_flag = 0;// 滤波器模式选择顺序标志 int c_state = 0;//颜色状态,绿球为0,红色为1,蓝色为2 int Interrupt_time =60;//一次识别的时间 int h=0;//颜色储存初状态值 int e=1;//颜色储存终状态值 //寻迹 int sensor[3] = {0, 0, 0};//存储A0A2A3 float max = 3.85;//误差最大值基数为60的 float s = 255; float Kp = 110, Ki = 15, Kd = 60; //PID控制算法 float error = 0, P = 0, I = 0, D = 0, PID_value = 0;//P比例I积分D微分 float previous_error = 0, previous_I = 0;//微分和积分常量 //int initial_motor_speed = 255;//电机速度基数 第一次调 kp=125 int initial_motor_speed = 200;//电机速度基数 第二次调 kp=100 //舵机 int servoPin =12;// 定义Servo对象来控制 int pos = 0;// 角度存储变量0 int poss =180;// 角度存储变量180
//跳变 int c = 0;//记三个灰度传感器变黑次数 int x=0; int before=0;//上一状态值 int button=0;//现在的值
void setup()//初始化 { pinMode(5,OUTPUT);//电机 pinMode(6,OUTPUT); pinMode(9,OUTPUT); pinMode(10,OUTPUT); pinMode(A0,INPUT);//传感 pinMode(A2,INPUT); pinMode(A3,INPUT); pinMode(S0, OUTPUT);//颜色 pinMode(S1, OUTPUT); pinMode(S2, OUTPUT); pinMode(S3, OUTPUT); pinMode(OUT, INPUT); pinMode(LED, OUTPUT);mylcd.init(); mylcd.backlight(); pinMode(2, OUTPUT);
digitalWrite(S0, HIGH);//颜色 digitalWrite(S1, HIGH); attachInterrupt(0, Count, RISING); Serial.begin(9600); delay(100); }
void read_sensor_values()//模拟灰度识别 { sensor[0] = analogRead(A0);//读取模拟量的值 sensor[1] = analogRead(A2); sensor[2] = analogRead(A3); if (sensor[0] < 130)//参数根据实际需要量修改 { sensor[0] = 1;//转化为数字量 } else { sensor[0] = 0; } if (sensor[1] < 130) { sensor[1] = 1; } else { sensor[1] = 0; } if (sensor[2] < 130) { sensor[2] = 1; } else { sensor[2] = 0; }
if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 1))//右转 { error = -2;//需要大角度修正 } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 1)) { error = -1;//需要小角度修正 } else if ((sensor[0] == 0) && (sensor[1] == 1) && (sensor[2] == 0)) { error = 0;//直行 } else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 0))//左转 { error = 1;//需要小角度修正 } else if ((sensor[0] == 1) && (sensor[1] == 0) && (sensor[2] == 0)) { error = 2;//需要大角度修正 } else if ((sensor[0] == 0) && (sensor[1] == 0) && (sensor[2] == 0))//跑上一状态 { if (error > 0) { error = max; } else { error = -max;
} } else if ((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1)) { if ((error > 0) && (previous_error > 0)) { error = 0; //c++; } else if ((error < 0) && (previous_error < 0)) { error = 0; //c++; } } }
void calculate_pid()//pid算法 { P = error; I = I + previous_I; D = error - previous_error;
PID_value = (Kp * P) + (Ki * I) + (Kd * D);//PID算式 Serial.println(PID_value);
previous_I = I; previous_error = error; }
void motor_control() { // 计算有效电机转速: int left_motor_speed = initial_motor_speed - PID_value;//左电机速度 int right_motor_speed = initial_motor_speed + PID_value;//右电机速度
left_motor_speed = constrain(left_motor_speed, -s, s);//设定范围 right_motor_speed = constrain(right_motor_speed, -s, s);
//run(left_motor_speed, right_motor_speed);
if((error>=-2)&&(error<=2)) { run(left_motor_speed, right_motor_speed); } else if(error<-2) { //error = 0; sright(220); } else { //error = 0; sleft(220); } }
void run(float Speed1,float Speed2)//前进 { if(Speed1>0) { analogWrite(9, Speed1); //左 analogWrite(10, 0); } else { analogWrite(9,0); //左 analogWrite(10,abs(Speed1) ); } if(Speed2>0) { analogWrite(5, Speed2); //右 analogWrite(6, 0); } else { analogWrite(5, 0); //右 analogWrite(6, abs(Speed2)); } }
void stop() //刹车 { analogWrite(5,200); analogWrite(6,200); analogWrite(9,200); analogWrite(10,200); }
void js() //加速 { analogWrite(5,200); analogWrite(6,0); analogWrite(9,200); analogWrite(10,0); }
void sleft(float Speed)//左转 {
analogWrite(9, 0); analogWrite(10, Speed); analogWrite(5, Speed); analogWrite(6, 0); }
void sright(float Speed)//右转 { analogWrite(9, Speed); analogWrite(10, 0); analogWrite(5, 0); analogWrite(6,Speed); }
//颜色 //选择滤波器模式,决定让红、绿、蓝,哪种光线通过滤波器 void FilterColor(int Level01, int Level02) { if(Level01 != 0) Level01 = HIGH; if(Level02 != 0) Level02 = HIGH; digitalWrite(S2, Level01); digitalWrite(S3, Level02); //delay(1000); }
//中断函数,计算TCS3200输出信号的脉冲数 void Count() { g_count ++ ; } void Callback() { switch(g_flag) { case 0: Serial.println("正在颜色识别"); WB(LOW, LOW);//选择让红色光线通过滤波器的模式 break; default: g_count = 0;//计数值清零 break; } }
//设置反射光中红、绿、蓝三色光分别通过滤波器时如何处理数据的标志 void WB(int Level0, int Level1) { g_count = 0; //计数值清零 g_flag ++; //输出信号计数标志 FilterColor(Level0, Level1); //滤波器模式 }
void get_color() { int turns = 1; int red_num =0; int gre_num =0; int blu_num =0; digitalWrite(LED, HIGH); FlexiTimer2::set(Interrupt_time/4,Callback); FlexiTimer2::start();//定时器中断开 for(int i =1;i<=turns;i++) { g_flag = 0; delay(Interrupt_time);//一次识别时间为Interrupt_time if((g_array[0] > g_array[1]) && (g_array[0] > g_array[2])) { red_num++; } else if((g_array[2] > g_array[1]) && (g_array[2] > g_array[0]) ) { blu_num++; } else { gre_num++; } } FlexiTimer2::stop();//定时器中断关 if(red_num>blu_num && red_num>gre_num) { Serial.println("红色小球 "); c_state = 1; } digitalWrite(LED, LOW); }
//跳变计数 void jishu()//记录跳变为 { if((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1)) { delay(20); if((sensor[0] == 1) && (sensor[1] == 1) && (sensor[2] == 1)) { x=1; } } else { x=0; } button=x; if(button==1 && before==0) { c++; } if(button!=before) { delay(20); } before=button; } void delayTracing(int time)//毫秒,短暂寻迹 { int m; for(m=0;m<time;m++) { read_sensor_values();//寻迹 calculate_pid(); motor_control(); delay(10); } }
void qianjin(int time)//毫秒,上台阶无巡线直行 { int m; for(m=0;m<time;m++) { js(); delay(10); } }
void judge()//颜色判断 { int n,m; for(n=0;n<20;n++) { stop();//停车 delay(20); get_color();//第二次获得颜色 e=c_state;//获取第二次颜色
}
}
void compare(int letter)//颜色获取比较 { int d,f,g; d=0; f=0; g=0; if(c_state==0) { d++; } else if(c_state==1) { f++; } else { g++; } if(d>f&&d>g) { c_state=0; } else if(f>d&&f>g) { c_state=1; } else { c_state=2; } }
void yanse()//颜色识别扎气球 { int m; int i,j,k; if(c==5)//三个寻迹传感器全识别到黑 { for(i=0;i<20;i++) { stop();//停车 delay(10); get_color();//获得颜色 h=c_state;//获取初始颜色 Serial.print(h);
} for(j=0;j<50;j++) { read_sensor_values();//寻迹 calculate_pid(); motor_control(); delay(10); } for(k=0;k<3;k++) { judge();//颜色判断 Serial.println(e); delayTracing(60); } c++;//跳出本次程序 }
} void taijie() { if(c==3) { qianjin(370); c++; } }
void zhongdiansotp() { if(c==7) { // delayTracing(250); qianjin(220); while(1) { stop();//延迟后刹车 } } }
void loop() { read_sensor_values();//寻迹 calculate_pid(); motor_control(); jishu(); taijie(); mylcd.setCursor(1-1, 1-1); mylcd.print("SMOKE:"); mylcd.setCursor(8-1, 1-1); mylcd.print(analogRead(A0)); if (analogRead(A0) > 450) { digitalWrite(2,LOW);
} if (analogRead(A0) <= 450) { digitalWrite(2,HIGH); zhongdiansotp(); //Serial.println(PID_value); //Serial.print(sensor[0]); Serial.print(h); Serial.println(e); Serial.println(c); delay(10);
} |
* 本项目未获得作者开源授权,无法提供资料下载。
|