《基于机械臂末端单目视觉的动态目标位置和姿态测量方法.pdf》由会员分享，可在线阅读，更多相关《基于机械臂末端单目视觉的动态目标位置和姿态测量方法.pdf（16页珍藏版）》请在专利查询网上搜索。
1、(10)申请公布号 CN 103759716 A (43)申请公布日 2014.04.30 CN 103759716 A (21)申请号 201410016272.0 (22)申请日 2014.01.14 G01C 11/08(2006.01) (71)申请人 清华大学 地址 100084 北京市海淀区清华园 1 号 (72)发明人 刘华平 陈杰 孙富春 卢山 刘宗明 张宇 (74)专利代理机构 北京清亦华知识产权代理事 务所 ( 普通合伙 ) 11201 代理人 罗文群 (54) 发明名称 基于机械臂末端单目视觉的动态目标位置和 姿态测量方法 (57) 摘要 本发明涉及一种基于机械臂末端单目。
2、视觉的 动态目标位置和姿态测量方法， 属于视觉测量领 域。该方法首先进行摄像机标定和手眼标定 ； 然 后用摄像机拍摄两幅图像， 利用尺度不变特征提 取方法， 提取图像中目标区域的空间特征点并进 行特征点匹配 ； 利用对极几何约束方法， 求解两 幅图像之间的基础矩阵， 得到本质矩阵， 进而求解 摄像机的旋转变换矩阵和位移变换矩阵 ； 之后对 特征点进行三维重构和尺度校正 ； 最后利用重构 后的特征点构建目标坐标系， 获得目标相对摄像 机的位置和姿态。 本发明方法采用单目视觉， 简化 计算过程， 使用了手眼标定， 可简化摄像机位姿信 息测量过程中错误解的剔除。本方法适用于测量 静止目标和低动态目标。
3、的相对位姿。 (51)Int.Cl. 权利要求书 3 页 说明书 9 页 附图 3 页 (19)中华人民共和国国家知识产权局 (12)发明专利申请 权利要求书3页 说明书9页 附图3页 (10)申请公布号 CN 103759716 A CN 103759716 A 1/3 页 2 1. 一种基于机械臂末端单目视觉的动态目标位置和姿态测量方法， 其特征在于该方法 包括以下步骤 ： （1） 设摄像机拍摄图像的平面坐标系为 （u， v） ， 采用张正友棋盘格标定法， 得到摄像机 的内参数矩阵 M，以及摄像机的径向畸变参数 k1和 k2以及切向畸变参 数 k3和 k4， 其中， f 为摄像机焦距，为摄。
4、像机在图像平面坐标系的 u 轴上的归一化焦距， 为摄像机在图像平面坐标系的 v 轴上的归一化焦距，（u0， v0） 为摄相机光轴与摄像机拍 摄图像平面交点的像素坐标 ； （2） 利用手眼标定法， 得到机械臂末端和摄像机之间的旋转变换矩阵， 记为以及机 械臂末端和摄像机之间的位移变换矩阵， 记为为 33 矩阵， 为 31 矩阵 ； （3） 在目标上设置两个标记点， 利用机械臂末端摄像机， 在两个不同位置拍摄到该目标 的两幅图像， 从两幅图像中区分出目标区域和背景区域， 利用尺度不变特征提取方法， 分别 从两幅图像中提取出目标区域的空间特征点， 该空间特征点包括目标上的两个标记点 ； （4） 根据。
5、上述空间特征点， 对两幅图像中目标区域的空间特征点进行匹配， 得到初始匹 配结果， 并根据初始匹配结果， 对步骤 （3） 的尺度不变特征提取方法中的参数进行调整， 以 获得两幅图像之间有 8 对以上匹配特征点， 匹配特征点中包括目标上的两个标记点 ； （5） 根据步骤 （4） 的两幅图像之间 8 对以上匹配特征点， 利用对极几何约束方法， 求解 两幅图像之间的基础矩阵 F ； （6） 根据上述步骤 （1） 的摄像机内参数矩阵 M 和步骤 （5） 的基础矩阵 F， 求解摄 像机在 拍摄目标的两幅图像时的两个不同位置之间的本质矩阵 E， 利用本质矩阵 E， 求解摄像机在 拍摄目标的两幅图像时的两个。
6、不同位置之间的旋转变换矩阵和位移变换矩阵 得到 四组候选解， 具体过程如下 ： （6-1） 利用如下公式， 根据步骤 （5） 的基础矩阵 F 和步骤 （1） 的摄像机内参数矩阵 M， 计 算摄像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵 E ： E MTFM， 其中 MT为矩阵 M 的转置矩阵 ； （6-2） 利用特征值分解方法， 对上述本质矩阵 E 进行分解， E Udiag(s,s,0)V， 得到 四组候选解 ： 其中 权 利 要 求 书 CN 103759716 A 2 2/3 页 3 （7） 根据机械臂的控制参数和上述步骤 （2） 得到的机械臂末端和摄像机之间的旋转变 换矩阵。
7、和位移变换矩阵计算得到摄像机在拍摄目标的两幅图像时的两个不同位置 之间的旋转初值和位移初值根据旋转初值和位移初值得到步骤 （6） 中的四 组候选解中的正解和具体过程如下 ： （7-1） 根据机械臂的控制参数得到机械臂末端的旋转变换矩阵和位移变换矩阵 根据上述步骤 （2） 得到的机械臂末端和摄像机之间的旋转变换矩阵和位移变换矩阵 通过下式计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转初值和 位移初值 （7-2） 分别计算上述步骤 （6） 得到的旋转矩阵候选解与旋转初值之间的旋转矩阵， 得到该旋转矩阵的范数， 将与该旋转矩阵范数中最小范数值相对应的候选解作为摄像机旋 转变换的正解 （7-。
8、3） 分别计算上述步骤 （6） 得到的位移矩阵候选解与位移初值之间的夹角， 将与 该夹角中最小夹角相对应的候选解作为摄像机位移变换的正解 （8） 根据上述步骤 （7） 中得到的摄像机旋转变换矩阵和位移变换矩阵对上述步 骤 （4） 得到的两幅图像之间的匹配特征点进行特征点三维重构， 根据目标上的两个标记点 之间的距离， 对摄像机的位移变换和三维重构的特征点进行尺度校正， 包括以下步骤 ： （8-1） 利用三角测量方法， 得到步骤 （4） 的两幅图像之间所有匹配特征点在摄像机坐标 系中的三维坐标 P ； （8-2） 从上述所有匹配特征点的三维坐标中， 得到目标上两个标记点之间的计算距离 d ； （。
9、8-3） 根据目标上的两个标记点之间的物理距离 D， 得到该物理距离与上述计算距离之 间的比值 （8-4） 根据上述比值 k， 对上述摄像机的位移变换矩阵和特征点在摄像机坐标系中 的三维坐标， 根据下式进行尺度校正， 得到尺度校正后的摄像机在拍摄目标的两幅图像时 所在的两个不同位置之间的物理位移以及三维重构特征点的物理坐标 P ： P kP （9） 构建目标坐标系， 求解目标的相对摄像机的位置和姿态， 具体过程如下 ： （9-1） 根据上述步骤 （8） 得到的三维重构特征点的物理坐标， 构建目标坐标系， 以目标上 3 个特征点为例构建目标坐标系， i， j， k 分别为目标坐标系 XYZ 坐标。
10、轴的单 权 利 要 求 书 CN 103759716 A 3 3/3 页 4 位向量， 坐标原点为点 A， AB 为 X 轴， 以 ABC 平面中垂直 AB 的方向为 Y 轴， 然后依据 右手法 则得到 Z 轴， 设 Pa， Pb， Pc为 3 特征点在摄像机坐标系下的坐标， 各坐标轴的单位向量计算如 下 ： （9-2） 依据如下公式计算任意目标特征点在目标坐标系中的坐标 oPi oPi i j k(Pi-Pa) 其中， Pi为任意特征点的重构坐标 ； （9-3） 利用坐标系转换方法或基于点特征定位的方法， 根据任意目标特征点在目标坐 标系中的坐标oPi和重构坐标 Pi， 计算目标的相对摄像机。
11、的位置和相对旋转矩阵 （9-4） 根据上述相对旋转矩阵计算目标的相对姿态， 以欧拉角 XYZ 顺序表示目标姿 态 （,)， 则旋转矩阵表示如下 ： 从上述旋转矩阵中， 得到目标的相对摄像机的姿态为 ： =asin(-Roc(1,3) 。 权 利 要 求 书 CN 103759716 A 4 1/9 页 5 基于机械臂末端单目视觉的动态目标位置和姿态测量方法 技术领域 0001 本发明涉及一种基于机械臂末端单目视觉的动态目标位置和姿态测量方法， 属于 视觉测量领域。 背景技术 0002 近年来， 视觉测量技术得到了深入的发展， 广泛应用与工业检测、 机器人视觉、 实 时测量等过程， 采用双目视觉。
12、和单目视觉是比较常用的方法。 相对双目视觉， 单目视觉不仅 克服了双目视觉中基线固定的约束， 同时将单目摄像机安装在载体末端， 可通过载体的运 动对目标不同的特征部位进行测量， 且不会发生遮挡现象。 0003 但现有单目视觉测量方法都假设场景静止， 即场景中机械臂平台和被测目标都静 止不动， 而没有考虑在摄像机移动的同时， 测量目标可能存在一定的缓慢运动。 因而不能准 确完成动态场景中的测量任务。 0004 经对现有的技术文献检索分析， 发现曾庆化等人的发明专利 “一种用于室内环境 的单目视觉 / 惯性全自主导航方法” ， 申请号为 201110273402.5， 该专利实现的方法以图像 局部。
13、不变特性和对极几何为基础估计摄像机位姿信息， 最后将视觉导航信息和惯导信息结 合获得相对精确、 可靠的导航信息， 并进行特征点 3D 重构， 获得环境信息地图， 完成载体的 自主导航。但该方法需要额外的惯性器件获取惯导信息， 并且其获取特征点 3D 信息用来构 建环境信息， 而没有对目标位姿进行测量。 发明内容 0005 本发明的目的是提出一种基于机械臂末端单目视觉的动态目标位置和姿态测量 方法， 用于测量动态目标的位置和姿态。 0006 本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法， 包括以 下步骤 ： 0007 （1） 设摄像机拍摄图像的平面坐标系为 （u， v） ， 采。
14、用张正友棋盘格标定法， 得到摄 像机的内参数矩阵 M，以及摄像机的径向畸变参数 k1和 k2以及切向畸 变参数k3和k4， 其中， f为摄像机焦距，为摄像机在图像平面坐标系的u轴上的归一化焦 距，为摄像机在图像平面坐标系的 v 轴上的归一化焦距，（u0， v0） 为摄相机光轴与摄像机 拍摄图像平面交点的像素坐标 ； 说 明 书 CN 103759716 A 5 2/9 页 6 0008 （2） 利用手眼标定法， 得到机械臂末端和摄像机之间的旋转变换矩阵， 记为以 及机械臂末端和摄像机之间的位移变换矩阵， 记为为 33 矩阵， 为 31 矩阵 ； 0009 （3） 在目标上设置两个标记点， 利用。
15、机械臂末端摄像机， 在两个不同位置拍摄到该 目标的两幅图像， 从两幅图像中区分出目标区域和背景区域， 利用尺度不变特征提取方法， 分别从两幅图像中提取出目标区域的空间特征点， 该空间特征点包括目标上的两个标记 点 ； 0010 （4） 根据上述空间特征点， 对两幅图像中目标区域的空间特征点进行匹配， 得到初 始匹配结果， 并根据初始匹配结果， 对步骤 （3） 的尺度不变特征提取方法中的参数进行调 整， 以获得两幅图像之间有 8 对以上匹配特征点， 匹配特征点中包括目标上的两个标记点 ； 0011 （5） 根据步骤 （4） 的两幅图像之间 8 对以上匹配特征点， 利用对极几何约束方法， 求解两幅。
16、图像之间的基础矩阵 F ； 0012 （6） 根据上述步骤 （1） 的摄像机内参数矩阵 M 和步骤 （5） 的基础矩阵 F， 求解摄像 机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵 E， 利用本质矩阵 E， 求解摄像 机在拍摄目标的两幅图像时的两个不同位置之间的旋转变换矩阵和位移变换矩阵得 到四组候选解， 具体过程如下 ： 0013 （6-1） 利用如下公式， 根据步骤 （5） 的基础矩阵 F 和步骤 （1） 的摄像机内参数矩阵 M， 计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵 E ： 0014 E MTFM， 其中 MT为矩阵 M 的转置矩阵 ； 0015 （6-2。
17、）本质矩阵 E 只和旋转变换矩阵与位移变换矩阵有关， 式中为的反对称矩阵， 利用特征值分解方法， 对上述本质矩阵 E 进行分解， E Udiag(s,s,0)V， 得到四组候选解 ： 0016 其中 0017 （7） 根据机械臂的控制参数和上述步骤 （2） 得到的机械臂末端和摄像机之间的旋 转变换矩阵和位移变换矩阵计算得到摄像机在拍摄目标的两幅图像时的两个不同 位置之间的旋转初值和位移初值根据旋转初值和位移初值得到步骤 （6） 中 的四组候选解中的正解和具体过程如下 ： 0018 （7-1） 根据机械臂的控制参数得到机械臂末端的旋转变换矩阵和位移变换矩 阵根据上述步骤 （2） 得到的机械臂末端。
18、和摄像机之间的旋转变换矩阵和位移变换 矩阵通过下式计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转初值 和位移初值 说 明 书 CN 103759716 A 6 3/9 页 7 0019 0020 0021 （7-2） 分别计算上述步骤 （6） 得到的旋转矩阵候选解与旋转初值之间的旋转矩 阵， 得到该旋转矩阵的范数， 将与该旋转矩阵范数中最小范数值相对应的候选解作为摄像 机旋转变换的正解 0022 （7-3） 分别计算上述步骤 （6） 得到的位移矩阵候选解与位移初值之间的夹角， 将与该夹角中最小夹角相对应的候选解作为摄像机位移变换的正解 0023 （8） 根据上述步骤 （7） 中得到的。
19、摄像机旋转变换矩阵和位移变换矩阵对 上述步骤 （4） 得到的两幅图像之间的匹配特征点进行特征点三维重构， 根据目标上的两个 标记点之间的距离， 对摄像机的位移变换和三维重构的特征点进行尺度校正， 包括以下步 骤 ： 0024 （8-1） 利用三角测量方法， 得到步骤 （4） 的两幅图像之间所有匹配特征点在摄像机 坐标系中的三维坐标 P ； 0025 （8-2） 从上述所有匹配特征点的三维坐标中， 得到目标上两个标记点之间的计算 距离 d ； 0026 （8-3） 根据目标上的两个标记点之间的物理距离 D， 得到该物理距离与上述计算距 离之间的比值 0027 （8-4） 根据上述比值 k， 对上。
20、述摄像机的位移变换矩阵和特征点在摄像机坐标 系中的三维坐标， 根据下式进行尺度校正， 得到尺度校正后的摄像机在拍摄目标的两幅图 像时所在的两个不同位置之间的物理位移以及三维重构特征点的物理坐标 P ： 0028 0029 P kP 0030 （9） 构建目标坐标系， 求解目标的相对摄像机的位置和姿态， 具体过程如下 ： 0031 （9-1） 根据上述步骤 （8） 得到的三维重构特征点的物理坐标， 构建目标坐标系， 以 目标上 3 个特征点为例构建目标坐标系， i， j， k 分别为目标坐标系 XYZ 坐标轴的单位向量， 坐标原点为点 A， AB 为 X 轴， 以 ABC 平面中垂直 AB 的方。
21、向为 Y 轴， 然后依据右手法则得到 Z 轴， 设 Pa， Pb， Pc为 3 特征点在摄像机坐标系下的坐标， 各坐标轴的单位向量计算如下 ： 0032 0033 （9-2） 依据如下公式计算任意目标特征点在目标坐标系中的坐标 oPi： 0034 oPi i j k(Pi-Pa) 说 明 书 CN 103759716 A 7 4/9 页 8 0035 其中， Pi为任意特征点的重构坐标 ； 0036 （9-3） 利用坐标系转换方法或基于点特征定位的方法， 根据任意目标特征点在目 标坐标系中的坐标 oPi和重构坐标 Pi， 计算目标的相对摄像机的位置 和相对旋转矩阵 0037 （9-4） 根据上。
22、述相对旋转矩阵计算目标的相对姿态， 以欧拉角 XYZ 顺序表示目 标姿态 （,)， 则旋转矩阵表示如下 ： 0038 0039 0040 0041 从上述旋转矩阵中， 得到目标的相对摄像机的姿态为 ： 0042 =asin(-Roc(1,3) 0043 0044 0045 至此， 已得到目标的相对位置和姿态。 0046 本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法， 具有以 下优点 ： 0047 1、 本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法， 采用 单目视觉， 简化计算过程法， 动态目标位置和姿态测量所需的硬件， 而且克服了双目视觉的 不足。 0048。
23、 2、 本发明测量方法中， 摄像机安装在机械臂末端， 可对目标的不同特征部位进行 测量， 且不会发生遮挡现象。 0049 3、 本发明测量方法中使用了手眼标定， 可简化摄像机位姿信息测量过程中错误解 的剔除。 0050 4、 本发明测量方法适用于测量静止目标和低动态目标的相对位姿。 附图说明 0051 图 1 是本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法 的流程框图。 0052 图 2 是本发明测量方法中使用的手眼标定原理示意图。 0053 图 3 是本发明测量方法中立体视觉对极几何关系示意图。 0054 图 4 是本发明测量方法中目标坐标系示意图。 说 明 书 CN 10。
24、3759716 A 8 5/9 页 9 具体实施方式 0055 本发明提出的基于机械臂末端单目视觉的动态目标位置和姿态测量方法， 其流程 框图如图 1 所示， 包括以下步骤 ： 0056 （1） 设摄像机拍摄图像的平面坐标系为 （u， v） ， 采用张正友棋盘格标定法， 得到摄 像机的内参数矩阵 M，以及摄像机的径向畸变参数 k1和 k2以及切向畸 变参数 k3和 k4， 其中， f 为摄像机焦距，为摄像机在图像平面坐标系的 u 轴上的归一化焦 距，为摄像机在图像平面坐标系的 v 轴上的归一化焦距，（u0， v0） 为摄相机光轴与摄像机 拍摄图像平面交点的像素坐标 ； 0057 （2） 利用手。
25、眼标定法， 得到机械臂末端和摄像机之间的旋转变换矩阵， 记为以 及机械臂末端和摄像机之间的位移变换矩阵， 记为为 33 矩阵， 为 31 矩阵 ； 0058 图 2 是手眼标定基本原理示意图。如图 2 所示 ： Cobj表示定标物坐标系， Cc1和 Ce1 表示机械臂运动前的摄像机坐标系和机械臂末端坐标系， Cc2和 Ce2表示机械臂运动后的摄 像机坐标系和机械臂末端坐标系 ； A、 B、 C、 D、 X 分别表示其连接的两个坐标系之间的相对方 位， 为 44 矩阵， 包括两个坐标系之间的旋转矩阵 Ri和平移向量 ti， 下标 i 表示矩阵名称， 以 X 为例进行说明， 则由图 3 可得到如下。
26、关系 ： 0059 CX XD 0060 展开后可得到 ： 0061 RcRx RxRd 0062 Rctx+tc Rxtd+tx 0063 标定时， 首先控制机械臂运动， 确保该运动不是纯平移， 得到 A 和 B， 从控制器参数 中得到 D， 根据上式得到一组约束方程 ； 控制机械臂再次运动， 确保此次运动和上次运动的 旋转轴不互相平行， 且此次运动也不是纯平移， 同理， 根据上式得到一组约束方程 ； 根据这 两组约束方程， 求解 Rx和 tx， 即为机械臂末端和摄像机之间的旋转变换矩阵和位移变换 矩阵从而完成了手眼标定。 0064 （3） 在目标上设置两个标记点， 利用机械臂末端摄像机， 。
27、在两个不同位置拍摄到该 目标的两幅图像， 从两幅图像中区分出目标区域和背景区域， 利用尺度不变特征提取方法， 分别从两幅图像中提取出目标区域的空间特征点， 该空间特征点包括目标上的两个标记 点 ； 说 明 书 CN 103759716 A 9 6/9 页 10 0065 （4） 根据上述空间特征点， 对两幅图像中目标区域的空间特征点进行匹配， 得到初 始匹配结果， 并根据初始匹配结果， 对步骤 （3） 的尺度不变特征提取方法中的参数进行调 整， 以获得两幅图像之间有 8 对以上匹配特征点， 匹配特征点中包括目标上的两个标记点 ； 0066 （5） 根据步骤 （4） 的两幅图像之间 8 对以上匹。
28、配特征点， 利用对极几何约束方法， 求解两幅图像之间的基础矩阵 F ； 0067 图 3 表示立体视觉中的对极几何约束关系。如图 3 所示 ： 基线为连接两摄像机光 心 O(O) 的直线， 对极点 e(e) 为基线与像平面的交点， 对极平面为过基线与特定点 P 的平 面， 极线是对极平面与像平面的交线， p(p) 为 P 在成像平面上的投影， 投影点和其对应的 极线满足关系 l Fp， 空间中任意点在两图像平面上的成像点像素坐标满足如下的对极 几何约束关系， 即极线几何约束 ： 0068 pFp 0 0069 基础矩阵 F 的秩为 2， 自由度为 7， 可根据 8 点算法， 由步骤 （4） 得。
29、到的 8 对以上的 特征点对， 计算基础矩阵 F。 0070 （6） 根据上述步骤 （1） 的摄像机内参数矩阵 M 和步骤 （5） 的基础矩阵 F， 求解摄像 机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵 E， 利用本质矩阵 E， 求解摄像 机在拍摄目标的两幅图像时的两个不同位置之间的旋转变换矩阵和位移变换矩阵 得到四组候选解， 具体过程如下 ： 0071 （6-1） 利用如下公式， 根据步骤 （5） 的基础矩阵 F 和步骤 （1） 的摄像机内参数矩阵 M， 计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的本质矩阵 E ： 0072 E MTFM， 其中 MT为矩阵 M 的转置矩阵。
30、 ； 0073 （6-2）本质矩阵 E 只和旋转变换矩阵与位移变换矩阵有关， 式中为的反对称矩阵， 利用特征值分解方法， 对上述本质矩阵 E 进行分解， E Udiag(s,s,0)V， 得到四组候选解 ： 0074 其中 0075 （7） 根据机械臂的控制参数和上述步骤 （2） 得到的机械臂末端和摄像机之间的旋 转变换矩阵和位移变换矩阵计算得到摄像机在拍摄目标的两幅图像时的两个不同 位置之间的旋转初值和位移初值根据旋转初值和位移初值得到步骤 （6） 中 的四组候选解中的正解和具体过程如下 ： 0076 （7-1） 根据机械臂的控制参数得到机械臂末端的旋转变换矩阵和位移变换矩 阵根据上述步骤 。
31、（2） 得到的机械臂末端和摄像机之间的旋转变换矩阵和位移变换 矩阵通过下式计算摄像机在拍摄目标的两幅图像时的两个不同位置之间的旋转初值 说 明 书 CN 103759716 A 10 7/9 页 11 和位移初值 0077 0078 0079 （7-2） 分别计算上述步骤 （6） 得到的旋转矩阵候选解与旋转初值之间的旋转 矩阵， 得到该旋转矩阵的范数， 将与该旋转矩阵范数中最小范数值相对应的候选解作为摄 像机旋转变换的正解 0080 （7-3） 分别计算上述步骤 （6） 得到的位移矩阵候选解与位移初值之间的夹角， 将与该夹角中最小夹角相对应的候选解作为摄像机位移变换的正解 0081 （8） 根。
32、据上述步骤 （7） 中得到的摄像机旋转变换矩阵和位移变换矩阵对 上述步骤 （4） 得到的两幅图像之间的匹配特征点进行特征点三维重构， 根据目标上的两个 标记点之间的距离， 对摄像机的位移变换和三维重构的特征点进行尺度校正， 包括以下步 骤 ： 0082 （8-1） 利用三角测量方法， 得到步骤 （4） 的两幅图像之间所有匹配特征点在摄像机 坐标系中的三维坐标 P ； 0083 空间中任意点在摄像机坐标系下的投影坐标可由如下公式计算 ： 0084 0085 0086 其中， u1,v1,1T和u2,v2,1T分别为摄像机拍摄的两幅图像中对应点的像素齐次 坐标， X,Y,Z,1T为空间点在世界坐标。
33、系下的齐次坐标， 设世界坐标系与摄像机运动前的 摄像机坐标系重合， 则将上式展开， 消除 Zc1和 Zc2， 可 得到关于 X， Y， Z 的四个线性方程 ： 0087 0088 上述方程组有 3 个变量 4 个方程， 由于已经假设对应像素点和摄像机坐标原点的 直线一定相交， 即方程组必定有唯一解， 实际上， 由于数据总有噪声， 可采用最小二乘法求 说 明 书 CN 103759716 A 11 8/9 页 12 解 X， Y， Z， 从而完成特征点的三维重构。 0089 （8-2） 从上述所有匹配特征点的三维坐标中， 得到目标上两个标记点之间的计算 距离 d ； 0090 （8-3） 根据目。
34、标上的两个标记点之间的物理距离 D， 得到该物理距离与上述计算距 离之间的比值 0091 （8-4） 根据上述比值 k， 对上述摄像机的位移变换矩阵和特征点在摄像机坐标 系中的三维坐标， 根据下式进行尺度校正， 得到尺度校正后的摄像机在拍摄目标的两幅图 像时所在的两个不同位置之间的物理位移以及三维重构特征点的物理坐标 P ： 0092 0093 P kP 0094 （9） 构建目标坐标系， 求解目标的相对摄像机的位置和姿态， 具体过程如下 ： 0095 （9-1） 根据上述步骤 （8） 得到的三维重构特征点的物理坐标， 构建目标坐标系， 0096 本发明的一个实施例中， 以目标上 3 个特征点。
35、为例构建目标坐标系， 如图 4 所示 ： A， B， C 为目标上的 3 个特征点， i， j， k 分别为目标坐标系 XYZ 坐标轴的单位向量， 坐标原点 为点 A， AB 为 X 轴， 以 ABC 平面中垂直 AB 的方向为 Y 轴， 然后依据右手法则得到 Z 轴， 设 Pa， Pb， Pc为 3 特征点在摄像机坐标系下的坐标， 各坐标轴的单位向量计算如下 ： 0097 0098 （9-2） 依据如下公式计算任意目标特征点在目标坐标系中的坐标 oPi： 0099 oPi i j k(Pi-Pa) 0100 其中， Pi为任意特征点的重构坐标 ； 0101 （9-3） 利用坐标系转换方法或基。
36、于点特征定位的方法， 根据任意目标特征点在目 标坐标系中的坐标oPi和重构坐标Pi， 计算目标的相对摄像机的位置和相对旋转矩阵 0102 （9-4） 根据上述相对旋转矩阵计算目标的相对姿态， 本发明的一个实施实例 中， 以欧拉角 XYZ 顺序表示目标姿态 （,)， 则旋转矩阵表示如下 ： 0103 0104 0105 说 明 书 CN 103759716 A 12 9/9 页 13 0106 从上述旋转矩阵中， 得到目标的相对摄像机的姿态为 ： 0107 =asin(-Roc(1,3) 0108 0109 0110 至此， 已得到目标的相对位置和姿态。 说 明 书 CN 103759716 A 13 1/3 页 14 图 1 说 明 书 附 图 CN 103759716 A 14 2/3 页 15 图 2 图 3 说 明 书 附 图 CN 103759716 A 15 3/3 页 16 图 4 说 明 书 附 图 CN 103759716 A 16 。