基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统

摘要

本发明公开了一种基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，结合计算机视觉技术，通过Kinect摄像头来对工业机械臂周围的生产环境进行动态的感知，检测和追踪环境中动态障碍物，再通过边缘检测算法得到动态障碍物的边缘信息。通过更新动态障碍物的包围盒，采用LazyPRM算法来实现动态避障规划以及物体抓取作业。本发明使得工业机械臂具备更加智能的感知生产环境的变化的能力，有效的提升了生产安全性以及工业机械臂作业的智能化程度，为未来的智能化工厂基本实现智能化和互联化提供了一个可行的方案。

权利要求

1.基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，其特征在于，包括：运动学模块，用于机械臂的正逆运动学求解，用户通过输入末端执行器的期望抓取位姿，通过该模块计算得到机械臂各个关节的角度值，用户也能够通过给定机械臂各个关节的角度值来计算末端执行器的位姿；

路径规划模块，用于通过运动学模块求解得到的抓取位姿，采用基于采样的LazyPRM算法，在机械臂状态空间中采样来进行路径规划；

物体抓取模块，用于对空间中物体的抓取，用户通过5DT数据手套来控制五指灵巧手的物体抓取作业，所述5DT数据手套能够通过传感器来采集用户手指关节的弯曲角度，通过关节映射来驱动五指灵巧手的电机使得五指灵巧手运动到抓取姿态；

动态环境检测模块，用于对空间中动态环境进行跟踪检测，能够对空间中的运动障碍物进行检测和追踪，进一步反馈到路径规划模块中，为后续的路径规划提供动态障碍物的空间位置；

仿真模块，用于对机械臂进行实时仿真，用户能够通过人机交互界面对机械臂进行运动控制；

其中，在所述运动学模块中，机械臂正逆运动学采用D-H矩阵进行描述，其逆运动学求解同时采用了几何方法和代数方法：第一个关节角通过从基坐标原点到第四个关节坐标原点的投影向量来求解：θ1＝arctan(py/px)

或θ1＝arctan(py/px)+π

其中px和py分别为投影向量在基坐标系的x,y轴上的分量；

第二个关节角度通过前五个关节的几何关系求出：

或

其中β1为从第二个关节坐标原点到第五个关节坐标原点形成的向量与第二个关节坐标系的y轴形成的夹角，β2为第二个关节坐标原点与第三个关节坐标原点、第五个关节坐标原点的连线形成的夹角；

第三个关节角度通过前五个关节的几何关系求出：

θ3＝π-φ-α或θ3＝π+φ-α

其中α为第三连杆与第三关节坐标原点到第五关节坐标原点的连线形成的夹角，φ为第三个关节坐标原点与第二个关节坐标原点、第五个关节坐标原点的连线形成的夹角；

第四个关节角通过代数方法求出：

θ4＝a tan2(c1c2-s1ax-c1c23ax,-s1c23ay-s23az)其中c1、c2、c23分别代表cosθ1、cosθ2、cos(θ2+θ3)；s1、s23分别代表sinθ1、sin(θ2+θ3)；ax、ay、az分别代表正运动学基坐标到末端执行器坐标原点变换矩阵的第三列分量；

第五个关节角通过向量点积法求出：

θ5＝arccos(N6N3)

其中N6、N3代表第六、第三关节轴的方向向量；

第六个关节角同样通过代数法求解：

θ6＝a tan2(c1s23ox+s1s23oy+c23oz,-c1s23nx-s1s23ny-c23nz)其中o和n分别代表基坐标到末端执行器坐标原点变换矩阵的第一、第二列列向量，其下标x,y,z表示列向量的分量。

2.根据权利要求1所述的基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，其特征在于：所述路径规划模块采用基于采样LazyPRM算法，LazyPRM算法首先在状态空间中通过快速随机采样来构建稀疏的roadmap，对于高维空间的路径规划，在构建完roadmap之后，在该roadmap上，进行路径求解，对于任意一条可行路径，LazyPRM会将碰撞检测推迟到求解之后进行，若发生碰撞则在附近重新规划一条新路径；由于Kinect深度摄像头的存在，状态空间会由于动态障碍物的出现而变化，其roadmap会根据下一次规划请求而更新。

