CN109910010A - 一种高效控制机器人的系统及方法 - Google Patents

Abstract

本发明属于程序控制技术领域，公开了一种高效控制机器人的系统及方法，所述高效控制机器人的系统具体包括：双目立体视觉模块用于利用摄像机获取图像，并计算物点的三维坐标；路径规划模块用于为机器人在特定的环境中寻找一条免避撞的到达目标物位置的最优路径；遥操作模块用于利用Teleoperation和Remote Control操作方式提高机器人的作业效率和可操作性；上位机控制模块用于利用MFC编程把各模块都封装在程序中；通讯模块用于完成上位机与后方下位机的双向信息交流。本发明提供的高效控制机器人的系统有效提高了系统的健壮性和容错处理，能够提高机器人的易操作性、作业效率以及作业的精细程度。

Description

一种高效控制机器人的系统及方法

技术领域

本发明属于程序控制技术领域，尤其涉及一种高效控制机器人的系统及方法。

背景技术

目前，业内常用的现有技术是这样的：

近年来，随着机器人技术的迅猛发展，机器人在各种领域获得了广泛的应用。以排爆，消防等机器人装备为代表的 各种极限环境作业机器人也逐渐进入应用领域，用来代替人类在很多危险环境中工作，如放射性污染区域，存在爆炸物的区 域，各种污染的环境，煤矿和宇宙空间等极限环境。

在国外，排爆机器人的研究起步较早且发展迅速，技术日益成熟，并已进入实用阶段，英、美、德、法、加拿大等 西方国家已广泛在军警部门装备使用。

由英国军用车辆研究所和皇家陆军军械部队研制、英国Morfax公司生产的“手推车”(Wheelbarrow)举世闻名，已向 50多个国家的军警部门销售了500多台，目前发展有多种型号：MK7、MK8、SuperM(超级手推车)等，其中，SuperM排爆 机器人是一种可在恶劣环境下工作的遥控车(图1-1)，该车重204kg，长1.2m，宽0.69m，完全展开时最大高度1.32m，摄像机可在距地面65mm处工作，因此它可用来检查可疑车辆底部；它采用橡胶履带，最大速度为2km/h，有一整套的无线电控 制系统及彩色电视摄像机、一支猎枪和两个爆炸物排除装置。

英国P.W.Allen公司生产的Defender是一款大型排爆机器人(图1-2)，它的一些先进的功能满足正在发展的反恐需 求，例如，处理核生化装置，分布式电子结构，扩展的光谱射频遥感测量装置，可通过线缆操控，也可通过无线SSRF遥控， 采用全向天线，控制半径达到2km，车体采用模块化结构，主要部件使用强度高、质量轻的钛，大范围的配置并采用标准配 件，结实耐用、维修简单、通用性好、可靠性高。

“搜索者”(Hunter)也是英国研制的轮履结合的排爆机器人。它有一个独特的辅助驱动系统，能迅速选用履带越过障 碍物，也可用轮子在平整的道路上以较高的速度行驶。其伸缩的臂活动半径达4m，最大仰角87°，能举起100kg的重物， 其手爪可旋转360°，手爪夹持力达54kg，数字式脉码调制无线电通信由微处理器控制，当出现无线电干扰时，将通信系统 换接到预先编好程序的自动防止故障的状态，直到干扰停止、信号质量恢复为止。它可安装一个或两个臂，臂上装有半自动 猎枪，可与激光目标指示器配合，在45m远处命中直径为2.5cm的小目标。

加拿大Pedsco公司生产的RMI-9WT排爆机器人是其生产的系列化排爆机器人中最大的一种型号(图1-3)，广泛应 用于搜查、排爆、监控及放射性物质的排除等危险环境。其主要特点有：6轮驱动配履带，攀爬能力强，移动灵活；4个彩 色摄像机，图像最大可放大128倍，另加配高灵敏度低照度红外摄像机；3种可选抓取器：标准型、可旋转型、超大型；双 水炮带闪烁激光瞄准器可连续打击目标，并且水炮枪控制器带自动延时功能，能有效保证操作人员安全；通过手控或智能遥 控现场拍摄可疑物图像，并可选配各种延伸杆[1]；它还配有一管装有激光瞄准器的5连发霰弹枪，曾在纽约有过击毙4名 歹徒的成功范例。另外，该公司生产的RMI-10则是一款中型排爆机器人，为4轮驱动配履带。

美国Wolstenholme机器公司生产的MR5和MR7排爆机器人(见图1-4)是一个用于户内及户外环境、适应各种地 形活动的排爆机器人。该机器人备有6个车轮、一套活动履带，必要时可将履带装上，用于跨越崎岖不平的地形及楼梯等各 种障碍。车上还装备有4部摄像枪及一个麦克风提供给操作员收取现场映象及声音。

国外排爆机器人产品，经过国外几十年的研究，已经得到了大量的应用，功能完善、技术先进是其主要特点，且国 外基于多年的基础性研究以及成熟的技术，着手于系列化、规模化的排爆机器人的生产。其系列化产品配件不仅齐全，而且 具有良好的通用性、互换性，模块化的设计思想使得一种产品经过简单的设计，即可派生出一系列适合不同用途的机器人。

相对国外，我国在排爆机器人的研究起步较晚。目前，国内在该领域进行研究的主要有中科院沈阳自动化研究所、 上海交通大学、北京航空航天大学、华南理工大学等。

中国科学院沈阳自动化研究所先后研制了“灵蜥-A”，“灵蜥-B”(图1-5)，“灵蜥-H”等反恐防暴机器人。其中，“灵蜥－ H”是它与广州卫富机器人公司研制的反恐防爆机

器人，自重200kg，最大直线运动速度2.40km/h，可通过小于40°斜坡和楼梯，三段履带设计让机器人平衡地上下楼 梯，可跨越400mm高的障碍；装备有爆炸物销毁器、连发霰弹枪、催泪弹等武器；六自由度机械手最大伸展时抓重为5kg， 最大作业高度达2.2m；还装备了自动收线装置、便捷操纵盒、高效电池等；具有有缆操作(控制距离100m)和无缆操作(控制 距离300m)两种控制方式，可根据需要进行切换。2005年8月通过国家“863”验收的排爆机器人，采用6自由度可伸缩式关 节手臂联动机构，开发有爆炸物转运箱，可提高爆炸物转移速度；车底爆炸物检测机器人采用两节等长履带腿复合型移动机 构，有很强的地形适应能力；其控制系统采用PC104计算机及基于其总线的四轴联动运动控制卡。

上海交通大学是我国最早从事机器人技术研发的高校之一，2002年以来开始排爆机器人的研制。Super-DII型排爆机 器人是863计划项目，由上海交大与北京中泰通公司联合研制，2004年6月在北京参加了第二届国际警用装备博览会。最 近研发的SPUER-III中型排爆机器人(图1-6)，整机重250kg，长1.6m，宽0.84m，高1.3m，行走速度2.4km/h；可跨越350mm 高的障碍物或沟壕，爬30°～40°斜坡或楼梯，同时可将整体机身抬高350mm；手臂伸展全长1.75m，由5+1自由度的三臂杆 结构组成，全长手臂抓取重量约15kg；大中小臂自由度运动范围0-210°，腰转水平运动±90°，手爪开合距离240mm，腕转 ±360°；另外还配备了国内外最大威力爆炸物销毁器，水弹，穿孔弹等攻击弹种。

北京金吾高科技有限公司研制了JW901B和JW902排爆机器人，具有多功能腕臂、大爪手(可张开50cm)、多路视频 传输系统可切换画面、排爆工作存储系统、挂接摧毁器和X光机等部件。

2005年，北京航空航天大学机器人研究所与北京瑞琦伟业科技开发有限公司合作开发了RT3-EDO小型模块化排爆 机器人和RAPTOR中型排爆机器人，在便携性、模块化和动力驱动等关键技术问题上具有鲜明特色。

与国外相比，国内排爆机器人的研究还处于发展阶段，从事排爆机器人研究生产的仅有极少数的厂家，基础性研究 方面的投入不够，缺乏具有自主知识产权的技术，部分产品还处于模仿国外产品的阶段，没有系列化的、较为成功的排爆机 器人产品，且产品单一、功能尚有欠。国内目前还主要依赖国外的排爆机器人用于反恐排爆，进口国外的排爆机器人费用也 相当的昂贵，军用领域的排爆机器人几乎还是一片空白。基于上述国内排爆机器人的研究现状以及军用领域对排爆机器人的 急迫需求，华南理工大学排爆机器人研究室在广东省公安厅、XXX公司的支持下，分别研制了排爆机器人I和排爆机器人 II，它们都是仿美国Wolstenholme机器公司生产的MR5排爆机器人，但在美国MR5的基础上增加了若干实用新功能， 除遥控功能以外，它能够在视觉系统的引导下准确计算可疑目标物的三维坐标，并控制手爪自动抓取可疑目标物，且本课题 组掌握了本排爆机器人全套的生产、加工、组装、调试技术。

目前排爆机器人等极限作业机器人，在结构上一般都由移动机构和一个多自由度操作臂组成，即为“移动机械臂”结 构。移动机构有轮式和履带式两种，它们一般体积不大，转向灵活，便于在狭窄的地方工作。多自由度操作臂为最主要的功 能部件，由操作人员在在几百米甚至几公里以外通过无线或电缆控制其活动，完成作业任务。在进行远程作业时，一般由机 器人本体上安装的多台摄像机传回的图像来对现场进行观察。对于各种类型的极限作业机器人，虽然某些已经投入市场，但 一般存在如下一些问题：

(1)操作困难。特别是多自由度操作臂，机械臂上的每一个关节自由度都对应于遥控面板上的一个按键开关，要控制 机械臂到达目标物的确定位置，必须由操作员手急眼快地操纵多个按键开关来实现机械臂的运动过程。

(2)精度不高。机器人本身不能精确定位，机器人关节的运动速度的快慢和连续性靠操作人员的熟练程度决定，即使 操作程度很熟，要达到连续的满意性也较难。

(3)作业效率低。由于整个过程都是人来控制，抓取一次目标物的作业时间较长。

综上所述，现有技术存在的问题是：

(1)目前极限作业机器人操作困难、智能化程度低、作业精度不高、作业效率低，同时成本高，性价比低；

(2)现有机器人采用的区域匹配算法精度不高，无法进行实时操作，具备延时性；

(3)现有机器人路径规划算法进化速度慢，容易陷入局部最优点，耗能大；

(4)现有机器人视觉图像的处理比较简单，无法精确、实时的进行图像的传输与控制；

(5)现有计算机标定方法传统标定标定过程复杂，基于主动视觉的标定方法不能用于用于摄像机运动未知和无法控 制地场合；

(6)现有经输入系统获取的图像信息中含有各种各样的噪声与畸变，会影响系统图像的清晰程度，降低了图像质量；

发明内容

针对现有技术存在的问题，本发明提供了一种高效控制机器人的系统及方法。

本发明是这样实现的，一种高效控制机器人的系统及方法，所述高效控制机器人的系统具体包括：

双目立体视觉模块、路径规划模块、遥操作模块、上位机控制模块、通讯模块以及下位机模块；

双目立体视觉模块：与上位机控制模块连接，包括图像采集与显示单元、标定单元、图像预处理单元、图像匹配单元以 及物点三维求距单元；用于利用两台性能相似、位置固定的摄像机，来获取同一景物的图像，再根据场景中的物点在两摄像 机中像点的图像坐标，来计算出物点的三维坐标；

路径规划模块：与上位机控制模块连接；用于为机器人在特定的环境中寻找一条免避撞的到达目标物位置的最优路径， 所寻找的最优路径是机器人在到达目标点的过程中拐弯次数少的情况下所经过的路径短；

遥操作模块：与上位机控制模块连接；用于利用Teleoperation和Remote Control操作方式提高机器人的作业效率和可操 作性；其中Teleoperation是从运动学角度实现的遥操作，是实现排爆机器人机械臂联动的基础；Remote Control是采用基于 USB接口的手柄去操作机器人；

上位机控制模块：与双目立体视觉模块、路径规划模块、遥操作模块、通讯模块连接；用于利用MFC编程把双目立体 视觉模块、路径规划模块、遥操作模块、通讯模块、下位机模块都封装在程序中；

通讯模块：与上位机控制模块、下位机模块连接；包括数据通讯单元、图像通讯单元以及智能化处理单元；用于完成上 位机与后方下位机的双向信息交流；

下位机模块：与通讯模块连接，包括受控制的机器人。

进一步，所述利用MFC编程将双目立体视觉模块、路径规划模块、遥操作模块、通讯模块、下位机模块封装在程序中 具体包括：

(1)将双目立体视觉模块封装在类CVision中，主要的功能函数如下：

CVision::OnActive()完成图像的显示，为系统显示实时采集来的图像；

CVision::OnSearch()完成目标物的搜索，为系统选择左视图中的目标点；

CVision::OnMatch()完成图像的匹配过程，为系统寻找右视图中对应的目标点；

利用动态链接链库CoordinateCalculate来封装三维求距功能，在求取时只需要调用内部的接口函数LvDistance(m_dfR, m_dfT,m_dfKL,m_dfKR,m_dfML,m_dfMR,m_dfMatchPt,res,error)计算目标点的三维坐标即可；

(2)将机器人的手动控制界面封装在CControlTestDlg中；实现机器人各个关节、车体的控制；包括：手爪控制、关 节控制、手臂方向控制以及车体控制，其中关节控制和手臂控制封装在类CManipulator中，CControlTestDlg只需调用其接 口函数；手爪控制和车体控制直接封装在CControlTestDlg中；

(3)将遥操作模块封装在类CTeleoperation中；

(4)将通讯模块封装在类CSerialPort中；

(5)将路径规模模块采用遗传算法路径规划封装在GA项目中。

进一步，所述双目视觉模块具体包括：

