一種基于多源信息融合的高精度運(yùn)動(dòng)軌跡檢測(cè)系統(tǒng)的制作方法
【技術(shù)領(lǐng)域】
[0001] 本發(fā)明涉及一種運(yùn)動(dòng)軌跡檢測(cè)系統(tǒng)，具體的涉及一種采用慣性傳感、機(jī)器視覺和 電磁定位子系統(tǒng)組成的高精度運(yùn)動(dòng)軌跡檢測(cè)系統(tǒng)，應(yīng)用領(lǐng)域包括機(jī)器人等運(yùn)動(dòng)部件的運(yùn)動(dòng) 軌跡檢測(cè)。
【背景技術(shù)】
[0002] 工業(yè)機(jī)器人和移動(dòng)機(jī)器人等運(yùn)動(dòng)部件日益廣泛應(yīng)用導(dǎo)致對(duì)其操作性能，尤其是對(duì) 運(yùn)動(dòng)執(zhí)行器的動(dòng)態(tài)定位精度提出很高的要求。例如工業(yè)機(jī)器人作為由減速器、伺服電機(jī)、增 量式編碼器和負(fù)載反饋單元實(shí)現(xiàn)半閉環(huán)的運(yùn)動(dòng)控制方式，其機(jī)械手臂結(jié)構(gòu)高度非線性，高 速末端動(dòng)態(tài)變異（偏移、抖動(dòng)）和高負(fù)載變異（末端工具置換）將影響路徑定位精度。所 以一種高精度的運(yùn)動(dòng)軌跡檢測(cè)系統(tǒng)來實(shí)現(xiàn)機(jī)器人的實(shí)時(shí)運(yùn)動(dòng)反饋與控制在裝配定位、振動(dòng) 分析以及性能指標(biāo)的測(cè)量與評(píng)價(jià)等應(yīng)用顯得非常的必要。
[0003]目前國內(nèi)外對(duì)運(yùn)動(dòng)跟蹤和定位技術(shù)的研究相對(duì)集中在射頻信號(hào)檢測(cè)定位、慣性傳 感、磁場(chǎng)定位、視覺定位和聲源定位等?；趹T性傳感技術(shù)的Xsens動(dòng)作捕捉系統(tǒng)，將加速 度計(jì)、陀螺儀和磁力計(jì)進(jìn)行信息融合，能獲得精度較高的三維姿態(tài)信息，因?yàn)榧铀俣戎刀?積分后誤差較大，獲得的線性位移只能作為參考值。電磁定位系統(tǒng)通過磁傳感器陣列對(duì)永 磁體或者電磁線圈在空間分布的三維磁場(chǎng)強(qiáng)度進(jìn)行檢測(cè)，再進(jìn)行迭代求解得到永磁體或電 磁線圈的空間位置和姿態(tài)信息，NDI公司的電磁定位系統(tǒng)就采用兩個(gè)垂直放置的3軸電磁 感應(yīng)線圈實(shí)現(xiàn)完整6軸的運(yùn)動(dòng)檢測(cè)，但是電磁定位系統(tǒng)容易受到環(huán)境電磁波及鐵磁物質(zhì)的 干擾，在工業(yè)環(huán)境下這種干擾難以避免；基于光學(xué)定位技術(shù)的VICON動(dòng)作捕捉系統(tǒng)由紅外 高速攝像機(jī)、一個(gè)數(shù)據(jù)處理器和配套的應(yīng)用軟件構(gòu)成，紅外高速攝像機(jī)捕捉被動(dòng)發(fā)光標(biāo)記 點(diǎn)，采用機(jī)器視覺原理和激光掃描技術(shù)，實(shí)現(xiàn)運(yùn)動(dòng)位置信息的測(cè)量，但光學(xué)定位系統(tǒng)只能測(cè) 量標(biāo)記點(diǎn)的空間位置信息，且容易受到遮擋以及環(huán)境光和背景的影響。
[0004] 針對(duì)光學(xué)定位系統(tǒng)容易受遮擋的問題，專利申請(qǐng)?zhí)枮?01410661823.9的中國發(fā) 明申請(qǐng)，該發(fā)明提出一種基于慣性檢測(cè)的激光跟蹤儀靶球定位系統(tǒng)，可實(shí)現(xiàn)斷光續(xù)接功能， 便于對(duì)難測(cè)點(diǎn)或遮檔位置的測(cè)量，但主要不是提升目標(biāo)定位跟蹤的精度和維度。
[0005] 因此，在復(fù)雜多變的測(cè)試環(huán)境中，使用單個(gè)定位測(cè)量系統(tǒng)會(huì)存在以下不足：1、測(cè)量 獲得的信息量單一，如光學(xué)定位系統(tǒng)只能測(cè)量到位置信息，慣性定位系統(tǒng)只能測(cè)量到姿態(tài) 信息；2、受到環(huán)境因素的干擾而導(dǎo)致定位精度不高，如電磁定位系統(tǒng)容易受電磁波的干擾， 光學(xué)定位系統(tǒng)容易受到遮擋以及環(huán)境光和背景的影響。

