CN103273489B - 基于主从遥操作机械臂的机器人控制系统及方法 - Google Patents

Abstract

本发明公开了一种基于主从遥操作机械臂的机器人控制系统及方法。包括主机械臂及信息采集板、监控中心PC、机器人主控制板、从机械臂及FPGA运动控制板、现场环境信息采集板。包括步骤：主机械臂的制作及数据采集；从机械臂、小车及云台的控制；机器人主控制板的视频采集和通信；现场环境信息采集板的数据采集及通信；控制中心PC的数据显示及通信。能在不增加系统复杂度的情况下控制机械臂来完成相对复杂的动作。设计采用模块化的方法，模块之间通过CAN总线进行通信，用FPGA作为运动控制板，方便了系统的搭建及升级，提高了系统的稳定性。本机器人不仅将作业现场的视频信息，而且将作业现场的环境信息传输到监控中心，便于控制机器人完成更加复杂的作业。

Description

基于主从遥操作机械臂的机器人控制系统及方法

技术领域

   本发明属于机器人控制领域，具体涉及的是一种基于主从遥操作机械臂的机器人控制系统及方法。

背景技术

   过去的十年间，随着科学技术的发展，机器人已经深入到生产生活的各个领域，机器人控制方法已成为各大高校及企事业单位的研究热点。机器人不仅能够代替人们从事一些比较复杂或者重复性比较高的工作，而且能够到达人们不能到达或者比较危险的现场。随着机器人功能的不断增强，在一些领域人们可以借助机器人从事以前不能做到的工作，机器人甚至已经完全替代工人的操作。到目前为止，在手术机器人的协助下，全球已完成多例跨国手术，给病患带来了更多的希望；2011年日本福岛核电站事故中，机器人多次扮演重要角色，不仅到达核辐射比较严重的区域进行数据采集，而且能够从事设备维修；全球最大的电子代工厂富士康计划2014年前引进30万台机器人代替人工等等。很多例子都证明进行机器人控制技术的研究具有深远的社会意义。

    机械臂的控制方法是机器人控制中的关键环节，现有的机械臂控制方法多采用结合图像对机械臂进行建模或者通过控制面板分别控制各个关节，这两种控制方法的实施有很多限制，并且存在一定的技术缺陷。第一，通过摄像头采集现场的三维图像信息，对机械臂进行建模，让机器人进行自主决策的控制方法对控制算法、摄像头精度及机械臂要求比较高，而且本方法在现场环境比较复杂的情况下不适用。第二，通过旋转控制面板上的旋钮控制对应机械臂关节的控制方法简单易行，但这种方法不能实现机械臂的多轴联动，难以完成比较复杂的操作。

发明内容

针对现有技术存在的缺陷，本发明的目的是提供一种基于主从遥操作机械臂的机器人控制系统及方法，能够在对机械臂、摄像头和作业现场环境要求不高的前提下，远程操作机械臂来完成相对复杂的动作，并且能收集作业现场的环境信息，提高了机器人的实用性。为达到上述目的，本发明的构思是：在机器人上搭载了无线通信设备，可直接与控制中心的PC通过WIFI(Wireless Fidelity)建立通信，也可以与现场的无线路由器通信，再通过无线路由器与控制中心之间的公共网络与PC机进行通信。设计并制作与从机械臂也就是机器人所带的机械臂同等比例尺寸的主机械臂，采集主机械臂的各个关节的角度数据，通过监控中心PC发送到作业现场机器人主控制板，通过FPGA(Field Programmable Gate Array)运动控制板控制从机械臂动作。在机器人主控制板上挂载了传感器信息采集板，采集作业现场的温度、湿度、GPS(Global Position System)及运动底盘的速度信息，通过主控制板发送到监控中心PC。机器人主控制板、FPGA运动控制板及现场环境信息采集板可通过CAN(Controller Area Network)总线实现通信。本设计采用可重构模块化设计理念，可以添加或删减部分模块。

根据上述发明构思，本发明的技术方案是：

一种基于主从遥操作机械臂的机器人控制系统，包括主机械臂及信息采集板、监控中心PC、机器人主控制板、从机械臂及FPGA运动控制板和现场环境信息采集板，其特征在于所述监控中心PC联接主机械臂及信息采集板，监控中心PC通过无线网络通信连接机器人主控制板，机器人主控制板通过CAN总线联接从机械臂及FPGA运动控制板和现场环境信息采集板板。

所述的主机械臂及信息采集板，是指由操作人员手动操作的主臂，以及主臂各个关节角度信息的采集板；所述的监控中心PC，是指监控终端，负责与作业现场机器人的通信、视频及各种信息的显示以及控制指令的输入；所述的机器人主控制板是指作业现场机器人的主控制板，是一个主控平台通过一个以太网接口联接一个USB无线网卡的无线通信模块，通过一个USB接口联接一个USB摄像头的视频采集模块，从而负责与监控中心PC通信、现场视频采集、与运动控制板及传感器信息采集板的通信；所述机械臂及FPGA运动控制板是指：所述从机械臂是指机器人所带的机械臂，所述FPGA运动控制板是指从机械臂、摄像头云台及小车控制板，控制指令来自机器人主控制板；所述的现场环境信息采集板是指机器人所带的传感器信息采集板，采集机器人作业现场的环境信息，并传输到主控制板。

