CN102819256B - 一种足式机器人状态感知系统 - Google Patents

Abstract

本发明公开了一种足式机器人状态感知系统，其特征在于，选择FPGA作为足式机器人状态感知的主芯片，为提高多传感器数据采集和处理的速度，利用IP核构成处理器外设，用硬逻辑电路定制关键算法指令，在FPGA片内构建双Nios II处理器系统，分别完成异构传感器数据多时间尺度采集和高速处理传感器数据获得状态信息。本发明的感知装置包括数据采集与处理控制器模块、动力单元传感器数据采集模块、组合导航传感器数据采集模块、CAN总线通信模块、人机交互模块和电源模块。本发明的足式机器人状态感知系统，电路的集成度高、体积和功耗小、成本低，具有可扩展性、升级性和较高的抗干扰性，数据处理速度快，便于机器人实时控制。

Description

一种足式机器人状态感知系统

技术领域

本发明涉及一种足式机器人状态感知系统。

背景技术

高性能足式机器人是一个由很多部分组成的复杂系统，主要包括环境感知部分、状态感知部分、行为规划与协调部分、四肢伺服控制器部分、驱动装置部分和机体机械部分。系统通过环境感知部分和状态感知部分采集机器人周围的环境信息和本体状态信息，送至行为规划与协调部分后，行为规划与协调部分根据环境信息和状态信息规划机器人的行为，并将控制目标序列送至四肢伺服控制器部分，四肢伺服控制器部分控制驱动装置驱动机体机械部分做出相应的动作。状态感知部分作为足式机器人的检测装置，直接决定着足式机器人性能的高低，是系统的一个重要组成部分。

足式机器人的运动与作业环境具有未知、非结构化、动态、不确定、复杂等特性，要完成其使命，必须配备具有多种类型的传感器系统来感知其自身的状态信息，如感知其位置、速度、姿态的组合导航传感器系统，感知其动力装置温度、流量、压力等的动力单元传感器系统，还有根据步态生成需要获取四肢状态的传感器系统，如线性电位器、力传感器等。随着传感器数目和信息处理量的不断增加，数据的复杂性和测量中的模糊性因素加大，要对这些传感器的数据进行综合处理采用一般的单片机很难满足实时性和可靠性。目前的解决方法主要是采用ARM或者DSP来构成数据采集和处理系统。这种方法的缺点主要有：硬件电路复杂，结构不紧凑，可靠性差，数据处理能力有限，无法满足大数据量高速采集与处理的要求。

因此，有必要设计一种新型的足式机器人状态感知系统。

发明内容

本发明所要解决的技术问题是提供一种足式机器人状态感知系统，该足式机器人状态感知系统具有高速数据采集和处理功能、硬件电路集成度高。

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

一种足式机器人状态感知系统，包括嵌入式处理器模块、组合导航传感器数据采集模块、组合导航传感器、动力单元传感器数据采集模块、动力单元传感器、人机交互模块和通信模块；

组合导航传感器通过组合导航传感器数据采集模块与嵌入式处理器模块相连；

动力单元传感器通过动力单元传感器数据采集模块与嵌入式处理器模块相连；

人机交互模块和通信模块均与嵌入式处理器模块相连；

通信模块还与上位机通信连接；

所述的嵌入式处理器模块包括基于FPGA的双端口RAM和基于FPGA的两个NIOS II嵌入式处理器核：即A核和B核，两个NIOS II嵌入式处理器核通过双端口RAM通信连接。

四肢伺服控制器和环境感知单元不属于状态感知系统。四肢伺服控制器和环境感知单元均与通信模块相连。环境感知单元用于感知机器人的外界环境信息，与状态感知相对（状态感知用于感知机器人自身信息），不隶属于上位机；同时，四肢伺服控制器采集四肢传感器数据，并控制四肢动作，它们会通过

CAN总线向规划与协调层（上位机）发送四肢传感器数据，状态感知通过监听CAN总线获得这些传感器数据。

所述的嵌入式处理器模块还包括SDRAM控制器模块、Flash控制器模块、CAN控制器模块、UART控制器模块、LCD控制器模块、PIO控制器模块、SPI控制器模块、定时器模块、DMA模块、总线桥（Pipeline Bridge）模块、单端口RAM模块、向量中断控制器模块，嵌入式处理器模块内的各模块之间由AVALON总线相连；A核采用的存储器为SDRAM和Flash，B核采用的存储器为单端口RAM，利用向量中断控制器模块实现A核和B核的中断系统。

