状态机在运动控制中的应用

1. 什么是状态机？

1.1 状态机的概念

      状态机，通俗的讲可以理解为一种建模方法。当一个逻辑非常复杂的程序放在面前，是非常令人头大的，使用状态转移图（是一个有向图形，包括各节点和状态转移条件）可以很好的梳理流程。以博主的理解，实现状态转移图的模块我们便可以称之为状态机。

      在程序中，一个状态机通常包括一个起始状态、一组状态集以及多个相应的状态转移条件。

      构建状态转移图时，需要注意每个节点对应一个状态，在这些节点中需要有至少一个终态，当达到终态时状态机停止。一个好的状态机应该做到设计安全（不会进入未知状态或异常死循环）、清晰易懂、易维护。

      百度百科把状态机归纳为了4个要素：即现态、条件、动作、次态。这样的归纳，主要是出于对状态机的内在因果关系的考虑。“现态”和“条件”是因，“动作”和“次态”是果1。

  • 现态：是指当前所处的状态。
  • 条件：又称为“事件”，当一个条件被满足，将会触发一个动作，或者执行一次状态的迁移。
  • 动作：条件满足后执行的动作。动作执行完毕后，可以迁移到新的状态，也可以仍旧保持原状态。动作不是必需的，当条件满足后，也可以不执行任何动作，直接迁移到新状态。
  • 次态：条件满足后要迁往的新状态。“次态”是相对于“现态”而言的，“次态”一旦被激活，就转变成新的“现态”了。

**举个栗子**
      以人的健康情况我们来举个例子，人有三个状态：健康、感冒、康复中。触发的条件有淋雨、吃药打针、好好休息、多喝热水和感觉良好。

      假设初始状态处于健康状态下，当淋雨后人会感冒。这时候我们有三种途径解决感冒这个问题：吃药打针、好好休息、多喝热水。无论哪一种方法，都能使人的状态由感冒转移到康复中。当人感觉到状态良好时，感冒好了，人也就又处于了健康状态。
在这里插入图片描述

注意

  1. 在进行状态机建模时，要区分“动作”和“状态”，前者一旦执行完就结束了，而后者如果没有条件触发，则状态会一直持续下去。

  2. 状态划分要完整

对于更形象的说明，可参考博文《设计模式：一目了然的状态机图》

1.2 状态机的思想

      关于状态机的思想，百度文库中有一个特别形象的例子2：

      网络上经常报道特级象棋大师车和多人一起下象棋，采用的方式是“车轮战”。车轮战有两种方式：（1）象棋大师先和甲开始下象棋，直到有了结果，然后才轮到乙和象棋大师对阵，下完了之后，然后是丙…，一直到和最后一个人下完。（2）象棋大师先和A下一步棋，然后再和B下一步棋，然后再和C，和…，和所有人下完一遍后，再回头从A开始，一个人接一个人。

      很显然“车轮战”的第1种方式效率不如第2种方式效率高，报道上的“车轮战”也是指的第2种方式。原因在于象棋大师的水平远远高于其他人，如果采用第一种方式，象棋大师下一步棋很快，甲需要考虑很长时间才能落子，象棋大师在和甲下棋的过程中，其他人只能等待。如果采用第二种方式，象棋大师和甲只下一步棋，然后再和乙也下一步棋，和所有人下完一步棋之后，再从甲开始，这样看起来是所有人都在下象棋，效率自然远高于第一种方式。

      “车轮战”的第2种方式，实际上就是程序中状态机的基本原理。程序中的多个任务可以看成是其他棋手，CPU是象棋大师，CPU在执行多个任务时，不再是先执行任务1，执行完任务1后，再执行任务2，而是把每个任务又划分出多个小任务（小任务中没有时间等待），CPU每次只执行每个任务中的小任务，执行完任务1中的一个小任务后，然后快速转向任务2中的小任务，按照这种模式轮询下去，由于CPU很快（象棋大师），整个程序中的任务都得到了实时的执行。任务中的小任务是按照任务的状态来划分的，故称为“状态机”。

