CN102853831B - 基于双核处理技术的足式机器人状态感知系统 - Google Patents

Abstract

本发明公开了一种基于双核处理技术的足式机器人状态感知系统，采用双核系统架构，即基于FPGA核心单元，采用SOPC技术，在片内嵌入两个RISC嵌入式处理器NIOS II软核实现。一个用于构建数据采集功能的数据采集内核；一个用于构建数据处理与通信功能的数据处理内核；两个内核通过邮箱内核Mailbox和共享片内RAM实现相互间的数据交互。NIOS II B软核能够实现更完备信息的采集，而NIOS IIA软核基于NIOS II B软核采集到的数据进行融合处理。本发明采用具有面向复杂足式机器人的状态感知技术，具有信息完备性检测、高速数据并行处理、实时数据通信以及可靠性高等特点。

Description

基于双核处理技术的足式机器人状态感知系统

技术领域

本发明涉及一种基于双核处理技术的足式机器人状态感知系统。

背景技术

随着经济的发展和各行各业对自动化程度要求的提高，各式各样的机器人步入了人类生活和生产的领域之中，以替代或模仿人类执行各种复杂任务。其中，足式机器人具有机构简单、运动灵活、承载能力强、稳定性好和环境适应能力强等特点，因此，足式机器人尤其适用于军事边防领域，用于伴随边防战士巡逻和运输物质等。在控制足式机器人活动的各项技术中，最为关键的技术之一便是足式机器人状态感知技术。

目前，国内外的足式机器人状态感知主要是从足部方面来检测自身足部的相关运动参量，并计算来获取机器人运动姿态信息，从而生成合理的步态。但这对足式机器人状态感知信息的需求不全面，从而带来信息不完备、可靠性较低等问题。同时，现有的足式机器人状态感知系统的控制核心都是采用单核处理器，一方面负责多种时间尺度的传感器数据采集、预处理、缓存以及标准化封装；另一方面基于传感器数据，运用数据融合技术实现状态感知信息的处理。这样的单核处理器担任双重功能，难免会影响足式机器人的实时性、快速性和可靠性。

而目前的足式机器人规划与协调等智能行为对状态感知的功能需求已不满足于具体传感器的检测信息的获取和简单信息处理，而是需要状态感知具有更丰富的特征提取功能，即结合多种同构或异构传感器检测信息，通过多种信息处理技术，消除传感器信号混入的干扰信号、估计无法直接测量的状态信息、获得面向规划与协调的综合特征信息等。这样缺乏面向复杂足式机器人的状态感知技术，从而不能满足复杂环境下的工作要求。

因此，有必要设计一种基于双核处理技术的足式机器人状态感知系统。

发明内容

本发明所要解决的技术问题是提供一种基于双核处理技术的足式机器人状态感知系统，该基于双核处理技术的足式机器人状态感知系统具有实时、并行数据处理能力，数据处理速度快。

发明的技术解决方案如下：

一种基于双核处理技术的足式机器人状态感知系统，包括核心模块和外部模块；

核心模块包括RISC嵌入式处理器、邮箱内核模块和片内RAM；

RISC嵌入式处理器为双核嵌入式处理器，由用于数据采集的NIOS II B软核和用于数据处理和实现通信功能的NIOS II A软核组成；NIOS II A软核及NIOS II B软核均与片内RAM连接；NIOS IIA软核及NIOS II B软核均与邮箱内核模块连接，NIOS II A软核及NIOS II B软核均通过邮箱内核模块访问片内RAM实现NIOS IIA软核和NIOS II B软核之间的通信；

所述的外部模块包括动力单元传感器、四肢伺服控制器、组合导航系统、LCD显示屏和蜂鸣器；动力单元传感器、四肢伺服控制器和组合导航系统均与NIOS II B软核相接；LCD显示屏和蜂鸣器均与NIOS IIA软核中的人机交互模块连接。

所述的NIOS II A软核中具有用于与外联模块通信的CAN总线通信模块；所述的外联模块为规划协调模块（规划协调模块是上位机，规划协调模块还包括环境感知模块，环境感知模块与状态感知系统并列，一起隶属于规划协调模块）。

所述的核心模块为基于FPGA的核心模块。

动力单元传感器与NIOS II B软核中的模数转换模块相连；四肢伺服控制器与NIOS II B软核中的CAN总线通信模块相连；组合导航系统与NIOS II B软核中的组合导航接收模块相连。

