CN103984235B - 基于c/s结构的空间机械臂控制系统软件架构及构建方法 - Google Patents

Abstract

本发明公开了一种基于C/S结构的空间机械臂控制系统软件架构及构建方法，该架构针对Server端的中央控制器设计，1)以多线程和循环队列为支撑，搭建架构整体框架；2)四个线程分别承担不同的功能，共同实现空间机械臂控制系统软件总体功能，线程之间并行运行，线程之间数据交换采用全局变量或自定义消息，线程之间同步采用临界区对象方式；3)两个循环队列用来建立缓存机制，其中指令循环队列仅缓存一条指令帧，信息循环队列可缓存多条信息帧。本发明采用多线程及循环队列技术实现了空间机械臂控制系统软件架构及构建方法，实践证明，适用于空间机械臂嵌入式平台，可有效降低空间机械臂控制系统软件的开发难度，缩短开发周期。

Description

基于C/S结构的空间机械臂控制系统软件架构及构建方法

技术领域

本发明属于空间机械臂控制系统软件架构领域，特别涉及基于C/S结构的空间机械臂控制系统软件架构及构建方法

背景技术

空间技术的研究始于20世纪50年代，50多年来技术进步之快、应用范围之广，特别是空间站、航天飞机、空间机器人的诞生及成功应用，使得空间技术的重要地位已被世界各国广泛重视。空间机械臂作为在轨支持、服务的一项关键性技术已经进入太空(如加拿大的SRMS、美国的MSS、俄罗斯的ERA以及日本的JEMRMS等)，并越来越受到人们的关注。美国有权威早曾预言：21世纪国家对航天能力的依赖可与19、20世纪国家对电力、石油的依赖相比拟。目前，世界航天强国正在积极探索地球系统以外空间，我国在载人航天技术方面也取得了长足进步，世界空间技术发展迅猛。

进入21世纪以来，我国空间技术领域飞速发展，对空间机械臂技术的需求也越来越迫切，然而空间机械臂各项关键技术的研究需要投入更多的人力、物力、财力，进行深入探究，其中适用于空间机械臂嵌入式平台的空间机械臂控制系统软件架构是比较突出的一个问题。同时，这个问题也是困扰整个空间技术领域发展的关键技术之一。

目前，从国内外的技术资料来看，解决这一技术难题主要有两种方法。一种方法是将服务机器人的控制系统软件架构移植到空间机械臂平台上，借助现有架构降低开发难度，这种方法的主要缺点在于服务机器人控制系统软件架构体系过于庞大，难以适用于对硬件资源和功耗要求比较苛刻的空间机械臂平台。同时，当前国内外服务机器人控制系统软件架构为了适应移动网络，正向着B/S(Browser/Server)结构发展，而B/S结构Server端的负载过大，并不适合空间机械臂的遥操作模式。

另一种方法是将通用机器人控制系统软件架构移植到空间机械臂平台上，这是当前国内外研究的热点，这种方法的特点是对国际上比较流行的通用机器人控制系统软件架构进行轻量化处理，如ROS、OROCOS等，保留其强大的功能库，并删除不必要的模块，使其适用于空间机械臂控制平台，如RGMP(Real-timeOperationSystemandGeneralPurposeOperationSystemonMulti-coreProcessor)解决方案，这种方案就尝试对ROS机器人控制系统软件架构加以改造。这种方法的主要问题在于当前的通用机器人控制系统软件架构对Linux操作系统依赖性强，且系统体系过于庞大，功能繁多，实施起来难度大，目前尚处于起步阶段，距离实现还有较大距离。

总体来说，现有技术中空间机械臂控制系统软件架构的控制时延，软件架构体系过于庞大，难以适用于对硬件资源和功耗要求比较苛刻的空间机械臂平台。

发明内容

本发明的目的是提供一种基于C/S结构的空间机械臂控制系统软件架构及构建方法，其能适用于空间机械臂嵌入式平台，有效降低控制时延，优化软件架构体系，降低空间机械臂控制系统软件开发难度，缩短开发周期。

一种基于C/S结构的空间机械臂控制系统软件架构，基于多线程和循环队列技术，采用空间机械臂中央控制器作为空间机械臂控制系统软件架构的服务端，采用空间机械臂的地面遥操作计算机作为空间机械臂控制系统软件架构的客户端，将空间机械臂控制系统软件的各项功能划分为四个功能模块，并为每个功能模块建立一个独立线程，线程之间通过指令循环队列和信息循环队列建立缓存；

所述四个功能模块分别为通信连接与数据接收模块、信息采集及保护模块、指令执行模块、信息数据发送模块，该四个功能模块对应的四个线程分别为架构主线程、信息采集及保护线程、指令执行线程和信息数据发送线程；所述四个线程之间并行运行；

所述循环队列用来建立缓存，均采用数据链表的方式实现，链表的大小在初始化后为固定值。

用于降低各线程处理时间不同步带来的控制延迟，数据链表属于现有技术。

架构主线程接收来自遥操作端、视觉控制器端、力传感器端或机械臂各电机驱动器端发送过来的数据，并对数据进行分类处理：将指令数据压入指令循环队列，并唤醒指令执行线程，而将信息数据存储到相应的全局变量，指令执行线程执行过程产生的执行结果信息和执行错误信息被封装成信息帧，并压入信息循环队列；

指令执行线程被唤醒后，从指令循环队列读取指令数据，并进行数据处理，实施机械臂电机控制，指令执行线程执行过程产生的执行结果信息和执行错误信息被封装成信息帧，并压入信息循环队列；