2. 状态机的种类

      状态机可分为有限状态机（FSM）、分层状态机（HFSM），其中有限状态机又可分为Moore状态机（输出只和状态有关而与输入无关）、Mealy状态机（输出不仅和状态有关而且和输入有关系）

      在实践中状态机有一个致命的缺点，当状态一旦多了之后，它的跳转就会变的不可维护3，这时我们可以把状态进行分类，把同类型的状态作为一个状态机，然后使用一个更大的状态机去维护这些自状态机，，这时用到的便是分层状态机，可以理解为状态机的嵌套，如下图所示。

在这里插入图片描述
      关于以上几者，博文《FSM（状态机）、HFSM（分层状态机）、BT（行为树）的区别》有更生动的例子可以参考。

      维护庞大数量的知识条目是个噩梦，如果使用有限状态机(FSM)、分层有限状态机(HFSM)、决策树(Decision Tree)还不能满足你的使用要求，这时试试Next-Gen AI的行为树(Behavior Tree)吧。

3. 状态机的写法

      状态机的实现由常见的几种实现方法4：

  1. 我们可以 在无限循环for(;;)或者while(1)中使用switch-case或if-else来实现简单的状态机；
  2. 使用二维状态表state-event实现，该方法逻辑清晰，但矩阵通常比较稀疏且维护麻烦；
  3. 用状态转移表stateTransfer Table实现，数组大小等于状体转移边个数，易扩展；

3.1 switch-case结构的状态机的实现

      在程序的写法上，有横着写和竖着写两种4：

  • 横着写：在事件中根据当前的状态，执行相应的操作，完成相应的状态转换。

    //声明：本代码转自 https://blog.csdn.net/zzz1014440164/article/details/79814160
//横着写
void event0func(void)
{
    switch(cur_state)
    {
        case State0:
             action0;
             cur_state = State1;
        break;
         case State1:
             action1;
             cur_state = State2;
        break;
         case State2:
             action1;
             cur_state = State0;
        break;
        default:break;
    }
}
 
void event1func(void)
{
    switch(cur_state)
    {
        case State0:
             action4;
             cur_state = State1;
        break;
        default:break;
    }
}
 
void event2func(void)
{
    switch(cur_state)
    {
        case State0:
             action5;
             cur_state = State2;
        break;
         case State1:
             action6;
             cur_state = State0;
        break;
        default:break;
    }
}

  • 竖着写：在状态中判断事件，并执行相应的操作，完成相应的状态转换。

    //声明：本代码转自 https://blog.csdn.net/zzz1014440164/article/details/79814160
//竖着写
switch(cur_state)
{
  case State0:
         if(event1)
          {
             action0;
             cur_state = State1;
           }
          else if(event2)
          {
              action4;
              cur_state = State1;
          }
          else if(event3)
          {
              action5;
              cur_state = State2;
          }
       break;
    case State1:
         if(event1)
          {
             action1;
             cur_state = State2;
           }
          else if(event3)
          {
              action6;
              cur_state = State0;
          }
        break;
     case State2:
         if(event1)
          {
             action3;
             cur_state = State0;
           }
        break;
     default:break;
}

      通常我们使用横着写的结构写！！！
      这是由于竖着写使用了if -else结构，其中if语句隐含了优先级，破坏可事件间的原有关系（各个时间应该同优先级），而且竖着写在结构上是顺序查询方式（查询事件），浪费大量的时间，而且时间不可估算。

      对于横着写的方式，因为在某个时间点上状态是唯一确定的，在时间处理函数中通过switch语句可直接定位到相同状态，执行时间也可以估算。这种方式比较直观，程序执行效率较高。

3.2 状态转移表联合函数指针数组实现5

声明：本节内容转自 https://blog.csdn.net/thisinnocence/article/details/47060285

      用枚举来定义状态和事件，操作数据节点转移到目的状态用函数实现。枚举本身默认是从0开始的int类型，利用这个特点将状态转移函数放到函数指针数组中与状态对应起来，方便操作。这种实现方法易于扩展，增加状态和事件都比较容易。

