双目视觉系统目标跟踪控制方法
【技术领域】
[0001 ]本发明设及机器人领域多自由度双目视觉系统的运动控制问题，尤其适用于行星 车多自由度双目视觉系统对目标的跟踪控制。
【背景技术】
[0002] 双目视觉测量在机器人领域广泛应用。在航天工程领域，行星车多自由度双目视 觉系统是一个应用典型，可用于对目标进行捜索和跟踪。一般是将双目视觉系统安装在多 自由度臂末端的平台上，通过多自由度臂扩展双目视觉相机的跟踪范围。行星车多自由度 双目视觉系统的目标跟踪任务可由如图1所示的模型表示，包括=个组成部分：多自由度 臂、双目视觉相机(包括左眼和右眼)和目标。图中{〇}、和{T}分别表示多自由度臂的基坐标 系、左眼相机坐标系和目标坐标系，UKi = I，2,…，n)表示多自由度臂各关节坐标系。对于 行星车多自由度双目视觉系统的目标跟踪任务，一般选取目标物上的一特征点作为跟踪目 标点，由于相机镜头边缘崎变的问题，因此希望该目标成像点能始终位于相机图像的中屯、 (本算法W目标成像点位于左眼相机图像的中屯、为例）。
[0003] 多自由度双目视觉系统的目标跟踪控制方法需要建立运动学模型：一是正运动学 模型，即已知多自由度臂各关节U}的位置参数，求得目标坐标系{T}在基坐标系{0}中的位 姿参数，由此可获得目标的位置信息;二是逆运动学，即已知目标坐标系{T}在基坐标系{0} 中的期望位姿参数，求得相应的多自由度臂各关节U}的位置参数，由此可用于对多自由度 臂进行控制，调整相机运动，实现对目标跟踪。
[0004] 目前，常规的运动学算法在求解逆运动学时，需要联合基坐标系下正运动学方程 和图像坐标系下的控制目标方程（目标点在相机坐标系下X、Y值为0)，求解困难，计算繁琐。
【发明内容】
[0005] 本发明的目的在于提供计算方式简便的双目视觉系统目标跟踪控制方法。
[0006] 为解决上述技术问题，本发明提供了一种双目视觉系统目标跟踪控制方法，包括 W下步骤：
[0007] Sl、通过正运动学取得目标点在基坐标系中的位置；
[000引S2、根据目标点在基坐标系中的位置，通过逆运动学取得多自由度机械臂各关节 期望位置；
[0009] S3、根据多自由度机械臂各关节期望位置W及多自由度机械臂当前位置计算出多 自由度机械臂各关节的控制角度；
[0010] 其中，在求解所述逆运动学中，引入虚拟移动变量d和虚拟坐标系促' }，虚拟坐标 系{Cl'}从基坐标系沿光轴方向移动d个单位，使虚拟坐标系{Cl'}的原点与目标点重合，通 过虚拟移动关节变量d和虚拟坐标系{打'}利用D-H法计算出多自由度机械臂各关节的期望 位置。
[0011 ] 其中，D-H法为Denavit和化Kenbe巧于1955年提出了 一种为关节链中的每一杆件 建立坐标系的矩阵方法，运种方法在机器人的每个连杆上都固定一个坐标系，然后用4X4 的齐次变换矩阵来描述相邻两连杆的空间关系。通过依次变换可最终推导出末端执行器相 对于基坐标系的位姿，从而建立机器人的运动学方程。
[0012] 作为优选的，目标点在基坐标系中的位置通过D-取去计算出。
[0013] 作为优选的，目标点在基坐标系中的位置的运动学模型为
[001引作为优选的，目标点在基坐标系中的位置为导戶托,，}^，而,），其中，
[0017]作为优选的，逆运动学的求解为n+1自由度机械臂的逆运动学求解。
[001引作为优选的，虚拟移动关节变量d的位置参数为d = f(xo，yo，zo)。
[0019] 作为优选的，多自由度机械臂各关节的位置参数为0i = f(xo，yo，zo)，（i = l，2,…， n) O
[0020] 作为优选的，正运动学的正运动学方程为
[0021 ]辦托於句=妙觸），晚）.…许贿）.於巧。
[0022] 作为优选的，通过正运动学方程求出多自由度机械臂位于目标位置时的多自由度 机械臂各关节的位置参数0 ' 1，求出关节控制量A 0。
[0023] 作为优选的，目标点处于运动状态时，双目视觉系统能够根据关节控制量A 0控制 多自由度机械臂移动，直至关节控制量A 0 = 0。
[0024] 本发明相比现有的双目视觉系统目标跟踪控制方法，利用虚拟移动关节变量d，替 换双目视觉系统目标跟踪控制方法的参数(Ul，vi)和(Ur, Vr),将n自由度双目视觉系统的逆 运动学求解转化为n+1自由度机械臂的逆运动学求解，运样便可W使用传统的D-H法进行求 解，具有算法巧妙、且计算量小的特点。
【附图说明】
[0025] 图1为本发明的正运动学算法模型示意图。
[0026] 图2为本发明的逆运动学算法模型示意图。
[0027] 图3为本发明的目标跟踪算法流程图。
【具体实施方式】
[0028] 为使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明的各实 施方式进行详细的阐述。
[0029] 在本发明的实施例中，对图1中所示的目标点位置信息，采用齐次变换可W建立运 动学模型，该运动学模型为
[0030] 件=巧?钟。y
[0031] 令目标点在基坐标系下的位置为^°巧而，y〇，z〇)，.在左眼相机坐标系下的位置为 ;'巧和,，yc,，?)，多自由度臂的各关节位置为0l(i = l，2，…，n)，则(l)式可写成 [003^ 拌柄，折,?) = ';7'的)?訂马巧
[00削式中，'乃白,）为多自由度臂的连杆变换，i = l，2,…，n;押为左眼相机相对多自由 度臂末关节In}的位姿。
[0034]目标点在左眼相机坐标系下的位置卽的，y(',，z(',），可W通过双目视觉模型求解， 如W下各式所示：
[003引式中kx，ky，UO，VO为相机内参数，通过标定得到；b为左右眼相机光轴的间距，且有 6 =馬-? ^ui, VI)和(Ur，Vr)分别为目标点在左右眼相机的图像坐标。
[0039] 因而，可由（2)式求得目标点在基坐标系下的位置;°p(.vy〇.Z。）。此即为多自由度 双目视觉系统的正运动学。
[0040] 如图2所示，在左眼相机光轴方向虚设一个移动关节，左眼相机坐标系{Cl}可W沿 相机光轴向前移动距离d，变为坐标系{C'l}，则d为关节变量。当坐标系{C'l}原点移动至与 目标点（坐标系{T}的原点）重合时，即为满足目标点位于左眼相机图像的中屯、。实现让右眼 相机光轴通过目标点。
[0041] 因为户=少，则（2)式可写为