信息采集及保护线程由主线程的定时机构定时唤醒，借助全局变量，通过架构主线程获取机械臂状态信息，并执行安全策略，最后将状态信息封装成信息帧，压入信息循环队列，同时，信息采集及保护线程执行过程产生的执行结果信息和执行错误信息被封装成信息帧，并压入信息循环队列；

架构主线程、指令执行线程、信息采集及保护线程每次将信息帧压入信息循环队列后，都唤醒一次信息数据发送线程，信息数据发送线程从信息循环队列读取信息帧数据，并发送到遥操作客户端。

所述四个线程优先级从高到低的顺序依次为：架构主线程、信息采集及保护线程、指令执行线程及信息数据发送线程；

所述架构主线程用于控制通信及协调任务，包括通信连接模块、数据接收模块、预处理模块和定时机构四个功能模块；

其中，通信连接模块用于与遥操作端计算机、视觉控制器、力传感器及机械臂各电机驱动器建立通信连接；

数据接收模块用于接收通信连接模块输出过来的数据，并将指令数据送往预处理模块，而将信息数据存储到相应的全局变量中，以供信息采集及保护线程使用；

预处理模块用于进行紧急指令响应，如紧急停止指令的响应，并将不需要紧急响应的指令压入指令循环队列，以供指令执行线程使用；

定时机构用于定时唤醒信息采集及保护线程，执行信息采集及保护策略；

所述信息采集及保护线程用于采集力传感器及机械臂各电机的状态信息，并执行安全保护策略，包括信息获取模块、安全保护模块、信息帧封装模块；

其中，信息获取模块用于获取状态信息，包括电机信息获取模块、限位器状态获取模块、力传感器信息获取模块，依次用于获取机械臂各电机状态信息、各限位器状态信息、力传感器信息；

安全保护模块用于判断是否有状态超标，如有超标则执行安全策略——紧急停止机械臂所有操作，锁定机械臂各电机，清空指令循环队列；

信息帧封装模块用于将以上获取的状态信息封装成帧，并压入信息循环队列；

所述指令执行线程用于解析指令帧，获取指令参数，进而实施机械臂电机控制，包括指令读取模块、指令解析模块、指令验证模块、参数提取模块、运动规划模块、驱动指令生成模块、电机控制模块；

其中，指令读取模块用于从指令循环队列中读取控制指令，且一次性读出指令循环队列内所有数据；

指令解析模块用于识别指令读取模块取出的指令；

指令验证模块用于验证指令的完整性和合法性，通过验证帧头帧尾是否正确来实现；

参数提取模块用于去除控制指令帧内的无效信息，提取出有效控制参数；

运动规划模块用于根据控制参数进行机械臂运动规划，并在需要时向视觉控制器端发送目标位姿请求指令，之后借助架构主线程的数据接收模块获得目标位姿；

驱动指令生成模块，用于将运动规划模块规划完成的指令转换为电机驱动指令；

电机控制模块用于向机械臂各电机驱动器发送驱动指令，实施机械臂控制；

所述信息数据发送线程用于将信息帧发送到遥操作端计算机，包括信息帧读取模块、信息帧验证模块、信息帧发送模块；

其中，信息帧读取模块用于从信息循环队列内读取信息帧，且每次仅读取一条完整的信息帧；

信息帧验证模块用于验证所取出的信息帧是否完整和合法，通过验证帧头帧尾来实现；

信息帧发送模块用于将传送过来的信息帧发送到遥操作端计算机。

所述信息循环队列缓存来自信息采集及保护线程的状态信息帧，以及其他三个线程执行过程中产生的错误信息帧和执行结果信息帧；

所述指令循环队列仅缓存一条指令帧。

一种基于C/S结构的空间机械臂控制系统软件架构构建方法，采用所述的基于C/S结构的空间机械臂控制系统软件架构，包括以下步骤：

1)以空间机械臂控制系统软件进程的主线程为基础构建架构主线程，实现架构主线程所有功能，并由架构主线程初步建立信息采集及保护线程函数、指令执行线程函数、信息数据发送线程函数；

2)建立架构指令循环队列的入队列操作函数和出队列操作函数，并实现指令循环队列的入队和出队操作；

3)建立架构信息循环队列的入队列操作函数和出队列操作函数，并实现信息循环队列的入队和出队操作；

4)在信息采集及保护线程函数内实现信息采集及保护线程的所有功能；

5)在指令执行线程函数内实现指令执行线程的所有功能；

6)在信息数据发送线程函数内实现信息数据发送线程的所有功能。

所述以空间机械臂控制系统软件进程的主线程为基础构建架构主线程，具体构建步骤如下：

步骤1：建立遥操作端的TCP/IP网络连接服务并监听连接请求，建立视觉服务器端的TCP/IP网络连接服务并监听连接请求，建立力传感器端的UDP网络连接服务并绑定端口与地址，建立与机械臂各电机驱动器的CAN网络连接并初始化机械臂各电机驱动器；

步骤2：创建遥操作端数据接收消息响应函数、视觉服务器端数据接收消息响应函数、力传感器端数据接收消息响应函数及机械臂各电机驱动器端数据接收回调函数；

步骤3：创建信息采集及保护线程函数、指令执行线程函数、信息数据发送线程函数，并挂起；

步骤4：采用数据链表的方式建立指令循环队列和信息循环队列，并分配存储空间，完成初始化；

步骤5：初始化电机状态信息变量、限位器状态信息变量、力传感器状态信息变量，三个变量均设置为全局变量，以供不同线程间共同访问；

步骤6：建立定时机构，定时激活信息采集及保护线程；