3.根据权利要求1所述的基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，其特征在于：所述物体抓取模块通过5DT数据手套来实现，5DT数据手套通过传感器来将采集到的角度数据映射到真实的角度值，机械臂通过下位机控制系统的驱动，运动到抓取位姿，操作人员通过5DT数据手套来控制五指灵巧手的运动来进行物体的抓取。

4.根据权利要求1所述的基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，其特征在于：所述的动态环境检测模块，其关键部分在于实时的障碍物检测部分，通过Kinect深度摄像头，系统能够实时收集到环境中的动态信息，根据获取到的动态信息进行分析，得到动态障碍物的位置和大小信息，通过更新环境状态来将Kinect捕捉到的动态障碍物位姿记录下来。

5.根据权利要求4所述的基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，其特征在于：所述Kinect深度摄像头获取到的帧序列，通过帧差法和二值化获得运动部分点云信息，以及Canny边缘检测算子得到点云的边缘信息，并进一步计算得到运动障碍物的包围盒，其中帧差法计算两帧之差公式如下：δ(i,j)＝|q(i,j)-b(i,j)|

式中，q(i,j)表示前景帧中的像素点，b(i,j)表示背景帧中的像素点，δ(i,j)表示像素值的绝对差值，接着对所有的像素值进行求和：为了能有效的检测运动物体，消除图像噪音，必须要设置一个阈值threshold，对于任何sum>＝threshold的帧差，则表示有运动物体出现；为了能确定运动物体的尺寸和位置，使用Canny边缘检测算子对得到的点云信息进行边缘检测，再进行包围盒的计算。

6.根据权利要求1所述的基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，其特征在于：所述仿真模块的图形引擎采用VC++以及现代OpenGL图形引擎开发，实现机械臂3D模型读取、加载、坐标变换；其中，工业机械臂的建模采用3DSMAX软件完成，仿真模块的网络通信接口，基于Winsock开发，网络通信接口负责将上位机控制命令通过Socket传输给下位机控制系统，控制系统通过串口将命令发送给机械臂来驱动机械臂运动，到达指定位姿。

说明书

基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统

技术领域

[0001] 本发明涉及工业机械臂的应用领域，尤其是指一种基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统。

背景技术

[0002] 自从20世纪60年代初期第一台工业机器人出现以来，它就一直是代替流水生产线上工人的最佳手段。经过半个多世纪的发展，工业机器人已经在越来越多的生产领域中发挥了巨大的作用。但是，简单地将工业机器人直接搭载到流水生产线上去完成作业具有很大的局限性。特别是随着信息技术的发展和更新，工业机器人通过传统的离线式编程和完成固定不变作业的生产方式已经越来越不能满足现在多品种少批量的生产需要。工业机器人要感知动态环境信息必须依赖于机器视觉，在工业机械臂作业过程中，由于工业机械臂是通过离线式编程，对工作环境中的动态障碍物不能有效的识别和躲避。

[0003] 作为全球的工业机器人大国，日本对于工业机器人的研究始于1980年。在引入美国的机器人技术之后，日本很快就将工业机器人应用到以汽车工业为代表的制造业中。尤其是20世纪80年代中期到90年代中期，工业机器人应用数量迅速增长，使得日本在很长的一段时间里保持世界领先的工业机器人持有量。日本的工业机器人生产厂商主要有Fanuc、安川、那智不二越、松下、川崎等公司，这些公司都非常专注于工业机器人产品研究与应用开发。Fanuc智能化机床，所使用的上下料均采用工业机器人及其组件。德国Kuka机器人公司也是世界上顶级的工业机器人制造商之一。Kuka生产的工业机器人现在已经广泛应用于汽车、航天等领域中的装配、包装、焊接、表面修整等。基于协作的Kuka工业机器人系统已经被广泛地应用于大众汽车装配生产过程中。

