基于双目相机的3D三角测量方法及装置制造方法及图纸

本申请公开了一种基于双目相机的3D三角测量方法。该基于双目相机的3D三角测量方法包括建立双目相机的坐标系，并获取坐标系参数；通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；通过双目相机的运动减小空间点的实际像素和预期像素的偏差；获取左相机和右相机的光轴，得到固定点p。本申请解决了由于立体外部参数和视差的不准确对3D三角测量造成的不利影响的技术问题。

【技术实现步骤摘要】
基于双目相机的3D三角测量方法及装置
本申请涉及双目主动视觉领域，具体而言，涉及一种基于双目相机的3D三角测量方法。
技术介绍
基于双目主动视觉的3D三角测量技术在计算机视觉和机器人技术中的应用越来越多，双眼主动视觉有益于多种应用，例如操纵，3D重建，导航，3D映射等。基于双目主动视觉的3D坐标估计由于其非接触，低成本和高精度的优点吸引了广泛的研究兴趣。双目主动视觉系统可分为两类：第一类具有固定相机，第二类具有非固定相机。第二类可以提高灵活性并扩展视野，它与人类视觉系统更相似。通常，可以使用视差和立体外部参数执行3D三角测量。立体外部参数可以离线或在线进行校准。第一类主动视觉系统仅需要离线校准，其立体外部参数是固定的，因为一个相机关于另一个相机是静态的。第二类立体外部参数需要在线校准，因为外部参数可能一直在变化。相关技术中采用集成双重校正(ITPC)的方法，通过正向运动学和校正后的头眼参数在线计算立体外部参数，使用立体外部参数执行基于视差的3D三角测量，以估计3D空间点的坐标。但是，在基于视差的三角测量方法中，立体外部参数和视差的准确性对3D三角测量有重大影响。针对基于视差的三角测量方法中由于立体外部参数和视差的不准确对3D三角测量造成的不利影响的问题，目前尚未提出有效的解决方案。
技术实现思路
本申请的主要目的在于提供一种基于双目相机的3D三角测量方法，以解决由于立体外部参数和视差的不准确对3D三角测量造成的不利影响的技术问题。为了实现上述目的，根据本申请的一个方面，提供了一种基于双目相机的3D三角测量方法及装置。第一方面，本申请提供了一种基于双目相机的3D三角测量方法。根据本申请的基于双目相机的3D三角测量方法包括：建立双目相机的坐标系，并获取坐标系参数；通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；通过双目相机的运动减小空间点的实际像素和预期像素的偏差；获取左相机和右相机的光轴，得到固定点p。进一步的，建立双目相机的坐标系，并获取坐标系参数包括：基于标准D-H方法建立坐标系；参数di，θi，ai，αi分别是连杆偏距，关节角，连杆长度和连杆转角；关节偏移量是初始状态下θi的值；坐标系i和i-1之间的转换矩阵为：进一步的，通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵包括：左相机和右相机分别是左眼和右眼的末端执行器；末端执行器的速度Vn＝[vxvyvzωxωyωz]T；其中，vx，vy，vz是线速度，ωx，ωy，ωz为角速度；关节速度关节雅可比矩阵且通过公式进行计算；其中，Zi和Oi是Z轴的方向向量和坐标系Oi-xiyizi的原点；通过公式将Jθ从基本坐标系转换到相机坐标系；其中，Jjoint是变换后的关节雅可比矩阵，R是基础坐标系关于相机坐标系的旋转矩阵；则，Vn和之间的关系为3D空间点的坐标p＝(u，v)；则，像素速度和末端执行器速度Vn之间的关系为其中，Jimage是图像雅可比矩阵；且，图像雅可比矩阵其中，fu和fv分别是相机的列和行方向的焦距，Zc是相机坐标系中空间点p的深度。进一步的，通过双目相机的运动减小空间点的实际像素和预期像素的偏差包括：空间点的实际像素坐标和预期像素坐标分别为p和p*；则，p和p*的偏差e＝p*-p；则，其中，是像素速度；其中，K是影响视觉注视性能的常数矩阵；构造常数矩阵其中，k1和k2是每个通道中的增益；则，空间点的像素速度则，在左右相机的连接处建立坐标系ON-xNyNzN，则左眼关节速度矢量为：则，其中，pl和是左相机中空间点的实际像素坐标和预期像素坐标；则，左眼的关节速度为其中，是矩阵Jl中第i行和第j列上的元素；同理，右眼得关节速度为优选的，当左相机和右相机的光轴不在一个平面上时，将两个偏斜光轴的共同垂直线段的中点作为固定点p。具体的，当左相机和右相机的光轴不在一个平面上时，将两个偏斜光轴的共同垂直线段的中点作为固定点p包括：Ll和Lr是同时通过固定点p的理想光轴，L′l和L′r是实际的光轴，点A和B在L′l上，点C和D在L′r上；在双目相机的初始状态下，L′l和L′r分别与和轴的方向相同；点A，B，C，D的齐次坐标分别为(xa，ya，za，1)，(xb，yb，zb，1)，(xc，yc，zc，1)和(xd，yd，zd，1)；点A，B，C，D的初始齐次坐标分别为(0，0，za′，1)，(0，0，zb′，1)，(0，0，zc′，1)和(0，0，zd′，1)；则，A的齐次坐标为：同理可得点B，C和D的齐次坐标；X1＝xb-xa，Y1＝yb-ya，Z1＝zb-za，X2＝xd-xc，Y2＝yd-yc和Z2＝zd-zc，则，L′l和L′r分别为则，L′l和L′r的公垂线的方向向量为：则，由L′l和公共垂直线确定的平面为：Xcp(X-xa)+Ycp(Y-ya)+Zcp(Z-za)＝0则，解得坐标p1＝(u1，v1)，同理得到坐标p2＝(u2，v2)；通过确定坐标系中点p1和p2的中点坐标来确定固定点坐标第二方面，本申请提供一种基于双目相机的3D三角测量装置，该装置包括：坐标系建立模块：用于建立双目相机的坐标系，并获取坐标系参数；信息获取模块：用于通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；偏差减小模块：用于通过双目相机的运动减小空间点的实际像素和预期像素的偏差；固定点获取模块：用于获取左相机和右相机的光轴，得到固定点p。第三方面，本申请提供一种电子设备，包括存储器、处理器及存储在存储器上并可在处理器上运行的计算机程序，所述处理器执行所述程序时实现第一方面提供的基于双目相机的3D三角测量方法的步骤。第四方面，本申请提供一种非暂态计算机可读存储介质，其上存储有计算机程序，该计算机程序被处理器执行时实现第一方面提供的基于双目相机的3D三角测量方法的步骤。在本申请实施例中，通过计算相机两个光轴的相交点来执行3D三角测量，从而避免了使用图像视差或立体外部参数的缺点，达到了具有较好的性能和较小的不确定度的目的，解决了由于立体外部参数和视差的不准确对3D三角测量造成的不利影响的技术问题。附图说明构成本申请的一部分的附图用来提供对本申请的进一步理解，使得本申请的其它特征、目的和优点变得更明显。本申请的示意性实施例附图及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：图1是根据本申请实施例的一种基于双目相机的3D三角测量方法的流程示意图；图2是根据本申请实施例的双目相机的坐标系；图3是根据本申请实施例的基于视觉注视的3D三角测量方法；以本文档来自技高网...

