基于先验知识模型优化的运动物位姿测量方法

摘要

本发明基于先验知识模型优化的运动物位姿测量方法属于计算机视觉测量技术领域，涉及一种使用双目视觉原理结合先验知识模型位姿优化进行运动物位姿测量。在测量方法中，先建立被测物体的离线先验模型，利用双目摄像机实时采集被测物运动图像，经过图像特征提取、匹配与重建的图像处理步骤得到标记点的三维坐标；利用标记点之间的先验距离关系，对位姿测量时的标记点位置信息进行优化，提高标记点的空间位置精度，最终达到高精度的位姿测量。本发明利用先验模型约束对运动物体位姿测量结果进行优化，提高复杂环境下位姿测量系统的测量精度与稳定性；采用组合式标记点作为图像特征，测量精度高，适用范围广。

说明书

技术领域

本发明属于计算机视觉测量技术领域，涉及一种适用于回转体运 动目标的空间位置和姿态高精度求取方法。

背景技术

航天、航空、航海等作为国家工业的前沿领域，其核心部件如运 载火箭、超高速飞行器投放物等高速运动的位姿测量是其领域研究的 一个重要问题，而其位姿测量精度又直接影响航天航空核心部件设计 与制造的安全性。计算机视觉测量方法具有非接触、灵活、可实现实 时测量等优点，是运动物体位姿测量的重要手段。但由于物体运动速 度快、被测物表面曲率变化大等原因，图像上特征点的中心提取精度 很难保证，这就对运动物体位姿信息的高精度测量提出了更高的要求。

中科院沈阳自动化研究所的周船等人2004年在仪器仪表学报发 表的《基于模型的位姿估计中优化方法研究》中提出了基于模型位姿 估计的两种优化方法，在摄像机标定过程中采用了基于三维重建误差 的优化，提高标定精度的同时，也提高了位姿估计的精度；并且在双 相机的位姿估计中采用实时遗传算法，加快了算法的收敛速度。四川 大学肖永亮等2011年在光电子■激光发表的《基于凸松弛全局优化算 法的视觉测量位姿估计》中提出基于凸松弛全局优化算法的视觉测量 全局最优位姿估计，利用归一化图像点和摄像机光心组成的正交投影 矩阵，构造以旋转四元数为参数的物空间误差目标非凸多项式函数并 进行LMI优化，得到旋转矩阵和平移矢量的全局最优解。

发明内容

本发明要解决的技术难题是克服现有技术的缺陷，发明一种基于 先验知识模型优化的运动物位姿测量方法，该方法利用离线状态下物 体表面标记点的空间点云作为其位姿优化的先验模型，克服了物体高 速运动状态下图像特征边缘模糊导致的测量精度不高的问题；采用高 精度标定的双目视觉系统配合高精度旋转平台建立被测物体的离线 先验模型，克服了采用激光跟踪仪测量时探头过大，难以保证先验模 型建立精度的问题。

本发明所采用的技术方案是一种基于先验知识模型优化的运动 物位姿测量方法，其特征是，测量方法中，先建立被测物体的离线先 验模型，利用双目摄像机实时采集被测物运动图像，经过图像特征提 取、匹配与重建的图像处理步骤得到标记点的三维坐标；利用标记点 之间的先验距离关系，对位姿测量时的标记点位置信息进行优化，提 高标记点的空间位置精度，最终达到高精度的位姿测量；测量方法的 具体步骤如下：

1、高速摄像机标定

本发明采用张氏标定方法配合高精度平面标定板进行双目视觉 系统的标定；以张正友提出的基于平面靶标标定方法为基础，并考虑 到制造时镜头畸变及其它因素的影响，引入带有畸变修正的双目视觉 系统模型，求解出摄像机的内、外参数和镜头畸变系数；

2、高速运动目标图像处理

图像处理采用组合式标记点作为被测物表面的特征点，采用两步 法进行标记点快速提取；首先利用重心提取算法快速搜索标记点图像 坐标，再在该坐标范围内进行高精度的Harris角点提取算法，实现物 体运动过程中标记点的高精度提取；利用已知的标记点布局形式，采 用基于空间编码方法的标记点快速匹配、识别方法，实现空间点的高 精度匹配与识别，最后通过视觉理论完成空间匹配点的三维坐标重建； 图像特征标记所在区域的梯度重心点坐标用下式表示：