【發(fā)明內(nèi)容】

[0006] 為克服單個(gè)定位系統(tǒng)存在的問題，本發(fā)明的目的在于提供一種基于多源信息融合 的運(yùn)動(dòng)軌跡檢測(cè)系統(tǒng)，實(shí)現(xiàn)機(jī)器人等運(yùn)動(dòng)部件的六維位姿的高精度、高穩(wěn)定和快速運(yùn)動(dòng)檢 測(cè)系統(tǒng)。
[0007] 為解決上述技術(shù)問題，本發(fā)明采用以下技術(shù)方案：
[0008] -種基于多源信息融合的高精度運(yùn)動(dòng)軌跡檢測(cè)系統(tǒng)，包括慣性傳感定位、電磁定 位、機(jī)器視覺定位三個(gè)模塊化的子系統(tǒng)以及數(shù)據(jù)處理平臺(tái)，慣性傳感定位子系統(tǒng)、電磁定位 子系統(tǒng)和機(jī)器視覺定位子系統(tǒng)采集的數(shù)據(jù)均傳輸?shù)剿鰯?shù)據(jù)處理平臺(tái)，以實(shí)現(xiàn)信息融合定 位和協(xié)作定位；其中：
[0009] 所述慣性傳感定位子系統(tǒng)用于測(cè)量運(yùn)動(dòng)部件三維姿態(tài)角；
[0010] 所述電磁定位子系統(tǒng)用于測(cè)量運(yùn)動(dòng)部件三維位置和三維姿態(tài)角；
[0011] 所述機(jī)器視覺定位子系統(tǒng)用于測(cè)量運(yùn)動(dòng)部件三維位置信息；
[0012] 所述數(shù)據(jù)處理平臺(tái)通過將慣性傳感定位子系統(tǒng)、電磁定位子系統(tǒng)、機(jī)器視覺定位 子系統(tǒng)中獲取的多源信息進(jìn)行綜合分析和協(xié)調(diào)處理，基于分布式狀態(tài)融合結(jié)構(gòu)模型，對(duì)數(shù) 據(jù)進(jìn)行坐標(biāo)轉(zhuǎn)換和數(shù)據(jù)校正、數(shù)據(jù)關(guān)聯(lián)和狀態(tài)估計(jì)融合，綜合考慮噪聲干擾和環(huán)境因素的 影響，提升目標(biāo)動(dòng)態(tài)定位跟蹤的精度和維度，增強(qiáng)系統(tǒng)的可靠性和魯棒性。
[0013] 優(yōu)選地，所述的慣性傳感定位子系統(tǒng)包括MEMS傳感器和第一子處理器，MEMS傳感 器（包括三軸加速度計(jì)、三軸陀螺儀、三軸磁力計(jì)）貼附于運(yùn)動(dòng)部件來實(shí)現(xiàn)實(shí)時(shí)三維姿態(tài)信 息的獲取，第一子處理器采用卡爾曼濾波或正交余弦矩陣融合算法對(duì)MEMS傳感器采集的 數(shù)據(jù)進(jìn)行融合，從而獲得運(yùn)動(dòng)部件精確的三維姿態(tài)角信息。
[0014] 更優(yōu)選地，所述的MEMS傳感器包括三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)，所述 三軸陀螺儀、三軸加速度計(jì)、三軸磁力計(jì)采集的數(shù)據(jù)均傳送到第一子處理器，第一子處理器 對(duì)三者數(shù)據(jù)進(jìn)行處理，得到MEMS傳感器當(dāng)前的姿態(tài)角，即運(yùn)動(dòng)部件的姿態(tài)角；其中：
[0015] 三軸陀螺儀測(cè)得角速度、一次積分后得到運(yùn)動(dòng)部件偏轉(zhuǎn)的姿態(tài)角，但誤差會(huì)隨著 時(shí)間而累積；
[0016] 三軸磁力計(jì)用于測(cè)量地磁強(qiáng)度，進(jìn)而得到運(yùn)動(dòng)部件的航向角；
[0017] 三軸加速度計(jì)測(cè)量得到的信號(hào)中的三軸重力分量，用于測(cè)量運(yùn)動(dòng)部件的絕對(duì)俯仰 角和翻滾角（相對(duì)于地球坐標(biāo)）；
[0018] 三軸磁力計(jì)和三軸加速度計(jì)輸出的姿態(tài)角動(dòng)態(tài)性能差，用于補(bǔ)償三軸陀螺儀信號(hào) 一次積分后得到的姿態(tài)角，去除漂移。
[0019] 優(yōu)選地，所述的電磁定位子系統(tǒng)包括三軸正交的激勵(lì)線圈和三軸正交的感應(yīng)線 圈、以及第二子處理器，其中：感應(yīng)線圈固定在運(yùn)動(dòng)部件，激勵(lì)線圈則作為固定點(diǎn)；在很短 時(shí)間內(nèi)，激勵(lì)線圈交替通過相同頻率和幅度的交流電流，交變的電流信號(hào)通過激勵(lì)線圈在 空間產(chǎn)生交變的電磁場(chǎng)，感應(yīng)線圈在交變的電磁場(chǎng)中輸出頻率相同的信號(hào)；第二子處理器 根據(jù)感應(yīng)線圈輸出信號(hào)的幅值和相位信息，通過定位算法計(jì)算出感應(yīng)線圈相對(duì)于激勵(lì)線圈 的位置和方向信息。
[0020] 優(yōu)選地，所述的機(jī)器視覺定位子系統(tǒng)由若干個(gè)相機(jī)和FPGA嵌入式處理器組成，其 中：若干個(gè)相機(jī)安裝在特征點(diǎn)的周圍，用于從不同的方位實(shí)時(shí)連續(xù)采集特征點(diǎn)的圖像信號(hào) 并傳遞給FPGA嵌入式處理器；FPGA嵌入式處理器用于控制相機(jī)獲取含有標(biāo)記點(diǎn)的圖像信 號(hào)，并將圖像信號(hào)進(jìn)行處理，實(shí)現(xiàn)特征點(diǎn)圖像坐標(biāo)的獲?。凰鎏卣鼽c(diǎn)采用主動(dòng)發(fā)光或被動(dòng) 發(fā)光的標(biāo)記點(diǎn)，且貼附于運(yùn)動(dòng)部件上。
[0021] 更優(yōu)選地，所述的機(jī)器視覺定位子系統(tǒng)采用多個(gè)相機(jī)從不同的方位實(shí)時(shí)連續(xù)采集 特征點(diǎn)的圖像信號(hào)，各路圖像信號(hào)采用基于顏色空間模型進(jìn)行目標(biāo)識(shí)別，進(jìn)而找出特征點(diǎn) 在各個(gè)相機(jī)中不同時(shí)刻的成像位置的二維坐標(biāo)；將特征點(diǎn)在各個(gè)相機(jī)中不同時(shí)刻的成像位 置二維坐標(biāo)通過優(yōu)化算法進(jìn)行2D坐標(biāo)插值計(jì)算；在所有相機(jī)成像面中的多個(gè)二維平面坐 標(biāo)所構(gòu)成多條空間異面直線，通過三維坐標(biāo)定位算法計(jì)算特征點(diǎn)的空間三維坐標(biāo)。
[0022] 更優(yōu)選地，所述的優(yōu)化算法采用最小二乘法、平均法或中值法等。
[0023]優(yōu)選地，所述的數(shù)據(jù)處理平臺(tái)采用多源信息融合算法，該算法基于分布式狀態(tài)融 合結(jié)構(gòu)模型，該結(jié)構(gòu)模型的特點(diǎn)是每個(gè)子系統(tǒng)的傳感器數(shù)據(jù)在進(jìn)入數(shù)據(jù)處理平臺(tái)前，先由 各個(gè)子系統(tǒng)的數(shù)據(jù)處理器生成局部的定位跟蹤軌跡，然后把處理過的信息送至數(shù)據(jù)處理平 臺(tái)，數(shù)據(jù)處理平臺(tái)根據(jù)各子系統(tǒng)的定位跟蹤軌跡數(shù)據(jù)，進(jìn)行坐標(biāo)轉(zhuǎn)換和數(shù)據(jù)較正、數(shù)據(jù)關(guān)聯(lián) 以及狀態(tài)估計(jì)融合，最終生成具有六維位姿的目標(biāo)定位跟蹤軌跡；此外，目標(biāo)定位跟蹤軌跡 數(shù)據(jù)還反饋信息到各個(gè)子系統(tǒng)中，為各子系統(tǒng)的定位跟蹤提供參考和較準(zhǔn)。
[0024] 更優(yōu)選地，所述多源信息融合算法，包括以下3個(gè)處理過程：
[0025] 1)坐標(biāo)轉(zhuǎn)換和數(shù)據(jù)校正：統(tǒng)一各子系統(tǒng)的坐標(biāo)系，并結(jié)合反饋的位姿信息對(duì)數(shù)據(jù) 進(jìn)行較正；
[0026] 2)數(shù)據(jù)關(guān)聯(lián)：對(duì)各個(gè)子系統(tǒng)測(cè)量的數(shù)據(jù)先進(jìn)行關(guān)聯(lián)；
[0027] 3)狀態(tài)估計(jì)融合：通過測(cè)量結(jié)果估計(jì)目標(biāo)當(dāng)前的狀態(tài)向量，利用上周期外推所得 的本周期目標(biāo)的可能位姿信息，和本周期實(shí)時(shí)檢測(cè)到的目標(biāo)位姿信息，采用濾波算法實(shí)現(xiàn) 融合。
[0028] 更優(yōu)選地，所述數(shù)據(jù)關(guān)聯(lián)，采用以下任一種方法：最鄰近數(shù)據(jù)關(guān)聯(lián)、聯(lián)合概率數(shù)據(jù) 關(guān)聯(lián)、統(tǒng)計(jì)關(guān)聯(lián)或模糊關(guān)聯(lián)；
[0029] 更優(yōu)選地，所述狀態(tài)估計(jì)融合，采用以下任一種方法：卡爾曼加權(quán)融合、協(xié)方差加 權(quán)軌跡融合或模糊軌跡融合。
[0030] 與現(xiàn)有技術(shù)相比，本發(fā)明的有益效果：
[0031]本發(fā)明采用三個(gè)子系統(tǒng)的設(shè)計(jì)獲取多源信息，并進(jìn)一步在每個(gè)子系統(tǒng)中均設(shè)置相