[发明专利]基于迭代更新的单目相机位姿测量装置与方法

权利要求书

1.基于迭代更新的单目相机位姿测量方法，其特征在于，所述基于迭代更新的单目相机位姿测量方法的具体步骤为：

步骤一：根据测量图像提取得到的二维测量数据反推三维世界坐标；

步骤二：通过位姿迭代更新的方式实现对待测目标的位姿测量；

通过位姿迭代更新的方式实现对待测目标的位姿测量的具体步骤为：

初始时刻t₀对应的物体坐标系S₀的坐标原点O在世界坐标系下的坐标如式(20)所示

为了求解需要使待测目标绕其转动轴旋转3次，并记录下标志点A在每次旋转后的世界坐标，分别记作

由标志点A与待测目标固连可知，标志点A与坐标原点O之间的距离d保持不变，即

根据式(24)可求解出和d，即为待测目标的初始位置；

接下来求解待测目标的初始姿态，根据初始时刻t₀采集得到的测量图片I₀可以提取出标志点A在世界坐标系下的坐标进一步可求得物体坐标系Ox轴方向上的单位向量如式(25)所示；

世界坐标系Ox轴方向上的单位向量记作如式(26)所示；

与之间的夹角记作θ₀，则

θ₀即为待测目标的初始姿态；

最后，根据求得的初始位置和初始姿态θ₀计算标志点A和B在物体坐标系S下的坐标P_A和P_B；

将从t₀时刻的物体坐标系S₀到世界坐标系W的坐标变换矩阵记作将其写为旋转矩阵和平移向量组合的形式，如式(28)所示；

旋转矩阵可由初始姿态θ₀计算得到，如式(29)所示；

平移向量可由初始位置计算得到，如式(30)所示；

将式(29)、式(30)代入式(28)得

由此可求得从世界坐标系W到t₀时刻的物体坐标系S₀的坐标变换矩阵如式(32)所示；

进一步可求得P_A和P_B，如式(33)和式(34)所示；

对于固连在待测目标上的两个特征点A和B，在t_i时刻和t_i+1时刻的世界坐标分别为将从t_i时刻下的物体坐标系S_i到世界坐标系W的坐标变换矩阵记作将从t_i+1时刻下的物体坐标系S_i+1到世界坐标系W的坐标变换矩阵记作则有

由和为坐标变换矩阵可知，应具有如下形式

将式(37)代入式(35)可得

将式(37)代入式(36)可得

根据式(38)的前两行和式(39)的前两行，可得

根据式(40)可求得C、S、X、Y的值，即求出了中未知元素的值；

求得后，可以根据式(41)进一步求得

求得后，再根据求解t_i+1时刻的位置和姿态θ_i+1；

设

代入得

其中

根据式(41)和式(44)可以得到cos(θ_i+1-θ_i)和sin(θ_i+1-θ_i)的值，进一步可求得Δθ＝θ_i+1-θ_i，从而得到t_i+1时刻的姿态θ_i+1；

根据式(41)和式(44)可以得到和的值，代入式(45)和式(46)中可进一步求得和从而得到t_i+1时刻的位置

根据和θ_i+1求出用于下一时刻的位姿迭代更新。

2.根据权利要求1所述的基于迭代更新的单目相机位姿测量方法，其特征在于，所述通过位姿迭代更新的方式实现对待测目标的位姿测量的具体步骤为：第一步是待测目标的初始位姿确定，第二步是待测目标的位姿迭代更新。