$C=\underset{i=-h}{\overset{h}{\Sigma }}\underset{j=-w}{\overset{w}{\Sigma }}\left[\right|G(i,j)|·P(i,j)]/\underset{i=-h}{\overset{h}{\Sigma }}\underset{j=-w}{\overset{w}{\Sigma }}\left|G(i,j)\right|---\left(1\right)$

其中，C为应用梯度重心法所提取的标记中心像素的坐标值，(i,j)点 的梯度幅值用|G(i,j)|表示，w，h分别为二维图像的宽和高，(i,j)点 的图像坐标表示为P(i,j)；

3、建立标记点的离线先验模型

用高精度旋转平台对被测物进行旋转，每旋转一定角度采集一张 图像，对其视场内的标记点进行提取、匹配与重建，然后对所有标记 点空间位置数据进行坐标系转换，转换到同一坐标系，即为被测物体 的离线先验模型；

4、基于先验模型的位姿优化

1)目标物轴线以及特征点坐标优化

本发明采用类圆柱被测物表面点距离轴线距离相等的目标约束 以及标记点间的先验关系约束对目标物轴线以及特征点坐标进行优 化，提高标记点空间精度，优化的目标函数如下所示：

$\left(\begin{array}{c}{F}_1\left(X_i\right)=\underset{i=1}{\overset{3n}{\Sigma }}{({(D-A{J}_i)}^T{\left(A{A}^T\right)}^{-1}(D-A{J}_i)-r^2)}^2\\ F_2\left(X_i\right)=\underset{i=1}{\overset{3n-1}{\Sigma }}\underset{j=1}{\overset{3n-i}{\Sigma }}{({(J_i-J_{i+j})}^T(J_i-J_{i+j})-L_{i,i+j}^2)}^2\end{array}\right)---\left(2\right)$

其中，F₁(X_i)为被测目标形状约束即所有标记点到轴线距离相等且等 于被测目标半径，F₂(X_i)为标记点先验模型约束即所有标记点之间的 网络关系，J_i＝(x_i y_i z_i)^T为第i个的标记点的空间坐标，J_i+j为第i+j个 的标记点的空间坐标，$A=\left(\begin{array}{ccc}{A}_1& B_1& C_1\\ A_2& B_2& C_2\end{array}\right)$为被测物轴线所在两相交平面 的法向量，$D=\left(\begin{array}{c}{D}_1\\ D_2\end{array}\right)$为平面系数，r为类圆柱的半径长度，L_i,i+j为第i 个和i+j个标记点之间的距离。当以上两个约束的目标函数值无限趋 近于最小值零时，标记点精度最高，其整体优化目标函数为：

S(X_i)＝min(F₁(X_i)+F₂(X_i))      (3)

2)基于距离约束的质心位置优化

从视场中提取亮度最高圆度最好的三点，根据标志点布局识别三 点相对模型布局位置，进而得到该三点距质心的理论距离；再根据距 离约束求解模型质心位置，作为质心坐标初值。采用视野中全部提取 点与质心位置距离进行优化；对于每个点设定其权重值，进行多点优 化；由于质心为轴线上一点，根据轴线位置对质心位置进行约束优化。 质心位置的优化公式如下：

$\left(\begin{array}{c}\min \left(\underset{i=1}{\overset{n}{\Sigma }}{\rho }_i{(D{c}_i-d{c}_i)}^2\right)\\ C(x,y,z)\in L(x,y,z)\end{array}\right)---\left(4\right)$

ρ_i为各可视点的加权值，Dc_i为质心到优化后可视标记点的测量距离， dc_i为质心到优化后可视标记点的实际加工距离，C(x,y,z)为质心坐标， L(x,y,z)为轴线方程；

5、目标物位姿求解

本发明采用坐标变换的方法对姿态进行求解，分别建立了世界坐 标系与物体局部坐标系，通过计算两个坐标系间的坐标转换关系求解 物体的位置和姿态信息。