所述的动力单元传感器包括流量传感器、温度计、液压油压力表、发动机转速表、发动机油量表、电源电压传感器；NIOS II B软核通过动力单元传感器采集的数据包括四肢油路流量、发动机温度、液压油温度、液压油压力、发动机转速、发动机油箱油量以及电源电压值；

NIOS II B软核通过四肢伺服控制器采集足式机器人的四肢特征数据，四肢特征数据包括关节角度、负载力、伺服阀电流以及足部三维力和力矩；

NIOS II B软核通过组合导航接收模块获取组合导航数据，包括：俯仰角、横滚角、航向角、三轴加速度以及三轴角速度和经度、纬度、高度以及速度。

有益效果：

本发明的基于双核处理技术的足式机器人状态感知系统，采用“以数据为中心”的分层结构构建数据采集层和数据处理层，并在此基础上构建双核系统架构，即基于FPGA核心单元，采用SOPC技术，在片内嵌入两个RISC嵌入式处理器NIOS II软核实现。一个用于构建数据采集功能的数据采集内核(简称NIOSII B软核)；一个用于构建数据处理与通信功能的数据处理内核(简称NIOS II A软核)；两个内核通过邮箱内核Mailbox和共享片内RAM实现相互间的数据交互。NIOS II B软核能够实现更完备信息的采集，而NIOS II A软核基于NIOS II B软核采集到的数据进行融合处理，同时将接收的组合导航数据通过高速可靠CAN总线发送于环境感知层，并与足式机器人规划协调层构成的基于CAN总线的信息传输网络，从而实现足式机器人状态感知系统各功能单元的并行处理。本发明具有面向复杂足式机器人的状态感知技术、信息完备性检测、高速数据并行处理、实时数据通信以及可靠性高等特点。

附图说明

图1为本发明的体系结构图；

图2为本发明的系统结构图。

具体实施方式

以下将结合附图和具体实施例对本发明做进一步详细说明：

实施例1：

如图1和图2所示，一种基于双核处理技术的足式机器人状态感知系统，

采用“以数据为中心”的分层结构构建数据采集层和数据处理层，并在此基础上构建双核系统架构，即基于FPGA核心单元，采用SOPC技术，在片内嵌入两个RISC嵌入式处理器NIOS II软核实现，具有一个模数转换模块、二个CAN总线通信模块、一个组合导航接收模块以及一个人机交互模块；动力单元传感器、四肢伺服控制器、组合导航系统分别与FPGA核心单元串行连接，LCD液晶屏、蜂鸣器分别与FPGA核心单元并行连接；其中：

用于构建数据采集功能的数据采集内核(简称NIOS II B软核)，负责动力单元传感器数据的采集、四肢伺服控制器CAN总线数据的监听以及组合导航系统输出数据的接收；

用于构建数据处理与通信功能的数据处理内核(简称NIOS II A软核)，负责数据处理(包括姿态估计、开机自检、碰撞检测和实时工作状态评估四大模块)、与规划协调层和环境感知层的CAN总线通信以及人机交互；

NIOS II B软核和NIOS IIA软核通过邮箱内核Mailbox和共享片内RAM实现相互间的数据交互，构成双核状态感知系统。

NIOS II B软核负责动力单元传感器数据的采集、四肢伺服控制器CAN总线数据的监听以及组合导航系统输出数据的接收，具体用于：

——FPGA核心单元向动力单元传感器写入控制信号，并通过模数转换模块获取动力单元传感器采集的并经ADC转换后的数据，包括四肢油路流量、发动机温度、液压油温度、液压油压力、发动机转速、发动机油箱油量以及电源电压，其对应的传感器包括：流量传感器、温度计、液压油压力表、发动机转速表、发动机油量表、电源电压传感器；

——FPGA核心单元向四肢伺服控制器写入控制信号，并通过CAN总线通信模块获取监听到的四肢伺服控制器CAN总线数据，从而得到四肢特征数据，包括关节角度、负载力、伺服阀电流以及足部三维力和力矩。这些传感器数据由四肢伺服控制器采集，并通过四条CAN总线上传至规划协调层，此时状态感知系统通过监听四条CAN总线获得四肢特征数据，其中四肢传感器包括：线性电位计、力传感器、伺服阀电流计和六轴力传感器；