双目视觉模块快速准确的计算爆炸物抓取点相对机器人手爪的位置坐标；包括图像的采集与显示单元、摄像机的标定单 元、图像的预处理单元、图像的匹配单元以及物点的三维求距单元；

图像采集与显示单元：用于利用两台性能相同、位置固定的摄像机从不同角度对同一景物进行拍摄获取图像；

标定单元：用于建立有效的成像模型，并确定摄像机内外部属性参数，以便确定空间坐标系中物点与它在图像平面上像 点之间的对应关系；

图像预处理单元：用于消除由摄像头畸变、光照条件、以及传输过程对图像引入的噪声；

图像匹配单元：用于在两幅图像的匹配点或者匹配基元之间建立对应关系；

物点三维求距单元：用于从二维灰度图像中获得目标点的三维深度信息。

进一步，所述图像采集与显示单元具体包括：

采用Euresys公司的PicoloTetra视频采集卡，利用视频采集卡自带的驱动程序MultiCam，在完成对采集卡的初化和参 数设置后，调用采集卡驱动函数库中的图像采集函数来采集图像，并将模拟图像转换为计算机能识别的数字图像，在对应的 程序窗口中进行显示；

进一步，所述标定单元具体包括：

标定单元用于确定摄像机的几何和光学参数，摄像机相对于世界坐标系的方位；用上标L和R来分别表示左摄像机和 右摄像机的各种变量和参数；在场景中建立世界坐标系，物点在左右两摄像机中分别成像，形成图像坐标(u,v)^L和 (u,v)^R；Projection^L()和Projection^R()分别代表两摄像机的成像过程；{A^L,k₁ ^L,k₂ ^L}为左摄像机内参数， {R^L,T^L}为左摄像机外参数；{A^R,k₁ ^R,k₂ ^R}为右摄像机内参数，{R^R,T^R}为右摄像机外参数；其中内参数由摄像机的物理结构决定，固定不变；而外参数随着世界坐标系选取的不同而不同；

标定的具体步骤包括：

(1)制作一张黑白方格相间的模版，打印并贴在一个平面上作为标定过程中所使用的平面标定靶标；

(2)将模板多个角度放置在摄像机前拍摄图像，并根据模板的空间位置建立世界坐标系O_wX_wY_wZ_w，使得模板位于世 界坐标系的X_wY_w平面上，即模版上任一点的在世界坐标系中Z_w=0；

(3)取黑白棋格的交点作为控制点，并称之为“角点”；

(4)利用Matlab编写的张氏标定法对单个摄像头进行标定，分别求成两个摄像头的内参数；

(5)利用Matlab编写的张氏标定法对双目视觉进行立体标定，求出两摄像头的外参数。

进一步，所述张氏标定法：

假设标定用平面图板在世界坐标系中Z＝0，通过线性模型分析计算得出摄像机参数的优化解，然后用基于最大似然法进 行非线性求精；在这个过程中，标定考虑到镜头畸变的目标函数，最后求出所需的摄像机的内、外部参数；

其基本原理如下式：

在这里假定模板平面在世界坐标系Z_w＝0的平面上；

式中，A为摄像机的内参数矩阵，令 为模板平面上点的齐次坐标， 为模板平面上点投影到图像平面上对应点的齐次坐标，[r₁ r₂t]和t分别是摄像机坐标系相对于世界坐标系的旋转 矩阵和平移向量；建立控制点的空间坐标与图像坐标之间的单映性关系式：

其中，H＝A[r₁ r₂ t]，描述了控制点的空间坐标与图像坐标之间的映射关系

每一个控制点的H都不同。但对于同一幅模板图像上的控制点，因A[r₁ r₂ t]固定，故其上的控制点的仅相差 一尺度因子；对于一幅模板图像，虽然不能完全确定其对应的A[r₁ r₂ t]，但却可以在某种程度上获得关于 A[r₁ r₂ t]的约束；

H提供了关于A[r₁ r₂ t]的约束，也即是提供了关于A的约束；不同的模板，其H不同，但是A是确定的； 可以通过多个不同的模板的H，来最终确定A，即有：

根据旋转矩阵的性质，即r₁ ^Tr₂＝0和||r₁||＝||r₂||＝1，每幅图可以获得以下两个对内参数矩阵的基本约束：

摄像机有五个位置参数，故当所摄取的图像数目大于3时，就可以线性唯一求解A；再利用A来估计各模板的外参数；

每幅模板的H都能提供了关于A的约束，具体的约束由旋转矩阵R＝[r₁ r₂ r₃]的正交性推导得到。旋转矩阵R 的正交性为：

r₁ ^Tr₂＝0

r₁ ^Tr₁＝r₂ ^Tr₂

因而可以推导得到：

[h₁ h₄ h₇]A^-TA^-1[h₂ h₅ h₈]^T＝0

[h₁ h₄ h₇]A^-TA^-1[h₁ h₄ h₇]^T＝[h₂ h₅ h₈]A^-TA^-1[h₂ h₅ h₈]^T

外参数{r₁ r₂ r₃ t}和H的尺度因子λ很容易由H与[R T]的关系求得：

r₁＝λA^-1[h₁ h₄ h₇]^T

r₂＝λA^-1[h₂ h₅ h₈]^T

r₃＝r₁×r₂

t＝λA^-1[h₃ h₆ h₉]^T

进一步，所述图像预处理单元具体包括：

图像预处理单元基于中值滤波进行去噪处理；

中值滤波基本原理是把序列(Sequence)或数字图像(digital image)中间一点的值，用该点邻域中各点值的中值来替代；

对序列而言中值的定义是这样的，若x₁,x₂,…,x_n为一组序列，先把其按大小排列为：

x_i1≤x_i2≤…≤x_in。

则该序列的中值y为：

对于一幅图像的象素矩阵，中值滤波就是用一个活动窗口沿图像移动，窗口中心位置的像素灰度用窗口内所有象素灰度 的中值来代替，取以目标像素为中心的一个子矩阵窗口，窗口可根据需要选取。

进一步，所述图像匹配单元具体包括：

图像匹配单元用于在左右摄像头拍摄的两幅存在视点差异、几何和灰度畸变以及噪声干扰的图像对之间建立特征之间的 对应关系，将同一空间物理点在不同图像中的映像点对应起来；

基于图像的外极线原理以及图像的动态搜索原理，采用基于外极线的动态立体匹配算法对图像沿着外极线先进行粗匹配， 找出感兴趣的区域，再对感兴趣的进行精匹配；

具体步骤如下：

(1)计算目标点对应的外极线；

(2)沿着外极线划分图像，进行粗匹配；

(3)利用算子进行精匹配。

进一步，步骤(1)中，所述外极线计算具体包括：

1)左右图像匹配对特征点的提取：采用左右摄像头在不同的角度拍摄黑白棋盘模板，再提取相对应的特征点以及对应 特征点的图像坐标；利用VC++和OpenCV编程利用亚像素角点提取方法提取20组图像匹配对的特征点图像坐标，再求取 对应的基本矩阵；

2)基本矩阵的求取，使用最小中值法来估计基本矩阵：

在48个匹配对中随机抽取m个样本，每个样本J包含K个匹配对，设K＝8，选取8点法解出一个基本矩阵F_J，设 整个匹配点对数据集中一对匹配点对是(m₁,m₂)，对每个F_J从整个匹配点对集合得到一个中值M_J；

其中表示点m₂到点m₁的极线的距离，d(m_1i,F_Jm_2i)表示点m₁到m₂的极 线的距离，med表示对所有的d²(m_2i,F_Jm_1i)+d²(m_1i,F_Jm_2i)按从大到小的顺序排列取中间的值；

从m个样本的所有M_J找出最小的一个，计为M_m，即最小中值，其对应的基本矩阵为F_M，再应用这个最小中值来 估计基本矩阵；

采用最小中值法求取最小矩阵的基本步骤如下：

a.获得图像中特征点的匹配对集合；

b.随机抽取m个样本，每个样本J包含K个匹配对，K＝8；

c.用8点算法求取基本矩阵的估算值；

d.重复步骤B共m次，并记录所有的中值和相应的基本矩阵估计值，找出最小中值和相应的基本矩阵的估计值；

其中随机抽取次数m的确定是假设整个数据集合中含有的正确匹配对的百分比为p，则m次采样中至少有一个样本 是完全由正确的匹配对组成的概率是P＝1-(1-p^K)^m；

3)外极线的求取：

根据步骤2)求取的多个基本矩阵，求其平均值；再根据外极线理论，则可以求取对应点的外极线；令图像I_L中的一点 为p₁，其对应的在图像I_R中的外极线l₂根据式(3-23)有：

l₂＝F*p₁

同理，令图像I₂中的一点p₂,其在图像I_L中的外极线l₁根据F^Tp₁＝l₀有：

l₁＝F^T*p₂

再根据式p₁ ^TFp₀＝0，p₀ ^TFp₁＝0结合l₂＝F*p₁和l₁＝F^T*p₂有：

再将相关数据代入则可以求出对应点的外极线方程a*u+b*v+c＝0。

进一步，步骤(2)中，所述粗匹配具体包括：

先沿着外极线对图像进行分割，缩小图像区域匹配范围，将其分成几个子图像以便进行粗匹配；

具体过程如下：

1)设基准图为T，搜索图为S，搜索子图为s^i，j；

2)把搜索图S平均划分为若干个子图

3)设基准图各象素灰度和为(t为基准图T中任意点的灰度值)，匹配图s^i，j的各象素灰度值和 为:T与s^i，j相关度为：阈值设为α(0<α<1)，计数变量设为N；

4)计算各块的η^i,j大小，设一阈值α(设为：0.25)如果η^i,j<α，N加1，并记下位置；如果N＝1，则直接可以进 行精匹配；如果N>1，则要直接找出多次最小值的位置；如果N＝0，表示干扰因素比较大，则要调整α值再进行匹配，α的 值是越小越好，如果选取的值过小，由于误差的存在可能找不到匹配点，此时在允许范围内可以适当放大α的值。

进一步，步骤(3)中，所述精匹配具体包括：

针对粗匹配找到的感兴趣的区域，进行精匹配；如果此时获得的区域没包括外极线，则应该扩大区域，增加实验位置； 再按常规的灰度匹配逐个像素进行匹配，在这里使用CV_TM_CCOEFF_NORMED归一化相关系数匹配算子进行相似度判断；

其相关度为:

式中I表示图像，T-模板，R-结果；模板与图像重叠区域x'＝0...w-1,y'＝0...h-1之间求和；模板滑动过整个 图像image，用l₂＝F*p₁比较模板templ与图像尺寸为w×h的重叠区域。

进一步，所述物点三维求距单元具体包括：

根据场景中的物点在两摄像机中像点的图像坐标，计算出物点的三维坐标，基本原理为根据O_l、p_l、O_r、p_r四点的 空间位置求直线O_lp_l、O_rp_r的交点P的空间位置，因为摄像机模型和投影点的近似误差，两台摄像机的投影线 O_lp_l、O_rp_r并没有在数学3D空间相交于一点，则通过计算这两条空间斜交投影线之间的最短距离，也就是计算它们公垂 线段的长度；如果公垂线比较短，则取公垂线的中点作为两条投影线的交点，定为(X,Y,Z)；如果公垂线太长，那么就 断定在进行像点对应计算时出现了问题。

进一步，所述路径规划模块具体包括：

路径规划模块采用基于改进GA的移动机器人规划；

具体包括：判断路径、构造路径、最短路径；

(1)判断路径：采用了闵科夫斯基和原理扩展障碍物，判断运动物体(即机器人)能否从起点s移动到终点g，即从s到 g是否存在一条自由路径；

(2)构造路径：基于改进的可视图方法，在地图中寻找机器人的可行区域；

(3)最短路径：基于NGA寻找机器人从s移动到g的一条最优自由路径。

进一步，所述闵科夫斯基和原理具体包括：

给定平面上两个点集S₁,S₂，在平面建立一个x,y坐标系，其中的点可以看成该坐标中的矢量，定义： S₁+S₂＝{x+y|x∈S₁,y∈S₂}；

其中：x+y为点x和点y的矢量和，这即为S₁和S₂的闵科夫斯基和；也可扩展点x与集合S₂的闵科夫斯基和为 x+S₂＝{x+y|y∈S₂}，即对于每一个是S₂的复制的并；

将障碍物p看作为S₁，机器人R看成是S₂，机器人在运动过程中以其几何中心看成参考点，设参考点在原点，综合 考虑机器人的转弯半径和本身的尺寸，将机器人看成圆盘，参考点即为圆心，对于所有x∈p，可以把P+R看成是依据 x转换的R的复制；由于R的中心在原点，x+p的中心将在x，因此P+R相当于放置中心在P的每个点之上的R的 复制，故P+R是P的扩展域p′；扩展之后的障碍物若相交，则此区域不可行，那么在下述构造路径时这块区域就可当 做障碍物处理；对于其他形状的机器人同样可以采用此原理扩展。

进一步，所述基于改进的可视图构造路径具体包括：

(1)生成常规的可视图；

(2)对生成的连线按从短到长的顺序进行排序，生成一个由线段组成的队列；

(3)取第一条线段m，检查是否和其后的线段相交；如果发现队列中某一条线段n和线段m相交，那么从队列中删除n 线段，以此类推，直到将所有队列中与线段m相交的线段删除；

(4)再取队列中m的下一条线段，重复步骤(3)，直到取完所有的线段。

进一步，所述基于NGA的最短路径具体包括：

(1)路径的个体编码：使用多段线表示路径，除了第一点和最后一点分别落在出发点和目标点上之外，其它路径线段 与线段之间的连接点都必须落在地图连接线中点上，故路径则可表示为((Xs,Ys),(X17,Y17),(X12,Y12),(X5,Y5),(X3,Y3), (Xg,Xg))，其中每个点的角标根据生成连接线按长度排序之后的位置，(Xi,Yi)代表是中间点的坐标，(Xs,Ys)和(Xg,Xg)分别 表示起点和终点；

