CN111563933B - 一种基于单目视觉的位姿测量方法 - Google Patents

一种基于单目视觉的位姿测量方法 Download PDF

Info

Publication number
CN111563933B
CN111563933B CN202010452635.0A CN202010452635A CN111563933B CN 111563933 B CN111563933 B CN 111563933B CN 202010452635 A CN202010452635 A CN 202010452635A CN 111563933 B CN111563933 B CN 111563933B
Authority
CN
China
Prior art keywords
coordinate system
measured object
pose
camera
point
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202010452635.0A
Other languages
English (en)
Other versions
CN111563933A (zh
Inventor
罗卓军
王展
付鑫
李一娴
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Individual
Original Assignee
Individual
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Individual filed Critical Individual
Priority to CN202010452635.0A priority Critical patent/CN111563933B/zh
Publication of CN111563933A publication Critical patent/CN111563933A/zh
Application granted granted Critical
Publication of CN111563933B publication Critical patent/CN111563933B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01B—MEASURING LENGTH, THICKNESS OR SIMILAR LINEAR DIMENSIONS; MEASURING ANGLES; MEASURING AREAS; MEASURING IRREGULARITIES OF SURFACES OR CONTOURS
    • G01B11/00—Measuring arrangements characterised by the use of optical techniques
    • G01B11/002—Measuring arrangements characterised by the use of optical techniques for measuring two or more coordinates
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Mathematical Physics (AREA)
  • Theoretical Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Data Mining & Analysis (AREA)
  • Computational Mathematics (AREA)
  • Algebra (AREA)
  • Databases & Information Systems (AREA)
  • Software Systems (AREA)
  • General Engineering & Computer Science (AREA)
  • Operations Research (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Computing Systems (AREA)
  • Length Measuring Devices By Optical Means (AREA)

Abstract

本发明公开了一种基于单目视觉的位姿测量方法，具体包括以下步骤：对针孔模型进行建模，得出被测对象位姿参数方程；定义迭代误差；根据迭代误差，基于搜索区间迭代更新的求解方法求解被测对象位姿参数；将计算出来的被测对象位姿参数代入被测对象位姿参数方程中，得出被测对象的位姿；本技术方案通过采用基于搜索区间迭代更新的求解方法进行被测对象的位姿计算，避免了复杂的矩阵运算和优化算法运算，有利于程序代码的实现。

Description

一种基于单目视觉的位姿测量方法
技术领域
本发明涉及一种位姿测量方法，尤其涉及的是一种基于单目视觉的位姿测量方法。
背景技术
基于单目摄像机得到的二维图像提取被测对象在图像中的特征，并将二维特征与三维空间中被测对象的某些特征建立联系，然后通过计算获得被测对象相对于摄像机坐标系的位置和姿态，这一过程称为基于单目视觉的位姿测量过程。
现有的单目视觉位姿测量方法在迭代求解被测对象位姿过程中或需要进行矩阵分解等复杂的矩阵运算，或采用较为复杂的最优化算法或者智能算法进行迭代求解，不利于程序代码的实现。
因此，现有的技术还有待于改进和发展。
发明内容
本发明的目的在于提供一种基于单目视觉的位姿测量方法，旨在解决现有的单目视觉位姿测量方法需要进行复杂的矩阵运算和优化算法运算，不利于程序代码实现的问题。
本发明的技术方案如下：
一种基于单目视觉的位姿测量方法，其中，具体包括以下步骤：
S1：对针孔模型进行建模，得出被测对象位姿参数方程；
S2：定义迭代误差；
S3：根据迭代误差，基于搜索区间迭代更新的求解方法求解被测对象位姿参数；
S4：将计算出来的被测对象位姿参数代入被测对象位姿参数方程中，得出被测对象的位姿。
本技术方案中，通过
所述的基于单目视觉的位姿测量方法，其中，所述S1包括以下步骤：
s11:设定摄像头坐标系，得出被测对象特征点在摄像头坐标系中的坐标；
s12：设定像平面坐标系，得出被测对象特征点在像平面坐标系中的像点坐标；
s13：根据s11和s12，得出被测对象特征点基于摄像头坐标系和像平面坐标系的直线参数方程，即被测对象的位姿参数方程；
其中，所述被测对象特征点的选取数量为至少三个。
所述的基于单目视觉的位姿测量方法，其中，被测对象特征点的选取数量为三个且三个被测对象特征点不在同一直线上。
所述的基于单目视觉的位姿测量方法，其中，三个特征点两两相连形成等边三角形。
所述的基于单目视觉的位姿测量方法，其中，当被测对象特征点选取三个，分别为P、Q、R时，所述S2包括以下过程：设定摄像头坐标系的原点位于摄像头光心处且Z轴与摄像头光轴共线；设定像平面坐标系的原点位于摄像头光轴上，u为像平面横坐标轴，v为像平面纵坐标轴；设定被测对象三个特征点之间的实际距离分别为
Figure 389569DEST_PATH_IMAGE002
、
Figure 103447DEST_PATH_IMAGE004
、
Figure 538976DEST_PATH_IMAGE006
，在迭代求解过程中
Figure 262081DEST_PATH_IMAGE008
、
Figure 358344DEST_PATH_IMAGE010
、
Figure 711965DEST_PATH_IMAGE012
三个被测对象位姿参数的迭代值分别记为
Figure 41316DEST_PATH_IMAGE014
、
Figure 17712DEST_PATH_IMAGE016
、
Figure 483329DEST_PATH_IMAGE018
，则定义迭代误差为：
Figure 7851DEST_PATH_IMAGE020
即：
Figure 575229DEST_PATH_IMAGE022
其中，
Figure 577821DEST_PATH_IMAGE024
、
Figure 897943DEST_PATH_IMAGE026
和
Figure 858946DEST_PATH_IMAGE028
为P点在摄像头坐标系中的坐标，其中，
Figure 412156DEST_PATH_IMAGE030
、
Figure 484017DEST_PATH_IMAGE032
和
Figure 674958DEST_PATH_IMAGE034
为Q点在摄像头坐标系中的坐标，其中，
Figure 541283DEST_PATH_IMAGE036
、
Figure 850298DEST_PATH_IMAGE038
和
Figure 460271DEST_PATH_IMAGE040
为R点在摄像头坐标系中的坐标；
Figure 568036DEST_PATH_IMAGE042
和
Figure 136420DEST_PATH_IMAGE044
为P点在像坐标系中的像点p的坐标，
Figure 618217DEST_PATH_IMAGE046
和
Figure 546728DEST_PATH_IMAGE048
为Q点在像坐标系中的像点q的坐标，
Figure 430370DEST_PATH_IMAGE050
和
Figure 169656DEST_PATH_IMAGE052
为R点在像坐标系中的像点r的坐标；
Figure 889481DEST_PATH_IMAGE054
为被测对象P点到摄像头坐标系原点的距离与被测对象P点在像坐标系中的像点p到摄像头坐标系原点的距离的比值，
Figure 841257DEST_PATH_IMAGE056
为被测对象Q点到摄像头坐标系原点的距离与被测对象Q点在像坐标系中的像点q到摄像头坐标系原点的距离的比值，
Figure 844985DEST_PATH_IMAGE058
为被测对象R点到摄像头坐标系原点的距离与被测对象R点在像坐标系中的像点r到摄像头坐标系原点的距离的比值；f为摄像头的焦距。
所述的基于单目视觉的位姿测量方法，其中，所述S3具体包括以下步骤：
s31:
Figure 755172DEST_PATH_IMAGE060
、
Figure 989058DEST_PATH_IMAGE062
、
Figure 619890DEST_PATH_IMAGE064
的初始搜索区间为
Figure 727392DEST_PATH_IMAGE066
，将该区间进行n-1等分，得到n个
Figure 542902DEST_PATH_IMAGE068
值、n个
Figure 486587DEST_PATH_IMAGE070
值、n个
Figure 311323DEST_PATH_IMAGE072
值，即可得到共计
Figure 886049DEST_PATH_IMAGE074
个
Figure 934776DEST_PATH_IMAGE075
、
Figure 382069DEST_PATH_IMAGE076
、
Figure 744917DEST_PATH_IMAGE077
迭代值的组合，将这
Figure 92591DEST_PATH_IMAGE079
组迭代值代入迭代误差公式中并比较得到使
Figure 984324DEST_PATH_IMAGE081
取最小值时的参数组合，记为
Figure 902601DEST_PATH_IMAGE083
、
Figure 351031DEST_PATH_IMAGE085
、
Figure 835102DEST_PATH_IMAGE087
；
s32:更新搜索区间，令
Figure 964249DEST_PATH_IMAGE068
的第i+1次搜索区间更改为
Figure 838664DEST_PATH_IMAGE089
，其中
Figure 808894DEST_PATH_IMAGE091
，
Figure 101466DEST_PATH_IMAGE093
，同理可更新
Figure 866160DEST_PATH_IMAGE076
、
Figure 493450DEST_PATH_IMAGE077
的搜索区间；将三个参数的搜索区间进行n-1等分，同时更新步长
Figure 516639DEST_PATH_IMAGE095
，即可得到共计
Figure 178564DEST_PATH_IMAGE097
个
Figure 661630DEST_PATH_IMAGE099
、
Figure 307375DEST_PATH_IMAGE101
、
Figure 933921DEST_PATH_IMAGE103
迭代值的组合，将这
Figure 201086DEST_PATH_IMAGE105
组迭代值代入迭代误差公式中并比较得到使
Figure 307582DEST_PATH_IMAGE107
取最小值的参数组合，记为
Figure 909465DEST_PATH_IMAGE109
、
Figure 8877DEST_PATH_IMAGE111
、
Figure 379815DEST_PATH_IMAGE113
；
s33:判断迭代误差
Figure 657213DEST_PATH_IMAGE107
与预设收敛误差阈值的大小以及迭代次数是否大于等于最大迭代次数，若迭代误差
Figure 231545DEST_PATH_IMAGE107
小于预设收敛误差阈值或迭代次数大于等于最大迭代次数，则执行S4，否则执行s34；
s34：重复执行s32和s33。
所述的基于单目视觉的位姿测量方法，其中，所述S3中对搜索区间的划分采用等分方式进行。
所述的基于单目视觉的位姿测量方法，其中，所述S4具体包括以下过程：完成迭代求解得到被测对象位姿参数后，将被测对象位姿参数代入被测对象位姿参数方程中计算被测对象特征点在摄像头坐标系中的坐标，然后利用已知被测对象特征点在被测对象坐标系中的坐标，即可计算出被测对象坐标系在摄像头坐标系中的描述，即被测对象在摄像头坐标系中的位姿。
本发明的有益效果：本发明通过提供一种基于单目视觉的位姿测量方法，通过采用基于搜索区间迭代更新的求解方法进行被测对象的位姿计算，避免了复杂的矩阵运算和优化算法运算，有利于程序代码的实现。
附图说明
图1是本发明中基于单目视觉的位姿测量方法的步骤流程图。
图2是本发明中单目视觉位姿测量针孔模型示意图。
具体实施方式
下面详细描述本发明的实施方式，所述实施方式的示例在附图中示出，其中自始至终相同或类似的标号表示相同或类似的元件或具有相同或类似功能的元件。下面通过参考附图描述的实施方式是示例性的，仅用于解释本发明，而不能理解为对本发明的限制。
在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“长度”、“宽度”、“厚度”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”、“顺时针”、“逆时针”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”的特征可以明示或者隐含地包括一个或者更多个所述特征。在本发明的描述中，“多个”的含义是两个或两个以上，除非另有明确具体的限定。
在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接或可以相互通讯；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通或两个元件的相互作用关系。对于本领域的普通技术人员而言，可以根据具体情况理解上述术语在本发明中的具体含义。
在本发明中，除非另有明确的规定和限定，第一特征在第二特征之“上”或之“下”可以包括第一和第二特征直接接触，也可以包括第一和第二特征不是直接接触而是通过它们之间的另外的特征接触。而且，第一特征在第二特征“之上”、“上方”和“上面”包括第一特征在第二特征正上方和斜上方，或仅仅表示第一特征水平高度高于第二特征。第一特征在第二特征“之下”、“下方”和“下面”包括第一特征在第二特征正下方和斜下方，或仅仅表示第一特征水平高度小于第二特征。
下文的公开提供了许多不同的实施方式或例子用来实现本发明的不同结构。为了简化本发明的公开，下文中对特定例子的部件和设置进行描述。当然，它们仅仅为示例，并且目的不在于限制本发明。此外，本发明可以在不同例子中重复参考数字和/或参考字母，这种重复是为了简化和清楚的目的，其本身不指示所讨论各种实施方式和/或设置之间的关系。此外，本发明提供了的各种特定的工艺和材料的例子，但是本领域普通技术人员可以意识到其他工艺的应用和/或其他材料的使用。
如图1所示，一种基于单目视觉的位姿测量方法，具体包括以下步骤：
一种基于单目视觉的位姿测量方法，其中，具体包括以下步骤：
S1：对针孔模型进行建模，得出被测对象位姿参数方程。
在某些具体实施例中，所述S1包括以下步骤：
s11:设定摄像头坐标系，得出被测对象特征点在摄像头坐标系中的坐标；
s12：设定像平面坐标系，得出被测对象特征点在像平面坐标系中的像点坐标；
s13：根据s11和s12，得出被测对象特征点基于摄像头坐标系和像平面坐标系的直线参数方程，即被测对象的位姿参数方程；
其中，所述被测对象特征点的选取数量为至少三个。
其中，所述被测对象特征点的选取数量越多，计算出来的结果越精确，但其中的计算过程也会越复杂。作为一种优选实施例，所述被测对象特征点的选取数量为三个且三个被测对象特征点不在同一直线上，选取三个被测对象特征点既可保证计算结果准确性满足要求，又能使计算过程尽可能简单；当三个特征点两两相连形成等边三角形时，整个计算过程较为简单。
S2：定义迭代误差。
其中，在求解被测对象位姿参数前，需要先定义被测对象位姿参数迭代误差的约束条件。
S3：根据迭代误差，基于搜索区间迭代更新的求解方法求解被测对象位姿参数。
其中，通过搜索区间迭代更新不断缩小区间的范围，当某一区间的被测对象位姿参数的迭代误差满足设定条件时，该被测对象位姿参数即为所求参数值。
S4：将计算出来的被测对象位姿参数代入被测对象位姿参数方程中，得出被测对象的位姿。
其中，所述S4具体包括以下过程：完成迭代求解得到被测对象位姿参数后，将被测对象位姿参数代入被测对象位姿参数方程中计算被测对象特征点在摄像头坐标系中的坐标，然后利用已知被测对象特征点在被测对象坐标系中的坐标，即可计算出被测对象坐标系在摄像头坐标系中的描述，即被测对象在摄像头坐标系中的位姿。
根据上述所述的基于单目视觉的位姿测量方法，现列举以下实施例加以说明：
S1：对针孔模型进行建模，得出被测对象位姿参数方程。
如图2所示，为本技术方案进行单目视觉位姿测量所使用的针孔模型，记摄像头坐标系为
Figure 619801DEST_PATH_IMAGE115
，其原点
Figure 110825DEST_PATH_IMAGE117
位于摄像头光心处且
Figure 293544DEST_PATH_IMAGE119
轴与摄像头光轴共线。
被测对象上的三个坐标点记为P、Q、R，它们在像平面上的像点分别为p、q、r；像平面坐标系
Figure 647516DEST_PATH_IMAGE121
的原点位于摄像头光轴上，u为像平面横坐标轴，v为像平面纵坐标轴。
根据摄像头所获得的二维图像，可获得p、q、r三点在像平面坐标系中的坐标
Figure 839463DEST_PATH_IMAGE123
、
Figure 935726DEST_PATH_IMAGE125
、
Figure 86084DEST_PATH_IMAGE127
；另外，记摄像头坐标系中，P、Q、R三点的坐标分别为
Figure 884276DEST_PATH_IMAGE129
、
Figure 660340DEST_PATH_IMAGE131
、
Figure 876689DEST_PATH_IMAGE133
，并定义摄像头的焦距为f，则可列出直线OP的参数方程（其中，O为摄像头坐标系的原点）：
Figure 932369DEST_PATH_IMAGE135
式（1）
由于
Figure 217857DEST_PATH_IMAGE137
从摄像头所获得的二维图像中可以得出，f为摄像头的已知参数，则求解P点的坐标的问题就是求解参数
Figure 3804DEST_PATH_IMAGE139
的问题，同理，可列出直线OQ和OR的参数方程（其中，O为摄像头坐标系的原点）：
Figure 589506DEST_PATH_IMAGE141
式（2）
Figure 832400DEST_PATH_IMAGE143
式（3）。
S2：定义迭代误差。
在求解参数
Figure 74025DEST_PATH_IMAGE145
、
Figure 411466DEST_PATH_IMAGE147
、
Figure 851674DEST_PATH_IMAGE149
前需先定义迭代误差的约束条件，记被测对象三个特征点之间的实际距离分别为
Figure 967267DEST_PATH_IMAGE151
、
Figure 492926DEST_PATH_IMAGE153
、
Figure 102899DEST_PATH_IMAGE155
，在迭代求解过程中
Figure 148346DEST_PATH_IMAGE157
、
Figure 185573DEST_PATH_IMAGE147
、
Figure 932949DEST_PATH_IMAGE149
三个参数的迭代值分别记为
Figure 858530DEST_PATH_IMAGE159
、
Figure 7751DEST_PATH_IMAGE161
、
Figure 215879DEST_PATH_IMAGE163
，则定义迭代误差为：
Figure 450551DEST_PATH_IMAGE165
即：
Figure 153059DEST_PATH_IMAGE167
式（4）。
S3：根据迭代误差，基于搜索区间迭代更新的求解方法求解被测对象位姿参数。
为求解参数
Figure 422366DEST_PATH_IMAGE169
、
Figure 801395DEST_PATH_IMAGE147
、
Figure 772631DEST_PATH_IMAGE149
以得到P、Q、R三点的坐标，本方案采用如下基于搜索区间迭代更新的求解方法进行求解：
s31:
Figure 262518DEST_PATH_IMAGE171
、
Figure 120753DEST_PATH_IMAGE173
、
Figure 936262DEST_PATH_IMAGE175
的初始搜索区间为
Figure 896259DEST_PATH_IMAGE177
（
Figure 189837DEST_PATH_IMAGE179
和
Figure 902578DEST_PATH_IMAGE181
根据摄像头的有效测量范围预先定义），将该区间进行n-1(
Figure 888988DEST_PATH_IMAGE183
)等分（即步长
Figure 837746DEST_PATH_IMAGE185
），得到n个
Figure 935015DEST_PATH_IMAGE186
值、n个
Figure 767842DEST_PATH_IMAGE188
值、n个
Figure 675886DEST_PATH_IMAGE189
值，即可得到共计
Figure 859743DEST_PATH_IMAGE191
个
Figure 229544DEST_PATH_IMAGE193
、
Figure 431724DEST_PATH_IMAGE195
、
Figure 759938DEST_PATH_IMAGE197
迭代值的组合，将这
Figure 899932DEST_PATH_IMAGE199
组迭代值代入式（4）并比较得到使
Figure 870162DEST_PATH_IMAGE201
取最小值时的参数组合，记为
Figure 162734DEST_PATH_IMAGE203
、
Figure 661849DEST_PATH_IMAGE205
、
Figure 289139DEST_PATH_IMAGE207
；
s32:更新搜索区间，令
Figure 266322DEST_PATH_IMAGE157
的第i+1次搜索区间更改为
Figure 705744DEST_PATH_IMAGE209
，其中
Figure 375760DEST_PATH_IMAGE211
（下标i表示第i次迭代计算得到的值），
Figure 506658DEST_PATH_IMAGE213
，同理可更新
Figure 21953DEST_PATH_IMAGE195
、
Figure 803964DEST_PATH_IMAGE197
的搜索区间；将三个参数的搜索区间进行n-1 (
Figure 644881DEST_PATH_IMAGE183
)等分，同时更新步长
Figure 496032DEST_PATH_IMAGE215
，即可得到共计
Figure 80597DEST_PATH_IMAGE217
个
Figure 248273DEST_PATH_IMAGE186
、
Figure 541982DEST_PATH_IMAGE195
、
Figure 896740DEST_PATH_IMAGE197
迭代值的组合，将这
Figure 537193DEST_PATH_IMAGE219
组迭代值代入式（4）并比较得到使
Figure 28217DEST_PATH_IMAGE221
取最小值的参数组合，记为
Figure 210937DEST_PATH_IMAGE223
、
Figure DEST_PATH_IMAGE225
、
Figure 69303DEST_PATH_IMAGE227
；
s33:判断迭代误差
Figure DEST_PATH_IMAGE229
与预设收敛误差阈值
Figure DEST_PATH_IMAGE231
的大小以及迭代次数是否大于等于最大迭代次数
Figure DEST_PATH_IMAGE233
，若迭代误差
Figure 369572DEST_PATH_IMAGE234
小于预设收敛误差阈值
Figure 715103DEST_PATH_IMAGE236
或迭代次数大于等于最大迭代次数
Figure 85035DEST_PATH_IMAGE238
，则执行S4，否则执行s34；
s34：重复执行s32和s33，用上一次迭代得到的参数值和步长更新下一次迭代的参数搜索区间和步长，同时得到用于下一次迭代的
Figure 148806DEST_PATH_IMAGE199
个
Figure 613286DEST_PATH_IMAGE240
、
Figure 348677DEST_PATH_IMAGE173
、
Figure 138779DEST_PATH_IMAGE189
迭代值的组合，并计算最小的
Figure 158687DEST_PATH_IMAGE221
值。
其中，通过采用区间等分，可以保证覆盖全部的参数组合，避免遗漏参数组合，影响计算结果的精确性。
S4：将计算出来的被测对象位姿参数代入被测对象位姿参数方程中，得出被测对象的位姿。
完成迭代求解得到
Figure 708749DEST_PATH_IMAGE242
、
Figure 763292DEST_PATH_IMAGE147
、
Figure DEST_PATH_IMAGE243
三个参数后，即可根据式（1）至式（3）计算P、Q、R三个特征点在摄像头坐标系中的坐标，然后利用已知的3个特征点在被测对象坐标系中的坐标，即可计算出被测对象坐标系在摄像头坐标系中的描述，即被测对象在摄像头坐标系中的位姿。
例如，如图2中的被测对象为等边三角形时，则被测对象坐标系的原点
Figure DEST_PATH_IMAGE245
在摄像头坐标系
Figure DEST_PATH_IMAGE247
中的坐标为：
Figure 567038DEST_PATH_IMAGE249
式（5）
即，被测对象坐标系
Figure 339822DEST_PATH_IMAGE251
相对于摄像头坐标系
Figure DEST_PATH_IMAGE253
的平移变换向量为：
Figure DEST_PATH_IMAGE255
式（6）
若记
Figure DEST_PATH_IMAGE257
，则被测对象坐标系
Figure DEST_PATH_IMAGE259
相对于摄像头坐标系
Figure 70405DEST_PATH_IMAGE253
的旋转变换矩阵为：
Figure DEST_PATH_IMAGE261
式（7）
其中，R为旋转变换矩阵。
本技术方案通过采用基于搜索区间迭代更新的求解方法进行被测对象的位姿计算，避免了复杂的矩阵运算和优化算法运算，有利于程序代码的实现。
在本说明书的描述中，参考术语“一个实施方式”、“某些实施方式”、“示意性实施方式”、“示例”、“具体示例”、或“一些示例”等的描述意指结合所述实施方式或示例描述的具体特征、结构、材料或者特点包含于本发明的至少一个实施方式或示例中。在本说明书中，对上述术语的示意性表述不一定指的是相同的实施方式或示例。而且，描述的具体特征、结构、材料或者特点可以在任何的一个或多个实施方式或示例中以合适的方式结合。
应当理解的是，本发明的应用不限于上述的举例，对本领域普通技术人员来说，可以根据上述说明加以改进或变换，所有这些改进和变换都应属于本发明所附权利要求的保护范围。

Claims (8)

1.一种基于单目视觉的位姿测量方法，其特征在于，具体包括以下步骤：
S1：对针孔模型进行建模，得出被测对象位姿参数方程；
S2：定义迭代误差；
S3：根据迭代误差，基于搜索区间迭代更新的求解方法求解被测对象位姿参数；
S4：将计算出来的被测对象位姿参数代入被测对象位姿参数方程中，得出被测对象的位姿。
2.根据权利要求1所述的基于单目视觉的位姿测量方法，其特征在于，所述S1包括以下步骤：
s11:设定摄像头坐标系，得出被测对象特征点在摄像头坐标系中的坐标；
s12：设定像平面坐标系，得出被测对象特征点在像平面坐标系中的像点坐标；
s13：根据s11和s12，得出被测对象特征点基于摄像头坐标系和像平面坐标系的直线参数方程，即被测对象的位姿参数方程；
其中，所述被测对象特征点的选取数量为至少三个。
3.根据权利要求2所述的基于单目视觉的位姿测量方法，其特征在于，被测对象特征点的选取数量为三个且三个被测对象特征点不在同一直线上。
4.根据权利要求3所述的基于单目视觉的位姿测量方法，其特征在于，三个特征点两两相连形成等边三角形。
5.根据权利要求2至4任一所述的基于单目视觉的位姿测量方法，其特征在于，当被测对象特征点选取三个，分别为P、Q、R时，所述S2包括以下过程：设定摄像头坐标系的原点位于摄像头光心处且Z轴与摄像头光轴共线；设定像平面坐标系的原点位于摄像头光轴上，u为像平面横坐标轴，v为像平面纵坐标轴；设定被测对象三个特征点之间的实际距离分别为
Figure DEST_PATH_IMAGE002
、
Figure DEST_PATH_IMAGE004
、
Figure DEST_PATH_IMAGE006
，在迭代求解过程中被测对象位姿参数
Figure DEST_PATH_IMAGE008
的迭代值记为
Figure DEST_PATH_IMAGE010
，被测对象位姿参数
Figure DEST_PATH_IMAGE012
迭代值记为
Figure DEST_PATH_IMAGE014
、被测对象位姿参数
Figure DEST_PATH_IMAGE016
的迭代值分别记为
Figure DEST_PATH_IMAGE018
，则定义迭代误差为：
Figure DEST_PATH_IMAGE020
即：
Figure DEST_PATH_IMAGE022
其中，
Figure DEST_PATH_IMAGE024
、
Figure DEST_PATH_IMAGE026
和
Figure DEST_PATH_IMAGE028
为P点在摄像头坐标系中的坐标，
Figure DEST_PATH_IMAGE030
、
Figure DEST_PATH_IMAGE032
和
Figure DEST_PATH_IMAGE034
为Q点在摄像头坐标系中的坐标，
Figure DEST_PATH_IMAGE036
、
Figure DEST_PATH_IMAGE038
和
Figure DEST_PATH_IMAGE040
为R点在摄像头坐标系中的坐标；
Figure DEST_PATH_IMAGE042
和
Figure DEST_PATH_IMAGE044
为P点在像平面坐标系中的像点p的坐标，
Figure DEST_PATH_IMAGE046
和
Figure DEST_PATH_IMAGE048
为Q点在像平面坐标系中的像点q的坐标，
Figure DEST_PATH_IMAGE050
和
Figure DEST_PATH_IMAGE052
为R点在像平面坐标系中的像点r的坐标；
Figure DEST_PATH_IMAGE054
为被测对象P点到摄像头坐标系原点的距离与被测对象P点在像平面坐标系中的像点p到摄像头坐标系原点的距离的比值，
Figure DEST_PATH_IMAGE056
为被测对象Q点到摄像头坐标系原点的距离与被测对象Q点在像平面坐标系中的像点q到摄像头坐标系原点的距离的比值，
Figure DEST_PATH_IMAGE058
为被测对象R点到摄像头坐标系原点的距离与被测对象R点在像平面坐标系中的像点r到摄像头坐标系原点的距离的比值；f为摄像头的焦距。
6.根据权利要求5所述的基于单目视觉的位姿测量方法，其特征在于，所述S3具体包括以下步骤：
s31:
Figure DEST_PATH_IMAGE059
、
Figure 828594DEST_PATH_IMAGE056
、
Figure DEST_PATH_IMAGE060
的初始搜索区间为
Figure DEST_PATH_IMAGE062
，将该区间划分为n-1个，得到n个
Figure 729423DEST_PATH_IMAGE059
值、n个
Figure 712423DEST_PATH_IMAGE056
值、n个
Figure 435528DEST_PATH_IMAGE060
值，即可得到共计
Figure DEST_PATH_IMAGE064
个
Figure DEST_PATH_IMAGE008A
、
Figure DEST_PATH_IMAGE012A
、
Figure DEST_PATH_IMAGE016A
的迭代值
Figure DEST_PATH_IMAGE010A
、
Figure DEST_PATH_IMAGE014A
、
Figure DEST_PATH_IMAGE018A
的组合，将这
Figure DEST_PATH_IMAGE072
组迭代值代入迭代误差公式中并比较得到使
Figure DEST_PATH_IMAGE074
取最小值时的参数组合，记为
Figure DEST_PATH_IMAGE076
、
Figure DEST_PATH_IMAGE078
、
Figure DEST_PATH_IMAGE080
；
s32:更新搜索区间，令
Figure 794441DEST_PATH_IMAGE059
的第i+1次搜索区间更改为
Figure DEST_PATH_IMAGE082
，其中
Figure DEST_PATH_IMAGE084
，
Figure DEST_PATH_IMAGE086
，同理可更新
Figure 662908DEST_PATH_IMAGE056
、
Figure 133204DEST_PATH_IMAGE060
的搜索区间；将三个参数的搜索区间划分为n-1个，同时更新步长
Figure DEST_PATH_IMAGE088
，即可得到共计
Figure 253476DEST_PATH_IMAGE072
个
Figure DEST_PATH_IMAGE090
、
Figure DEST_PATH_IMAGE092
、
Figure 801917DEST_PATH_IMAGE060
迭代值的组合，将这
Figure 264122DEST_PATH_IMAGE072
组迭代值代入迭代误差公式中并比较得到使
Figure 346348DEST_PATH_IMAGE074
取最小值的参数组合，记为
Figure DEST_PATH_IMAGE094
、
Figure DEST_PATH_IMAGE096
、
Figure DEST_PATH_IMAGE098
；
s33:判断迭代误差
Figure 801469DEST_PATH_IMAGE074
与预设收敛误差阈值的大小以及迭代次数是否大于等于最大迭代次数，若迭代误差
Figure DEST_PATH_IMAGE099
小于预设收敛误差阈值或迭代次数大于等于最大迭代次数，则执行S4，否则执行s34；
s34：重复执行s32和s33。
7.根据权利要求6所述的基于单目视觉的位姿测量方法，其特征在于，所述S3中对搜索区间的划分采用等分方式进行。
8.根据权利要求1所述的基于单目视觉的位姿测量方法，其特征在于，所述S4具体包括以下过程：完成迭代求解得到被测对象位姿参数后，将被测对象位姿参数代入被测对象位姿参数方程中计算被测对象特征点在摄像头坐标系中的坐标，然后利用已知被测对象特征点在被测对象坐标系中的坐标，即可计算出被测对象坐标系在摄像头坐标系中的描述，即被测对象在摄像头坐标系中的位姿。
CN202010452635.0A 2020-05-26 2020-05-26 一种基于单目视觉的位姿测量方法 Active CN111563933B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010452635.0A CN111563933B (zh) 2020-05-26 2020-05-26 一种基于单目视觉的位姿测量方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010452635.0A CN111563933B (zh) 2020-05-26 2020-05-26 一种基于单目视觉的位姿测量方法

Publications (2)

Publication Number Publication Date
CN111563933A CN111563933A (zh) 2020-08-21
CN111563933B true CN111563933B (zh) 2020-11-03

Family

ID=72069825

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010452635.0A Active CN111563933B (zh) 2020-05-26 2020-05-26 一种基于单目视觉的位姿测量方法

Country Status (1)

Country Link
CN (1) CN111563933B (zh)

Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103528571A (zh) * 2013-10-12 2014-01-22 上海新跃仪表厂 单目立体视觉相对位姿测量方法
CN104154918A (zh) * 2014-07-14 2014-11-19 南京航空航天大学 单目视觉导航特征点丢失故障处理方法

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN105913417B (zh) * 2016-04-05 2018-09-28 天津大学 基于透视投影直线的几何约束位姿方法
CN109115184B (zh) * 2018-07-13 2021-06-22 哈尔滨工业大学（深圳） 基于非合作目标协同测量方法及系统
CN109712172A (zh) * 2018-12-28 2019-05-03 哈尔滨工业大学 一种初始位姿测量结合目标追踪的位姿测量方法
CN110544279B (zh) * 2019-08-26 2023-06-23 华南理工大学 一种结合图像识别和遗传算法精配准的位姿估计方法

Patent Citations (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN103528571A (zh) * 2013-10-12 2014-01-22 上海新跃仪表厂 单目立体视觉相对位姿测量方法
CN104154918A (zh) * 2014-07-14 2014-11-19 南京航空航天大学 单目视觉导航特征点丢失故障处理方法

Also Published As

Publication number Publication date
CN111563933A (zh) 2020-08-21

Similar Documents

Publication Publication Date Title
Li et al. A 4-point algorithm for relative pose estimation of a calibrated camera with a known relative rotation angle
CN110390697B (zh) 一种基于lm算法的毫米波雷达与摄像头联合标定方法
CN108510551B (zh) 一种远距离大视场条件下相机参数的标定方法及系统
CN110264528B (zh) 一种鱼眼镜头双目像机快速自标定方法
CN111380502B (zh) 标定方法、位置确定方法、装置、电子设备及存储介质
CN106457562A (zh) 用于校准机器人的方法和机器人系统
CN108198219B (zh) 用于摄影测量的相机标定参数的误差补偿方法
CN110969662A (zh) 鱼眼摄像机内参标定方法、装置、标定装置控制器和系统
CN112785655A (zh) 一种基于车道线检测的环视相机外参自动标定方法、装置、设备及计算机存储介质
CN105910584B (zh) 大尺寸动态摄影测量系统的高精度定向及定向精度评价方法
CN110345921A (zh) 立体视场视觉测量及垂轴像差和轴向像差校正方法及系统
CN106254860A (zh) 照相机的校准装置、照相机系统、以及照相机的校准方法
CN109003309B (zh) 一种高精度相机标定和目标姿态估计方法
CN107330927B (zh) 机载可见光图像定位方法
CN102452081A (zh) 机械手臂系统参数的校正方法与校正装置
CN109272555B (zh) 一种rgb-d相机的外部参数获得及标定方法
CN109465830B (zh) 机器人单目立体视觉标定系统及方法
CN106991705B (zh) 一种基于p3p算法的位置参数估计方法
CN115187658B (zh) 多相机视觉的大目标定位方法、系统及设备
CN110796710B (zh) 基于流形空间的快速在线标定相机外参的方法和模块
JP2012198031A (ja) 画像補正方法及び画像補正装置
CN114758011A (zh) 融合离线标定结果的变焦相机在线标定方法
CN111563933B (zh) 一种基于单目视觉的位姿测量方法
CN116576850A (zh) 一种位姿确定方法、装置、计算机设备及存储介质
CN112819900B (zh) 一种智能立体摄影内方位、相对定向和畸变系数标定方法

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant