注：本报告只考虑控制算法，视觉算法将专题讨论。

需编写例程：

1. turn_circle(omiga) 以角速度omiga原地转动一周，在内部循环调用rotate（omiga）。

2. release_ turn_circle() 停止转动，释放硬件

3. aim(offset,err) 对准目标，err为角度误差（通过红色标记与图像x轴中心位置的偏移得到err，参见视觉算法部分），offset为偏离目标的角度。对准时通常设offset为0。若希望机器人转动a弧度，可使用aim(a,0)，此时err=0，为开环控制。

4. rotate（omiga） 以角速度omiga转动，必须在每个时间周期调用一次，否则将自行停止转动。

问题描述：

1. 机器人从任意位置、任意角度出发进入隧道门，机器人与隧道门之间应无障碍物。

2. 机器人采用差分驱动，顶部安装一摄像头。

3. 隧道门采用（红、绿、蓝）三色圆形标记（LCD或彩色标记块）。如图1。

图1 隧道门及标识颜色

1. 实现方法1：静止对准法

1.1. 机器人工作流程：

匀速转动，寻找小灯－>原地对准－>前进－>偏差超过阈值－>停止前进－>原地对准－>

1.1.1. Step1寻找小灯

若三个小灯不在机器人视野内，机器人原地旋转（调用turn_circle(omiga)），寻找三个小灯。若视野出现三个小灯，机器人停止转动（release_ turn_circle()）。进入对准程序（aim(err)），对准程序是一个自动控制例程，可使用使用简单比例控制器（图2），也可是更为复杂的高阶控制器。当数个周期节拍中err均小于阈值时，便可判定机器人对准结束，aim结束。

图2 比例控制器

由于摄像机的性能限制，将不可避免的存在观测噪声（设噪声能量为0.0001），如图3上所示。设机器人初始方向为0rad（以下均使用弧度），在第5秒时机器人开始对准，（门在5rad处）。控制器输出的控制信号（转动角速度omiga）如图3中所示，所生成的转动角速度omiga指令将调用rotate（omiga）例程。机器人最终的方向如图3下所示。通过仿真发现，机器人的角度控制结果总体令人满意，但在5秒处控制指令要求机器人的转动角速度从0瞬间增大到1.1rad/s（图3中），这在机械上难以实现。

图3 比例控制器仿真结果

图4 一阶控制器

为解决此问题，同时为进一步改善小车的动态性能，我们设计了一阶控制器（如图4）。取自然频率为pi/4，阻尼比为0.7（可更快速的达到稳态）。（设计依据可参阅《现代控制工程》（第四版）204－210）。仿真结果如图5所示，图5中我们可以看到，机器人将以更平滑的角速度转动（转动角速度最大不超过0.15rad/s），同时也减少了噪声影响。

图5 一阶控制器的仿真结果

1.1.1. Step2前进

对准完成后，机器人将沿直线前进。但实际上机器人的运动总会存在偏差，前进过程中摄像机将不断检测机器人的航向误差，当误差超过阈值时，机器人将停下，重新进入对准程序（aim(err)）。如此循环。

1.1. 优缺点分析

优点：简单、易于实现

缺点：调整方向时必需停下，做不到边前进边调整。

1. 实现方法2：最优控制器

1.1. 状态方程的构建

图6 机器人二维运动

设计最优控制u

最优控制律设计可参考《Optimal Control with Engineering Applications》一书中2.1.5的能量最优设计。

牵扯到小车运动的非线性方程，比较麻烦，待仿真成功后再行报告。

1.1. 优缺点分析

优点：可边行进边调整，可根据时间进行能量最优、时间最优设计。

缺点：算法复杂，计算量大，有待验证。