(2)初始种群的产生：在地图的起点和终点确定，以及连接点生成之后，随机的选取这些连接点组成路径；

(3)适应度函数：主要考虑路径长度与能量的最优：

f_fitness＝dist+g(α,n,...,μ)

其中：dist-对路径长度的评价式中：x_k-第k个连接点的x坐标； g(α,n,...,μ)是能量函数，其能量与拐弯半径、连接点数量以及地面的摩擦系数等因素有关；

(4)NGA操作算子：

1)选择算子采用相似度算子：随机生成种群后，在遗传算法进行选择运算前，对群体中每两个个体逐位比较；如果两 个个体在相对应的位置上存在着相同的字符(基因)，则将相同字符数量定义为相似度R；

设T＝适应度平均值，在群体中取大于T的个体进行个体相似程度判断；R小则表示这两个个体相似性差，当R≥L/2(L为个 体的长度)时，即认为这两个个体相似；

2)交叉算子：共有节点处一点交叉，潜在节点处一点交叉，以及连通节点对处一点交叉共计三中交叉算子；

3)变异算子

采用动态确定变异概率，首先对生成的路径根据其适应度值由小到大排序，对于适应度值大(大于适应度值的均值，其 中适应度越小越好)的路径取大变异率，对于排在前面的适应度值较小的路径取小变异率；

4)终止准则：利用多代之间的平均适应度之差小于某值Δ来终止；

(5)NGA路径规划迭代步骤：

1)首先随机生成m条从起点到终点的路径；所有生成的路径都落在连接线的中点上，再利用相似度判断这些路径之间的 相似度，如果相同路径过多，则需要再随机生成一些路径，扩大初始种群，避免陷入局部最优解；

2)随机将路径进行两两交叉，并随机取路径连接点进行移动，生成新路径；

3)对生成的路径根据其适应度值，由小到大排序，对于适应度值大(大于适应度值的均值，其中适应度越小越好)的路径 取大变异率，对于排在前面的适应度值较小的路径取小变异率；

4)经过几次迭代最终找到评估值最小的可行路径；

5)当经过多次迭代之后，如果连续几次出现的平均适应度值Δ<＝0.5时，则停止迭代。

进一步，所述遥操作模块具体包括：

遥操作模块利用Teleoperation实现机器人联动的遥操作，利用Remote Control实现手柄控制的遥操作；

Teleoperation：包括位于上位机的遥操作子系统软件，和位于机器人本体上的机械臂位置伺服控制系统；用于根据用户 发出的对机械臂进行控制的命令(一般由机械臂的位姿来描述)，通过解析的逆运动学方程，实时解算出各关节角度，并通 过无线通讯系统传递给位于机器臂上的现场位置伺服控制系统，完成各关节的位置伺服控制，并能实时获取机械臂当前的运 动学状态，将其反馈给用户；运动学状态可以是各关节的实际角度，或者利用解析的正运动学方程转变为机械臂的位姿描述；

进一步，所述智能化处理单元具体包括：

(1)用多线程串口通信CSerialPort进行控制指令的发送与接收都利；CSerialPort封装了串口的初始化和数据的接收， 对串口进行数据的读写操作采用的是多线程和消息通知模式；

(2)数据接收时，首先应该在CTestView类中用该类实例化一个全局对象，然后在OnNewDocument()函数中初始化串 口，如果初始化成功则开启多线程监控并读取串口程序。

进一步，所述机器人异常数据具体包括：

x1和x0表示电池电量状态；00表示电量饱满；01表示电量正常；10表示电量低压；11表示电量欠压；x2＝1表示机器 人在自动抓取时目标物在不可抓取范围；x3＝1表示驱动器报警；x4＝1表示接收到错误指令。

进一步，所述异常处理具体包括：

(1)当机器人检测到上述异常时，在操作界面上都会提示操作者，让其对异常做出相应的处理；在操作主界面上添加 了电量显示图标，让操作者能够很直观的观察机器人电量的变化情况；

(2)驱动器异常时，驱动器会进行报警，可通过驱动器开关及时对驱动器进行断电操作；

(3)当接收方出现误码，监测程序可实时检测指令的正确性，对错误指令进行及时的处理；

每当上位机发一个控制指令，下位机会检测指令是否正确，如果正确的话即x4＝0，上位机则返回OnSendCommand ((char)85,(float)data2,(float)data3,(float)data4,(float)data5,(char)data6,(char)data7,(char)data8,(char)data9,(char) data10,(double)data11)

如果x4＝1，则重新发送原来的数据OnSendCommand((char)data1,(float)data2,(float)data3,(float)data4,(float) data5,(char)data6,(char)data7,(char)data8,(char)data9,(char)data10,(double)data11)；

(4)对于下位机出现死机异常，则是通过硬件重启；当通讯出现异常时，机器人三秒钟收不到任何指令，则通过硬件自 动关机。

进一步，所述高效控制机器人的控制方法具体包括：

步骤一，设置串口参数，初始化串口，

步骤二，利用摄像头获取图像，导入摄像头抓取的目标物，并对图像进行匹配，根据匹配结果求取目标物的三维坐标；

步骤三，基于NGA算法为机器人寻找到目标物三维坐标的最优路径；

步骤四，利用Teleoperation与Remote Control控制机器人运动。

综上所述，本发明的优点及积极效果为：

本发明提供的高效控制机器人的系统有效提高了系统的健壮性和容错处理，能够提高机器人的易操作性、作业效率以及 作业的精细程度的同时，在上下位机通讯出现异常，底层控制计算机出现异常，死机等情况下可以实现系统的自动重启和恢 复，避免机器人失控情况的出现。

本发明提供的高效控制机器人的系统及方法只需要给出高层次的作业需求，机器人就能自主的完成某些任务。本发明提 供的高效控制机器人的系统及方法能够有效控制排爆机器人进行排爆工作，减少了动用警力手工拆除和处理爆炸物品的情况， 尽可能保证了人员的安全。

本发明利用MFC编程把上述的子模块都封装在程序中，设计便于操作的界面，使系统易于修改和扩充；本发明的双目 立体视觉模块能够使机器人撞阕的计算目标物的空间三维坐标；本发明基于外极线的动态图像匹配算法，以高精度、实时性 匹配为目标，能在保证匹配精度的前提下，大大提高实时性；本发明基于改进遗传算法的路径规划算法能够有效改善现有算 法进化速度慢，容易陷入局部最优点的缺陷，同时还能够降低能耗。

本发明提供的高效控制机器人的系统及方法能够实现控制机器人行走、转弯、抓放、搬运可疑爆炸物的功能，能够实现 控制机器人将激光瞄准器对准可疑物的功能；其次本发明能够实现机器人自动控制手臂靠近并抓取可疑物，完成遥控操作， 一次到位的任务。

本发明采用的Picolo Tetra是一款高性价比的PCI图像采集卡，适用于并行实时采集多台摄像机的图像。且Picolo Tetra 具有良好的视频流和视频切换的管理能力。由于卡上带有四个独立的视频A/D转换器,使Tetra可以以每路每秒25帧的速度 同时从4台摄像机采集768x576(PAL)的图像。Tetra最多可以连接3个视频扩展模块，使视频输入最大达到16路，且Picolo Tetra卡上集成了一个硬件看门狗，用于监视应用程序的运行状态。如当应用程序出现死机时，看门狗立即对PC硬复位，重 新启动系统，确保无人职守系统的可靠运行。Picolo Tetra采用了64bits，66MHz的PCI总线。该总线的最高数据传输带宽 可达528Mbytes/秒。

本发明利用张氏标定法避免了传统方法设备要求高、操作繁琐等缺点，且又较自标定方法精度高。本发明采用中值滤波 进行去噪平滑效果好，能去除图像中的噪声点并保持图像边界以及使图像中的排爆物(矿泉水瓶)的轮廓变的清晰，从而便于 此物体的识别。

本发明基于NGA寻找最短路径时采用多线段编码，大大简化了编码长度；结合排爆机器人的实际情况提出合适的适应 度函数；对选择算子和变异算子进行改进，扩大种群的范围，避免快速进入局部最优解。

本发明采用Teleoperation实现机器人联动的遥操作解决了五自由度机器手逆运动学求解困难的前提下，实现了复合人类 抓取习惯的“联动遥控操作”模式，即：腰、肩、肘、腕关节联合运动。在欧式三维(X，Y，Z)空间内，用操纵杆实现手爪 的上、下、左、右、拉、伸操作，彻底摆脱以前各排爆机器人难于抓取爆炸物目标的实际困难，大大地提高了排爆机器人的 可操作性。

附图说明

图1是本发明实施例提供的高效控制机器人结构示意图。

图2是本发明实施例提供的高效控制机器人结构设计示意图。

图3是本发明实施例提供的基于双目立体视觉的目标物自动抓取示意图。

图4是本发明实施例提供的双目立体视觉系统示意图。

图5是本发明实施例提供的双目立体视觉系统结构示意图。

图6是本发明实施例提供的图像采集工作流程图。

图7是本发明实施例提供的双摄像机成像模型示意图。

图8是本发明实施例提供的标定流程图。

图9是本发明实施例提供的黑白方格模板及世界坐标系示意图。

图10是本发明实施例提供的标定模板图。

图11是本发明实施例提供的角点提取示意图。

图12是本发明实施例提供的立体标定示意图。

图13是本发明实施例提供的单映性映射示意图。

图14是本发明实施例提供的外极线的动态立体匹配流程图。

图15是本发明实施例提供的外极线求取流程图。

图16是本发明实施例提供的对应左右图像特征点提取示意图。

图17是本发明实施例提供的搜索图和基准图。

图18是本发明实施例提供的搜索图S的划分示意图。

图19是本发明实施例提供的三维求距的数学模型。

图20是本发明实施例提供的三维求距流程图。

图21是本发明实施例提供的障碍物的扩展示意图。

图22是本发明实施例提供的利用可视图法创建的地图。

图23是本发明实施例提供的地图创建过程流程图。

图24是本发明实施例提供的改进的地图。

图25是本发明实施例提供的选择算子流程图。

图26是本发明实施例提供的NGA路径规划流程图。

图27是本发明实施例提供的Teleoperation模块示意图。

图28是本发明实施例提供的手柄-下位机控制模式示意图。

图29是本发明实施例提供的高效控制机器人的控制方法流程图。

图30是本发明实施例提供的双目视觉系统硬件关系示意图。

图31是本发明实施例提供的双目视觉系统框图。

图32是本发明实施例提供的采集卡通道初始化流程图。

图33是本发明实施例提供的手动匹配模式流程图。

图34是本发明实施例提供的自动匹配模式流程图。

图中：1、双目立体视觉模块；2、路径规划模块；3、遥操作模块；4、上位机控制模块；5、通讯模块；6下位机模块； 7、图像采集与显示单元；8、标定单元；9、图像预处理单元；10、图像匹配单元；11、物点三维求距单元；12、数据通讯 单元；13、图像通讯单元；14、智能化处理单元。

具体实施方式

为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例，对本发明进行进一步详细说明。应当理解，此 处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

下面结合附图对本发明的应用原理做进一步描述。

如图1-图2所示，本发明实施例提供的高效控制机器人的系统具体包括：

双目立体视觉模块1、路径规划模块2、遥操作模块3、上位机控制模块4、通讯模块5以及下位机模块6；

双目立体视觉模块1：与上位机控制模块4连接，包括图像采集与显示单元7、标定单元8、图像预处理单元9、图像匹 配单元10以及物点三维求距单元11；用于利用两台性能相似、位置固定的摄像机，来获取同一景物的图像，再根据场景中 的物点在两摄像机中像点的图像坐标，来计算出物点的三维坐标；

路径规划模块2：与上位机控制模块4连接；用于为机器人在特定的环境中寻找一条免避撞的到达目标物位置的最优路 径，所寻找的最优路径是机器人在到达目标点的过程中拐弯次数少的情况下所经过的路径短；

遥操作模块3：与上位机控制模块4连接；用于利用Teleoperation和RemoteControl操作方式提高机器人的作业效率和 可操作性；其中Teleoperation是从运动学角度实现的遥操作，是实现排爆机器人机械臂联动的基础；Remote Control是采用 基于USB接口的手柄去操作机器人；

上位机控制模块4：与双目立体视觉模块1、路径规划模块2、遥操作模块3、通讯模块5连接；用于利用MFC编程把 双目立体视觉模块、路径规划模块、遥操作模块、通讯模块、下位机模块都封装在程序中；

通讯模块5：与上位机控制模块4、下位机模块6连接；包括数据通讯单元12、图像通讯单元13以及智能化处理单元 14；用于完成上位机与后方下位机的双向信息交流；

下位机模块6：与通讯模块5连接，包括受控制的机器人。

本发明实施例提供的进一步，所述利用MFC编程将双目立体视觉模块1、路径规划模块2、遥操作模块3、通讯模块5、 下位机模块6封装在程序中具体包括：

(1)将双目立体视觉模块封装在类CVision中，主要的功能函数如下：

CVision::OnActive()完成图像的显示，为系统显示实时采集来的图像；

CVision::OnSearch()完成目标物的搜索，为系统选择左视图中的目标点；

CVision::OnMatch()完成图像的匹配过程，为系统寻找右视图中对应的目标点；

利用动态链接链库CoordinateCalculate来封装三维求距功能，在求取时只需要调用内部的接口函数LvDistance(m_dfR, m_dfT,m_dfKL,m_dfKR,m_dfML,m_dfMR,m_dfMatchPt,res,error)计算目标点的三维坐标即可；