[0004] 上世纪70年代初期，我国也开始了工业机器人研究，现在已经掌握了工业机器人本体设计制造技术，开发出了一大批用于点焊、装配、搬运、切割作业的工业机器人，以及双足机器人、无缆深潜机器人、遥控移动作业机器人等。尽管我国在工业机器人的结构设计、架构研发及零件制造等方面与国外产品有较大的差距，但是在智能化应用方面却与国外先进水平逐渐靠近。

[0005] 2013年4月，德国政府在汉诺威工业博览会上，正式提出了其建立高度自由的数字化和个性化产品及服务的生产模式——工业4.0战略。工业4.0战略作为第四次工业革命，其最明显的标识就是信息物理系统(Cyber-Physical System，CPS)。CPS是整个计划的基础，充分利用其进行各类信息交互，以达到将制造业向智能化转型的目的。所以实施工业4.0战略的关键原因就在于实现智能工厂，主要是实现智能化生产系统及过程的网络化分布式生产。在智能工厂中，数字世界和物理世界(包括工业机器人、输送机、仓储系统、生产设施)能实现无缝融合，其最大的特点就是制造业中所有参与者及生产资源的高度技术互动，同时将个人客户及产品的独特特性融入到产品生产的各个阶段中。作为“世界工厂”的中国随着人口红利的不断消失，以及社会发展的需求，为应对外部环境对制造业的冲击，中国政府提出了我国信息化和工业化深度融合的发展战略——中国制造2025，这也标志着中国正式开展了以中国制造2025为指导的智能化制造，中国制造2025可以说为中国工业信息化和智能化发展及经济的又一次腾飞指明了方向。

[0006] 根据中国制造2025战略，可以看出我国未来的工业发展方向主要是实现生产制造过程的智能化与生产要素的网络化。据此，本专利申请在智能制造和网络互联两个主要关注点上，提出了基于Kinect深度摄像头的一种工业机械臂动态避障规划系统：通过深度摄像头来获取实时环境的变化，通过帧差法计算得到运动物体的位置和大小，实现了机械臂在动态环境中避障规划，使得机械臂能够在物体抓取作业中具有更强的鲁棒性以及环境适应性。

[0007] 目前，随着智能技术和现代制造技术的发展，机器人出现了两个发展方向，即工业机器人和只能机器人。工业机器人的主要特点是结构集成度低，执行的工作简单固定，智能程度不高。当前工业机器人的发展已经成熟，且广泛应用于制造领域中，智能机器人是目前各国学者和研究机构重点研究的具有局部自主功能的机器人。作为仿人型机器人的末端执行机构的机器人灵巧手来说，其类人形的手指相当于一组具有多自由度，多感知能力的微小型智能机器人的组合，一方面类人形的机器人灵巧手的外形尺寸、手指结构以及手指间的位置关系都同人手类似。所以，采用数据手套，用过主从控制，将数据手套采集到的人手关节角度，映射到灵巧手的实际角度，由下位机驱动灵巧手电机，使得灵巧手运动到响应姿态。

发明内容

[0008] 本发明的目的在于克服现有工业机械臂对作业环境的自主感知能力不足，提出了一种基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统。

[0009] 为实现上述目的，本发明所提供的技术方案为：基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，包括：

[0010] 运动学模块，用于机械臂的正逆运动学求解，用户通过输入末端执行器的期望抓取位姿，通过该模块计算得到机械臂各个关节的角度值，用户也能够通过给定机械臂各个关节的角度值来计算末端执行器的位姿；

[0011] 路径规划模块，用于通过运动学模块求解得到的抓取位姿，采用基于采样的LazyPRM算法，在机械臂状态空间中采样来进行路径规划；

[0012] 物体抓取模块，用于对空间中物体的抓取，用户通过5DT数据手套来控制五指灵巧手的物体抓取作业，5DT数据手套能够通过传感器来采集用户手指关节的弯曲角度，通过关节映射来驱动五指灵巧手的电机使得五指灵巧手运动到抓取姿态；

[0013] 动态环境检测模块，用于对空间中动态环境进行跟踪检测，能够对空间中的运动障碍物进行检测和追踪，进一步反馈到路径规划模块中，为后续的路径规划提供动态障碍物的空间位置；

[0014] 仿真模块，用于对机械臂进行实时仿真，用户能够通过人机交互界面对机械臂进行运动控制。