——FPGA核心单元向组合导航系统写入控制信号，并通过组合导航接收模块获取组合导航数据，包括：俯仰角、横滚角、航向角、三轴加速度以及三轴角速度和经度、纬度、高度以及速度。

NIOS IIA软核负责数据处理(包括姿态估计、开机自检、碰撞检测和实时工作状态评估四大模块)、与规划协调层和环境感知层的CAN总线通信以及人机交互，具体用于：

对所述NIOS II B软核采集到的四肢特征数据和组合导航数据经过姿态估计模块，计算得到姿态估计结果，包括足式机器人本体位姿和四肢相对本体的位姿；

对所述NIOS II B软核采集到的动力单元传感器数据、四肢特征数据以及组合导航数据经过开机自检模块，计算得到开机自检结果，包括开机自检信息、传感器和动作机构的故障报警信息；

对所述NIOS II B软核采集到的四肢特征数据和组合导航数据经过碰撞检测模块，计算得到碰撞检测结果，包括足尖或四肢是否发生碰撞以及本体是否受到外部冲击，从而实时获得碰撞检测信息；

对所述NIOS II B软核采集到的动力单元传感器数据、四肢特征数据以及组合导航数据经过实时工作状态评估模块，计算得到实时工作状态评估结果，包括传感器和动作机构的实时故障报警信息、执行机构工作裕量以及四肢关节跟踪控制效果；

对所述NIOS II B软核采集到的组合导航数据通过CAN总线发送于环境感知层，作为环境感知的依据；

对所述姿态估计结果、开机自检结果、碰撞检测结果以及实时工作状态评估结果通过CAN总线发送于规划协调层，作为协调规划的依据；

对所述开机自检结果和实时故障报警信息通过人机交互模块以文字或声音形式分别显式反映到LCD液晶屏和蜂鸣器中，以实现人机交互。

NIOS II B软核和NIOS II A软核通过邮箱内核Mailbox和共享片内RAM实现相互间的数据交互。具体用于：

NIOS II B软核将采集到的动力单元传感器数据、四肢特征数据以及组合导航数据存入片内RAM区，并发送一指向共享内存块的指针，从而替代实际的数据传输；NIOS II A软核以阻塞方式等待数据，一有数据就接收进来，并将指针传递给各个数据处理模块进行融合处理。

NIOS II B软核将采集到的动力单元传感器数据、四肢特征数据以及组合导航数据分别定义一个数据帧头，并统一定义一个数据帧尾；NIOS II A软核按照数据帧的序号接收每一种数据。

参见图1，本发明实施例提供一种足式机器人状态感知系统，所述系统采用分层体系结构，把足式机器人状态感知系统分为二层：信息感知层和信息融合层。

信息感知层，即对应数据采集层，目的是为了完成本体的动力单元传感器数据的采集、四肢伺服控制器CAN总线数据的监听以及组合导航系统输出数据的接收。其中，动力单元传感器数据的采集包括：流量传感器、温度计、液压油压力表、发动机转速表、发动机油量表以及电源电压传感器数据的采集；四肢伺服控制器CAN总线数据的监听包括：线性电位计、力传感器、伺服阀电流计和六轴力传感器数据的监听；组合导航系统输出数据的接收包括：IMU和GPS数据的接收。

信息融合层，即对应数据处理层，目的是通过对传感器感知的本体全局信息的分析和推理，有效运用信息融合等数据处理技术，从而获得足式机器人本体运动特征信息，并通过CAN总线为规划协调层提供关键性的本体信息。

具体地说，信息感知层根据动力单元传感器、四肢伺服控制器以及组合导航系统来感知足式机器人本体信息，并发送给信息融合层；信息融合层负责数据处理与通信，一方面运用信息融合等数据处理技术实现本体感知信息的特征提取和本体机构工作状态的评估；一方面与环境感知层和规划协调层【即上位机】进行通信，即通过CAN总线将接收的组合导航数据发送于环境感知层，作为环境感知的依据，通过CAN总线接收规划协调层的下发数据并为规划协调层发送姿态估计信息和机构工作状态的监测和报警信息。