一种基于主从遥操作机械臂的机器人控制系统方法，采用上述系统进行操作，其特征在于操作步骤如下：

1）主机械臂及信息采集板的制作和数据采集：制作与从机械臂相同尺寸比例的操作杆作为主机械臂，主机械臂的各个臂杆通过角度传感器相连接；以基于Cortex-M3芯片的STM32开发板作为信息采集板，采集角度传感器的模拟信号，经过A/D(Analog/Digit)转换器转换成数字信号并通过串口把数字信号传输到PC机；

2）从机械臂及FPGA运动控制板的制备及其控制：以FPGA作为运动控制板，通过CAN总线与机器人主控制板通信，通过6路PWM(Pulse Width Modulation)波控制从机械臂，通过2路PWM波控制小车行驶，通过1路PWM波控制摄像头云台；在FPGA内配置CAN控制器、9个PWM波生成器以及niosII处理器，niosII处理器完成CAN控制器的配置、命令解析已经PWM波生成器的配置；

3）机器人主控制板的视频采集和通信：机器人主控制板采用搭载嵌入式Linux操作系统的ARM9(Advanced RISC Machines 9)开发板，机器人主控制板通过USB无线网卡与监控中心PC或现场无线路由器通信，现场路由器通过公共网络与监控中心PC通信。机器人主控制板通过USB摄像头采集现场视频信息，经过xvidcore(一种开源软件名称)视频编解码库编码后传输到控制中心PC；主控制板接收监控中心的命令信息，经过信息解析后，完成对从机械臂、小车和摄像头云台的控制，并把通过现场环境信息采集板采集到的各种信息经过处理和融合后发送到监控中心PC。

4）现场环境信息采集板的数据采集及通信：以另一块STM32开发板作为现场环境信息采集板，采集板上装有GPS、温度、湿度和红外传感器，会定时采集各种传感器信息并通过CAN总线上传到主控制板；

5）监控中心PC的数据显示及通信：监控中心PC可以直接通过WIFI与作业现场机器人建立连接，也可以通过公共网络与远程作业现场机器人建立连接；监控中心PC负责通过串口接收主机械信息采集板发送的角度信息，通过控制界面接收操作人员的控制命令，接收作业现场机器人发送的视频和传感器信息，将主机械臂的传感器信息发送到机器人，并实时解码和显示现场视频信息以及现场传感器信息；

所述步骤1）主机械臂的制作及信息采集，信息采集板采集主机械臂的各个角度传感器信息，经过A/D转换后，通过串口发送的控制中心PC，其具体步骤为：

① 依据从机械臂的尺寸和结构设计制作操作杆作为主机械臂，主机械臂的各个关节通过角度传感器相互连接，角度传感器的型号为WDD22A(一种角度传感器型号)；

② 编写基于STM32开发板的模拟信号采集程序和A/D转换程序；

③ 编写与PC机的串口通信程序，等待接收PC机的数据采集使能信号，并将A/D转换后的数字信息上传到PC。

由于从机械臂的各个关节转动角度有限，而主机械臂的各个关节可以实现360度转动，因此从机械臂信息采集板需要判断采集到的角度传感器数据，只有在规定范围之内的数据才能被上传到PC；为了提高对从机械臂的保护，除了此次对角度信息检查，在现场主控制板也要实施对该角度信息的检查；主从机械臂之间建立同步后，操作人员可以通过手动操作主机械臂来实现对从机械臂，也就是对机器人所带机械臂的控制。

所述步骤2）以FPGA作为运动控制板，控制从机械臂、摄像头云台以及小车，其具体步骤为：

① 在FPGA内配置经济型niosII(一种嵌入式处理器)处理器，并配置程序运行所需的RAM(Random-Access Memory)，调试所需的JTAG(调试接口)及晶振外设；

② 在PFGA内配置CAN控制器IP(Intellectual Property)核，该IP采用开源硬件描述程序，采用CAN20.B协议，通过外置CAN收发器与CAN总线通信；

③ 在FPGA内配置9路PWM波生成器，其中6路用来控制六自由度机械臂，2路用来控制小车的转向舵机和前进电机，1路用来控制摄像头云台中的舵机；

④ 开发基于niosII处理器的程序，配置CAN控制器，通过CAN总线接收控制信息，控制PWM波生成器实现对小车、从机械臂和摄像头云台的控制。

从机械臂中没有反馈系统和自我保护系统，只能实现开环控制，由于机器人主控制板已经对接收到的控制信息进行了阈值检查，在FPGA内部可以将直接根据通过CAN总线接收到的角度信息控制到PWM波生成器。FPGA运动控制板CAN节点ID分配为0x02，采用标准帧格式。

