CN103791889A - 一种利用十字结构光辅助的单目视觉位姿测量方法 - Google Patents

Abstract

本发明提供了一种利用十字结构光辅助的单目视觉位姿测量方法，将相机与十字结构光传感器同向放置，根据物体表面的直线特征调整测量装置，使十字结构光与物体表面的一条直线有两个交点，测量时通过处理器对相机捕获的图像进行处理计算，得到物体于相机之间的相对位置和姿态信息。本发明实现了在相机对缺乏平行约束条件的非合作目标位姿的测量，大大简化了计算，提高了运算效率，满足室内微小型载体的载重要求。

Description

一种利用十字结构光辅助的单目视觉位姿测量方法

技术领域

本发明涉及位姿测量领域，具体是一种利用十字结构光辅助的单目视觉位姿测量方法。

背景技术

利用视觉进行位姿测量具有精度高、非接触和低成本等优点，根据测量所需的相机数一般可分为单目视觉测量和多目视觉测量。单目视觉测量由于结构简单、测量灵活，仅使用一台相机即可得到目标相对于相机的三维姿态和三维位移数据，得到了广泛的研究和应用。

目前，国内外对视觉位姿测量进行了大量并且深入的研究，视觉位姿测量可以分为对合作目标的测量和对非合作目标的测量，其中，合作测量算法已经较为成熟，比较成熟的算法是利用目标物体上N个特征点在物体坐标系下的位置关系进行求解，即PNP问题，而非合作视觉测量算法却无法事先获得特征点在物体坐标系下的坐标。非合作视觉测量算法的相关公开文献较少，大部分研究方法都是致力于将非合作问题转化为合作问题来处理，这样的方法在实际应用中有一定的局限性。目前部分直接进行非合作视觉测量的算法需要利用激光测距仪辅助获得位置和姿态信息，否则只能获得物体的姿态信息，由于激光测距仪重量与尺寸较大，不适合应用在室内微小型载体上进行自主导航；有专利利用单幅图像上具有平行四边形几何约束的四个特征点实现物体的空间定位；有论文针对无法获取大型非合作航天器完整特征图像的情况进行位姿求解，利用线结构光辅助获得航天器平行边上的四个特征点，同样利用平行约束条件进行位姿求解，以上两种方法在无法获得平行约束时无法进行位姿解算。总的来说，目前对非合作目标进行视觉位姿测量的方法存在计算量较大、辅助设备尺寸重量大以及需要平行约束条件，并不完全满足室内微小型载体视觉导航的要求。

发明内容

本发明为了解决室内微小型载体视觉导航时的位姿测量问题，提出了十字结构光辅助的单目视觉位姿测量方法，实现了在相机对缺乏平行约束条件的非合作目标位姿的测量，满足室内微小型载体的载重要求，同时计算量小，大大增加了计算效率。

一种利用十字结构光辅助的单目视觉位姿测量方法，其特征在于包括以下步骤：

1）建立相机坐标系C-X_cY_cZ_c、图像坐标系o-uv、十字结构光坐标系L-X_lY_lZ_l以及物体坐标系O_o-X_oY_oZ_o，其中相机坐标系、图像坐标系与通常的定义一致；十字结构光坐标系的原点为十字结构光发出点，Z_l轴与十字结构光两平面的交线重合，方向与光线射出方向一致，X_l轴、Y_l轴分别沿两个垂直光线平面与Z_l轴构成右手系；物体坐标系原点位于物体表面某一线段的端点，X_o轴与该线段重合指向另一端点，Z_o轴垂直物体表面向外，Y_o轴与X_o轴、Z_o轴构成右手系；

2）定义物体坐标系相对相机坐标系的旋转关系R与平移关系T，其中R表示物体坐标系先绕X_o轴逆时针转动γ角，再绕Y_o轴逆时针转动θ角，最后绕Z_o轴逆时针转动ψ角，即 $R=\left(\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right);$

3）将相机与十字结构光传感器同向放置并进行标定，包括相机内参、相机坐标系与十字结构光坐标系间的位移和旋转关系；使十字结构光在物体表面的亮斑与物体表面在X_o轴上的线段有两个交点，此交点即为特征点，相机捕捉这两点以及十字结构光与物体表面轮廓另外任意一个交点；

4）利用图像处理算法获得步骤3）中所需的三个交点，利用单点三角测距法计算这三个点在相机坐标系下的坐标