(2)将机器人的手动控制界面封装在CControlTestDlg中；实现机器人各个关节、车体的控制；包括：手爪控制、关 节控制、手臂方向控制以及车体控制，其中关节控制和手臂控制封装在类CManipulator中，CControlTestDlg只需调用其接 口函数；手爪控制和车体控制直接封装在CControlTestDlg中；

(3)将遥操作模块3封装在类CTeleoperation中；

(4)将通讯模块5封装在类CSerialPort中；

(5)将路径规模模块2采用遗传算法路径规划封装在GA项目中。

如图3至图5所示，本发明实施例提供的双目视觉模块1具体包括：

双目视觉模块1快速准确的计算爆炸物抓取点相对机器人手爪的位置坐标；包括图像的采集与显示单元7、摄像机的标 定单元8、图像的预处理单元9、图像的匹配单元10以及物点的三维求距单元11；

图像采集与显示单元7：用于利用两台性能相同、位置固定的摄像机从不同角度对同一景物进行拍摄获取图像；

标定单元8：用于建立有效的成像模型，并确定摄像机内外部属性参数，以便确定空间坐标系中物点与它在图像平面上 像点之间的对应关系；

图像预处理单元9：用于消除由摄像头畸变、光照条件、以及传输过程对图像引入的噪声；

图像匹配单元10：用于在两幅图像的匹配点或者匹配基元之间建立对应关系；

物点三维求距单元11：用于从二维灰度图像中获得目标点的三维深度信息。

如图6所示，本发明实施例提供的图像采集与显示单元7具体包括：

采用Euresys公司的PicoloTetra视频采集卡，利用视频采集卡自带的驱动程序MultiCam，在完成对采集卡的初化和参 数设置后，调用采集卡驱动函数库中的图像采集函数来采集图像，并将模拟图像转换为计算机能识别的数字图像，在对应的 程序窗口中进行显示；

所述图像采集函数主要包括：

Channel::Channel(Board*board,int connector)；//初始化通道

void SetParam(param,value)；//参数设置

Template<class T>void RegisterCallback(T*owner,void(T::*callbackMethod)(Channel&,SignalInfo&),MCSIGNAL signal)；//注册回调函数

void UpdateImageConfig(const Surface&s,EImageC24&img)；//读取缓存区的图像

void Channel::SetActive()；//显示图像

void Channel::SetIdle()；//释放资源。

如图7至12所示，本发明实施例提供的标定单元8具体包括：

标定单元8用于确定摄像机的几何和光学参数，摄像机相对于世界坐标系的方位；用上标L和R来分别表示左摄像机 和右摄像机的各种变量和参数；在场景中建立世界坐标系，物点在左右两摄像机中分别成像，形成图像坐标(u,v)^L和 (u,v)^R；Projection^L()和Projection^R()分别代表两摄像机的成像过程；{A^L,k₁ ^L,k₂ ^L}为左摄像机内参数， {R^L,T^L}为左摄像机外参数；{A^R,k₁ ^R,k₂ ^R}为右摄像机内参数，{R^R,T^R}为右摄像机外参数；其中内参数由摄像机的物理结构决定，固定不变；而外参数随着世界坐标系选取的不同而不同；

标定的具体步骤包括：

(1)制作一张黑白方格相间的模版，打印并贴在一个平面上作为标定过程中所使用的平面标定靶标；

(2)将模板多个角度放置在摄像机前拍摄图像，并根据模板的空间位置建立世界坐标系O_wX_wY_wZ_w，使得模板位于世 界坐标系的X_wY_w平面上，即模版上任一点的在世界坐标系中Z_w=0；

(3)取黑白棋格的交点作为控制点，并称之为“角点”；

(4)利用Matlab编写的张氏标定法对单个摄像头进行标定，分别求成两个摄像头的内参数；

(5)利用Matlab编写的张氏标定法对双目视觉进行立体标定，求出两摄像头的外参数。

如图13所示，本发明实施例提供的张氏标定法：

假设标定用平面图板在世界坐标系中Z＝0，通过线性模型分析计算得出摄像机参数的优化解，然后用基于最大似然法进 行非线性求精；在这个过程中，标定考虑到镜头畸变的目标函数，最后求出所需的摄像机的内、外部参数；

其基本原理如下式：

在这里假定模板平面在世界坐标系Z_w＝0的平面上；

式中，A为摄像机的内参数矩阵，令 为模板平面上点的齐次坐标， 为模板平面上点投影到图像平面上对应点的齐次坐标，[r₁ r₂t]和t分别是摄像机坐标系相对于世界坐标系的旋转 矩阵和平移向量；建立控制点的空间坐标与图像坐标之间的单映性关系式：

其中，H＝A[r₁ r₂ t]，描述了控制点的空间坐标与图像坐标之间的映射关系

每一个控制点的H都不同。但对于同一幅模板图像上的控制点，因A[r₁ r₂ t]固定，故其上的控制点的仅相差 一尺度因子；对于一幅模板图像，虽然不能完全确定其对应的A[r₁ r₂ t]，但却可以在某种程度上获得关于 A[r₁ r₂ t]的约束；

H提供了关于A[r₁ r₂ t]的约束，也即是提供了关于A的约束；不同的模板，其H不同，但是A是确定的； 可以通过多个不同的模板的H，来最终确定A，即有：

根据旋转矩阵的性质，即r₁ ^Tr₂＝0和||r₁||＝||r₂||＝1，每幅图可以获得以下两个对内参数矩阵的基本约束：

摄像机有五个位置参数，故当所摄取的图像数目大于3时，就可以线性唯一求解A；再利用A来估计各模板的外参数；

每幅模板的H都能提供了关于A的约束，具体的约束由旋转矩阵R＝[r₁ r₂ r₃]的正交性推导得到。旋转矩阵R 的正交性为：

r₁ ^Tr₂＝0

r₁ ^Tr₁＝r₂ ^Tr₂

因而可以推导得到：

[h₁ h₄ h₇]A^-TA^-1[h₂ h₅ h₈]^T＝0

[h₁ h₄ h₇]A^-TA^-1[h₁ h₄ h₇]^T＝[h₂ h₅ h₈]A^-TA^-1[h₂ h₅ h₈]^T

外参数{r₁ r₂ r₃ t}和H的尺度因子λ很容易由H与[R T]的关系求得：

r₁＝λA^-1[h₁ h₄ h₇]^T

r₂＝λA^-1[h₂ h₅ h₈]^T

r₃＝r₁×r₂

t＝λA^-1[h₃ h₆ h₉]^T

本发明实施例提供的图像预处理单元9具体包括：

图像预处理单元9基于中值滤波进行去噪处理；

中值滤波基本原理是把序列(Sequence)或数字图像(digital image)中间一点的值，用该点邻域中各点值的中值来替代；

对序列而言中值的定义是这样的，若x₁,x₂,…,x_n为一组序列，先把其按大小排列为：

x_i1≤x_i2≤…≤x_in。

则该序列的中值y为：

对于一幅图像的象素矩阵，中值滤波就是用一个活动窗口沿图像移动，窗口中心位置的像素灰度用窗口内所有象素灰度 的中值来代替，取以目标像素为中心的一个子矩阵窗口，窗口可根据需要选取。

如图14-图15所示，本发明实施例提供的图像匹配单元10具体包括：

图像匹配单元10用于在左右摄像头拍摄的两幅存在视点差异、几何和灰度畸变以及噪声干扰的图像对之间建立特征之 间的对应关系，将同一空间物理点在不同图像中的映像点对应起来；

基于图像的外极线原理以及图像的动态搜索原理，采用基于外极线的动态立体匹配算法对图像沿着外极线先进行粗匹配， 找出感兴趣的区域，再对感兴趣的进行精匹配；

具体步骤如下：

(1)计算目标点对应的外极线；

(2)沿着外极线划分图像，进行粗匹配；

(3)利用算子进行精匹配。

如图16所示，步骤(1)中，本发明实施例提供的外极线计算具体包括：

1)左右图像匹配对特征点的提取：采用左右摄像头在不同的角度拍摄黑白棋盘模板，再提取相对应的特征点以及对应 特征点的图像坐标；利用VC++和OpenCV编程利用亚像素角点提取方法提取20组图像匹配对的特征点图像坐标，再求取 对应的基本矩阵；

2)基本矩阵的求取，使用最小中值法来估计基本矩阵：

在48个匹配对中随机抽取m个样本，每个样本J包含K个匹配对，设K＝8，选取8点法解出一个基本矩阵F_J，设 整个匹配点对数据集中一对匹配点对是(m₁,m₂)，对每个F_J从整个匹配点对集合得到一个中值M_J；

其中表示点m₂到点m₁的极线的距离，d(m_1i,F_Jm_2i)表示点m₁到m₂的极 线的距离，med表示对所有的d²(m_2i,F_Jm_1i)+d²(m_1i,F_Jm_2i)按从大到小的顺序排列取中间的值；

从m个样本的所有M_J找出最小的一个，计为M_m，即最小中值，其对应的基本矩阵为F_M，再应用这个最小中值来 估计基本矩阵；

采用最小中值法求取最小矩阵的基本步骤如下：

a.获得图像中特征点的匹配对集合；

b.随机抽取m个样本，每个样本J包含K个匹配对，K＝8；

c.用8点算法求取基本矩阵的估算值；

d.重复步骤B共m次，并记录所有的中值和相应的基本矩阵估计值，找出最小中值和相应的基本矩阵的估计值；

其中随机抽取次数m的确定是假设整个数据集合中含有的正确匹配对的百分比为p，则m次采样中至少有一个样本 是完全由正确的匹配对组成的概率是P＝1-(1-p^K)^m；

3)外极线的求取：

根据步骤2)求取的多个基本矩阵，求其平均值；再根据外极线理论，则可以求取对应点的外极线；令图像I_L中的一点 为p₁，其对应的在图像I_R中的外极线l₂根据下式有：

l₂＝F*p₁

同理，令图像I₂中的一点p₂,其在图像I_L中的外极线l₁根据F^Tp₁＝l₀有：

l₁＝F^T*p₂

再根据式p₁ ^TFp₀＝0，p₀ ^TFp₁＝0结合l₂＝F*p₁和l₁＝F^T*p₂有：

p₁ ^T*F^T*p₂＝0

再将相关数据代入则可以求出对应点的外极线方程a*u+b*v+c＝0。

如图17-图18所示，步骤(2)中，本发明实施例提供的粗匹配具体包括：

先沿着外极线对图像进行分割，缩小图像区域匹配范围，将其分成几个子图像以便进行粗匹配；

具体过程如下：

1)设基准图为T，搜索图为S，搜索子图为S^i，j；

2)把搜索图S平均划分为若干个子图

3)设基准图各象素灰度和为(t为基准图T中任意点的灰度值)，匹配图s^i，j的各象素灰度值和 为:T与s^i，j相关度为：阈值设为α(0<α<1)，计数变量设为N；

4)计算各块的η^i,j大小，设一阈值α(设为：0.25)如果η^i,j<α，N加1，并记下位置；如果N＝1，则直接可以进 行精匹配；如果N>1，则要直接找出多次最小值的位置；如果N＝0，表示干扰因素比较大，则要调整α值再进行匹配，α的 值是越小越好，如果选取的值过小，由于误差的存在可能找不到匹配点，此时在允许范围内可以适当放大α的值。

步骤(3)中，本发明实施例提供的精匹配具体包括：

针对粗匹配找到的感兴趣的区域，进行精匹配；如果此时获得的区域没包括外极线，则应该扩大区域，增加实验位置； 再按常规的灰度匹配逐个像素进行匹配，在这里使用CV_TM_CCOEFF_NORMED归一化相关系数匹配算子进行相似度判断；

其相关度为:

式中I表示图像，T-模板，R-结果；模板与图像重叠区域x'＝0...w-1,y'＝0...h-1之间求和；模板滑动过整个 图像image，用l₂＝F*p₁比较模板templ与图像尺寸为w×h的重叠区域。

如图19-图20所示，本发明实施例提供的物点三维求距单元11具体包括：

根据场景中的物点在两摄像机中像点的图像坐标，计算出物点的三维坐标，基本原理为根据O_l、p_l、O_r、p_r四点的 空间位置求直线O_lp_l、O_rp_r的交点P的空间位置，因为摄像机模型和投影点的近似误差，两台摄像机的投影线 O_lp_l、O_rp_r并没有在数学3D空间相交于一点，则通过计算这两条空间斜交投影线之间的最短距离，也就是计算它们公垂 线段的长度；如果公垂线比较短，则取公垂线的中点作为两条投影线的交点，定为(X,Y,Z)；如果公垂线太长，那么就 断定在进行像点对应计算时出现了问题。

本发明实施例提供的路径规划模块2具体包括：

路径规划模块2采用基于改进GA的移动机器人规划；

具体包括：判断路径、构造路径、最短路径；

(1)判断路径：采用了闵科夫斯基和原理扩展障碍物，判断运动物体(即机器人)能否从起点s移动到终点g，即从s到 g是否存在一条自由路径；

(2)构造路径：基于改进的可视图方法，在地图中寻找机器人的可行区域；

(3)最短路径：基于NGA寻找机器人从s移动到g的一条最优自由路径。

如图21所示，本发明实施例提供的闵科夫斯基和原理具体包括：

给定平面上两个点集S₁,S₂，在平面建立一个x,y坐标系，其中的点可以看成该坐标中的矢量，定义： S₁+S₂＝{x+y|x∈S₁,y∈S₂}；

其中：x+y为点x和点y的矢量和，这即为S₁和S₂的闵科夫斯基和；也可扩展点x与集合S₂的闵科夫斯基和为 x+S₂＝{x+y|y∈S₂}，即对于每一个是S₂的复制的并；

