第 6 章 基于视频技术的野外探险机器人
6.4 野外探险机器人的程序实现
6.4.1 上位机程序模块
图6.3 野外探险机器人的系统结构
本系统结构以 AS-UII 为主，在 AS-UII 机器人上安装 AS 无线摄像机和微功率无线通讯 模块。在上位机上安装视频采集卡。AS 无线摄像机的接收模块的视频输出接到视频采集卡 的视频输入端。在上位机上也安装微功率无线通讯模块。上位机的控制信号是通过微功率无 线通讯模块进行传输的。其原理与美国的火星探险机器人“勇气”号类似。它即能自主运动，
也能通过上位机的人机交互方式进行控制。
6.4 野外探险机器人的程序实现
6.4.1
上位机程序模块
上位机主要包括两个程序模块，一个是无线视频的采集和处理模块，另一个模块是人机 交互模块。由 VC 进行编写。界面如图：
视频信号无线通讯 控制信号无线通讯
图 6.4 野外探险机器人的上位机界面 6.4.2
AS-UII 的 JC 程序
/*野外探测*/
int IDLE = 0;
int MODE_SELF =248;
int MODE_REMOTE =249;
int FORWARD =250;
int PAUSE =251;
int BACKWARD =252;
int TURNCW =253;
int TURNCCW =254;
int EXIT =255;
int TRANS = 20;
int ROT = 20;
float fTransTime = 1.5;
float fRotTime = 0.2;
int iRobotStatus = 0;
int iMode = 0;
void main() { int i = 0;
printf("I'm Field Explorer!\n");
wait(2.5);
for(i = 0;i<3;i++) beep();
hold_serial();
iRobotStatus = MODE_REMOTE;
start_process(Execute());
start_process(AutoRun());
Process_Rev();
stop();
for(i = 0;i<2;i++) beep();
share_serial();
}
void Process_Rev() { int a;
int i = 0;
int iId;
printf("Rev...\n");
while(iRobotStatus != EXIT) { a=serial_getchar();
if(a == FORWARD)
iRobotStatus { = FORWARD;
printf("Rev FORWARD\n");
beep();
printf("Rev BACKWARD\n");
beep();
else;
wait(0.1);
} return;
} void Execute() { int iV,iR;
while(iRobotStatus != EXIT)
{ if(iRobotStatus == IDLE || iRobotStatus == PAUSE) { stop();
} void Move(int trans,int rot,float t) { drive(trans,rot);
wait(t);
while(iRobotStatus != EXIT) { if(iMode == 2)
}
}
printf("ir=%d,bump=%d,V=%d\n",iIr,iBump,TRANS);
wait(0.2);
} }
}
第7章 数字 PID 及其算法
7.1 控制方法介绍
计算机控制的主要任务就是设计一个数字调节器，常用以下控制方法。
7.1.1
程序和顺序控制
程序控制是被控量按照一定的、预先规定的时间函数变化，被控量是时间的函数。顺序 控制可以看作是程序控制的扩展，在各个时期所给出的设定值可以是不同的物理量，每次设 定值的给出，不仅取决于时间，还取决于对前段控制结果的逻辑判断。
7.1.2
比例积分微分控制（简称 PID 控制）
调节其器的输出是其输入的比例，积分、微分的函数。PID 控制现在应用最广，技术最 成熟。其控制结构简单，参数容易调整，不必求出被控对象的数学模型便可以调节，因此无 论模拟调节器或者数字调节器大都采用 PID 调节规律。
7.1.3
直接数字控制
直接数字控制是根据采样理论，首先把被控对象的数学模型进行离散，然后由计算机根 据离散化的数字模型控制。这种方法与 PID 控制相比，其针对性更强，调节品质更好。
7.1.4
最优控制
在生产过程中为了提高产品的品质量，增加产量，节省原材料，要求生产管理及生产过 程始终处于最佳工作状态。因此，产生了一种称为最优控制的方法，也称为自适应控制。在 这种控制中，要求系统能够根据被测参数、环境及原材料的成分的变化而自动对系统进行调 节，使系统随时都处于最佳状态。最优控制包括性能估计（辨别），决策和修改三个环节。
7.1.5
模糊控制
模糊控制也叫 Fuzzy 控制。上述 3、4 两种控制方法种，都必须精确地求出系统的数学 模型，而在实际应用种影响变化的因素很多，所以很难用一个精确的数学模型表示，因此，
应用受到一定的限制，有时甚至很难实现。采用模糊控制，不需要数学模型，只要把设计者 的控制决策（即专家意见）用模糊规则加以描述，即可以实现模糊控制。
模糊控制的特点使简单，执行速度快，占用内存少，开发方便、迅速。因而得到了广泛 的应用。
7.2 PID 算法的数字化实现
)] 65mm，所以左轮的分辨率为 6.19mm/脉冲，右轮的分辨率为 6.59mm/脉冲，这样，求实际速
度的算法如下：
float v1,v2;
float T=1.5;
void main() {
motor(1,50);
motor(2,50);
rotation(1);
rotation(2);
resettime();
while(1) {
if(seconds()>5.0) {
stop();
break;
} wait(T);
v1=(float)rotation(1)*0.619/T;
v2=(float)rotation(2)*0.659/T;
printf("v1=%f,v2=%f\n",v1,v2);
} }
2）观察对象的特性
a．输入定值（左轮右轮的输入值都为 50，T=1.5 秒），记录不同时刻的输出值，测 得的实验数据如下：
1T 2T 3T 4T 5T 6T
左轮 47 51 48 47 47 49
右轮 46 48 47 44 46 46
输入输出图形大致如下
这样观察对象的特性，存在很多误差（每个周期记录的速度只是这个周期的平均速度，没法
看到轮胎速度变化的全过程；光电编码器的读值有误差，这样，平均速度就不是很准确），
但因为我们这个实验只是用于说明我们可以控制能力风暴走直线这样一个事实，不追求很大 的精确性，所以这样的误差就不去考虑了。
从上图可看出，输出对输入响应很快，反应时间几乎不用考虑，并且输出基本在某个定值附 近脉动。
b.输入不同值，记录输出值，实验测得的数据如下：（T=1.5 秒）
左轮输入值 左轮输出值 右轮输入值 右轮输出值
1 10．00 10．75 10．00 10．25
2 20．00 33．00 20．00 31．75
3 30．00 42．75 30．00 41．25
4 40．00 47．00 40．00 45．50
5 50．00 51．00 50．00 49．25
6 60．00 55．75 60．00 53．50
7 70．00 56．75 70．00 54．75
8 80．00 61．25 80．00 58．50
9 90．00 61．75 90．00 59．75
因为电机的控制函数 motor（）或 drive（）的控制范围在 0～100 之间，所以取 10～90，
在图纸上描点，发现对象是非线性的。而且，在相同输入下，右轮比左轮输出值小。
我们得到了对象大致的特性后，就可以控制它，使它走直线。使左轮右轮走直线，可以用两 种方式。第一种，使左轮右轮都趋近于同一个设定值；第二种，使右轮跟随左轮的速度。采 用第一种，使左右轮都趋近于 50。
从表一的图可看出，我们要控制的不是对象的反应速度，而是控制它的脉动和偏差，尽量趋 向于设定值，从表二，可以看出右轮的特性与左轮不同，所以可以对左右轮分开控制。因为 PID 调节方法对任何对象都有较大的调节作用，所以我们采用该调节方法对对象特性进行调 节。
7.3.2
AS－UII 走直线的开环特性
代码如下：
void main() {motor（1，50）;
motor（2，50）；
}
让 AS-UII 纵向走两米，观察它的横向偏移。
改变输入值，测得的实验数据如下：
输入值 偏移距离（米）
1 10 >2
2 20 >2
3 30 >2
4 40 >2
5 50 1.31
6 60 0.90
7 70 1.13
8 80 1.00
9 90 1.00
7.3.3
AS-UII 的闭环特性
应用 PID 算法进行反馈控制。根据控制量，我们采用位置式算法。
1）位置式算法 P 公式如：U=KEi+Uo;
PI 公式如：U=K(Ei+T/Ti*ΣEj)+Uo;
PID 公式如：U=K(Ei+T/Ti*ΣEj+Td/T*(Ei-Ei-1))+Uo;
因为左右轮的特性不一样，可以对他们分别控制。
这里 Uo 是控制量的基准，这里是 50，电机期望的速度是 50，E 是实际速度与期望 速度的偏差，U 是控制量，将做为 motor（）的控制参数。
代码如下：
/*用 PID 控制算法实现的走直线控制,位置式算法*/
/*左右轮有相同的设定值，对误差进行调整；根据左右轮的误差再对右轮进行调整，
使左右轮尽量保持一致，从而走直线。*/
float K1=0.05;
float Ti1=15.0;
float Td1=0.1;
float K2=0.2;
float Ti2=15.0;
float Td2=0.1;
float T=0.5;
float velocity_1;
float velocity_2;
float u_1_i;/*控制量*/
float u_2_i;
float error_1_i,error_1_i1,error_1_i2;
float error_2_i,error_2_i1,error_2_i2;
void main() {
motor(0,50);
motor(0,50);
rotation(1); /*将左右轮脉冲记数清零*/
rotation(2);
resettime(); /*系统时间复位*/
error_1_i1=0.0;
error_2_i1=0.0;
error_1_i2=0.0;
error_2_i2=0.0;
while(1) {
if(seconds()>4.0) { stop();
break;
} wait(T);
/*计算 error_i*/
velocity_1=(float)rotation(1)*0.619/T;
error_1_i=50.0-velocity_1;
velocity_2=(float)rotation(2)*0.659/T;
error_2_i=50.0-velocity_2;
printf("v1=%f",velocity_1);
printf("v2=%f\n",velocity_2);
u_1_i=K1*(error_1_i+T/Ti1*error_1_i2+Td1/T*(error_1_i-error_1_i1))+50.0;
u_2_i=K2*(error_2_i+T/Ti2*error_2_i2+Td2/T*(error_2_i-error_2_i1))+50.0;
/*
u_1_i=K1*error_1_i+50.0;
u_2_i=K2*error_2_i+50.0;
*/
/*
u_1_i=K1*(error_1_i+T/Ti1*error_1_i2)+50.0;
u_2_i=K2*(error_2_i+T/Ti2*error_2_i2)+50.0;
*/
motor(1,(int)u_1_i);
motor(2,(int)u_2_i);
/*将 error_i 的值赋给 error_i1,并将 error_i 叠加到 error_i2 上*/
error_1_i1=error_1_i;
error_2_i1=error_2_i;
error_1_i2=error_1_i2+error_1_i;
error_2_i2=error_2_i2+error_2_i;
} }
对偏移有如下规定：
A．先用 P 算法，保持 T=0.5 不变。
左轮比例系数 K 右轮比例系数 K 终点偏移距离
1 0.1 0.1 0．8
2 0.1 0.3 0.6
3 0.1 0.5 0.3
4 0.1 0.6 0.1，先左偏后右偏
5 0.1 1.0 -0.2
6 0.1 1.2 -0.4
7 0.5 0.5 0.6
8 0.5 0.8 0.2
9 0.5 1.2 0．1，先左偏后右偏
10 0.5 1．8 -0.1
11 1.0 1.0 0.3
12 1.0 1.6 几乎无偏差
13 1.0 2.0 -0.1，
14 5．0 5．0 运动曲线不平滑，会有突兀的变化 由以上数据可见，要使能力风暴走直线，右轮的比例系数应比左轮的比例系数大一点,但右 轮比左轮大到一定程度时走路会发生弯曲，并从偏移正值变为负值。另一方面，他们的比例 系数增大时，横向偏差越来越小。但当比例系数大到一定程度时，速度会有较大的振荡。
取上面最理想的比例系数 K1=1.0,K2=1.6 不变，改变 T。
T 偏移距离
1 0.1 走时左右微弱振荡
2 0.2 走时微弱振荡，左偏
3 0.5 偏差小
4 0.8 0．1
5 1．0 0．2
所以，T 取 0.5 最合适。
B．用 PI 算法，保持 K1=1.0 ，K2=1.6，T=0.5 不变。
左轮积分系数 Ti 右轮积分系数 Ti 偏移距离
1 10 5 终点几乎无误差，先
右偏后左偏
2 10 10 偏差小
3 10 20 0.1
4 20 5 -0.3
5 20 20 偏差小
6 20 30 0．1
7 25 5 -0.1
8 25 10 偏差小
9 25 25 0．2
PI 算法可以减小静差，用在这里，效果不是很大，甚至会因为积分累积引起 AS-UII 走路弯 曲。
C．PID 算法，取 K1＝1.0，K2=1.6，Ti1=20，Ti2=20，T=0.5
左轮微分系数 Td1 右轮微分系数 Td2 偏移距离
1 0.01 0.2 0.2
2 0.1 0.1 -0.1,偏差小
3 0.1 0.01 -0.2
PID 可以减小超调，在 PI 基础上加上微分因子，超调就小了，从 AS-UII 的弯曲程 度上就可看出。
2）增量式算法
P 算法公式如：∆Ui=Ui-Ui-1=K*(Ei-Ei-1);
PI 算法公式如：∆Ui=Ui-Ui-1=K*(Ei-Ei-1+T/Ti*Ei);
PID 算法公式如：∆Ui=Ui-Ui-1=K*(Ei-Ei-1+T/Ti*Ei+Td/T*(Ei-2Ei-1+Ei-2));
原来增量式算法是用于一些步进电机之类的对象，因为它的控制量是增量。以增量 为控制量可以减少切换时的冲击。但是我们这里的电机并非步进电机，所以输入的 控制量应是绝对数值。所以算法公式应改成如下：
P 算法公式如：Ui =K*(Ei-Ei-1)+ Ui-1;
PI 算法公式如：Ui=K*(Ei-Ei-1+T/Ti*Ei)+Ui-1;
PID 算法公式如：Ui=K*(Ei-Ei-1+T/Ti*Ei+Td/T*(Ei-2Ei-1+Ei-2))+Ui-1;
但是，这样的话，增量式算法和位置式算法的结果实质上是一样的。这里就不采用 了。
7.3.4
结论
float K1=0.1;
float Ti1=20.0;
float Td1=0.1;
float K2=0.1;
float Ti2=20.0;
float Td2=0.1;
float T=0.5;
float velocity_1;
float velocity_2;
float u_1_i;/*控制量*/
float u_2_i;
float error_1_i,error_1_i1,error_1_i2;
float error_2_i,error_2_i1,error_2_i2;
void main() { if(seconds()>5.5) { stop();
/* u_1_i=K1*(error_1_i+T/Ti1*error_1_i2+Td1/T*(error_1_i-error_1_i1))+50.0;
u_2_i=K2*(error_2_i+T/Ti2*error_2_i2+Td2/T*(error_2_i-error_2_i1))+50.0;
*/
u_1_i=K1*error_1_i+50.0;
u_2_i=K2*error_2_i+50.0;
/* u_1_i=K1*(error_1_i+T/Ti1*error_1_i2)+50.0;
u_2_i=K2*(error_2_i+T/Ti2*error_2_i2)+50.0;
*/ motor(1,(int)u_1_i);
motor(2,(int)u_2_i);
/*将 error_i 的值赋给 error_i1,并将 error_i 叠加到 error_i2 上*/
error_1_i1=error_1_i;
error_2_i1=error_2_i;
error_1_i2=error_1_i2+error_1_i;
error_2_i2=error_2_i2+error_2_i;
} }
第8章 AS-UII 的无线控制实现
自主式移动机器人是一个对外界环境高度开放的智能系统，能够按照预先给定的任务指 令，根据已知的地图信息作出全局路径规划，并在行进中不断感知周围的局部环境信息，自 主地作出各种决策，自动避开障碍物，引导自身安全行驶到指定的目标，执行要求的动作和 操作。它与固定式机械手相比具有更广阔的运动空间，更强的灵活性。移动机器人可广泛应 用于工业、国防、航空航天以及服务性行业，如工业上危险地区的标本采样和故障处理、国 防上无人驾驶战车等。随着人口老化和工作、生活节奏的加快，人类在服务性行业迫切需要 助手。无论是家庭、医院，还是在宾馆、商店，都有移动机器人的用武之地。
近年来，在机器人和自动化领域里移动机器人的研究吸引了众多研究者的注意力，其中 轮式移动机器人以其结构简单、容易实现成为受到欢迎的控制平台。另外一方面，移动机器 人在与外部的通信方式选择上，由于其具有广阔的运动空间，无线通信成为移动机器人所必 须具备的通信方式。在这种背景下，有关机器人无线控制的课题便应运而生。本章以 AS-UII 移动机器人作为控制平台，在计算机与 AS-UII 之间实现无线通信进而通过计算机与机器人 之间的接口，成功地实现了无线方式控制移动机器人。
8.1 机器人平台
机器人仅依靠初始程序和自身传感器来运行，已具自主式移动机器人雏形。为了让机器 人更准确地感知周围的局部环境信息，机器人往往需要处理周围的语音信息和图像信息。对 这类信息的处理，一般有两种方式：（1）机器人用自己的“大脑”处理这些信息，然后分析、
机器人仅依靠初始程序和自身传感器来运行，已具自主式移动机器人雏形。为了让机器 人更准确地感知周围的局部环境信息，机器人往往需要处理周围的语音信息和图像信息。对 这类信息的处理，一般有两种方式：（1）机器人用自己的“大脑”处理这些信息，然后分析、