三維姿態(tài)角信息。
[0036]在本實(shí)施例中，所述慣性傳感定位子系統(tǒng)2固定在機(jī)械手臂法蘭盤(運(yùn)動(dòng)部件1)末端，第一子處理器對(duì)MEMS傳感器21數(shù)據(jù)進(jìn)行處理，以獲得機(jī)械手臂法蘭盤(運(yùn)動(dòng)部件1)末端精確的三維姿態(tài)角信息。第一子處理器的具體數(shù)據(jù)處理采用現(xiàn)有技術(shù)，比如卡爾曼濾波或正交余弦矩陣融合算法等，這些不屬于本實(shí)用新型的創(chuàng)新，本實(shí)用新型創(chuàng)新在于提供了裝置整體結(jié)構(gòu)。
[0037]進(jìn)一步的，所述MEMS傳感器21包括一個(gè)三軸陀螺儀、一個(gè)三軸加速度計(jì)和一個(gè)三軸磁力計(jì)，所述三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)均連接到第一子處理器。三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)采集的數(shù)據(jù)均傳送到第一子處理器，第一子處理器對(duì)三者數(shù)據(jù)進(jìn)行處理，得到MEMS傳感器21當(dāng)前的姿態(tài)角，由于MEMS傳感器21貼附于運(yùn)動(dòng)部件1，因此也是運(yùn)動(dòng)部件1的姿態(tài)角。
[0038]優(yōu)選的，三軸陀螺儀測得角加速度、一次積分后得到物體偏轉(zhuǎn)的姿態(tài)角；三軸磁力計(jì)測量地磁強(qiáng)度，得到物體的航向角；三軸加速度計(jì)測得三軸重力分量，用于測量絕對(duì)俯仰角和翻滾角(相對(duì)于地球坐標(biāo))；三軸磁力計(jì)和三軸加速度計(jì)輸出的姿態(tài)角動(dòng)態(tài)性能差，用于補(bǔ)償三軸陀螺儀信號(hào)一次積分后得到的姿態(tài)角，去除漂移。
[0039]所述第一子處理器可以采用邏輯電路、集成電路、單片機(jī)等實(shí)現(xiàn)，其具體處理技術(shù)可以采用現(xiàn)有技術(shù)實(shí)現(xiàn)。
[0040]如圖3所示，在另一優(yōu)選實(shí)施例中，所述電磁定位子系統(tǒng)由三軸正交的激勵(lì)線圈32和三軸正交的感應(yīng)線圈31、以及第二子處理器組成，其中:感應(yīng)線圈31固定在機(jī)械手臂法蘭盤(運(yùn)動(dòng)部件1)末端，激勵(lì)線圈32則作為固定點(diǎn)。在短時(shí)間內(nèi)，激勵(lì)線圈32交替通過相同頻率和幅度的交流電流，使激勵(lì)線圈32在空間中產(chǎn)生交變的電磁場，感應(yīng)線圈31在交變的電磁場中輸出頻率相同的信號(hào)；以及第二子處理器根據(jù)感應(yīng)線圈31輸出信號(hào)的幅值和相位信息，處理得到感應(yīng)線圈31相對(duì)于激勵(lì)線圈32的位置和方向信息。
[0041 ] 所述第二子處理器可以采用邏輯電路、集成電路、單片機(jī)等實(shí)現(xiàn)，其具體處理技術(shù)可以采用現(xiàn)有技術(shù)實(shí)現(xiàn)，當(dāng)然在一優(yōu)選實(shí)施例中也可以采用以下技術(shù):
[0042]假設(shè)所述激勵(lì)線圈32的中心位置為(a，b，c)，所述感應(yīng)線圈31的中心位置為(x，y，z)且其相對(duì)激勵(lì)線圈32的方向用三個(gè)旋轉(zhuǎn)角(α，β, γ)表示，電勢幅值ΕΜ是相對(duì)位置參數(shù)(x-a, y-b, z_c)和角度參數(shù)(α，β，γ )的函數(shù)，即EM =f (x-a, y-b, z-c, α, β , γ)。因此采用六種或以上獨(dú)立的激勵(lì)線圈32和感應(yīng)線圈31的組合關(guān)系，對(duì)不同激勵(lì)線圈32下的感應(yīng)線圈31電勢信號(hào)進(jìn)行采樣，通過定位算法就能夠計(jì)算出六個(gè)位置參數(shù)(x-a, y-b, z-c)和角度參數(shù)(α，β，γ )的值。
[0043]如圖4所示，在另一優(yōu)選實(shí)施例中，所述機(jī)器視覺定位子系統(tǒng)包括若干個(gè)相機(jī)41和FPGA嵌入式處理器43，其中:若干個(gè)相機(jī)41安裝在特征點(diǎn)42的周圍，相機(jī)41采集的圖像數(shù)據(jù)傳到FPGA嵌入式處理器中去處理；特征點(diǎn)42可采用主動(dòng)發(fā)光或被動(dòng)發(fā)光的標(biāo)記點(diǎn)，且貼附于運(yùn)動(dòng)部件上。FPGA嵌入式處理器43用于控制相機(jī)獲取含有標(biāo)記點(diǎn)的圖像信號(hào)，并將圖像信號(hào)傳入內(nèi)嵌DSP builder模塊進(jìn)行處理，實(shí)現(xiàn)特征點(diǎn)圖像坐標(biāo)的獲取。
[0044]所述FPGA嵌入式處理器43，其具體處理技術(shù)可以采用現(xiàn)有技術(shù)實(shí)現(xiàn)，當(dāng)然在一優(yōu)選實(shí)施例中也可以采用以下技術(shù):首先多個(gè)相機(jī)41從不同的方位實(shí)時(shí)連續(xù)采集特征點(diǎn)42的圖像信號(hào)；各路圖像信號(hào)采用基于顏色空間模型進(jìn)行目標(biāo)識(shí)別，即先多次拍攝特征點(diǎn)取平均值，提取出特征點(diǎn)42的顏色分量模型[R，G, B]，再和含有該特征點(diǎn)42的待識(shí)別圖像的每個(gè)顏色分量進(jìn)行對(duì)比，進(jìn)而找出特征點(diǎn)42在該幅圖像中的二維坐標(biāo)；將特征點(diǎn)42在各個(gè)相機(jī)41中不同時(shí)刻的成像位置二維坐標(biāo)通過優(yōu)化算法進(jìn)行2D坐標(biāo)插值計(jì)算，優(yōu)化算法可以采用最小二乘法、平均法或中值法；在所有相機(jī)41成像面中的多個(gè)二維平面坐標(biāo)所構(gòu)成的多條空間異面直線，通過三維坐標(biāo)定位算法計(jì)算特征點(diǎn)42的空間三維坐標(biāo)。
[0045]上述的各子系統(tǒng)的數(shù)據(jù)傳到用于數(shù)據(jù)融合的處理器5，所述第二子處理器可以采用現(xiàn)有產(chǎn)品實(shí)現(xiàn)，比如數(shù)據(jù)融合處理器，單片機(jī)等，其具體數(shù)據(jù)融合技術(shù)可以采用現(xiàn)有技術(shù)實(shí)現(xiàn)，當(dāng)然在一優(yōu)選實(shí)施例中也可以采用以下技術(shù):用于數(shù)據(jù)融合的處理器5采用分布式狀態(tài)融合結(jié)構(gòu)模型，該結(jié)構(gòu)模型的特點(diǎn)是每個(gè)子系統(tǒng)的傳感器數(shù)據(jù)在進(jìn)入用于數(shù)據(jù)融合的處理器5前，先由自己的數(shù)據(jù)處理器(即第一子處理器、第二子處理器、FPGA嵌入式處理器)生成局部的定位跟蹤軌跡，然后把處理過的信息送至用于數(shù)據(jù)融合的處理器5，用于數(shù)據(jù)融合的處理器5根據(jù)各子系統(tǒng)的定位跟蹤軌跡數(shù)據(jù)，進(jìn)行坐標(biāo)轉(zhuǎn)換和數(shù)據(jù)較正、數(shù)據(jù)關(guān)聯(lián)以及狀態(tài)估計(jì)融合，最終生成具有6維位姿的目標(biāo)定位跟蹤軌跡。此外，目標(biāo)定位跟蹤軌跡數(shù)據(jù)還反饋信息到各個(gè)子系統(tǒng)中，為各子系統(tǒng)的定位跟蹤提供參考和較準(zhǔn)。用于數(shù)據(jù)融合的處理器5能夠有效利用多重定位系統(tǒng)的定位冗余信息和互補(bǔ)信息，綜合考慮噪聲干擾和環(huán)境因素的影響，能提升目標(biāo)定位跟蹤的精度和維度，還能增強(qiáng)系統(tǒng)的可靠性和魯棒性。
[0046]本實(shí)用新型裝置設(shè)置三個(gè)定位子系統(tǒng)，通過這三個(gè)定位子系統(tǒng)獲取的多源信息，后續(xù)再進(jìn)行處理，能實(shí)現(xiàn)工業(yè)機(jī)器人等運(yùn)動(dòng)部件的三維位置和三維姿態(tài)角的高精度、高穩(wěn)定和快速運(yùn)動(dòng)檢測。本實(shí)用新型提供了一種結(jié)構(gòu)合理的檢測裝置，其中各個(gè)部分所采用的數(shù)據(jù)處理技術(shù)不屬于本實(shí)用新型要求保護(hù)的內(nèi)容。
[0047]以上對(duì)本實(shí)用新型的具體實(shí)施例進(jìn)行了描述。需要理解的是，本實(shí)用新型并不局限于上述特定實(shí)施方式，本領(lǐng)域技術(shù)人員可以在權(quán)利要求的范圍內(nèi)做出各種變形或修改，這并不影響本實(shí)用新型的實(shí)質(zhì)內(nèi)容。
【主權(quán)項(xiàng)】
1.一種高精度運(yùn)動(dòng)軌跡檢測裝置，其特征在于，包括慣性傳感定位子系統(tǒng)、電磁定位子系統(tǒng)、機(jī)器視覺定位子系統(tǒng)以及用于數(shù)據(jù)融合的處理器，其中: 所述慣性傳感定位子系統(tǒng)，其輸入端測量運(yùn)動(dòng)部件三維姿態(tài)角，輸出端連接到所述用于數(shù)據(jù)融合的處理器； 所述電磁定位子系統(tǒng)，其輸入端測量運(yùn)動(dòng)部件三維位置和三維姿態(tài)角，輸出端連接到所述用于數(shù)據(jù)融合的處理器； 所述機(jī)器視覺定位子系統(tǒng)，其輸入端測量運(yùn)動(dòng)部件三維位置信息，輸出端連接到所述用于數(shù)據(jù)融合的處理器； 所述用于數(shù)據(jù)融合的處理器同時(shí)連接慣性傳感定位子系統(tǒng)、電磁定位子系統(tǒng)、機(jī)器視覺定位子系統(tǒng)的輸出端，并將三個(gè)子系統(tǒng)的數(shù)據(jù)進(jìn)行融合，得到運(yùn)動(dòng)部件的運(yùn)動(dòng)軌跡。2.根據(jù)權(quán)利要求1所述的一種高精度運(yùn)動(dòng)軌跡檢測裝置，其特征在于，所述的慣性傳感定位子系統(tǒng)包括MEMS傳感器和第一子處理器，MEMS傳感器貼附于運(yùn)動(dòng)部件并實(shí)現(xiàn)實(shí)時(shí)三維姿態(tài)信息的獲取，所述MEMS傳感器的輸出端連接到第一子處理器，第一子處理器對(duì)MEMS傳感器采集的數(shù)據(jù)進(jìn)行融合，獲得運(yùn)動(dòng)部件精確的三維姿態(tài)角信息。3.根據(jù)權(quán)利要求2所述的一種高精度運(yùn)動(dòng)軌跡檢測裝置，其特征在于，所述的MEMS傳感器包括三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)，所述三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)均連接到第一子處理器。4.根據(jù)權(quán)利要求1-3任一項(xiàng)所述的一種高精度運(yùn)動(dòng)軌跡檢測裝置，其特征在于:所述的電磁定位子系統(tǒng)包括三軸正交的激勵(lì)線圈和三軸正交的感應(yīng)線圈、以及第二子處理器，其中:感應(yīng)線圈固定在運(yùn)動(dòng)部件上，激勵(lì)線圈則作為固定點(diǎn)；激勵(lì)線圈交替通過相同頻率和幅度的交流電流，交變的電流信號(hào)通過激勵(lì)線圈在空間產(chǎn)生交變的電磁場，感應(yīng)線圈在交變的電磁場中輸出頻率相同的信號(hào)；第二子處理器根據(jù)感應(yīng)線圈輸出信號(hào)的幅值和相位信息，得到感應(yīng)線圈相對(duì)于激勵(lì)線圈的位置和方向信息。5.根據(jù)權(quán)利要求1-3任一項(xiàng)所述的一種高精度運(yùn)動(dòng)軌跡檢測裝置，其特征在于，所述的機(jī)器視覺定位子系統(tǒng)由若干個(gè)相機(jī)和FPGA嵌入式處理器組成，其中:若干個(gè)相機(jī)安裝在特征點(diǎn)的周圍，用于從不同的方位實(shí)時(shí)連續(xù)采集特征點(diǎn)的圖像信號(hào)并輸出給FPGA嵌入式處理器；所述特征點(diǎn)采用主動(dòng)發(fā)光或被動(dòng)發(fā)光的標(biāo)記點(diǎn)，且貼附于運(yùn)動(dòng)部件上；FPGA嵌入式處理器用于控制相機(jī)獲取含有標(biāo)記點(diǎn)的圖像信號(hào)，并將圖像信號(hào)進(jìn)行處理，實(shí)現(xiàn)特征點(diǎn)圖像坐標(biāo)的獲取。
【專利摘要】本實(shí)用新型提供一種高精度運(yùn)動(dòng)軌跡檢測裝置，其中：慣性傳感定位子系統(tǒng)，測量運(yùn)動(dòng)部件三維姿態(tài)角；電磁定位子系統(tǒng)，測量運(yùn)動(dòng)部件三維位置和三維姿態(tài)角；機(jī)器視覺定位子系統(tǒng)，測量運(yùn)動(dòng)部件三維位置信息；用于數(shù)據(jù)融合的處理器同時(shí)連接慣性傳感定位子系統(tǒng)、電磁定位子系統(tǒng)、機(jī)器視覺定位子系統(tǒng)的輸出端，并將三個(gè)子系統(tǒng)的數(shù)據(jù)進(jìn)行融合，得到運(yùn)動(dòng)部件的運(yùn)動(dòng)軌跡。本實(shí)用新型克服單個(gè)系統(tǒng)獲得的定位信息量單一以及容易受環(huán)境因素干擾的問題。
【IPC分類】G01C21/00, G01C21/16
【公開號(hào)】CN205066775
【申請(qǐng)?zhí)枴緾N201520721883
【發(fā)明人】戴厚德, 蘇詩薦, 夏許可, 林志榕, 曾雅丹, 吳周新
【申請(qǐng)人】泉州裝備制造研究所
【公開日】2016年3月2日
【申請(qǐng)日】2015年9月17日