[0015] 在所述运动学模块中，机械臂正逆运动学采用D-H矩阵进行描述，其逆运动学求解同时采用了几何方法和代数方法：

[0016] 第一个关节角通过从基坐标原点到第四个关节坐标原点的投影向量来求解：

[0017] θ1＝arctan(py/px)

[0018] 或θ1＝arctan(py/px)+π

[0019] 其中px和py分别为投影向量在基坐标系的x,y轴上的分量；

[0020] 第二个关节角度通过前五个关节的几何关系求出：

[0021] 或

[0022] 其中β1为从第二个关节坐标原点到第五个关节坐标原点形成的向量与第二个关节坐标系的y轴形成的夹角，β2为第二个关节坐标原点与第三个关节坐标原点、第五个关节坐标原点的连线形成的夹角；

[0023] 第三个关节角度通过前五个关节的几何关系求出：

[0024] θ3＝π-φ-α或θ3＝π+φ-α

[0025] 其中α为第三连杆与第三关节坐标原点到第五关节坐标原点的连线形成的夹角，φ为第三个关节坐标原点与第二个关节坐标原点、第五个关节坐标原点的连线形成的夹角；

[0026] 第四个关节角通过代数方法求出：

[0027] θ4＝atan2(c1c2-s1ax-c1c23ax,-s1c23ay-s23az)

[0028] 其中c1、c2、c23分别代表cosθ1、cosθ2、cos(θ2+θ3)；s1、s23分别代表sinθ1、sin(θ2+θ3)；ax、ay、az分别代表正运动学基坐标到末端执行器坐标原点变换矩阵的第三列分量；

[0029] 第五个关节角通过向量点积法求出：

[0030] θ5＝arccos(N6N3)

[0031] 其中N6、N3代表第六、第三关节轴的方向向量；

[0032] 第六个关节角同样通过代数法求解：

[0033] θ6＝atan2(c1s23ox+s1s23oy+c23oz,-c1s23nx-s1s23ny-c23nz)

[0034] 其中o和n分别代表基坐标到末端执行器坐标原点变换矩阵的第一、第二列列向量，其下标x,y,z表示列向量的分量。

[0035] 所述路径规划模块采用基于采样LazyPRM算法，LazyPRM算法首先在状态空间中通过快速随机采样来构建稀疏的roadmap，对于高维空间的路径规划，在构建完roadmap之后，在该roadmap上，进行路径求解，对于任意一条可行路径，LazyPRM会将碰撞检测推迟到求解之后进行，若发生碰撞则在附近重新规划一条新路径；由于Kinect深度摄像头的存在，状态空间会由于动态障碍物的出现而变化，其roadmap会根据下一次规划请求而更新。

[0036] 所述物体抓取模块通过5DT数据手套来实现，5DT数据手套通过传感器来将采集到的角度数据映射到真实的角度值，机械臂通过下位机控制系统的驱动，运动到抓取位姿，操作人员通过5DT数据手套来控制五指灵巧手的运动来进行物体的抓取。

[0037] 所述的动态环境检测模块，其关键部分在于实时的障碍物检测部分，通过Kinect深度摄像头，系统能够实时收集到环境中的动态信息，根据获取到的动态信息进行分析，得到动态障碍物的位置和大小信息，通过更新环境状态来将Kinect捕捉到的动态障碍物位姿记录下来。

[0038] 所述Kinect深度摄像头获取到的帧序列，通过帧差法和二值化获得运动部分点云信息，以及Canny边缘检测算子得到点云的边缘信息，并进一步计算得到运动障碍物的包围盒，其中帧差法计算两帧之差公式如下：

[0039] δ(i,j)＝|q(i,j)-b(i,j)|

[0040] 式中，q(i,j)表示前景帧中的像素点，b(i,j)表示背景帧中的像素点，δ(i,j)表示像素值的绝对差值，接着对所有的像素值进行求和：

[0041]

[0042] 为了能有效的检测运动物体，消除图像噪音，必须要设置一个阈值threshold，对于任何sum>＝threshold的帧差，则表示有运动物体出现；为了能确定运动物体的尺寸和位置，使用Canny边缘检测算子对得到的点云信息进行边缘检测，再进行包围盒的计算。