$\left(\begin{array}{c}{x}^{\prime }\\ y^{\prime }\\ z^{\prime }\end{array}\right)=\left(\begin{array}{cc}R& T\\ 0^T& 1\end{array}\right)\left(\begin{array}{c}x\\ y\\ z\end{array}\right)---\left(5\right)$

其中，T和R分别为两个坐标系之间的平移矩阵和旋转矩阵，即代表 了物体的平移和旋转参数，(x y z)为标记点在世界坐标系的坐标， (x′ y′ z′)为其在局部坐标系中的对应坐标；

本发明的有益效果是利用先验模型约束对运动物体位姿测量结 果进行优化，提高复杂环境下位姿测量系统的测量精度与稳定性；采 用组合式标记点作为图像特征，克服了复杂曲面表面特征点提取精度 低的问题；采用高精度标定的双目视觉系统建立先验模型对物体的位 姿信息进行优化，解决了大视场标定精度低所导致的位姿测量系统精 度低的问题。

附图说明

图1为基于先验知识模型优化的运动物位姿测量系统图。其中， 1-左相机，1＇-右相机，2-左光源，2＇-右光源，3-被测物，4-被测物 表面标记点，5-高精度旋转平台。

图2为基于先验知识模型优化的物体位姿测量流程图。

具体实施方式

以下结合技术方案和附图详细叙述本发明的具体实施方式。

附图1为基于先验知识模型优化的运动物位姿测量系统图。本方 法通过在被测物3表面安装被测物表面标记点4，摄像机标定过程采 用两台高速摄像机1和1＇配合高精度旋转平台5，每隔一定角度采 集物体表面标记点图像，建立离线状态下所有标记点的先验知识模型； 进行位姿测量时用两台高速摄像机实时采集物体运动图像，经过标记 点提取、匹配和重建后得到其三维坐标，再利用物体的先验模型约束 对标记点的坐标和物体的回转轴线进行优化；建立局部坐标系和世界 坐标系，通过两个坐标系之间的转换得到物体的高精度位置和姿态信 息，基于先验知识模型优化的物体位姿测量流程图如附图2所示。

本实施例采用的超速高黑白工业摄像机型号为FASTCAM SA-X， 摄像机分辨率：1024×1024，帧频：在50fps-50000fps范围内可调， 在保证分辨率1024×1024情况下最高可达10000fps，CCD面积： 2cm×2cm。广角镜头型号为AF-S 17-35mm f/2.8D IF-ED，镜头焦距： f＝17-35，APS焦距：25.5-52.5，最大光圈：F2.8，最小光圈：F22， 镜头重量：745g，镜头尺寸：82.5×106。高精度旋转平台型号为 Zolix-RSA100，精度为0.001°。本具体实施方案的拍摄条件如下：高 速摄像机帧频为3000fps，图片像素为1024×1024，镜头焦距为17mm， 物距为750mm，视场约为800mm×800mm。

本实施例测量方法的具体步骤如下：

1.高速摄像机标定

将精密加工的二维棋盘格标定板放置在物体运动的平面附近，用 双目摄像机采集标定板图像，改变标定板的角度拍摄得到12组图像， 对摄像机的内外参数进行标定，得到摄像机内参数M₁，外参数R和T， 以及摄像机的畸变参数K。高速摄像机的标定结果如表1所示：

表1像机内外参数以及畸变参数

2.高速运动目标图像处理

采用梯度重心法提取被测物3运动时被测物表面标记点4，利用 公式(1)求出图像在各点的梯度中心点，然后提取图像上的标记特 征，再经过左右相机对应标记点的识别与匹配，利用三角原理重建出 视场中标记点在空间的三维坐标。

3.建立标记点的离线先验模型

采用专用夹具将被测物固定在高精度旋转平台上，旋转被测被测 物每隔30度采集一张图像，对视场内的9个标记点进行提取、匹配 与重建，然后对其所有标记点空间位置数据进行坐标系转换，转换到 同一坐标系，得到被测物的离线先验模型。

4.基于先验模型的位姿优化

1)目标物轴线以及特征点坐标优化

