[发明专利]基于视觉系统结构参数的前方交会测量方法及系统

权利要求书

1.一种基于视觉系统结构参数的前方交会测量方法，其特征在于，包括：

根据靶标相对于像平面的姿态和视觉系统中载体的预设位，在世界坐标系中分别表达所述载体位于实际位时的主靶点矢量；其中，所述载体位于实际位时的主靶点矢量为所述世界坐标系的原点到所述载体位于实际位时的主靶点的矢量；所述主靶点，指所述视觉系统中的视觉传感器的主点通过主光轴在靶标上的投影点；

基于所述视觉系统结构参数中的焦距、交会点和外方位元素、所述视觉系统中陀螺装置的陀螺值以及在所述世界坐标系中表达的所述载体位于实际位时的主靶点矢量，求解交切距和主光轴旋转半径，并确定所述焦距与交切距之间的拟合函数；

利用所述视觉传感器，获取多张包括待测点的目标图像之后，对所述待测点在各所述目标图像中对应的待测像点进行立体场误差校正，并在所述世界坐标系中表达经过立体场误差校正之后的各所述待测像点矢量；其中，任意两张所述目标图像中，所述待测点相对于所述视觉系统的姿态和交会点矢量不同；所述视觉传感器，设置于所述载体上；所述交会点矢量，指所述载体的旋转中心到所述交会点的矢量；

基于通过所述拟合函数获得的交切距、所述视觉系统结构参数中的主光轴旋转半径、所述陀螺装置的陀螺值以及在所述世界坐标系中表达的经过立体场误差校正之后的各所述待测像点，对所述待测点进行前方交会测量，获得所述待测点在所述世界坐标系中的坐标值；

其中，所述主光轴旋转半径，是基于主光轴进行切线共球交会变换确定的；所述待测点为预先确定的测量空间中的任意点；所述世界坐标系的原点位于所述载体的旋转中心，所述世界坐标系的各坐标轴的方向是根据所述陀螺装置位于初始位时各坐标轴的方向确定的；

所述根据靶标相对于像平面的姿态和所述视觉系统中载体的预设位，分别在世界坐标系中表达所述载体位于实际位时的主靶点矢量，具体包括：

标定所述视觉系统的焦距和主点坐标；

获取任意姿态的各所述靶标与像平面间的外方位元素其中，m为所述靶标的标识，m＝1,2,3....；n为所述靶标的姿态的标识，n＝1,2,3....；每一所述靶标的位置是预先确定的，每一所述靶标的姿态是在所述载体位于实际位时通过后方交会或根据陀螺模块获取的；

求取n姿态标识为m的靶标的坐标系到所述世界坐标系间的旋转变换矩阵

其中，表示n姿态的靶标的坐标系到所述视觉传感器的坐标系的旋转变换矩阵；表示所述视觉传感器的坐标系到所述载体实际位的坐标系的旋转变换矩阵；表示n姿态靶标对应的所述载体实际位的坐标系到所述陀螺装置位于初始位时的坐标系的旋转变换矩阵；表示所述陀螺装置位于初始位时的坐标系到所述世界坐标系的旋转变换矩阵；其中，所述载体为云台或机械臂，所述载体搭载所述陀螺装置和所述视觉传感器；所述视觉传感器包括镜头；所述载体位于预设位时的坐标系与世界坐标一致；所述载体的实际位是根据所述陀螺装置的陀螺值确定的；所述陀螺装置位于初始位时的坐标系是根据所述陀螺装置内置导航模块确定的；

根据所述载体的预设位，获取标识为n姿态的靶标对应的所述载体的预设位到所述载体位于实际位时的旋转变换矩阵

其中，表示n姿态标识为m的靶标对应的载体的预设位到所述载体位于实际位时的旋转变换矩阵；表示所述陀螺装置的初始位到所述载体的预设位的旋转变换矩阵；表示标识为n姿态的靶标对应的所述陀螺装置的初始位到所述载体位于实际位时的主靶点的旋转变化矩阵；

在所述载体位于预设位时，在所述世界坐标系中将所述世界坐标系的原点O_W到所述主光轴与主光轴旋转球的切点P_p的矢量表示为将所述世界坐标系的原点O_W到所述切点P_p对应的交会点F_p的矢量表示为其中，ρ₀为所述主光轴旋转半径；d_z为交切距；所述交切距为所述切点对应的交会点与所述切点之间的距离；所述主光轴旋转球是基于所述切线共球交会确定的；

基于所述和所述在所述世界坐标轴中，将所述世界坐标系的原点O_W到所述载体位于预设位时的主靶点A_p的矢量表示为

对于标识为m的靶标，在所述世界坐标系中，从预设位的到所述世界坐标系的原点O_W到所述载体位于实际位时的主靶点A_m的矢量的旋转矩阵为：

将主靶点矢量从初始位旋转至

作为靶标坐标系与世界坐标系间的平移矢量表达为：

其中：和表示中的各项；A_m为靶标m的坐标系原点O_tm；所述标识为m的靶标为某一姿态时主靶点A_m对应的交会点F_m与所述主靶点A_m位于同一所述主光轴上；其中，A_mF_m表示交会点F_m与对应的主靶点A_m之间的距离；表示世界坐标系与靶标坐标系间的平移量；

所述基于所述视觉系统结构参数中焦距、交会点和外方位元素、所述视觉系统中陀螺装置的陀螺值以及在所述世界坐标系中表达的所述载体位于实际位时的主靶点矢量，求解交切距和主光轴旋转半径，并确定所述焦距与交切距之间的拟合函数，具体包括：