所述步骤3）机器人主控制板通过无线网卡与监控中心PC通信，采集现场视频，通过CAN总线与现场环境信息采集板及FPGA运动控制板通信，其具体步骤为：

    ① 在ARM9开发板上移植嵌入式Linux系统，采用基于中星微公司的ZC301芯片的摄像头，移植该USB摄像头驱动程序；通过SPI(Serial Peripheral Interface)接口连接CAN控制器MCP2510，开发该CAN控制器驱动程序；MCP2510通过CAN收发器接入CAN总线；

② 利用嵌入式Linux操作系统自带的V4L2(Video for Linux 2)图像采集驱动程序通过集USB摄像头采集图像，由于采用的摄像头内部集成了JPEG(Joint Photographic Experts Group)压缩器，因此需要调用JPEG解压缩程序对采集到的图像进行解压缩；移植开源视频编解码库xvidcore，调用该库的API(Application Program Interface)函数对解压缩后的图像数据进行编码，由于采用的实时传输库jrtplib(一种开源软件库名称)每次只能发送大小不大于1400字节的数据，因此需要对编码后的数据分割，并添加标志位，再调用jrtplib的发送API顺序发送分割后的数据；

③ 通过socket(套接字) API循环接收监控中心PC发送的控制信号，对该控制信号进行解析得到对主控制板的控制信息、对传感器信息采集板的控制信息以及对运动控制板的控制信息；

④ 从控制信息中解析得到对从机械臂的控制命令，经过角度合法性检查后，通过CAN总线将主机械臂的角度信息传输到FPGA运动控制板，控制机械臂来完成相关动作；

⑤ 从控制信息中解析得到对传感器信息采集板的控制命令，完成传感器信息采集板的相关控制，比如采集板使能、各个传感器模块开启或者关闭；

⑥ 编写CAN总线通信程序，完成对CAN节点的配置，重新创建一个线程，在该线程内通过CAN总线完成数据的发送和接收，将接收的数据进行融合并通过socket发送到监控中心PC。

在机器人主控制板程序中创建了两个线程，一个线程循环完成视频数据的采集、编码及发送，另一个线程循环完成对控制信息的接收、解析以及通过CAN总线将控制信息传输到运动控制板和信息采集板。在主控制板中移植了嵌入式Linux操作系统，并在该系统中移植了开源视频编解码库及实时传输库，实现视频的编码和实时发送；由于从机械臂的各个关节摆动幅度有限，而主机械臂可以实现360度转动，为了保护从机械臂，需要再次对解析得到的从机械臂的控制信息进行检查，只将正确的角度信息传输到运动控制板；现场主控制板CAN节点ID分配为0x01。

所述步骤（4）现场环境信息采集板借助多种传感器采集作业现场的环境信息，并通过CAN总线将环境信息传输到机器人主控制板，将现场情况反馈到监控中心，其具体步骤为：

    ① 硬件平台的搭建：信息采集板采用基于Cortex-M3芯片的STM32开发板，通过串口连接GPS模块，通过GPIO(General Purpose Input/Output)口连接红外传感器、温度传感器和速度采集模块；

    ② 编写串口通信程序，通过串口接收GPS模块发送过来的字符串，经过字符串处理后得到小车所在位置的经纬度数据；

    ③ 通过GPIO模拟与红外传感器通信的时序，采集安装在车身四周的红外传感器数据，得到车身周围的障碍物距离；通过GPIO模拟与温度传感器通信的时序，采集车身所带的温度传感器数据；通过GPIO口与速度传感器模块通信，采集小车的实时速度信息；

    ④ 编写CAN通信程序，配置CAN节点，将采集得到的红外、温度、速度以及GPS数据打包，将打包后的数据通过CAN总线传输到机器人主控制板；CAN协议的帧格式定义如下：

0 byte

1 byte

2 byte

3-4 byte

5-7 byte

帧开头及ID

红外

温度

速度

GPS

留用

CRC校验及帧结束

传感器信息采集板上电后，通过CAN总线完成与机器人主控制板的握手，等待主控制板发送采集板使能信号；接收到使能信号后，开始各种传感器信息的采集和发送，每个传感器都可以在主控制板的控制下实现开启或关闭；传感器信息采集板CAN节点ID分配为0x03。