对应于足式机器人状态感知体系结构的分层设计思想，足式机器人状态感知系统的控制核心为FPGA核心单元，采用SOPC技术，在片内嵌入两个RISC嵌入式处理器NIOS II软核，一个用于构建数据采集功能的数据采集内核(简称NIOS II B软核)；一个用于构建数据处理与通信功能的数据处理内核(简称NIOSIIA软核)；两个内核通过邮箱内核Mailbox和共享片内RAM实现相互间的数据交互。请参阅图2，下面具体介绍本发明足式机器人状态感知的系统结构，包括：

FPGA(Field-Programmable Gate Array，现场可编程门阵列)核心单元1、动力单元传感器10、四肢伺服控制器11、组合导航系统12、LCD液晶屏13、蜂鸣器14。

FPGA核心单元1，采用SOPC技术，在片内嵌入两个RISC嵌入式处理器NIOS II软核实现，具有一个模数转换模块6、二个CAN总线通信模块7、一个组合导航接收模块8以及一个人机交互模块9；动力单元传感器10、四肢伺服控制器11、组合导航系统12分别与FPGA核心单元1串行连接，LCD液晶屏13、蜂鸣器14分别与FPGA核心单元1并行连接；其中：

用于构建数据采集功能的数据采集内核(简称NIOS II B软核)，负责动力单元传感器数据的采集、四肢伺服控制器CAN总线数据的监听以及组合导航系统输出数据的接收；

用于构建数据处理与通信功能的数据处理内核(简称NIOS II A软核)，负责数据处理(包括姿态估计、开机自检、碰撞检测和实时工作状态评估四大模块)、与规划协调层和环境感知层的CAN总线通信以及人机交互；

NIOS II B软核2和NIOS II A软核3通过邮箱内核Mailbox4和共享片内RAM5实现相互间的数据交互，构成双核状态感知系统。

NIOS II B软核的主要功能包括采集各种传感器信号，为足式机器人提供必要的本体状态信息。具体包括动力单元传感器数据的采集、四肢伺服控制器CAN总线数据的监听以及组合导航系统输出数据的接收。

动力单元传感器数据的采集采用轮询方式，实现动力单元传感器数据采集的稳定性和高正确率。FPGA核心单元1向动力单元传感器10写入控制信号，并通过模数转换模块6以数字形式获取足式机器人动力单元的四肢油路流量、发动机温度、液压油温度、液压油压力、发动机转速、发动机油箱油量以及电源电压信息。

四肢伺服控制器CAN总线数据的监听采用中断方式，实现四肢伺服控制器CAN总线数据监听的实时性和可靠性。FPGA核心单元1向四肢伺服控制器11写入控制信号，并通过CAN总线通信模块7获取监听到的足式机器人的左前肢、右前肢、左后肢以及右后肢的关节角度、负载力、伺服阀电流以及足部三维力和力矩。

组合导航系统输出数据的接收采用中断方式，实现组合导航数据接收的实时性和可靠性。FPGA核心单元1向组合导航系统12写入控制信号，并通过组合导航接收模块8获取足式机器人的俯仰角、横滚角、航向角、三轴加速度以及三轴角速度和经度、纬度、高度以及速度。

NIOS II A软核的主要功能包括三个部分：一是实现状态感知信息的处理，包括姿态估计、开机自检、碰撞检测和实时工作状态评估四大处理模块，运用信息融合等数据处理技术实现本体感知信息的特征提取和本体机构工作状态的评估，从而获取姿态估计信息、机构工作状态的监测和报警信息；二是实现与用户的交互，即通过人机交互模块9以文字或声音报警形式分别显式反映到LCD液晶屏13和蜂鸣器14中；三是实现与环境感知层16和规划协调层17的CAN总线通信，分别获取组合导航信息和本体运动姿态估计信息、机构工作状态的监测与报警信息组合。

NIOS II B软核2和NIOS II A软核3通过邮箱内核Mailbox4和共享片内RAM5实现相互间的数据交互。双核通信流程如下：NIOS II B软核2将采集到的动力单元传感器数据、四肢特征数据以及组合导航数据存入片内RAM5区，并发送一指向共享内存块的指针到邮箱内核Mailbox4的FIFO中，从而替代实际的数据传输；NIOS II A软核3以阻塞方式等待数据，一有数据就接收进来，并将指针传递给各个数据处理模块进行融合处理。NIOS II B软核将采集到的动力单元传感器数据、四肢特征数据以及组合导航数据分别定义一个数据帧头0xaa、0xbb、0xcc，并统一定义一个数据帧尾0x55；NIOS II A软核按照数据帧的序号接收每一种数据。