对于每一靶标，所述视觉传感器在任一目标焦距下，获取多张包括任意姿态的所述靶标的样本图像；其中，所述靶标的任意两张所述样本图像中，所述靶标相对于所述视觉传感器之间的距离和姿态均不同；

基于在各所述目标焦距获取的各所述靶标各所述姿态的样本图像，分别获取各所述目标焦距对应的各所述交切距d_z的解，具体包括：

对于标识为m的靶标，基于任一目标焦距下获取的n姿态的所述样本图像；获取所述载体位于实际位时的n姿态的靶标上的主靶点A_n在所述标识为m的靶标的坐标系中的坐标

基于所述主靶点A_n在所述标识为m的靶标的坐标系中坐标在所述世界坐标系中表达所述主靶点A_n的坐标

基于所述目标焦距下获取的包括所述n姿态标识为m的靶标的各所述样本图像，获取所述载体位于实际位时所述主光轴与所述主光轴旋转球的切点对应的交会点F_n在所述靶标的坐标系中的坐标

基于交会点F_n在所述的靶标的坐标系中的坐标在所述世界坐标系中表达所述交会点F_n的坐标

基于所述目标焦距下获取的n姿态的靶标的所述样本图像，在所述世界坐标系表达所述主靶点A_n和所述交会点F_n对应的所述主光轴与所述主光轴旋转球的切点

其中，所述主靶点A_n、所述交会点F_n和所述切点P_n均位于同一所述主光轴上；

对于n姿态标识为m的靶标，基于所述主靶点A_n在所述世界坐标系中的坐标所述交会点F_n在所述世界坐标系中的坐标和所述切点P_n的坐标获得切线三点共线方程：

其中，λ表示常数；

将基于所述主光轴的旋转半径ρ₀和所述交切距d_z表达的所述主靶点A_n的坐标、所述交会点F_n的坐标和所述切点P_n的坐标代入所述切线三点共线方程，解得：

其中，

对解得的进行泰勒展开，保留一次项，得到：

其中，

其中，

其中

其中，和表示矩阵的各项；

将误差方程表示为：

其中，常数项矢量为系数列矩阵为

解得第n次迭代改正数为X_n＝(Λ_n^TΛ_n)^-1Λ_n^TL_n；

根据所述误差方程的改正数项拆解X_n＝[d(d_z)，dρ₀]^T，迭代运算得到所述目标焦距对应的交切距d_z和主光轴旋转半径ρ₀的解；

获取各所述目标焦距对应的各所述交切距d_z的解之后，基于每一所述目标焦距与所述目标焦距对应的所述交切距d_z的解，获得所述焦距f与所述交切距d_z的拟合函数；

所述利用所述视觉系统中的视觉传感器，获取多张包括待测点的目标图像之后，对所述待测点在各所述目标图像中对应的待测像点进行立体场误差校正，并在所述世界坐标系中表达经过立体场误差校正之后的各所述待测像点，具体包括：

利用视觉系统中的视觉传感器，获取多张包括待测点的目标图像；

基于各所述目标图像，确定所述待测点在每一所述目标图像中对应的像点在所述视觉传感器坐标系中的像坐标

利用预先获取的像方垂轴校正数据W＝[W_x，W_y]^T校正所述待测像点的像坐标获得所述视觉传感器坐标系中每一所述待测像点像方垂轴校正后的第一坐标利用轴向校正数据a校正每一所述待测像点坐标获得所述视觉传感器坐标系中每一所述待测像点校正后的第二坐标

将所述世界坐标系的原点到像平面坐标系原点的预设位矢量在所述世界坐标系中表达为[O，ρ₀，-f-d_z]^T之后，将所述视觉传感器坐标系中的所述第一坐标和所述第二坐标转换至所述世界坐标系中，得到所述待测点对应的各所述待测像点在所述世界坐标系中的第三坐标和第四坐标

所述基于所述拟合函数、所述视觉系统结构参数以及在所述世界坐标系中表达的经过立体场误差校正之后的各所述待测像点，对所述待测点进行前方交会测量，获得所述待测点在所述世界坐标系中的坐标值，具体包括：

对所述视觉传感器进行焦距标定，确定所述视觉传感器的所述焦距f；

基于预先确定的所述焦距f与所述交切距d_z的拟合函数，确定所述焦距f对应的所述交切距d_z；

基于所述载体位于预设位时，所述世界坐标系的原点O_W到所述交会点F_p的矢量将所述载体位于实际位时，所述世界坐标系的原点O_W到所述待测点对应的交会点的矢量表达为：其中，为所述载体的预设位到所述载体位于实际位时的主靶点的旋转变换矩阵；

基于所述待测点B_n在所述世界坐标系中的坐标待测点B_n对应的各待测像点在所述世界坐标系中的第三坐标和所述待测点B_n对应的交会点F_n，获得三点共线方程：

解得：

进行泰勒展开，得到：

其中，

将误差方程表达为：

其中，常数项

系数列矩阵

解得第n次迭代改正数为：χ_n＝(Λ_n^TΛ_n)^-1Λ_n^TL_n；

根据第n次迭代改正数χ_n，迭代运行得到所述第三坐标对应的待测点B_n的第一解

基于所述待测点B_n、待测点B_n对应的各待测像点在所述世界坐标系中的所述第四坐标和所述待测点B_n对应的交会点F_n，基于上述方法，得到所述第四坐标对应的待测点B_n的第二解

根据所述第一解和所述第二解确定所述待测点B_n在所述世界坐标系中的坐标值为