所述步骤5）监控中心PC实现与作业现场机器人的通信，将主机械臂的关节角度信息发送到主控制板，接收主控制板发送的视频数据和各种现场环境信息数据，并实现视频信息和各种环境信息的实时显示，其具体步骤为：

    ① 编写串口通信程序，通过串口与主机械臂信息采集板通信，向信息采集板发送使能信息，并接收信息采集板发送过来的主机械臂的角度信息；

    ② 编写视频数据接收程序。通过实时传输库jrtplib完成对视频数据的实时接收，并根据数据包中的标志位，将接收到的分割后的数据打包成一帧数据，并调用开源视频编解码库xvidcore完成对数据的解码；

    ③ 通过socket API接收现场主控制板发送过来的各种传感器数据，并对该数据进行解析得到现场的温度、车身周围环境及GPS信息；

    ④ 通过界面接收监控中心人员输入的各种控制信息，将输入的控制信息与主机械臂的角度信息融合后通过socket API发送到作业现场机器人；

    ⑤ 利用MFC库编写显示程序，实现作业现场视频和各种现场环境信息的显示，并在界面中添加多个控制按钮，包括主机械臂信息采集使能按钮、从机械臂的控制使能按钮、现场信息采集板使能按钮、摄像头云台的控制按钮、视频显示以及开始或停止录像按钮。

    监控中心PC机程序中开启了三个线程，一个线程完成视频数据的接收、解码和显示，一个线程完成主机械臂的关节角度信息的接收、界面控制命令的接收、数据的融合以及通过socket API的发送，另外一个线程完成通过socket API接收机器人主控制板发送的传感器信息数据以及数据的分析和显示。监控中心PC程序基于VC++ 6.0平台编写，并移植了开源视频编解码库xvidcore和实时传输库jrtplib。

    本发明与现有技术相比较，具有如下显而易见的突出实质性特点和显著技术进步：

    本发明中采用遥操作方式对车载机械臂进行控制，在对机械臂结构和作业环境没有较高要求的条件下，可以控制机械臂完成移动物体、旋转螺丝等相对复杂的任务。本发明对机器人平台，采用可重构模块化设计理念，提高了系统的稳定性与适应性，机器人各个模块采用CAN总线进行连接，可以再不影响系统其它模块的前提下完成功能模块的增添或删减；机器人采用FPGA作为运动控制板，可以通过对FPGA进行重新编程方便的实现机械臂等运动模块的更换。

附图说明

   图1为本发明的系统结构框图。

    图2为本发明的主从机械臂的结构对照图。

    图3为本发明的机器人主控制板、FPGA运动控制板和现场环境信息采集板。

    图4为本发明的机器人主控制板程序流程图。

    图5为本发明的监控中心PC程序流程图。

    图6为本发明的监控中心PC控制面板说明。

    图7为实验结果截图。

具体实施方式

   下面结合附图对本发明的优选实施例作详细说明：

    实施例一：

   如图1所示，本基于主从遥操作机械臂的机器人控制系统，包括主机械臂及信息采集板(1)、监控中心PC(2)、机器人主控制板(3)、从机械臂及FPGA运动控制板(4)、现场环境信息采集板(5)。所述监控中心PC(2)联接主机械臂及信息采集板(1)，监控中心PC(2)通过无线网络通信连接机器人主控制板(3)，机器人主控制板(3)通过CAN总线联接从机械臂及FPGA运动控制板(4)、现场环境信息采集板(5)。

 所述的主机械臂及信息采集板(1)，是指本系统中由操作人员手动操作的主臂，以及主臂各个关节角度信息的采集板；所述的监控中心PC(2)，是指本系统中的监控终端，负责与作业现场机器人的通信、视频及各种信息的显示以及控制指令的输入；所述的机器人主控制板(3)，是指本系统中作业现场机器人的主控制板，是一个主控平台通过一个以太网接口联接一个USB无线网卡的无线通信模块，通过一个USB接口联接一个USB摄像头的视频采集模块，从而负责与监控中心PC通信、现场视频采集、与运动控制板及传感器信息采集板的通信；所述的从机械臂(4)，是指本系统中的机器人所带的机械臂，所述的FPGA运动控制板(4)，是指本系统中的从机械臂、摄像头云台及小车控制板，控制指令来自机器人主控制板；所述的现场环境信息采集板(5)，是指本系统中的机器人所带的传感器信息采集板，采集机器人作业现场的环境信息，并传输到主控制板。

    如图2所示，为主从机械臂的结构对照图。从机械臂有六个自由度，即六个关节，每个臂杆通过舵机相连，主机械臂也有六个关节，臂长的比例与从机械臂臂长的比例相同，这样保证通过手动操作主机械臂，从机械臂可以完成相同的动作，主机械臂的臂杆通过角度传感器相互连接。

    实施例二：

    如图3所示，为操作现场硬件平台，包括机器人主控制板、FPGA运动控制板和传感器信息采集板。主控制板作为主控模块，是一块搭载了嵌入式Linux的ARM开发板，开发板上安装有USB摄像头、USB无线网卡、CAN总线控制器MCP2510，USB摄像头完成作业现场视频采集，USB无线网卡完成通过WIFI与监控中心PC或现场无线路由通信，MCP2510控制器通过CAN总线收发器接入CAN总线，与两外两个模块通信。嵌入式Linux操作系统中除了移植了摄像头驱动程序、无线网卡驱动程序，还移植了开源视频编解码库xvidcore和实时传输库jrtplib，负责视频的编码和实时传输。传感器信息采集板是一块基于Cortex-M3芯片的开发板，开发板上安装了温度、湿度、速度传感器以及GPS模块，还可以根据需要增减传感器模块，开发板可通过自身自带的CAN控制器与主控制板通信。FPGA运动控制板内部配置了niosII处理器，开源CAN控制器IP核，9路PWM波生成器，其中6路PWM波控制从机械臂，2路PWM波控制小车，1路PWM波控制摄像机云台舵机，由于FPGA可以重新配置，因此可以根据需要更换机械臂、小车。

 基于主从遥操作机械臂的机器人控制系统的机器人控制系统方法，其特征如下，包括以下步骤：