所述的通信模块为CAN总线通信模块；

组合导航传感器为惯性/卫星组合导航传感器【由惯性检测单元和GPS接收机组成，前者检测机器人的加速度和角速度信息，后者检测机器人的经度、纬度和高度。】

动力单元传感器包括四个流量传感器、两个压力传感器、三个温度传感器、一个转速传感器和一个电压传感器；

人机交互模块包括液晶显示屏、蜂鸣器和无线射频收发器。

FPGA为Altera公司生产的EP3C25F256A7N

动力单元传感器数据采集模块采用ADC芯片MAX188和参考源芯片MAX6350；组合导航传感器数据采集模块采用MAX3232芯片。

本发明选择FPGA作为足式机器人状态感知的主芯片，在FPGA片内构建双Nios II处理器系统，动力单元传感器数据采集模块将机器人动力单元传感器输出的模拟量转换为数字量；组合导航传感器数据采集模块通过串口读取惯性/卫星组合导航传感器的数据；CAN总线通信模块包含六路CAN总线，四路与机器人四肢伺服控制器相连，用于接收四肢传感器数据，另两路分别与规划与协调层【即上位机】和环境感知单元相连；嵌入式处理器模块包括Nios II处理器A核和B核，B核接收动力单元传感器数据采集模块、组合导航传感器数据采集模块、CAN总线通信模块的传感器数据并进行预处理，A核处理传感器数据获得状态信息，通过CAN总线通信模块传至规划与协调层【即上位机】和环境感知单元；人机交互模块用于显示机器人开机自检信息和故障报警信息，提供声音报警信号，并接收无线射频操控命令；电源模块为电路板上其他部分和动力单元传感器以及组合导航传感器提供电源。

所述FPGA内部模块划分为Nios II处理器模块（两个）、SDRAM控制器模块、Flash控制器模块、CAN控制器模块（六个）、UART控制器模块（三个）、LCD控制器模块、PIO控制器模块、SPI控制器模块（两个）、定时器模块、DMA模块（两个）、Pipeline Bridge模块（两个）、单端口RAM模块、双端口RAM模块、向量中断控制器模块（两个），各模块之间由AVALON总线连接起来。

A核和B核采用独立的存储器，A核存储器为SDRAM和Flash，B核存储器为单端口RAM（或ROM），双核之间通过双端口RAM进行通信，利用向量中断控制器实现A核和B核的中断系统，这比由Nios II处理器直接处理中断响应快，效率高。

所述FPGA内部模块中的CAN控制器，是Opencores公司专为CAN总线通信而设计的IP核。这种CAN控制器IP核是仿SJA1000芯片设计的，编程方便，时钟速度是SJA1000两倍以上。利用六个CAN控制器IP核构成六路CAN总线，处理器通过AVALON总线访问CAN控制器，可以实现六路CAN总线高速并行收发。

所述FPGA内部模块中的UART控制器IP核共三个，其中两个用于接收惯性/卫星组合导航传感器数据，利用两个DMA转存这两个UART控制器数据至B核存储器单端口RAM（或ROM），以减轻处理器频繁地转存该数据的负担，使处理器有更多的时间去处理其他的事情；另一个用于接收无线射频操控命令。

所述用硬逻辑电路定制关键算法指令，具体是在数据采集与处理控制器模块中A核内，用硬逻辑电路定制矩阵算法指令，用于加速机器人姿态估计时的坐标变换运算，提高数据处理的实时性；在B核内，用硬逻辑电路定制循环冗余码校验指令，用于四肢CAN总线数据的校验，提高数据采集的实时性。

所述FPGA具体为Altera公司生产的EP3C25F256A7N。这种芯片具有快速的数字信号传输速度和丰富的可编程资源，其包括25K逻辑单元、608Kbits嵌入式存储器以及4个PLL，能够搭建双Nios II处理器系统，并配置必要的处理器外设。同时，利用156个用户I/O连接外围芯片，可实现多种传感器数据的并行采集和处理。其内嵌的66个嵌入式18×18乘法器，可以辅助实现各种数字信号处理算法，提高数据处理速度。该芯片可正常工作于-40～125摄氏度，能保证足式机器人在很低和很高的温度环境下工作不受影响。

