Yangyang Duan

Yangyang Duan

do anything you want to do

RoboCup仿真3D中文教程-第3章

15 minute read

Published:

这是关于系统结构与功能模块，本章将以UT Austin Villa 3D团队发布的底层代码为例，介绍该底层的主要结构、运行流程以及核心模块，介绍实现一支RoboCup仿真3D球队所需要实现的功能，在之后的章节中将针对核心的功能进行原理的介绍与实现。

3.1 系统整体架构

​ 根据上述对RCSSServer3D比赛环境的介绍，SimSpark与底层代码仅通过网络通信进行交互，因此底层代码的所有输入输出格式均为S表达式格式的字符串。底层代码需要与服务器创建连接，向服务器发送创建智能体消息，之后接收服务器发送的消息，解析处理，更新机器人状态，向服务器发送需要更新的消息，以此循环，并根据仿真的状态自主进行决策，为一个完整的仿真流程。注意到SimSpark的仿真周期为20ms，因此从接收消息，到底层代码处理，到向服务器发送消息到达服务器整体的时间应该不超过该值。对于性能较差的计算机来说可能会导致代码处理消息耗时太长而导致仿真周期变长。不过，由于采用网络通讯与服务器连接，因此SimSpark服务器与球队代码不必在同一台计算机上运行，可以分别运行在同一局域网下的不同主机上，这种方式可以减少计算机的运行负担，适合于计算资源较少而导致球队代码计算时间过长的情况。这种情况下，需要保证主机间的网络通信质量较高，尽量缩短主机间的网络延迟，否则也会导致仿真延迟。

运行比赛时的整体结构应当如下图所示：

​ 上图中，表示的是使用三台计算机时的仿真流程，其中主机0上运行SimSpark服务器，主机1和主机2分别运行比赛双方的球队代码，两者均与主机0通过网络通信进行连接（与服务器默认的3100端口进行通讯）。在运行过程中，服务器将转发场景图，RoboViz监视器可以与主机0的3200端口连接，观察仿真的过程，RoboViz同样可以运行在同一局域网下的任意一台主机上，以保证计算资源的充足。需要注意的是，该图中在网络通信部分展示了agent代理功能，其是由magmaOffenburg团队为解决Client球队代码的思考时间导致与服务器的不同步问题，在分布式运行时一般会采用，不过并非是必须的。

​ 在比赛流程中，各个队伍的11个球员是无法直接进行通信的，这要求团队的代码中能够分析收到的信息，构建世界模型，能够不断更新世界状态，根据有限的信息确定自身、队友和对方机器人的状态，并以此自主进行决策。虽然服务器中包含了广播效应器和听觉感知器能够模拟人类交流过程，但由于信息大小有限，更新周期较长，机器人对除自身的仿真状态的感知仍主要依靠视觉感知器。

3.2 运行流程

​ 在机器人的仿真过程中，需要不断的与服务器发送信息和从服务器获取信息，这些信息是以S表达式的格式存在的，而一支球队为实现能够正常比赛所必须的功能，就需要实现对服务器信息的解析功能，并根据解析的信息构建世界模型，感知自身和周围环境的状态，并作出自己的决策，控制自身关节的移动，实现行走和踢球的动作。然而，控制具有几个关节的机械臂本身就不是一件易事，对于具有22个关节的机器人，实现站立不摔倒对于刚了解该领域的新手来说更是难以实现。因此学习的最好的方式是根据一个开源项目进行实践操作，在此选择了UT Austin Villa 3D团队的utaustinvilla3d开源项目。utaustinvilla3d是UT Austin Villa在RoboCup仿真3D比赛联盟中的一个团队，关于该团队的介绍在上一章中已经有所介绍，更多信息可以在该团队官网上查阅，在此不再多做介绍。

​ utaustinvilla3d项目自2015年发布以来受到了非常多的关注，尤其是在RoboCup仿真3D联盟中。在该比赛中，对于新队伍想重新创建属于自己的球队是一件工作量很大的事情，并且创建新球队的门槛也比较高，这也使得联盟中缺少年轻的队伍。该项目的发布很大程度上为解决该问题提供了典范，通过开源自身球队的底层代码能够帮助其他团队学习，同时也能促进自身队伍的发展。utaustinvilla3d项目基本涵盖了实现仿真球队的所有功能，包括服务器通信，消息解析，身体模型，步态引擎和基本的动作技能。但是，为避免底层代码过于强大导致联盟中的不公平现象，底层代码中仅包括了基础的步态参数和简单的示例，去除了优化后的步态参数，高级行为技能和高层决策。新团队可通过该项目上手RoboCup仿真3D项目并在此基础上进行二次开发或者创建自己的新球队。由于项目的复杂性，即使是底层代码也包括了数十个模块和数万行代码，因此在本节对该底层代码的运行流程进行介绍，帮助读者了解该项目。

从main函数开始

main函数是程序的入口，也是一个项目的开始，从该函数开始介绍整个项目的运行流程。在项目主目录下，main.cc文件中，结尾处定义了main函数如下：

int main(int argc, char* argv[])
{
    // registering the handler, catching SIGINT signals
    signal(SIGINT, handler);
    // Actually print out the errors that are thrown.
    try
    {
        PrintGreeting();
        ReadOptions(argc,argv);

        if (! Init())
        {
            return 1;
        }
        Run();
        Done();
    }
    catch (char const* c)
    {
        cerr << "-------------ERROR------------" << endl;
        cerr << c << endl;
        cerr << "-----------END ERROR----------" << endl;
    }
    catch (string s)
    {
        cerr << "-------------ERROR------------" << endl;
        cerr << s << endl;
        cerr << "-----------END ERROR----------" << endl;
    }
}

​ 在主函数中，首先注册了一个signal函数，该函数可以捕获用户输入信号（例如Ctrl+C）并进行处理，一般用于关闭程序。之后利用try语句运行主代码并捕获抛出的异常，主代码中首先运行PrintGreeting函数，该函数打印指定的信息，然后运行ReadOptions函数，该函数对运行程序时加入的参数进行处理。之后利用Init函数对程序进行初始化，例如建立和检查与服务器的连接等功能，在初始化成功后，运行Run函数，和Done函数。其中Run函数为主要的循环处理函数，Done函数为程序结束后进行一定的处理（关闭TCP连接，并输出提示）。

接下来查看Run函数：

void Run()
{
    Behavior *behavior;
    if (agentType == "naoagent") {
        behavior = new NaoBehavior(teamName, uNum, namedParams, rsg);
    }
    else if (agentType == "simplesoccer") {
        behavior = new SimpleSoccerBehavior(teamName, uNum, namedParams, rsg);
    }
    /*
    ...
    create other type agent
    ...
    */
    else {
        throw "unknown agent type";
    }
    PutMessage(behavior->Init()+"(syn)");
    string msg;
    while (gLoop)
    {
        GetMessage(msg);
        string msgToServer = behavior->Think(msg);
        // To support agent sync mode
        msgToServer.append("(syn)");
        PutMessage(msgToServer);
        if (mPort != -1) {
            PutMonMessage(behavior->getMonMessage());
        }
    }
}

​ Run函数中首先创建了一个指向Behavior类型的指针，之后根据agentType参数创建不同类型的behavior对象，这些类型均是Behavior父类的子类。创建对象后使用PutMessage函数向服务器发送消息，该消息是创建的behavior对象中的Init函数返回的字符再加上(syn)符号。Init函数的作用是在服务器中创建智能体，因此需要符合第二章中对创建效应器的描述，例如：(scene rsg/agent/nao/nao.rsg)，在代码中也可以看到符合该格式，（syn）符号用于与服务器同步，在发送到服务器的所有消息中都应当包含此消息。通过这样的一个操作后，服务器便会根据设置的场景描述文件创建一个具有物理属性的机器人。但是该机器人还未赋予球场机器人属性（例如编号，属于的队伍等），因此后续还需继续初始化。

后面是主循环代码块，gLoop是一个全局变量，默认为true，当接受信号（Ctrl+C）后，信号处理函数将其值变为false即退出循环。在循环中，首先使用GetMessage函数从服务器接受发来的消息，之后使用behavior中的Think函数处理消息，并返回要发送到服务器的消息，之后加入同步消息符并发送到服务器。如果设置监视器端口，则发送需要从监视器端口发送的训练命令，并以此不断循环。

Think函数为Behavior类中的一个方法，Behavior基类的结构如下：

#ifndef BEHAVIOR_H
#define BEHAVIOR_H
#include <string>
class Behavior {
public:
    Behavior();
    virtual ~Behavior();
    /** called once when the initially connected to the server */
    virtual std::string Init() = 0;
    /** called for every message received from the server; should
        return an action string */
    virtual std::string Think(const std::string& message) = 0;
    /** Get message for sending to the server through the monitor port */
    virtual std::string getMonMessage() = 0;
};
#endif // BEHAVIOR_H