(i＝0,1,2)，得到特征点在物体坐标系下X_o轴方向的差，表示为b；

5）利用步骤2）中定义的旋转关系R描述步骤4）中获得的三个点，得到三个方程组，利用这三个点在物体坐标系下的关系，通过线性变换对方程组进行化简消元，得到物体坐标系三轴相对相机坐标系的旋转角度

$\mathrm{\γ}=\arctan =\frac{(z_1^c-z_0^c)\cos \mathrm{\θ}-(y_1^c-y_0^c)\sin \mathrm{\θ}\cos \mathrm{\ψ}-(x_1^c-x_0^c)\sin \mathrm{\θ}\sin \mathrm{\ψ}}{\cos \mathrm{\ψ}(y_1^c-y_0^c)-\sin \mathrm{\ψ}(x_1^c-x_0^c)};$

6）利用步骤2）中定义的旋转矩阵R以及步骤3）得到的三个交点中任意一点在相机坐标系下的坐标，可以得到该点在物体坐标系下的坐标，进而获得物体坐标系原点在相机坐标系下的坐标，即可得到物体坐标系与相机坐标系之间的平移矩阵 $T={[x_0^c-x_0^o,y_0^c-y_0^o,z_0^c-z_0^o]}^T.$

所述的步骤4）中单点三角测距法为：安装时获得Z_L轴与平面X_wO_wZ_w的夹角λ与Y_L轴与平面X_wO_wZ_w的夹角

由结构光坐标系与相机坐标系之间的旋转关系可知

从而得到十字结构光在物体表面投影上任意一点在相机坐标系下的坐标：

其中， $U_A=\frac{(u_{A_0}-u_0)}{\mathrm{ax}},V_A\frac{(v_{A_0}-v_0)}{\mathrm{ay}}.$

本发明有益效果在于：

（1）本发明使用的十字结构光辅助的单目视觉位姿测量装置尺寸小，重量轻，不需要重量尺寸较大的激光测距仪辅助，满足室内微小型载体的载重要求；

（2）本发明仅利用十字结构光与物体表面的一条特征线段即可进行位姿测量，不需要事先知道合作特征点的信息，实现了在相机对缺乏平行约束条件的非合作目标位姿的测量；

（3）本发明使用的位姿测量方法利用结构光辅助单目视觉获得十字结构光与特征线段的两个交点在相机系下的坐标，通过沿特征线段建立物体坐标系获得两个交点在物体系下坐标的差，使得简单的线性变换即可得到相机系与物体系间三轴的旋转角度，大大简化了计算，提高了运算效率。

附图说明

图1为本发明整体流程图。

图2为结构光辅助单目视觉测量原理图。

图3为十字结构光在物体表面投影的示意图。

具体实施方式

下面结合附图对本发明作进一步说明。：

1、十字结构光辅助的单目视觉位姿测量系统模型建立

本发明利用十字结构光辅助单目相机进行位姿测量。首先将十字结构光传感器与相机同向放置（不需要平行），保证相机能捕捉到十字结构光在物体表面的亮斑即可。如图2所示，建立相机坐标系C-X_cY_cZ_c、图像坐标系o-uv、十字结构光坐标系L-X_lY_lZ_l，其中相机坐标系、图像坐标系与通常的定义一致；十字结构光坐标系的原点为十字结构光原点，Z_l轴与十字结构光两平面交线重合，方向与光线射出方向一致，X_l轴、Y_l轴分别沿两个垂直光线平面与Z_l轴构成右手系；如图3所示，建立物体坐标系O_o-X_oY_oZ_o，物体坐标系原点位于物体表面某一线段的端点，X_o轴与该线段重合，Z_o轴垂直于物体表面，Y_o轴与X_o轴、Z_o轴构成右手系。

2、改进的三角法测距模型

如图2所示，平面S为任意平面，L-X_LY_LZ_L为十字结构光坐标系，原点L即结构光发射点，C-X_cY_cZ_c为相机坐标系，C为相机光心。两者在同一水平面且间距为d，CC₁为相机光轴，与像平面的交点为C₀(u₀,v₀)，CC₀＝f为相机焦距。AL₁为结构光平面与平面S的交线，A为结构光上任意一点，A，L₁在像平面对应的点分别为

过L₁做L₁C₁垂直CC₁于C₁，做LL′平行于CC₁交L₁C₁于L′。