步骤7：创建数据预处理函数，对接收到的数据进行预处理，指令数据预处理方式为：对紧急处理指令做出紧急处理，并将非紧急处理指令压入指令循环队列，对状态数据进行全局状态变量更新的预处理。

所述建立架构指令循环队列的入队列操作函数和出队列操作函数的具体构建步骤如下：

步骤1：创建指令循环队列入队列操作函数，入队列操作函数执行指令数据入队前，先判断指令循环队列是否空，如果不空，就循环等待，否则执行入队操作，入队时采用队列常用队尾指针操作方式，以字节为单位逐个拷贝至循环队列内；

步骤2：创建指令循环队列出队列操作函数，出队列操作函数执行出队操作前，先判断指令循环队列是否空，如果不空就执行出队操作，否则不执行任何动作，出队时采用队列常用队首指针操作方式，以字节为单位逐个取出，并一次性取出指令循环队列内所有数据。

所述建立架构信息循环队列的入队列操作函数和出队列操作函数的具体构建步骤如下：

步骤1：创建信息循环队列入队列操作函数，入队列操作函数执行信息数据入队前，先判断信息循环队列是否满，如果满，就循环等待，否则执行入队操作，入队时采用队列常用队尾指针操作方式，以字节为单位逐个拷贝至循环队列内；

步骤2：创建信息循环队列出队列操作函数，出队列操作函数执行出队操作前，先判断信息循环队列是否空，如果不空就执行出队操作，否则不执行任何动作，出队时采用队列常用队首指针操作方式，以字节为单位逐个取出，并一次性仅取出一条完整的信息帧。

所述信息采集及保护线程的功能通过信息采集及保护线程函数实现，具体步骤如下：

步骤1：线程被唤醒后，首先向机械臂各电机驱动器和力传感器发送状态请求指令，通过所述架构主线程接收状态信息，并存储到电机状态信息变量、限位器状态信息变量、力传感器状态信息变量中；

步骤2：信息采集及保护线程读取电机状态信息变量获得电机状态信息，读取限位器状态信息变量获取各限位器状态信息，读取力传感器状态信息变量获取力传感器信息；

步骤3：通过验证以上各状态信息是否存在超标量执行安全策略，如果存在超标量就紧急停止机械臂，防止机械臂受损，并封装相应的错误信息帧，通过信息循环队列入队列操作函数压入信息循环队列，并唤醒信息数据发送线程；否则，将以上各状态信息封装成机械臂状态信息帧，通过信息循环队列入队列操作函数压入信息循环队列，并唤醒信息数据发送线程。

步骤4：采集完成后，所述信息采集及保护线程自动挂起，直到下次被架构主线程的定时机构唤醒。

所述指令执行线程的功能通过指令执行线程函数实现，具体步骤如下：

步骤1：线程被唤醒后，首先判断指令循环队列是否空，如果空，就自动挂起指令执行线程，否则通过指令循环队列出队列函数取出队列内所有数据；

步骤2：进行帧解析，通过控制帧的标识信息，识别控制指令，并通过判断帧头帧尾的正确性来实施指令验证，来确定指令帧的合法性和完整性；

步骤3：将指令帧内的控制参数提取出来，并根据控制参数进行运动规划，生成电机驱动指令；

步骤4：判断机械臂与各电机驱动器连接是否正常，如果正常，就调用相应的电机控制函数，将电机驱动指令发送到相应的电机驱动器，实施机械臂电机控制；

步骤5：判断执行是否成功，如果成功，就封装相应的执行成功信息帧，通过信息循环队列入队列操作函数，将信息帧压入信息循环队列，并唤醒信息数据发送线程，返回步骤1；

步骤6：在本线程执行以上各步骤是，如果遇到错误信息，就停止执行，并封装相应的错误信息帧，通过信息循环队列入队列操作函数，将信息帧压入信息循环队列，并唤醒信息数据发送线程，返回步骤1。

所述信息数据发送线程的功能由信息数据发送线程函数实现，具体步骤如下：

步骤1：线程被唤醒后，首先判断信息循环队列是否空，如果空，就挂起信息数据发送线程，否则通过信息循环队列出队列操作函数取出一条完整指令帧；

步骤2：验证取出指令帧的帧头、帧尾，判断帧的完整性与合法性，如果都合法就进入下一步，否则封装相应的错误信息帧，并替代取出的指令帧，进入下一步；

步骤3：判断控制器软件与目标端网络连接是否正常，如果不正常就挂起信息数据发送线程，否则，将传递过来的信息帧发送到目标计算机，发送完毕后，返回步骤1。

有益效果

本发明由于采用以上技术方案，具备以下优点：1、发明基于多线程与循环队列等现有技术，实施过程简单，可有效降低空间机械臂控制系统软件的开发难度，缩短开发周期；2、发明结构简单，针对空间机械臂控制平台设计，能够很好地适应空间机械臂嵌入式平台，且与操作系统无关，不依赖于Linux操作系统；3、发明将空间机械臂控制系统软件的各项功能合理地分配到四个并行线程中，兼顾了机械臂控制系统功能的多样性与性能的高效性，各模块之间独立性高，方便维护和升级改造；4、发明具备由两个循环队列组成的缓存机制，能有效减少控制系统软件数据处理过程中不必要的等待时间，降低控制延迟。

附图说明

图1为本发明所基于的C/S结构空间机械臂控制系统拓扑图；

图2为本发明的空间机械臂控制系统软件功能分析及模块划分图；

图3为本发明的基于C/S结构的空间机械臂控制系统软件架构设计图；

图4为本发明的架构主线程构建方法流程图；

图5为本发明的指令循环队列构建方法的操作流程图；

图6为本发明的信息循环队列构建方法的操作流程图。