Behavior中声明了最基本的函数，例如Init函数用于与服务器进行连接的初始化，getMonMessage函数，获取需要从监视器端口发送下信息，Think函数根据接收的信息思考并返回需要发送的信息，它的输入参数为从服务器接受到的字符消息，返回值为向服务器发送的字符消息。该函数是智能体思考和决策的主体部分，主要包括以下处理流程：输入字符的解析，关节信息的更新，世界模型的更新，智能体姿态估计，噪音信息的滤波，步态引擎的加载，动作技能的执行，高级策略的决策，发送消息的生成等方面。该基类中的函数均为虚函数，具体的实现需要在子类中设定，例如上述Run函数最开始时根据参数创建了特定的Behavior子类对象。由于内容较为复杂，下面将按照功能向读者介绍各个模块。

3.3 基础模块

3.3.1 服务器通信

​ 在第二章中的网络协议中我们介绍了SimSpark仿真服务器的通信格式，其基本格式为S表达式，在此基础上做了一些简化。在网络通信上使用默认的 ASCII 字符集，也就是一个字符编码为单个字节，为确保通信的准确性，服务器向智能体发送的消息以有效负载的长度为前缀，该长度前缀是按照网络顺序排列的32位无符号整数（unsigned int），因此在接收服务器信息时需要额外注意。

1. 接收服务器消息

main函数中的bool Init()函数：作用为与服务器的3100端口建立连接（如果需要，同时也与3200端口建立连接），建立成功或失败输出提示。

main函数中的void PutMessage(const string& msg)函数：该函数作用为检查连接中是或否有可读数据，并对接收的数据进行处理。在读取时，需要首先读取服务器消息的长度前缀，该数据类型为unsigned int，占四个字节，在接收后需要使用ntohl()将网络字端序转为本地字端序，从而得到真实的消息长度大小。之后，根据获取的消息长度，继续读取剩余的有效消息负载，实现一次对服务器消息的获取。

2. 发送服务器消息

main函数中的void PutMonMessage(const string& msg)函数用于智能体向服务器发送消息（通过3100端口），在发送时同样需要在消息前添加长度前缀，大小为有效消息负载的长度，数据类型为unsigned int，发送时加入到数据开头，并以网络字端顺序，占四个字节。

3. 发送监视器消息

main函数中的void PutMonMessage(const string& msg)函数用于从监视器端口向服务器发送消息（通过3200端口），主要用于训练命令的发送，例如移动球，改变比赛模式等。在发送时同样需要在消息前添加长度前缀，大小为有效消息负载的长度，数据类型为unsigned int，发送时加入到数据开头，并以网络字端顺序，占四个字节。

3.3.2 消息解析

1. S表达式

S表达式来源于Lisp语言，由于其特殊的结构，能够将程序和数据统一的表示，从而允许我们一同处理。源于其紧凑的语法表示，在解析时可以很方便的处理，即使出现未知的信息，对于程序也可以很方便的进行忽略。S表达式的灵感来源于二叉树数据结构：

        *
     /     \
   *         *
 /   \     /   \
a     b   c     d

上述表示的即为一颗二叉树，它包含三个节点和四个子叶，当然按照上述方式表示一颗二叉树是不方便的，因此可以按照点对的方式进行标识，上述二叉树可以表示为：

((a . b) . (c . d))

上述中的.代表一个节点，因此S表达式实际上就是二叉树的点对表示：

S表达式 -> 原子数据 | (S表达式 . S表达式)

所以一个S表达式应由原子数据或者其他递归的S表达式组成。

但这样表示，一棵树中会包括很多节点，因此有一个化简方法：当.之后紧跟左括号（，则可以将这个.、左括号和对应的右括号省略，上述树即可表示为：

((a . b) c . d)

不过这是对于Lisp编程语言来说，对于服务器发送比赛信息来说依然比较复杂，因此SimSpark的网络通信对其再次做出了简化。

2. 简化的S表达式

一颗表示当前时间的树可这样表示：

(time (now 25.40))

它表示这样的一个树：

      time
     /     \
   now     25.40

但对于包含更多信息的结构，例如：

表示比赛状态信息：

(GS (sl 0) (sr 0) (t 0.00) (pm BeforeKickOff))

它表示当前比赛状态：左方比分0，右方比分0，比赛时间0.00，比赛模式Beforekickoff，所代表的树如下：

               GS 
         /            \
        *              *
      /   \         /      \ 
     *     *       *        *
    / \   / \     / \      /  \
   sl  0 sr  0   t 0.00   pm Beforekickoff

对于更加复杂的信息，例如视觉信息：

(See 
	(P 
		(team UTAustinVilla_Base) 
		(id 11) 
		(rlowerarm (pol 0.24 -35.22 -24.09)) 
		(llowerarm (pol 0.24 28.85 -19.35)) 
		(rfoot (pol 0.52 -8.47 -45.61)) 
		(lfoot (pol 0.52 8.43 -44.06))) 
	(G2R (pol 19.00 -20.45 43.92)) 
	(G1R (pol 19.00 -11.82 45.29)) 
	(F1R (pol 21.43 21.96 41.09)) 
	(F2R (pol 21.45 -48.31 31.79)) 
	(B (pol 3.99 -14.33 36.80)) 
	(L (pol 9.33 60.21 21.94) (pol 5.51 -60.25 19.26)) 
	(L (pol 21.39 -48.25 31.97) (pol 21.44 22.30 41.16)) 
	(L (pol 21.47 22.23 41.06) (pol 11.10 60.01 23.01)) 
	(L (pol 21.44 -47.94 31.87) (pol 15.23 -60.12 23.90)) 
	(L (pol 17.42 -2.17 43.21) (pol 17.42 -28.02 39.47)) 
	(L (pol 17.41 -2.18 42.98) (pol 19.19 -3.35 43.07)) 
	(L (pol 17.43 -28.10 39.55) (pol 19.21 -27.07 39.84)) 
	(L (pol 5.98 -14.77 38.63) (pol 5.71 0.70 39.38)) 
	(L (pol 5.72 0.73 39.46) (pol 4.98 14.02 37.53)) 
	(L (pol 4.98 14.03 37.48) (pol 3.88 22.19 34.15)) 
	(L (pol 3.88 21.95 34.08) (pol 2.68 17.70 31.18)) 
	(L (pol 2.68 17.72 31.22) (pol 2.03 -12.26 28.09)) 
	(L (pol 2.03 -12.43 28.17) (pol 2.67 -41.25 23.17)) 
	(L (pol 2.67 -41.01 23.41) (pol 3.88 -45.72 25.28)) 
	(L (pol 3.88 -45.96 25.25) (pol 4.98 -39.68 30.26)) 
	(L (pol 4.98 -39.82 30.31) (pol 5.73 -28.77 35.18)) 
	(L (pol 5.73 -29.14 35.31) (pol 5.99 -14.37 38.47))
	)

​ 上述是比赛过程中收到的一条真实的视觉信息，可以看到包含了很多球场标识的信息。虽然较为复杂，并且已经无法仅使用二叉树进行表示，但其依然符合S表达式的核心思想，即S表达式由原子数据或者递归的S表达式组成，因此依然可以利用程序很方便的对其进行处理，分析其中包含的信息。比如可以直观发现视觉信息中包含了一个球员信息，四个标志信息，一个足球信息，十七个可视线条信息。

3. 消息的解析

消息的解析主要采用了Parser类，该类的主要定义和实现在parser文件夹下，包括parser.h、parser.cc和VisionObject.h三个文件。

VisionObject.h

该文件定义了一个VisionObject的结构体，主要对可视觉看到的物体进行描述，结构体定义如下：

struct VisionObject {
    VisionObject( double r, double theta, double phi, int id_ );
    VisionObject( double r1, double theta1, double phi1, double r2, double theta2, double phi2, int id_ );
    VisionObject();
    VecPosition polar; 
    VecPosition polar2;
	int id; 
};

其中声明了三个构造函数，分别对应不同的参数，例如对于可见的旗帜，可用一个VecPosition变量和一个id进行描述；对于可视的线段，可用两个VecPosition变量分别描述端点，id进行标识。

parser.h

该文件定义了Parser类，该类中定义了对接收到的服务器消息进行解析，从而得到球场环境信息，该类声明的部分内容如下：