所述Nios II具体为Altera公司专为构建片上系统而设计的32位RSIC嵌入式处理器Nios II/f，其性能超过200DMIPS，拥有6级流水线，支持单周期硬件乘法/除法器、桶形移位器，带有高速指令和数据缓存，有单片机无法比拟的运算速度。另外，可定制256个用户指令，可采用硬件加速器提高软件性能，这就可以利用FPGA并行计算的特点加速软件关键算法，保证机器人状态感知系统实时完成复杂的算法。

有益效果：

本发明的足式机器人状态感知系统，选择FPGA作为足式机器人状态感知的主芯片，为提高多传感器数据采集和处理的速度，利用IP核构成处理器外设，用硬逻辑电路定制关键算法指令，在FPGA片内构建双Nios II处理器系统，分别完成异构传感器数据多时间尺度采集和高速处理传感器数据获得状态信息。本发明的感知装置包括数据采集与处理控制器模块、动力单元传感器数据采集模块、组合导航传感器数据采集模块、CAN总线通信模块、人机交互模块和电源模块。采用本发明制备的足式机器人状态感知系统，电路的集成度高、体积和功耗小、成本低，具有可扩展性、升级性和较高的抗干扰性，数据处理速度快，便于机器人实时控制。

附图说明

图1为本发明足式机器人状态感知装置方框图；

图2为本发明状态感知装置电源模块电路原理图；

图3为本发明状态感知装置FPGA片内硬件原理图；

图4为本发明状态感知装置数据采集与处理控制器模块电路原理图；

图5为本发明状态感知装置动力单元传感器数据采集模块电路原理图；

图6为本发明状态感知装置CAN总线通信模块电路原理图；

图7为本发明状态感知装置组合导航传感器数据采集模块电路原理图；

图8为本发明状态感知装置人机交互模块电路原理图。

具体实施方式

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

实施例1：

1.如图1所示，本发明用FPGA芯片——EP3C25F256A7N作为足式机器人状态感知的主芯片，在FPGA片内构建双Nios II处理器系统，分别完成异构传感器数据多时间尺度采集和高速地处理传感器数据以获得状态信息，双核之间通过片内双端口RAM进行通信。数据采集与处理控制器模块、动力单元传感器数据采集模块、组合导航传感器数据采集模块、CAN总线通信模块、人机交互模块和电源模块一起组成足式机器人状态感知装置。动力单元传感器数据采集模块将机器人动力单元各个传感器输出的模拟量转换为数字量，主要芯片是ADC芯片MAX188和参考源芯片MAX6350；组合导航传感器数据采集模块通过串口读取惯性/卫星组合导航传感器IMU和GPS的数据，主要芯片是MAX3232；CAN总线通信模块包含六路CAN总线，四路与机器人四肢伺服控制器相连，另两路分别与规划与协调层和环境感知单元相连，主要芯片是CTM8251D；

数据采集与处理控制器模块由Nios II处理器A核和B核组成，具体为NiosII/f，B核接收动力单元传感器数据采集模块、组合导航传感器数据采集模块、CAN总线通信模块的传感器数据并进行预处理，A核处理传感器数据获得状态信息，通过CAN总线通信模块传至规划与协调层和环境感知单元；人机交互模块采用128×64点阵液晶显示屏、蜂鸣器和无线射频收发器组成，用于显示机器人开机自检信息和故障报警信息，提供声音报警信号，并接收无线射频操控命令；电源模块采用线性稳压与开关稳压相结合的方式，降低电源功率损耗，为电路板上其他部分和动力单元传感器以及组合导航传感器提供低纹波电源。

2.如图2所示，系统供电电压为+18~40V，采用宽电压范围，以消除供电电源波动对系统的影响。电源模块所提供的电压值有+15V、+5V、+3.3V、+2.5V和+1.2V。输入电压经开关稳压芯片LM2596-ADJ降为+15V，经过线性稳压芯片LT1085-5降为+5V，然后经过线性稳压芯片LT1085-3.3降为+3.3V和线性稳压芯片LM1117-2.5降为+2.5V，+3.3V经过线性稳压芯片LM1117-1.2降为+1.2V。

3.如图3所示，FPGA内部模块划分为两个Nios II处理器模块、一个SDRAM控制器模块、一个Flash控制器模块、六个CAN控制器模块、三个UART控制器模块、一个LCD控制器模块、一个PIO控制器模块、两个SPI控制器模块、一个定时器模块、两个DMA模块、两个Pipeline Bridge模块、一个单端口RAM模块、一个双端口RAM模块、两个向量中断控制器模块，各模块之间由AVALON总线连接起来。A核的IP核外设两个CAN控制器模块、一个UART控制器模块、一个LCD控制器模块、一个PIO控制器模块通过一个Pipeline Bridge模块与A核相连；B核的IP核外设四个CAN控制器模块、两个SPI控制器模块通过一个Pipeline Bridge模块与B核相连，以简化系统总线，提高系统可运行的频率。