图7为本发明的信息采集及保护线程构建方法流程图；

图8为本发明的指令执行线程构建方法流程图；

图9为本发明的信息数据发送线程构建方法流程图。

具体实施方式

下面结合附图和实施例对本发明做进一步详细说明。

一种基于C/S结构的空间机械臂控制系统软件架构，其包括以下内容：

1)本发明的所基于的C/S结构空间机械臂控制系统拓扑图如图1所示，空间机械臂的控制指令由地面遥操作端(Client)发送到机械臂上的中央控制器端(Server)，中央控制器在接收到控制指令后，根据指令对机械臂实施相应控制；另一方面，中央控制器还要将机械臂各主要部件的状态信息实时地传送到地面，以便操作人员随时监控机械臂状态。从图中可以看出，中央控制器是整个控制系统的核心所在，不仅负责协调所有部件间的通信，而且本机内部还要执行大量的复杂运算，是决定机械臂控制系统性能的关键，集中体现了空间机械臂控制系统软件的特点，本发明就是针对中央控制器软件而设计。

2)本发明所述空间机械臂控制系统软件架构指的是C/S结构下Server端的软件架构，即空间机械臂中央控制器端软件架构。架构基于多线程和循环队列技术，将机械臂控制系统软件功能划分为四个功能模块，并为每个功能模块建立一个独立线程；同时，在线程之间通过两个循环队列建立缓存机制。所述软件架构运行于空间机械臂中央控制器上，与其进行数据通信的外部部件包括：C/S结构下Client端的地面遥操作计算机、视觉控制器计算机、力传感器及机械臂各电机驱动器。

3)图2为本发明的空间机械臂控制系统软件功能分析及模块划分图，基于该图将空间机械臂控制系统软件划分为四个功能模块，分别为：通信连接与数据接收模块、信息采集及保护模块、指令执行模块、信息数据发送模块，其中信息数据发送模块是因为考虑到中央控制器端与地面遥操作端通信频繁，数据量较大，为提高通信效率而设立的；对应的四个线程为：架构主线程、信息采集及保护线程、指令执行线程、信息数据发送线程，各个线程的具体功能可参考图3。所述两个循环队列为：指令循环队列、信息循环队列。

4)由于对机械臂实施控制的过程是依次进行的，即在机械臂执行当前任务时，不实施下一步任务，所述指令循环队列仅缓存一条指令帧，其大小比最大的指令帧稍大便可。所述信息循环队列可缓存多条信息帧，且不仅缓存来自信息采集及保护线程的状态信息帧，还要缓存其他各线程执行过程中产生的错误信息帧及执行结果信息帧。另外，从图3还可以看出，指令循环队列是用来在架构主线程与指令执行线程之间建立缓存的，仅有架构主线程对其实施入队列操作，也仅有指令执行线程对其实施出队列操作；而信息循环队列是用来在信息数据发送线程与其他三个线程之间建立缓存的，架构主线程、指令执行线程、信息采集及保护线程都会对其实施入队列操作，但仅有信息数据发送线程对其实施出队列操作。所述指令循环队列和信息循环队列由架构主线程创建和初始化。

5)所述架构主线程，还通过通信连接模块与遥操作端、视觉控制器端、力传感器端及机械臂各电机驱动器端建立通信连接；通过数据接收模块接收以上各部件发送过来的数据，并将指令数据送往预处理模块，而将信息数据存储到相应的全局变量中，以供信息采集及保护线程使用；通过预处理模块进行紧急指令响应，并将非紧急处理指令压入指令循环队列，以供指令执行线程进一步处理数据；通过定时机构定时唤醒信息采集及保护线程。同时，架构主线程还负责创建其他三个线程，并将其挂起。架构主线程一直处于活动状态。

6)所述架构主线程的定时机构每隔一段时间唤醒信息采集及保护线程，采集力传感器及机械臂各电机的状态信息，并执行安全保护策略。所述信息采集及保护线程，通过信息获取模块向力传感器端及机械臂各电机驱动器端发送信息请求指令，并借助架构主线程通过全局变量获取相应状态信息，其中，各电机的状态信息及各限位器状态信息，通过相应的电机驱动器获得，力/力矩信息通过力传感器获得；之后通过安全保护模块判断是否有状态超标，执行安全策略，即遇到超标状态时，紧急停止机械臂各电机，并清除所有待执行指令，重置控制系统；最后通过信息帧封装模块，将各种状态信息封装成信息帧，并压入信息循环队列。信息采集及保护线程，在执行完上述任务后自动挂起。

7)所述架构主线程在将非紧急处理指令压入指令队列后，唤醒指令执行线程，执行机械臂控制指令。所述指令执行线程通过指令读取模块从指令循环队列中读取控制指令，通过指令解析模块解析获取的指令，通过指令验证模块验证指令的完整性和合法性，通过参数提取模块提取有效控制参数，通过运动规划模块进行机械臂运动规划，并根据需求向视觉控制器端发送目标位姿请求指令，之后借助架构主线程的数据接收模块获得目标位姿，通过驱动指令生成模块生成电机驱动指令，通过电机控制模块向机械臂各电机驱动器发送驱动指令，实施机械臂控制。指令执行线程，在执行完上述任务后自动挂起。

8)所述架构主线程、信息采集及保护线程、指令执行线程，在每次将信息帧压入信息循环队列后，均唤醒一次信息数据发送线程，将信息帧发送到遥操作端。所述信息数据发送线程通过信息帧读取模块从信息循环队列读取信息帧，通过信息帧验证模块验证帧的完整性和合法性，通过信息帧发送模块将获取的信息帧发送到遥操作端计算机。信息数据发送线程，在执行完上述任务后自动挂起。