状态：枚举类型
事件：枚举类型
状态转移结构体：{当前状态、事件、下个状态}，定义一个全局数组来使用
状态变更函数：到下个状态(放到数组中与状态枚举对应起来)

#include <iostream>
using namespace std;

typedef enum{
    OPENED,
    CLOSED,
    LOCKED,
}  State;

typedef enum{
    OPEN,
    CLOSE,
    LOCK,
    UNLOCK
} Event;

typedef struct{
    State currentState;
    Event event;
    State NextState;
} StateTransfer;

typedef struct{
    State state;
    int transferTimes;
}Door;

StateTransfer g_stateTransferTable[]{
    {OPENED, CLOSE,  CLOSED},
    {CLOSED, OPEN,   OPENED},
    {CLOSED, LOCK,   LOCKED},
    {LOCKED, UNLOCK, CLOSED},
};

void toOpen(Door& door);
void toClose(Door& door);
void toLock(Door& door);
typedef void (*pfToState)(Door& door);
pfToState g_pFun[] = {toOpen, toClose, toLock}; //状态枚举值对应下标

void toOpen(Door& door){
    door.state = OPENED;
    cout << "open the door!\n";
}

void toClose(Door& door){
    door.state = CLOSED;
    cout << "close the door!\n";
}

void toLock(Door& door){
    door.state = LOCKED;
    cout << "lock the door!\n";
}

void transfer(Door& door,const Event event){
    for (int i = 0; i < sizeof(g_stateTransferTable)/sizeof(StateTransfer); ++i) {
        if(door.state == g_stateTransferTable[i].currentState &&
           event == g_stateTransferTable[i].event){
            g_pFun[g_stateTransferTable[i].NextState](door);
            door.transferTimes++;
            cout << "transfer ok!\n";
            return;
        }
    }
    cout << "This event cannot transfer current state!!\n";
    return;
}

void printDoor(const Door& door){
    string stateNote[] = {"opened","closed","locked"}; // 下标正好对应状态枚举值
    cout << "the door's state is: " << stateNote[door.state] << endl;
    cout << "the door transfer times is: " << door.transferTimes << endl;
}

int main(){
    Door door = {CLOSED, 0};
    printDoor(door);
    transfer(door, OPEN);
    printDoor(door);
    transfer(door, LOCK);
    printDoor(door);
    transfer(door, CLOSE);
    printDoor(door);
    return 0;
}

3.3 其他

      对于C++中状态机的类封装，可以参考博文《C++设计模式之状态模式(二)》

4. 状态机在运动控制中是如何应用的？

      下面以机械臂的控制为例，讲解状态机在运动控制中的应用。

      当我们给一个关节下发指令后，机械响应到理想位置需要一定的时间，而这时我们已经完成了该关节的命令下发，在实际设备完成相应的这段时间，我们完全可以干些其他事情，例如电机状态的反馈（速度、位置、加速度等数据的反馈）。这就是上文提到的“象棋大师”同时与多人进行对战的思想。

      如果在运动控制过程要实现类似于多线程的功能，我们便可以用状态机来实现。

      如下图所示，假设运动控制器在常态下处于接收数据的状态，当收到上位机给运动控制器下发的回零命令时，运动控制器需要将状态置为回零状态，当回零状态下的动作执行完成时，再将状态跳至常态(接收数据状态)；同样的，当运动控制器收到执行轨迹命令时，状态置为执行轨迹点状态，当轨迹点执行动作完成后，状态跳转至常态……

      这样我们便实现了在运动执行的同时，还能接收继续接收和反馈数据的目的。

注意，有些运动控制器是可以写多线程程序的，但运动控制器一般是基于RT-Linux开发的，如果使用多线程可能会产生竞态，从而导致一些未知错误，因此建议在需要多线程的功能时，通过状态机来实现。

在这里插入图片描述
状态机主体代码如下：

 //	Author : 		Jack Soong
//	Created on: 	2019-6-26
 
/*
============================================================================
								状态机常量及状态变量
============================================================================
*/