class Parser {
private:
    WorldModel *worldModel;
    BodyModel *bodyModel;
    PFLocalization* particleFilter;
    int uNum;
    int side;
    string teamName;
    bool fProcessedVision;
    vector< boost::shared_ptr<VisionObject> > visionObjs;//视觉物体
protected:
    vector<string> tokenise(const string &s);//分割字符串(按照空格、左右括号分割)
    vector<string> segment(const string &str, const bool &omitEnds);//分割字符串(分割成对的左右括号内容)
    bool parseTime(const string &str);//解析服务器时间
    bool parseGameState(const string &str);//解析比赛状态
    bool parseGyro(const string &str);//解析陀螺仪
    bool parseAccelerometer(const string &str);//解析加速度计
    bool parseHear(const string &str);//解析听觉
    bool parseHingeJoint(const string &str);//解析铰链关节
    bool parseSee(const string &str);//解析视觉
    bool parseLine(const string &str, vector< boost::shared_ptr<VisionObject> >& visionObjs);//解析线段
    bool parseGoalPost(const string &str, vector< boost::shared_ptr<VisionObject> >& visionObjs);//解析球门门柱
    bool parseFlag(const string &str, vector< boost::shared_ptr<VisionObject> >& visionObjs);//解析旗帜
    bool parseBall(const string &str, vector< boost::shared_ptr<VisionObject> >& visionObjs);//解析足球信息
    bool parsePlayer(const string &str, vector< boost::shared_ptr<VisionObject> >& visionObjs);//解析球员信息
    bool parseFRP(const string &str);//解析阻力
public:
    Parser(WorldModel *worldModel, BodyModel *bodyModel, const string &teamName, PFLocalization* particleFilter, FrameInfoBlock* vision_frame_info, FrameInfoBlock* frame_info, JointBlock* joint_block, SensorBlock* sensor_block);
    ~Parser();
    bool parse(const string &input, bool &fParsedVision);
};

其中包含了对服务器字符消息处理的主要函数，在代码运行流程中主要使用parse()函数进行解析，其中通过调用该类中的其余函数完成对信息的提取和处理。

首先，接收的信息为一串字符串，首先使用segment()函数将字符串分割为各个有效信息包（一个成对括号所包含的所有内容），对于每个包使用tokenise()函数将字符串按照空格、左括号、右括号进行分割，从而实现有效信息的提取并存储为一个vector容器中。之后按照第二章中对消息格式的介绍，根据独特的标识和对应的格式进行信息的匹配和转换。

下面通过对加速度计信息的处理介绍这一流程：

首先在parse()函数中使用segment()函数分割信息包，当匹配到对应信息，进行下一步处理：

vector<string> inputSegments = segment(input, false);
    for(size_t i = 0; i < inputSegments.size(); ++i) {
        //Time
        if(inputSegments[i].at(1)== 't') {
            valid = parseTime(inputSegments[i]) && valid;
        }
        /* ... 
        其他标识的处理
        ...*/
        //Accelerometer
        else if(inputSegments[i].at(1) == 'A') {//匹配到ACC信息标识
            valid = parseAccelerometer(inputSegments[i]) && valid;
        }
        else {
            valid = false;
        }
    }

​ 当识别到某个信息包中的第一个字符为‘A’，则将该信息包传入进一步的数据处理。使用parserAccelerometer()函数，该函数的主要针对加速度计所读取的数据进行获取并处理，实现如下：

bool Parser::parseAccelerometer(const string &str) {
    bool valid = false;
    double rateX = 0.0, rateY = 0.0, rateZ = 0.0;
    vector<string> tokens = tokenise(str);
    for(size_t i = 0; i < tokens.size(); ++i) {
        if(!tokens[i].compare("a")) {//匹配到a标识
            if(i + 3 < tokens.size()) {
                rateX = atof(tokens[i + 1].c_str());
                rateY = atof(tokens[i + 2].c_str());
                rateZ = atof(tokens[i + 3].c_str());
                valid = true;
            }
        }
    }
    if(valid) {
        // 阈值控制
        double spuriousThreshold = 20.0;
        if(rateX != rateX || (fabs(rateX) > spuriousThreshold)) {
            rateX = 0;
        }
        if(rateY != rateY || (fabs(rateY) > spuriousThreshold)) {
            rateY = 0;
        }
        if(rateZ != rateZ || (fabs(rateZ) > spuriousThreshold)) {
            rateZ = 0;
        }
        // 转换坐标系
        double correctedRateX = -rateY;
        double correctedRateY = rateX;
        double correctedRateZ = -rateZ;
        // 简单均值滤波
        float K = 0.9;
        VecPosition lastAccel = bodyModel->getAccelRates();
        correctedRateX = K*lastAccel.getX() + (1-K)*correctedRateX;
        correctedRateY = K*lastAccel.getY() + (1-K)*correctedRateY;
        correctedRateZ = K*lastAccel.getZ() + (1-K)*correctedRateZ;
        bodyModel->setAccelRates(correctedRateX, correctedRateY, correctedRateZ);
    }
    return valid;
}

​ 首先根据加速度感知器的消息格式，当匹配到‘a’字符时，表示之后的三个信息为加速度计的三轴信息，并转换为浮点型数据。之后对数据进行处理，例如判断是或否在一定的阈值之内，从机器人自身坐标系，转为球场坐标系，并根据历史信息进行简单的均值滤波等操作，即实现了本次消息中加速度计信息的获取和处理。其他信息，例如比赛模式、铰链关节、视觉信息的获取和处理流程与之相似，不同的是处理的格式和消息复杂程度不同，其他信息需要使用其他的处理手段，例如使用粒子滤波器进行定位等，在此不在详细介绍。

3.3.3 世界模型

​ 世界模型是机器人根据感知的信息自主识别和构建的世界，在该模型中，机器人以自我为中心，能够表示机器人所处的环境信息。从而根据所构建的模型进行自主决策，该模型是智能体的代码的核心部分，是机器人的“大脑”，为机器人提供驱动力。该模块的实现主要在worldmodel文件夹下，包括WorldObject.h、worldmodel.h和worldmodel.cc三个文件。

1. 世界物体

​ WorldObject.h该文件主要枚举了足球环境中的物体，例如足球、球门门柱、旗帜、球场线、我方11名球员、对方11名球员以及相应的身体部位。可以看到，这些物体基本是可由机器人视觉可见的，这也说明了机器人的感知过程中视觉信息为球场主要信息的来源。该文件中还定义了一个WorldObject的结构体，用于表示相应的信息，该结构体的定义如下：

struct WorldObject {
    WorldObject() {//构造函数
        pos.setVecPosition( 0, 0, 0 );
        orien = 0;
        id = -1;
        currentlySeen = false;
        cycleLastSeen = -1;
        timeLastSeen = -1;
        cycleOrienLastSeen = -1;
        timeOrienLastSeen = -1;
        validPosition = false;
        sighting.setVecPosition(0,0,0);
    }
    VecPosition pos; // Global coordinate system position of object
    int id; // unique id for all objects that can ever be seen
    bool currentlySeen; // if we currently see an object
    bool currentlySeenOrien; // if we can currently determine object's oreintation
    int cycleLastSeen; // Cycle we last saw object
    VisionObject vision;
    VecPosition sighting; // Position of object as reported by teammate
    int sightingUNum; // Uniform number of teammate who reported sighting
    vector<VisionObject> lines; // Used for field line observations
    /*其余成员变量
    ...
    */
};

​ 该结构体用于表示球场中各种物体的信息，例如自己看到的位置、编号、出现的时间、队友看到的位置等等，该结构体可以全面的表示球场物体的信息，帮助机器人结构化存储和处理球场信息。、

2. 世界模型

​ 该类主要在worldmodel.h文件中声明，在worldmodel.cc文件中实现。worldmodel类为机器人提供了几乎全部的信息获取接口，包括机器人本体位置和状态、队友位置、地方球员信息等，除单一的信息获取外，还包括了简单的信息处理过程，例如计算机器人距离我方球门的距离、全局球场坐标系和机器人局部坐标系的转换等方法，帮助获取更多信息。worldmodel类主要函数如下：

class WorldModel {
private:
    unsigned long cycle;
    int scoreLeft;
    int scoreRight;
    double time;
    double gameTime;
    int playMode;
    int uNum;
    int side;
    VecPosition myPosition;
    // remember last two skills
    vector<SkillType> lastSkills;
    vector<SkillType> executedSkillsForOdometry;
    string opponentTeamName;
    /*其余成员变量
    ...
    */
public:
    WorldModel();
    ~WorldModel();
    inline void setMyPosition( const VecPosition& newPos );//三维
    inline void setMyPosition( const SIM::Point2D& newPos );//二维
    inline VecPosition getMyPosition();
    void updateMatricesAndMovingObjs();
    /*其余成员函数
    ...
    */
   };

​ 在上述worldmodel类中，存储了智能体根据获取的信息构建的各种比赛状态信息，例如cycle表示周期数，scoreLeft、scoreRight表示双方比分，time表示服务器时间，gametime表示比赛时间等等，在成员函数中包含了设定自身位置的函数，包括不同参数的重载函数，以及获取自身位置的函数等等。其中updateMatricesAndMovingObjs()函数为worldmodel类中更新信息的函数，该函数通过遍历所有定义的WorldObject物体，获取该物体的位置信息，并更新到worldmodel类中的成员变量，从而实现机器人“大脑”信息的更新。其主要流程如下：

  • 根据左右半边信息设定各球门门柱以及旗帜的位置信息
  • 设定机器人局部坐标系到全局坐标系的转换参数
  • 设定全局坐标系到机器人局部坐标系的转换参数
  • 遍历所有定义的WorldObject，根据视觉信息，将其从机器人局部坐标系转换为全局坐标系