一种基于C/S结构的空间机械臂控制系统软件架构构建方法，其包括以下步骤：

1)以空间机械臂控制系统软件进程的主线程为基础构建架构主线程，如图4所示，步骤如下：

步骤1：建立遥操作端的TCP/IP网络连接服务并监听连接请求，建立视觉服务器端的TCP/IP网络连接服务并监听连接请求，建立力传感器端的UDP网络连接服务并绑定端口与地址，建立与机械臂各电机驱动器的CAN网络连接并初始化机械臂各电机驱动器；

遥操作端及视觉服务器端的TCP/IP网络服务是面向连接的，在监听到连接请求后，保存连接信息，并准备接收数据；而力传感器端的UDP网络连接服务是非面向连接的，在网络服务建立后，需将其绑定特定的端口与地址，之后在所绑定地址与端口上等待数据。为了直观，在图4中对这三个网络连接做了统一表述，这里做出解释。

步骤2：创建遥操作端数据接收消息响应函数、视觉服务器端数据接收消息响应函数、力传感器端数据接收消息响应函数及机械臂各电机驱动器端数据接收回调函数；

步骤3：创建信息采集及保护线程函数、指令执行线程函数、信息数据发送线程函数，并挂起；

在实施的过程中会遇到线程之间不同步的问题，这一问题会导致多个线程同时访问同一个共享资源，并对其进行操作，进而产生错误。这里采用“临界区”对象进行线程间同步，任一时刻只有一个线程可以拥有临界区对象，拥有临界区的线程可以访问被保护起来的资源或代码段，其他希望进入临界区的线程将被挂起等待，直到拥有临界区的线程放弃临界区时为止，这样就保证了不会在同一时刻出现多个线程访问同一个共享资源。

步骤4：采用数据链表的方式建立指令循环队列和信息循环队列，并分配存储空间，完成初始化；

队列是一种先进先出(FIFO)的线性表，采用双指针操作方式，既Rear(队尾)和Front(队首)，它只允许在表的Rear端进行数据插入，而在Front端进行数据删除，初始时队首队尾指针相等。队列通常采用链表或者数组来实现，考虑到操作的简易性，这里采用数据链表的方式实现，链表的大小在初始化后为固定值。循环队列是队列的一种，它的特点在于可以有效地防止数据伪溢出现象发生，可最大限度地利用队列内存空间，节省计算机资源。但循环队列中，由于入队时Rear指针向前追赶Front指针，出队时Front指针向前追赶Rear指针，造成队空和队满时头尾指针均相当。为了解决这个问题，区分两种情况，保证数据安全，这里采用数据结构常用的方法：队满时，(Rear+1)％N＝＝Front，其中N为队列长度(所用数组大小)，％为取余运算。

步骤5：初始化电机状态信息变量、限位器状态信息变量、力传感器状态信息变量，三个变量均设置为全局变量，以供不同线程间共同访问；

步骤6：建立定时机构，定时激活信息采集及保护线程，以及其他需要定时执行的任务；

步骤7：创建数据预处理函数，对接收到的数据进行预处理，指令数据预处理方式为：对紧急处理指令做出紧急处理，并将非紧急处理指令压入指令循环队列，对状态数据进行预处理方式为：更新相应的全局状态变量。

2)建立架构指令循环队列的入队列操作函数和出队列操作函数，如图5所示，步骤如下：

步骤1：创建指令循环队列入队列操作函数，入队列操作函数执行指令数据入队前，先判断指令循环队列是否空，如果不空，就循环等待，否则执行入队操作，入队时采用队列常用队尾指针操作方式，以字节为单位逐个拷贝至循环队列内；

步骤2：创建指令循环队列出队列操作函数，出队列操作函数执行出队操作前，先判断指令循环队列是否空，如果不空就执行出队操作，否则不执行任何动作，出队时采用队列常用队首指针操作方式，以字节为单位逐个取出，并一次性取出指令循环队列内所有数据。

3)建立架构信息循环队列的入队列操作函数和出队列操作函数，如图6所示，步骤如下：

步骤1：创建信息循环队列入队列操作函数，入队列操作函数执行信息数据入队前，先判断信息循环队列是否满，如果满，就循环等待，否则执行入队操作，入队时采用队列常用队尾指针操作方式，以字节为单位逐个拷贝至循环队列内；

步骤2：创建信息循环队列出队列操作函数，出队列操作函数执行出队操作前，先判断信息循环队列是否空，如果不空就执行出队操作，否则不执行任何动作，出队时采用队列常用队首指针操作方式，以字节为单位逐个取出，并一次性仅取出一条完整的信息帧。

4)所述架构主线程通过定时机构，每个一段时间唤醒信息采集及保护线程。所述信息采集及保护线程的功能通过信息采集及保护线程函数实现，如图7所示，步骤如下：

步骤1：线程被唤醒后，首先向机械臂各电机驱动器和力传感器发送状态请求指令，通过所述架构主线程接收状态信息，并存储到电机状态信息变量、限位器状态信息变量、力传感器状态信息变量中；

步骤2：信息采集及保护线程读取电机状态信息变量获得电机状态信息，读取限位器状态信息变量获取各限位器状态信息，读取力传感器状态信息变量获取力传感器信息；

步骤3：通过验证以上各状态信息是否存在超标量执行安全策略，如果存在超标量就紧急停止机械臂，防止机械臂受损，并封装相应的错误信息帧，通过信息循环队列入队列操作函数压入信息循环队列，并唤醒信息数据发送线程；否则，将以上各状态信息封装成机械臂状态信息帧，通过信息循环队列入队列操作函数压入信息循环队列，并唤醒信息数据发送线程。