将障碍物p看作为S₁，机器人R看成是S₂，机器人在运动过程中以其几何中心看成参考点，设参考点在原点，综合 考虑机器人的转弯半径和本身的尺寸，将机器人看成圆盘，参考点即为圆心，对于所有x∈p，可以把P+R看成是依据 x转换的R的复制；由于R的中心在原点，x+p的中心将在x，因此P+R相当于放置中心在P的每个点之上的R的 复制，故P+R是P的扩展域p′；扩展之后的障碍物若相交，则此区域不可行，那么在下述构造路径时这块区域就可当 做障碍物处理；对于其他形状的机器人同样可以采用此原理扩展。

如图22至24所示，本发明实施例提供的基于改进的可视图构造路径具体包括：

(1)生成常规的可视图；

(2)对生成的连线按从短到长的顺序进行排序，生成一个由线段组成的队列；

(3)取第一条线段m，检查是否和其后的线段相交；如果发现队列中某一条线段n和线段m相交，那么从队列中删除n 线段，以此类推，直到将所有队列中与线段m相交的线段删除；

(4)再取队列中m的下一条线段，重复步骤(3)，直到取完所有的线段。

如图25-26所示，本发明实施例提供的基于NGA的最短路径具体包括：

(1)路径的个体编码：使用多段线表示路径，除了第一点和最后一点分别落在出发点和目标点上之外，其它路径线段 与线段之间的连接点都必须落在地图连接线中点上，故路径则可表示为((Xs,Ys),(X17,Y17),(X12,Y12),(X5,Y5),(X3,Y3), (Xg,Xg))，其中每个点的角标根据生成连接线按长度排序之后的位置，(Xi,Yi)代表是中间点的坐标，(Xs,Ys)和(Xg,Xg)分别 表示起点和终点；

(2)初始种群的产生：在地图的起点和终点确定，以及连接点生成之后，随机的选取这些连接点组成路径；

(3)适应度函数：主要考虑路径长度与能量的最优：

f_fitness＝dist+g(α,n,...,μ)

其中：dist-对路径长度的评价式中：x_k-第k个连接点的x坐标； g(α,n,...,μ)是能量函数，其能量与拐弯半径、连接点数量以及地面的摩擦系数等因素有关；

(4)NGA操作算子：

1)选择算子采用相似度算子：随机生成种群后，在遗传算法进行选择运算前，对群体中每两个个体逐位比较；如果两 个个体在相对应的位置上存在着相同的字符(基因)，则将相同字符数量定义为相似度R；

设T＝适应度平均值，在群体中取大于T的个体进行个体相似程度判断；R小则表示这两个个体相似性差，当R≥L/2(L为个 体的长度)时，即认为这两个个体相似；

2)交叉算子：共有节点处一点交叉，潜在节点处一点交叉，以及连通节点对处一点交叉共计三中交叉算子；

3)变异算子

采用动态确定变异概率，首先对生成的路径根据其适应度值由小到大排序，对于适应度值大(大于适应度值的均值，其 中适应度越小越好)的路径取大变异率，对于排在前面的适应度值较小的路径取小变异率；

4)终止准则：利用多代之间的平均适应度之差小于某值Δ来终止；

(5)NGA路径规划迭代步骤：

1)首先随机生成m条从起点到终点的路径；所有生成的路径都落在连接线的中点上，再利用相似度判断这些路径之间的 相似度，如果相同路径过多，则需要再随机生成一些路径，扩大初始种群，避免陷入局部最优解；

2)随机将路径进行两两交叉，并随机取路径连接点进行移动，生成新路径；

3)对生成的路径根据其适应度值，由小到大排序，对于适应度值大(大于适应度值的均值，其中适应度越小越好)的路径 取大变异率，对于排在前面的适应度值较小的路径取小变异率；

4)经过几次迭代最终找到评估值最小的可行路径；

5)当经过多次迭代之后，如果连续几次出现的平均适应度值Δ<＝0.5时，则停止迭代。

如图27-28所示，本发明实施例提供的遥操作模块3具体包括：

遥操作模块3利用Teleoperation实现机器人联动的遥操作，利用Remote Control实现手柄控制的遥操作；

Teleoperation：包括位于上位机的遥操作子系统软件，和位于机器人本体上的机械臂位置伺服控制系统；用于根据用户 发出的对机械臂进行控制的命令(一般由机械臂的位姿来描述)，通过解析的逆运动学方程，实时解算出各关节角度，并通 过无线通讯系统传递给位于机器臂上的现场位置伺服控制系统，完成各关节的位置伺服控制，并能实时获取机械臂当前的运 动学状态，将其反馈给用户；运动学状态可以是各关节的实际角度，或者利用解析的正运动学方程转变为机械臂的位姿描述；

Remote Control：用于操作者检测摇杆的输入信息，经手柄判断后向机器人发送不同的控制指令，实现利用操纵杆(Remote Control)实现机器人手爪的上、下、左、右、拉、伸操作；

使用winmm.dll动态链接库文件的函数编程驱动手柄以及将机器人的指令加入操纵杆，操纵杆控制函数具体包括：

joyGetDevCaps查询指定的游戏杆设备以确定其性能；

joyGetNumDevs返回系统支持的游戏杆设备的数量；

joyGetPos查询指定的游戏杆设备的位置和活动性；

joyGetPosEx查询一个游戏杆设备的位置和它的按扭状态；

joyGetThreshold查询指定的游戏杆设备的当前移动阈值；

joyReleaseCapture释放由JoySetCapture函数设置的在指定游戏杆设备上的捕获；

joySetCapture发送一个游戏杆消息到指定的窗口；

joySetThreshold设置指定的游戏杆设备的移动阈值；

操纵杆控制主要程序如下：

//摇杆初始化

UINT uXPos[3],uYPos[3],uZPos[3],uRPos[3],uButton[3],uNoButtonPressed[3],uPOV[3]；//定义游戏杆的一些状态量 CString sStr；

joyGetPosEx(JOYSTICKID1,&joyinfoex[1])；//查询一个游戏杆设备的位置和它的按扭状态

uXPos[1]＝joyinfoex[1].dwXpos；

uYPos[1]＝joyinfoex[1].dwYpos；

uZPos[1]＝joyinfoex[1].dwZpos；

uRPos[1]＝joyinfoex[1].dwRpos；

//getbutton number

uButton[1]＝joyinfoex[1].dwButtons；

//get no of button pressed

uNoButtonPressed[1]＝joyinfoex[1].dwButtonNumber；

//Get POV Point Of View---set view point

uPOV[1]＝joyinfoex[1].dwPOV；

uButton[1]＝joyinfoex[1].dwButtons；

//get no of button pressed

uNoButtonPressed[1]＝joyinfoex[1].dwButtonNumber；

//Get POV Point Of View---set view point

uPOV[1]＝joyinfoex[1].dwPOV；

sStr.Format("％u",uPOV[1])；

SetDlgItemText(IDC_EDIT_POV,sStr)；

//车体运动指令

if(uButton[1]＝＝1&&uPOV[1]＝＝0&&uZPos[1]＝＝180)

{

m_strMessage＝"车体向前"；

pview->wrong_command＝true；

pview->OnSendCommand((char)15,(float)0,(float)0,(float)0,(float)0,

(char)TELE_OP_BODY_SPEED,(char)TELE_OP_BODY_SPEED,(char)pview->Drive,

(char)pDoc->pano_waist,(char)0,(double)0)；//发送指令

UpdateData(false)；

}

。

本发明实施例提供的智能化处理单元14具体包括：

(1)用多线程串口通信CSerialPort进行控制指令的发送与接收都利；CSerialPort封装了串口的初始化和数据的接收， 对串口进行数据的读写操作采用的是多线程和消息通知模式；

(2)数据接收时，首先应该在CTestView类中用该类实例化一个全局对象，然后在OnNewDocument()函数中初始化串 口，如果初始化成功则开启多线程监控并读取串口程序；

具体流程包括：

1)初始化串口和开启多线程监控程序：

参数说明：m_iPortNum–串口号1-COM1

iBaud–波特率9600

chParity[0]–校验位‘N’-None

iDataBits–数据位8

iStopBits－停止位1

2)把数据写入串口的函数：

……

m_Port.WriteToPort(pchSendData,sizeof(dfOriginData))；//把数据写入串口

此函数把控制指令写入串口，通过电台的无线通信发送给下位机PC104，PC104根据接收到的指令控制机器人作出相应 的动作；

3)读取串口中数据：采用CSerialPort读取下位机反馈的数据，再对反馈的异常数据进行处理，提取所需信息；

在CTestView.h中添加串口字符接收消息WM_COMM_RXCHAR(串口接收缓冲区内有一个字符)的响应函数申明：

接着在CTestView.cpp中加入函数的实现：

本发明实施例提供的机器人异常数据具体包括：

x1和x0表示电池电量状态；00表示电量饱满；01表示电量正常；10表示电量低压；11表示电量欠压；x2＝1表示机器 人在自动抓取时目标物在不可抓取范围；x3＝1表示驱动器报警；x4＝1表示接收到错误指令。

本发明实施例提供的异常处理具体包括：

(1)当机器人检测到上述异常时，在操作界面上都会提示操作者，让其对异常做出相应的处理；在操作主界面上添加 了电量显示图标，让操作者能够很直观的观察机器人电量的变化情况；

(2)驱动器异常时，驱动器会进行报警，可通过驱动器开关及时对驱动器进行断电操作；

(3)当接收方出现误码，监测程序可实时检测指令的正确性，对错误指令进行及时的处理；

每当上位机发一个控制指令，下位机会检测指令是否正确，如果正确的话即x4＝0，上位机则返回OnSendCommand ((char)85,(float)data2,(float)data3,(float)data4,(float)data5,(char)data6,(char)data7,(char)data8,(char)data9,(char) data10,(double)data11)

如果x4＝1，则重新发送原来的数据OnSendCommand((char)data1,(float)data2,(float)data3,(float)data4,(float) data5,(char)data6,(char)data7,(char)data8,(char)data9,(char)data10,(double)data11)；

(4)对于下位机出现死机异常，则是通过硬件重启；当通讯出现异常时，机器人三秒钟收不到任何指令，则通过硬件自 动关机。

如图29所示，本发明实施例提供的高效控制机器人的控制方法具体包括：

S101，设置串口参数，初始化串口，

S102，利用摄像头获取图像，导入摄像头抓取的目标物，并对图像进行匹配，根据匹配结果求取目标物的三维坐标；

S103，基于NGA算法为机器人寻找到目标物三维坐标的最优路径；

S104，利用Teleoperation与Remote Control控制机器人运动。

下面结合具体实施例对本发明的应用原理做进一步描述。

实施例1：视觉系统设计

1、视觉系统设计目标

机器人视觉系统是体现机器人智能的核心部分，本发明排爆机器人视觉系统主要实现以下几部分：

(1)通过图像采集卡进行图像采集

(2)对摄像头进行标定

(3)对图像进行匹配，找到目标点在左右图像上的位置

(4)对左右图像进行三维求距，求取目标点的三维坐标

(5)通过串口发送数据

2、视觉系统的硬件平台和软件平台

(1)硬件平台

在本发明机器人双目立体视觉子系统中，总共有两台摄像机，两摄像机安装在机械臂的小臂上，作为机器人计算物体坐 标的“双眼”。两台摄像机采集的图像通过无线图像传送设备传送到后台服务器。后台接收到图像后，通过安装在计算机中的 图像采集卡，便可以将图像送入计算机进行处理，处理结果通过数据传输电台再传送给PC/104。

搭建该视觉系统所需要的硬件设备包括：摄像机，无线图像收发器，图像采集卡，服务器，数传电台，硬件设备列表如 表1-1所示。

表1-1硬件设备列表

各硬件之间的关系如图30所示。

(2)软件平台

视觉系统软件运行于服务器的Windows2003平台，软件开发环境基于MicrosoftVisual C++6.0和Evision 6.7。

Microsoft Visual C++6.0是运行于Windows平台上的交互式可视化集成开发环境，它集程序的代码编辑、编译、连接和 调试功能于一体。Visual C++6.0不但支持所有Windows API函数，而且提供MFC类库。MFC提供了面向对象的Windows 应用程序接口，它有效的简化了编写Windows应用程序的难度，缩短了开发周期。

Evision机器视觉软件包是由比利时Euresys公司推出的一套机器视觉软件开发SDK，调用Evision中提供的图象处理算 法可以进行图像的相关处理。Evision6.2软件是专门针对机器视觉的一套图像处理应用软件，它覆盖了大多数的数字图像处 理技术，适于图像采集、图像处理、三维测距、图像识别等应用，该软件基于面向对象程序OOP，允许用户进行二次开发。 3、视觉系统的总体设计

视觉系统由五部分组成：图像采集与显示，摄像机离线标定，图像匹配，三维求距，串口通讯。图31是视觉系统总的 功能框图。

(1)图像的采集与显示：主要是通过图像采集卡将模拟图像转换为计算机能识别的数字图像，然后在对应的程序窗口 中进行显示。(test程序中的开启按钮中程序)。

(2)双目匹配模块的目的是找到场景中物体在左右图像中的像素位置，以便进行下一步的三维坐标的计算，该模块的 输入为图像采集卡左右摄像机两通道采集到的RGB图像，输出为共轭对(物体在两摄像机中的投影点)的像素坐标。(test 程序中的开始匹配按钮中程序)

(3)摄像机离线标定模块的目的是辨识左右两摄头的内参数以及左右两摄头之间的外参数。标定是视觉系统的一个基 础，为后面视觉计算提供了条件，输入为多幅棋盘模板的图像，输出为左右摄像机的内参数以及右摄像头相对于左摄像机的 外参数。(主要采用Matlab中的TOOLBOX_Calib工具箱进行标定)

(4)三维坐标计算模块的目的为计算空间物体的三维坐标，通过共轭对的位置和标定后的摄像机的内参数以及外参数， 计算出场景中物体的三维坐标。

(5)串口通讯主要是通过调用串口指令发送前面计算的空间点的三维坐标给PC104控制机器人的抓取。