通过以上步骤便实现了对世界物体的信息更新并转换为全局坐标，除此之外，还包含了许多有用的函数，能够方便智能体获取相关信息，在此不在进行介绍。

3.3.4 身体模型

bodymodel定义了一个身体模型类，用于模拟一个具有多个关节和效应器的机器人，这个类主要用于机器人或模拟环境中的身体模型，例如机器人足球中的 Nao 机器人，该类的主要实现在bodymodel文件夹下，包括bodymodel.h和bodymodel.cc文件。为准确表示机器人的状态空间，该文件中国还定义了Effector结构体用于表示效应器的状态信息，Component结构体用于表示机器人组件（例如胳膊、大腿等）的状态信息。

效应器结构体的定义：

struct Effector {
    double minAngle;
    double maxAngle;
    double currentAngle;
    double targetAngle;
    // Constants for PID control
    double k1, k2, k3;
    double scale;
    Effector(const double &minAngle, const double &maxAngle, const double &k1, const double &k2, const double &k3, const double &errorTolerance) {
        this->minAngle = minAngle;
        this->maxAngle = maxAngle;
        currentAngle = 0;
        targetAngle = 0;
        this->k1 = k1;
        this->k2 = k2;
        this->k3 = k3;
        scale = 1.0;
    }
    void resetErrors();
    void updateErrors();
    void setTargetAngle(const double &angle);
    void update(const double &angle);
};

效应器结构体中包含了对应效应器的角度范围，设定的目标角度，当前角度以及PID参数，用于控制机器人相应关节的转动。在向服务器发送信息时，便会根据对应效应器的目标角度和PID参数进行计算从而得到过程中各个周期需要改变的角度。

部件结构体的定义：

struct Component {
    int parent;
    double mass;
    VecPosition translation;
    VecPosition anchor;
    VecPosition axis;
    HCTMatrix transformFromParent;
    HCTMatrix transformFromRoot;
    HCTMatrix translateMatrix;
    HCTMatrix backTranslateMatrix;
    Component(const int &parent, const double &mass, const VecPosition &translation, const VecPosition &anchor, const VecPosition &axis) {
        this->parent = parent;
        this->mass = mass;
        this->translation = translation;
        this->anchor = anchor;
        this->axis = axis;
        transformFromParent = HCTMatrix();
        transformFromRoot = HCTMatrix();
        translateMatrix = HCTMatrix(HCT_TRANSLATE, VecPosition(translation) + VecPosition(anchor));
        backTranslateMatrix = HCTMatrix(HCT_TRANSLATE, -VecPosition(anchor));
    }
};

​ 部件结构体中包含了机器人模型中的部件关系，例如父关节、质量、旋转矩阵等，能够帮助计算各个关节的状态，估计机器人整体姿态，与效应器结构体一起可以组成机器人主要身体的状态表示。当然仅用几个结构体去表示一个机器人是不够的，一个机器人应当包括关节、身体部件等物理特性，还需要包含信息处理、感知、决策等抽象特性。机器人身体模型将机器人的物理特性完全地包含起来，能够准确 地仿真和表示当前周期下的机器人状态，便于后续获取信息，从而进行正确的决策。

​ BodyModel类的主要定义如下：

class BodyModel {
private:
    WorldModel *worldModel;
    // Joints
    std::vector<SIMJoint> joint;
    // Effectors
    std::vector<Effector> effector;
    // Reflectors
    std::vector<int> reflectedEffector;
    std::vector<bool> toReflectAngle;
    // Components
    std::vector<Component> component;
    void refreshTorso();
    void refreshComponent(const int &index);
    void refreshHead();
    void refreshLeftArm();
    void refreshRightArm();
public:
    BodyModel(WorldModel *worldModel);
    BodyModel(const BodyModel *current, const int legIndex, const double a1, const double a2, const double a3, const double a4, const double a5, const double a6);
    ~BodyModel();
    inline double getJointAngle(const int &i) {
        return joint[i].angle;
    }
    inline void setJointAngle(const int &i, const double &a) {
        joint[i].angle = a;
    }
    inline double getCurrentAngle(const int &EffectorID) const {
        return effector[EffectorID].currentAngle;
    }
    inline void setCurrentAngle(const int &EffectorID, const double &angle) {
        effector[EffectorID].currentAngle = angle;
    }

    inline double getTargetAngle(const int &EffectorID) const {
        return effector[EffectorID].targetAngle;
    }
    inline void setTargetAngle(const int &EffectorID, const double &angle) {
        effector[EffectorID].setTargetAngle(angle);
    }
    inline void increaseTargetAngle(const int &EffectorID, const double &increase) {
        effector[EffectorID].setTargetAngle(effector[EffectorID].targetAngle + increase);
    };
}

上述代码展示了bodymodel类的部分内容，主要包括表示机器人关节和身体状态的数据存储（效应器，身体部件等）以及随周期更新的函数（如获取和设定关节角度，获取和设定效应器的当前角度以及目标角度）。这部分涵盖了仿真机器人的几乎所有物理特性，并使用了上述的结构体描述仿真机器人的各个关节和部件的执行角度，以及身体区域的朝向和空间中的位置。身体模型是完成仿真机器人循环周期中重要的一环。在基础方面，该类提供了状态信息的更新和存储，为其他基础模块如服务器通信提供帮助，在高层方面，身体模型为程序提供了机器人自身状态获取的接口，为高级决策模块提供了数据支撑。

3.3.5 步态引擎

步态引擎是实现一个完整机器人的关键，只有机器人可以稳定地行走，才能考虑下一步更高级的行为。UT Austin Villa 3D的底层代码中设计了一个稳定的全向行走步态引擎，并提供了在特定任务下的参数优化示例。实际上，该步态引擎在最初是用于真实的物理Nao机器人，并在其上进行了测试，最终用于了仿真机器人。在以往的机器人步态构建中，往往采用仿真设计与测试，最终用于实际机器人，这是第一次尝试。在此基础上通过对控制步态的相关参数进行迭代和优化达到了比较快的速度和稳定性。

上图是基础代码从步态引擎生成关节命令的流程，主要分为以下步骤：

  1. 根据从环境中获取的感知信息，更新世界模型，高级行为（决策层）根据以往和目前的场上比赛状态，做出行走方向的决策。该部分的主要内容主要在naobehavior类中的Think函数中实现，该部分属于高级行为，将在下一节中讲述。

  2. 将高级行为所产生的行走方向输入步态引擎中，通过计算和模拟得到机器人各个关节所需要到达的角度（需要注意的是该步态引擎所确定的关节角度并未机器人所有关节，而是决定机器人姿态的部分关节的姿态，例如臀部相对于躯干的偏移，脚部的位置等）。该部分的实现在utwalk文件夹下的MotionCore.cpp文件中，主要的方法为move函数。该函数的声明如下：

    void MotionCore::move(WalkRequestBlock::ParamSet paramSet, float xvel, float yvel, float rotvel)

    该函数需要四个参数，分别是步态参数paramSet、x方向速度xvel、y方向速度yvel、旋转角度rotvel，其中步态参数是比较重要的，它决定了机器人能够移动的速度和稳定性。通过对步态参数的优化可以实现更高的移动速度和更加稳定的机器人步态。

  3. 获取（部分）关节预期的关节角度后，便确定了机器人的目标姿态，但如何控制机器人达到该目标姿态便需要使用逆运动学。在机器人动力学中已知所有关节角度，确定末端位置为正运动学；而已知末端位置，确定所有关节的角度为逆运动学。本文所描述的步态引擎基于双足倒立摆模型，也就是需要控制机器人脚部稳定地向预定方向移动。在此过程中，末端位置（脚部）已知，需要采用逆运动学求解所有效应器（关节）的角度。

    该部分主要实现在utwalk文件夹下kinematics模块中，其中ForwardKinematics.cpp为正运动学实现，InverseKinematics.cpp为逆运动学实现。逆运动学实现中的calcLegJoints函数计算脚部到达目标位置的腿部关节角度，同时由于Nao机器人的髋关节是机械连接的，因此需要确保两条腿的关节0相同，以避免无法达到的脚部位置。

  4. 得到各个效应器角度，便需要控制效应器执行。基于物理世界，电机角度是无法瞬时改变的，需要一定的过程。除此之外，为减少外力因素（如碰撞）造成的效应器执行失败，需要采用PID算法控制效应器的执行过程。PID控制器通过计算误差（设定值与实际值之差）的三个部分（比例、积分、微分）的加权和，来调整控制器的输出，从而实现系统的稳定控制。

    在BodyModel类中定义了computeTorque函数，用于PID控制器的实现，相关代码如下:

    double BodyModel::computeTorque(const int &effectorID) {
    effector[effectorID].updateErrors();
    double torque = effector[effectorID].k1 * effector[effectorID].currentError;//比例项
    torque += effector[effectorID].k2 * effector[effectorID].cumulativeError;//积分项
    torque += effector[effectorID].k3 * (effector[effectorID].currentError - effector[effectorID].previousError);//微分项
    return effector[effectorID].scale * torque;
}

    该实现用于一般任务，如执行起身、踢腿动作的控制。针对步态引擎的关节控制，加入了额外参数对输出进行控制，相关代码如下：

    double BodyModel::computeTorque(const int &effectorID, SimEffectorBlock* sim_effectors_, JointBlock* raw_joint_angles_, JointCommandBlock* raw_joint_commands_) {
    effector[effectorID].updateErrors();
    double torque = effector[effectorID].k1 * effector[effectorID].currentError;//比例项
    torque += effector[effectorID].k2 * effector[effectorID].cumulativeError;//积分项
    torque += effector[effectorID].k3 * (effector[effectorID].currentError - effector[effectorID].previousError);//微分项
    float m = -0.7 / 990;
    float b = 1.0 - m * 10;
    float command_time_factor = m * raw_joint_commands_->angle_time_ + b;//与处于站立还是移动状态有关
    return  effector[effectorID].scale * torque * command_time_factor;
}