1）主机械臂及信息采集板(1)的制作和数据采集：制作与从机械臂相同尺寸比例的操作杆作为主机械臂，主机械臂的各个臂杆通过角度传感器相连接；以基于Cortex-M3芯片的STM32开发板作为信息采集板，采集角度传感器的模拟信号，经过A/D转换器转换成数字信号并通过串口把数字信号传输到PC机。

2）运动控制：以FPGA作为运动控制板，通过CAN(Controller Area Network)总线与机器人主控制板通信，通过6路PWM波控制从机械臂，通过2路PWM波控制小车行驶，通过1路PWM波控制摄像头云台；在FPGA内配置CAN控制器、9个PWM波生成器以及niosII处理器，niosII处理器完成CAN控制器的配置、命令解析已经PWM波生成器的配置。

3）机器人主控制板：机器人主控制板采用搭载嵌入式Linux操作系统的ARM9开发板，主控制板通过USB无线网卡与控制中心PC或现场无线路由器通信，现场路由器通过公共网络与监控中心PC通信；主控制板通过USB摄像头采集现场视频信息，经过xvidcore视频编解码库编码后传输到监控中心PC；主控制板接收监控中心PC的命令信息，经过信息解析后，完成对从机械臂、小车和摄像头云台的控制，并把通过现场环境信息采集板采集到的各种信息经过处理和融合后发送到监控中心PC。

4）现场环境信息采集板：以另一块STM32开发板作为现场环境信息采集板，采集板上装有GPS、温度、湿度、红外等传感器，会定时采集各种传感器信息并通过CAN总线上传到主控制板；

5）监控中心PC：监控中心PC可以直接通过WIFI与作业现场机器人建立连接，也可以通过公共网络与远程作业现场机器人建立连接。监控中心PC负责通过串口接收主机械信息采集板发送的角度信息，通过控制界面接收操作人员的控制命令，接收作业现场机器人发送的视频和传感器信息，将主机械臂的传感器信息发送到机器人，并实时解码和显示现场视频信息以及现场传感器信息。

 如图4所示，所述步骤3）机器人主控制板通过无线网卡与监控中心PC通信，采集现场视频，通过CAN总线与现场环境信息采集板及FPGA运动控制板通信，其具体步骤为：

    ① 在ARM9开发板上移植嵌入式Linux系统，本发明采用基于中星微公司的ZC301芯片的摄像头，移植该USB摄像头驱动程序；通过SPI接口连接CAN控制器MCP2510，开发该CAN控制器驱动程序；MCP2510通过CAN收发器接入CAN总线；

② 利用嵌入式Linux操作系统自带的V4L2图像采集驱动程序通过集USB摄像头采集图像，由于本发明采用的摄像头内部集成了JPEG压缩器，因此需要调用JPEG解压缩程序对采集到的图像进行解压缩；移植开源视频编解码库xvidcore，调用该库的API函数对解压缩后的图像数据进行编码，由于本发明采用的实时传输库jrtplib每次只能发送大小不大于1400字节的数据，因此需要对编码后的数据分割，并添加标志位，再调用jrtplib的发送API顺序发送分割后的数据；

③ 通过socket API循环接收监控中心PC发送的控制信号，对该控制信号进行解析得到对主控制板的控制信息、对传感器信息采集板的控制信息以及对运动控制板的控制信息；

④ 从控制信息中解析得到对从机械臂的控制命令，经过角度合法性检查后，通过CAN总线将主机械臂的角度信息传输到FPGA运动控制板，控制机械臂来完成相关动作；

⑤ 从控制信息中解析得到对传感器信息采集板的控制命令，完成传感器信息采集板的相关控制，比如采集板使能、各个传感器模块开启或者关闭；

⑥ 编写CAN总线通信程序，完成对CAN节点的配置，重新创建一个线程，在该线程内通过CAN总线完成数据的发送和接收，将接收的数据进行融合并通过socket发送到监控中心PC。

在机器人主控制板程序中创建了两个线程，一个线程循环完成视频数据的采集、编码及发送，另一个线程循环完成对控制信息的接收、解析以及通过CAN总线将控制信息传输到运动控制板和信息采集板；在主控制板中移植了嵌入式Linux操作系统，并在该系统中移植了开源视频编解码库及实时传输库，实现视频的编码和实时发送；由于从机械臂的各个关节摆动幅度有限，而主机械臂可以实现360度转动，为了保护从机械臂，需要再次对解析得到的从机械臂的控制信息进行检查，只将正确的角度信息传输到运动控制板。现场主控制板CAN节点ID分配为0x01。