步骤4：采集完成后，所述信息采集及保护线程自动挂起，直到下次被架构主线程的定时机构唤醒。

5)所述架构主线程在将非紧急处理指令压入指令循环队列后，唤醒指令执行线程。所述指令执行线程的功能通过指令执行线程函数实现，如图8所示，步骤如下：

步骤1：线程被唤醒后，首先判断指令循环队列是否空，如果空，就自动挂起指令执行线程，否则通过指令循环队列出队列函数取出队列内所有数据；

步骤2：进行帧解析，通过控制帧的标识信息，识别控制指令，并通过判断帧头帧尾的正确性来实施指令验证，来确定指令帧的合法性和完整性；

步骤3：将指令帧内的控制参数提取出来，并根据控制参数进行运动规划，生成电机驱动指令；

步骤4：判断机械臂与各电机驱动器连接是否正常，如果正常，就调用相应的电机控制函数，将电机驱动指令发送到相应的电机驱动器，实施机械臂电机控制；

步骤5：判断执行是否成功，如果成功，就封装相应的执行成功信息帧，通过信息循环队列入队列操作函数，将信息帧压入信息循环队列，并唤醒信息数据发送线程，返回步骤1；

步骤6：在本线程执行以上各步骤是，如果遇到错误信息，就停止执行，并封装相应的错误信息帧，通过信息循环队列入队列操作函数，将信息帧压入信息循环队列，并唤醒信息数据发送线程，返回步骤1。

6)所述架构主线程、信息采集及保护线程、指令执行线程，在每次将信息帧压入信息循环队列后，均唤醒一次信息数据发送线程。所述信息数据发送线程的功能由信息数据发送线程函数实现，如图9所示，步骤如下：

步骤1：线程被唤醒后，首先判断信息循环队列是否空，如果空，就挂起信息数据发送线程，否则通过信息循环队列出队列操作函数取出一条完整指令帧；

步骤2：验证取出指令帧的帧头、帧尾，判断帧的完整性与合法性，如果都合法就进入下一步，否则封装相应的错误信息帧，并替代取出的指令帧，进入下一步；

步骤3：判断控制器软件与目标端网络连接是否正常，如果不正常就挂起信息数据发送线程，否则，将传递过来的信息帧发送到目标计算机，发送完毕后，返回步骤1。

本架构及实现方法在自主研发的空间机械臂平台上得到验证，该平台中央控制器具体配置为：紧凑型主板EmCORE-i2501，SolderedonboardAtomN2600处理器，主频1.6GHZ，2GDDR31066内存，WindowsXPEmbedded嵌入式操作系统；中央控制器软件开发环境为MicrosoftVisualC++6.0，编写语言为C++。采取如下验证方法：由遥操作端向中央控制器发送一条符合实际控制要求的带有较复杂运算的指令，中央控制器记录下收到指令后执行的每一步操作及其时刻值，进行100次重复试验，求取平均值。地面遥操作端所发送的指令为末端位姿控制指令，中央控制器接收到指令后需要进行的复杂运算有：1次从笛卡尔空间末端位姿到关节空间关节位置的反解运算，6次两点之间的样条插值运算，插值间隔100ms，插值时间100s，总插值路径点数为6000个。经验证，架构在执行大运算量任务时，控制延迟约为5700.506203us；执行未有大运量任务时，控制延迟约为529.205304us。同时，我们知道从地面控制地球同步轨道上的空间机械臂，存在至少约0.24s的通信延时，通信延时是一直存在且不可避免的，而实际控制过程中的控制延时包括通信延时、控制软件数据处理延时以及电机驱动器通信与数据处理延时等，但由于电机驱动器通信与数据处理延时测量起来比较困难，且通常较小，这里暂不考虑。因此，带有大运算量时，相对于仅有通信延时的情况，本架构在该平台下实际控制过程中带来的控制延时增加率约为5700.506203/240000＝2.375％；未有大运算量时，带来的控制延时增加率约为529.205304/240000＝0.221％。可以看出：1、架构能够适应空间机械臂嵌入式平台，且可在WindowsXPEmbedded嵌入式操作系统上实现，不依赖于Linux操作系统；2、架构的性能优越，能够在较短的时间内完成数据处理，以较小的控制延迟增加率，实现对机械臂的控制。

同时，架构内的四个线程处理的数据量不同，有些线程处理数据量小，所需处理时间也就相应短，循环队列建立的缓存，能够保存那些已经处理完成的数据，从而使得处理完数据的线程不必等待其他线程就能进入下一个处理循环，因此能够减少不比较的等待时间，减少整体数据处理时间，降低控制延迟。

本架构强化了各模块之间的独立性，将整个控制系统软件划分为四大功能模块，分布到四个并行线程中，而每个功能模块又划分为若干个小模块，各模块之间区分清晰，添加或删除某些功能是可方便实施，同时出现问题时，也可通过逐个模块的排查，迅速解决问题，可非常方便地进行维护及升级改造任务。

相对于技术背景中提到现有技术，本架构技术直接针对空间机械臂平台设计，体系简洁，资源占用量小，不必花费大量时间研究其他平台架构；同时，架构采用的是比较通用的计算机技术，即多线程及队列，开发人员能在短时间内将架构的线程及队列实现；而线程内的小模块的功能划分已经达到或接近任务级，可参考类似架构的开发过程实现。因此，架构的实施过程简单，可有效降低空间机械臂控制系统软件的开发难度，缩短开发周期。

Claims (9)