[0043] 所述仿真模块的图形引擎采用VC++以及现代OpenGL图形引擎开发，实现机械臂3D模型读取、加载、坐标变换；其中，工业机械臂的建模采用3DSMAX软件完成，仿真模块的网络通信接口，基于Winsock开发，网络通信接口负责将上位机控制命令通过Socket传输给下位机控制系统，控制系统通过串口将命令发送给机械臂来驱动机械臂运动，到达指定位姿。

[0044] 本发明与现有技术相比，具有如下优点与有益效果：

[0045] 1、本发明着眼于改进工业机械臂在作业过程中对工作环境的动态感知，使得工业机械臂的路径规划具有感知动态障碍物的能力。

[0046] 2、本发明结合计算机视觉、数字图像处理技术，实现工业机械臂的避障规划。

[0047] 3、本发明提出了基于数据手套的物体抓取方式以及基于Kinect的深度摄像头动态障碍物定位和追踪的方式，从而使得工业机械臂能够更加智能、合理、有效的提高作业的安全性、稳定性和鲁棒性，为未来的工业制造智能化和互联化提供了一个可行的方案。

附图说明

[0048] 图1为系统架构图。

[0049] 图2为路径规划模块的流程图。

[0050] 图3为机械臂结构图。

[0051] 图4为LazyPRM算法流程图。

[0052] 图5-1为灵巧手结构图。

[0053] 图5-2为灵巧手大拇指结构图。

[0054] 图5-3为灵巧手其余四指结构图。

具体实施方式

[0055] 下面结合具体实施例对本发明进一步说明。

[0056] 本发明所提供的基于Kinect深度摄像头的工业机械臂实时避障规划抓取系统，具体是结合了计算机视觉、数字图像处理技术，通过Kinect摄像头感知工业机械臂工作空间内的动态障碍物位置和大小信息，再利用路径规划算法进行路径规划。计算得到最优路径之后，驱动机械臂达到最佳抓取位姿，再通过5DT数据手套控制灵巧手进行抓取作业。

[0057] 系统架构如图1所示，它由5个部分构成：运动学模块、路径规划模块、物体抓取模块、动态环境检测模块以及仿真模块。5个模块在系统运行过程中相辅相成互相协作来完成机械臂在动态环境中的避障规划以及物体抓取作业，其中仿真模块采用VC++和OpenGL编写，任何机械臂的运动必须要在仿真系统中进行验证，才能驱动电机运动机械臂。

[0058] 本发明设计的运动学模块负责机械臂的正运动学求解和逆运动求解，用户通过输入末端执行器得期望抓取姿态，通过该模块计算得到机械臂各个关节的角度值。正运动学即给定机械臂各个关节变量，计算机械臂末端执行器的位置姿态；逆运动学即一直机器人末端的位置姿态，计算机器人对应位置的全部关节变量。

[0059] 如图3所示，系统中采用的工业机械臂具有6个自由度，满足Pieper准则，即最后三个机械臂的关节轴重合于一点，也即机械臂一定存在闭合逆解。对于机械臂的运动学，采用DH坐标系来建模，其中DH参数表如下：

[0060]

[0061] 从机械臂的基坐标系到末端执行器坐标系的变换可以通过六个齐次变换矩阵相乘得到：

[0062] T6＝A1A2A3A4A5A6

[0063] 对于两个相邻的坐标系之间的变换，可以通过4个基本步骤完成，首先饶Z轴旋转θ角，再平移d使得x轴共线，然后再平移a使得坐标系原点重合，最后饶x轴旋转α使得坐标系完全重合，所以齐次变换矩阵为：

[0064] Tn+1＝Rot(z,θn+1)×Tran(0,0,dn+1)×Tran(an+1,0,0)×Rot(a,αn+1)

[0065] 由于机械臂满足Pieper准则，故一定存在闭式逆解，对于逆运动学的求解，采用几何法与代数法的结合，通过几何法求解得到前4个关节的角度，通过代数法求解后两个角度的角度。