【技术保护点】
1.一种基于双目相机的3D三角测量方法，其特征在于，包括：/n建立双目相机的坐标系，并获取坐标系参数；/n通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；/n通过所述双目相机的运动减小空间点的实际像素和预期像素的偏差；/n获取左相机和右相机的光轴，得到固定点p。/n

【技术特征摘要】
1.一种基于双目相机的3D三角测量方法，其特征在于，包括：
建立双目相机的坐标系，并获取坐标系参数；
通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵；
通过所述双目相机的运动减小空间点的实际像素和预期像素的偏差；
获取左相机和右相机的光轴，得到固定点p。


2.根据权利要求1所述的一种基于双目相机的3D三角测量方法，其特征在于，所述建立双目相机的坐标系，并获取坐标系参数包括：
基于标准D-H方法建立坐标系；
参数di，θi，ai，αi分别是连杆偏距，关节角，连杆长度和连杆转角；关节偏移量是初始状态下θi的值；
坐标系i和i-1之间的转换矩阵为：





3.根据权利要求1所述的一种基于双目相机的3D三角测量方法，其特征在于，所述通过视觉注视，获取末端执行器速度、关节速度、关节雅可比矩阵包括：
所述左相机和所述右相机分别是左眼和右眼的末端执行器；
所述末端执行器的速度Vn＝[vxvyvzωxωyωz]T；其中，vx，vy，vz是线速度，ωx，ωy，ωz为角速度；
所述关节速度
所述关节雅可比矩阵
且所述通过公式确定；其中，Zi和Oi是Z轴的方向向量和坐标系Oi-xiyizi的原点；
通过公式将所述Jθ从基本坐标系转换到相机坐标系；其中，Jjoint是变换后的关节雅可比矩阵，R是基础坐标系关于相机坐标系的旋转矩阵；
则，所述Vn和所述之间的关系为
3D空间点的坐标p＝(u,v)；
则，像素速度和所述Vn之间的关系为其中，Jimage是图像雅可比矩阵；
且，所述图像雅可比矩阵其中，fu和fv分别是相机的列和行方向的焦距，Zc是相机坐标系中空间点p的深度。


4.根据权利要求1所述的一种基于双目相机的3D三角测量方法，其特征在于，所述通过所述双目相机的运动减小空间点的实际像素和预期像素的偏差包括:
空间点的实际像素坐标和预期像素坐标分别为p和p*；
则，所述p和所述p*的偏差e＝p*-p；
则，其中，是像素速度；
所述其中，K是影响视觉注视性能的常数矩阵；
构造所述常数矩阵其中，k1和k2是每个通道中的增益；
则，空间点像素速度
则，
在所述左相机和所述右相机的连接处建立坐标系ON-xNyNzN，则所述左眼关节速度矢量
则，其中，pl和是所述所述左相机中空间点的实际像素坐标和预期像素坐标；
则，所述左眼关节速度为其中，是矩阵Jl中第i行和第j列上的元素；
同理，所述右眼关节速度为


5.根据权利要求1所述的一种基于双目相机的3D三角测量方法，其特征在于，所述获取所述左相机和所述右相机的光轴，得到固定点p包括
建立所述左眼坐标系为O6-x6y6z6，所述右眼坐标系为O9-x9y9z9，所述左相机坐标系为所述右相机坐标系为且Ll和Lr分别是所述左相机和...

【专利技术属性】
技术研发人员：陈晓鹏，黄强，王启航，徐德，樊迪，高峻峣，余张国，陈学超，
申请(专利权)人：北京理工大学，
类型：发明
国别省市：北京;11