您还可以使用以下方式登录
当前位置：&>&&>& > NAO机器人的视觉伺服物品抓取设计与实现
机器人视觉定位抓取 NAO机器人的视觉伺服物品抓取设计与实现
分类号：ＴＰ２４２密级：公开学号：２０１２１３０１６．∥▲东岁，番硕士学位论文ＴｈｅｓｉｓｆｏｒＭａｓｔｅｒＤｅｇｒｅｅ（专业学位）论文题目：ＮＡＯ机器人的视觉伺服物品抓取设计与实现ＤｅｓｉｇｎａｎｄＩｍｐｌｅｍｅｎｔａｔｉｏｎｏｆｖｉｓｕａｌｓｅｒｖｏｇｒａｓｐｉｎｇｏｆｈｏｕｓｅｈｏｌｄｏｂｊｅｃｔｓｆｏｒＮＡｏｒｏｂｏｔ作者姓名耋亘童、丽学院名称控制科学与工程学院专业名称控制工程指导教师职≮合作导师２０１５年５月１９日万方数据ＪＩｌｌｌｌｌＩＪＩｌｌｌｌｌｌＨＩＭｌｌＪＹ２７９１７７３原创性声明本人郑重声明：所呈交的学位论文，是本人在导师的指导下，独立进行研究所取得的成果。除文中已经注明引用的内容外，本论文不包含任何其他个人或集体已经发表或撰写过的科研成果。对本文的研究作出重要贡献的个人和集体，均已在文中以明确方式标明。本声明的法律责任由本人承担。论文作者签名：圭函日关于学位论文使用授权的声明本人完全了解山东大学有关保留、使用学位论文的规定，同意学校保留或向国家有关部门或机构送交论文的复印件和电子版，允许论文被查阅和借阅；本人授权山东大学可以将本学位论文的全部或部分内容编入有关数据库进行检索，可以采用影印、缩印或其他复制手段保存论文和汇编本学位论文。（保密论文在解密后应遵守此规定）论文作者签名：墓函导师签名：衅日期：酬万方数据山东大学硕士学位论文目录摘要……………………………………．……………………………………………．．………………………．ＩＡＢＳＴＲＡＣＴ………………………………………………………………………………………………．．ＩＩＩ第—章绪论………………………………………………………………………………………１１．１研究背景及意义………………………………………………………………１１．２相关问题的研究现状…………………………………………………………２１．２．１ＮＡＯ机器人抓取研究现状………………………………………………２１．２．２视觉测量定位研究现状…………………………………………………３１．２．３视觉伺服控制研究现状…………………………………………………４１．３本文主要研究内容……………………………………………………………５１．４论文章节安排…………………………………………………………………７第二章实验平台及ＮＡＯ机器人的建模…………………………………………………．９２．１实验平台简介…………………………………………………………………９２．１．１ＮＡＯ机器人………………………………………………………………９２．１．２Ｂｕｍｂｌｅｂｅｅ２双目视觉…………………………………………………．．１２２．２ＮＡＯ机械臂的运动学建模…………………………………………………．１３２．２．１运动学正解……………………………………………………………．．１４２．２．２运动学逆解……………………………………………………………．．１６２．３本章小结…………．ｊ…………………………………………………………１８第三章视觉测量定位…………………………………………………………………………１９３．１基于本体单目视觉的物体定位……………………………………………．１９３．１．１摄像机模型及参数标定………………………………………………。１９３．１．２基于单应矩阵分解的物品位姿估计…………………………………．．２２３．２基于双目立体视觉的物体定位……………………………………………．２６３．２．１对极几何及基本矩阵分解……………………………………………．．２６３．２．２位姿估计………………………………………………………………．．２９３．３实验结果与分析……………………………………………………………．３ｌ３．３．１单目视觉定位实验……………………………………………………．．３１３．３．２双目视觉定位实验……………………………………………………．．３２３．４本章小结……………………………………………………………………．３５第四章基于本体单目视觉的物品抓取………………………………………………。３７４．１单臂抓取的ＰＢＶＳ控制律…………………………………………………．３７万方数据山东大学硕士学位论文４．２双臂抓取的ＰＢＶＳ控制律…………………………………………………．３８４．３增加路径点的ＰＢＶＳ控制律………………………………………………．４０４．４实验结果与分析……………………………………………………………．４０４．４．１单臂抓取实验…………………………………………………………．４１４．４．２双臂抓取实验…………………………………………………………．４２４．５本章小结……………………………………………………………………．４４第五章基于双目立体视觉的物品抓取……………………………………………４５５．１视觉伺服系统标定…………………………………………………………．４６５．２静止物体抓取………………………………………………………………．４８５．２．１ＰＢＶＳ抓取控制律………………………………………………………４８５．２．２结合迭代学习控制的改进控制律……………………………………．５０５．３运动物体抓取………………………………………………………………．５１５．３．１基于扩展卡尔曼滤波的物体位姿估计与运动预测…………………．５１５．３．２ＰＢＶＳ抓取控制律………………………………………………………５３５．３．３结合迭代学习控制的改进控制律……………………………………．５４５．４实验结果与分析……………………………………………………………．５５５．４．１系统标定实验…………………………………………………………．５５５．４．２静止物品抓取实验……………………………………………………．５６５．４．３运动物品抓取实验……………………………………………………．５８５．５本章小结……………………………………………………………………．６２第六章总结与展望…………………………………………………………………６３６．１总结……………………………………………………………………………………………………６３６．２展望……………………………………………………………………………………………………６４参璃汶献………………………………………………………………………………………………………６５致谢……………………………………………………………………………………………………………７１硕士期间发表的论文和科研成果…………………………………………………．７３硕士期间参加的科研工作……………………………………………………………。７３万方数据山东大学硕士学位论文ＣｏＮＴＥＮＴＳＣｈｉｎｅｓｅＡｂｓｔｒａｃｔ……．．………………………………………．……………………．………………………．ＩＥｎｇｌｉｓｈＡｂｓｔｒａｃｔ………………………………………………．……………………．……………………ＩｌｌＣｈａｐｔｅｒ１Ｉｎｔｒｏｄｕｃｔｉｏｎ…………………………………………………………………………………．．１１．１ＢａｃｋｇｒｏｕｎｄａｎｄＳｉｇｎｉｆｉｃａｎｃｅ…………．…．……．．…．．…．．…．…．…．．………．．…．．……．．…．．１１．２ＲｅｓｅａｒｃｈＳｔａｔｕｓｏｆＲｅｌａｔｅｄＰｒｏｂｌｅｍｓ…．……．．……．……．………………．……………．…．２１．２．１ＲｅｖｉｅｗｏｆＮＡＯＲｏｂｏｔＧｒａｓｐｉｎｇ………………………………………………………．２１．２．２ＲｅｖｉｅｗｏｆＶｉｓｕａｌＭｅａｓｕｒｅｍｅｎｔａｎｄＬｏｃａｌｉｚａｔｉｏｎ…………………………………３１．２．３ＲｅｖｉｅｗｏｆＶｉｓｕａｌＳｅｒｖｏｉｎｇＣｏｎｔｒｏｌ…．……．．……．…．…．．……．……．．……．…．…．．．４１．３１１ｌｅＭａｉｎＣｏｎｔｅｎｔｏｆｔｈｅｔｈｅｓｉｓ…………………………………………………………………５１．４ＣｈａｐｔｅｒＡｒｒａｎｇｅｍｅｎｔｏｆｔｈｅｔｈｅｓｉｓ…．．………．．…．．……．………．．……．．…．．…．．…．…．…．．７Ｃｈａｐｔｅｒ２ＥｘｐｅｒｉｍｅｎｔａｌＰｌａｔｆｏｒｍａｎｄＭｏｄｅｌｉｎｇｏｆＮＡＯＲｏｂｏｔ………………………９２．１ＥｘｐｅｒｉｍｅｎｔａｌＰｌａｔｆｏｒｍ…．．…………．．…．…．……．．…．……．．…．…．…．…．…．……．．…………．９２．１．１ＮＪｚ！ＬＯＲｏｂｏｔ．．…．…．．…．…．………．．…．…．…．．…．．……．…．…．…．．……．．…．……．…．．…．．９２．】【．２Ｂｕｍｂｌｅｂｅｅ２Ｃａｍｅｒａ．…．…．．……．……．…．………………．…．…．…．…．．…．．…．…．…．１２２．２ＫｉｎｅｍａｔｉｃＭｏｄｅｌｉｎｇｏｆＮＡＯ’ＳＭａｎｉｐｕｌａｔｏｒ……………………………………………．１３２．２．１ＦｏｒｗａｒｄＫｉｎｅｍａｔｉｃｓ．…．…．………．．……．．……．．…．……．…．…．…．…．…．．……．…．…１４２．２．２ＩｎｖｅｒｓｅＫｉｎｅｍａｔｉｃｓ…………………………………………………………………………１６２．３Ｓｕｍｍａｒｙ…．．…．…．…．…．．…．．…．…．………．．………．．…．……．………．．……．．…．……．…．…．．１８Ｃｈａｐｔｅｒ３ＶｉｓｕａｌＭｅａｓｕｒｅｍｅｎｔａｎｄＬｏｃａｌｉｚａｔｉｏｎ……………………．……………………．１９３．１ＯｂｉｅｃｔＬｏｃａｌｉｚａｔｉｏｎＢａｓｅｄｏｎＮｏｕｍｅｎａｌＭｏｎｏｃｕｌａｒＶｉｓｉｏｎ………………………１９３．１．１ＣａｍｅｒａＭｏｄｅｌａｎｄＰａｒａｍｅｔｅｒＣａｌｉｂｒａｔｉｏｎ…………………………………………１９３．１．２ＯｂｊｅｃｔＰｏｓｅＥｓｔｉｍａｔｉｏｎＢａｓｅｄｏｎＨｏｍｏｇｒａｐｈｙＤｅｃｏｍｐｏｓｉｔｉｏｎ…………．．２２３．２ＯｂｉｅｃｔＬｏｃａｌｉｚａｔｉｏｎＢａｓｅｄｏｎＢｉｎｏｃｕｌａｒＳｔｅｒｅｏＶｉｓｉｏｎ………………………………２６３．２．１ＥｐｉｐｏｌａｒＧｅｏｍｅｔｒｙａｎｄＤｅｃｏｍｐｏｓｉｔｉｏｎｏｆＦｕｎｄａｍｅｎｔａｌＭａｔｒｉｘ…………。２６：；．２．２ＰｏｓｅＥｓｔｉｍａｔｉｏｎ．．…．．…．…．………．……．…．…．．…．……．…．…．……．．…．……………．．２９：；．３ＥｘｐｅｒｉｍｅｎｔａｎｄＲｅｓｕｌｔＡｎａｌｙｓｉｓ．．……．…．…．……．………．……．．……．……．……．…．…．３ｌ，３．３．１ＥｘｐｅｒｉｍｅｎｔｏｆＭｏｎｏｃｕｌａｒＶｉｓｕａｌＬｏｃａｌｉｚａｔｉｏｎ…………………………………。３１３．３．２ＥｘｐｅｒｉｍｅｎｔｏｆＳｔｅｒｅｏＶｉｓｕａｌＬｏｃａｌｉｚａｔｉｏｎ…………………………………………３２：；．４Ｓｕｍｍａｒｙ……………………………………………………………………………………………．．３５Ｃｈａｐｔｅｒ４ＮｏｕｍｅｎａｌＭｏｎｏｃｕｌａｒＶｉｓｉｏｎＢａｓｅｄＯｂｊｅｃｔＧｒａｓｐｉｎｇ……………………．３７４．１ＰＢＶＳＣｏｎｔｒｏｌＬａｗｏｆＵｎｉｍａｎｕａｌＧｒａｓｐｉｎｇ……………………………………………．３７４．２ＰＢＶＳＣｏｎｔｒｏｌＬａｗｏｆＢｉｍａｎｕａｌＧｒａｓｐｉｎｇ………………………………………………．３８４．３ＰＢＶＳＣｏｎｔｒｏｌＬａｗｏｆＡｄｄｉｎｇＰａｔｈＰｏｉｎｔｓ．……．……．…．…．…．…．…．……．…………．４０万方数据山东大学硕士学位论文４．４ＥｘｐｅｒｉｍｅｎｔａｎｄＲｅｓｕｌｔＡｎａｌｙｓｉｓ……………………………………………………………。４０４．４．１ＥｘｐｅｒｉｍｅｎｔｏｆＵｎｉｍａｎｕａｌＧｒａｓｐｉｎｇ…………………………………………………４１４．４．２ＥｘｐｅｒｉｍｅｎｔｏｆＢｉｍａｎｕａｌＧｒａｓｐｉｎｇ…………………………………………………．．４２４．５Ｓｕｍｍａｒｙ……………………………………………………………………………………………．．４４ＢａｓｅｄＣｈａｐｔｅｒ５ＢｉｎｏｃｕｌａｒＳｔｅｒｅｏＶｉｓｉｏｎＯｂｊｅｃｔＧｒａｓｐｉｎｇ……………………………?４５５．１ＣａｌｉｂｒａｔｉｏｎｏｆＶｉｓｕａｌＳｅｒｖｏＳｙｓｔｅｍ…………………………………………………………４６５．２ＳｔａｔｉｃＯｂｊｅｃｔＧｒａｓｐｉｎｇ…………………………………………………………………………．．４８ＣｏｎｔｒｏｌＬａｗｏｆＣｏｍｂｉｎｉｎｇＩｔｅｒａｔｉｖｅＬｅａｒｎｉｎｇＣｏｎｔｒｏｌ…………５０５．２．１ＰＢＶＳＣｏｎｔｒｏｌＬａｗ………………………………………………………………………。４８５．２．２５．３ＭｏｄｉｆｉｅｄＭｏｖｉｎｇＯｂｊｅｃｔＧｒａｓｐｉｎｇ………………………………………………………………………?５１５．３．１ＯｂｊｅｃｔＰｏｓｅＥｓｔｉｍａｔｉｏｎａｎｄＭｏｔｉｏｎＰｒｅｄｉｃｔｉｏｎＢａｓｅｄｏｎＥＫＦ……………．５１５．３．２ＰＢＶＳＣｏｎｔｒｏｌＬａｗ………………………………………………………………………．．５３５．３．３ＭｏｄｉｆｉｅｄＣｏｎｔｒｏｌＬａｗｏｆＣｏｍｂｉｎｉｎｇＩｔｅｒａｔｉｖｅＬｅａｒｎｉｎｇＣｏｎｔｒｏｌ…………５４ＲｅｓｕｌｔＡｎａｌｙｓｉｓ……………………………………………………………??５５５．４Ｅｘｐｅｒｉｍｅｎｔａｎｄ５．４．１ＥｘｐｅｒｉｍｅｎｔｏｆＳｙｓｔｅｍＣａｌｉｂｒａｔｉｏｎ……………………………………………………５５５．４．２ＥｘｐｅｒｉｍｅｎｔｏｆＳｔａｔｉｃＯｂｊｅｃｔＧｒａｓｐｉｎｇ………………………………………………５６５．４．３ＥｘｐｅｒｉｍｅｎｔｏｆＭｏｖｉｎｇＯｂｊｅｃｔＧｒａｓｐｉｎｇ…………………………………………．．５８５．５Ｓｕｍｍａｒｙ………………………………………………………………………………．一…………”６２Ｃｈａｐｔｅｒ６ＣｏｎｃｌｕｓｉｏｎｓａｎｄＦｕｔｕｒｅＷｏｒｋｓ………………………．．……………………………６３６．１Ｃｏｎｃｌｕｓｉｏｎｓ………………；………………………………………………………………………。６３６．２ＦｕｔｕｒｅＷｏｒｋｓ………………………………………………………………………………………．６４Ｒｅｆｅｒｅｎｃｅｓ……………………………………．……．．………………………………………………………６５Ａｃｋｏｎｗｌｅｄｇｍｅｎｔｓ………………………………………。………………………．．．．…………。………．７１ｆｏｒｔｈｅＭａｓｔｅｒａｔｅ……………………………………?７３ＰａｐｅｒｓＰｕｂｌｉｓｈｅｄＤｕｒｉｎｇＳｔｕｄｙｉｎｇＲｅｓｅａｒｃｈＰｒｏｊｅｃｔｓＰａｒｔｉｃｉｐａｔｅｄＤｕｒｉｎｇＳｔｕｄｙｉｎｇｆｏｒｔｈｅＭａｓｔｅｒａｔｅ………………??７３万方数据山东大学硕士学位论文摘要随着人口老龄化问题日益加剧，“空巢老人”不断增多，导致如何解决照顾陪伴“空巢老人”的问题成为社会难题。在日常生活中，物品的操作是最为频繁的行为之一，老年人和残疾人由于行动不便或各种其他的原因，难以完成这些日常行为，需要家庭服务机器人给予帮助，所以服务机器人不仅需要在非结构化的家庭环境中运动，更为重要的是需要具有操作能力，能够帮助人完成家庭物品的抓取和传送。本文以家庭服务机器人为用户提供家庭物品抓取和传递服务为应用背景，以仿人机器人ＮＡＯ为实验平台，为使ＮＡＯ能够更智能更自主地进行运动控制，对ＮＡＯ机器人的五自由度手臂进行运动学建模，利用ＮＡＯ的本体单目视觉和外设双目立体视觉构成视觉伺服控制系统，提出了准确的物品位姿估计算法，设计了基于位置的视觉伺服（Ｐｏｓｉｔｉｏｎ．ＢａｓｅｄＶｉｓｕａｌＳｅｒｖｏｉｎｇ，ＰＢＶＳ）控制方法，并且结合迭代学习控制改进了ＰＢＶＳ控制律，并通过实验验证提出的机器人控制方法能够使机器人很好的完成家庭物品抓取、传递任务，对机器人走入家庭为人提供服务具有重要意义。针对在机器人附近且在机器人视野范围内物品的抓取问题，基于本体单目视觉提出基于Ｎａｏｍａｒｋ标签的物品识别和定位方法，利用世界平面单应矩阵分解算法对目标物的位姿进行估计，使本体单目视觉能够对物体进行快速识别和准确定位。同时，本文设计了单臂抓取和双臂抓取的ＰＢＶＳ控制律，利用运动学逆解进行关节控制量的求解实现抓取，并提出双臂抓取在运送物品过程中需满足的位姿约束条件。为避免抓取过程中末端执行器与障碍物和目标抓取物的碰撞，对原有ＰＢＶＳ控制律引入路径规划进行了改进，通过实验验证了改进控制律的优越性。对于未贴Ｎａｏｍａｒｋ标签的物体和传递过程中运动的物体，本体视觉存在分辨率低，深度信息未知单目视觉难以进行精确定位，图像特征提取困难，难以进行快速图像处理，易产生图像动态模糊等问题。为解决这些难题以实现本体单目视觉难以准确识别与定位的物品的抓取，借助Ｂｕｍｂｌｅｂｅｅ２双目立体相机构建视觉伺服系统，通过图像处理获取物品的自然特征点，利用对极几何及本质矩阵分解和线性三角形法获取特征点的三维坐标，进而估计出摄像机坐标系与物体坐标系的相对位姿。对Ｔ万方数据山东大学硕士学位论文于传递过程中运动物体的定位问题，提出了基于扩展卡尔曼滤波的物体位姿估计与运动预测方法。针对静止物品和传递过程中运动物品的抓取问题，分别设计了李雅普诺夫渐进稳定的ＰＢＶＳ抓取控制律，但由于机械误差和物品运动预测误差的存在，导致末端执行器不能完全跟踪上期望的抓取轨迹，本文结合迭代学习控制对ＰＢＶＳ控制律进行了改进，通过迭代实验验证提出的方法能够快速校正系统误差，提高系统响应速度，使机器人能够快速、稳定的完成目标物品的抓取，实现相应的家庭服务。关键词．ＩＮＡＯ机器人；物品抓取；视觉伺服；位姿估计：迭代学习控制ｌＩ万方数据山东大学硕士学位论文ＡＢＳＴＲＡＣＴＷｉｔｈｔｈｅｉｎｃｒｅａｓｉｎｇａｇｉｎｇｐｏｐｕｌａｔｉｏｎａｎｄｔｈｅｇｒｏｗｉｎｇｎｕｍｂｅｒｏｆ”ｅｍｐｔｙ—ｎｅｓｔｅｒｓ”，ｈｏｗｔｏｃａｒｅａｎｄａｃｃｏｍｐａｎｙ”ｅｍｐｔｙ—ｎｅｓｔｅｒｓ”ｈａｓｂｅｃｏｍｅａｓｅｒｉｏｕｓｓｏｃｉａｌｐｒｏｂｌｅｍ．Ｉｎｄａｌｌｙｌｉｆｅ，ｔｈｅｏｐｅｒａｔｉｏｎｏｆｈｏｕｓｅｈｏｌｄｏｂｊｅｃｔｓｉｓｏｎｅｏｆｔｈｅｍｏｓｔｆｒｅｑｕｅｎｔｂｅｈａｖｉｏｒ，ｔｈｅｅｌｄｅｒｌｙａｎｄｔｈｅｄｉｓａｂｌｅｄａｒｅｈａｒｄｔｏａｃｃｏｍｐｌｉｓｈｔｈｅｓｅｄａｌｌｙａｃｔｓｄｕｅｔｏｌｉｍｉｔｅｄｍｏｂｉｌｉｔｙｏｒｍａｎｙｏｔｈｅｒｒｅａｓｏｎｓ，ｔｈｅｙｎｅｅｄｈｏｍｅｓｅｒｖｉｃｅｒｏｂｏｔｓｆｏｒｈｅｌｐ．Ｓｏｎｏｔｏｎｌｙｍｏｖｉｎｇｉｎｕｎｓｔｒｕｃｔｕｒｅｄｈｏｍｅｅｎｖｉｒｏｎｍｅｎｔ，ｉｔｉｓｍｏｒｅｉｍｐｏｒｔａｎｔｆｏｒｔｈｅｓｅｒｖｉｃｅｒｏｂｏｔｓｔｏｈａｖｅｔｈｅａｂｉｌｉｔｙｏｆｏｐｅｒａｔｉｏｎ，ｔｏｈｅｌｐｐｅｏｐｌｅａｃｃｏｍｐｌｉｓｈｔｈｅｈａｎｄｌｉｎｇａｎｄｄｅｌｉｖｅｒｉｎｇｏｆｈｏｕｓｅｈｏｌｄｏｂｊｅｃｔｓ．Ｔｈｅｔｈｅｓｉｓｔａｋｅｓｈｏｍｅｓｅｒｖｉｃｅｒｏｂｏｔｓｐｒｏｖｉｄｉｎｇｓｅｒｖｉｃｅｓｏｆｈａｎｄｌｉｎｇａｎｄｄｅｌｉｖｅｒｉｎｇｈｏｕｓｅｈｏｌｄｏｂｊｅｃｔｓａｓａｐｐｌｉｃａｔｉｏｎｂａｃｋｇｒｏｕｎｄ，ａｐｐｌｉｅｓＮＡＯｈｕｍａｎｏｉｄｒｏｂｏｔａｓｅｘｐｅｒｉｍｅｎｔａｌｐｌａｔｆｏｒｍ．ＴｏｅｎａｂｌｅＮＡＯｔｏｐｒｏｃｅｅｄｓｍａｒｔｅｒａｎｄｍｏｒｅａｕｔｏｎｏｍｏｕｓｍｏｔｉｏｎｃｏｎｔｒｏｌ，ｔｈｅｋｉｎｅｍａｔｉｃｓｍｏｄｅｌｏｆｔｈｅＮＡＯ’Ｓｆｉｖｅｄｅｇｒｅｅｓｏｆｆｒｅｅｄｏｍａｒｉａｉｓｅｓｔａｂｌｉｓｈｅｄ．１１１ｅｎｏｕｍｅｎａｌｍｏｎｏｃｕｌａｒｖｉｓｉｏｎａｎｄｂｉｎｏｃｕｌａｒｓｔｅｒｅｏｖｉｓｉｏｎａｒｅｕｓｅｄｔｏｃｏｎｓｔｉｔｕｔｅｖｉｓｕａｌｓｅｒｖｏｃｏｎｔｒｏｌｓｙｓｔｅｍ，ａｃｃｕｒａｔｅｐｏｓｅｅｓｔｉｍａｔｉｏｎａｌｇｏｒｉｔｈｍｉｓｐｒｏｐｏｓｅｄａｎｄｐｏｓｉｔｉｏｎ—ｂａｓｅｄｖｉｓｕａｌｓｅｒｖｏｉｎｇ（ＰＢＶＳ）ｃｏｎｔｒｏｌｍｅｔｈｏｄｉｓｄｅｓｉｇｎｅｄａｎｄｉｔｅｒａｔｉｖｅｌｅａｒｎｉｎｇｃｏｎｔｒｏｌｉｓａｐｐｌｉｅｄｔｏｉｍｐｒｏｖｅｔｈｅＰＢＶＳｃｏｎｔｒｏｌｌａｗ．Ｅｘｐｅｒｉｍｅｎｔｓｖｅｒｉｆｙｔｈａｔｔｈｅｐｒｏｐｏｓｅｄｍｅｔｈｏｄｅｎａｂｌｅｓｔｈｅｒｏｂｏｔｔｏａｃｃｏｍｐｌｉｓｈｔｈｅｔａｓｋｏｆｈａｎｄｌｉｎｇａｎｄｄｅｌｉｖｅｒｉｎｇｈｏｕｓｅｈｏｌｄｏｂｊｅｃｔｓ，ｗｈｉｃｈｉｓｏｆｇｒｅａｔｓｉｇｎｉｆｉｃａｎｃｅｆｏｒｔｈｅｒｏｂｏｔｓｔｏｐｒｏｖｉｄｅｈｏｍｅｓｅｒｖｉｃｅｓｆｏｒｐｅｏｐｌｅ．Ｔｏｇｒａｓｐｔｈｅｏｂｊｅｃｔｓａｒｏｕｎｄｔｈｅｒｏｂｏｔａｎｄｉｎｔｈｅｓｉｇｈｔｏｆｔｈｅｒｏｂｏｔ，ｔｈｅｒａｐｉｄｉｄｅｎｔｉｆｉｃａｔｉｏｎａｎｄｌｏｃａｌｉｚａｔｉｏｎｏｆｏｂｊｅｃｔｉｓａｃｈｉｅｖｅｄｂｙｕｓｉｎｇＮａｏｍａｒｋｌａｂｅｌｂａｓｅｄｏｎｎｏｕｍｅｎａｌｍｏｎｏｃｕｌａｒｖｉｓｉｏｎａｎｄｔｈｅｐｏｓｉｔｉｏｎａｎｄｏｒｉｅｎｔａｔｉｏｎｉｎｆｏｎｎａｔｉｏｎｏｆｏｂｊｅｃｔｉｓｅｓｔｉｍａｔｅｄｔｈｒｏｕｇｈｔｈｅｗｏｒｌｄｈｏｍｏｇｒａｐｈｙｍａｔｒｉｘｄｅｃｏｍｐｏｓｉｔｉｏｎｒａｐｉｄｌｙａｎｄａｃｃｕｒａｔｅｌｙ．Ａｔｔｈｅｓａｍｅｔｉｍｅ，ｔｈｅＰＢＶＳｃｏｎｔｒｏｌｌａｗｉｓｄｅｓｉｇｎｅｄｆｏｒｅｉｔｈｅｒｕｎｉｍａｎｕａｌｏｒｂｉｍａｎｕａｌｇｒａｓｐｉｎｇ，ｂｙｕｓｉｎｇｉｎｖｅｒｓｅｋｉｎｅｍａｔｉｃｓｔｏａｃｑｕｉｒｅｔｈｅｊｏｉｎｔｃｏｎｔｒｏｌａｎｇｌｅｏｆｒｅａｌｉｚｉｎｇｏｂｊｅｃｔｇｒａｓｐｉｎｇ．ＡｎｄｔｈｅｐｏｓｅｃｏｎｓｔｒａｉｎｔｉｓｐｒｏｐｏｓｅｄｄｕｒｉｎｇｔｈｅｐｒｏｃｅｓｓｏｆｔｒａｎｓｐｏｒｔｉｎｇｏｂｊｅｃｔｓＩＩＩ万方数据山东大学硕士学位论文ｂｉｍａｎｕａｌｌｙ．Ｔｏａｖｏｉｄｔｈｅｃｏｌｌｉｓｉｏｎｏｆｔｈｅｅｎｄ－ｅｆｆｅｃｔｏｒｗｉｔｌｌｔｈｅｏｂｓｔａｃｌｅｓａｎｄｔｈｅｔａｒｇｅｔｏｂｊｅｃｔｄｕｒｉｎｇｔｈｅｇｒａｓｐｉｎｇｐｒｏｃｅｓＳ，ｐａｔｈｐｌａｎｎｉｎｇｏｆｔｈｅｅｎｄ－ｅｆｆｅｃｔｏｒｉｓａｄｄｅｄｔｏｔｈｅｏｒｉｇｉｎａｌｃｏｎｔｒｏｌｌａｗ．ＥｘｐｅｒｉｍｅｎｔａｌｒｅｓｕｌｔｓｓｈｏｗｔｈｅｓｕｐｅｒｉｏｒｉｔｙｏｆｉｍｐｒｏｖｅｄＰＢＶＳｃｏｎｔｒｏｌＦｏｒｔｈｅｏｂｊｅｃｔｓｗｉｔｈｏｕｔＮａｏｍａｒｋｌａｂｅｌａｎｄｔｈｅｍｏｖｉｎｇｏｂｊｅｃｔｓｄｕｒｉｎｇｄｅｌｉｖｅｒｙ，ｔｈｅｎｏｕｍｅｎａｌｍｏｎｏｃｕｌａｒｖｉｓｉｏｎｉｓｈａｒｄｔｏｒｅｃｏｇｎｉｚｅａｎｄｌｏｃａｔｅｔｈｅｍａｃｃｕｒａｔｅｌｙｄｕｅｔｏｔｈｅｌｏｗｒｅｓｏｌｕｔｉｏｎ，ｔｈｅｕｎｋｎｏｗｎｄｅｐｔｈｉｎｆｏｒｍａｔｉｏｎ，ｄｉｆｆｉｃｕｌｔｉｅｓｏｆｉｍａｇｅｆｅａｔｕｒｅｅｘｔｒａｃｔｉｏｎａｎｄｆａｓｔｉｍａｇｅｐｒｏｃｅｓｓｉｎｇ，ｍｏｔｉｏｎｂｌｕｒ．Ｔｏａｄｄｒｅｓｓｔｈｅｓｅｃｈａｌｌｅｎｇｅｓｏｆｇｒａｓｐｉｎｇｔｈｅｏｂｊｅｃｔｓｗｈｉｃｈｉｓｄｉｆｆｉｃｕｌｔｔｏｒｅａｌｉｚｅｂａｓｅｏｎｎｏｕｍｅｎａｌｍｏｎｏｃｕｌａｒｖｉｓｉｏｎ，ｔｈｅｔｈｅｓｉｓｕｓｅｓｂｉｎｏｃｕｌａｒｓｔｅｒｅｏｃａｍｅｒａＢｕｍｂｌｅｂｅｅ２ｔｏｃｏｎｓｔｉｔｕｔｅｖｉｓｕａｌｓｅｒｖｏｓｙｓｔｅｍ．Ｔｈｅｒａｐｉｄｉｄｅｎｔｉｆｉｃａｔｉｏｎｏｆｏｂｊｅｃｔｉｓａｃｈｉｅｖｅｄｔｈｒｏｕｇｈｎａｔｕｒａｌｆｅａｔｕｒｅｅｘｔｒａｃｔｉｏｎｂｙｉｍａｇｅｐｒｏｃｅｓｓｉｎｇ，ｔｈｅｐｏｓｅｏｆｏｂｊｅｃｔｉｓｅｓｔｉｍａｔｅｄｂｙｅｐｉｐｏｌａｒｇｅｏｍｅｔｒｙａｎｄｆｕｎｄａｍｅｎｔａｌｍａｔｒｉｘｄｅｃｏｍｐｏｓｉｔｉｏｎ．Ｔｈｅ３Ｄｃｏｏｒｄｉｎａｔｅｓｏｆｆｅａｔｕｒｅｐｏｉｎｔｓａｌｅａｃｑｕｉｒｅｄｂｙｌｉｎｅａｒｔｒｉａｎｇｕｌａｒｍｅｔｈｏｄ，ｔｈｅｎｔｈｅｒｅｌａｔｉｖｅｐｏｓｉｔｉｏｎａｎｄｏｒｉｅｎｔａｔｉｏｎｏｆｔｈｅｃａｍｅｒａｃｏｏｒｄｉｎａｔｅｓｙｓｔｅｍａｎｄｔｈｅｏｂｊｅｃｔｃｏｏｒｄｉｎａｔｅｓｙｓｔｅｍｉｓｃａｌｃｕｌａｔｅｄ．Ｔｏｓｏｌｖｅｔｈｅｌｏｃａｌｉｚａｔｉｏｎｐｒｏｂｌｅｍｏｆｍｏｖｉｎｇｏｂｊｅｃｔｓｄｕｒｉｎｇｄｅｌｉｖｅｒｙｐｒｏｃｅｓｓ，ａｎｅｘｔｅｎｄｅｄＫａｌｍａｎｆｉｌｔｅｒｂａｓｅｄｏｂｊｅｃｔｐｏｓｅｅｓｔｉｍａｔｉｏｎａｎｄｍｏｔｉｏｎｐｒｅｄｉｃｔｉｏｎｍｅｔｈｏｄｉｓｐｒｏｐｏｓｅｄ．ＴｈｅＰＢＶＳＬｙａｐｕｎｏｖａｓｙｍｐｔｏｔｉｃｓｔａｂｌｅｃｏｎｔｒｏｌｌａｗｉｓｄｅｓｉｇｎｅｄｆｏｒｅｉｔｈｅｒｓｔａｔｉｃｏｂｊｅｃｔｇｒａｓｐｉｎｇｏｒｍｏｖｉｎｇｏｂｊｅｃｔｇｒａｓｐｉｎｇ．Ｈｏｗｅｖｅｒ，ｔｈｅｐｒｅｓｅｎｃｅｏｆｍｅｃｈａｎｉｃａｌｅｒｒｏｒａｎｄｏｂｊｅｃｔｍｏｔｉｏｎｐｒｅｄｉｃｔｉｏｎｅｒｒｏｒｍａｋｅｓｔｈｅｅｎｄ－ｅｆｆｅｃｔｏｒｈａｒｄｔｏｃｏｍｐｌｅｔｅｌｙｔｒａｃｋｔｈｅｄｅｓｉｒｅｄｇｒａｓｐｉｎｇｔｒａｊｅｃｔｏｒｙ．ＩｔｅｒａｔｉｖｅｌｅａｒｎｉｎｇｃｏｎｔｒｏｌｉｓａｄｄｅｄｔｏＰＢＶＳｃｏｎｔｒｏｌｌａｗｔｏｉｍｐｒｏｖｅｔｈｅｐｅｒｆｏｒｍａｎｃｅｏｆｔｈｅｓｅｒｖｏｓｙｓｔｅｍ．ＩｔｅｒａｔｉｖｅｅｘｐｅｒｉｍｅｎｔｓｖｅｒｉｆｙｔｈａｔｔｈｅｉｍｐｒｏｖｅｄＰＢＶＳｃｏｎｔｒｏｌｌａｗＣａｎｃｏｒｒｅｃｔｅｒｒｏｒｓｑｕｉｃｋｌｙａｎｄｉｍｐｒｏｖｅｔｈｅｒｅｓｐｏｎｓｅｓｐｅｅｄｏｆｓｙｓｔｅｍ．Ｔｈｅｐｒｏｐｏｓｅｄｍｅｔｈｏｄｃａｎｈｅｌｐｔｈｅｒｏｂｏｔａｃｈｉｅｖｅｏｂｊｅｃｔｇｒａｓｐｉｎｇａｎｄｄｅｌｉｖｅｒｙｒａｐｉｄｌｙａｎｄｓｔａｂｌｙ，ａｃｃｏｍｐｌｉｓｈｈｏｍｅｓｅｒｖｉｃｅｓ．Ｋｅｙｗｏｒｄｓ：ＮＡＯＲｏｂｏｔ；ＯｂｊｅｃｔＧｒａｓｐｉｎｇ；ＶｉｓｕａｌＳｅｒｖｏｉｎｇ；ＰｏｓｅＥｓｔｉｍａｔｉｏｎ；ＩｔｅｒａｔｉｖｅＬｅａｍｉｎｇＣｏｎｔｒｏｌ；ＩＶ万方数据山东大学硕士学位论文第一章绪论１．１研究背景及意义人口老龄化已经成为世界性难题，而中国的老龄化现象尤为严重。国家统计局在２０１５年２月公布上一年度社会发展与国民经济统计数据显示，截止２０１４年末我国６０周岁及以上的入口占总入口的１５．５％，达到２１２４１万。６５周岁及以上的人口突破１０％，占总人口的１０．１％，达到１３７５５万，可见我国已进入老年型社会，人口老龄化不可避免。据全国老龄办统计数据显示，中国“空巢老人”占老年人口比例接近５０％，去年突破１亿人，“空巢老人”中许多存在孤独、抑郁甚至厌世等心理问题，导致如何解决照顾陪伴“空巢老人”的问题成为社会难题。因此，家庭服务陪伴机器人研究和开发具有重大的社会价值和意义。随着服务机器人逐渐进入家庭，服务机器人不仅需要在非结构化的家庭环境中运动，更为重要的是需要具有操作能力，能够帮助人完成家庭物品的抓取和传送。世界各国日益加剧的人口老龄化问题是推动家庭服务机器人操作能力研究的主要原因之一。老年人由于各个器官老化，导致视力下降、反应变慢、记忆力减退、动作不灵活，甚至患有或大或小的疾病，其子女在外工作无暇照顾，导致他们的生活存在困难。除此之外，我国还有大量的残障人士，他们由于先天或者后天导致身体某些方面的残疾，甚至生活不能自理，急需能够帮助他们的服务机器人。在日常生活中，物品的操作是最为频繁的行为之一，老年人和残疾人由于行动不便或各种其他的原因，难以完成这些日常行为，需要家庭服务机器人给予帮助，所以机器人抓取技术的研究是服务机器人开发急需解决的问题。●为使机器人迅速准确地完成家庭环境中物体的抓取任务，最有效的途径是给服务机器人机械臂赋予视觉能力以构成视觉伺服系统。不同于机器视觉，视觉伺服是以实现对机器人的控制为目的而进行的图像自动获取与分析【ｌＪ，实现机器人的视觉伺服系统涉及到控制理论、图像处理、机器视觉、运动学、动力学、实时数学计算等多方面内容，因此实现机器人的视觉伺服控制的难度很大，是机器人学中的一个万方数据山东大学硕士学位论文十分具有挑战性的课题吲。本文以为用户提供家庭物品抓取和传递服务为应用背景，以仿人机器人ＮＡＯ为实验平台，针对机器人抓取中机器人建模、视觉测量定位、视觉伺服控制等相关问题展开研究，不仅使机器人能够准确快速智能的完成指定任务，而且对服务机器人的应用和发展具有重要意义。１．２相关问题的研究现状１．２．１ＮＡＯ机器人抓取研究现状家庭服务机器人是工作于家庭环境中，能够为人提供各种家庭服务的智能机器人。其中人形机器人以其亲和拟人的外表和高自由度、具有双手协调操作等有利条件以及能够完成各种类人的动作、实现各种服务任务等优点，正成为当代服务机器人的一个重点研究领域。ＮＡＯ机器人是一款智能的仿人机器人，目前，国外已经有相关的ＮＡＯ机器人抓取的研究。Ｌｏｕｌｏｕｄｉ“３】等在智能家居环境下，ＮＡＯ接受人的服务指令利用分布式视觉获取的位置信息导航行进至指定处，通过预设的固定动作对目标物品进行抓取和递送。ＭｕｌｌｅｒＪ【４】等将ＮＡＯ机器人上下分布的摄像头改装为左右分布的双目视觉，通过颜色分割和边缘检测等从图像中分割出杯子，建立杯子的３Ｄ模型，提出基于Ａ｝算法的无碰撞路径规划算法完成对杯子的抓取（如图１—１所示）。ＭＯＵＧＨＬＢＡＹＡＡ［５】等设计了两种新型误差校正策略来加快任务的执行速度，根据误差的大小设置不同的误差收敛指数，实现了特定物品的快速抓取，但未考虑末端执行器和物品的碰撞情况，导致实际抓取能力受到限制。ＢｕｒｇｅｔＦ【６】等利用快速扩展随机树（Ｒａｐｉｄｌｙ－ｅｘｐｌｏｒｉｎｇＲａｎｄｏｍＴｒｅｅ，Ⅺ盯）算法进行无碰撞的路径和姿态规划，建立ＮＡＯ机器人操作物品运动过程中保持平衡的各姿态数据库，在机器人操作系统（ＲｏｂｏｔＯｐｅｒａｔｉｏｎＳｙｓｔｅｍ，ＲＯＳ）下进行研究开发，将Ｋｉｎｅｃｔ固定于ＮＡＯ头部（如图１．２所示）辅助机器人获取物品及环境的三维信息，从数据库选取相应的机器人运动姿态完成物品的操作。ｖａｎｄｅｒＷａｌＥ【７】提出机器学习的方法，对比了神经网络、Ｋ．近邻对抓取示教数据的离线训练结果和增强学习的在线训练结果，得出增强学习是实现抓取最优万方数据山东大学硕士学位论文的学习方法，通过学习和探索运动空间得到的最优抓取策略和轨迹，进而控制ＮＡＯ机器人完成物品抓取。ＭｅｌｌｍａｎｎＨ【８１等提出了一种适应性的双臂抓取算法，但由于算法只约束了左右机械臂的末端执行器与物体质心的距离和相对位置，导致实际抓取的应用受到一定限制。图１．１ＮＡＯ机器人规划抓取杯子的路径图１．２ＮＡＯ机器人抓取和投递物品１．２．２视觉测量定位研究现状视觉测量定位是机器人视觉控制的重要研究内容，也是实现视觉控制的基础。视觉测量主要是研究从二维图像信息到二维或三维笛卡尔空间信息的映射以及视觉测量系统的构成【９】，即三维重建（３ＤＲｅｃｏｎｓｔｒｕｃｔｉｏｎ）问题。针对三维重建过程中涉及的特征提取、图像匹配、模型识别、射影重构等图像处理与空间几何恢复的问题，ＬｏｗｅＤ等【ｌｏ】提出了在二维灰度图像中三维物品的识别方法，基于物品三维模型的先验知识，不通过深度信息重建的方式，而是利用空间对应关系和线一线对应关系、以及对应关系的空间不变性，利用这些关系和性质进行拍摄的二维图像与物品三维模型的特征匹配完成图像中物品识别；ＺｈａｎｇＺ等【１１】针对未标定的图像，图像的相对运动和摄像机参数均未知，利用极线约束基于最小中值估计方法提出鲁棒的图像匹配算法，由计算出的基础矩阵筛选符合条件的匹配点对，实现准确的图像匹配；ＨａｒｔｌｅｙＲＩ等【１２】全面分析了根据若干幅景物图像恢复三维景物空间结构的方法，首先介绍了射影几何、变换和估计的基础知识，接着讨论了摄像机几何和单视图几何，然后对两视图几何中的对极几何和基本矩阵、摄像机矩阵恢复、平面单应等进行了万方数据山东大学硕士学位论文阐述，最后扩展到三视图几何和Ｎ视图几何的情况，从理论和实现详细剖析了多视图几何的三维重构问题。对于视觉伺服控制系统中位姿（位置与旋转）估计的问题，ＭａｒｉｏｔｔｉｎｉＧＬ等【１３】针对非完整移动机器人基于图像的视觉伺服控制中位姿估计问题，基于针孔摄像机提出基于图像特征点提取和对极几何的位姿估计算法，不依赖于景物先验的三维模型知识，控制移动机器人由初始位姿运动至期望位姿；ＣｈｏｉＣ等【１４ｌ提出用于视觉伺服的实时物品三维位姿估计方法，同时适用于单目相机和双Ｅｌ立体相机，使用ＳＩＦＴ（ＳｃａｌｅＩｎｖａｒｉａｎｔＦｅａｔｕｒｅＴｒａｎｓｆｏｒｍ）算法进行自然特征的提取与匹配，结合ＫＬＴ（Ｋａｎｄｅ．Ｌｕｃａｓ—Ｔｏｍａｓｉ）目标跟踪算法对固定特征点进行实时跟踪，并通过实验验证提出算法的有效性，且双目立体相机的定位效果优于单目相机；Ｊａｎａｂｉ．ＳｈａｒｉｆｉＦ等‘１５】针对机器人视觉伺服中位姿估计问题，改进了扩展卡尔曼滤波（ＥｘｔｅｎｄｅｄＫａｌｍａｎＦｉｌｔｅｒ，ＥＫＦ）算法，提出迭代自适应扩展卡尔曼滤波（ｉｔｅｒａｔｉｖｅａｄａｐｔｉｖｅＥＫＦ，ＩＡＥＫＦ），通过实验对比ＥＫＦ、白适应扩展卡尔曼滤波（ａｄａｐｔｉｖｅＥＫＦ，ＡＥＫＦ）、迭代扩展卡尔曼滤波（ｉｔｅｒａｔｉｖｅＥＫＦ，ＩＥＫＦ）、ＩＡＥＫＦ的位姿估计结果，得出ＩＡＥＫＦ取得的效果最好，能够消除机械噪声，解决错误的位姿初始化、非线性、快速轨迹跟踪等问题。１．２．３视觉伺服控制研究现状家庭物品的抓取和传送是服务机器人的基本功能，如何获取物体相对于机器人的位姿，并据此控制和设定机器人的位姿是要解决的核心问题‘１６】。而视觉伺服则是解决这一核心问题的有效途径。机器人视觉伺服系统，按照状态反馈信号的不同，主要分为基于位置的视觉伺服控制（ｐｏｓｉｔｉｏｎｂａｓｅｄｖｉｓｕａｌｓｅｒｖｏ，ＰＢＶＳ）［１７－１９１、基于图像的视觉伺服控制（ｉｍａｇｅｂａｓｅｄｖｉｓｕａｌｓｅｒｖｏ，ＩＢＶＳ）［２０－２１１和混合视觉伺服（ｈｙｂｒｉｄｖｉｓｕａｌｓｅｒｖｏ，ＨＹＢＶＳ）［２２－２４】等３类。ＰＢＶＳ在任务空间中以三维笛卡尔坐标的形式定义反馈误差信号，其基本原理是根据提取的图像特征，由已知目标物的几何模型和摄像机模型，在三维直角坐标系中对目标物品和末端执行器相对于摄像机的位姿进行定位估计，然后根据机械臂末端执行器当前位姿与目标期望位姿之间的误差，进行轨迹规划并计算出控制量，４万方数据山东大学硕士学位论文驱动末端执行器向目标物品运动，最终实现抓取功能；ＩＢＶＳ的误差反馈信号直接用二维图像特征的误差，其基本原理是由二维图像误差信号计算出控制量，通过计算图像雅克比矩阵将控制量转换得到机器人运动空间的控制量，从而控制机械臂末端执行器向目标物品运动，完成相应的伺服任务；ＨＹＢＶＳ的反馈信号既有用任务空间中的三维笛卡尔坐标形式表示，也有用图像特征表示。混合视觉伺服控制既能像ＩＢＶＳ避免机器人运动学建模误差和摄像机标定误差对系统控制精度产生的影响，又可以像ＰＢＶＳ有效地将目标物体保持在观测视野之内，避免进入图像雅克比矩阵的奇异点。因ＰＢＶＳ直接在笛卡尔空间下进行位姿估计和运动控制，具有很好的直观性和简单有效性，故被大多数系统所采用。为解决ＰＢＶＳ控制中物体的位姿估计问题，ＶＡＲＧＡＳＭ提出了一种解析式的单应矩阵分解的算法【２５１。为解决ＰＢＶＳ过程中目标ＤＨ、ＴＨＵＩＬＯＴＢ等人根据实时计算出的物脱离视野导致伺服失败的问题，ＰＡＲＫ目标物位姿设计出摄像机的跟踪轨迹，从而保证目标物体时刻在视野中口６２７１。为解决伺服过程中目标物被遮挡而无法完整成像、机械臂与障碍物发生碰撞、机械臂关节角度限制等问题，ＣＨＥＳＩＧ等人提出将末端执行器的路径规划与视觉伺服相结合的处理方法１２８捌。针对运动物体的跟踪和抓取问题，ＪｉａｎｇＰ【３∞１１等提出迭代学习控制结合基于图像的视觉伺服完成机械臂对示教的离散轨迹的跟踪，在每步迭代中不断逼近未知且变化的图像雅克比矩阵。ＧｅＬｔ３２１等提出了一种实时视觉伺服控制平行算法，物体的识别是通过快速模板匹配实现，利用卡尔曼滤波进行位置和速度的估计。１．３本文主要研究内容针对目前ＮＡＯ机器人物品抓取研究存在的一些问题与不足，本文以基于位置的视觉伺服为主要的控制方法，基于ＮＡＯ机器人的本体视觉与双目立体视觉，分别设计了具有鲁棒性的抓取控制算法，围绕机器人抓取中物品位姿估计、机器人运动控制、视觉伺服控制、抓取路径规划等问题展开研究和实验。论文的研究工作主要包括以下几个方面：万方数据山东大学硕士学位论文（１）机器人的运动学建模。针对ＮＡＯ机械臂的运动控制问题，目前大部分研究是使用示教的方式建立ＮＡＯ机器人的各种姿势动作数据库，根据要完成的动作从数据库中选取相应的动作样例，但如果在建立的姿态库中没有对应的动作样例，将导致运动控制失败。本文为使ＮＡＯ能够更智能更自主地进行运动规划，提出对ＮＡＯ机器人的五自由度手臂进行运动学建模，推导出运动学正逆解和机器人雅克比矩阵，根据给定的期望位姿计算得到各关节自由度的角度控制量，从而对ＮＡＯ进行精确、稳定的运动控制。（２）物品位姿估计算法的设计与实现。为使本体单目视觉能够对物体进行快速识别和准确定位，本文提出基于Ｎａｏｍａｒｋ标签的物品识别和定位方法，利用世界平面单应矩阵分解算法对目标物的位姿进行估计。但对于未贴Ｎａｏｍａｒｋ标签的物体和传递过程中运动的物体，本体视觉存在分辨率低，深度信息未知单目视觉难以进行精确定位，图像特征提取困难，难以进行快速图像处理，易产生图像动态模糊等问题。为解决这些难题，本文借助Ｂｕｍｂｌｅｂｅｅ２双目立体相机，通过图像处理获取物品的特征点，利用对极几何及本质矩阵分解和线性三角形法获取特征点的三维坐标，进而估计出摄像机矩阵与物体坐标系的相对位姿。对于运动物体的定位问题，提出了基于扩展卡尔曼滤波的物体位姿估计与运动预测方法。（３）基于本体单目视觉物品抓取方法的设计与实现。在机器人附近且在机器人视野范围内物品，基于本体单目视觉分别设计了单臂抓取和双臂抓取的ＰＢＶＳ控制律，利用运动学逆解进行关节控制量的求解实现抓取，并提出双臂抓取在运送物品过程中需满足的位姿约束条件；目前大部分的ＰＢＶＳ研究只关注末端执行器初始位姿与期望位姿，未考虑抓取过程中末端执行器与障碍物和目标抓取物的碰撞，本文引入路径规划，对原有ＰＢＶＳ算法进行改进，通过实验验证了改进控制律的优越性。（４）基于双目立体视觉物品抓取方法的设计与实现。为解决本体单目视觉对物品识别和定位存在的限制，结合Ｂｕｍｂｌｅｂｅｅ２双目相机构建视觉伺服系统，以实现本体单目视觉难以准确识别与定位的物品的抓取。针对静止物品和传递过程中的运动物品，分别设计了李雅普诺夫渐进稳定的ＰＢＶＳ抓取控制律，但由于机械误差和物品运动预测误差的存在，导致末端执行器不能完全跟踪上期望的抓取轨迹，本文结合迭代学习控制对ＰＢＶＳ控制律进行了改进，通过迭代实验验证提出的方法能够６万方数据山东大学硕士学位论文快速校正系统误差，提高系统响应速度，使机器人能够快速、稳定的完成目标物品的抓取。１．４论文章节安排本文的章节内容安排如下：第一章：绪论。首先描述了课题的研究背景，并介绍了课题的研究意义。针对ＮＡＯ机器人抓取、视觉测量定位和视觉伺服控制的国内外研究现状进行了阐述，并给出本文解决的问题、主要研究内容和论文的结构安排。第二章：实验平台及ＮＡＯ机器人的建模。介绍了本课题的实验平台ＮＡＯ机器人和Ｂｕｍｂｌｅｂｅｅ２双目视觉，并对ＮＡＯ机器人的机械臂进行了运动学建模和正逆解的数学分析。第三章：视觉测量定位。首先介绍了摄像机模型和摄像机参数的标定方法，基于本体单目视觉辅助Ｎａｏｍａｒｋ标签提出了世界平面单应矩阵分解的位姿估计算法，基于双目立体视觉提出了基于对极几何及基本矩阵分解的位姿估计算法，并通过实验分别验证了定位算法的有效性。第四章：基于本体单目视觉的物品抓取。设计了单臂和双臂的ＰＢＶＳ抓取算法，同时，提出了双臂抓取在物品运送过程中的约束条件。为避免末端执行器与障碍物和目标抓取物的碰撞，对ＰＢＶＳ控制律进行了增加路径点的改进，通过抓取实验对传统控制律和改进控制律进行了误差分析。第五章：基于双目立体视觉的物品抓取。针对本体单目视觉物品定位的限制，利用双目立体视觉构成视觉伺服系统，并提出了系统标定的算法，分别设计了静止物品抓取和传递过程中运动物品抓取的ＰＢＶＳ控制律。为减小系统误差，改善控制系统性能，进一步结合迭代学习控制改善控制律，通过实验结果表明该方法可以有效提高控制精度和视觉伺服系统的鲁棒性。第六章：总结与展望。对本文的研究内容进行了总结，并分析了现技术手段存在的问题，对进一步的研究进行了展望与讨论。万方数据８万方数据山东大学硕士学位论文山东大学硕士学位论文第二章实验平台及ＮＡＯ机器人的建模作为一款可自主编程机器人，ＮＡＯ机器人被广泛应用到定位导航、运动控制和人工智能等研究领域。本章将详细介绍实验平台ＮＡＯ机器人的软件框架、动作模块和视觉模块，以及Ｂｕｍｂｌｅｂｅｅ２双目相机的性能数据参数。针对ＮＡＯ机器人的物品抓取服务任务，对其机械臂进行运动学建模与正逆解的数学分析，为视觉伺服控制的实现奠定了基础。２．１实验平台简介２．１．１ＮＡＯ机器人本课题使用的是Ｖ３．２版本的ＮＡＯ机器人，全身具有２５个自由度，可完成各种仿人动作，因此ＮＡＯ被机器人世界杯ＲｏｂｏＣｕｐ选定为竞赛标准平台，高自由度为其完成类似物品抓取传递等家庭服务任务提供了可能。ＮＡＯ机器人全身自由度分布如图２．１所示。零一：善Ｉ囊鬻蠹纛ｊ；４”８鬻毳薹懑％纛兰＝．｜三兰爱鬟添篾Ａｌｄｅｒｂａｒａｎ：：ｊ一＿Ｉｉ肇攀誊魏潮黪蕊黼黝《。９。”图２－１ＮＡＯ机器人的全身自由度分布ｉ踽氅蒸缫纛；Ｒｏｂｏｔｉｃｓ公司【３３’３卅为ＮＡＯ机器人配置１００多个传感器，如摄像头、麦克风、红外、声呐、惯性单元等。ＮＡＯ各个传感器分布如图２．２所示。完备的各种传感器使ＮＡＯ具备声音识别、视觉识别与定位、探测障碍物和发光二极管灯光９万方数据山东大学硕士学位论文效果等功能，为服务任务的获取、人机交互、家庭环境的导航和机器人的运动控制提供了条件。触觉传摩器扬声器［ｘ２ｌ红外发射与接收器头部关节摄像头Ｉｘ２ｌ侧面麦克风Ｉｘ２ｌ肩关节前后麦克风胸部按钮声呐Ｉｘ２ｌ肘关节电池髓关节可抓握的手肮关节触觉传巷器膝关节联关节扬声器【ｘ２】图２－２ＮＡＯ机器人的传感器分布（１）软件框架zkq
ＮＡＯ的处理器是ｘ８６的ＡＭＤＧＥＯＤＥ主频为５００ＭＨｚ的ＣＰＵ，ＮＡＯ的操作系统是嵌入式Ｌｉｎｕｘ。ＮＡ０ｑｉ是运行在ＮＡＯ机器人上的总体软件框架，控制机器人实现音频、视频、动作等不同模块之间的信息传递与共享。ＮＡ０ｑｉ是一个跨平台软件框架，提供了各种模块的应用程序接口函数，支持多种语言Ｃ＋＋、Ｐｙｔｈｏｎ、．Ｎｅｔ、Ｊａｖａ、Ｕｒｂｉ等编程调用，同时支持各种扩展开发包，例如０ｐｅｎＣＶ进行图像处理。通过无线网络连接ＮＡ０，可在Ｗｉｎｄｏｗｓ、Ｌｉｎｕｘ或ＭａｃＯＳ等不同操作系统下进行远程在线或离线编程控制，实现相应的服务功能需求。Ｃｈｏｒｅｇｒａｐｈｅ［３５１是ＮＡＯ机器人的可视化图形编程界面，其中每个指令盒保存了机器人某项的行为，根据事件的逻辑关系连接指令盒的输入点和输出点，从而可控制机器人完成一项整体的服务任务，对比于单纯的代码编程控制Ｃｈｏｒｅｇｒａｐｈｅ为使用者提供了更直观的方式。Ｍｏｎｉｔｏｒ用于获取机器人摄像头拍摄到的图像和视频数据，管理机器人的摄像头设定参数。Ｗｅｂｏｔｓ模拟器提供各种仿真场景，且能够与Ｃｈｏｒｅｇｒａｐｈｅ连接进行机器人在仿真场景中的控制，例如视觉模块、动作模块等的控ｌＯ万方数据山东大学硕士学位论文制，如图２．３所示。图２－３Ｗｅｂｏｔｓ中ＮＡＯ机器人的场景动作仿真（２）动作模块ＮＡＯ的动作模块主要包括关节控制和笛卡尔空间控制两种方式。关节控制（Ｊｏｉｎｔｃｏｎｔｒ０１）是通过控制ＮＡＯ机器人各自由度对应的电机，使各自由度达到指zkq
定位置从而使ＮＡＯ完成指定动作。笛卡尔空间控制（Ｃａｒｔｅｓｉａｎｃｏｎｔｒ０１）是直接对ＮＡＯ的各末端执行器（如图２－４所示）进行三维空间位置与姿态的控制，但任意指定的末端执行器位姿可能存在达不到无解的情况，所以要完成精确的动作控制，只能使用建立运动学模型进行具体各关节控制的方式。图２．４ＮＡＯ机器人的各末端执行器万方数据山东大学硕士学位论文（３）视觉模块ＮＡＯ机器人的两个摄像头安装在面部，它们具有相同的参数。图２．５为摄像机的位置分布及参数，摄像头的分辨率为６４０×４８０，上、下摄像头的视角范围均为３４．８０。由于两摄像头之间没有重叠区域，所以ＮＡＯ机器人的本体视觉是单目视觉。表２．１为摄像头的具体参数。图２－５ＮＡＯ摄像头分布及参数表２－１摄像头参数摄像头型号zkq
０Ｖ７６７０摄像头输出ＶＧＡ＠３０ｆｐｓｆＹｌ．『Ｖ４２２ｃｏｌｏｒｓｐａｃｅ）视野范围５８０ＤＦＯＶ（４７．８０ＨＦＯＶ，３４．８０ＶＦＯＶ）聚焦范围３０ｃｍ—。ｉｎｆ＇ｍｉｔｙ聚焦类型Ｆｉｘｅｄｆｏｃｕｓ２．１．２Ｂｕｍｂｌｅｂｅｅ２双目视觉由于ＮＡＯ机器人的两摄像头之间没有重叠区域，利用本体视觉测量定位的原理是单目视觉定位，无法获取深度信息，导致结果不准确且误差大。所以要完成更精确的物品定位与抓取，本课题借助Ｔ拿大Ｐｏｉｎｔｇｒａｙ公司的Ｂｕｍｂｌｅｂｅｅ２双目视觉（如图２－６所示）构建视觉伺服系统，相机的图像扫描处理速率为平均４８帧／秒。Ｐｏｉｎｔｇｒａｙ公司提供的程序开发包编写了图像采集程序，使图像采集功能集成到应用程序中。Ｂｕｍｂｌｅｂｅｅ２相机可以快速获取图像和抓拍，并同时获得目标运动或静止物１’万方数据山东大学硕士学位论文体在右摄像机坐标系中的３Ｄ坐标，并将这些数据与信息通过１３９４接口传送给主控计算机。Ｂｕｍｂｌｅｂｅｅ２６４０ｘ４８０ａｔ４８ＦＰＳｏｒ１０２叙７６８ａｔ２０ＦＰＳ图２－６Ｂｕｍｂｌｅｂｅｅ２双目相机Ｂｕｍｂｌｅｂｅｅ２相机的主要性能指标如下：．ＳＯＮＹ１／３”逐行扫描传感器；?分辨率６４０×４８０像素，４８ＦＰＳ：１０２４×７６８像素，１８ＦＰＳ；?基线距离１２ｃｍ：?用于传输图像数据的接口为６针的ＩＥＥＥ．１３９４ａ接口；?触发方式为ＤＣＡＭｖｌ．３ｌ触发方式０，１，３和１４。zkq
２．２ＮＡＯ机械臂的运动学建模为完成对ＮＡＯ手臂末端执行器的精确控制，需要对ＮＡＯ机器人的机械臂进行运动学建模，获取其精确的运动学参数。图２．７是ＮＡＯ左手臂各自由度的活动角度范围。驴ＬＳｈｏｕｌｄｅｔＰｉｔｃｈ磨－１０４，５＂图２．７ＮＡＯ手臂各自由度的角度范围ＮＡＯ手臂具有５个自由度，其结构图如图２．８所示。肩膀处有俯仰和横滚两个万方数据山东大学硕士学位论文自由度，肘部具有偏转和横滚两个自由度，腕部具有偏转一个自由度。除此之外，手爪具有张合一个自由度。图２－９为建立的ＮＡＯ手臂的各连杆坐标系。图２－８ＮＡＯ手臂的结构图图２－９ＮＡＯ手臂的各连杆坐标系根据建立的各连杆坐标系和ＮＡＯ机器人手臂的各参数，可以得到如表２—２所示ＮＡＯ手臂的ＭｏｄｉｆｉｅｄＤｅｎａｖｉｔ．Ｈａｎｅｎｂｅｒｇ方法规定的连杆参数冈。由此可确定ＮＡＯ手臂的运动学模型。表２－２ＮＡＯ手臂的连杆参数zkq
其中，ａ、口、ｄ、ｐ分别为各连杆的扭角、长度、偏置和转角。２．２．１运动学正解运动学正解是根据建立的连杆坐标系，已知各连杆的位移或旋转，确定末端执行器坐标系｛Ｐ）相对基坐标系｛０）的位姿。相邻两连杆坐标系之间的变换矩阵通式为：ｃｏｓｏｊ——ｓｉｎＯｉ０ｑ一１’１Ｚ＝ｓｉｎＯｉｃｏｓＯ，ＣＯＳ６ｔｉ一１一ｓｉｎ呸一１一Ｚｓｉｎｏｌｆ—ｌ（２－１）ｓｉｎ０，ｃｏｓＯ，ｓｉｎａｉ—ｌＣＯＳ呸一１Ｚｃｏｓｔｚ，一ｌ０Ｏ０ｌ根据建立好的ＮＡＯ手臂连杆坐标系和得到的Ｄ．Ｈ参数带入变换矩阵通式可得到１４万方数据山东大学硕士学位论文各连杆间的齐次变换矩阵：『ｑ１０Ｏｑ２０Ｏｌ。ｚ：Ｉ西ｑ００ｏｏ１０１１０ｏｌＯ，?五：ｌｌ乞Ｏ０Ｉ１Ｉ哪２吖２０ｏ０１ｌ０００１，孕睢－－Ｓ３㈡０，Ｌ【１０００１ｊ毕慝Ｏ０ｌ岛一Ｓ５００咄０００ｑＯＯＯＯ１，｝雠瓣５５将以上连杆齐次变换矩阵依次相乘，可得到。疋：ｏＴｅ＝ｏ互１正２五３Ｔ４４瓦（２?２）记ｎｘｏｘｎｘｐｘｏｒｅ＝ｎｙｏｙａｙｐｙ（２—３）ｎｚｏｚｎｚｐｚzkq
００Ｏ仇＝ｃ５（ｃ４（ｓｌ墨＋ＣＬＣ２Ｃ３）一ＣｌＳ２Ｓ４）＂１＂Ｓ５（ｃ３ｓ１一ＣＩＣ２邑）胛。＝一Ｃｓ（ｑ（ｑｓ３一Ｃ２Ｃ３Ｓ１）＋ＳＩＳ２Ｓ４）一Ｓｓ（ｑＣ３＋ｃ２Ｓ１Ｓ３）ｎｚ＝ｓ２￥３Ｓ５一Ｃ５（Ｃ２Ｓ４＋Ｃ３Ｃ４Ｓ２）Ｏｘ＝Ｃ５（ｃ３ｓｌ—ｑｃ２Ｓ３）一ｓｓ（ｃ４（＿ｓ３＋ＣｌＣ２Ｃ３）一ＣｌＳ２Ｓ４）Ｏｙ＝Ｓ５（Ｃ４（ＣｌＳ３一Ｃ２Ｃ３Ｓ１）＂１－墨ｓ２３＇４）－－Ｃ５（ＣｌＣ３＋Ｃ２ＳＩＳ３）０：＝ｓ５（Ｃ２Ｓ４＋Ｃ３Ｃ４Ｓ２）＋Ｃ５Ｓ２Ｓ３（２－４）ａｘ＝一Ｓ４（Ｓ１Ｓ３＋ｑｃ２ｃ３）一ＣｌＣ４Ｓ２ａｙ＝Ｓ４（ＣｌＳ３——Ｃ２Ｃ３Ｓ１）—‘Ｃ４ＳＩＳ２ｎｚ２ｃ３Ｓ２Ｓ４一ｃ≯４见＝一Ｏ．０９ｑｓ２—０．０５０５５（Ｓ４（ＳＩＳ３＋ｑｃ２ｃ３）一ＣｌＣ４Ｓ２）Ｐｙ＝０．０５０５５（ｓ４（ｃ１岛一ｃ２巳■）一ｃ４＆是）一Ｏ．０９ｓｌｓ２ｐｚ＝０．０５０５５（Ｃ３Ｓ２Ｓ４一ｃ２ｃ４）一０．０％２其中，引用了简写符号Ｃｌ＝ｃｏｓＯｌ，ｓｌ＝ｓｉｎＯｌ等。根据建立的ＮＡ０机械臂运动学模型，利用机器人工具箱‘３７１仿真出ＮＡＯ手臂的连杆模型，如图２—１０所示。１５万方数据山东大学硕士学位论文Ｏ．１Ｏ．０５Ｎ０”．０．０５－０—ＹＸ图２．１０ＮＡＯ手臂的连杆模型２．２．２运动学逆解运动学逆解‘３８１是根据已给定的满足工作要求的末端执行器相对参考坐标系的位置和姿态，求各关节的运动参数。如（２—３）和（２．４）所示，（２－３）中各元素已知，需求解（２—４）中的各关节变量目１，如，仍，以，０５。直接求解很困难，因此，可将。疋左乘。五一１把变量ｐ１分离出来：一ｃ５（ｓ２＆一ｃ２ｃ３ｃ４）一Ｃ２Ｓ３Ｓ５Ｓ５（Ｓ２Ｓ４一ｃ２ｃ３ｃ４）一Ｃ２Ｃ５Ｓ３一Ｃ４Ｓ２一Ｃ２Ｃ３Ｓ４一ｃ３Ｓ５一ｃ一５Ｓ３ｃ４Ｓ３ｓｓ—ｃ３ｃ５Ｓ３ｓ４ｓ２邑墨一巳（乞ｓ４＂－［－Ｃ３Ｃ４Ｓ２）Ｓ５（Ｃ２Ｓ４＋乞气是）＋６屯岛ｃ３ｓ２Ｓ４一ｃ２ｃ４０００（２－５）一Ｏ．０９ｓ２—０．０５０５５（ｃ４ｓ２＂－ＦＣ２Ｃ３Ｓ４）０．０５０５５Ｓ３Ｓ４—０．０９ｃ２＋０．０５０５５（Ｃ３Ｓ２Ｓ４一巳ｑ）１Ｃｌ吃－Ｉ－Ｓ１ＦｌｙＣｔｏｘ＋ＳｌＯｙＣｌｑ＋Ｓ１ａｙｃＩＰｘ＋Ｓ１Ｐｙ－－Ｓ１唿＋Ｃ１ｌ＇ｌｙ—ｓｐｘ－ｔ－ＣｌＯｙ—ｓｐｘ七ｃｌａｙ—Ｓｌｐｘ＋ｃｌＰｙ（２—６）吃ｏ２哆ｐｚ０Ｏ０ｌ令（２．５）与（２．６）中各对应元素相等，有如下等式：１６万方数据山东大学硕士学位论文ｌ—咯ｌｑ＋ｃｌ口ｙ＝ｓ３ｓ４＜【飞Ｂ＋ｃｌ岛２０．０５０５５ｓ３ｓ４求解可得：岛＝Ａｔａｎ２（ｐｙ一０．０５０５５ａｙ，ｎ一０．０５０５５ａ，）同样，将。疋左乘。巧１把变量口ｌ，眈分离出来，进而解出如，岛，０４，侠：ｃ３ｃ４ｃ５一邑黾一ｃ５ｓ３一ｃ３ｃ４ｓ５一ｃ３ｓ４—０．０５０５５ｃ３ｓ４Ｉ一巳竺一巳５一Ｓｃ４ｃ５ｓ３．０＋ｃ４ｓ３ｓ５４ｃ哪！５…ｃ４一ｃ３ｃ５５０５伽ｓ３Ｓ４０ｎｕ．０５０５５蚋０９１０Ｉ００１Ｉｑｃ２ｎ，＋乞墨ｂ一是他ｑｃ２０ｘ＋Ｃ２Ｓ１０Ｐ—Ｓｐｚｃｆ２ｎｘ＋Ｃ２Ｓｌａｙ—ｓ２ｎｚ一ｑｓ２ｎｊ—ＳＩＳ２ｒｌｙ—ｃ２他—ＣＩＳ２０Ｊ—ｓｌｓ２０ｙ—Ｃ２０２—ＣｌＳ２ａｘ—Ｓｌｓ２ａｙ—ｃ２ｎｚ一ｓｌ恢＋ｃｌｂ一ｓｐｘ＋ｑｏｙ—ｓｐｘ＋ｑａｙ０ＯＯｑｃ２ｐ，七ｃ２Ｓ、ｐｙ—ｓ２ｐＺ—ｑｓ２ｐｘ—ＳｌＳ２Ｐｙ—ｃ２ＰＺ—ｓｔｐｘ＋ｃ、ｐｙ１令（２—９）与（２－１０）ｑｂ各对应元素相等，则有如下等式：Ｉｑｃ２吱＋ｃ２Ｓｌａｙ—ｓ２哆＝飞＆【ｑｃ２Ｐ：，＋ｃｊ■Ｂ一?Ｓ１２见＝－０．０５０５５ｑ＆≮够：：０二２ｉ现｛－ｃｌ篙三嚣篡硝邓啪糍少，嘭一１以哆弘一一＆＾由（２－１１）求解可得：岛＝Ａｔａｎ２（Ｇ（０．０５０５５ａ；一见）＋量（ｏ．０５０５５ａｙ—Ｐｙ），０．０５０５５ａ：一见）将（２．１２）两式相除得：万方数据（２—７）（２－８）（２－９）（２—１０）（２－１１）（２－１２）（２—１３）（２—１４）（２－１５）山东大学硕士学位论文０３＝Ａｔａｎ２（ｓｌｑ—Ｃｌａｙ，ＣｌＣ２ｄｘ＋ＣＥＳｌａｙ—ｓ２ａ：）（２－１６）将（２－１３）两式相除得：幺＝Ａｔａｎ２（ｓｌａ＊一ｑａｙ，ｑｓ２邑ｑ＋ＳｉＳ２Ｓ３ａｙ＋Ｃ２Ｓ３Ｇ：）（２—１７）将（２－１４）两式相除得：岛＝Ａｔａｎ２（一ｑｓ２ｑ一曲曼嘭一Ｃ２０ｚ，Ｃｌｓ２ｎｘ＋ｓｌｓ２刀ｙ＋ｃ２％）（２—１８）至此，式（２．８）．（２．１８）给出了全部的五组解析解。２．２．２．２数值解２．２．２．１节给出ＴＮＡＯ机器人手臂各连杆间的齐次变换矩阵，据此可依次求得‘疋，ｉ＝１，２…５，记ＩｌｎｘｌＱｘＬＯｘｐｘ。Ｔ卜‘口‘口‘Ｐ］ｌＬ２ｌｎｙａｙ‘Ｏｙ‘Ｐｙ０００１ｌ２ｌ（２－１９）ｔｎｚｌｎ：ｌｏｚｐｚＯＯＯ１对于转动关节ｉ＝１，２，３，４，５而言：。‘＝［（。ｐ幽）：（７ｐ巾）：（‘ｐ×ｆ口）：ｎｚ哆心］Ｔ（２－２０）则８．，＝［。以。五。以。‘８以］为机械臂末端执行器相对于末端执行器坐标系｛Ｐ）的速度与关节角速度间的线性映射关系的雅克比矩阵，是关于关节角的函数：计坝拼（２－２１）２．３本章小结本章主要介绍了实验所用到的平台仿人机器人ＮＡＯ和Ｂｕｍｂｌｅｂｅｅ２双目视觉，对ＮＡＯ机器人的自由度分布、传感器参数和软件框架，以及双目视觉的性能参数进行了介绍。同时，对ＮＡＯ机器人的手臂进行了运动学建模，从运动学正解和逆解进行了数学分析，为家庭环境下机器人进行物品抓取服务的实现奠定了基础。万方数据山东大学硕士学位论文第三章视觉测量定位视觉测量定位是实现视觉控制的基础，由摄像机获取的景物图像恢复其与摄像机的相对位姿和景物的３Ｄ结构问题，是基于位置的视觉伺服所必需的关键环节。而摄像机标定是视觉测量定位的前提，本章首先介绍了摄像机模型和摄像机参数的标定方法。然后针对基于本体单目视觉和基于双目立体视觉两种不同情况，分别介绍了相应的物品位姿估计算法，并分别进行了定位实验，验证所提出算法的有效性，对实验结果进行了详细分析和说明。３．１基于本体单目视觉的物体定位３．１．１摄像机模型及参数标定所有景物通过摄像机光轴中心点投射到成像平面上的摄像机模型称为针孔模型［３９１。假设摄像机坐标系的原点Ｃ位于投影中心，／是摄像机的焦距，则平面Ｚ＝ｆ图像平面。在针孔摄像机模型下，空间坐标点Ｘ＝（置Ｌｚ）Ｔ被映射到图像平面上的一点，该图像上的点是连接点Ｘ与投影中心的直线与图像平面的交点，如图３．１所示。根据相似三角形原理，可得到点Ｘ被映射到图像平面上点的坐标为（／ｘｌｚ，∥／ｚ，厂）７。省略最后一个图像坐标，得到点从世界坐标到图像坐标的中心投影关系是：（置Ｌｚ）１专（∥／ｚ，ｆｉ＇ｌＺ）１（３－１）投影中心即为摄像机中心，也就是摄像机镜头的光心。摄像机中心到图像平面的垂线称为摄像机的主轴或主射线，而主轴与图像平面的交点Ｐ称为主点。如果用齐次坐标表示世界和图像点，那么中心投影可以简单地表示成齐次坐标之间的线性映射。式（３—１）可写成如下矩阵乘积形式：１９万方数据山东大学硕士学位论文Ｘｘ】，Ｚ１料［厂厂。ｏ］】，Ｚ（３—２）１ＹＺＣ图３—１针孔摄像机模型式（３—２）中的矩阵可写成ｄｉａｇ（／，ｆ，１）【１１０】，世界点ｘ用４维齐次坐标（ｘ，Ｙ，Ｚ，１）Ｔ表示，图像点ｘ表示成３维齐次坐标形式，则ｘ＝ＰＸ（３—３）式（３．３）中，Ｐ为中心投影的针孔模型的摄像机矩阵：Ｐ－－ｄｉａｇ（ｆ，ｆ，１）［Ｉ０】（３－４）式（３—１）假定图像平面的坐标原点在主点上，但实际情况一般存在主点偏置，因此一般情形的映射为（ｘ，】，，ｚ）Ｔ专（弘／ｚ＋见，ｆＹ／Ｚ＋ｐｙ）１（３－５）其中慨瑚Ｔ是实际主点的坐标，该映射用齐次坐标可以表示成ｘＸ】，专吲＝㈠ＰＸ０１】厂ＺＺ（３?６）１１肛［厂Ｐｘ］记ｆＰｙＩ（３－７）１ＪＩ则（３—６）可表示为ｘ＝ｇ［ＩＩｏ】ｋ（３－８）２０万方数据山东大学硕士学位论文矩阵置称为摄像机的内参数矩阵，可通过棋盘标定获得。Ｋ锄是点Ｘ在摄像机坐标系下的笛卡尔坐标。一般情况下，空问中的点可用不同的笛卡尔坐标系（世界坐标系）表示，各坐标系之间通过旋转和平移相互转换，如图３．２所示，如果贾是世界坐标系下一点的３维非齐次坐标，而疋。是该点在摄像机坐标系下表示的同一点，那么≈＝足（文一ｅ），其中ｅ表示摄像机中心在世界坐标系下的坐标，Ｒ是一个３×３的旋转变换矩阵，则这个方程表示成齐次坐标的形式ｘ瓦＝［Ｚ１】，Ｚ＝［＿ｅ卜ｐ９，ｌ与（３—８）结合得到ｘ＝皿Ｉ广＿１ＩＩ—ｃＩｘ（３—１０）其中Ｘ用世界坐标系表示，为了方便，把世界到图像的变换表示成屯。＝武＋ｆ，摄像机矩阵简化成Ｐ＝置陋Ｉｆ】，其中ｆ＝趣ｅ，变换矩阵【胄｜ｆ】称为摄像机的外参数。图３－２世界和摄像机坐标系的相对位姿本文采用张正友【４０】提出的非线性模型摄像机的线性标定算法进行摄像机标定，获取摄像机的内参数模型。标定过程中棋盘角点检测如图３．３所示，摄像机与棋盘的相对位姿如图３．４所示。２ｌ万方数据山东大学硕士学位论文∞伽伽抛瑚一∞Ｅ喜ｉｕ！一ｕ卜娜伽伽００２００３００４００ｆｒａｍｅ）５００６００沁仰ｃａｍｅｒａ置＝［７５９．≯５９．并筹吲ｋ［４０１．一∽≥攀刁ｊ‘。ｎ＝［３７。’ｉ６４７３７８．÷５３９主三：：毛；兰雪三］３．１．２基于单应矩阵分解的物品位姿估计Ｂ㈣ｐ㈦ｃ３－?３，为方便ＮＡＯ机器人本体单目摄像头能够实现被操作目标物品的快速识别和准确的位姿估计，避免产生较大误差，通过在具有不同形状和特征的各类物品上布置已知尺寸的不同Ｎａｏｍａｒｋ标签，从而解决了家庭环境中物品种类繁多、操作方式多样复杂而造成识别区分定位困难的问题【４ｌ也】。如图３．５所示，Ｎａｏｍａｒｋ是由黑色圆圈及以圆心为中心打开的白色扇叶组成的人工地标。其中心的数字代表了Ｎａｏｍａｒｋ的ＩＤ用于标识不同种类的物品，以确定对应的抓取方式。万方数据山东大学硕士学位论文图３．５不同的Ｎａｏｍａｒｋ对获取的目标物品图像进行边缘检测和Ｈｏｕｇｈ直线拟合获取Ｎａｏｍａｒｋ的各特征点，进而识别出Ｎａｏｍａｒｋ的ＩＤ，如图３－６所示。（ａ）咖啡盒（ｂ）积木图３－６从目标物体的图像中检测出Ｎａｏｍａｒｋ由于Ｎａｏｍａｒｋ标签各特征点共面，取Ｚ＝０的平面作为物体坐标系的ｘｙ平面，如图３．７所示。设Ｎａｏｍａｒｋ标签的各特征点在物体坐标系下的三维坐标为。Ｐ＝『－。ｘ，。】Ｊ：，０，１］１，ｉ＝１，２，…，各特征点在摄像机坐标系｛ｃ）中的齐次图像坐标为。辟＝ｈ，ｖ，１】１，ｉ＝１，２，…，由射影成像模型可得，图３．７平面上点的透视图像ｑ。Ｐｉ＝研。ＲｏＩ。ｆｏ】。只（３－１４）２３万方数据山东大学硕士学位论文其中，毋为特征点在摄像机的成像深度，Ｋ为摄像机内参数矩阵，［。ｇｏＩ。ｔｏ］为３．１．１节中所介绍的摄像机外参数，即物体坐标系｛Ｄ＞相对于摄像机坐标系｛ｃ）的旋转记。Ｒ＝［。ｒｌ，ｃ眨，。‘ב吒］，则式（３－１４）可写为ａｌｊｆ—ｌ。ｊＩ＝＝ｃ。，ｊ。，ｊ。ｆ，［。手］ｃ３－?５，记物体特征点在摄像机坐标系｛ｃ）中的图像射影坐标为。慨＝Ｋ－１Ｃｐｉ＝［。誓。咒１］Ｔ（３－１６）记奢＝［置ｚ１】Ｔ，则日＝［。吒。吒。ｔ］（３－１７）为世界平面的欧氏单应阵ｍ４５１，它表征如下的线性变换：ｑ。啦＝越（３—１８）ＩＩＨ，ＩＩＦ：ｌ（３－１９）ｔＴ，Ｃｍ，＝％日霉（３－２０）问题转换为已知。鸭和孝，在（３—１９）的约束下求日’。为消去单目相机无法获取的尺度因子ｑ和口，可让式（３—２０）作矢量叉乘：ｑ。幌×（％日雾）＝０（３－２１）０Ｔ一户？ｒ。只ｐ？ｒ户ｊ０Ｔ一。一户？ｒ—ｃｙｌｔｃｘ？礼０１［善；］＝４厅’＝。ｃ３－２２，万方数据山东大学硕士学位论文４，∈Ｒ３删，则彳＝［４Ｔ，４Ｔ，Ａ／，４Ｔ］Ｔ，Ｈ’的Ｆ一范数为１意味着渺０：＝１，因此，式（３．２２）的解ｈ’可以由直接线性变换法（ＤＬＴ）解出，即ｈ’为方阵ＡＴＡ的最小特征值对应的单位特征矢量。解出口’后，记为日’＝【口ｂｃ】。根据式（３－１７）、（３－１９）ｎ－１９｝ＩＩ，ａｂｃ】＿吐。ｒｌ。吒。ｔ］（３－２３）对式（３－２３）前两列列矢量分别取２．范数，则一华ｐ２４，将式（３—２４）代入式（３－１９），可求得世界平面的欧氏单应为Ｈ：上日，（３．２５）式（３—２５）求解出的Ｈ有两种可能解，但Ｈ必须满足约束：特征点的三维坐标珥一定位于摄像机的前方。在此约束条件下，可消除日的二义性，得到符合约束条件的唯一日。在式（３—２０）０７，０＂ｉ为摄像机｛ｃ＞图像平面前方的３Ｄ点只的成像深度，因此，目标物体坐标系｛Ｄ）的原点必然位于｛ｃ）的前方才能保证约束的成立。将户＝【ｏ０１】代／Ｋ（３—１８），并根据（３—１９）可得：ｑ：绣，：堕（３．２６）其中，魄，，ｋ分别是欧氏单应阵Ｈ，归一化欧氏单应阵Ｈ’的最后一个元素。欲使驴ｏ，ｎ－Ｉ＇ｌｌ（３．２４）中的仅为正值，∥，，为正值。这意味着ｈ’取方阵４■的最小特征值对应的单位特征向量，且ｈ’最后一个元素取正值。由式（３．２２）、（３—２４）、（３—２５）得到了世界平面的单应日，继而根据式（３．１７）可以求得摄像机外参数［。ＲＩ。乞］＝［。‘，。吒，。，ｉ×。眨，。ｆ０］。图像噪声和Ｎａｏｍａｒｋ特征点提取误差的存在，导致上述恢复出的。Ｒ不是一个严格的旋转变换矩阵，因此，对。咒进行奇异值分解。ｇｏ＝ＵＤＶＴ，则Ｒ＝ＵＶＴ是满足Ⅱ乒锄盈一。兄ＩｌＦ｝条件的旋转矩阵。万方数据山东大学硕士学位论文３．２基于双目立体视觉的物体定位３．２．１对极几何及基本矩阵分解虽然３．１节针对本体单目视觉提出了基于世界平面单应矩阵分解这种相对准确的定位算法，但是Ｎａｏｍａｒｋ标签只能在近距离时进行识别，较远距离时很难实现准确的识别与定位。除此之外，单目视觉定位由于深度信息未知，分辨率低使得图像特征提取不准确等诸多因素导致难以实现精确的定位，因此本文借助Ｂｕｍｂｌｅｂｅｅ２双目相机来进行位姿估计，以实现更准确的视觉控制。下面将介绍双目视觉定位相关对极几何的原理与算法。定义两摄像机中心的连线为基线，以基线为轴的平面簇与图像平面的交的几何是两幅视图之间的对极几何Ｈ６】。对极几何是表示两幅视图之间内在联系的射影几何，如图３．８所示。图３－８对极几何示意图图中Ｃ和Ｃ分别表示两摄像机的中心，中心前方的平面分别是两摄像机的成像平面。摄像机中心Ｃ和Ｃ、３维空间点Ｘ和它的图像点柳ｘ’都位于对极平面７ｃ上。图像点Ｘ反向投影成３维空间的一条射线，它由第一个摄像机中心ｃ和ｘ确定，射线Ｃｘ在第二幅视图中被影像成一条直线，’。投影Ｎｘ的３维空间点Ｘ必然在射线Ｃｘ上，因雌第二幅视图上的图像点Ｘ’必然在Ｚ’上。摄像机基线与每幅图像平面交于对极点Ｐ和ｅ’。任何包含基线的平面是一张对极平面，并且与图像平面交于相应的对极线琊Ｚ’。当３Ｄ空间点ｘ位置变化时，该对极平面绕基线旋转，形成的平面簇称为对极平面束，所２６万方数据山东大学硕士学位论文有对极线相交于对极点。（１）基本矩阵与本质矩阵基本矩阵是对极几何的代数表示，由图３—８可看出对于一副图像上的点ｘ，在另一副图像中存在一条与之对应的对极线≠’。在第二幅图像上与该点Ｘ匹配的点Ｘ’必然在对极线，’上。该对极线是过点Ｘ与第一个摄像机中心Ｃ的射线在第二幅图像上的投影。因此存在一个从一个图像上的点到另一幅图像上与之对应的对极线的映射ｘＨＪ『’（３—２７）用基本矩阵朦示这个从点到直线的射影映射。给定图像点ｘ’，通过ｘ’和对极点Ｐ’的对极线｜ｆ’可记为，’－－ｅ’×ｘ’－－［ｅ’Ｌｘ’，ｐ’】ｘ表示三维向量ｅ’的反对称矩阵。ｘ’可记为ｘｔ－致ｘ，以是从一副图像到另一幅图像通过任意平面兀的转移映射，则有萝’＝ｐ’ＬＩ坚．ｘ＝ｒｘＬ（３—２８）得出基本矩阵Ｆ＝ｋ‘１．以，因为ｋ’１秩为２和识的秩为３，所以脆秩为２的矩阵。ＪדＬ““基本矩阵的性质：ａ．假设两幅图像由中心不重合的摄像机获得，则基本矩阵Ｆ为所有的对应点ｘ付ｘ’，都满足ｘ”Ｆｘ＝０的秩为２的唯一的３×３齐次矩阵。ｂ．对极线：，’＝Ｆｘ是对应于图像点ｘ的对极线，Ｉ『＝ＦＴｘ，是对应于图像点ｘ’的对极线。ｃ．对极点：Ｆｅ＝０，ＦＴｅｗ＝０。假设两个摄像机的的内参数矩阵都已标定，世界坐标系的原点在第一个摄像机上，则Ｐ＝Ｋ［Ｉｌ０】，Ｐ，＿Ｆ㈣ｔ】（３—２９）与归一化摄像机矩阵Ｆ１Ｐ＝【Ｉｌｏ】和Ｋ，－１Ｐ７＝降Ｉｆ】对应的基本矩阵称为本质矩阵【４７】，基本矩阵和本质矩阵的关系：Ｅ＝置’ＴＦＫ（３．３０），７万方数据山东大学硕士学位论文最常用的是归一化八点法Ｈ羽，该算法是由Ｌｏｎｇｕｅｔ．Ｈｉｇｇｉｎｓ于１９８１年提出的。由基ｘ”Ｆｘ＝０（３．３１）８对），可用来计算未知的矩阵Ｆ。记ｘ＝（ｘ，Ｙ，１）Ｔ，ｘ，＝（ｘ’，Ｙ’，１）Ｔ，（ｘ’Ｘ，Ｘ’Ｙ，ｘ’，Ｙ’ｘ，Ｙ’Ｙ，Ｙ’，ｘ，ｙ，１）ｆ＝０（３—３３）Ｘ１’五五’Ｍ一’Ｙｌ’五Ｍ’ＭＭ’五Ｍ１ｌＡｓ＂＝Ｉｉ；；ｉｉｉ；ｉ；Ｉｆ＝０（３－３４）Ｉ％’％％’％矗’此’％以’此此’吒乩１Ｉ噪声，导致数据不准确，４的秩可能大于８，此时可用最ｄｘ－－乘法求解唯一解。肭最最后一列矢量，用这种方法得到的解矢蜕条件０厂ｌ＝１－Ｆ取ｌｌＡｆｌＩ的最小值。范数忙一Ｆ１Ｉ达到最小。确ｓＶＤ分解严叻旷，其中Ｄ是对角阵，Ｄ＝ｄｉａｇ（ｒ，ｓ，ｆ）满足，．≥ｓ≥ｆ，则Ｆ７＝Ｕｄｉａｇ（ｒ，Ｊ，Ｏ）ＶＴ最小化ＦＦ的ＦｒｏｂｅＩｌｉｕｓ范数，满足秩为２的要求。万方数据山东大学硕士学位论文假设第一个摄像机矩阵Ｐ＝［Ｉ０】，先对本质矩阵进行奇异值分解，Ｅ＝ＵＤ旷（３．３５）（３—３５）中Ｄ＝ｄｉａｇ（Ｉ，１，０）。令蹦Ｒ，则ｓ＝ｐ】×，…㈥一㈥硎……月＝删或者ＵＷＴＶＴ，ｔ＝ｕ３或一ｕ３，即Ｕ的最后一列。Ｐｔ＿［唧ＴＩ坞］：［删ＴＩ一鸭］：［ＵＷＴｙＴＩ鸭］；［ＵＷＴｙＴＩ一坞］。ＡＢ（１）图３—９由本质矩阵分解出的四种司能解其中字母Ａ，Ｂ分别表示左右两个摄像机的中心，用横线表示摄像机的成像平面，竖线表示摄像机的光轴。（１）与（２），（３）与（４）的区别是摄像机的基线倒置，（１）与（４），（２）与（３）的区别是只有摄像机Ｂ绕基线旋转了１８０度。可以看出，只有在（１）中，重构的点同时在两个摄像机的前方，即在Ａ，Ｂ摄像机中的成像深度均为正值。为从四个不同的解中确定唯一满足条件摄像机矩阵Ｐ’，用一个空间点进行测试，验证该点是否在两个摄像机的成像深度均为正，即可确定出正确的Ｐ’。万方数据山东大学硕士学位论文３．２．２位姿估计由本质矩阵恢复得到摄像机矩阵后，可利用线性三角形法【叫来计算得到图像中的点在摄像机坐标系下的三维坐标。由匹配点Ｘ＜－－－＞ｘ’估计对应的３Ｄ点Ｘ，使它满足给定的摄像机几何，即ｘ＝ＰＸ，ｘ，＝Ｐ’Ｘ（３—３６）将（３．３６）等式左边叉乘等式右边，消去齐次纯量因子，组合成４Ｘ＝０的形式。每一个图像点则对应三个方程，其中任意两个是线性独立的，例如，对第一幅图像中的特征点ｘｘ（ＰＸｌ＝ｏ展开得ｘ（Ｐ３Ｔｘ）－（Ｐ＂ｘ）＝ｏｙ（ｐ３Ｔｘ）－（ｅ２Ｔｘ）＝ｏ（３—３７）ｘ（Ｐ打ｘ）一ｙ（ｐＴｘ）＝ｏ其中，是Ｐ的第ｉ行，组成形女ＨＡＸ＝０的方程，ｘ（Ｐ３Ｔ）－ｐ１ＴＡＸ：＝Ｙ（Ｐ３Ｔ）－ｐ２Ｔｘ’（Ｐ弓Ｔ）－ｐ１ＴＸ＝０（３—３８）Ｊ，’（Ｐ弓Ｔ）一Ｐ哩Ｔ对每一组匹配点，均可得到上述线性方程。使用最ｄｘ－．－乘法进行求解，对矩陬进行ＳＶＤ分解Ａ＝ＵＤ旷，姗勺解对应于啪最后一列。因为要求求出的空间点艉齐次的，可将坐标矢量的每个元素都除以矢量的第四个元素进行归一化，进而得至ｔＪ３Ｄ点的齐次坐标。ｆｌ了（３－３８）得到三个不共线的特征点在Ｂｕｍｂｌｅｂｅｅ２右摄像头坐标系｛ｃ”：的三维坐标。只，ｉ＝１，２，３，。丑＝‘Ｒｏ。丑＋。ｔｏ。置＝。ｌｔｏ。最＋。ｔｏ（３－３９）。只＝。Ｒｏ。只＋。ｔｏ等式两两相减，消去‘ｔ。，得：万方数据山东大学硕士学位论文ｑ一鼍＝巩（ｑ一卫ｉ‘罡一。只＝ｃＮ（３．４０）一。忍）憾。二镪简。。兄黼侩三６（３－４２）Ｒｃ＝ｐ４。，一ｄｃ２：＝＝［ｃ：；口ｃ：。］３．３实验结果与分析３．３．１单目视觉定位实验‘３－４４’利用３．１．２节中提出的基于世界平面单应矩阵分解的物品位姿估计算法，可得到ＮＡＯ机器人的本体单目摄像机坐标系与物体坐标系的相对位姿为：７．０．０６２３６３２－０．０２０５２１５。ｒｏ＝鲥引Ｍ弘烈为，ｔ，Ｉ、－，，ＩＬ钳饥渤弛嘴ｍｏ绷ｍ娜。必髫毗。０．９９７８４３Ｏ、Ｊ．、Ｊ２＆Ｈ．孔弱倦叭弦妣，ｆ３－４５）豇锥旧●万方数据山东大学硕士学位论文三维空间中的相对位姿表示如图３．１０所示：０１００５Ｅ商０．００５．０１图３．１０单目摄像机与物体的相对位姿３．３．２双目视觉定位实验双目视觉定位的实验流程框图如图３．１１所示：图３．１１双目视觉物体定位流程框图３２万方数据山东大学硕士学位论文本文中的图像处理用的是ＯｐｅｎＣＶ［５０１库，对双目视觉获取的左右视图进行极线校正，校正后的图像如图３一１２所示。图３．１２双目视觉获取的校正后左右视图对校正后的图像进行ｓｕｒｆ特征提取与匹配，利用随机采样一致性算法（ＲａｎｄｏｍＳａｍｐｌｅＣｏｎｓｅｎｓｕｓ，ＲＡＮＳＡＣ）去除匹配点集中的外点，处理结果如图３－１３所示。图３．１３双目视觉左右视图特征匹配根据图像中的匹配点对计算出基本矩阵Ｆ。按３．２．１中提出的算法进行基本矩阵Ｆ分解，计算出满足条件的摄像机矩阵Ｐｌ，见。通过物品模板与图像匹配分割出物品，再利用Ｓｌｌｉ．Ｔｏｍａｓｉ角点检测获取物品在左右视图的特征点，筛选匹配出对应的特征点，如图３．１４所示，由３．２．２中提出的位姿估计算法可知，只要已知任意不共线三个点的三维坐标就能计算出摄像机与物品的相对三维位姿，为方便建立物品坐标系，我们选取物品顶面的三个特征点并获取其各自的图像坐标。万方数据山东大学硕士学位论文图３．１４物品的识别与特征点提取由线性三角形法计算得到所选取的物品特征点在双目视觉坐标系下的三维坐标，进而由３．２．２提出的方法计算得到双目相机坐标系与物品坐标系的相对位姿结果为：０．８７８５４７０．１６０７８７．０．４４９７８２０．１４４１６１０．１７７３０８．０．９８４１４０．０．００５４７６７９０．０２７５４６７。Ｌ＝（３－４６）．０．４４３５２９．０．０７４９３８３．０．８９３１２２０．４３１０９１０００１三维空间中的相对位姿表示如图３．１５所示：０４图３．１５双目摄像机与物体的相对位姿３４万方数据山东大学硕士学位论文３．４本章小结本章首先对摄像机模型和摄像机参数标定的方法进行了详细介绍。基于本体单目视觉提出了基于Ｎａｏｍａｒｋ标签的物品识别定位方法，设计了基于单应矩阵分解的物品位姿估计算法。为弥补本体单目视觉定位的不足，基于双目立体视觉提出了对极几何及本质矩阵分解和线性三角形法的物品位姿估计算法，实验结果表明提出的方法能够实现抓取物品的准确定位，为家庭环境下机器人进行物品抓取服务的实现奠定了基础。万方数据３６万方数据山东大学硕士学位论文山东大学硕士学位论文第四章基于本体单目视觉的物品抓取针对家庭环境中服务机器人基于机器人本体单目视觉抓取在机器人附近且在机器人视野范围内物品的问题，提出了一种改进的基于位置的单／双臂视觉伺服抓取算法。物体的快速识别是通过Ｎａｏｍａｒｋ标签完成，物体的位姿估计是通过世界平面单应矩阵分解算法完成；针对不同尺寸不同重量物品的抓取，分别设计了相应的单臂和双臂抓取的视觉伺服控制律，通过利用对ＮＡＯ机器人的左右机械臂所建立的运动学模型完成对末端执行器的精确控制；最后，为进一步提高抓取的稳定性和鲁棒性，对机械臂的末端执行器进行了路径规划。通过实验表明，提出的方法能够使ＮＡＯ机器人快速、稳定地抓取目标物品。视觉伺服系统的各坐标系如图４．１所示。图４．１（ａ）中｛ｒ｝为机器人坐标系，｛ｃ）为摄像机坐标系，｛Ｄ）为目标物体坐标系，｛ｅ）为末端执行器坐标系，图４．１（ｂ）中｛Ｌ）、｛Ｒ）分别代表左、右手末端执行器坐标系。（ａ）单臂抓取（ｂ）双臂抓取图４．１视觉伺服系统各坐标系４．１单臂抓取的ＰＢＶＳ控制律对于形状小、质量轻的物体，ＮＡＯ机器人只用单臂就可完成抓取和运送任务，万方数据山东大学硕士学位论文其ＰＢＶＳ控制方法原理如图４．２所示。建立运动学模型后，机械臂末端执行器相对机器人坐标系的位姿７霉＝ｒＺ（口），它是关于机械臂各关节转角的函数，即它由机械臂各关节自由度的转角所确定。ＰＢＶＳ控制律是根据视觉测量得到的物体位姿求出抓取成功时的７￡，可按（４．１）式计算。式中，７瓦为摄像机坐标系相对机器人坐标系的位姿，。Ｌ为目标物体坐标系相对摄像机坐标系的位姿，利用世界平面单应矩阵分解算法由提取的图像特征点求出，。互＝占Ｚ．为抓取到物体时末端执行器与物体的相对位姿。７ｚ２７霉。ｒｏ。互＝＝，ｒ。Ｃｒｏ（，】Ｃｃｒｏ。）‘ｌｒｒ。（４－１）（ａ）期望抓取成功的控制原理图嘞实际抓取成功的控制原理图图４．２单臂抓取控制方法原理图根据ＰＢＶＳ控制律求得的’Ｚ和运动学逆解可得到各关节的期望角度值０‘，定义误差ｅ＝０一矿，设定每次的关节控制量为ｅ的１０％，每次控制各关节移动后，进行实时视觉反馈得到最新的期望角度值和实时采样关节角度值得到最新的误差ｅ，重复上述过程让末端执行器逐步逼近物品完成抓取操作。４．２双臂抓取的ＰＢＶＳ控制律对于操作形状大、质量重的物体，单臂抓取是难以实现的，需要双臂协调操作，其ＰＢＶＳ控制方法原理如图４．３所示。抓取任务要求根据摄像机定位估计得到的物体位姿，求出能成功抓取的左、右机械臂末端执行器相对机器人坐标系的期望位姿７正与’砭，类似的，由获取到的抓取成功时末端执行器与目标物体坐标系的相对位姿，ｔ．与。’Ｌ。，可由（４．２）式计算出７互、７瓦，根据运动学逆解求解出左、右机械臂各关节的期望角度值，进而控制左、右机械臂末端执行器逐步逼近目标物万方数据山东大学硕士学位论文体完成抓取。７瓦＝７正。ｒｏ。瓦＝７Ｃ。ｔｏ（７ｔｆ乃）＿７弓７疋＝７乏‘ｒｏ口瓦（４?２）＝７瓦。ｔｏ（７‘。ｒｏ。）。１７％｛三）｛Ｄ）｛Ｒ，图４－３双臂抓取控制方法原理图抓取成功后，左、右机械臂、物体构成一个闭式运动链（如图４－４所示）。｛尺）和｛三｝分别为左、右机械臂末端连杆坐标系，｛ｏ｝为目标物坐标系。在抓取到物体进行运送的过程中，双臂的位姿需要满足约束关系：岛乏三ｒｏ＝岛致％毛ＲＴｏ（４—３）即Ｒ靠＝（ｂ氏）－１岛五三ｔｏ（尺瓦）－１＝（ｋ氏厂１岛互￡瓦（４—４）其中‘砭＝‘ｔｏ（Ｒｔｏ）ｑ是个常数阵，在物品运送过程中应保持不变。如｝｛三ｏ＞｛Ｒｏ）图４＿４双臂抓持位姿约束尺寸链３９万方数据山东大学硕士学位论文４．３增加路径点的ＰＢＶＳ控制律目前的视觉伺服控制研究都主要集中于机械臂末端执行器的当前位姿与期望位姿差的控制，而对到达期望位姿的过程中末端执行器的路径一般不予以规划，这样容易使机械臂的末端执行器与障碍物或目标抓取物发生避碰，从而导致抓取成功率降低。在整个抓取控制过程中，利用基于位置的视觉伺服原理在规划出的无碰撞抓取路径上设置１３．个路径点仞Ｉ，Ｐ２，．．．，肌＞能很好的解决这个问题，该末端执行器到达各路径点处的位姿通过相对于目标物坐标系的相对旋转平移关系。Ｌ确定，ＰＢＶＳ控制原理图如图４．５所示。这样控制过程被分成ｎ＋ｌ段，机械臂末端执行器由初始位置到达各路径点，再由第聆个路径点趋近并到达目标位姿完成抓取。控制律为慕ｒＴｅ－－瓦ｏ＊Ｔ≥．＝。麓筹ｃ－≥ｉ：＂、。…¨％，＝１，２，…，，２。％＝岛件５，、’｛Ｏ）｛ｒ）图４．５增加路径点的ＰＢＶＳ控制原理图４．４实验结果与分析对单臂、双臂抓取和其各自增加路径规划的改进控制律算法各进行１００组抓取实验，每次实验目标抓取物体的位置任意摆设，ＮＡＯ手臂末端执行器的初始位置也任意给定，统计出１００组实验中不同抓取方式各自的成功次数以及成功率（如表４—１所示）。由表４．１可知，改进的算法相比于传统的ＰＢＶＳ控制能够明显地提高抓取成功率。下面根据抓取过程中各关节角度误差的实时变化对抓取的稳定性进行分析。万方数据ｐｒｏｇｒａｍｍｉｎｇ［Ｃ］／／Ｒｏｂｏｔ ａｎｄ１ ８ｔｈ ＩＥＥＥ ＩｎｔｅｒｎａｔｉｏｎａｌＨｕｍａｎ ＩｎｔｅｒａｃｔｉｖｅＣｏｍｍｕｎｉｃａｔｉｏｎ，２００９．ＲＯ－ＭＡＮ２００９．ＴｈｅＳｙｍｐｏｓｉｕｍ ｏｎ．ＩＥＥＥ，２００９：４６－５ １．【３６】蔡自兴．机器人学［Ｍ】．清华大学出版社，２０００． ［３７】ＣｏｒｋｅＰ．Ａ ｒｏｂｏｔｉｃｓ ｔｏｏｌｂｏｘ ｆｏｒＭＡＴＬＡＢ［Ｊ］．Ｒｏｂｏｔｉｃｓ＆Ａｕｔｏｍａｔｉｏｎ Ｍａｇａｚｉｎｅ，ＩＥＥＥ，１９９６，Ｍ Ｇ ａｎａｌｙｔｉｃａｌｉｎｖｅｒｓｅ ｋｉｎｅｍａｔｉｃｓ ｆｏｒ３（１）：２４－３２． ［３８】Ｋｏｆｉｎａｓ Ｎ，Ｏｒｆａｎｏｕｄａｋｉｓ ＮＡＯ［Ｃ］／／Ａｕｔｏｎｏｍｏｕｓ２０１３：１－６． ｍａｃｈｉｎｅ Ｅ，Ｌａｇｏｕｄａｋｉｓ ＣｏｍｐｌｅｔｅＲｏｂｏｔＳｙｓｔｅｍｓ（Ｒｏｂｏｔｉｃａ），２０ １ ３１ ３ｔｈ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ．ＩＥＥＥ，［３９】Ｓｏｎｋａ Ｍ，Ｈｌａｖａｃ ＶＬｅａｒｎｉｎｇ，２０ １ ４．Ｂｏｙｌｅ Ｒ．Ｉｍａｇｅｐｒｏｃｅｓｓｉｎｇ，ａｎａｌｙｓｉｓ，ａｎｄｖｉｓｉｏｎ［Ｍ］．Ｃｅｎｇａｇｅ［４０］ＺｈａｎｇＺ．Ａ ｆｌｅｘｉｂｌｅ ｎｅｗ ｔｅｃｈｎｉｑｕｅ ｆｏｒ ｃａｍｅｒａｃａｌｉｂｒａｔｉｏｎ［Ｊ］．ＰａｔｔｅｍＡｎａｌｙｓｉｓ ａｎｄ ＭａｃｈｉｎｅＩｎｔｅｌｌｉｇｅｎｃｅ，ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓ ｏｎ，２０００，２２（１ １）：１３３０—１３３４．［４ｌ】李国栋，田国会，薛英花．基于ＱＲＣｏｄｅ技术的家庭服务机器人视觉伺服抓取操作研究［Ｊ】． 东南大学学报：自然科学版，２０１０，４０：３０．３６． ［４２】穴洪涛，田国会，李晓磊，路飞．ＱＲＣｏｄｅ在多种类物体识别与操作中的应用【Ｊ】．山东大学 学报（工学版），２００７，３７（６）：２５—３０． 【４３】李国栋，田国会，王洪君，周风余．一种景物平面法向已知条件下的单应矩阵快速分解算法 【Ｊ】．控制与决策，２０１４，２９（４）：７３５—７３８．［４４】ＭＡＬＬＳＥ，ＶＡＲＧＡＳＭ．Ｄｅｅｐｅｒ ｕｎｄｅｒｓｔａｎｄｉｎｇ ｏｆ ｔｈｅｈｏｍｏｇｒａｐｈｙｄｅｃｏｍｐｏｓｉｔｉｏｎｆｏｒｖｉｓ