如图5所示，所述步骤5）监控中心PC实现与作业现场机器人的通信，将主机械臂的关节角度信息发送到主控制板，接收主控制板发送的视频数据和各种现场环境信息数据，并实现视频信息和各种环境信息的实时显示，其具体步骤为：

    ① 编写串口通信程序，通过串口与主机械臂信息采集板通信，向信息采集板发送使能信息，并接收信息采集板发送过来的主机械臂的角度信息；

    ② 编写视频数据接收程序。通过实时传输库jrtplib完成对视频数据的实时接收，并根据数据包中的标志位，将接收到的分割后的数据打包成一帧数据，并调用开源视频编解码库xvidcore完成对数据的解码；

    ③ 通过socket API接收现场主控制板发送过来的各种传感器数据，并对该数据进行解析得到现场的温度、车身周围环境及GPS信息；

    ④ 通过界面接收监控中心人员输入的各种控制信息，将输入的控制信息与主机械臂的角度信息融合后通过socket API发送到作业现场机器人；

    ⑤ 利用MFC库编写显示程序，实现作业现场视频和各种现场环境信息的显示，并在界面中添加多个控制按钮，包括主机械臂信息采集使能按钮、从机械臂的控制使能按钮、现场信息采集板使能按钮、摄像头云台的控制按钮、视频显示以及开始或停止录像按钮。

    监控中心PC机程序中开启了三个线程，一个线程完成视频数据的接收、解码和显示，一个线程完成主机械臂的关节角度信息的接收、界面控制命令的接收、数据的融合以及通过socket API的发送，另外一个线程完成通过socket API接收机器人主控制板发送的传感器信息数据以及数据的分析和显示。监控中心PC程序基于VC++ 6.0平台编写，并移植了开源视频编解码库xvidcore和实时传输库jrtplib。

    实施例三：

    如图6，图7所示，为实验例二的实验部分，即功能验证部分。

    如图6所示，为基于MFC编写的监控中心的控制面板截图。如图标注所示，控制面板大体包括视频显示、控制面板操作、子模块操作、机器人位置显示、现场机器人IP地址输入、传感器开关及现场环境信息和车身状况显示部分。在建立通信后，操作人员可以通过手动操作主机械臂来完成对从机械臂的控制，作业现场的操作画面可以通过视频传输到控制面板，同时机器人的位置、作业现场的环境信息、机器人运行状况可以实时显示到控制面板中。操作人员也可以根据需要关闭主控制板、传感器信息采集板、各个传感器模块等部分。

    如图7所示，为机器人在上海大学延长校区电机楼门口运行时的控制面板截图，可以看到机器人采集到的现场图像可以清晰地传输到监控中心，机器人所处的位置(地图中黑点所示)、环境信息、机器人运行状况也都反映在了控制面板上。

    由图6、图7所示，本发明基于主从遥操作机械臂的机器人控制系统的功能已全部实现。

本实施例在以本发明技术方案为前提下进行实施，给出了详细的实施方式和具体的操作过程，但本发明的保护范围不限于上述的实施例。

Claims (6)

1.一种基于主从遥操作机械臂的机器人控制方法，采用的基于主从遥操作机械臂的机器人控制系统包括主机械臂及信息采集板(1)、监控中心PC(2)、机器人主控制板(3)、从机械臂及FPGA运动控制板(4)和现场环境信息采集板(5)，其特征在于所述监控中心PC(2)联接主机械臂及信息采集板(1)，监控中心PC(2)通过无线网络通信连接机器人主控制板(3)，机器人主控制板(3)通过CAN总线联接从机械臂及FPGA运动控制板(4)和现场环境信息采集板(5)；

所述的主机械臂及信息采集板(1)，是指由操作人员手动操作的主臂，以及主臂各个关节角度信息的采集板；所述的监控中心PC(2)，是指监控终端，负责与作业现场机器人的通信、视频及各种信息的显示以及控制指令的输入；所述的机器人主控制板(3)是指作业现场机器人的主控制板，是一个主控平台，通过一个以太网接口与无线通信模块中的一个USB无线网卡联接，通过一个USB接口与视频采集模块中的一个USB摄像头联接，从而负责与监控中心PC(2)通信、现场视频采集、与运动控制板及传感器信息采集板的通信；所述机械臂及FPGA运动控制板(4)是指：所述从机械臂是指机器人所带的机械臂，所述FPGA运动控制板是指从机械臂、摄像头云台及小车控制板，控制指令来自机器人主控制板；所述的现场环境信息采集板(5)，是指机器人所带的传感器信息采集板，采集机器人作业现场的环境信息，并传输到主控制板，其特征在于该方法的操作步骤如下：

1）主机械臂及信息采集板(1)的制作和数据采集：制作与从机械臂相同尺寸比例的操作杆作为主机械臂，主机械臂的各个臂杆通过角度传感器相连接；以基于Cortex-M3芯片的STM32开发板作为信息采集板，采集角度传感器的模拟信号，经过A/D(Analog/Digit)转换器转换成数字信号并通过串口把数字信号传输到监控中心PC(2)；

