专利名称：一种球形机器人视觉定位系统及视觉定位方法
技术领域：
本发明涉及机器人定位导航领域，特别涉及一种球形机器人视觉定位系统及视觉定位方法
背景技术：
球形机器人是指一类将运动执行机构、传感器、控制器、能源装置都内置于一个球形壳体内部、利用球形外壳作为滚动行走装置的系统的总称。与轮式，足式和履带式的机器人相比，具有结构新颖，运动灵活和效率高的特定，在军事、民用和工业领域有广泛的应用前景。球形机器人的定位问题是实现球形机器人智能化的一个关键问题。目前球形机器人主要采用惯性导航系统、里程计、GPS等方式进行定位，但是惯性导航系统需要通过积分来得到位移和角度，存在时间漂移误差；里程计由于球面滑动，跳跃等因素，将产生累计误差，同时球形机器人平台不稳定，难以找到合适的搭载里程计的位置；GPS精度不高，室内无法使用。目前视觉定位是研究热点，是未来机器人导航定位的一个方向，其具有信息量大、 感应时间短的特点，可以通过获取周围环境位置的变化，得到机器人精确的机器人运动和姿态。球形机器人利用视觉定位还可以克服由于球面滑动，跳跃等采用内部传感器定位造成的累计误差。但是目前在球形机器人上采用视觉定位的研究成果以及在球形机器人上搭载完整的视觉定位系统基本没有，主要是球形机器上采用视觉定位存在不少问题1、视觉定位算法复杂、计算量大，球形机器人的处理系统难以进行实时处理。2、球形机器人外面是封闭球壳，相机透过球壳采集图像质量很差甚至无法采集图像。3、相机安装困难，通常的安装位置会使相机随球360度转动4、球形机器人平台不稳定，相机坐标系相对与机器人坐标系会不断变化，导致定位不准。
发明内容
本发明的目的是为了克服现有球形机器人导航的技术不足，提出一种精度高、实时性好的球形机器人视觉定位系统及视觉定位方法。本发明的采用如下技术方案一种球形机器人视觉定位系统，包括双目视觉系统、一维陀螺仪、球形机器人本体、核心控制系统、无线通信�？�。本发明中，所述的视觉定位系统的球形机器人本体设计成左右两个半球壳，左右半球独立驱动，同时左右半球之间开缝设计。所述的视觉定位系统的双目视觉系统包括两个型号相同的CCD相机，其主光轴平行放置，双目相机搭载到球形机器人与重摆相连的框架上，保证球静止时，相机光轴与地面平行，并指向正前方；所述的陀螺仪安装在双目相机的正上方。定位系统通过相机和陀螺仪配合进行定位，定位算法在核心控制系统中在线实现，定位的数据通过无线通信�？榻性冻檀洌没Э梢栽冻淌凳奔嗫鼗魅说奈恢谩Ｒ恢智蛐位魅耸泳醵ㄎ环椒ǎㄒ韵虏街璨街�1 对相机和陀螺仪距相机中心的位置进行标定。步骤2 开启双目相机，进行左右图像连续采集，开启陀螺仪，对相机姿态进行测量。步骤3 如果图像是第一帧，对采集的左右图像提取Sii-Tomasi特征，并利用SIFT 描述符进行描述，通过立体视觉坐标转化原理，将二维图像坐标转换为三维空间坐标，利用陀螺仪对特征点的位置进行补偿，初始化旋转矩阵R、平移矩阵T，R设为单位矩阵，T元素全置零，并设当前位置为原点，重新进入步骤3 ；否则直接进入步骤4。步骤4 利用KLT算法对前后两帧左图和前后两帧右像分别进行特征的跟踪， 同时对前后两帧图像中左右图像利用极线约束条件去除误跟踪特征对，将如果跟踪特征点总数小于阈值，重新对前后帧图像进行Sii-Tomasi提取并匹配，将匹配特征点二维图像坐标转换为三维空间坐标，得到前后两帧图像的三维特征点集，并利用陀螺仪对特征点的位置进行补偿；步骤5 利用前后两帧的三维特征点集，采用解析法并结合RANSAC算法进行运动估计，得到R和T ；其中的详细步骤如下5-1.对前后帧三维特征点集进行筛�。〕銎ヅ涑晒Φ奶卣鞯悖槌尚碌奶卣鞯慵�。5-2.对于前后帧新的特征点集，随机选择3对三维特征点，通过解析法，进行一次运动估计，得到一个R和T。5-3.判断当前R和T，能使多少对特征点成为局内点，记录局内点对数。5-4.针对5-2、5_3，进行N次迭代，得到局内特征对数最多时候的局内特征对。5-5.将这些特征对，重新根据解析法，进行运动估计，得到更优的R和T。步骤6 利用卡尔曼滤波算法对R，T进行更新，并得到当前机器人在世界坐标系中的位置，返回步骤3，进行下一次计算。本发明的优点有1)本发明采用双目视觉结合陀螺仪的方式进行定位，降低了球形机器人不稳定带来的定位误差，提高了定位精度。2)本发明中球形机器人本体采用左右半球之间开缝设计，外部传感器可以摆脱球壳的约束，更好的获取外部的环境信息；3)本发明视觉系统搭载到相对稳定的主框上，是其保证一直指向球形机器人正前方位置，并只有小范围的偏转，保证采集图像的稳定性。3)本发明中视觉定位算法，使用Sii-Tomasi特征和SIFT描述符相结合的特征提取方法，既满足了特征点提取的实时性，又提高特征点匹配的鲁棒性。4)本发明中视觉定位算法采用KLT算法进行特征跟踪，不用每两帧图像进行特征重新提取和匹配，计算量大为减少。5)本发明中视觉定位算法，使用解析法和RANSAC算法进行运动估计，保证了定位
4精度和实时性。
图1是本发明提出的球形机器人视觉导航系统结构图。图2是本发明中所定义的各个坐标系。图3是本发明中机器人运动时，球形机器人坐标系和摄像机坐标系变化示意图。图4是本发明提出的球形机器人视觉导航算法流程图。图5是本发明提出的球形机器人视觉导航算法中运动估计算法结合RANSAC算法流程图
具体实施例方式下面结合附图对本发明进行进一步说明如图1所示为本发明所述的球形机器人视觉导航系统。该系统包括球形机器人本体1、双目视觉系统2、陀螺仪3、核心控制系统4、无线通信�？�5。定位系统通过双目定位系统2和陀螺仪3配合进行定位，定位算法在核心控制系统4中在线实现，定位的数据通过无线通信�？�5进行远程传输，用户可以远程实时监控球形机器人本体1的位置。球形机器人本体1由左右两个半球壳构成球形外壳，中间开缝；在球壳内装有主框架，与重摆固连，重摆设置在竖直平面上，主框架两侧与球壳通过支撑架滑动连接。球形机器人本体1两个半球壳进行分别驱动控制。球形机器人本体1包括机械结构和底层电机驱动电路。双目视觉系统2由两个同型号的C⑶黑白摄像头构成，事先进行标定，已获得其内参数和畸变参数。双目视觉系统2搭载到球形机器人与重摆相连的框架上，保证球静止时， 相机光轴与地面平行，并指向正前方。双目视觉系统2与核心控制系统4通过1394数据线相连，核心控制系统4不断接收双目视觉系统2的图像，并进行处理。陀螺仪3为一维陀螺仪，可以测量指定方向的角度变化。其安装在双目视觉系统上方，保证其可以测量重摆摆起的角度。陀螺仪3与核心控制系统4通过串口相连，心控制系统4不断接收陀螺仪3反馈的角度进行处理。核心控制系统4上运行球形机器人视觉导航算法，核心控制系统4采用MINI-ATX 主板、13 3. IGHZ处理器、30G固态硬盘、4G内存，DC-ATX电源�？椋蟮奶岣吡讼低车氖荽砟芰痛娲⒛芰Γ奔婀肆讼低澄榷ㄐ�、功率和小尺寸的要求。核心控制系统4运行稳定性、安全性好的Linux操作系统，通过配备的DC12V锂电池，可以持续工作两个小时。无线通信�？�5为宽带宽、大功率的无线网卡，实现与远程主机进行通信，将定位的结果不断发送给远程客户端，实现实时监控。同时无线通信�？橐部梢越杉耐枷裨冻檀洌迪滞枷裨冻碳嗫�。无线通信�？�5采用USB接口与核心控制系统4相连。参照图2，对本发明球形机器人视觉定位算法中用到的几种不同的坐标系和坐标系间转换作简单介绍(1)世界坐标系{A :Ow-xwywzw}用来表示球形机器人和特征点的绝对位置，在空间中垂直地面为Z轴，设单位为米。
5
(2)球形机器人本体坐标系{B :Or-xryrzr}坐标原点位于球形机器人球心，ζ轴指向机器人运动方向，y轴指向地面，χ轴与y 轴、ζ轴构成右手坐标系，单位为米。(3)摄像机坐标系{C :oc-xcyczc}坐标原点位于立体视觉系统左相机光心，ζ轴与立体视觉系统左相机光轴重合，χ 轴指向右相机光心，y轴与χ、ζ轴构成右手坐标系。单位为米。(4)图像坐标系{D Ou-XuyJ在立体视觉系统左相机成像平面内，以左上角为坐标原点，水平方向为χ轴，竖直方向为y轴，单位为像素。由于摄像机位于球形机器人本体上不稳定平台上，如图3，当机器人重摆摆动，球形机器人本体坐标系和摄像机坐标系关系将发生变化，球形机器人本体坐标系和摄像机坐标系转化需要知道重摆摆动的角度θ。θ由陀螺仪测出，由几何关系，可得球形机器人本体和摄像机坐标系转化关系
权利要求
1.一种球形机器人视觉定位系统，其特征在于，包括双目视觉系统、一维陀螺仪、球形机器人本体、核心控制系统和无线通信�？榈�。定位系统通过相机和陀螺仪配合进行定位， 定位算法在核心控制系统中在线实现，定位的数据通过无线通信�？榻性冻檀洌没Э梢栽冻淌凳奔嗫鼗魅说奈恢�。
2.权利要求1所述的一种球形机器人视觉定位系统，其特征在于，球形机器人本体设计成左右两个半球壳，左右半球独立驱动，同时左右半球之间开缝设计。双目视觉定位系统搭载到球形机器人与重摆相连的框架上，通过左右球壳之间的缝隙，伸出球壳外部，保证机器人静止时，相机光轴与地面平行，并指向正前方。陀螺仪安装在双目相机的正上方。
3.一种球形机器人视觉定位方法，其特征在于，包含以下步骤步骤1 对相机和陀螺仪距相机中心的位置进行标定。步骤2 开启双目相机，进行左右图像连续采集，开启陀螺仪，对相机姿态进行测量。步骤3 如果图像是第一�。圆杉淖笥彝枷裉崛ii-Tomasi特征，并利用SIFT描述符进行描述，通过立体视觉三角测量法将特征点的二维图像坐标转换为三维空间坐标， 利用陀螺仪对特征点的位置进行补偿，初始化旋转矩阵R、平移矩阵T，R设为单位矩阵，T元素全置零，并设当前位置为原点，重新进入步骤3 ；如果图像不是第一�。苯咏氩街�4。步骤4 利用KLT算法对前后两帧左图和前后两帧右像分别进行特征的跟踪，同时对前后两帧图像中左右图像利用极线约束条件去除误跟踪对，得到特征匹配对，如果跟踪特征对总数小于阈值，重新对前后帧图像进行Sii-Tomasi提取并匹配，将新匹配的点加至特征匹配对，通过立体视觉三角测量法将匹配特征对二维图像坐标转换为三维空间坐标， 得到前后两帧图像的三维特征点集，并利用陀螺仪对特征点的位置进行补偿；步骤5 利用前后两帧的三维特征点集，采用解析法并结合RANSAC算法进行运动估计， 得到R和T ；步骤6 利用卡尔曼滤波算法对R，T进行更新，并得到当前机器人在世界坐标系中的位置。返回步骤3，进行下一次计算。
4.权利要求4所述的一种球形机器人视觉定位方法，其特征在于，步骤5具体步骤如下(1).对前后帧三维特征点集进行筛�。〕銎ヅ涑晒Φ牡悖槌尚碌奶卣鞯慵�。(2).对于前后帧新的特征点集，随机选择3对三维特征点，通过解析法，进行一次运动估计，得到一个R和T。(3).判断当前R和T，能使多少对特征点成为局内点，记录局内点对数。(4).针对0)、(3)，进行N次迭代，得到局内特征对数最多时候的局内特征对。(5).将这些特征对，重新根据解析法，进行运动估计，得到更优的R和T。
全文摘要
本发明公开了一种球形机器人视觉定位系统及视觉定位方法。视觉定位系统包括双目视觉系统、一维陀螺仪、球形机器人本体、核心控制系统和无线通信�？樽槌伞６ㄎ幌低惩ü嗷屯勇菀桥浜辖卸ㄎ唬ㄎ凰惴ㄔ诤诵目刂葡低持性谙呤迪郑ㄎ坏氖萃ü尴咄ㄐ拍？榻性冻檀洌没Э梢栽冻淌凳奔嗫鼗魅说奈恢�。定位方法是通过双目相机实时采集左右图像序列，通过双目视觉里程计算法，计算出球形机器人姿态和位置的变化，通过陀螺仪对球形机器人平台不稳定造成的特征定位误差进行补偿，最后采用卡尔曼滤波方法进行状态更新。整个过程不断迭代，实现实时对机器人的姿态和位置进行计算。
文档编号G01C11/04GK102435172SQ20111025896
公开日2012年5月2日 申请日期2011年9月2日 优先权日2011年9月2日
发明者叶平, 孙汉旭, 张天石, 李自亮, 王轩, 贾庆轩 申请人:北京邮电大学
专业数控铣床产品供应商
20年生产经验，实力工厂，精工品质，质量有保障