1.一种基于C/S结构的空间机械臂控制系统软件架构，其特征在于，基于多线程和循环队列技术，采用空间机械臂中央控制器作为空间机械臂控制系统软件架构的服务端，采用空间机械臂的地面遥操作计算机作为空间机械臂控制系统软件架构的客户端，将空间机械臂控制系统软件的各项功能划分为四个功能模块，并为每个功能模块建立一个独立线程，线程之间通过指令循环队列和信息循环队列建立缓存；

所述四个功能模块分别为通信连接与数据接收模块、信息采集及保护模块、指令执行模块、信息数据发送模块，该四个功能模块对应的四个线程分别为架构主线程、信息采集及保护线程、指令执行线程和信息数据发送线程；所述四个线程之间并行运行；

所述循环队列用来建立缓存，均采用数据链表的方式实现，链表的大小在初始化后为固定值；

所述四个线程优先级从高到低的顺序依次为：架构主线程、信息采集及保护线程、指令执行线程及信息数据发送线程；

所述架构主线程用于控制通信及协调任务，包括通信连接模块、数据接收模块、预处理模块和定时机构四个功能模块；

其中，通信连接模块用于与遥操作端计算机、视觉控制器、力传感器及机械臂各电机驱动器建立通信连接；

数据接收模块用于接收通信连接模块输出过来的数据，并将指令数据送往预处理模块，而将信息数据存储到相应的全局变量中，以供信息采集及保护线程使用；

预处理模块用于进行紧急指令响应，并将不需要紧急响应的指令压入指令循环队列，以供指令执行线程使用；

定时机构用于定时唤醒信息采集及保护线程，执行信息采集及保护策略；

所述信息采集及保护线程用于采集力传感器及机械臂各电机的状态信息，并执行安全保护策略，包括信息获取模块、安全保护模块、信息帧封装模块；

其中，信息获取模块用于获取状态信息，包括电机信息获取模块、限位器状态获取模块、力传感器信息获取模块，依次用于获取机械臂各电机状态信息、各限位器状态信息、力传感器信息；

安全保护模块用于判断是否有状态超标，如有超标则执行安全策略——紧急停止机械臂所有操作，锁定机械臂各电机，清空指令循环队列；

信息帧封装模块用于将以上获取的状态信息封装成帧，并压入信息循环队列；

所述指令执行线程用于解析指令帧，获取指令参数，进而实施机械臂电机控制，包括指令读取模块、指令解析模块、指令验证模块、参数提取模块、运动规划模块、驱动指令生成模块、电机控制模块；

其中，指令读取模块用于从指令循环队列中读取控制指令，且一次性读出指令循环队列内所有数据；

指令解析模块用于识别指令读取模块取出的指令；

指令验证模块用于验证指令的完整性和合法性，通过验证帧头帧尾是否正确来实现；

参数提取模块用于去除控制指令帧内的无效信息，提取出有效控制参数；

运动规划模块用于根据控制参数进行机械臂运动规划，并在需要时向视觉控制器端发送目标位姿请求指令，之后借助架构主线程的数据接收模块获得目标位姿；

驱动指令生成模块，用于将运动规划模块规划完成的指令转换为电机驱动指令；

电机控制模块用于向机械臂各电机驱动器发送驱动指令，实施机械臂控制；

所述信息数据发送线程用于将信息帧发送到遥操作端计算机，包括信息帧读取模块、信息帧验证模块、信息帧发送模块；

其中，信息帧读取模块用于从信息循环队列内读取信息帧，且每次仅读取一条完整的信息帧；

信息帧验证模块用于验证所取出的信息帧是否完整和合法，通过验证帧头帧尾来实现；

信息帧发送模块用于将传送过来的信息帧发送到遥操作端计算机。

2.根据权利要求1所述的基于C/S结构的空间机械臂控制系统软件架构，其特征在于，信息循环队列缓存来自信息采集及保护线程的状态信息帧，以及其他三个线程执行过程中产生的错误信息帧和执行结果信息帧；

所述指令循环队列仅缓存一条指令帧。

3.一种基于C/S结构的空间机械臂控制系统软件架构构建方法，其特征在于，采用权利要求1-2任一项所述的基于C/S结构的空间机械臂控制系统软件架构，包括以下步骤：

1)以空间机械臂控制系统软件进程的主线程为基础构建架构主线程，实现架构主线程所有功能，并由架构主线程初步建立信息采集及保护线程函数、指令执行线程函数、信息数据发送线程函数；

2)建立架构指令循环队列的入队列操作函数和出队列操作函数，并实现指令循环队列的入队和出队操作；

3)建立架构信息循环队列的入队列操作函数和出队列操作函数，并实现信息循环队列的入队和出队操作；

4)在信息采集及保护线程函数内实现信息采集及保护线程的所有功能；

5)在指令执行线程函数内实现指令执行线程的所有功能；

6)在信息数据发送线程函数内实现信息数据发送线程的所有功能。

4.根据权利要求3所述的基于C/S结构的空间机械臂控制系统软件架构构建方法，其特征在于，所述以空间机械臂控制系统软件进程的主线程为基础构建架构主线程，具体构建步骤如下：

步骤1：建立遥操作端的TCP/IP网络连接服务并监听连接请求，建立视觉服务器端的TCP/IP网络连接服务并监听连接请求，建立力传感器端的UDP网络连接服务并绑定端口与地址，建立与机械臂各电机驱动器的CAN网络连接并初始化机械臂各电机驱动器；

步骤2：创建遥操作端数据接收消息响应函数、视觉服务器端数据接收消息响应函数、力传感器端数据接收消息响应函数及机械臂各电机驱动器端数据接收回调函数；