4、视觉系统的详细设计

(1)图像采集与显示(test程序中的开启按钮中程序)

计算机通常不能直接处理摄像机采集的图像，必须把图像通过数字化形成数字图像，形成能够被计算机处理的格式，这 个过程为图像采集和数字化。本发明主要采用的是Euresys公司的Picolo Tetra视频采集卡，主要采用此视频采集卡的Evision 机器视觉软件包，它是由比利时Euresys公司推出的一套机器视觉软件开发SDK，调用Evision中提供的图象处理算法可以 进行图像的相关处理。本发明主要采用了里面的图像采集、图像匹配模块。下面主要介绍图像采集、图像匹配模块的实现， 在实现这些功能之前，首先要对视频采集卡的初始化设置。

1)视频采集卡的初始化设置

图像的采集完全是通过视频采集卡操作，利用视频采集卡自带的驱动程序，在完成对采集卡的初化后，就可以通过调用 它驱动函数库中的图像采集函数来采集图像，这样我们就把对硬件的操作完全用软件屏蔽起来。以后视频的采集就可以通过 软件完全控制了。

主要通过图像采集卡软件包中Configuration,Board,Channel,Surface 4个类来完成：

Configuration类用来配置Multicam驱动，系统中只存在一个用Configuration定义的全局对象，且由系统自动创建，不 需要用户定义。

Board类，它是由系统自动创建的全局数组，每个BOARD对象代表一块EURESYS图像采集卡，例如Boards[0]代表 PC上的第一块EURESYS图像采集卡，Board[1]代表PC上的第二块EURESYS图像采集卡。

Channel类代表Multicam的通道相关类，所有通道的参数设置都通过此类实现，本系统采用的图像采集板卡有4个通道， 可以同时接入四台摄像机。

Surface类代表图像缓存区，用于应用程序在获取图像时管理图像。

由于Configuration和Board类是由系统自动定义对象，且会根据系统的板卡自动进行初始化，我们要做的事情则是设 置通道的参数，注册回调函数，然后提取图像缓存区的图像数据即可。

通道初始化流程图如图32所示，相应模块介绍如下：

动态生成Channel对象且绑定采集卡通道:调用Channel类的构造函数Channel(Board*board,const char*Connector)动 态创建Channel对像，其中构造函数的一个参数为Board类指针，如果是对应PC上第一块图像采集卡则为Boards[0],第二 块采集卡对应Boards[1],如此类推，总共可以支持4块图像采集卡。Boards数组是由系统自动生成的全局变量，数组的长度 为4。参数Connector为字符串类型，指明图像采集卡的通道号，“VID1”表示Picolo图像采集卡的第一个通道，“VID2”表示 Picolo图像采集卡的第二个通道，其余两个通道类似定义。本发明中用到图像采集卡的三个通道，即为“VID1”,“VID2”, “VID3”。

设置视频信号制式：调用Channel类的成员函数void SetParam(param,value),参数param表示要设置的参数，value 表示设置参数的值，在本系统中，这里param参数为MC_CamFile，value值为”PAL.cam”,”PAL.cam”表示摄像机为PAL制 式。

设置图像数据格式：同样调用Channel类的成员函数void SetParam(param,value),在本发明中，这里param参数为 MC_ColorFormat，value值为”MC_ColorFormat_RGB24”，

“MC_ColorFormat_RGB24”,表示图像格式为24位的RBG格式。

注册回调函数：调用Channel对象的成员函数void RegisterCallback(T*owner,void(T::*callbackMethod)(Channel&, SignalInfo&),MCSIGNAL signal)；callbackMethod为回调函数的指针，此回调函数为自定义的函数，回调函数的输入参数为(Channel&,SignalInfo&),T为此回调函数的拥有者,SignalInfo中存有图像缓冲区信息，参数signal表示与回调函数相对应的 信号，本发明中signal为MC_SIG_SURFACE_PROCESSING。当注册成功后，开启回调函数，每当图像采集卡抓取到新的 图像帧后就会触发回调函数，同时把图像采集卡相应通道的图像缓存区地址传递给回调函数。

使能回调信号：调用成员函数void SetParam(param,value)；本系统中param参数为 MC_SignalEnable+MC_SIG_SURFACE_PROCESSING,value值为MC_SignalEnable_ON,表示启动 MC_SIG_SURFACE_PROCESSING信号。

设置通道获取图像的模式：调用成员函数void SetParam(param,value)；参数param为MC_SeqLength_Fr,表示为设置采 集图像的模式，本发明中value值为MC_INFINITE,表示连续采集图像。

准备就绪：调用成员函数void Prepare()，此函数保证所有的配置操作全部完成，一旦触发采集命令就可以马上进行图像 的采集。

在VC中的文档类中OnNewDocument()进行初始化。

在上面完成采集卡的参数配置工作的前提下，调用Channel对像的成员函数SetActive()启动采集卡进行图像采集，这时 每当采集卡的相应通道的缓存区有了新的图像数据，程序将进入之前注册的回调函数中，在回调函数中取出缓冲区的信息， 然后调用函数void UpdateImageConfig(const Surface&s,EImageC24&img)；把缓存区的图像数据赋值给EImageC24定义的 图像对象然后在MFC中的视图中进行显示。这样就可以在程序中看到摄像头所拍摄到的图像了。

(2)图像匹配(test程序中的开始匹配按钮中程序)

双目匹配的目的是找出空间某物体在左右两摄像机中的投影点，针对半智能排爆机器人，为达到工程项目中系统要求的精 度，本发明采用了工业上应用很成熟的机器视觉软件eVision，其提供的EasyMatch是颜色和灰度级别的模式匹配库。它可 以让系统在图像中找到与基准模式相匹配的部分，即使目标发生旋转，等放性或任意方向的缩放，它都可以找到目标，双目 匹配框图如图33-图34所示。

本发明中匹配模式分为两种：(1)自动匹配模式：在左图中手动选择ROI区域，系统调用匹配算法，在右图中自动找 出相应的ROI区域。(2)手动匹配模式：手动选择左图和右图相应的ROI区域；在图像质量严重受损且通信不稳定的情况 下，应用自动匹配模式很难达到理想的效果，这时可以采用手动模式进行匹配。手动匹配模式的缺点在于匹配速度慢且精度 低。

自动匹配模式：在图像显示窗口拖动MFC中的橡皮筋类CRectTracker定义的橡皮筋对象，确定目标后，获取橡皮筋对 象的位置信息，用EROIC24分别定义一个ROI对象；之后调用函数void SetPlacement(INT32n32OrgX,INT32n32OrgY, INT32n32Width,INT32n32Height)设置ROI的位置信息，然后与左图进行绑定，绑定函数为void Attach(EImageC24* pParent)；这时ROI的内容为左图在橡皮筋区域内的内容，最后调用EMatch类进行图像的匹配。

EMatch为eVision软件库中的主要类，主要函数如下：

void EMatch::LearnPattern(EROIC24*pPattern)；输入ROI进行预处理

void EMatch::Match(EROIC24*pImage)；输入母图进行匹配

EMatchPosition*EMatch::GetPosition(UINT32un32Index)；输出匹配结果，本发明中un32Index＝0表示只取最佳匹配区 域的坐标。

(3)摄像机参数标定

在本发明中主要是利用张氏标定法进行标定的，主要是利用matlab的标定工具箱进行的。

(4)三维坐标的计算(主要是test程序中计算三维坐标按钮中的代码，要计算三维坐标在程序中必须加入funtion.dll) 三维坐标计算模块的主要功能为通过取两射线公垂线的中点来计算场景中某物体的三维坐标，根据场景中的物点在两摄像机 中像点的图像坐标，来计算出物点的三维坐标，即根据O_l、p_l、O_r、p_r四点的空间位置求直线O_lp_l、O_rp_r的交点P的 空间位置，因为摄像机模型和投影点的近似误差，两台摄像机的投影线O_lp_l、O_rp_r并没有在数学3D空间相交于一点，最 好的解决方案是计算这两条空间斜交投影线之间的最短距离，也就是计算它们公垂线段的长度。如果公垂线比较短，就取公 垂线的中点作为两条投影线的交点，定为(X,Y,Z)。如果公垂线太长，那么就断定在进行像点对应计算时出现了问题。 如图19所示,为射线

(5)视觉系统软件界面介绍

在视觉软件系统中，计算物体三维坐标的主要步骤：首先搜索抓取目标。在此有两种图像匹配方式，一种为“手动匹配”， 一种为“自动匹配”。“手动匹配”用于图像干扰严重的情形下，由操作者分别在两个摄像机的成像中选定相匹配的目标。然后 点击“开始搜索”，弹出目标选取框，选取好目标点后，再点击“开始匹配”，所显示的十字光标，即为选择的目标，虽然在两 幅图像中位置不一样，但却在此程序中，被认为就是同一物点。再点击“确认目标”以及“计算三维坐标”,即可计算出十字光标 处的三维坐标。

实施例2：机器人操作说明

1、机器人的软件界面主要包括以下几部分，操作主界面：(操作主界面图)

a.图像显示区：主要是显示机器人两个眼睛的图像，且实现对两张图片的处理，处理主要包括一下几部分，分别为：

1)导入两摄像头所抓取的图像

2)搜索及选取感兴趣的目标物

3)对左右图像进行匹配

4)根据匹配结果求取目标物的三维坐标。

b.功能区：

整个机器人的操作主要分为以下几部分：

1)双目视觉操作部分

2)手动控制部分

3)手柄遥操作部分

4)其它辅助部分

注意：整个控制系统都是通过串口发送数据的，故在操作时你首先需要设置串口，选择可用的串口，点击手动控制中的 设置串口，弹出串口子窗口，选择你所需的的参数。在串口设置好后就可就进行操作。

本发明机器人的目的是抓取指定的目标物，在打开机器人的电源之后，要实现对机器人的任何操作必须打开紧急停止打开， 要实现对机器人手臂的操作必须首先打开关节打开开关，如果是在夜晚或光线不好的环境中可打开机器人手腕上方的照明灯， 只需点击照明开关打开，之后就可以控制机器人运动找寻以及抓取目标物。控制机器人主要有两种方式一种是必须在电脑前 的手动控制方式，一种是边看机器人运动边通过游戏手柄控制。

(1)手动控制方式

点击主界面的手动控制按钮，则弹出排爆机器人控制系统子界面。

手动控制子系统包含机器人手臂的各种运动控制，以及车体的运动控制，以下就详细介绍各个部分的功能。

1)手爪控制

主要包含手爪的张开以及抓紧以及手爪的正转和反转，力度滑块可以实现手爪抓取力在0-128之间变化。

2)关节控制

此机器人手臂主要由四个关节组成，此部分可以根据所需的转动角度控制各个关节。

要实现机器人准确抓取目标物，必须确定一个基准，即零位置，此零位置是肩关节、肘关节和腕关节都位于水平位置， 故在机器人运动之前必须调节各个关节的位置使其位于零位置，关节位置的调整是在编辑框输入想其运动的角度，再点击旁 边的运动按钮即可，如：想让腕关节运动5度，则在腕关节处输入5。调整好各个关节之后再点击设置零位置，由于零位置 对抓取精度影响很大，不能随意点击此按钮，故在此设一密码，防止操作着不小心点击。

复位按钮：可以实现机器人手臂随时恢复到零位置。

停止按钮：主要是停止机器人手臂的一起运动。

3)手臂方向控制；

此部分主要是实现手臂的联动，在机器人手臂零位置确定之后主要是通过此部分来控制手臂的运动。

此部分可以实现手臂前后左右上下六个方向的运动，步长是手臂在每次运动的一个距离，如步长为100mm，如点击“前”， 表示整个手臂向前运动100mm，其它类似，夹角指手臂运动后手腕的相对与水平面的角度。根据不同的情况设定不同的步 长和夹角，水平抓取夹角角度设为0，斜抓则根据程度不一样设定不同的角度。一般都为0-90。

4)车体方向控制：

此部分主要控制车体的前进、后退以及转弯。

速度滑块可以调节车体运动速度的大小，其变化范围为：0-118.

(2)手柄控制

本发明机器人的另外一个特色就是加入了基于游戏手柄的遥操作，操作者不需在电脑前，可以跟随机器人进行操作。

点击主界面的手柄操作，弹出手柄控制界面：

手柄操作和手动控制的内容一样，不同的是手柄操作全部为固定步长的，手臂和车体都没实现调速。

只要按下设定指令功能的按钮就可实现机器人的运动，且在发送状态编辑框中显示所发送命令的详细信息，由于游戏手柄功 能和手动控制系统一样在此就不详细说明。

(3)双目视觉操作部分

视觉系统软件操作界面：

a.全景摄像头部分主要是帮助操作者观察机器人运动过程中的周围环境，以及确定目标物相对与机器人的位置便于定 位抓取，全景摄像头部分的方向控制可以实现六个方向的运动如下图点击方向控制按钮及出现全景摄像头方向控制子窗口。

b.双目视觉部分

(1)显示图像：由于双目视觉部分的图像收发器比较费电，一般在需用时才打开电源，故本发明在此处安装了一开关， 如需要开启双目视觉系统图像，必需先点击，再点击双目摄像头的开启按钮。

(2)搜索目标物：点击开始搜索，即可以在左视图中选取目标物，选取目标物之后，根据目标物的匹配方式的不同，进 行不同方式的匹配，如果选取的是手动匹配则会出现如下情况开始匹配按钮变灰(不可用)，需要人为的在右视图中选取左 图已经选取的目标物。

如果选取的是自动匹配方式则只需点击开始匹配按钮右视图就自动的找出左视图的目标物。

在左右视图匹配之后，以目标物的几何中心作为目标点进行计算，点击确认目标计算出目标物的几何中心。

(3)计算目标物的几何中心