[0066] 第一个关节角通过从基坐标原点到第四个关节坐标原点的投影向量来求解：

[0067] θ1＝arctan(py/px)

[0068] 或θ1＝arctan(py/px)+π

[0069] 其中px和py分别为投影向量在基坐标系的x,y轴上的分量；

[0070] 第二个关节角度通过前五个关节的几何关系求出：

[0071] 或

[0072] 其中β1为从第二个关节坐标原点到第五个关节坐标原点形成的向量与第二个关节坐标系的y轴形成的夹角，β2为第二个关节坐标原点与第三个关节坐标原点、第五个关节坐标原点的连线形成的夹角；

[0073] 第三个关节角度通过前五个关节的几何关系求出：

[0074] θ3＝π-φ-α或θ3＝π+φ-α

[0075] 其中α为第三连杆与第三关节坐标原点到第五关节坐标原点的连线形成的夹角，φ为第三个关节坐标原点与第二个关节坐标原点、第五个关节坐标原点的连线形成的夹角；

[0076] 第四个关节角通过代数方法求出：

[0077] θ4＝atan2(c1c2-s1ax-c1c23ax,-s1c23ay-s23az)

[0078] 其中c1、c2、c23分别代表cosθ1、cosθ2、cos(θ2+θ3)；s1、s23分别代表sinθ1、sin(θ2+θ3)；ax、ay、az分别代表正运动学基坐标到末端执行器坐标原点变换矩阵的第三列分量；

[0079] 第五个关节角通过向量点积法求出：

[0080] θ5＝arccos(N6N3)

[0081] 其中N6、N3代表第六、第三关节轴的方向向量；

[0082] 第六个关节角同样通过代数法求解：

[0083] θ6＝atan2(c1s23ox+s1s23oy+c23oz,-c1s23nx-s1s23ny-c23nz)

[0084] 其中o和n分别代表基坐标到末端执行器坐标原点变换矩阵的第一、第二列列向量，其下标x,y,z表示列向量的分量。

[0085] 本发明设计的路径规划模块，如图2所示的流程，说明了路径规划模块在仿真过程中的工作流程，仿真系统首先必须经过初始化(Initialize)、装入机械臂模型以及环境信息(Load model and environment)才能进行路径求解,路径规划模块通过接收到运动学模块(Kinematics Solver)求出的逆运动学解，通过避障算法(Planner)来搜索得到一条避开所有障碍物的路径，在搜索之前，首先对目标状态和起始状态的正确性进行验证(Collision detection)，其中路径规划算法采用LazyPRM算法，该算法是一种基于采样的路径规划算法，通过在机械臂状态空间中进行采样，建立稀疏图，然后采用A*算法在稀疏图上进行搜索路径。LazyPRM算法跟普通基于采样的PRM算法相比，具有延迟碰撞检测的特点，使得机械臂规划的速度得到提升。为了满足机械臂在实时动态环境中的路径规划，路径规划模块采用了AABB碰撞检测方法来使得机械臂具有较高效的碰撞检测效率，若得到可行路径(Solved)，则进行下一步的仿真模拟(Simulate)。

[0086] 如图4所示的LazyPRM算法流程图，对于给定的两个查询位姿qinit和qgoal，即起始状态和目标状态，以及采样点个数Ninit。首先进行随机均匀采样，采样的同时，会涉及到领域选取的问题，因为每当一个采样点被加到地图上，它都要立刻和它领域中的点相连来形成路径p，这样，当Ninit个采样点都被加入到地图中的时候，包含所有边和顶点的稀疏图就生成了(Build initial roadmap)。由于工业机械臂底部关节的运动比末端运动更容易产生碰撞，因此必须要采用加权欧式距离来选取领域：

[0087]

[0088] 其中d是状态空间的维数wi为正权值。

[0089] 故对于任意两个节点对(q,q'),若pcolli(q,q')≤Rneibor则将两结点相连。一旦稀疏图构建完毕，则通过A*算法来进行最短路径的搜索(Search for a shortest path)，LazyPRM算法是一种延迟碰撞检测算法，它将碰撞检测步骤延迟到找到最短路径之后再进行检测(Chech path for collision)，如果碰撞检测失败，即路径上有障碍物，则去除掉该边(Remove colliding edge)并进行节点增强操作，结点增强操作(Node enhancement)，即在碰撞的边附近重新选择节点加入到图中，然后进行下一次最短路径的搜索最终得到无碰撞路径(Collision-free path)。