由于安装时不易保证激光坐标系与相机坐标系完全平行，需要考虑两坐标系间的旋转关系。将激光笔与相机安装在同一水平面，可以保证Z_L轴与水平面的夹角为0。Z_L轴与平面X_wO_wZ_w的夹角θ与Y_L轴与平面X_wO_wZ_w的夹角

对测量精度有较大影响，计算时需要考虑，这两个角度可以在安装时获得，并通过拟合获得精确值。

由相机的成像原理可知，相机坐标系到图像坐标系有如下转换关系：

$\begin{array}{c}{u}_{A_0}=\frac{f}{\mathrm{dx}}g\frac{x_A^c}{z_A^c}+u_0\\ v_{A_0}=\frac{f}{\mathrm{dy}}g\frac{y_A^c}{z_A^c}+v_0\end{array}---\left(1\right)$

所以点A在相机坐标系下的坐标为：

$\left[\begin{array}{c}{x}_A^c\\ y_A^c\\ z_A^c\end{array}\right]=\left[\begin{array}{c}\frac{(u_{A_0}-u_0)}{\mathrm{ax}}\\ \frac{(v_{A_0}-v_0)}{\mathrm{ay}}\\ 1\end{array}\right]z_A^c---\left(2\right)$

其中

dx,dy分别是相机成像平面每个像素在水平方向和垂直方向的物理长度，可以通过标定相机内参获得。

因此，只要求出

即可获得

下面对

进行求解：

由相似三角形可知：

$\frac{A{A}_1^{\′}}{A_0{A}_0^{\′}}=\frac{C{A}_1^{\′}}{{\mathrm{CA}}_0^{\′}}=\frac{{\mathrm{CA}}_1}{{\mathrm{CC}}_0}=\frac{z_A^c}{f}---\left(3\right)$

由结构光坐标系与相机坐标系之间的旋转关系可知：

其中， $A_1{A}_1^{\′}=(v_0-v_{A_0})\mathrm{dyg}\frac{d+z_A^c\tan \mathrm{\θ}}{(u_0-u_{A_0})\mathrm{dx}}.$

将(4)代入(3)，得到：

其中 $U_A=\frac{(u_{A_0}-u_0)}{\mathrm{ax}},V_A=\frac{(v_{A_0}-v_0)}{\mathrm{ay}}.$

将(5)代入(2)即可得到点A在相机系下的坐标，

其中， $U_A=\frac{(u_{A_0}-u_0)}{\mathrm{ax}},V_A=\frac{(v_{A_0}-v_0)}{\mathrm{ay}}$

此坐标

(i＝0,1,2)为求出物体系相对相机系的旋转矩阵R提供必要参数。

3、旋转矩阵与平移矩阵的求解

如图3所示，十字结构光在物体表面S的投影与物体表面轮廓的交点为P_i(i＝0,1,2,3)，其中P₁和P₂是十字结构光与物体表面的特征线段(X_o轴)的两个交点。P_i(i＝0,1,2,3)在相机系中的坐标为

(i＝0,1,2,3)，则可得交点在物体系下坐标

与交点在相机系下坐标

之间的旋转关系：

$\left[\begin{array}{c}{x}_i^c\\ y_i^c\\ z_i^c\end{array}\right]=R\×\left[\begin{array}{c}{x}_i^o\\ y_i^o\\ z_i^o\end{array}\right]---\left(6\right)$

其中，R表示物体系先绕X_o轴逆时针转动γ角，再绕Y_o轴逆时针转动θ角，最后绕Z_o轴逆时针转动ψ角。

由图3可知，P₁P₂＝b，P_i(i＝0,1,2)之间存在如下关系：

$\left[\begin{array}{c}{x}_1^o\\ y_1^o\\ z_1^o\end{array}\right]=\left[\begin{array}{c}{x}_0^o-c\\ y_0^o-h\\ z_0^o\end{array}\right],\left[\begin{array}{c}{x}_2^o\\ y_2^o\\ z_2^o\end{array}\right]=\left[\begin{array}{c}{x}_0^o-b-c\\ y_0^o-h\\ z_0^o\end{array}\right]---\left(7\right)$

将(7)代入(6)得：

$\left[\begin{array}{c}{x}_0^c\\ y_0^c\\ z_0^c\end{array}\right]=\left(\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)\×\left[\begin{array}{c}{x}_0^o\\ y_0^o\\ z_0^o\end{array}\right]$

$\left[\begin{array}{c}{x}_1^c\\ y_1^c\\ z_1^c\end{array}\right]=\left(\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)\×\left[\begin{array}{c}{x}_0^o-c\\ y_0^o-h\\ z_0^o\end{array}\right]$