short	giStateN;
enum eMainStateMachines
{
	eIDLE		= 	0,						// Main state machine #0 - Receive Data
	eSM1 		= 	1,						// Main state machine #1 - Home
	eSM2		= 	2,						// Main state machine #2 - PVT
	eSM3		= 	3,						// Main state machine #3 - Motor Jog
	eSM4		= 	4,						// Main state machine #3 - Stop
} ;
/*
============================================================================
								状态机函数主体
============================================================================
*/
void MachineSequences()
{
	giStateN = eIDLE;		// 初始化初始状态

	bool MachineStop = false;
	while(!MachineStop)
	{

		gbReadOver = TRUE;
		switch(giStateN)
		{
			case eIDLE:		// Main state machine #0 - Receive Data & Write Data
			{
				FeedbackData();
				ReadCmdData();
				if(gbHomeEnable)
				{
					giStateN = eSM1;
				}
				if(gbReadEnable)
				{
					ReadAxisData();
					giStateN = eSM2;
				}
				break;
			}
			case eSM1:		// Main state machine #1 - Home
			{
				GoHome();
				giStateN = eIDLE;
				break;
			}
			case eSM2:		// Main state machine #2 - PVT
			{
				PvtMove();
				giStateN = eIDLE;
				break;
			}
			case eSM3:		// Main state machine #3 - Motor Jog
			{
				JogMove();
				break;
			}
			case eSM4:		// Main state machine #3 - Stop
			{
				StopMove();
				break;
			}
			default:		// Main state error   #x - Exit
			{
				MachineStop = true;
				break;
			}
		}

		usleep(10000);		// 降低CPU负载
	}
	return;
}

5. 总结

  1. 使用状态机进行运动控制时首先要建立状态机模型，把各状态、各状态下要执行的动作以及触发条件抽象出来；
  2. 务必确保状态机的安全，要做到逻辑完整无误；
  3. 在进行状态机建模时，务必要区分开 “动作”和“状态”；
  4. 由于运动控制直接对应的是实际设备，因此在调试之前先使用打印大法测试逻辑的正确性，以确保在实际调试过程中不会出现危险。

毕竟，设备都是挺贵的~
在这里插入图片描述

  1. https://baike.baidu.com/item/%E7%8A%B6%E6%80%81%E6%9C%BA/6548513?fr=aladdin#2 ↩︎

  2. https://wenku.baidu.com/view/0574da1f640e52ea551810a6f524ccbff121ca64.html ↩︎

  3. http://www.aisharing.com/archives/393 ↩︎

  4. https://blog.csdn.net/thisinnocence/article/details/47060285 ↩︎ ↩︎

  5. https://blog.csdn.net/zzz1014440164/article/details/79814160 ↩︎

Logo
全球具身智能开发者社区

立足具身智能前沿赛道，致力于搭建全球化、开源化、全栈式技术交流与实践共创平台。

更多推荐

cover

开源聚势·具身启智，杭州这场沙龙给出中国具身智能产业化新答案

avatar 全球具身智能开发者社区

YoMo安全机制详解：TLS v1.3如何保护你的AI代理通信

在当今AI驱动的分布式系统中，安全通信已成为不可忽视的核心需求。YoMo作为Serverless AI Agent Framework，采用TLS v1.3加密协议构建了强大的安全防护机制，确保AI代理在地理分布式边缘计算环境中的通信安全。本文将深入解析YoMo的TLS实现原理、配置方法及最佳实践，帮助开发者构建安全可靠的AI应用。## 为什么TLS v1.3是AI代理通信的理想选择TLS

avatar 全球具身智能开发者社区

InternScenes开源数据集

数据集介绍 InternScenes 是上海人工智能实验室发布的大规模、可模拟室内场景数据集，论文收录于 NeurIPS 2025。具身人工智能的发展高度依赖于具有场景多样性和逼真布局的大规模、可模拟3D场景数据集。然而，现有数据集通常存在以下不足：数据规模或多样性有限、布局经过"净化"处理导致小物体缺失，以及严重的物体碰撞问题。 为解决上述问题，InternScenes 整合

avatar 全球具身智能开发者社区