主权项 |
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>→</mo></mover><mrow><mi>o</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>→</mo></mover><mrow><mi>o</mi><mn>2</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>→</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>→</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>→</mo></mover><mrow><mi>s</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>→</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>→</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>→</mo></mover><mrow><mi>o</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>→</mo></mover><mrow><mi>o</mi><mn>2</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>→</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>→</mo></mover><mrow><mi>s</mi><mn>1</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>→</mo></mover><mrow><mi>s</mi><mn>2</mn></mrow></msub></mtd><mtd><msub><mover><mi>n</mi><mo>→</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>γ</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>β</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>γ</mi><mo>-</mo><msub><mi>r</mi><mn>10</mn></msub><mi>sin</mi><mi>γ</mi></mrow></mfrac></mtd></mtr><mtr><mtd><mi>α</mi><mo>=</mo><mi>arctan</mi><mfrac><mrow><msub><mi>r</mi><mn>02</mn></msub><mi>sin</mi><mi>γ</mi><mo>+</mo><msub><mi>r</mi><mn>12</mn></msub><mi>cos</mi><mi>γ</mi></mrow><mrow><msub><mi>r</mi><mn>01</mn></msub><mi>sin</mi><mi>γ</mi><mo>+</mo><msub><mi>r</mi><mn>11</mn></msub><mi>cos</mi><mi>γ</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>所述目标本体坐标系是以目标物体的质心为坐标原点;所述像坐标系是以相机光轴与像平面的交点为坐标原点;所述相机坐标系是以摄像机光心为坐标原点。 |