​ 经过上述流程，便实现了机器人步态的控制，不过基础的步态并非最优状态，控制步态的相关参数可采用一定方法对特定任务进行优化，达到更好的效果。

3.3.6 滤波器

在RoboCup仿真比赛中，为真实地模拟现实世界，机器人所获取的数据包含一定的噪声。在此基础上，滤波器被广泛用于处理传感器数据，以获得更准确和可靠的机器人状态估计。

1. 粒子滤波器

​ 粒子滤波器（Particle Filter）是一种基于蒙特卡洛方法的状态估计技术，主要用于在非线性、非高斯系统中对动态系统的状态进行实时追踪。它通过一组随机样本（称为“粒子”）来近似表示系统的后验概率分布，适用于复杂场景下的状态估计问题，如机器人定位、目标跟踪等。

​ 粒子滤波的核心是用大量带权重的粒子（样本）近似概率分布。每个粒子代表系统的一个可能状态，权重表示该状态与观测数据的匹配程度。通过不断迭代以下步骤，逐步逼近真实状态：

  1. 预测：根据系统模型预测粒子的下一个状态。
  2. 更新：利用观测数据调整粒子的权重。
  3. 重采样：淘汰低权重粒子，复制高权重粒子，避免粒子退化。

底层代码中particlefilter文件夹下实现了粒子滤波器用于机器人自身的定位，PFLocalization.h和PFLocalization.cpp文件定义了PFLocalization类，该类中通过粒子模拟机器人状态位置，根据机器人所观测到的视觉信息与实际物体（如线段、标识等）的区别计算该粒子的可信度，并不断重采样得到机器人状态的估计。

PFLocalizaton类中最主要的函数为processFrame，用于处理每一帧的数据，在RoboCup仿真比赛中，每20ms更新一次数据。具体步骤如下：

  1. 迭代计数：通过静态变量processingIteration记录处理帧的次数，每次调用函数时递增。
  2. 时间更新：获取当前时间，并计算自上次处理以来经过的时间。
  3. 从里程计更新粒子：调用updateParticlesFromOdometry函数，根据里程计数据更新粒子的位置。里程计主要用于机器人移动状态下，模拟粒子的移动状态，通过获取机器人当前执行的动作类型（稳定动作或全向行走动作），计算粒子所对应的移动位置。
  4. 填充观测线段：调用fillMyObservedLines函数，填充机器人当前观测到的线段信息。由于视觉信息是有限的，当比赛场上的可视线段较长而机器人所看到的线段仅是部分线段时，通过补全线段，形成一个完整的可视线段。
  5. 判断是否观测到物体或线条：如果至少观测到一个物体或一条线条，并且机器人没有摔倒，则设置机器人已定位。
  6. 从观测更新粒子：调用updateParticlesFromObservations函数，根据观测信息更新粒子的分布。在更新粒子时，主要根据机器人观测到的物体信息与实际的物体信息进行对比，计算该粒子的权重。例如对于线段信息，根据该粒子位置与视觉信息中线段的信息，计算该线段在球场的位置。与实际该线段的位置的差异（角度差、长度差、最近点等）计算该粒子的置信度，实现粒子状态的更新。
  7. 估计机器人姿态：调用estimateRobotPose函数，估计机器人的位置和姿态，并更新worldModel中的位置、角度和置信度。
  8. 重采样粒子：如果满足重采样频率条件，调用resampleParticles函数进行粒子重采样，并调用randomWalkParticles函数添加随机粒子，以保证粒子的多样性。

由于粒子滤波器主要根据视觉信息进行位置估计，因此在消息解析中，便采用了粒子滤波器对观测的数据进行处理，直接更新并估计机器人的位置。在机器人初始化时，会看到机器人在场上不断地抖动，这实际上是处于悬停时间（hoverTime），程序在不断的重新定位机器人，并等待粒子滤波的收敛。除机器人自身定位之外，粒子滤波器还用于我方队员位置的估计，并根据视觉信息中关于我方队员的信息进行粒子的更新。

2. 卡尔曼滤波器

卡尔曼滤波（Kalman Filter）是一种基于线性高斯系统的最优状态估计算法，主要用于从包含噪声的观测数据中实时估计动态系统的内部状态。它通过递归的预测-更新过程，结合系统模型和观测数据，提供对状态的最小均方误差（MMSE）估计，广泛应用于导航、控制、信号处理等领域。

卡尔曼滤波的核心是融合预测与观测，通过以下两个步骤迭代实现最优估计：

  1. 预测：基于系统动态模型，预测下一时刻的状态和不确定性。
  2. 更新：利用观测数据修正预测结果，降低不确定性。

在底层代码中，kalman文件夹下实现了卡尔曼滤波，其中OrigKalmanFilter实现了一个基本的卡尔曼滤波器，为应用于RoboCup仿真比赛，根据基本的卡尔曼滤波器，实现了用于跟踪球（BallKF）和跟踪球员（PlayerKF）的卡尔曼滤波器。

与粒子滤波器的结构类似，BallKF与PlayerKF滤波器的主要函数为processFrame函数，用于处理每一帧的数据。对于BallKF其过程如下：

  1. 时间更新：获取当前时间，并计算自上次处理以来经过的时间。
  2. 时间更新步骤：调用timeUpdate函数，根据时间间隔更新卡尔曼滤波器的状态。
  3. 机器人姿态测量更新：调用poseMeasurementUpdate函数，所有的观测值均以机器人的自身状态为基础，根据上述粒子滤波器的估计，更新卡尔曼滤波器中机器人的姿态。
  4. 球测量更新：调用ballMeasurementUpdate函数，根据观测到的球的信息更新卡尔曼滤波器的球的位置和速度。
  5. 更新球的位置：调用updateBallFromKF函数，将卡尔曼滤波器的球的位置和速度更新到worldModel中。

对于跟踪球员PlayerKF，其处理过程与之类似，只不过是对于11个球员分别进行上述过程。由于同样使用视觉信息，因此用于球跟踪和球员跟踪的卡尔曼滤波器同样直接在消息解析过程中进行更新，需要注意的是卡尔曼滤波器仅用于对方球员的跟踪。

3.3.7 动作技能

机器人除步态行走之外，还需要能够执行任务的能力，例如由于碰撞导致的机器人摔倒，这要求机器人能够根据不同状态自行起身并稳定。在RoboCup仿真比赛中，赢得比赛的关键是进球数，因此踢球行为是各个队伍所关注的问题。

UT Austin Villa 3D团队发布的底层代码中包括起立和踢腿的技能，每个技能都作为具有多个关键帧的周期性状态机实现，其中关键帧是固定关节位置的静态姿势。关键帧之间有一段等待时间，让关节达到目标角度。为了提供灵活的设计和参数化技能，该设计了一个直观的技能描述语言，便于规范的关键帧和等待时间之间。下面是一个例子：

SKILL KICK_LEFT_LEG
KEYFRAME 1
setTarget JOINT1 $jointvalue1 JOINT2 $jointvalue2 ...
setTarget JOINT3 4.3 JOINT4 52.5
wait 0.08
KEYFRAME 2
increaseTarget JOINT1 -2 JOINT2 7 ...
setTarget JOINT3 $jointvalue3 JOINT4 (2 * $jointvalue3)
wait 0.08