2）从机械臂及FPGA运动控制板(4)的制备及其控制：以FPGA作为运动控制板，通过CAN总线与机器人主控制板通信，通过6路PWM波控制从机械臂，通过2路PWM波控制小车行驶，通过1路PWM波控制摄像头云台；在FPGA内配置CAN控制器、9个PWM波生成器以及niosII处理器，niosII处理器完成CAN控制器的配置、命令解析以及PWM波生成器的配置；

3）机器人主控制板(3)的视频采集和通信：机器人主控制板(3)采用搭载嵌入式Linux操作系统的ARM9开发板，该主控制板通过USB无线网卡与监控中心PC(2)或现场无线路由器通信，现场路由器通过公共网络与监控中心PC(2)通信；机器人主控制板(3)通过USB摄像头采集现场视频信息，经过xvidcore开源软件的视频编解码库编码后传输到监控中心PC(2)；机器人主控制板(3)接收监控中心PC(2)的命令信息，经过信息解析后，完成对从机械臂、小车和摄像头云台的控制，并把通过现场环境信息采集板采集到的各种信息经过处理和融合后发送到监控中心PC(2)；

4）现场环境信息采集板(5)的数据采集及通信：以另一块STM32开发板作为现场环境信息采集板(5)，该采集板上装有GPS、温度、湿度、红外传感器，会定时采集各种传感器信息并通过CAN总线上传到机器人主控制板(3)；

5）监控中心PC(2)的数据显示及通信：监控中心PC(2)可以直接通过WIFI与作业现场机器人建立连接，或者通过公共网络与远程作业现场机器人建立连接；监控中心PC(2)负责通过串口接收主机械臂信息采集板发送的角度信息，通过控制界面接收操作人员的控制命令，接收作业现场机器人发送的视频和传感器信息，将主机械臂的传感器信息发送到机器人，并实时解码和显示现场视频信息以及现场传感器信息。

2.根据权利要求1所述的基于主从遥操作机械臂的机器人控制方法，其特征在于，所述步骤1）主机械臂及信息采集板(1)的制作和信息采集：信息采集板采集主机械臂的各个角度传感器信息，经过A/D转换后，通过串口发送到监控中心PC(2)的具体步骤为：

① 依据从机械臂的尺寸和结构设计制作操作杆作为主机械臂，主机械臂的各个关节通过角度传感器相互连接，角度传感器的型号为WDD22A；

② 编写基于STM32开发板的模拟信号采集程序和A/D转换程序；

③ 编写与监控中心PC(2)的串口通信程序，等待接收监控中心PC(2)的数据采集使能信号，并将A/D转换后的数字信息上传到监控中心PC(2)；

由于从机械臂的各个关节转动角度有限，而主机械臂的各个关节可以实现360度转动，因此主机械臂信息采集板需要判断采集到的角度传感器数据，只有在规定范围之内的数据才能被上传到监控中心PC(2)；为了提高对从机械臂的保护，除了此次对角度信息检查，在现场主控制板也要实施对该角度信息的检查；主从机械臂之间建立同步后，操作人员通过手动操作主机械臂来实现对从机械臂，也就是对机器人所带机械臂的控制。

3.根据权利要求1所述的基于主从遥操作机械臂的机器人控制方法，其特征于，所述步骤2）中FPGA运动控制板控制从机械臂、摄像头云台以及小车的具体步骤为：

① 在FPGA内配置经济型niosII嵌入式处理器，并配置程序运行所需的RAM存储器，调试所需的JTAG调试接口及晶振外设；

② 在PFGA内配置CAN控制器IP核，该IP采用开源硬件描述程序，采用CAN20.B协议，通过外置CAN收发器与CAN总线通信；

③ 在FPGA内配置9路PWM波生成器，其中6路用来控制六自由度机械臂，2路用来控制小车的转向舵机和前进电机，1路用来控制摄像头云台中的舵机；

④ 开发基于niosII处理器的程序，配置CAN控制器，通过CAN总线接收控制信息，控制PWM波生成器实现对小车、从机械臂和摄像头云台的控制；

从机械臂中没有反馈系统和自我保护系统，只能实现开环控制，由于机器人主控制板已经对接收到的控制信息进行了阈值检查，在FPGA内部将直接根据通过CAN总线接收到的角度信息控制PWM波生成器；FPGA运动控制板CAN节点ID分配为0x02，采用标准帧格式。

4.根据权利要求1所述的基于主从遥操作机械臂的机器人控制方法，其特征于，所述步骤3）机器人主控制板(3)采集现场视频和通信的具体步骤为：

    ① 在ARM9开发板上移植嵌入式Linux系统，采用基于中星微公司的ZC301芯片的摄像头，移植该USB摄像头驱动程序；通过SPI接口连接CAN控制器MCP2510，开发该CAN控制器驱动程序；MCP2510通过CAN收发器接入CAN总线；

② 利用嵌入式Linux操作系统自带的V4L2图像采集驱动程序通过集USB摄像头采集图像，由于采用的摄像头内部集成了JPEG压缩器，因此需要调用JPEG解压缩程序对采集到的图像进行解压缩；移植开源视频编解码库xvidcore，调用该库的API函数对解压缩后的图像数据进行编码，由于采用的实时传输库jrtplib开源软件库，每次只能发送大小不大于1400字节的数据，因此需要对编码后的数据分割，并添加标志位，再调用jrtplib的发送API顺序发送分割后的数据；