A核存储器为SDRAM和Flash，通过SDRAM控制器模块和Flash控制器模块三态桥访问外接SDRAM和NOR Flash芯片。B核存储器为片内单端口RAM（或ROM），具有20kB存储空间，它可以固化程序，同时系统运行时又作为数据存储器。双核之间通过双端口RAM进行通信，每个核通过AVALON总线访问双端口RAM，数据位宽为32位,具有4kB存储空间。

利用两个向量中断控制器（VIC,Vectored Interrupt Controller）分别实现A核和B核的中断系统，处理器IP核外设的中断信号均送到VIC，由VIC判断中断标志，对中断优先级进行仲裁，并处理中断向量给处理器，支持中断嵌套和可变优先级，比由Nios II处理器直接处理中断响应快，效率高。

FPGA内部模块中的CAN控制器，是Opencores公司专为CAN总线通信而设计的IP核。这种CAN控制器IP核是仿SJA1000芯片设计的，支持Basic CAN模式和Peli CAN模式，位速率可达1Mbps，编程方便，时钟速度是SJA1000两倍以上。利用六个CAN控制器IP核构成六路CAN总线，处理器通过AVALON总线访问CAN控制器，可以实现六路CAN总线高速并行收发。

FPGA内部模块中的UART控制器IP核共三个，其中两个用于接收惯性/卫星组合导航传感器数据，利用两个DMA转存这两个UART控制器数据至B核存储器单端口RAM（或ROM）。UART控制器每接收到一个字符，DMA就将其转存至单端口RAM（或ROM），在批量接收数据时，减轻了处理器频繁地转存该数据的负担，使处理器有更多的时间去处理其他的事情。另一个UART控制器用于接收无线射频操控命令，操作员可通过无线射频遥控器控制状态感知装置取消声音报警和滚动屏幕显示内容。

FPGA内部模块中的定时器模块，其定时间隔设置为1ms，用于控制传感器数据采集的时间节拍。LCD控制器模块具体为LCD16207控制器，它提供了与LCD12864相兼容的引脚连接和访问方式，能方便地实现12864液晶的刷新显示。两个SPI控制器模块通过SPI总线分别控制两片ADC芯片，完成通道选择、时钟输入、转换结果读取。

4.FPGA与SDRAM、Flash、配置芯片的连接方式如图4所示。FPGA具体为Altera公司生产的EP3C25F256A7N。FPGA配置芯片采用EPCS16，具有16MBits的存储空间。SDRAM采用HY57V561620芯片，具有32Mbytes存储空间。NOR FLASH采用S29AL032N，具有4Mbytes存储空间。系统启动后将固化程序从Flash调入SDRAM运行，SDRAM既是程序存储器又是数据存储器。

5.如图5所示，动力单元传感器数据采集模块采用两片8通道12位串行ADC芯片MAX188，实现16路模拟信号的采集。MAX188支持8通道单端输入，可以5V供电，支持SPI串行通信方式，通信速度为5Mbps，采样率为133kHz。若16个通道同时采样，则每个通道在1ms内可采样16次。两片ADC共用一个5V参考源。参考源采用MAX6350芯片，输入电压为15V，输出电压为5.000V，精度为±0.001V，满足采集变送器0～5V或1～5V输出的要求。在12位的采样精度下，ADC分辨率为1.22mV。图5中U5和U6的DIN、DOUT、SCLK、/CS引脚，分别连接至图4中U1的I/O引脚。FPGA芯片通过ADC芯片的DIN、DOUT、SCLK、/CS引脚访问ADC，完成模数转换过程。