步骤3：创建信息采集及保护线程函数、指令执行线程函数、信息数据发送线程函数，并挂起；

步骤4：采用数据链表的方式建立指令循环队列和信息循环队列，并分配存储空间，完成初始化；

步骤5：初始化电机状态信息变量、限位器状态信息变量、力传感器状态信息变量，三个变量均设置为全局变量，以供不同线程间共同访问；

步骤6：建立定时机构，定时激活信息采集及保护线程；

步骤7：创建数据预处理函数，对接收到的数据进行预处理，指令数据预处理方式为：对紧急处理指令做出紧急处理，并将非紧急处理指令压入指令循环队列，对状态数据进行全局状态变量更新的预处理。

5.根据权利要求4所述的基于C/S结构的空间机械臂控制系统软件架构构建方法，其特征在于，所述建立架构指令循环队列的入队列操作函数和出队列操作函数的具体构建步骤如下：

步骤1：创建指令循环队列入队列操作函数，入队列操作函数执行指令数据入队前，先判断指令循环队列是否空，如果不空，就循环等待，否则执行入队操作，入队时采用队列常用队尾指针操作方式，以字节为单位逐个拷贝至循环队列内；

步骤2：创建指令循环队列出队列操作函数，出队列操作函数执行出队操作前，先判断指令循环队列是否空，如果不空就执行出队操作，否则不执行任何动作，出队时采用队列常用队首指针操作方式，以字节为单位逐个取出，并一次性取出指令循环队列内所有数据。

6.根据权利要求5所述的基于C/S结构的空间机械臂控制系统软件架构构建方法，其特征在于，所述建立架构信息循环队列的入队列操作函数和出队列操作函数的具体构建步骤如下：

步骤1：创建信息循环队列入队列操作函数，入队列操作函数执行信息数据入队前，先判断信息循环队列是否满，如果满，就循环等待，否则执行入队操作，入队时采用队列常用队尾指针操作方式，以字节为单位逐个拷贝至循环队列内；

步骤2：创建信息循环队列出队列操作函数，出队列操作函数执行出队操作前，先判断信息循环队列是否空，如果不空就执行出队操作，否则不执行任何动作，出队时采用队列常用队首指针操作方式，以字节为单位逐个取出，并一次性仅取出一条完整的信息帧。

7.根据权利要求5所述的基于C/S结构的空间机械臂控制系统软件架构构建方法，其特征在于，所述信息采集及保护线程的功能通过信息采集及保护线程函数实现，具体步骤如下：

步骤1：线程被唤醒后，首先向机械臂各电机驱动器和力传感器发送状态请求指令，通过所述架构主线程接收状态信息，并存储到电机状态信息变量、限位器状态信息变量、力传感器状态信息变量中；

步骤2：信息采集及保护线程读取电机状态信息变量获得电机状态信息，读取限位器状态信息变量获取各限位器状态信息，读取力传感器状态信息变量获取力传感器信息；

步骤3：通过验证以上各状态信息是否存在超标量执行安全策略，如果存在超标量就紧急停止机械臂，防止机械臂受损，并封装相应的错误信息帧，通过信息循环队列入队列操作函数压入信息循环队列，并唤醒信息数据发送线程；否则，将以上各状态信息封装成机械臂状态信息帧，通过信息循环队列入队列操作函数压入信息循环队列，并唤醒信息数据发送线程；

步骤4：采集完成后，所述信息采集及保护线程自动挂起，直到下次被架构主线程的定时机构唤醒。

8.根据权利要求6所述的基于C/S结构的空间机械臂控制系统软件架构构建方法，其特征在于，所述指令执行线程的功能通过指令执行线程函数实现，具体步骤如下：

步骤1：线程被唤醒后，首先判断指令循环队列是否空，如果空，就自动挂起指令执行线程，否则通过指令循环队列出队列函数取出队列内所有数据；

步骤2：进行帧解析，通过控制帧的标识信息，识别控制指令，并通过判断帧头帧尾的正确性来实施指令验证，来确定指令帧的合法性和完整性；

步骤3：将指令帧内的控制参数提取出来，并根据控制参数进行运动规划，生成电机驱动指令；

步骤4：判断机械臂与各电机驱动器连接是否正常，如果正常，就调用相应的电机控制函数，将电机驱动指令发送到相应的电机驱动器，实施机械臂电机控制；

步骤5：判断执行是否成功，如果成功，就封装相应的执行成功信息帧，通过信息循环队列入队列操作函数，将信息帧压入信息循环队列，并唤醒信息数据发送线程，返回步骤1；

步骤6：在本线程执行以上各步骤时，如果遇到错误信息，就停止执行，并封装相应的错误信息帧，通过信息循环队列入队列操作函数，将信息帧压入信息循环队列，并唤醒信息数据发送线程，返回步骤1。

9.根据权利要求7所述的基于C/S结构的空间机械臂控制系统软件架构构建方法，其特征在于，所述信息数据发送线程的功能由信息数据发送线程函数实现，具体步骤如下：

步骤1：线程被唤醒后，首先判断信息循环队列是否空，如果空，就挂起信息数据发送线程，否则通过信息循环队列出队列操作函数取出一条完整指令帧；

步骤2：验证取出指令帧的帧头、帧尾，判断帧的完整性与合法性，如果都合法就进入下一步，否则封装相应的错误信息帧，并替代取出的指令帧，进入下一步；

步骤3：判断控制器软件与目标端网络连接是否正常，如果不正常就挂起信息数据发送线程，否则，将传递过来的信息帧发送到目标计算机，发送完毕后，返回步骤1。