$\left[\begin{array}{c}{x}_2^c\\ y_2^c\\ z_2^c\end{array}\right]=\left(\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)\×\left[\begin{array}{c}{x}_0^o-c-b\\ y_0^o-h\\ z_0^o\end{array}\right]$

第二个方程组与第一个方程组相减得：

$\begin{array}{c}{x}_1^c-x_0^c=(-c)\cos \mathrm{\ψ}\cos \mathrm{\θ}+(-\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ})(-h)\\ y_1^c-y_0^c=(-c)\sin \mathrm{\ψ}\cos \mathrm{\θ}+(-\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ})-(-h)\\ z_1^c-z_0^c=(-c)\sin \mathrm{\θ}+\left(\cos \mathrm{\θ}\sin \mathrm{\γ}\right)(-h)\end{array}---\left(8\right)$

第三个方程组与第二个方程组相减得：

$\begin{array}{c}{x}_1^c-x_2^c=b\left(\cos \mathrm{\ψ}\cos \mathrm{\θ}\right)\\ y_1^c-y_2^c=b\left(\sin \mathrm{\ψ}\cos \mathrm{\θ}\right)\\ z_1^c-z_2^c=b\left(\sin \mathrm{\θ}\right)\end{array}---\left(9\right)$

由(9)可以得到：

$\sin \mathrm{\θ}=\frac{z_1^c-z_2^c}{b}---\left(10\right)$

$\tan \mathrm{\ψ}\frac{y_1^c-y_2^c}{x_1^c-x_2^c}---\left(11\right)$

由(8)中第一式两边同乘sinψ，第二式两边同乘cosψ再相减可以得到：

$\cos \mathrm{\ψ}(y_1^c-y_0^c)-\sin \mathrm{\ψ}(x_1^c-x_0^c)=(-h)\cos \mathrm{\γ}---\left(12\right)$

由(8)中第一式两边同乘cosψ，第二式两边同乘sinψ再相加可以得到：

$\cos \mathrm{\ψ}(y_1^c-y_0^c)+\sin \mathrm{\ψ}(x_1^c-x_0^c)=(-c)\cos \mathrm{\θ}-(-h)\sin \mathrm{\θ}\sin \mathrm{\γ}---\left(13\right)$

(12)的两边同乘以sinθ，(9)中第三式两边同乘以cosθ，两式相减得到：

$(z_1^c-z_0^c)\cos \mathrm{\θ}-(y_1^c-y_0^c)\sin \mathrm{\θ}\cos \mathrm{\ψ}-(x_1^c-x_0^c)\sin \mathrm{\θ}\sin \mathrm{\ψ}=(-d)\sin \mathrm{\γ}---\left(14\right)$

(14)除以(12)得：

$\tan \mathrm{\γ}=\frac{(z_1^c-z_0^c)\cos \mathrm{\θ}-(y_1^c-y_0^c)\sin \mathrm{\θ}\cos \mathrm{\ψ}-(x_1^c-x_0^c)\sin \mathrm{\θ}\sin \mathrm{\ψ}}{\cos \mathrm{\ψ}(y_1^c-y_0^c)-\sin \mathrm{\ψ}(x_1^c-x_0^c)}---\left(15\right)$

根据(10)(11)以及(15)即可得到旋转矩阵R中的角度，则旋转矩阵R为：

$R=\left(\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right)---\left(16\right)$

其中， $\mathrm{\γ}=\arctan \frac{(z_1^c-z_0^c)\cos \mathrm{\θ}-(y_1^c-y_0^c)\sin \mathrm{\θ}\cos \mathrm{\ψ}-(x_1^c-x_0^c)\sin \mathrm{\θ}\sin \mathrm{\ψ}}{\cos \mathrm{\ψ}(y_1^c-y_0^c)-\sin \mathrm{\ψ}(x_1^c-x_0^c)},\mathrm{\θ}=\arcsin \frac{z_1^c-z_0^c}{b},$ $\mathrm{\ψ}=\arctan \frac{y_1^c-y_2^c}{x_1^c-x_2^c}.$

图3中，通过式(3)得到P₀在物体下的坐标

则向量

