根据比赛任务设计算法-模型预测控制 -大牛rawlings的作品(2009)model predictive control(theory and design)

第四章 根据比赛任务设计算法 根据比赛任务，可分析得出完成任务的基本步骤，本演示程序中设计的搬运 任务是把五个不同颜色的色块随机放在场地图中的 A、B、C、E 和 G 处。算法 的整体设计思想是先把 A、B、C 三个点处的物块搬到起点到中心的直线上排列 好，然后将第四个直接送到对应的放置地点，再用相同的方法将起点直线上的三 个物块放到对应的地点，最后再把处于灰色地带的第五个物块搬到对应地点。 本搬运算法的具体实现的步骤如下： （1）机器人从起点循线到中心交叉路口处，为方便描述记为 O 点； （2）在 O 点左转 90 度后继续循线，当 4 个 QTI 传感器全部检测到白色时， 即判断到达 A 点，机器人原地掉头，带着物块离开； （3）循线到 O 点右转 90 度，开启定时器，定时循线到接近起点处，后退 几步放下第一个物块，再原地掉头； （4）循线到 O 点左转 45 度，当 4 个 QTI 传感器全部检测到白色时，即判 断到达 B 点，机器人原地掉头，带着物块离开； （5）循线到 O 点右转 45 度，开启定时器，与上一个物块作一定的时间间 隔，定时循线，后退几步放下第二个物块，再原地掉头； （6）直接循线到达 C 点，到达 O 点后，开启定时器继续前进，与上一个物 块作一定的时间间隔，定时循线，后退几步放下第三个物块，再原地掉头； （7）再次循线到达 O 点，原地右转 90 度，循线来到 E 点，当 4 个 QTI 传 感器全部检测到白色时，开启颜色传感器识别物块颜色。 若物块是蓝色，则直行直接循线到物块放置处，放下物块后退几步再原地掉 头，继续循线到中心点 O 处，左转 90 度； 若物块是黄色，则原地掉头，循线到 O 点后继续直行，一直到达物块放置处， 放下物块后退几步再原地掉头，继续循线到中心点 O 处，右转 90 度； 若物块是白色，则原地掉头，循线到 O 点后右转 45 度，到达物块放置处，