[0090] 通过路径规划模块得到的可行的路径之后通过五次多项式进行轨迹规划：

[0091]

[0092] 其中θ(t)表示关节在t时刻的角度值。边界条件已知：

[0093]

[0094] 则多项式系数可以求得：

[0095]

[0096] 通过该多项式可以拟合得到机械臂运动曲线，来实现平滑运动。

[0097] 本发明设计的动态环境检测模块，采用帧间差分法得到连续帧的差分，然后通过二值化以及阈值化处理，得到有运动物体的帧，来进行运动物体检测，再通过Canny边缘检测算法得到运动物体的轮廓信息，进一步求得运动物体的包围盒。通过Kinect深度摄像头获取到障碍物的深度信息，进一步转化为障碍物的尺寸，并将障碍物的空间信息更新到路径规划系统中。为了使用Kinect进行空间位置的获取，必须要对Kinect进行外参的标定，由于Kinect出厂之前已经进行了内参的标定，故在对精度要求不是特别高的情况下，不需要对Kinect进行内参的标定。我们采用张正友标定法，利用棋盘图案的标定板以不同的位置和姿态对Kinect进行标定，并利用MATLAB的标定工具箱完成标定过程。

[0098] 上述的帧间差分法，首先要将前景帧和背景帧转换到RGB颜色空间，再转化为灰度图，最后计算连续帧的帧间差分：

[0099] δ(i,j)＝|q(i,j)-b(i,j)|

[0100] 其中δ(i,j)为前景帧和背景帧像素之差的绝对值，q(i,j)为前景帧的像素，b(i,j)为背景帧的像素。运动物体检测必须满足帧间差分的像素之和大于给定的阈值：

[0101]

[0102] 采用Canny边缘检测算子对得到的帧间差分进行轮廓检测，将得到的轮廓像素点，通过深度摄像头获取对应的深度值，然后求得轮廓像素点对应的世界坐标围成的包围盒，最后将包围盒加入环境信息或者修改已存在的包围盒位置和姿态信息。

[0103] 本发明设计的物体抓取模块，通过5DT数据手套来实现控制五指灵巧手的物体抓取作业。如图5-1所示灵巧手的结构图，五指灵巧手一共由5根指头组成，其中大拇指和其余四肢结构略微有所不同，大拇指如图5-2所示，其余四指如图5-3所示。5DT数据手套在每一个指关节处都放置有角度传感器来进行关节角度数据的采集，5DT数据手套在使用前必须要根据用户的手掌大小和使用习惯进行标定校准，5DT数据手套自带了标定SDK，通过该SDK可以通过简单的握拳和放松动作进行数据手套的标定，再通过标定得到的数据进行线性插值计算采集到的真实值：

[0104]

[0105] 其中output为输出的校正角度值，maxval为角度传感器能够采集到的最大角度值,rawcur为当前传感器的真实值,rawmin为传感器采集范围的下界,rawmax为传感器采集范围的上界。

[0106] 本发明设计的仿真模块，分为图形引擎、机械臂运动学模块、网络通信接口、数据手套以及Kinect接口。其中图形引擎采用VC++和现代OpenGL图形引擎开发，机械臂的建模采用3DSMAX软件完成，通过图形引擎导入、加载、绘制；机械臂运动学模块负责机械臂正逆运动学求解以及坐标变换；网络通信接口负责将上位机控制命令通过Socket传输给下位机控制系统，控制系统通过串口将命令发送给机械臂来驱动机械臂运动；数据手套以及Kinect接口分别封装了5DT数据手套SDK和Kinect 2.0SDK，使得仿真模块可以读取数据手套以及Kinect传感器采集到的数据。

[0107] 以上所述实施例只为本发明之较佳实施例，并非以此限制本发明的实施范围，故凡依本发明之形状、原理所做的变化，均应涵盖在本发明的保护范围内。