上述文字定义了一个名为KICK_LEFT_LEG的动作，该动作具有两个关键帧，每个关键帧中包含了所期望实现的关节角度，其中关节角度可以是确定的数字，也可以是从外部定义的变量。这样的方式便于测试和优化不同关节角度对于特定任务的影响。在每个关键帧后包含了等待时间，来等待效应器执行所需要改变的角度。由于仿真周期是20ms，因此该等待时间应该为0.02的整数倍。

底层代码中skills文件夹下包含了skill类的定义、从外部文件解析动作的程序以及动作技能的示例文件。

1. 技能类的实现

其中skill类的声明如下：

class Skill {
private:
protected:
    vector< boost::shared_ptr<KeyFrame> > keyFrames;
    int currentKeyFrame;
    bool currentKeyFrameSet;
    double currentKeyFrameSetTime;
public:
    Skill();
    ~Skill() {};
    void reset();
    bool done(BodyModel *bodyModel, const WorldModel *worldModel);
    void execute(BodyModel *bodyModel, const WorldModel *worldModel);
    bool canExecute(const BodyModel *bodyModel, const WorldModel *worldModel) const;
    void appendKeyFrame( boost::shared_ptr<KeyFrame> keyFrame );
    boost::shared_ptr<Skill> getReflection(BodyModel *bodyModel);
    void display();
    int getCurrentKeyFrame();
};

一个技能由多个描述关节状态的关键帧（KeyFrame）组成，技能包括如下功能：重置、是否完成、执行、是否可以执行、添加关键帧、获取反馈、显示以及获取当前执行的关键帧。这些函数能够帮助机器人比赛过程中选择合适的时机执行相应的技能，从而完成相应的任务。在高级决策中，实际上所需要决定的便是对现有技能的组合与执行（可以将行走也看作一个特殊的动作技能）。通过所观测到的球场信息，决定当前时刻所需要执行的技能，以此反复，便是完整的比赛过程。因此技能是球员水平的基础，也是决定比赛胜利的关键所在。

一个特定的技能应当由多个关键帧组成，为描述关键帧，代码中设定了KeyFrame类，以下是该类的声明：

class KeyFrame {
private:
protected:
    vector< boost::shared_ptr<Macro> > macros;
    bool toWaitTime; // Default: false.
    double waitTime; // Default: 0.
    bool toWaitTargets; // Default: false.
    double maxWaitTime; // Default: INF.
    void updateCurveMacros(BodyModel *bodyModel, const WorldModel *worldModel, const double & t);
public:
    KeyFrame();
    ~KeyFrame() {};
    void appendMacro(boost::shared_ptr<Macro> macro);
    void execute(BodyModel *bodyModel, const WorldModel *worldModel); // Mainly a set.
    void setToWaitTime( bool value );
    void setWaitTime( double value );
    void setToWaitTargets( bool value );
    void setMaxWaitTime( double value );
    bool done(BodyModel *bodyModel, const WorldModel *worldModel, const double &setTime);
    boost::shared_ptr<KeyFrame> getReflection(BodyModel *bodyModel);
    virtual bool canExecute(const BodyModel *bodyModel, const WorldModel *worldModel) const;
    void display();
};

一个关键帧由多个宏指令（macros）组成，每个宏指令代表着一个关节的角度和旋转速度。关键帧包括如下功能：添加宏指令、执行、设定是否等待、设定等待时间、设定是否等待到达目标、设定最大等待时间、是否完成、获取反馈、是否可以执行以及显示。这些函数能够确定一个特定姿态的机器人，通过不同关键帧的执行，从而实现一个完整的动作。

一个宏指令（macros）是描述动作技能的最基本单元，其声明如下：

class Macro {
private:
protected:
public:
    Macro() {}
    virtual ~Macro() {}
    virtual void execute(BodyModel *bodyModel, const WorldModel *worldModel) = 0;
    virtual boost::shared_ptr<Macro> getReflection(BodyModel *bodyModel) = 0;
    virtual void display() = 0;
    virtual bool canExecute(const BodyModel *bodyModel, const WorldModel *worldModel) const {
        return true;
    }
};

可以看到，Macro类中主要的函数均声明为了纯虚函数，因此其为一个抽象基类，采用所有子类来实现不同类型的宏指令，帮助实现不同的关节状态。主要有如下子类：

  1. IncTar：增量目标类，用于设置关节角度的增量。它包含以下成员变量：
    • effectorIDs：关节ID的向量。
    • increments：关节角度增量的向量。
  2. SetTar：设置目标类，用于设置关节角度的目标值。它包含以下成员变量：
    • effectorIDs：关节ID的向量。
    • targetAngles：关节角度的目标值的向量。
  3. SetFoot：设置脚部类，用于设置脚部的6DOF姿态。它包含以下成员变量：
    • legIDX：脚部的索引（左脚或右脚）。
    • targetPos：目标姿态，包括XYZ偏移和绝对RPY角度。
  4. Curve：曲线类，用于描述脚部要跟随的曲线。它包含以下成员变量：
    • legIDX：脚部的索引（左脚或右脚）。
    • curve：XYZ曲线，相对于球的位置。
    • rpy_curve：RPY曲线。
    • curveOffsetWrtTorso：曲线相对于躯干的偏移。
    • curveOffsetWrtBall：曲线相对于球的偏移。
    • t：时间参数。
  5. Reset：重置类，用于重置关节角度。它包含以下成员变量：
    • components：需要重置的关节的向量。
  6. SetScale：设置比例类，用于设置关节角度的比例。它包含以下成员变量：
    • effectorIDs：关节ID的向量。
    • targetScales：关节角度的比例值的向量。
  7. Stabilize：稳定类，用于使中心质量在给定的脚部上平衡。它包含以下成员变量：
    • legIndex：脚部的索引（左脚或右脚）。
    • zmp：相对于脚部的平衡位置。

这些子类分别描述了一种关节变化的类型，通过分配不同类型的关节变化，实现两种关键帧之间的转移，再由多个关键帧的顺序执行来实现一个预期的动作技能。

2. 技能文件的定义

如上文所介绍，为便于定义动作技能，该团队开发了一种技能语言，用于描述一个特定的动作。配合skillparser.h文件，可以实现从文件到skill技能的转换。

该技能语言的描述如下：

开始并命名技能STARTSKILL <name_of_skill>
开始关键帧STARTSTATE
设定目标角度settar <joint> <target_angle> end
增加目标角度inctar <joint> <amount_to_increase_angle> end
设置比例（关节的比例控制器值）setscale <joint> <scale> end
重置关节reset <joint> end
将质心移至给定脚的ZMP值stabilize <LEG_LEFT | LEG_RIGHT> <x_zmp_value> <y_zmp_value>
将脚移动到相对于球的预设位置setfoot <LEG_LEFT | LEG_RIGHT> <xoffset> <yoffset> <zoffset> <role> <pitch> <yaw> end
 STARTCURVE <LEG_LEFT | LEG_RIGHT>
通过相对于球的控制点路径移动脚controlpoint end
 ENDCURVE