6.如图6所示，CAN总线通信模块采用三个双路隔离CAN收发器CTM8251D，实现六路CAN总线数据收发。四路与机器人四肢伺服控制器相连，另两路分别与规划与协调层和环境感知单元相连。每个收发器外接两个螺钉压线框式接线端子，实现与CAN总线传输介质双绞线的连接。CTM8251D芯片内部集成了所有必需的CAN隔离及CAN收发器件，将CAN控制器的逻辑电平转换为CAN总线的差分电平并且具有DC2500V的隔离功能，速率最高达1Mbit/s，提供短路保护和热保护，对电磁干扰有高的抗干扰性，至少可连接110个节点。图6中CAN1、CAN2、CAN3的RXD1、TXD1、RXD2、TXD2引脚，分别连接至图4中U1的I/O引脚。FPGA芯片通过CTM8251D芯片的RXD1、TXD1、RXD2、TXD2引脚访问CTM8251D，完成CAN总线数据的收发。

7.如图7所示，组合导航传感器数据采集模块采用一片双路232电平转换器MAX3232芯片，实现3.3V的TTL电平到232电平的转换。每个通道外接一个9针串口接口，实现与RS232数据线的连接。MAX3232支持3V到5.5V的供电，速率最高达250kbit/s，高于RS232总线标准最大波特率，能满足所有RS232总线应用要求。图7中U8的T1IN、T2IN、R1OUT、R2OUT引脚，分别连接至图4中U1的I/O引脚。FPGA芯片通过MAX3232芯片的T1IN、T2IN、R1OUT、R2OUT引脚访问MAX3232，完成串口数据的收发。

8.如图8所示，人机交互模块由液晶、蜂鸣器和无线射频收发器组成。液晶模块采用一块带中文字库的液晶屏LCD12864，是128×64点阵的汉字图形型液晶显示模块，内置国标GB2312码简体中文字库（16×16点阵）、128个字符（8×16点阵）。FPGA的三根控制信号线HCS0～HCS2和八根数据信号线LCD_DB00～LCD_DB07，经两片74HC245芯片U9、U10驱动后，连接至LCD12864，完成信息显示。FPGA的一根信号线BELL，经三极管放大后，连接至蜂鸣器，控制其发声。无线射频收发器采用APC220-43模块，其嵌入了高性能射频芯片ADF7020-1，传输距离可达1200米，工作频率范围为418M～455MHz，1KHz步进，提供UART/TTL接口，通信波特率可达57600bps。FPGA的两根信号线UART3_TX、UART3_RX，连接至射频模块接口，完成无线射频通信。

基于FPGA的高性能足式机器人的状态感知装置，硬件接口丰富，能完全感知机器人控制所需的状态信息，且电路集成度高，有益于提高系统抗干扰性，降低体积、功耗和成本，同时充分发挥了FPGA并行计算的优势，具备高速数据采集和处理的能力，另外，可扩展性好，系统的软硬件升级方便。

Claims (3)

1.一种足式机器人状态感知系统，其特征在于，包括嵌入式处理器模块、组合导航传感器数据采集模块、组合导航传感器、动力单元传感器数据采集模块、动力单元传感器、人机交互模块和通信模块；

组合导航传感器通过组合导航传感器数据采集模块与嵌入式处理器模块相连；

动力单元传感器通过动力单元传感器数据采集模块与嵌入式处理器模块相连；

人机交互模块和通信模块均与嵌入式处理器模块相连；

通信模块还与上位机通信连接；

所述的嵌入式处理器模块包括基于FPGA的双端口RAM和基于FPGA的两个NIOS II嵌入式处理器核：即A核和B核，两个NIOS II嵌入式处理器核通过双端口RAM通信连接；

所述的嵌入式处理器模块还包括SDRAM控制器模块、Flash控制器模块、CAN控制器模块、UART控制器模块、LCD控制器模块、PIO控制器模块、SPI控制器模块、定时器模块、DMA模块、总线桥模块、单端口RAM模块、向量中断控制器模块，嵌入式处理器模块内的各模块之间由AVALON总线相连；A核采用的存储器为SDRAM和Flash，B核采用的存储器为单端口RAM，利用向量中断控制器模块实现A核和B核的中断系统。

2.根据权利要求1所述的足式机器人状态感知系统，其特征在于，所述的通信模块为CAN总线通信模块；

组合导航传感器为惯性/卫星组合导航传感器；

动力单元传感器包括四个流量传感器、两个压力传感器、三个温度传感器、一个转速传感器和一个电压传感器；

人机交互模块包括液晶显示屏、蜂鸣器和无线射频收发器。

3.根据权利要求2所述的足式机器人状态感知系统，其特征在于，FPGA为Altera公司生产的EP3C25F256A7N；

动力单元传感器数据采集模块采用ADC芯片MAX188和参考源芯片MAX6350；组合导航传感器数据采集模块采用MAX3232芯片。