发明名称 基于TOF无扫描三维成像的位姿测量方法
摘要 本发明公开了一种基于TOF无扫描三维成像的位姿测量方法,其特征是首先对目标物体建立一个三维的坐标信息数据库,利用TOF相机作为成像和距离数据获取装置,在拍摄的图片中选取三个可识别的物体作为标记点,在数据库中获得各标记点在目标本体坐标系下的坐标信息,建立一个向量组;通过TOF相机获得的数据信息,由非迭代三维空间两点间距离计算方法得出光心到各标记点的距离,经过计算得到标记点在相机坐标系下的坐标,再建立另一个向量组,由两个向量组之间的关系可以计算得出旋转矩阵和平移矩阵,从而获得姿态角和平移量,即目标物体的相对姿态。本发明避免了大量的迭代算法,并且可以快速求解满足位姿参数确定精度的要求。
申请公布号 CN102252653B 申请公布日期 2012.09.26
申请号 CN201110175406.X 申请日期 2011.06.27
申请人 合肥工业大学 发明人 张旭东;高隽;叶子瑞;范之国;李文龙;高丽娟;吴国松
分类号 G01C1/00(2006.01)I;G01C3/00(2006.01)I;G01C11/00(2006.01)I 主分类号 G01C1/00(2006.01)I
代理机构 安徽省合肥新安专利代理有限责任公司 34101 代理人 何梅生
主权项 1.一种基于TOF无扫描三维成像的位姿测量方法,其特征在于包括以下步骤:1)通过TOF相机拍摄目标物体,选取目标物体的成像图像中三个可识别物体为标记点P<sub>i</sub>,i=1,2,3,并从事先建立的目标物体三维坐标信息数据库中获得标记点P<sub>i</sub>在目标本体坐标系下的坐标P<sub>i</sub>=(X<sub>i</sub>,Y<sub>i</sub>,Z<sub>i</sub>),i=1,2,3,通过任意两个标记点的连线求出两个线性无关的单位列向量<img file="FDA00001752948100011.GIF" wi="46" he="74" />和<img file="FDA00001752948100012.GIF" wi="66" he="74" />令<img file="FDA00001752948100013.GIF" wi="38" he="74" />为<img file="FDA00001752948100014.GIF" wi="142" he="74" />所构成的单位列向量,构造出三个标记点P<sub>i</sub>在目标本体坐标系下的向量组:<maths num="0001"><![CDATA[<math><mrow><msub><mi>n</mi><mi>o</mi></msub><mo>=</mo><mfenced open='[' close=']'><mtable><mtr><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>o</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>o</mi><mn>2</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>o</mi><mn>3</mn></mrow></msub></mtd></mtr></mtable></mfenced><mo>;</mo></mrow></math>]]></maths>2)通过TOF相机拍摄目标物体获得的标记点P<sub>i</sub>的像点在像坐标系下的像素坐标计算出标记点P<sub>i</sub>在相机坐标系下的坐标S<sub>i</sub>,所述标记点P<sub>i</sub>在相机坐标系下的坐标S<sub>i</sub>计算方法为:a.将标记点P<sub>i</sub>的像点在像坐标系下的像素坐标(u<sub>i</sub>,v<sub>i</sub>)转化为像点在相机坐标系下的坐标;b.由标记点P<sub>i</sub>的像点在相机坐标系下的坐标确定出TOF相机光心到标记点P<sub>i</sub>的方向向量,获得单位方向向量<img file="FDA00001752948100016.GIF" wi="58" he="73" />c.由TOF相机测得标记点P<sub>i</sub>到像平面的垂直距离L<sub>i</sub>,并通过非迭代三维空间两点间距离方法得出TOF相机光心到各标记点P<sub>i</sub>的距离D<sub>i</sub>;d.由式(1)求出各标记点P<sub>i</sub>在相机坐标系下的坐标S<sub>i</sub>,i=1,2,3;<maths num="0002"><![CDATA[<math><mrow><msub><mi>S</mi><mi>i</mi></msub><mo>=</mo><msub><mi>D</mi><mi>i</mi></msub><msub><mover><mi>a</mi><mo>&RightArrow;</mo></mover><mi>i</mi></msub><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>1</mn><mo>)</mo></mrow></mrow></math>]]></maths>通过任意两个标记点P<sub>i</sub>的连线求出两个线性无关的单位列向量<img file="FDA00001752948100018.GIF" wi="38" he="74" />和<img file="FDA00001752948100019.GIF" wi="70" he="74" />令<img file="FDA000017529481000110.GIF" wi="52" he="73" />为<img file="FDA000017529481000111.GIF" wi="139" he="74" />所构成的单位列向量;构造出三个标记点P<sub>i</sub>在相机坐标系下的向量组:<maths num="0003"><![CDATA[<math><mrow><msub><mi>n</mi><mi>s</mi></msub><mo>=</mo><mfenced open='[' close=''><mtable><mtr><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>s</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>s</mi><mn>2</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow></math>]]></maths><maths num="0004"><![CDATA[<math><mrow><mfenced open='' close=']'><mtable><mtr><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>s</mi><mn>3</mn></mrow></msub></mtd></mtr></mtable></mfenced><mo>;</mo></mrow></math>]]></maths>3)由向量组<maths num="0005"><![CDATA[<math><mrow><msub><mi>n</mi><mi>o</mi></msub><mo>=</mo><mfenced open='[' close=']'><mtable><mtr><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>o</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>o</mi><mn>2</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>o</mi><mn>3</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow></math>]]></maths>和<maths num="0006"><![CDATA[<math><mrow><msub><mi>n</mi><mi>s</mi></msub><mo>=</mo><mfenced open='[' close=']'><mtable><mtr><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>s</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>s</mi><mn>2</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>&RightArrow;</mo></mover><mrow><mi>s</mi><mn>3</mn></mrow></msub></mtd></mtr></mtable></mfenced></mrow></math>]]></maths>通过式(2)式(3)的运算可得出旋转矩阵R和平移矩阵T;R=n<sub>s</sub>n<sub>o</sub><sup>-1</sup>         (2)T=S<sub>i</sub>-RP<sub>i</sub>         (3)4)将旋转矩阵<maths num="0007"><![CDATA[<math><mrow><mi>R</mi><mo>=</mo><mfenced open='(' close=')'><mtable><mtr><mtd><msub><mi>r</mi><mn>00</mn></msub></mtd><mtd><msub><mi>r</mi><mn>01</mn></msub></mtd><mtd><msub><mi>r</mi><mn>02</mn></msub></mtd></mtr><mtr><mtd><msub><mi>r</mi><mn>10</mn></msub></mtd><mtd><msub><mi>r</mi><mn>11</mn></msub></mtd><mtd><msub><mi>r</mi><mn>12</mn></msub></mtd></mtr><mtr><mtd><msub><mi>r</mi><mn>20</mn></msub></mtd><mtd><msub><mi>r</mi><mn>21</mn></msub></mtd><mtd><msub><mi>r</mi><mn>22</mn></msub></mtd></mtr></mtable></mfenced></mrow></math>]]></maths>通过式(4)计算获得绕相机坐标系的x轴、y轴和z轴沿逆时针方向的转角α、β和γ;旋转顺序是从x轴到y轴再到z轴;所述转角α、β和γ为目标物体相对于相机坐标系的三个姿态角;<maths num="0008"><![CDATA[<math><mrow><mfenced open='{' close=''><mtable><mtr><mtd><mi>&gamma;</mi><mo>=</mo><mo>-</mo><mi>arctan</mi><mfrac><msub><mi>r</mi><mn>01</mn></msub><msub><mi>r</mi><mn>00</mn></msub></mfrac></mtd></mtr><mtr><mtd><mi>&beta;</mi><mo>=</mo><mi>arctan</mi><mfrac><msub><mi>r</mi><mn>20</mn></msub><mrow><msub><mi>r</mi><mn>00</mn></msub><mi>cos</mi><mi>&gamma;</mi><mo>-</mo><msub><mi>r</mi><mn>10</mn></msub><mi>sin</mi><mi>&gamma;</mi></mrow></mfrac></mtd></mtr><mtr><mtd><mi>&alpha;</mi><mo>=</mo><mi>arctan</mi><mfrac><mrow><msub><mi>r</mi><mn>02</mn></msub><mi>sin</mi><mi>&gamma;</mi><mo>+</mo><msub><mi>r</mi><mn>12</mn></msub><mi>cos</mi><mi>&gamma;</mi></mrow><mrow><msub><mi>r</mi><mn>01</mn></msub><mi>sin</mi><mi>&gamma;</mi><mo>+</mo><msub><mi>r</mi><mn>11</mn></msub><mi>cos</mi><mi>&gamma;</mi></mrow></mfrac></mtd></mtr></mtable></mfenced><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>4</mn><mo>)</mo></mrow></mrow></math>]]></maths>5)将平移矩阵<maths num="0009"><![CDATA[<math><mrow><mi>T</mi><mo>=</mo><mfenced open='(' close=')'><mtable><mtr><mtd><msub><mi>t</mi><mn>0</mn></msub></mtd></mtr><mtr><mtd><msub><mi>t</mi><mn>1</mn></msub></mtd></mtr><mtr><mtd><msub><mi>t</mi><mn>2</mn></msub></mtd></mtr></mtable></mfenced></mrow></math>]]></maths>通过式(5)计算获得目标本体坐标系和相机坐标系之间的距离d,其中t<sub>0</sub>、t<sub>1</sub>和t<sub>2</sub>分别为目标物体相对于相机坐标在x轴、y轴和z轴上的平移量;<maths num="0010"><![CDATA[<math><mrow><mi>d</mi><mo>=</mo><msqrt><msubsup><mi>t</mi><mn>0</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>t</mi><mn>1</mn><mn>2</mn></msubsup><mo>+</mo><msubsup><mi>t</mi><mn>2</mn><mn>2</mn></msubsup></msqrt><mo>-</mo><mo>-</mo><mo>-</mo><mrow><mo>(</mo><mn>5</mn><mo>)</mo></mrow><mo>;</mo></mrow></math>]]></maths>所述目标本体坐标系是以目标物体的质心为坐标原点;所述像坐标系是以相机光轴与像平面的交点为坐标原点;所述相机坐标系是以摄像机光心为坐标原点。
地址 230009 安徽省合肥市屯溪路193号