·22 智能系统学报 第2卷 这种混合控制策略可用下列规则标出： 3 位姿估计与跟踪控制 f1el≤a,PID控制， 同构双目视觉机器人的优势在于可以进行双目 f|e|>a,|ul=tomax (Bang Bang控制 测距，获取目标中心在左右2眼图像中的位置坐标 (9列 后，计算目标与摄像机的相对位置，并反馈至控制回 式中：a为控制切换线，e为误差，u为控制量 路.利用双目摄像机的结构约束内极线约束？用 控制器的输出是机器人的前进速度ⅴ和旋转角 来进行双目匹配和检查目标中心提取的正确性 速度ω.并由速度闭环控制电机驱动2轮.采用分立 3.1立体测距 单独控制的思想)，e=Za-Z控制跟踪速度v, 按照小孔成像模型61和文献171中的双目测 ew=0-0控制机器人跟踪旋转角速度ω，即 量模型，同构、焦平面重合、同物点在2图中只有水 v =VSC,(e:), 平方向的平移的三维测量式为(8)，其中x,y,:是 (10) @=VSCu(eo). 目标中心在摄像机坐标系中的三维坐标，(m,v) 式中：VSC,VSC。为按式3)设计的视觉伺服控制器 为右图像的中心像素坐标，(，2)，(M,)分别是 依轮式移动机器人的运动模型分别驱动控制模 目标中心在左、右摄像机的图像坐标，b,∫是基线长 型1，如式11)所示. 度和焦距以像素表示)： =v+w·L/2 11) x b x(u-un)/(i m), 、VR=V-P·L/2 y =b x(vI vo)/(u-m). 8) 式中：L为机器人轮间距，z,VR分别为机器人左右 :=fXb/(·M). 轮速度 根据坐标系转换关系，可计算目标在跟踪机器 人坐标系的位置 4实验结果 3.2跟踪控制 4.1曲线跟踪 如图5所示，建立机器人跟踪坐标系，2轮间中 跟踪曲线前进的目标具有一般意义.目标机器 点为原点，机器人正前方为Z轴正向，向右为X轴 人在前方以vR=9cm/s,z=11cm/s的速度逆时 正向.系统在室内平坦地面实施跟踪，认为目标中心 针曲线行进，初始时其在跟踪坐标系的位置为X= 与跟踪机器人的相对高度不变，故只考虑目标在 -10cm,Z=114cm,则目标机器人的路径示意如图 XOZ二维平面的位置信息 6所示.跟踪过程中要求跟踪机器人在目标后方80 cm,偏右10cm处，即X,=-10cm,Z,=80cm.图 像分割的阈值选为252，待识别区域个数取1个，式 9)中a取10cm.图6-2中矩形阴影框部分为识别 标 出的目标 、目标机器人 II cms 跟踪机器人 9 cm/s 图5目标跟踪坐标关系 跟踪机器人 Fig 5 Relationship of coordinates for target tracking (a) (b) 图5中，0为机器人的前进方向与目标连线矢 径的夹角，Z,X为目标中心在跟踪坐标系的位置坐 图6跟踪路径示意与识别效果图 标在图1的控制回路中，视觉伺服控制器采用 Fig 6 Diagram tracking path and recognition effect Bang-Bang控制与PD控制相结合的策略u],保证 实验所得的全局跟踪轨迹为图7.目标在跟踪 跟踪系统的快速性，且具有较高控制精度和稳定性 坐标系的坐标位置曲线如图8示，图9为误差曲线 1994-2009 China Academic Journal Electronic Publishing House.All rights reserved.http://www.cnki.net3 位姿估计与跟踪控制 同构双目视觉机器人的优势在于可以进行双目 测距 ,获取目标中心在左右 2 眼图像中的位置坐标 后 ,计算目标与摄像机的相对位置 ,并反馈至控制回 路. 利用双目摄像机的结构约束 —内极线约束[15 ]用 来进行双目匹配和检查目标中心提取的正确性. 311 立体测距 按照小孔成像模型[16 ] 和文献[17 ]中的双目测 量模型 ,同构、焦平面重合、同物点在 2 图中只有水 平方向的平移的三维测量式为 (8) ,其中 x , y , z 是 目标中心在摄像机坐标系中的三维坐标 , ( uro , vro ) 为右图像的中心像素坐标 , ( u2 , v2 ) , ( u1 , v1 ) 分别是 目标中心在左、右摄像机的图像坐标 , b, f 是基线长 度和焦距(以像素表示) : x = b ×( u1 - ur0 ) / ( u2 - u1 ) , y = b ×( v1 - vr0 ) / ( u2 - u1 ) , z = f ×b/ ( u2 - u1 ) . (8) 根据坐标系转换关系 ,可计算目标在跟踪机器 人坐标系的位置. 312 跟踪控制 如图 5 所示 ,建立机器人跟踪坐标系 , 2 轮间中 点为原点 ,机器人正前方为 Z 轴正向 ,向右为 X 轴 正向. 系统在室内平坦地面实施跟踪 ,认为目标中心 与跟踪机器人的相对高度不变 , 故只考虑目标在 XO Z 二维平面的位置信息. 图 5 目标跟踪坐标关系 Fig15 Relationship of coordinates for target tracking 图 5 中 ,θ为机器人的前进方向与目标连线矢 径的夹角 , Z , X 为目标中心在跟踪坐标系的位置坐 标. 在图 1 的控制回路中 , 视觉伺服控制器采用 Bang2Bang 控制与 PID 控制相结合的策略[18 ] ,保证 跟踪系统的快速性 ,且具有较高控制精度和稳定性. 这种混合控制策略可用下列规则标出 : if | e | ≤a ,PID 控制 , if | e | > a , | u | = umax (Bang2Bang 控制) . (9) 式中 : a 为控制切换线 , e 为误差 , u 为控制量. 控制器的输出是机器人的前进速度 v 和旋转角 速度ω,并由速度闭环控制电机驱动 2 轮. 采用分立 单独控制的思想[19 ] , ez = Zd - Z 控制跟踪速度 v , eθ =θd - θ控制机器人跟踪旋转角速度ω,即 v = V S Cv ( ez ) , ω = V S Cω( eθ) . (10) 式中:VSCv ,VSCω 为按式(3)设计的视觉伺服控制器. 依轮式移动机器人的运动模型分别驱动控制模 型[19 ] ,如式(11) 所示. vL = v + w ·L/ 2 , vR = v - w ·L/ 2. (11) 式中 :L 为机器人轮间距 , vL ,V R 分别为机器人左右 轮速度. 4 实验结果 411 曲线跟踪 跟踪曲线前进的目标具有一般意义. 目标机器 人在前方以 vR = 9 cm/ s, vL = 11 cm/ s 的速度逆时 针曲线行进 ,初始时其在跟踪坐标系的位置为 X = - 10 cm , Z = 114 cm ,则目标机器人的路径示意如图 6 所示. 跟踪过程中要求跟踪机器人在目标后方 80 cm ,偏右 10 cm 处 ,即 Xr = - 10 cm , Zr = 80 cm. 图 像分割的阈值选为 252 ,待识别区域个数取 1 个 ,式 (9) 中 a 取 10 cm. 图 622 中矩形阴影框部分为识别 出的目标. 图 6 跟踪路径示意与识别效果图 Fig16 Diagram tracking path and recognition effect 实验所得的全局跟踪轨迹为图 7 ,目标在跟踪 坐标系的坐标位置曲线如图 8 示 ,图 9 为误差曲线 , ·22 · 智 能 系 统 学 报 第 2 卷