本发明采用被测物表面点距离轴线距离相等的目标约束以及标 记点间的先验关系约束对目标物轴线以及特征点坐标进行优化，提高 标记点空间精度，优化的目标函数如公式(2)所示。当以上两个约 束的目标函数值无限趋近于最小值零时，标记点精度最高，其整体优 化目标函数如公式(3)所示。

2)基于距离约束的质心位置优化

从视场中提取亮度最高圆度最好的三点，根据距离约束求解被测 物质心坐标位置初值。采用视野中全部提取点与质心位置距离进行优 化。对于每个点设定其权重值，进行多点优化。根据轴线位置对质心 位置的约束关系采用公式(4)对质心位置坐标进行优化。

5.目标物位姿求解

采用坐标变换的方法对姿态进行求解，分别建立了世界坐标系与 物体局部坐标系，利用公式(5)所示的坐标变换关系求解物体的位 置和姿态信息。物体平移矩阵的具体求解公式为：

T＝[x_p y_p z_p]      (6)

物体整体旋转矩阵为三个坐标轴旋转矩阵的乘积：

$R=R_{{\theta }_Y}{R}_{{\theta }_Z}{R}_{{\theta }_Z}---\left(7\right)$

其中，三个坐标轴旋转矩阵分别为：

$R_{{\theta }_Z}=\left(\begin{array}{ccc}\cos \left({\theta }_Z\right)& \sin \left({\theta }_Z\right)& 0\\ -\sin \left({\theta }_Z\right)& \cos \left({\theta }_Z\right)& 0\\ 0& 0& 1\end{array}\right)---\left(8\right)$

$R_{{\theta }_X}=\left(\begin{array}{ccc}1& 0& 0\\ 0& \cos \left({\theta }_X\right)& \sin \left({\theta }_X\right)\\ 0& -\sin \left({\theta }_X\right)& \cos \left({\theta }_X\right)\end{array}\right)---\left(9\right)$

$R_{{\theta }_Y}=\left(\begin{array}{ccc}\cos \left({\theta }_Y\right)& 0& \text{-sin}\left({\theta }_Y\right)\\ 0& 1& 0\\ -\sin \left({\theta }_Y\right)& 0& \cos \left({\theta }_Y\right)\end{array}\right)---\left(10\right)$

其中，-θ_Z，-θ_X，-θ_Y分别表示物体的偏航角，俯仰角和滚转角； (x_p y_p z_p)为物体在x，y，z三个方向的平移量。采用基于先验知识 优化的双目视觉位姿测量方法得到的物体空间位姿参数如表2所示。

表2被测物的位姿参数测量结果

时间/s 偏航角/° 俯仰角/° 滚转角/° X位移/mm Y位移/mm Z位移/mm 0.00023 3.630 59.423 3.389 101.42 64.23 291.08 0.00048 3.740 60.012 3.172 101.16 64.29 290.21 0.00073 3.715 59.243 3.321 101.10 64.18 290.13 0.00098 3.790 59.342 3.193 101.52 64.12 290.05 0.00123 3.802 59.678 3.152 101.78 64.10 290.01 0.00148 3.776 60.102 3.203 101.32 64.07 289.94 0.00173 3.795 60.110 3.228 101.34 63.95 289.89 0.00198 3.867 60.121 3.072 101.22 63.84 289.81 0.00223 3.796 59.832 3.201 101.08 63.80 288.76 0.00248 3.742 59.721 3.202 100.63 63.76 289.74 0.00273 3.765 59.793 3.226 100.61 63.63 289.64 0.00298 3.811 59.984 3.209 100.49 63.54 289.59 0.00323 3.802 60.775 3.122 100.04 63.43 289.53 0.00348 3.762 60.663 3.236 100.03 63.41 289.43 0.00373 3.813 59.652 3.150 99.67 63.35 289.26

本发明利用双目视觉系统建立被测物的离线先验知识模型并对 其位姿参数进行测量，具有非接触、灵活、速度快等优点；采用组合 式表面标记点提高图像特征提取精度，并且解决了复杂曲面表面标记 点中心提取误差大的问题；利用模型先验知识作为约束条件对位姿参 数优化方法，提高位姿视觉测量系统的整体精度，适用于回转类物体 的空间位置和姿态信息的高精度测量。