③ 通过socket API循环接收监控中心PC(2)发送的控制信号，对该控制信号进行解析得到对机器人主控制板(3)的控制信息、对传感器信息采集板的控制信息以及对运动控制板的控制信息；

④ 从控制信息中解析得到对从机械臂的控制命令，经过角度合法性检查后，通过CAN总线将主机械臂的角度信息传输到FPGA运动控制板，控制机械臂来完成相关动作；

⑤ 从控制信息中解析得到对传感器信息采集板的控制命令，完成传感器信息采集板的相关控制，即完成采集板使能、各个传感器模块开启或者关闭；

⑥ 编写CAN总线通信程序，完成对CAN节点的配置，重新创建一个线程，在该线程内通过CAN总线完成数据的发送和接收，将接收的数据进行融合并通过socket发送到监控中心PC；

在机器人主控制板(3)程序中创建了两个线程，一个线程循环完成视频数据的采集、编码及发送，另一个线程循环完成对控制信息的接收、解析以及通过CAN总线将控制信息传输到运动控制板和传感器信息采集板；在主控制板中移植了嵌入式Linux操作系统，并在该系统中移植了开源视频编解码库及实时传输库，实现视频的编码和实时发送；由于从机械臂的各个关节摆动幅度有限，而主机械臂可实现360度转动，为了保护从机械臂，需要再次对解析得到的从机械臂的控制信息进行检查，只将正确的角度信息传输到运动控制板；现场主控制板CAN节点ID分配为0x01。

5.根据权利要求1所述的基于主从遥操作机械臂的机器人控制方法，其特征于，所述步骤4）现场环境传感器信息采集板(5)的数据采集及通信的具体步骤为：

    ① 硬件平台的搭建：现场环境信息采集板(5)采用基于Cortex-M3芯片的STM32开发板，通过串口连接GPS模块，通过GPIO口连接红外传感器、温度传感器和速度采集模块；

    ② 编写串口通信程序，通过串口接收GPS模块发送过来的字符串，经过字符串处理后得到小车所在位置的经纬度数据；

    ③ 通过GPIO模拟与红外传感器通信的时序，采集安装在车身四周的红外传感器数据，得到车身周围的障碍物距离；通过GPIO模拟与温度传感器通信的时序，采集车身所带的温度传感器数据；通过GPIO口与速度传感器模块通信，采集小车的实时速度信息；

    ④ 编写CAN通信程序，配置CAN节点，将采集得到的红外、温度、速度以及GPS数据打包，将打包后的数据通过CAN总线传输到机器人主控制板(3)；CAN协议的帧格式定义如下：

  0 byte 1 byte 2 byte 3-4 byte  5-7 byte   帧开头及ID 红外 温度 速度 GPS   留用 CRC校验及帧结束

传感器信息采集板上电后，通过CAN总线完成与机器人主控制板(3)的握手，等待机器人主控制板(3)发送采集板使能信号；接收到使能信号后，开始各种传感器信息的采集和发送，每个传感器都可以在机器人主控制板(3)的控制下实现开启或关闭；传感器信息采集板CAN节点ID分配为0x03。

6.根据权利要求1所述的基于主从遥操作机械臂的机器人控制方法，其特征于，所述步骤5）监控中心PC(2)数据显示及通信的具体步骤为：

    ① 编写串口通信程序，通过串口与主机械臂信息采集板通信，向主机械臂信息采集板发送使能信息，并接收信息采集板发送过来的主机械臂的角度信息；

    ② 编写视频数据接收程序，通过实时传输库jrtplib完成对视频数据的实时接收，并根据数据包中的标志位，将接收到的分割后的数据打包成一帧数据，并调用开源视频编解码库xvidcore完成对数据的解码；

    ③ 通过socket API接收现场主控制板发送过来的各种传感器数据，并对该数据进行解析得到现场的温度、车身周围环境及GPS信息；

    ④ 通过界面接收监控中心人员输入的各种控制信息，将输入的控制信息与主机械臂的角度信息融合后通过socket API发送到作业现场机器人；

    ⑤ 利用MFC库编写显示程序，实现作业现场视频和各种现场环境信息的显示，并在界面中添加多个控制按钮，包括主机械臂信息采集使能按钮、从机械臂的控制使能按钮、现场信息采集板使能按钮、摄像头云台的控制按钮、视频显示以及开始或停止录像按钮；

       监控中心PC机程序中开启了三个线程，一个线程完成视频数据的接收、解码和显示，一个线程完成主机械臂的关节角度信息的接收、界面控制命令的接收、数据的融合以及通过socket API的发送，另外一个线程完成通过socket API接收机器人主控制板发送的传感器信息数据以及数据的分析和显示；监控中心PC(2)程序基于VC++ 6.0平台编写，并移植了开源视频编解码库xvidcore和实时传输库jrtplib。