等待指定时间wait <time> end
结束关键帧ENDCURVE``ENDSTATE`
结束技能描述ENDSKILL
反射技能（根据镜像以创建另一个技能）REFLECTSKILL <name_of_skill_to_reflect> <reflected_skill_name>

一个最基本的站立技能的文件描述如下（位于skills下的stand.skl文件）：

STARTSKILL SKILL_STAND
STARTSTATE
reset ARM_LEFT ARM_RIGHT LEG_LEFT LEG_RIGHT end
inctar EFF_LL3 19 EFF_LL4 -40 EFF_LL5 20 EFF_RL3 19 EFF_RL4 -40 EFF_RL5 20 end
wait 0.04 end
ENDSTATE
ENDSKILL

上述是一个定义站立动作的skl文件，该动作名称为SKILL_STAND，其中中仅包含了一个关键帧，在该关键帧中，使用reset指令将左胳膊、右胳膊、左腿、右腿上所有关节重置到默认位置，并增加（减少）了六个关节的目标角度。在该关键帧后，设定了0.04秒的等待时间以等待关节到达指定位置。这是一个最为简单的示例，在该文件加下还定义了kick.skl和kick_ik_0.skl两个踢腿行为，其中使用了更加丰富的语法和更多的关键帧，由于文件较长，在此不再展示。

3.3.8 广播交流

在第二章中，我们了解到，Nao机器人包含了听觉感知器和广播效应器，支持队伍之内各个队员进行信息的交流。不过由于限制，机器人智能发送有限的信息，为增加所传递的信息的容量，需对其进行编码和解码处理。

底层代码中audio文件夹下，实现了编码和解码的相关内容。根据限制，通信过程中可采用92个字符，但该团队仅使用了其中的64个字符（包括abcdefghijklmnopqrstuvwxyzABCDEFGHIJKLMNOPQRSTUVWXYZ0123456789*#），这意味着该底层代码中采用的通信系统仍有可提升的空间。广播系统中的主要函数如下：

通用函数：

  1. intToBits：将整数转换为二进制位向量。
  • intToBits(const int &n, const int &numBits)：将整数 n 转换为 numBits 位长的二进制位向量。
  • intToBits(const unsigned long long &n, const int &numBits)：将无符号长整数 n 转换为 numBits 位长的二进制位向量。
  1. bitsToInt：将二进制位向量转换为整数。
  • bitsToInt(const std::vector &bits, const int &start, const int &end)：将二进制位向量 bits 中从 start 到 end 的位转换为整数。

编码函数：

  1. makeSayMessage：生成说消息。
  • makeSayMessage(const int &uNum, const double &currentServerTime, const double &ballLastSeenServerTime, const double &ballX, const double &ballY, const double &myX, const double &myY, const bool &fFallen, std::string &message)：生成说消息，包含代理的编号、当前服务器时间、球最后看到的服务器时间、球的位置、代理的位置和是否摔倒的信息。
  1. dataToBits：将数据转换为二进制位向量。
  • dataToBits(const double &time, const double &ballLastSeenTime, const double &ballX, const double &ballY, const double &myX, const double &myY, const bool &fFallen, std::vector &bits)：将时间、球最后看到的时间、球的位置、代理的位置和是否摔倒的信息转换为二进制位向量。
  1. bitsToString：将二进制位向量转换为字符串。
  • bitsToString(const std::vector &bits, std::string &message)：将二进制位向量 bits 转换为字符串。

解码函数：

  1. processHearMessage：处理听到的消息。
  • processHearMessage(const std::string &message, const double &heardServerTime, int &uNum, double &ballLastSeenServerTime, double &ballX, double &ballY, double &agentX, double &agentY, bool &fFallen, double &time)：处理听到的消息，提取代理的编号、球最后看到的服务器时间、球的位置、代理的位置和是否摔倒的信息。
  1. bitsToData：将二进制位向量转换为数据。
  • bitsToData(const std::vector &bits, double &time, double &ballLastSeenTime, double &ballX, double &ballY, double &agentX, double &agentY, bool &fFallen)：将二进制位向量 bits 转换为时间、球最后看到的时间、球的位置、代理的位置和是否摔倒的信息。
  1. stringToBits：将字符串转换为二进制位向量。
  • stringToBits(const std::string &message, std::vector &bits)：将字符串 message 转换为二进制位向量。

在向服务器发送消息时使用编码函数，服务器消息解析时使用解码函数，可以实现不同球员之间消息的通信，从而便于团队合作与决策。

3.4 高级模块

3.4.1 RoboViz绘图

​ RoboViz作为比赛的监视器，可以帮助观测比赛的情况。同时，其还支持智能体向其发送命令，在球场上绘制不同的图形，来帮助对程序进行调试。其支持多种颜色和形状，通过接收智能体发送的绘图命令，在RoboViz上显示相关内容。底层代码中，rvdraw文件夹下实现了相关功能。

TODO：展示绘图效果

​ rvdraw.h和rvdraw.cc文件中定义了RVSender类，该类用于在比赛过程中通过套字节向RoboViz发送绘图命令。RoboViz默认接收绘图指令的端口为32769，因此在绘图时，程序需要与RoboViz运行主机的32769端口进行连接。在该类中所支持的颜色类型为11种，其枚举类型如下：

enum Color {
        RED       = 1, ORANGE      =  2, YELLOW    =  3, GREEN     = 4,
        BLUEGREEN = 5, LIGHTBLUE   =  6, BLUE      =  7, DARKBLUE  = 8,
        VIOLET    = 9, PINK        = 10, MAGENTA   = 11
    };

目前，底层代码中支持绘制的图像方法如下：

drawCircle绘制圆圈
drawLine绘制线段
drawText绘制文本
drawPoint绘制点
drawSphere绘制球体
drawPolygon绘制多边形
drawAgentText绘制文本(在智能体位置)
drawAnnotation绘制注释

在程序过程中可通过worldModel->getRVSender()来调用上述绘图函数，例如在球的位置绘制一个点，可通过如下方法实现：

worldModel->getRVSender()->clear(); // 清除上个周期中的图形
worldModel->getRVSender()->drawPoint("ball", ball.getX(), ball.getY(), 10.0f, RVSender::MAGENTA);

通过上述方法，可以在球的位置绘制一个半径为10像素、颜色为洋红色的原点。

3.4.2 参数优化

1. 参数文件的加载

​ 为便于球队运行过程中对动态参数的优化，底层代码中支持通过外部文件来加载所需的参数。参数文件应以字符串到浮点数的键值对的形式格式化一组参数。参数名应与它的值用制表符（不是空格）分隔，参数之间应用单个换行符分隔。参数文件支持C++风格的注释，包括//、/* */和#。在paramfiles文件夹下，包含六个有效的参数文件（defaultParams.txt、defaultParams_t0.txt、defaultParams_t1.txt、defaultParams_t2.txt、defaultParams_t3.txt、defaultParams_t4.txt）。

​ 参数文件通过--paramsfile <parameter_file>命令行参数指定和加载，并且可以通过一个参数文件后跟另一个参数文件来加载多个参数文件，新加载的参数值将替换具有相同名称的先前加载的参数值。程序中，所有参数被加载到一个名为namedParams的std::map中。在加载任何其他参数文件之前，所有代理在启动时应首先加载defaultParams.txt参数文件，然后根据代理的体型加载适当的*defaultParams_t.txt*参数文件。例如对于类型4的机器人，在命令行中与加载参数文件有关的命令应该是：

--paramsfile paramfiles/defaultParams.txt --paramsfile paramfiles/defaultParams_t4.txt

​ 在main.cc文件中定义了map<string, string> namedParams变量用于存储参数名-参数，并使用LoadParams函数对指定的参数文件读取相关参数。在程序中访问时，可以采用namedParams[key]的方式获取参数名为key的参数值。对于技能文件中包含参数的技能，在NaoBehavior类中定义了readSkillsFromFile()函数，能够读取指定的动作文件，其中在遇到以$标识的参数时，将会自动在namedParams中查找相关参数。例如在skills文件夹下定义的kick.skl动作文件中所需的13个参数，均可在defaultParams.txt中找到。同时，在defaultParams_t1.txt中也可找到，不过此时的参数已经替换掉原先的参数，从而实现了对于类型1的机器人执行动作时，采用的是特殊的参数。与之类似，在各参数文件中对于步态参数也存在许多不同的类型。这些主要是采用优化方法，对基本参数进行优化后，适用于特定任务的参数，例如适用于快速走向目标位置、保持姿态稳定、针对于带球等任务。通过在执行不同任务时，采用合适的参数，达到提高速度与稳定性的效果。

2. 优化示例

​ 在球队代码中展示了一个用于优化参数的示例，位于optimization文件夹下，该示例可以指定额外参数，测试并得到使用该参数执行相应任务的适应度，作为衡量该套参数的标准。在此基础上，可以采用其他的方法，例如CMAES（协方差矩阵自适应进化）算法、强化学习等对该参数进行调整，达到更好的效果。

kick任务：

​ 为实现对踢球相关参数的测试，optimizationbehavior函数中从Naobehavior类中派生了OptimizationBehaviorFixedKick子类，该类中定义了selectSkill()和updateFitness()函数，用于控制机器人执行相应任务和计算适应度。selectSkill()实现如下：

SkillType OptimizationBehaviorFixedKick::selectSkill() {
    double time = worldModel->getTime();
    if (timeStart < 0) {
        initKick();
        return SKILL_STAND;
    }
    if (time-timeStart <= INIT_WAIT_TIME) {
        return SKILL_STAND;
    }
    if (!hasKicked) {
        hasKicked = true;
        return SKILL_KICK_LEFT_LEG; // 正在优化的动作技能
    }
    return SKILL_STAND;
}

​ 如上可知，selectSkill中仅实现了一个功能，即控制机器人执行一次SKILL_KICK_LEFT_LEG动作技能。该动作执行成功过后，Naobehavior中定义的Think函数的循环中会执行子类的updateFitness()函数，用于更新该次任务的适应度。updateFitness函数的主要流程如下：

  1. 初始化和变量定义：
    • totalFitness是一个静态变量，用于累计适应度值。
    • kick是一个计数器，用于记录踢球的次数。
    • timeStart记录踢球开始的时间。
    • beamChecked是一个布尔变量，用于标记是否已经检查过踢球是否成功。
    • failedLastBeamCheck也是一个布尔变量，用于标记上一次踢球是否失败。
    • hasKicked标记机器人是否已经踢球。
    • ranIntoBall标记机器人是否与球相撞。
    • backwards标记踢球是否是向后踢。
    • fallen标记机器人是否摔倒。
  2. 检查踢球次数：
    • 如果kick等于10，表示已经完成了10次踢球，此时将totalFitness除以踢球次数并写入输出文件，然后返回。
  3. 获取时间和位置信息：
    • 获取当前时间time和机器人的真实位置meTruth，并将meTruth的Z坐标设为0。
  4. 等待初始时间：
    • 如果当前时间与timeStart之差小于等于INIT_WAIT_TIME，则返回，表示等待初始时间。
  5. 检查踢球是否成功：
    • 如果beamChecked为false，表示还没有检查过踢球是否成功。
    • 获取机器人的期望位置meDesired，计算真实位置与期望位置的距离distance，以及真实角度与期望角度的差值angle。
    • 获取球的真实位置ballPos和球的位置大小ballDistance。
    • 如果距离或角度不符合预期，则记录日志，增加kick计数，减少totalFitness值，并重新初始化踢球。
  6. 检查是否踢球：
    • 如果hasKicked为false，表示还没有踢球，则返回。
  7. 检查球的位置和机器人状态：
    • 如果球的X坐标小于-0.25，则标记为backwards。
    • 如果机器人摔倒，则增加totalFitness值，增加kick计数，并重新初始化踢球。
  8. 检查踢球结果：
    • 如果时间超过INIT_WAIT_TIME加上15秒，并且球没有移动，则计算适应度值。
    • 如果是向后踢、距离不足或与球相撞，则适应度值为-100。
    • 记录日志，增加totalFitness值，增加kick计数，并重新初始化踢球。

walk任务：

​ 与踢球任务类似，为实现对踢球相关参数的测试，optimizationbehavior函数中从Naobehavior类中派生了OptimizationBehaviorWalkForward子类，该类中定义了selectSkill()和updateFitness()函数，用于控制机器人执行相应任务和计算适应度。其中selectSkill()实现的任务是控制机器人以最大速度向对方球门中心移动，updateFitness()函数记录机器人在一定时间内移动的距离，并根据是否摔倒等情况计算适应度，在完成10次行走后，计算平均值并写入输出文件。

​ 与kick任务不同的是，由于初始化球场时，球位于球场的中心，机器人行走过程中会造成碰撞，影响评估结果。为避免这种情况，在该过程中，使用教练命令，向监视器端口发送指定命令，移动球位于不影响机器人行动的位置。

3. 优化环境

​ 在优化过程中，额外的噪声可能会影响测量值的准确性，特别是在使用worldModel->getMyPositionGroundTruth()、worldModel->getMyAngDegGroundTruth()和worldModel->getBallGroundTruth()方法时，地面真实信息对于获取准确的测量值和正确的值非常重要。

​ 为启用地面真实信息，需要编辑<server_install_dir>/share/rcssserver3d/rsg/agent/nao/naoneckhead.rsg文件，并将setSenseMyPos、setSenseMyOrien和setSenseBallPos的值设置为true。

​ 除此之外，如果代理需要始终准确地知道自己在球场上的位置（例如，在优化行走时需要代理故意走到球场上的特定目标点），则可能需要调用worldModel->setUseGroundTruthDataForLocalization(true)。

​ 在优化过程中，往往需要大量重复的测试，20ms的仿真周期中大部分时间为空闲时间。为加快优化的效率，可以采用以下操作，加快服务器的仿真速度。

  1. 关闭实时模式并开启同步模式：
    • 在~/.simspark/spark.rb文件中，将$agentSyncMode设置为true，以启用同步模式。同步模式可以减少仿真过程中的延迟，从而提高运行速度。
    • 在<server_install_dir>/share/rcssserver3d/rcssserver3d.rb文件中，将$enableRealTimeMode设置为false，以关闭实时模式。
  2. 关闭定位噪声：
    • 如果在检查被定位的代理的位置时（如在示例优化任务中），可能需要关闭定位噪声。定位噪声会影响定位检测的准确性。
    • 在<server_install_dir>/share/rcssserver3d/naosoccersim.rb文件中，将BeamNoiseXY和BeamNoiseAngle设置为0，以关闭定位噪声。

3.4.3 高级行为与决策

1. 高级行为

​ main.cc文件中接收服务器消息和产生发送信息分别作为behavior类中纯虚函数Think()的输入与输出，是球队代码运行的核心。Behavior作为基类，为更高级的行为提供了接口，实现了智能体运行过程中的多态性。

NaoBehavior

​ NaoBehavior类是Behavior的派生类，其重写并实现了基类中的重要函数，如初始化、思考过程。除此之外，NaoBehavior针对Nao机器人拓展了更多的功能，例如控制关节、进行决策等，完整的实现了智能体在比赛过程中所需的所有功能。使用NaoBehavior高级行为可以控制Nao机器人的行为，包括行走、踢球、碰撞避免等。其提供了一系列函数，用于获取和设置机器人的状态，以及选择和执行不同的技能。这个类可以用于机器人足球比赛中的行为控制，帮助机器人完成传球、射门等动作。

PKShooterBehavior与PKGoalieBehavior

​ PKShooterBehavior类与PKGoalieBehavior均是是NaoBehavior类的派生类，其被设计的目的是为比赛过程中点球比赛。在点球比赛中，双方各自仅能派出两名球员，一个点球球员，一个作为守门员，双方轮流踢球，直至一方获胜。PKShooterBehavior类重写了NaoBehavior类中的beam函数与selectskill函数，以实现新的初始化定位和专注于进攻的技能选择。同样地，PKGoalieBehavior类也重写了beam函数与selectskill函数，将该球员初始化于球门前并执行守门策略。

SimpleSoccerBehavior

​ SimpleSoccerBehavior类是NaoBehavior类的派生类，如果说NaoBehavior类实现了一个包含特殊技能的智能体，那么，SimpleSoccerBehavior类便实现了将11个球员综合起来，形成一支拥有合作和策略的足球队伍。在SimpleSoccerBehavior类中除重写beam函数与selectskill函数，为实现团队合作以及不同球员的角色分配，其实现了基于简单贪婪算法的动态角色分配。在比赛过程中，将动态地为场上的分配角色与目标位置，来达到便于进攻或者防守的阵型。目前该策略分配的角色如下：

enum Role {
    GOALIE,  SUPPORTER,  BACK_LEFT,  BACK_RIGHT,
    MID_LEFT,  MID_RIGHT,  WING_LEFT,  WING_RIGHT,
    FORWARD_LEFT,  FORWARD_RIGHT,  ON_BALL,  NUM_ROLES
};

运行时效果如下：

2. 决策

​ 决策作为球队运行的核心，决定着比赛过程中球员的配合和及时的技能分配。它涉及到策略制定、行为选择、目标设定、决策执行和决策调整等多个方面。通过有效的决策制定和执行，机器人可以更好地完成比赛任务，提高比赛表现和比赛结果。正如上文所述，决策实际上就是不同技能的组合。因此，决策功能的实现一般位于NaoBehavior类中的selectskill函数。其定义如下：

 virtual SkillType selectSkill();

​ 其定义为一个虚函数，因此其派生类可通过重写该函数实现不同的功能。返回类型为SkillType，这是一个自定义的数据类型，表示技能的类型，球员所拥有的所有技能均在header.h中定义。目前所有技能类型如下：

enum SkillType {
    SKILL_WALK_OMNI,
    SKILL_STAND,
    SKILL_KICK_LEFT_LEG,
    SKILL_KICK_RIGHT_LEG,
    SKILL_KICK_IK_0_LEFT_LEG,
    SKILL_KICK_IK_0_RIGHT_LEG,
    SKILL_NONE
};

​ 这意味着选择的技能必须包含在上述的枚举中，其包括全向行走、站立、左腿踢、右腿踢、左腿踢（采用逆运动学的0类型）、右腿踢（采用逆运动学的0类型）以及空技能。为将策略功能独立展示，球队代码中将selectskill函数的实现写入behaviors文件夹下的strategy.cc文件。在其中的selectskill函数中设计了一个demoKickingCircle函数，该函数实现了11个球员围绕半场中心旋转并不断将球踢向中心的示例。

​ 当然仅实现该示例是达不到进球的目的，应当如SimpleSoccerBehavior中介绍的，设计球员角色配合，选择合适的技能从而实现进球。为实现这一目的需要更多的设计和方法，后续将继续介绍。

You May Also Enjoy

WIFI CSI感知学习

less than 1 minute read

Published:

这是一篇个人学习笔记，记录了WIFI CSI感知学习的一些内容，包括CSI的定义、获取方式、应用场景等。

毫米波雷达基本原理教程

5 minute read

Published:

这是一篇关于毫米波雷达基础知识的教程,主要介绍了线性调频连续波(LFMCW)雷达的基本原理,并通过一个实际探测需求,设计毫米波雷达的相关参数,并进行仿真验证。

RoboCup仿真3D中文教程-第4章

5 minute read

Published:

介绍机器人关节、铰链、动作、ODE运动模型、状态空间，五种机器人的变体、各关节所在位置、属性