【智能车竞赛】状态机编程在智能车竞速赛道中的应用 - 编程小记

我曾试图通过一些代码上的小技巧，优化智能车竞速赛道的元素处理逻辑。
后来学长告诉我这叫做 “状态机” 。
——2022.07.20

一、我们为什么需要状态机？

定宽的白色赛道，在深色背景布上，以不同的角度扭曲交错，便形成了 “环岛”、“三岔” 等智能车竞速赛道的元素。相比于纯粹的直道和弯道，这些元素以其更高的图像复杂度和特殊的行驶规则，使我们在编写算法时不得不对其做特殊处理。

每个元素都有其唯一性的特征，如果我们能够从那一串图像数组中将其分辨出来，就相当于实现了元素的 “识别”。然而元素的处理远不止 “认出元素” 这么简单，我们还要考虑智能车在元素中各个位置不同的图像特征、以及相应的引导方式——比如在第一次识别到 “环岛” 时要引导车辆拐弯入环，而在识别到将要离开环岛时则要引导车辆不再拐进去。这个过程包含了一连串的动作，状态机编程正好能够帮助我们理清其中的逻辑，并大大提高代码的可读性和易修改性。

如果代码由多个组员共同完成，代码的可读性十分重要——这可不是光凭一堆注释就能解决的；若是只有一个人负责，就需要着重关注代码的易修改性——不然写到后期就是一部天书，修改简直无从下手，全删了又觉得可惜……
——《论模块化编程的重要性》

二、什么是 “状态机编程” ？

状态机（FSM. Finite State Machine）概念的最早来源我没能考证到。倒是有个蛮统一的定义：

状态机是有限状态自动机的简称，是现实事物运行规则抽象而成的一个数学模型。

它有另外一个更显见的称呼——“状态转移图”。它是一个有向的 “图（Graph）”，通过条件（Branch）将一个个不同的状态（Not）以一个既有的逻辑关系连接起来。

如果讲人话，就是：

使程序在所有可能（或者说 “需要”）的状态间，以预定的条件实现自动切换

状态机早时出现于 “状态寄存器和组合逻辑电路” 的硬件实现。其在编程中也有类似的实现形式，这篇文章的后半部分就给出了一个很好的例子：什么是状态机？状态机的概念 CSDN
后文中也会有有关智能车的示例。

在竞速赛道上，我们需要对付的每种元素都可以视为一个状态机。由于车子的行驶是单向的（正常情况下应该不会出现 “倒车”……），状态机的条件逻辑也应该依照正向的时序设置为单向，即：
$$\begin{gathered} FSM.begin\stackrel{Event1}{\to }State{1}_{Action1}\stackrel{Event2}{\to }State{2}_{Action2}\stackrel{Event3}{\to } \\ \cdots \stackrel{EventN}{\to }State{N}_{ActionN}\stackrel{Event(N+1)}{\to }FSM.end \end{gathered}$$

十分简单。

三、如何将状态机运用到代码中？

正如之前说的，为了 “代码的可读性和易修改性”，我们采用 “enum枚举型” + “switch判断语句” 的模式：前者使每个状态的含义清晰易懂，后者使不同的状态在高代码层面更加模块化（在汇编层次，它很可能跟一大团错综复杂的if()嵌套是同一个东西……）。

以三岔路口右拐为例，把车辆经过三岔路口（Fork）的全过程分为三个状态（State）：无三岔（NO_FORK）、进入三岔（ENTER_FORK）、离开三岔（LEAVE_FORK）。

//定义所有元素的枚举
typedef enum {
    
    
    NONE, FORK, CIRCLE // , ...
}state;
//定义三岔路口所有状态的枚举
typedef enum{
    
    
	NO_FORK, ENTER_FORK, LEAVE_FORK
}stateFork;

状态之间的切换条件（即触发事件 Event）我们定义：

EventFork1：①赛道两侧对称变宽 && ②上方赛道边界呈向下凸起状 的特征 → 判断为三岔，进入状态机；
EventFork2：赛道边界在三岔路口的左下拐点消失 → 已驶入三岔；
EventFork3：①上方赛道边界的下凸点消失 || ②赛道边界的最低点回到图像底边附近 → 已驶离三岔，退出状态机；
（随便写的，仅供参考）

