本發(fā)明涉及信息技術(shù)處理領(lǐng)域，更具體地，涉及一種三維測(cè)量數(shù)據(jù)到機(jī)器人運(yùn)動(dòng)坐標(biāo)系的變換方法。
背景技術(shù)：
：智能機(jī)器人借助激光掃描儀等三維測(cè)量裝置，能獲取目標(biāo)位置和工作環(huán)境的3維數(shù)據(jù)，從而自動(dòng)規(guī)劃?rùn)C(jī)器人移動(dòng)路徑，規(guī)避環(huán)境中的障礙移動(dòng)到目標(biāo)位置。但實(shí)際中由于機(jī)器人運(yùn)動(dòng)坐標(biāo)值和激光掃描儀的坐標(biāo)值都是在各坐標(biāo)系下得到的并不統(tǒng)一。因此需要先將測(cè)量得到的3維數(shù)據(jù)轉(zhuǎn)換到機(jī)器人的運(yùn)動(dòng)坐標(biāo)系下，使測(cè)量數(shù)據(jù)的3維坐標(biāo)等于運(yùn)動(dòng)系統(tǒng)下的3維坐標(biāo)。目前，所有的坐標(biāo)轉(zhuǎn)換方法都是通過(guò)在兩個(gè)坐標(biāo)系中找若干個(gè)公共點(diǎn)，然后通過(guò)建立方程組求解旋轉(zhuǎn)平移參數(shù)。例如，七參數(shù)模型求解，將3個(gè)平移量、3個(gè)旋轉(zhuǎn)量和1個(gè)尺度因子作為待求解的未知量，然后通過(guò)聯(lián)立方程求解；十三參數(shù)模型求解，將3個(gè)旋轉(zhuǎn)矩陣用一個(gè)3×3矩陣表示，則有9個(gè)未知參數(shù)，加上3個(gè)平移量和1個(gè)尺度因子合計(jì)13個(gè)未知量，然后通過(guò)聯(lián)立方程求解。但將這些方法應(yīng)用在智能機(jī)器人時(shí)存在以下問(wèn)題：(1)難以尋找大量的公共點(diǎn)用于求解轉(zhuǎn)換矩陣參數(shù)；(2)實(shí)際中所有的公共點(diǎn)是存在誤差的。因此，需要一種坐標(biāo)轉(zhuǎn)換方法，只需要少量的公共點(diǎn)就能計(jì)算出坐標(biāo)轉(zhuǎn)換矩陣，同時(shí)能修正由于公共點(diǎn)誤差帶來(lái)的轉(zhuǎn)換誤差。技術(shù)實(shí)現(xiàn)要素：本發(fā)明為克服上述現(xiàn)有技術(shù)所述的公共點(diǎn)難以尋找，公共點(diǎn)存在誤差等問(wèn)題，提供一種機(jī)器人坐標(biāo)系與三維測(cè)量坐標(biāo)系的變換方法，適用于各種智能機(jī)器人運(yùn)動(dòng)坐標(biāo)系和三維測(cè)量系統(tǒng)坐標(biāo)系的坐標(biāo)變換。為解決上述技術(shù)問(wèn)題，本發(fā)明的技術(shù)方案如下：一種機(jī)器人坐標(biāo)系與三維測(cè)量坐標(biāo)系的變換方法，所述方法包括以下步驟：S1：公共點(diǎn)坐標(biāo)提?。涸跈C(jī)器人工作環(huán)境中選定4個(gè)公共點(diǎn)，然后在機(jī)器人運(yùn)動(dòng)坐標(biāo)系下和三維測(cè)量坐標(biāo)系下，分別提取這4個(gè)公共點(diǎn)的三維空間坐標(biāo)作為坐標(biāo)系變換的依據(jù)；S2：變換矩陣計(jì)算：將4個(gè)公共點(diǎn)劃分成4個(gè)特征三角形，并建立一個(gè)標(biāo)準(zhǔn)坐標(biāo)系；計(jì)算運(yùn)動(dòng)坐標(biāo)系下和掃描坐標(biāo)系下的特征三角形轉(zhuǎn)換到標(biāo)準(zhǔn)坐標(biāo)系的正變換矩陣和逆變換矩陣；根據(jù)正變換矩陣和逆變換矩陣，將4個(gè)公共點(diǎn)的掃描坐標(biāo)轉(zhuǎn)換到運(yùn)動(dòng)坐標(biāo)系下，并求取每個(gè)公共點(diǎn)轉(zhuǎn)換后的均值坐標(biāo)；最后計(jì)算公共點(diǎn)的掃描坐標(biāo)到均值坐標(biāo)的旋轉(zhuǎn)平移矩陣；S3：掃描數(shù)據(jù)坐標(biāo)系轉(zhuǎn)換：根據(jù)S2計(jì)算得到的變換矩陣，對(duì)測(cè)量系統(tǒng)得到的三維數(shù)據(jù)進(jìn)行三維旋轉(zhuǎn)和平移，使測(cè)量數(shù)據(jù)從測(cè)量系統(tǒng)的坐標(biāo)系變換到機(jī)器人運(yùn)動(dòng)系統(tǒng)的坐標(biāo)系中。在一種優(yōu)選的方案中，步驟S1中，公共點(diǎn)坐標(biāo)提取具體步驟為：S1.1：公共點(diǎn)選取：在機(jī)器人工作環(huán)境中，選取4個(gè)位置不同，且絕對(duì)位置不會(huì)發(fā)生改變的角點(diǎn)作為公共點(diǎn)。S1.2：公共點(diǎn)的運(yùn)動(dòng)坐標(biāo)提?。和ㄟ^(guò)機(jī)器人的示教器或點(diǎn)動(dòng)控制器，將機(jī)器人的末端執(zhí)行機(jī)構(gòu)依次移動(dòng)到4個(gè)公共點(diǎn)的位置，并按順序記錄4個(gè)公共點(diǎn)在運(yùn)動(dòng)坐標(biāo)系下的三維空間坐標(biāo)。S1.3：公共點(diǎn)的測(cè)量坐標(biāo)提?。簩?duì)三維測(cè)量系統(tǒng)測(cè)量得到的三維數(shù)據(jù)進(jìn)行處理，利用角點(diǎn)識(shí)別方法，得到4個(gè)公共點(diǎn)在測(cè)量系統(tǒng)下的三維空間坐標(biāo)。在一種優(yōu)選的方案中，步驟S2中，變換矩陣具體計(jì)算步驟為：S2.1：特征三角形劃分：對(duì)4個(gè)公共點(diǎn)進(jìn)行編號(hào)(編號(hào)為1、2、3、4)；然后抽取一個(gè)點(diǎn)作為特征三角形的頂點(diǎn)，選取另一個(gè)點(diǎn)與頂點(diǎn)連線形成參考邊，最后再選取一個(gè)點(diǎn)與另外兩點(diǎn)連線形成一個(gè)三角形；S2.2：標(biāo)準(zhǔn)坐標(biāo)系建立：以特征三角形的頂點(diǎn)作為坐標(biāo)系的原點(diǎn)，以特征三角形參考邊的方向作為X軸方向，以特征三角形平面內(nèi)垂直于參考邊的方向作為Y軸方向，以垂直于三角形平面的方向?yàn)閆軸方向建立一個(gè)標(biāo)準(zhǔn)坐標(biāo)系；S2.3：標(biāo)準(zhǔn)坐標(biāo)系變換矩陣計(jì)算：計(jì)算機(jī)器人運(yùn)動(dòng)坐標(biāo)系和測(cè)量坐標(biāo)系的特征三角形到標(biāo)準(zhǔn)坐標(biāo)系的正變換矩陣和逆變換矩陣。令特征三角形三個(gè)點(diǎn)的坐標(biāo)為：(x1,y1,z1)，(x2,y2,z2)，(x3,y3,z3)，則正變換矩陣為：xi′yi′zi′=xi-x1yi-y1zi-z1Tcosγ1sinγ10-sinγ1cosγ10001cosβ10-sinβ1010sinβ10cosβ11000cosα1sinα10-sinα1cosα1]]>γ1=-arctan(y2-y1x2-x1)β1=-arctan(z2-z1x2-x1)α1=-arctan(z3-z1y3-y1)]]>式中，i＝1、2、3。逆變換矩陣為：xiyizi=xi′yi′zi′T1000cosα2sinα20-sinα2cosα2cosβ20-sinβ2010sinβ20cosβ2cosγ2sinγ20-sinγ2cosγ20001+x1y1z1]]>α2=arctan(z3-z1y3-y1)β2=arctan(z2-z1x2-x1)γ2=arctan(y2-y1x2-x1)]]>S2.4：公共點(diǎn)坐標(biāo)變換：將S2.3得到的測(cè)量坐標(biāo)系到標(biāo)準(zhǔn)坐標(biāo)系的4個(gè)正變換矩陣和機(jī)器人坐標(biāo)系到標(biāo)準(zhǔn)坐標(biāo)系的4個(gè)逆變換矩陣，根據(jù)特征三角形的對(duì)應(yīng)關(guān)系一一對(duì)應(yīng)后形成4對(duì)變換矩陣。然后，依次對(duì)測(cè)量系統(tǒng)下公共點(diǎn)的坐標(biāo)乘上測(cè)量系統(tǒng)坐標(biāo)系到標(biāo)準(zhǔn)坐標(biāo)系的正變換矩陣，再乘上對(duì)應(yīng)的逆變換矩陣；S2.5：均值化處理：將S2.4得到的4種變換結(jié)果的坐標(biāo)值進(jìn)行均值計(jì)算，從而降低公共點(diǎn)坐標(biāo)誤差導(dǎo)致的坐標(biāo)轉(zhuǎn)換誤差；S2.6：變換矩陣計(jì)算：任意提取3個(gè)均值化處理后的公共點(diǎn)坐標(biāo)，形成一個(gè)特征三角形，計(jì)算該特征三角形到標(biāo)準(zhǔn)坐標(biāo)系的逆變換矩陣。然后在計(jì)算掃描坐標(biāo)系下對(duì)應(yīng)特征三角形到標(biāo)準(zhǔn)坐標(biāo)系的正變換矩陣。將上述正變換矩陣和逆變換矩陣作為坐標(biāo)變換矩陣組；然后將測(cè)量數(shù)據(jù)先乘上正變換矩陣，再乘上逆變換矩陣，實(shí)現(xiàn)測(cè)量坐標(biāo)系到運(yùn)動(dòng)坐標(biāo)系的變換。與現(xiàn)有技術(shù)相比，本發(fā)明技術(shù)方案的有益效果是：(1)本發(fā)明中需通過(guò)4個(gè)公共點(diǎn)，且公共點(diǎn)的確定方式以及坐標(biāo)提取方式簡(jiǎn)單可行；(2)本發(fā)明中變換矩陣只需通過(guò)計(jì)算簡(jiǎn)單的三角函數(shù)即可得到，無(wú)需復(fù)雜的迭代計(jì)算；(3)本發(fā)明通過(guò)均值化處理方式，在一定程度上修正了公共點(diǎn)誤差帶來(lái)的變換誤差。附圖說(shuō)明圖1為本發(fā)明機(jī)器人坐標(biāo)系與三維測(cè)量坐標(biāo)系變換方法的流程圖。圖2為特征三角形到標(biāo)準(zhǔn)坐標(biāo)系的變換示意圖。圖3為掃描數(shù)據(jù)到機(jī)器人坐標(biāo)系的變換示意圖。具體實(shí)施方式附圖僅用于示例性說(shuō)明，不能理解為對(duì)本專(zhuān)利的限制；為了更好說(shuō)明本實(shí)施例，附圖某些部件會(huì)有省略、放大或縮小，并不代表實(shí)際產(chǎn)品的尺寸；對(duì)于本領(lǐng)域技術(shù)人員來(lái)說(shuō)，附圖中某些公知結(jié)構(gòu)及其說(shuō)明可能省略是可以理解的。下面結(jié)合附圖和實(shí)施例對(duì)本發(fā)明的技術(shù)方案做進(jìn)一步的說(shuō)明。實(shí)施例1以水火彎板機(jī)器人運(yùn)動(dòng)坐標(biāo)系和三維激光掃描儀坐標(biāo)系的坐標(biāo)變換為例，如圖1所示，本發(fā)明方法包括以下步驟：S1：公共點(diǎn)坐標(biāo)提?。涸跈C(jī)器人工作環(huán)境中選定4個(gè)公共點(diǎn)，然后在機(jī)器人運(yùn)動(dòng)坐標(biāo)系下和三維測(cè)量坐標(biāo)系下，分別提取這4個(gè)公共點(diǎn)的三維空間坐標(biāo)作為坐標(biāo)系變換的依據(jù)。在實(shí)例1中，公共點(diǎn)坐標(biāo)提取具體步驟為：S1.1：公共點(diǎn)選?。哼x取機(jī)器人工作平臺(tái)的4個(gè)角點(diǎn)作為公共點(diǎn)。S1.2：公共點(diǎn)的運(yùn)動(dòng)坐標(biāo)提?。和ㄟ^(guò)水火彎板機(jī)器人運(yùn)動(dòng)控制系統(tǒng)的點(diǎn)動(dòng)控制按鈕將機(jī)器人末端的火槍頭移動(dòng)到公共點(diǎn)的位置，并按順序記錄4個(gè)公共點(diǎn)在運(yùn)動(dòng)坐標(biāo)系下的三維空間坐標(biāo)。S1.3：公共點(diǎn)的測(cè)量坐標(biāo)提取：對(duì)掃描儀測(cè)量得到的點(diǎn)云數(shù)據(jù)進(jìn)行處理，提取工作平臺(tái)4個(gè)角點(diǎn)的三維空間坐標(biāo)。S2：變換矩陣計(jì)算：將4個(gè)公共點(diǎn)劃分成4個(gè)特征三角形，并建立一個(gè)標(biāo)準(zhǔn)坐標(biāo)系；計(jì)算運(yùn)動(dòng)坐標(biāo)系下和掃描坐標(biāo)系下的特征三角形轉(zhuǎn)換到標(biāo)準(zhǔn)坐標(biāo)系的正變換矩陣和逆變換矩陣；根據(jù)正變換矩陣和逆變換矩陣，將4個(gè)公共點(diǎn)的掃描坐標(biāo)轉(zhuǎn)換到運(yùn)動(dòng)坐標(biāo)系下，并求取每個(gè)公共點(diǎn)轉(zhuǎn)換后的均值坐標(biāo)；最后計(jì)算公共點(diǎn)的掃描坐標(biāo)到均值坐標(biāo)的旋轉(zhuǎn)平移矩陣。具體計(jì)算步驟為：S2.1：特征三角形劃分：對(duì)4個(gè)公共點(diǎn)進(jìn)行編號(hào)(編號(hào)為1、2、3、4)；然后抽取一個(gè)點(diǎn)作為特征三角形的頂點(diǎn)，選取另一個(gè)點(diǎn)與頂點(diǎn)連線形成參考邊，最后再選取一個(gè)點(diǎn)與另外兩點(diǎn)連線形成一個(gè)三角形；S2.2：標(biāo)準(zhǔn)坐標(biāo)系建立：以特征三角形的頂點(diǎn)作為坐標(biāo)系的原點(diǎn)，以特征三角形參考邊的方向作為X軸方向，以特征三角形平面內(nèi)垂直于參考邊的方向作為Y軸方向，以垂直于三角形平面的方向?yàn)閆軸方向建立一個(gè)標(biāo)準(zhǔn)坐標(biāo)系；S2.3：標(biāo)準(zhǔn)坐標(biāo)系變換矩陣計(jì)算：計(jì)算機(jī)器人運(yùn)動(dòng)坐標(biāo)系和測(cè)量坐標(biāo)系的特征三角形到標(biāo)準(zhǔn)坐標(biāo)系的正變換矩陣和逆變換矩陣。令特征三角形三個(gè)點(diǎn)的坐標(biāo)為：(x1,y1,z1)，(x2,y2,z2)，(x3,y3,z3)，則正變換矩陣為：xi′yi′zi′=xi-x1yi-y1zi-z1Tcosγ1sinγ10-sinγ1cosγ10001cosβ10-sinβ1010sinβ10cosβ11000cosα1sinα10-sinα1cosα1]]>γ1=-arctan(y2-y1x2-x1)β1=-arctan(z2-z1x2-x1)α1=-arctan(z3-z1y3-y1)]]>式中，i＝1、2、3。逆變換矩陣為：xiyizi=xi′yi′zi′T1000cosα2sinα20-sinα2cosα2cosβ20-sinβ2010sinβ20cosβ2cosγ2sinγ20-sinγ2cosγ20001+x1y1z1]]>α2=arctan(z3-z1y3-y1)β2=arctan(z2-z1x2-x1)γ2=arctan(y2-y1x2-x1)]]>具體的變換結(jié)果如圖2所示。S2.4：公共點(diǎn)坐標(biāo)變換：將S2.3得到的測(cè)量坐標(biāo)系到標(biāo)準(zhǔn)坐標(biāo)系的4個(gè)正變換矩陣和機(jī)器人坐標(biāo)系到標(biāo)準(zhǔn)坐標(biāo)系的4個(gè)逆變換矩陣，根據(jù)特征三角形的對(duì)應(yīng)關(guān)系一一對(duì)應(yīng)后形成4對(duì)變換矩陣。然后，依次對(duì)測(cè)量系統(tǒng)下公共點(diǎn)的坐標(biāo)乘上測(cè)量系統(tǒng)坐標(biāo)系到標(biāo)準(zhǔn)坐標(biāo)系的正變換矩陣，再乘上對(duì)應(yīng)的逆變換矩陣；S2.5：均值化處理：將S2.4得到的4種變換結(jié)果的坐標(biāo)值進(jìn)行均值計(jì)算，從而降低公共點(diǎn)坐標(biāo)誤差導(dǎo)致的坐標(biāo)轉(zhuǎn)換誤差；S2.6：變換矩陣計(jì)算：任意提取3個(gè)均值化處理后的公共點(diǎn)坐標(biāo)，形成一個(gè)特征三角形，計(jì)算該特征三角形到標(biāo)準(zhǔn)坐標(biāo)系的逆變換矩陣。然后在計(jì)算掃描坐標(biāo)系下對(duì)應(yīng)特征三角形到標(biāo)準(zhǔn)坐標(biāo)系的正變換矩陣。將上述正變換矩陣和逆變換矩陣作為坐標(biāo)變換矩陣組。S3：掃描數(shù)據(jù)坐標(biāo)系轉(zhuǎn)換：根據(jù)S2計(jì)算得到的變換矩陣，對(duì)測(cè)量系統(tǒng)得到的三維數(shù)據(jù)進(jìn)行三維旋轉(zhuǎn)和平移，首先將測(cè)量數(shù)據(jù)先乘上坐標(biāo)變換矩陣組的正變換矩陣，然后再乘上坐標(biāo)變換矩陣組的逆變換矩陣，實(shí)現(xiàn)測(cè)量坐標(biāo)系到運(yùn)動(dòng)坐標(biāo)系的變換。使測(cè)量數(shù)據(jù)從測(cè)量系統(tǒng)的坐標(biāo)系變換到機(jī)器人運(yùn)動(dòng)系統(tǒng)的坐標(biāo)系中。具體的變換過(guò)程如圖3所示。附圖中描述位置關(guān)系的用語(yǔ)僅用于示例性說(shuō)明，不能理解為對(duì)本專(zhuān)利的限制；顯然，本發(fā)明的上述實(shí)施例僅僅是為清楚地說(shuō)明本發(fā)明所作的舉例，而并非是對(duì)本發(fā)明的實(shí)施方式的限定。對(duì)于所屬領(lǐng)域的普通技術(shù)人員來(lái)說(shuō)，在上述說(shuō)明的基礎(chǔ)上還可以做出其它不同形式的變化或變動(dòng)。這里無(wú)需也無(wú)法對(duì)所有的實(shí)施方式予以窮舉。凡在本發(fā)明的精神和原則之內(nèi)所作的任何修改、等同替換和改進(jìn)等，均應(yīng)包含在本發(fā)明權(quán)利要求的保護(hù)范圍之內(nèi)。當(dāng)前第1頁(yè)1 2 3