FPGA核心单元1通过CAN总线与环境感知层16和规划协调层17相连。规划协调层17是足式机器人运动控制的核心，一般采用功能强大的工控机和专用的实时操作系统，状态感知系统15通过CAN总线与规划协调层17相连，向规划协调层17发送足式机器人状态感知信息，包括姿态估计信息、机构工作状态的监测和报警信息；状态感知系统15通过CAN总线与环境感知层16相连，向环境感知层16发送组合导航信息。

所述的FPGA核心单元采用Altera公司的Cyclone III系列的EP3C25Q240C8，具有25K逻辑单元、600Kbits嵌入式存储器、66个嵌入式乘法器、4个PLL以及148个用户I/O，可以扩展丰富的外设资源。FPGA硬件设计采用SOPC技术，在片内嵌入两个32位RISC嵌入式处理器NIOS II软核，其性能超过250DMIPS，可以添加定制指令，提高软件算法的执行速度。NIOS II B软核2的外设包括四个CAN控制器、两个UART控制器和两个SPI控制器，NIOSII A软核3的外设包括二个CAN控制器、多个通用I/O。

NIOS II B软核2与NIOS II A软核3之间的信息交互通过邮箱内核模块Mailbox4和共享片内RAM5来实现。Altera公司提供带Avalon接口的邮箱内核Mailbox在双核处理器之间发送信息。邮箱内核Mailbox4含有两个互斥体：一个保证对共享片内RAM5的唯一写访问；另一个保证对共享片内RAM5的唯一读访问，它与共享片内RAM5一起实现双核处理器之间的信息交互。邮箱内核Mailbox4更适合双核间单方向的信息交互，其传送一指向共享内存块的指针到邮箱内核Mailbox4的FIFO中，从而替代实际的数据传输，提高了数据交换效率。

该组合导航数据的接收频率为100Hz，四肢特征数据以及动力单元传感器数据的采集频率为1KHz，状态感知数据处理周期约200Hz。

以上所述，仅为本发明的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉该技术的人在本发明所揭露的技术范围内，可理解想到的变换或替换，都应涵盖在本发明的包含范围之内，因此，本发明的保护范围应该以权利要求书的保护范围为准。

Claims (1)

1.一种基于双核处理技术的足式机器人状态感知系统，其特征在于，包括核心模块和外部模块；核心模块包括RISC嵌入式处理器、邮箱内核模块和片内RAM；

RISC嵌入式处理器为双核嵌入式处理器，由用于数据采集的NIOS II B软核和用于数据处理和实现通信功能的NIOS II A软核组成；NIOS II A软核及NIOS II B软核均与片内RAM连接；NIOS II A软核及NIOS II B软核均与邮箱内核模块连接，NIOS II A软核及NIOS II B软核均通过邮箱内核模块访问片内RAM实现NIOS II A软核和NIOSII B软核之间的通信；

所述的外部模块包括动力单元传感器、四肢伺服控制器、组合导航系统、LCD显示屏和蜂鸣器；动力单元传感器、四肢伺服控制器和组合导航系统均与NIOS II B软核相接；LCD显示屏和蜂鸣器均与NIOS II A软核中的人机交互模块连接；

所述的NIOS II A软核中具有用于与外联模块通信的CAN总线通信模块；所述的外联模块为规划协调模块；

所述的核心模块为基于FPGA的核心模块；

动力单元传感器与NIOS II B软核中的模数转换模块相连；四肢伺服控制器与NIOSII B软核中的CAN总线通信模块相连；组合导航系统与NIOS II B软核中的组合导航接收模块相连；

所述的动力单元传感器包括流量传感器、温度计、液压油压力表、发动机转速表、发动机油量表、电源电压传感器；NIOS II B软核通过动力单元传感器采集的数据包括四肢油路流量、发动机温度、液压油温度、液压油压力、发动机转速、发动机油箱油量以及电源电压值；

NIOS II B软核通过四肢伺服控制器采集足式机器人的四肢特征数据，四肢特征数据包括关节角度、负载力、伺服阀电流以及足部三维力和力矩；

NIOS II B软核通过组合导航接收模块获取组合导航数据，包括：俯仰角、横滚角、航向角、三轴加速度以及三轴角速度和经度、纬度、高度以及速度。