每个状态都有其相应的动作（Action）（当然，也可以没有动作）：

ActionFork1：补线——连接 赛道上方的下凸点 和 左侧赛道边界拐点；
ActionFork2：补线——连接 赛道上方的下凸点 和 图像左下界或赛道左边界在图像中的典型位置；

于是可以给出如下模板：

//判断元素
switch(state){
    
    
    case NONE:{
    
    
        //所有元素的识别判断
        if(EventFork1){
    
    
            state=FORK;
            stateFork=ENTER_FORK;
            break;
        }
        //...
        break;
    }
    case FORK:
        //判断三岔状态
        switch(stateFork){
    
    
            case NO_FORK:
                break;
        	case ENTER_FORK：{
    
    
                //发现岔路，引导车辆拐弯
                if(EventFork2)
                    stateFork=LEAVE_FORK;
                else
            		ActionFork1();
                break;
            }
            case LEAVE_FORK:{
    
    
                //车辆已经拐入岔路口，继续引导车辆驶出岔路
                if(EventFork3)
                    stateFork=NO_FORK;
                else
	                ActionFork2();
                break;
            }
		}
        break;
    case LOOP:
        //...
        break;
    //...
}

正如模板所写，我们还可以用一个更大的状态机将处理各个元素的小状态机统筹起来。

这里给一个大致的思路以供参考：

$$\begin{array}{ccccc}编写元素识别判断& \to & 确定元素中的所有状态& \to & 设计每个状态中的动作\\ & & \downarrow & & \\ & & 构造状态间的切换条件& & \end{array}$$

这样一来，元素的识别和处理都被划分为一个个模块，再配合适当的注释，便可以让每个部分都有头有尾、目的明确。

四、具体实践中可能遇到的问题

• 回顾状态机（FSM）的编写，其中有个很重要的问题：我该确定哪些状态（state）？
  三个 state（实际上是两个）可以解决三岔路口，但倘若三岔的分道内部放置了坡道，将前后两个三岔的 state 连同坡道的 state，一起构建成 2+1+2=5 个 state 的 FSM 也是个不错的选择。
  这可以说是状态机编程中最基础，也是最关键的部分。每一届比赛都会有一些新元素或者新组合，所以这个问题就留待各位自行思考了。

• 当元素的识别不稳定时，容易在本没有元素的位置出现元素误判（这不多见……）。而上述的单向度 FSM 并不能很好地应对误判的情况——不匹配的 event 会让程序无所适从，不知所措……在你想尽了办法、焦头烂额仍无法解决误判时，为了提高程序的鲁棒性，可以试着在状态机内部设置一些 ”陷阱（trap）“，倘若满足 trap 的条件则立刻退出状态机。
  当然，如果真这么写了，就需要保证 trap 的条件在元素识别正确时永远不会成立！

• switch()判断语句在每个相对独立的case后面都应该有一个break——教科书会这么说的，GCC 的 warning 也是这么给的，但实际上不一定非得这样。举个栗子：

摄像头获取每帧图像并处理需要一定时间（你可以写个函数把程序不同部分的时间 print 出来，这可以提醒你去提升算法效率——尤其是用到国产低端芯片的时候……），因而智能车的反应存在延时，这个说长不长的延时就是限制智能车速度上限的两大天花板之一（另一个是物理结构极限）。

假设你的智能车正以 5m/s 的速度行驶在环岛中，而你队友写的程序又臭又长，一个周期有将近50ms。现在，车子的摄像头在拐出环岛时刚好发现了离开环岛的特征，于是状态机置stateCircle=LEAVE_CIRCLE，然后十分开心地break走了。但当程序来到下一个周期，车子已经向前开了 25cm，好巧不巧，本准备用来补线引导车辆的关键点已经开过头看不见了，于是……你只好祈祷自己写的出界保护能管用了。

车子开到 5m/s 当然是在做梦，但错过关键点的可能性还是很现实的。两个解决办法：
①把下一个 state 的 action 写到上一个 state 的 event 里，相当于写了两处同样的 action；
②在一个 if(event){state=NEXT_STATE;} else{action;} 语句中，将 break; 放在 else{ } 里，也就是说，“如果切换状态的条件成立，则不会执行 break; ，而是直接进入下一个 case ，立刻执行相应的动作”。这样一来，action 只需要写一次就够了