向量 $O_c{P}_0={[x_0^c,y_0^c,z_0^c]}^T,$ 向量 $O_c{O}_o=O_c{P}_0-O_o{P}_0={[x_0^c-x_0^o,y_0^c-y_0^o,z_0^c-z_0^o]}^T,$ 物体系相对相机系的平移矩阵 $T=O_c{O}_o={[x_0^c-x_0^o,y_0^c-y_0^o,z_0^c-z_0^o]}^T,$

本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。

Claims (2)

1.一种利用十字结构光辅助的单目视觉位姿测量方法，其特征在于包括以下步骤：

1）建立相机坐标系C-X_cY_cZ_c、图像坐标系o-uv、十字结构光坐标系L-X_lY_lZ_l以及物体坐标系O_o-X_oY_oZ_o，其中相机坐标系、图像坐标系与通常的定义一致；十字结构光坐标系的原点为十字结构光发出点，Z_l轴与十字结构光两平面的交线重合，方向与光线射出方向一致，X_l轴、Y_l轴分别沿两个垂直光线平面与Z_l轴构成右手系；物体坐标系原点位于物体表面某一线段的端点，X_o轴与该线段重合指向另一端点，Z_o轴垂直物体表面向外，Y_o轴与X_o轴、Z_o轴构成右手系；

2）定义物体坐标系相对相机坐标系的旋转关系R与平移关系T，其中R表示物体坐标系先绕X_o轴逆时针转动γ角，再绕Y_o轴逆时针转动θ角，最后绕Z_o轴逆时针转动ψ角，即 $R=\left(\begin{array}{ccc}\cos \mathrm{\ψ}\cos \mathrm{\θ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}-\sin \mathrm{\ψ}\cos \mathrm{\γ}& -\cos \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}+\sin \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\ψ}\cos \mathrm{\θ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\sin \mathrm{\γ}+\cos \mathrm{\ψ}\cos \mathrm{\γ}& -\sin \mathrm{\ψ}\sin \mathrm{\θ}\cos \mathrm{\γ}-\cos \mathrm{\ψ}\sin \mathrm{\γ}\\ \sin \mathrm{\θ}& \cos \mathrm{\θ}\sin \mathrm{\γ}& \cos \mathrm{\θ}\cos \mathrm{\γ}\end{array}\right);$

3）将相机与十字结构光传感器同向放置并进行标定，包括相机内参、相机坐标系与十字结构光坐标系间的位移和旋转关系；使十字结构光在物体表面的亮斑与物体表面在X_o轴上的线段有两个交点，此交点即为特征点，相机捕捉这两点以及十字结构光与物体表面轮廓另外任意一个交点；

4）利用图像处理算法获得步骤3）中所需的三个交点，利用单点三角测距法计算这三个点在相机坐标系下的坐标

(i＝0,1,2)，得到特征点在物体坐标系下X_o轴方向的差，表示为b；

5）利用步骤2）中定义的旋转关系R描述步骤4）中获得的三个点，得到三个方程组，利用这三个点在物体坐标系下的关系，通过线性变换对方程组进行化简消元，得到物体坐标系三轴相对相机坐标系的旋转角度

$\mathrm{\γ}=\arctan =\frac{(z_1^c-z_0^c)\cos \mathrm{\θ}-(y_1^c-y_0^c)\sin \mathrm{\θ}\cos \mathrm{\ψ}-(x_1^c-x_0^c)\sin \mathrm{\θ}\sin \mathrm{\ψ}}{\cos \mathrm{\ψ}(y_1^c-y_0^c)-\sin \mathrm{\ψ}(x_1^c-x_0^c)};$

6）利用步骤2）中定义的旋转关系R以及步骤3）得到的三个交点中任意一点在相机坐标系下的坐标，可以得到该点在物体坐标系下的坐标，进而获得物体坐标系原点在相机坐标系下的坐标，即可得到物体坐标系与相机坐标系之间的平移矩阵 $T={[x_0^c-x_0^o,y_0^c-y_0^o,z_0^c-z_0^o]}^T.$

2.根据权利要求1所述的利用十字结构光辅助的单目视觉位姿测量方法，其特征在于：所述的单点三角测距法为安装时获得Z_L轴与平面X_wO_wZ_w的夹角λ与Y_L轴与平面X_wO_wZ_w的夹角

由结构光坐标系与相机坐标系之间的旋转关系可知

从而得到十字结构光在物体表面投影上任意一点在相机坐标系下的坐标：

其中， $U_A=\frac{(u_{A_0}-u_0)}{\mathrm{ax}},V_A\frac{(v_{A_0}-v_0)}{\mathrm{ay}}.$

Images