在计算出目标物的几何中心后，就需计算目标物的三维坐标，只需点击计算三维坐标按钮，即在编辑框中显示目标物的 三维坐标。

在一切准备好之后，点击主界面的自动运行，则机器人就自动去抓取目标物。

(4)其它辅助部分

在主界面的右方有电池显示部分，主要是便于操作者实时观察机器人电量的多少。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替 换和改进等，均应包含在本发明的保护范围之内。

Claims (10)

1.一种高效控制机器人的系统，其特征在于，所述高效控制机器人的系统包括：

双目立体视觉模块：与上位机控制模块连接，包括图像采集与显示单元、标定单元、图像预处理单元、图像匹配单元以及物点三维求距单元；用于利用两台性能相似、位置固定的摄像机，获取同一景物的图像，再根据场景中的物点在两摄像机中像点的图像坐标，来计算出物点的三维坐标；

路径规划模块：与上位机控制模块连接；用于为机器人在特定的环境中寻找一条免避撞的到达目标物位置的最优路径，所寻找的最优路径是机器人在到达目标点的过程中拐弯次数少的情况下所经过的路径短；

遥操作模块：与上位机控制模块连接；用于利用Teleoperation和Remote Control操作方式提高机器人的作业效率和可操作性；其中Teleoperation是从运动学角度实现的遥操作，是实现排爆机器人机械臂联动的基础；Remote Control是采用基于USB接口的手柄去操作机器人；

上位机控制模块：与双目立体视觉模块、路径规划模块、遥操作模块、通讯模块连接；用于利用MFC编程把双目立体视觉模块、路径规划模块、遥操作模块、通讯模块、下位机模块都封装在程序中；

通讯模块：与上位机控制模块、下位机模块连接；包括数据通讯单元、图像通讯单元以及智能化处理单元；用于完成上位机与后方下位机的双向信息交流；

下位机模块：与通讯模块连接，包括受控制的机器人。

2.如权利要求1所述高效控制机器人的系统，其特征在于，所述双目视觉模块计算爆炸物抓取点相对机器人手爪的位置坐标；包括图像的采集与显示单元、摄像机的标定单元、图像的预处理单元、图像的匹配单元以及物点的三维求距单元；

图像采集与显示单元：用于利用两台性能相同、位置固定的摄像机从不同角度对同一景物进行拍摄获取图像；

标定单元：用于建立有效的成像模型，并确定摄像机内外部属性参数，确定空间坐标系中物点与它在图像平面上像点之间的对应关系；

图像预处理单元：用于消除由摄像头畸变、光照条件、以及传输过程对图像引入的噪声；

图像匹配单元：用于在两幅图像的匹配点或者匹配基元之间建立对应关系；

物点三维求距单元：用于从二维灰度图像中获得目标点的三维深度信息。

3.如权利要求2所述高效控制机器人的系统，其特征在于，所述图像采集与显示单元具体包括：

采用Euresys公司的Picolo Tetra视频采集卡，利用视频采集卡自带的驱动程序MultiCam，在完成对采集卡的初化和参数设置后，调用采集卡驱动函数库中的图像采集函数来采集图像，并将模拟图像转换为计算机能识别的数字图像，在对应的程序窗口中进行显示；

所述图像采集函数包括：

Channel::Channel(Board*board,int connector)；//初始化通道

void SetParam(param,value)；//参数设置

Template<class T>void RegisterCallback(T*owner,void(T::*callbackMethod)(Channel&,SignalInfo&),MCSIGNAL signal)；//注册回调函数

void UpdateImageConfig(const Surface&s,EImageC24&img)；//读取缓存区的图像

void Channel::SetActive()；//显示图像

void Channel::SetIdle()；//释放资源。

4.如权利要求2所述高效控制机器人的系统及方法，其特征在于，所述标定单元具体包括：

标定单元用于确定摄像机的几何和光学参数，摄像机相对于世界坐标系的方位；用上标L和R来分别表示左摄像机和右摄像机的各种变量和参数；在场景中建立世界坐标系，物点在左右两摄像机中分别成像，形成图像坐标(u,v)^L和(u,v)^R；Projection^L()和Projection^R()分别代表两摄像机的成像过程；{A^L,k₁ ^L,k₂ ^L}为左摄像机内参数，{R^L,T^L}为左摄像机外参数；{A^R,k₁ ^R,k₂ ^R}为右摄像机内参数，{R^R,T^R}为右摄像机外参数；其中内参数由摄像机的物理结构决定，固定不变；而外参数随着世界坐标系选取的不同而不同。

5.如权利要求2所述的高效控制机器人的系统，其特征在于，所述图像预处理单元具体包括：图像预处理单元基于中值滤波进行去噪处理；

所述图像匹配单元具体包括：

图像匹配单元用于在左右摄像头拍摄的两幅存在视点差异、几何和灰度畸变以及噪声干扰的图像对之间建立特征之间的对应关系，将同一空间物理点在不同图像中的映像点对应起来；

所述遥操作模块具体包括：

遥操作模块利用Teleoperation实现机器人联动的遥操作，利用Remote Control实现手柄控制的遥操作；

Teleoperation：包括位于上位机的遥操作子系统软件，和位于机器人本体上的机械臂位置伺服控制系统；用于根据用户发出的对机械臂进行控制的命令(一般由机械臂的位姿来描述)，通过解析的逆运动学方程，实时解算出各关节角度，并通过无线通讯系统传递给位于机器臂上的现场位置伺服控制系统，完成各关节的位置伺服控制，并能实时获取机械臂当前的运动学状态，将其反馈给用户；运动学状态可以是各关节的实际角度，或者利用解析的正运动学方程转变为机械臂的位姿描述；

Remote Control：用于操作者检测摇杆的输入信息，经手柄判断后向机器人发送不同的控制指令，实现利用操纵杆实现机器人手爪的上、下、左、右、拉、伸操作；

使用winmm.dll动态链接库文件的函数编程驱动手柄以及将机器人的指令加入操纵杆，操纵杆控制函数具体包括：

joyGetDevCaps查询指定的游戏杆设备以确定其性能；

joyGetNumDevs返回系统支持的游戏杆设备的数量；

joyGetPos查询指定的游戏杆设备的位置和活动性；

joyGetPosEx查询一个游戏杆设备的位置和它的按扭状态；

joyGetThreshold查询指定的游戏杆设备的当前移动阈值；

joyReleaseCapture释放由JoySetCapture函数设置的在指定游戏杆设备上的捕获；

joySetCapture发送一个游戏杆消息到指定的窗口；

joySetThreshold设置指定的游戏杆设备的移动阈值；

操纵杆控制主要程序如下：

//摇杆初始化

UINT uXPos[3],uYPos[3],uZPos[3],uRPos[3],uButton[3],uNoButtonPressed[3],uPOV[3]；//定义游戏杆的一些状态量

CString sStr；

joyGetPosEx(JOYSTICKID1,&joyinfoex[1])；//查询一个游戏杆设备的位置和它的按扭状态

uXPos[1]＝joyinfoex[1].dwXpos；

uYPos[1]＝joyinfoex[1].dwYpos；

uZPos[1]＝joyinfoex[1].dwZpos；

uRPos[1]＝joyinfoex[1].dwRpos；

//get button number

uButton[1]＝joyinfoex[1].dwButtons；

//get no of button pressed

uNoButtonPressed[1]＝joyinfoex[1].dwButtonNumber；

//Get POV Point Of View---set view point

uPOV[1]＝joyinfoex[1].dwPOV；

uButton[1]＝joyinfoex[1].dwButtons；

//get no of button pressed

uNoButtonPressed[1]＝joyinfoex[1].dwButtonNumber；

//Get POV Point Of View---set view point

uPOV[1]＝joyinfoex[1].dwPOV；

sStr.Format("％u",uPOV[1])；

SetDlgItemText(IDC_EDIT_POV,sStr)；

//车体运动指令

if(uButton[1]＝＝1&&uPOV[1]＝＝0&&uZPos[1]＝＝180)

{

m_strMessage＝"车体向前"；

pview->wrong_command＝true；

pview->OnSendCommand((char)15,(float)0,(float)0,(float)0,(float)0,

(char)TELE_OP_BODY_SPEED,(char)TELE_OP_BODY_SPEED,(char)pview->Drive,

(char)pDoc->pano_waist,(char)0,(double)0)；//发送指令

UpdateData(false)；

}。

6.一种执行权利要求1所述高效控制机器人的系统的高效控制机器人的方法，其特征在于，所述高效控制机器人的方法包括：

步骤一，设置串口参数，初始化串口，

步骤二，利用摄像头获取图像，导入摄像头抓取的目标物，并对图像进行匹配，根据匹配结果求取目标物的三维坐标；

步骤三，基于NGA算法为机器人寻找到目标物三维坐标的最优路径；

步骤四，利用Teleoperation与Remote Control控制机器人运动。

7.如权利要求6所述的高效控制机器人的方法，其特征在于，所述高效控制机器人的方法利用MFC编程将双目立体视觉模块、路径规划模块、遥操作模块、通讯模块、下位机模块封装在程序中具体包括：

(1)将双目立体视觉模块封装在类CVision中，主要的功能函数如下：

CVision::OnActive()完成图像的显示，为系统显示实时采集来的图像；

CVision::OnSearch()完成目标物的搜索，为系统选择左视图中的目标点；

CVision::OnMatch()完成图像的匹配过程，为系统寻找右视图中对应的目标点；

利用动态链接链库CoordinateCalculate来封装三维求距功能，在求取时只需要调用内部的接口函数LvDistance(m_dfR,m_dfT,m_dfKL,m_dfKR,m_dfML,m_dfMR,m_dfMatchPt,res,error)计算目标点的三维坐标即可；

(2)将机器人的手动控制界面封装在CControlTestDlg中；实现机器人各个关节、车体的控制；包括：手爪控制、关节控制、手臂方向控制以及车体控制，其中关节控制和手臂控制封装在类CManipulator中，CControlTestDlg只需调用其接口函数；手爪控制和车体控制直接封装在CControlTestDlg中；

(3)将遥操作模块封装在类CTeleoperation中；

(4)将通讯模块封装在类CSerialPort中；

(5)将路径规模模块采用遗传算法路径规划封装在GA项目中。

8.如权利要求6所述的高效控制机器人的方法，其特征在于，所述高效控制机器人的方法的标定的具体步骤包括：

(1)制作一张黑白方格相间的模版，打印并贴在一个平面上作为标定过程中所使用的平面标定靶标；

(2)将模板多个角度放置在摄像机前拍摄图像，并根据模板的空间位置建立世界坐标系O_wX_wY_wZ_w，使得模板位于世界坐标系的X_wY_w平面上，即模版上任一点的在世界坐标系中Z_w＝0；

(3)取黑白棋格的交点作为控制点，并称之为“角点”；

(4)利用Matlab编写的张氏标定法对单个摄像头进行标定，分别求成两个摄像头的内参数；

(5)利用Matlab编写的张氏标定法对双目视觉进行立体标定，求出两摄像头的外参数；

所述张氏标定法：

假设标定用平面图板在世界坐标系中Z＝0，通过线性模型分析计算得出摄像机参数的优化解，然后用基于最大似然法进行非线性求精；在这个过程中，标定考虑到镜头畸变的目标函数，最后求出所需的摄像机的内、外部参数；

其基本原理如下式：

在这里假定模板平面在世界坐标系Z_w＝0的平面上；

式中，A为摄像机的内参数矩阵，令 为模板平面上点的齐次坐标，为模板平面上点投影到图像平面上对应点的齐次坐标，[r₁ r₂ t]和t分别是摄像机坐标系相对于世界坐标系的旋转矩阵和平移向量；建立控制点的空间坐标与图像坐标之间的单映性关系式：

其中，H＝A[r₁ r₂ t]，描述了控制点的空间坐标与图像坐标之间的映射关系

每一个控制点的H都不同。但对于同一幅模板图像上的控制点，因A[r₁ r₂ t]固定，故其上的控制点的仅相差一尺度因子；对于一幅模板图像，虽然不能完全确定其对应的A[r₁r₂ t]，但却可以在某种程度上获得关于A[r₁ r₂ t]的约束；

H提供了关于A[r₁ r₂ t]的约束，也即是提供了关于A的约束；不同的模板，其H不同，但是A是确定的；可以通过多个不同的模板的H，来最终确定A，即有：

根据旋转矩阵的性质，即r₁ ^Tr₂＝0和||r₁||＝||r₂||＝1，每幅图可以获得以下两个对内参数矩阵的基本约束：

摄像机有五个位置参数，故当所摄取的图像数目大于3时，就可以线性唯一求解A；再利用A来估计各模板的外参数；

每幅模板的H都能提供了关于A的约束，具体的约束由旋转矩阵R＝[r₁ r₂ r₃]的正交性推导得到。旋转矩阵R的正交性为：

r₁ ^Tr₂＝0

r₁ ^Tr₁＝r₂ ^Tr₂

因而可以推导得到：

[h₁ h₄ h₇]A^-TA^-1[h₂ h₅ h₈]^T＝0

[h₁ h₄ h₇]A^-TA^-1[h₁ h₄ h₇]^T＝[h₂ h₅ h₈]A^-TA^-1[h₂ h₅ h₈]^T

外参数{r₁ r₂ r₃ t}和H的尺度因子λ很容易由H与[R T]的关系求得：

r₁＝λA^-1[h₁ h₄ h₇]^T

r₂＝λA^-1[h₂ h₅ h₈]^T

r₃＝r₁×r₂

t＝λA^-1[h₃ h₆ h₉]^T。

9.如权利要求6所述的高效控制机器人的方法，其特征在于，所述高效控制机器人的方法的图像预处理基于中值滤波进行去噪处理；

中值滤波基本原理是把序列或数字图像中间一点的值，用该点邻域中各点值的中值来替代；

对序列而言中值的定义是这样的，若x₁,x₂,…,x_n为一组序列，先把其按大小排列为：

x_i1≤x_i2≤…≤x_in。

则该序列的中值y为：

对于一幅图像的象素矩阵，中值滤波就是用一个活动窗口沿图像移动，窗口中心位置的像素灰度用窗口内所有象素灰度的中值来代替，取以目标像素为中心的一个子矩阵窗口，窗口可根据需要选取；

图像匹配单元用于在左右摄像头拍摄的两幅存在视点差异、几何和灰度畸变以及噪声干扰的图像对之间建立特征之间的对应关系，将同一空间物理点在不同图像中的映像点对应起来；基于图像的外极线原理以及图像的动态搜索原理，采用基于外极线的动态立体匹配算法对图像沿着外极线先进行粗匹配，找出感兴趣的区域，再对感兴趣的进行精匹配；

具体步骤如下：

(1)计算目标点对应的外极线；

(2)沿着外极线划分图像，进行粗匹配；

(3)利用算子进行精匹配；

步骤(1)中，所述外极线计算具体包括：

1)左右图像匹配对特征点的提取：采用左右摄像头在不同的角度拍摄黑白棋盘模板，再提取相对应的特征点以及对应特征点的图像坐标；利用VC++和OpenCV编程利用亚像素角点提取方法提取20组图像匹配对的特征点图像坐标，再求取对应的基本矩阵；

2)基本矩阵的求取，使用最小中值法来估计基本矩阵：

在48个匹配对中随机抽取m个样本，每个样本J包含K个匹配对，设K＝8，选取8点法解出一个基本矩阵F_J，设整个匹配点对数据集中一对匹配点对是(m₁,m₂)，对每个F_J从整个匹配点对集合得到一个中值M_J；

其中表示点m₂到点m₁的极线的距离，d(m_1i,F_Jm_2i)表示点m₁到m₂的极线的距离，med表示对所有的d²(m_2i,F_Jm_1i)+d²(m_1i,F_Jm_2i)按从大到小的顺序排列取中间的值；

从m个样本的所有M_J找出最小的一个，计为M_m，即最小中值，其对应的基本矩阵为F_M，再应用这个最小中值来估计基本矩阵；

采用最小中值法求取最小矩阵的基本步骤如下：

a.获得图像中特征点的匹配对集合；

b.随机抽取m个样本，每个样本J包含K个匹配对，K＝8；

c.用8点算法求取基本矩阵的估算值；

d.重复步骤B共m次，并记录所有的中值和相应的基本矩阵估计值，找出最小中值和相应的基本矩阵的估计值；

其中随机抽取次数m的确定是假设整个数据集合中含有的正确匹配对的百分比为p，则m次采样中至少有一个样本是完全由正确的匹配对组成的概率是P＝1-(1-p^K)^m；

3)外极线的求取：

根据步骤2)求取的多个基本矩阵，求其平均值；再根据外极线理论，则可以求取对应点的外极线；令图像I_L中的一点为p₁，其对应的在图像I_R中的外极线l₂有：

l₂＝F*p₁

同理，令图像I₂中的一点p₂,其在图像I_L中的外极线l₁根据F^Tp₁＝l₀有：

l₁＝F^T*p₂

再根据式p₁ ^TFp₀＝0，p₀ ^TFp₁＝0结合l₂＝F*p₁和l₁＝F^T*p₂有：

再将相关数据代入则可以求出对应点的外极线方程a*u+b*v+c＝0；

步骤(2)中，所述粗匹配具体包括：

先沿着外极线对图像进行分割，缩小图像区域匹配范围，将其分成几个子图像以便进行粗匹配；

具体过程如下：

1)设基准图为T，搜索图为S，搜索子图为s^i，j；

2)把搜索图S平均划分为若干个子图

3)设基准图各象素灰度和为(t为基准图T中任意点的灰度值)，匹配图s^i，j的各象素灰度值和为:T与s^i，j相关度为：阈值设为α(0<α<1)，计数变量设为N；

4)计算各块的η^i,j大小，设一阈值α(设为：0.25)如果η^i,j<α，N加1，并记下位置；如果N＝1，则直接可以进行精匹配；如果N>1，则要直接找出多次最小值的位置；如果N＝0，表示干扰因素比较大，则要调整α值再进行匹配，α的值是越小越好，如果选取的值过小，由于误差的存在可能找不到匹配点，此时在允许范围内可以适当放大α的值；

步骤(3)中，所述精匹配具体包括：

针对粗匹配找到的感兴趣的区域，进行精匹配；如果此时获得的区域没包括外极线，则应该扩大区域，增加实验位置；再按常规的灰度匹配逐个像素进行匹配，在这里使用CV_TM_CCOEFF_NORMED归一化相关系数匹配算子进行相似度判断；其相关度

为:

式中I表示图像，T-模板，R-结果；模板与图像重叠区域x'＝0...w-1,y'＝0...h-1之间求和；模板滑动过整个图像image，用l₂＝F*p₁比较模板templ与图像尺寸为w×h的重叠区域。

10.如权利要求6所述的高效控制机器人的方法，其特征在于，所述高效控制机器人的方法的物点三维求距单元具体包括：

根据场景中的物点在两摄像机中像点的图像坐标，计算出物点的三维坐标，基本原理为根据O_l、p_l、O_r、p_r四点的空间位置求直线O_lp_l、O_rp_r的交点P的空间位置，因为摄像机模型和投影点的近似误差，两台摄像机的投影线O_lp_l、O_rp_r并没有在数学3D空间相交于一点，则通过计算这两条空间斜交投影线之间的最短距离，也就是计算它们公垂线段的长度；如果公垂线比较短，则取公垂线的中点作为两条投影线的交点，定为(X,Y,Z)；如果公垂线太长，那么就断定在进行像点对应计算时出现了问题；

路径规划模块采用基于改进GA的移动机器人规划；具体包括：判断路径、构造路径、最短路径；

(1)判断路径：采用了闵科夫斯基和原理扩展障碍物，判断运动物体(即机器人)能否从起点s移动到终点g，即从s到g是否存在一条自由路径；

(2)构造路径：基于改进的可视图方法，在地图中寻找机器人的可行区域；

(3)最短路径：基于NGA寻找机器人从s移动到g的一条最优自由路径；

所述闵科夫斯基和原理具体包括：

给定平面上两个点集S₁,S₂，在平面建立一个x,y坐标系，其中的点可以看成该坐标中的矢量，定义：S₁+S₂＝{x+y|x∈S₁,y∈S₂}；

其中：x+y为点x和点y的矢量和，这即为S₁和S₂的闵科夫斯基和；也可扩展点x与集合S₂的闵科夫斯基和为x+S₂＝{x+y|y∈S₂}，即对于每一个是S₂的复制的并；

将障碍物p看作为S₁，机器人R看成是S₂，机器人在运动过程中以其几何中心看成参考点，设参考点在原点，综合考虑机器人的转弯半径和本身的尺寸，将机器人看成圆盘，参考点即为圆心，对于所有x∈p，可以把P+R看成是依据x转换的R的复制；由于R的中心在原点，x+p的中心将在x，因此P+R相当于放置中心在P的每个点之上的R的复制，故P+R是P的扩展域p′；扩展之后的障碍物若相交，则此区域不可行，那么在下述构造路径时这块区域就可当做障碍物处理；对于其他形状的机器人同样可以采用此原理扩展；

所述基于改进的可视图构造路径具体包括：

(1)生成常规的可视图；

(2)对生成的连线按从短到长的顺序进行排序，生成一个由线段组成的队列；

(3)取第一条线段m，检查是否和其后的线段相交；如果发现队列中某一条线段n和线段m相交，那么从队列中删除n线段，以此类推，直到将所有队列中与线段m相交的线段删除；

(4)再取队列中m的下一条线段，重复步骤(3)，直到取完所有的线段；

所述基于NGA的最短路径具体包括：

(1)路径的个体编码：使用多段线表示路径，除了第一点和最后一点分别落在出发点和目标点上之外，其它路径线段与线段之间的连接点都必须落在地图连接线中点上，故路径则可表示为((Xs,Ys),(X17,Y17),(X12,Y12),(X5,Y5),(X3,Y3),(Xg,Xg))，其中每个点的角标根据生成连接线按长度排序之后的位置，(Xi,Yi)代表是中间点的坐标，(Xs,Ys)和(Xg,Xg)分别表示起点和终点；

(2)初始种群的产生：在地图的起点和终点确定，以及连接点生成之后，随机的选取这些连接点组成路径；

(3)适应度函数：主要考虑路径长度与能量的最优：

f_fitness＝dist+g(α,n,...,μ)

其中：dist-对路径长度的评价式中：x_k-第k个连接点的x坐标；g(α,n,...,μ)是能量函数，其能量与拐弯半径、连接点数量以及地面的摩擦系数等因素有关；

(4)NGA操作算子：

1)选择算子采用相似度算子：随机生成种群后，在遗传算法进行选择运算前，对群体中每两个个体逐位比较；如果两个个体在相对应的位置上存在着相同的字符(基因)，则将相同字符数量定义为相似度R；

设T＝适应度平均值，在群体中取大于T的个体进行个体相似程度判断；R小则表示这两个个体相似性差，当R≥L/2(L为个体的长度)时，即认为这两个个体相似；

2)交叉算子：共有节点处一点交叉，潜在节点处一点交叉，以及连通节点对处一点交叉共计三中交叉算子；

3)变异算子

采用动态确定变异概率，首先对生成的路径根据其适应度值由小到大排序，对于适应度值大(大于适应度值的均值，其中适应度越小越好)的路径取大变异率，对于排在前面的适应度值较小的路径取小变异率；

4)终止准则：利用多代之间的平均适应度之差小于某值Δ来终止；

(5)NGA路径规划迭代步骤：

1)首先随机生成m条从起点到终点的路径；所有生成的路径都落在连接线的中点上，再利用相似度判断这些路径之间的相似度，如果相同路径过多，则需要再随机生成一些路径，扩大初始种群，避免陷入局部最优解；

2)随机将路径进行两两交叉，并随机取路径连接点进行移动，生成新路径；

3)对生成的路径根据其适应度值，由小到大排序，对于适应度值大(大于适应度值的均值，其中适应度越小越好)的路径取大变异率，对于排在前面的适应度值较小的路径取小变异率；

4)经过几次迭代最终找到评估值最小的可行路径；

5)当经过多次迭代之后，如果连续几次出现的平均适应度值Δ<＝0.5时，则停止迭代；

所述高效控制机器人的方法的通讯模块智能化处理具体包括：

(1)用多线程串口通信CSerialPort进行控制指令的发送与接收都利；CSerialPort封装了串口的初始化和数据的接收，对串口进行数据的读写操作采用的是多线程和消息通知模式；

(2)数据接收时，首先应该在CTestView类中用该类实例化一个全局对象，然后在OnNewDocument()函数中初始化串口，如果初始化成功则开启多线程监控并读取串口程序；

具体流程包括：

1)初始化串口和开启多线程监控程序：

参数说明：m_iPortNum–串口号1-COM1

iBaud–波特率9600

chParity[0]–校验位‘N’-None

iDataBits–数据位8

iStopBits－停止位1

2)把数据写入串口的函数：

……

m_Port.WriteToPort(pchSendData,sizeof(dfOriginData))；//把数据写入串口

此函数把控制指令写入串口，通过电台的无线通信发送给下位机PC104，PC104根据接收到的指令控制机器人作出相应的动作；

3)读取串口中数据：采用CSerialPort读取下位机反馈的数据，再对反馈的异常数据进行处理，提取所需信息；

在CTestView.h中添加串口字符接收消息WM_COMM_RXCHAR(串口接收缓冲区内有一个字符)的响应函数申明：

接着在CTestView.cpp中加入函数的实现：

LONG CSCPortTestView::OnCommunication(WPARAM ch,LPARAM port)

{……}

//{{AFX_MSG(CSCPortTestView)

afx_msg LONG OnCommunication(WPARAM ch,LPARAM port)；

//}}AFX_MSG

然后在CTestView.cpp文件中进行WM_COMM_RXCHAR消息映射：

BEGIN_MESSAGE_MAP(CSCPortTestView,CView)

//{{AFX_MSG_MAP(CSCPortTestView)

ON_MESSAGE(WM_COMM_RXCHAR,OnCommunication)

//}}AFX_MSG_MAP

END_MESSAGE_MAP

以下为读取串口数据程序：

机器人异常数据具体包括：

x1和x0表示电池电量状态；00表示电量饱满；01表示电量正常；10表示电量低压；11表示电量欠压；x2＝1表示机器人在自动抓取时目标物在不可抓取范围；x3＝1表示驱动器报警；x4＝1表示接收到错误指令；

异常处理具体包括：

(1)当机器人检测到上述异常时，在操作界面上都会提示操作者，让其对异常做出相应的处理；在操作主界面上添加了电量显示图标，让操作者能够很直观的观察机器人电量的变化情况；

(2)驱动器异常时，驱动器会进行报警，可通过驱动器开关及时对驱动器进行断电操作；

(3)当接收方出现误码，监测程序可实时检测指令的正确性，对错误指令进行及时的处理；

每当上位机发一个控制指令，下位机会检测指令是否正确，如果正确的话即x4＝0，上位机则返回OnSendCommand((char)85,(float)data2,(float)data3,(float)data4,(float)data5,(char)data6,(char)data7,(char)data8,(char)data9,(char)data10,(double)data11)

如果x4＝1，则重新发送原来的数据OnSendCommand((char)data1,(float)data2,(float)data3,(float)data4,(float)data5,(char)data6,(char)data7,(char)data8,(char)data9,(char)data10,(double)data11)；

(4)对于下位机出现死机异常，则是通过硬件重启；当通讯出现异常时，机器人三秒钟收不到任何指令，则通过硬件自动关机。