सेंसर से रैखिक और कोणीय डेटा को कैसे फ्यूज करें

26

मेरी टीम और मैं एक आउटडोर रोबोट स्थापित कर रहे हैं जिसमें एनकोडर, एक वाणिज्यिक-ग्रेड आईएमयू और जीपीएस सेंसर हैं। रोबोट में एक बुनियादी टैंक ड्राइव है, इसलिए एन्कोडर्स बाएं और दाएं पहियों से पर्याप्त रूप से टिक्स की आपूर्ति करते हैं। IMU x, y और z में रोल, पिच, यॉ और रैखिक त्वरण देता है। हम बाद में अन्य IMU जोड़ सकते हैं, जो अतिरेक देगा, लेकिन इसके अलावा रोल, पिच, और यव के कोणीय दर भी प्रदान कर सकता है। GPS वैश्विक x, y और z निर्देशांक प्रकाशित करता है।

रोबोट की एक्स पोजीशन और हेडिंग को जानना रोबोट के लिए उपयोगी होगा और नेविगेट करने के लिए पर्यावरण को मैप कर सकता है। रोबोट का वेग भी सुचारू गति के निर्णय लेने के लिए उपयोगी हो सकता है। यह एक ग्राउंड-आधारित रोबोट है, इसलिए हम z अक्ष के बारे में बहुत ज्यादा परवाह नहीं करते हैं। रोबोट में लिडार सेंसर और कैमरा भी होता है - इसलिए बेहतर ओरिएंटेशन के लिए लिडार और कैमरा डेटा को बदलने के लिए रोल और पिच उपयोगी होगी।

मैं यह पता लगाने की कोशिश कर रहा हूं कि इन सभी नंबरों को एक साथ कैसे फ्यूज करना है जो सभी सेंसर की सटीकता का लाभ उठाते हैं। अभी हम सरल संक्रमण मैट्रिक्स के साथ का अनुमान उत्पन्न करने के लिए एक कलमन फ़िल्टर का उपयोग कर रहे हैं [x, x-vel, x-accel, y, y-vel, y-accel]:

[[1, dt, .5*dt*dt, 0,  0,        0],
 [0,  1,       dt, 0,  0,        0],
 [0,  0,        1, 0,  0,        0],
 [0,  0,        0, 1, dt, .5*dt*dt],
 [0,  0,        0, 0,  1,       dt],
 [0,  0,        0, 0,  0,        1]]

IMU द्वारा प्रदान किए गए त्वरण के आधार पर फ़िल्टर विशेष रूप से राज्य का अनुमान लगाता है। (IMU सबसे अच्छी गुणवत्ता नहीं है, लगभग 30 सेकंड के भीतर यह रोबोट को दिखाएगा (आराम से) अपने प्रारंभिक स्थान पर एक अच्छा 20 मीटर बहने वाला।) मैं यह जानना चाहता हूं कि रोल, पिच और यव का उपयोग कैसे करें। आईएमयू, और संभावित रोल, पिच, और यव दरों, पहियों से एनकोडर डेटा, और राज्य अनुमान में सुधार करने के लिए जीपीएस डेटा।

गणित के एक बिट का उपयोग करते हुए, हम दो एन्कोडर्स का उपयोग करके एक्स, वाई और रोबोट पर शीर्षक की जानकारी, साथ ही रैखिक और कोणीय वेग उत्पन्न कर सकते हैं। एन्कोडर बहुत सटीक हैं, लेकिन वे एक बाहरी क्षेत्र पर फिसलन के लिए अतिसंवेदनशील हो सकते हैं।

मुझे ऐसा लगता है कि यहां डेटा के दो अलग-अलग सेट हैं, जिन्हें फ्यूज करना मुश्किल है:

एक्स, एक्स-वेल, एक्स-एक्सिलेंस, वाई, वाई-वेल, वाई-एक्सिलिट का अनुमान है
रोल, पिच, yaw और रोल, पिच और yaw की दरों का अनुमान

भले ही इन दो सेटों के बीच क्रॉसओवर हो, लेकिन मुझे यह परेशानी हो रही है कि उन्हें एक साथ कैसे रखा जाए। उदाहरण के लिए, यदि रोबोट निरंतर गति से चल रहा है, तो उसके x-vel और y-vel द्वारा निर्धारित रोबोट की दिशा, उसके yaw जैसी ही होगी। हालांकि, अगर रोबोट आराम पर है, तो y को x और y वेग द्वारा सटीक रूप से निर्धारित नहीं किया जा सकता है। इसके अलावा, एनकोडर द्वारा प्रदान किए गए डेटा, कोणीय वेग का अनुवाद, yaw दर के लिए एक अद्यतन हो सकता है ... लेकिन कैसे yaw दर के लिए एक अद्यतन बेहतर स्थितिगत अनुमान प्रदान कर सकता है?

क्या सभी 12 नंबरों को एक ही फिल्टर में डालने का कोई मतलब है, या उन्हें सामान्य रूप से अलग रखा जाता है? क्या इस प्रकार की समस्या से निपटने का पहले से ही एक विकसित तरीका है?

sensors kalman-filter sensor-fusion

— Robz
स्रोत

32

दो चीज़ें।

यदि आप मैपिंग करने की योजना बनाते हैं, तो आपको एक पूर्ण स्थानीयकरण और मैपिंग (SLAM) एल्गोरिथम की आवश्यकता है। देखें: एक साथ स्थानीयकरण और मानचित्रण (SLAM): भाग I आवश्यक एल्गोरिथ्म । SLAM में, रोबोट स्थिति का अनुमान केवल आधी समस्या है। कैसे किया जाए कि एक बड़ा सवाल है जिसका उत्तर यहां दिया जा सकता है।
स्थानीयकरण (रोबोट की स्थिति का आकलन) के बारे में, यह एक कलमन फ़िल्टर के लिए कोई काम नहीं है। से संक्रमण के लिए $x(t)=[x,y,\dot{x},\dot{y},\theta,\dot{\theta}]$ $x(t+1)$ कोणीय त्वरण और वेग के कारण रैखिक कार्य नहीं है। इसलिए आपको इस कार्य के लिए गैर-रैखिक अनुमानकों पर विचार करने की आवश्यकता है। हां, ऐसा करने के मानक तरीके हैं। हां, वे साहित्य में उपलब्ध हैं। हां, आम तौर पर सभी इनपुट एक ही फिल्टर में डाल दिए जाते हैं। रोबोट के स्थान, वेग, अभिविन्यास और कोणीय वेग को आउटपुट के रूप में उपयोग किया जाता है। और हाँ, मैं उनके सामान्य विषयों का संक्षिप्त परिचय यहाँ प्रस्तुत करूँगा। मुख्य टेक-वे हैं
1. अपने राज्य में Gyro और IMU पूर्वाग्रह को शामिल करें या आपके अनुमानों में बदलाव होगा
2. एक विस्तारित कलमन फ़िल्टर (EKF) आमतौर पर इस समस्या के लिए उपयोग किया जाता है
3. कार्यान्वयन खरोंच से प्राप्त किया जा सकता है, और आमतौर पर "ऊपर देखा" होने की आवश्यकता नहीं है।
4. कार्यान्वयन स्थानीयकरण और SLAM समस्या के अधिकांश के लिए मौजूद हैं, इसलिए आपके द्वारा किए गए कार्य से अधिक काम न करें। देखें: रोबोट ऑपरेटिंग सिस्टम आरओएस

अब, अपने सिस्टम के संदर्भ में ईकेएफ को समझाने के लिए। हमारे पास IMU + Gyro, GPS, और odometry है। प्रश्न में रोबोट उल्लिखित एक अंतर ड्राइव है। छानने के कार्य रोबोट की वर्तमान मुद्रा अनुमान लेने के लिए है , नियंत्रण आदानों , और प्रत्येक संवेदक, से माप , और अगली बार कदम पर अनुमान उत्पन्न । हम IMU माप फोन करता हूँ , जीपीएस है , और odometry, । ${\hat{x}_t}$ $u_t$ $z_t$ $\hat{x}_{t+1}$ $I_t$ $G_t$ $O_t$

मुझे लगता है हम के रूप में रोबोट मुद्रा का आकलन कर रहे हैं में रुचि रखने वाले । IMU और Gyros के साथ समस्या बहाव है। त्वरण में एक गैर-स्थिर पूर्वाग्रह है जो आपको ईकेएफ में होना चाहिए। यह पूर्वाग्रह में अनुमानित स्थिति में डालकर (आमतौर पर) किया जाता है। यह आपको प्रत्येक समय कदम पर सीधे पूर्वाग्रह का अनुमान लगाने की अनुमति देता है। $x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta}}$ $x_t={x,y,\dot{x},\dot{y},\theta,\dot{\theta},b}$ , गैसों के एक वेक्टर के लिए । $b$

मैं मान रहा हूँ:

= दो दूरी माप जो कि कुछ छोटे समय वृद्धि में यात्रा की दूरी को दर्शाती है $O_t$
= तीन उन्मुखीकरण माप और तीन त्वरण माप । $I_t$ ${\alpha, \beta, \theta}$ ${\ddot{x},\ddot{y},\ddot{z}}$
=वैश्विकफ्रेममें रोबोट की स्थिति, । $G_t$ $^Gx_t,^Gy_t$

आमतौर पर, नियंत्रण इनपुट (प्रत्येक चलने के लिए वांछित गति) के परिणामस्वरूप आउटपुट (रोबोट के मुद्रा में परिवर्तन) के लिए मैप करना मुश्किल होता है। के स्थान में , यह सामान्य है (देखें Thrun , Odometry प्रश्न ) नियंत्रण से "परिणाम" के रूप में odometry उपयोग करने के लिए। जब आप निकट-घर्षण रहित सतह पर नहीं होते हैं तो यह धारणा अच्छी तरह से काम करती है। IMU और GPS स्लिपेज के लिए सही मदद कर सकते हैं, जैसा कि हम देखेंगे। $u$

तो पहला काम वर्तमान स्थिति से अगले राज्य भविष्यवाणी करने के लिए । डिफरेंशियल ड्राइव रोबोट के मामले में, यह भविष्यवाणी सीधे साहित्य से प्राप्त की जा सकती है (देखें कि मोबाइल रोबोट्स की किनेमैटिक्स पर या किसी आधुनिक रोबोटिक्स की पाठ्यपुस्तक में अधिक संक्षिप्त उपचार), या यहां दिखाए गए अनुसार खरोंच से व्युत्पन्न: ओडोमेट्री प्रश्न । $\hat{x}_{t+1} = f(\hat{x}_t,u_t)$

तो, हम अब भविष्यवाणी कर सकते हैं । यह प्रचार या भविष्यवाणी कदम है। आप केवल प्रचार करके एक रोबोट संचालित कर सकते हैं। यदि मान पूरी तरह से सही हैं, तो आप एक अनुमान है कभी नहीं होगा जो वास्तव में अपने सही स्थिति के बराबर हो। व्यवहार में ऐसा कभी नहीं होता। $\hat{x}_{t+1} = f(\hat{x}_t, O_t)$ $O_t$ $\hat{x}$

यह केवल पिछले अनुमान से अनुमानित मूल्य देता है, और हमें यह नहीं बताता है कि अनुमान की सटीकता समय के साथ कैसे घट जाती है। इसलिए, अनिश्चितता को फैलाने के लिए, आपको EKF समीकरणों का उपयोग करना चाहिए (जो गॉसियन शोर मान्यताओं के तहत बंद रूप में अनिश्चितता का प्रचार करते हैं), एक कण फ़िल्टर (जो नमूना-आधारित दृष्टिकोण का उपयोग करता है) *, UKF (जो एक बिंदु-वार का उपयोग करता है) अनिश्चितता का अनुमान), या कई अन्य प्रकारों में से एक है।

ईकेएफ के मामले में, हम निम्नानुसार आगे बढ़ते हैं। चलो रोबोट राज्य के सहप्रसरण मैट्रिक्स हो। हम समारोह linearize एक रेखीय प्रणाली प्राप्त करने के लिए टेलर सीरीज विस्तार का उपयोग। कलमन फ़िल्टर का उपयोग करके एक रैखिक प्रणाली को आसानी से हल किया जा सकता है। समय में अनुमान के सहप्रसरण मान लें है , और odometry में शोर के ग्रहण सहप्रसरण मैट्रिक्स के रूप में दिया जाता है (आमतौर पर एक विकर्ण की तरह, मैट्रिक्स )। फ़ंक्शन के मामले में , हम याकूब को प्राप्त करते हैं $P_t$ $f$ $t$ $P_t$ $U_t$ $2\times2$ $.1\times I_{2\times 2}$ $f$ और $F_x=\frac{\partial f}{\partial x}$ , फिर अनिश्चितता का प्रचार करें, $F_u=\frac{\partial f}{\partial u}$

{पी}_{टी + 1} = {एफ}_{एक्स} * {पी}_{टी} * {एफ}_{एक्स}^{टी} + {एफ}_{यू} * {यू}_{टी} * {एफ}_{यू}^{टी}

$P_{t+1}=F_x*P_t*F_x^T + F_u*U_t*F_u^T$

$I_t$ $G_t$

$h_g()$ $h_i()$ $R$ $R_g$ $R_i$ $h$

$s$ $z_s$

v_{रों} = z_{रों} - ज_{रों} ({\hat{एक्स}}_{टी + 1})

$v_s=z_s- h_s(\hat{x}_{t+1})$

{एस}_{रों} = {एच}_{रों} * {पी}_{टी + 1} * {एच}_{रों}^{टी} + {आर}_{रों}

$S_s = H_s*P_{t+1}*H_s^T + R_s$

कश्मीर = {पी}_{टी + 1} * {एच}_{रों}^{टी} {एस}_{रों}^{- 1}

$K = P_{t+1}*H_s^T S^{-1}_s$

{\hat{एक्स}}_{टी + 1} = {\hat{एक्स}}_{टी + 1} - कश्मीर * v

$\hat{x}_{t+1} = \hat{x}_{t+1} - K*v$

{पी}_{टी + 1} = (मैं - कश्मीर * {एच}_{रों}) * {पी}_{टी + 1}

$P_{t+1} = (I-K*H_s)*P_{t+1}$

$z_g=h_g()$ $H_g$ $R_g$

$z_i=h_i()$ $p_i$ $R_i$ $p_i$ । पूर्वाग्रह के लिए अद्यतन को शामिल करना अधिक कठिन है, और मेरी विशेषज्ञता से बाहर है। हालाँकि, जब से आप प्लांटर गति में रुचि रखते हैं, आप शायद समस्या को सरल कर सकते हैं। आपको इसके लिए साहित्य में देखना होगा।

कुछ ऑफ-द-टॉप-ऑफ-माइ-हेड संदर्भ:

यह क्षेत्र इतना परिपक्व है कि Google (विद्वान) शायद आपको एक कार्यशील कार्यान्वयन मिल सकता है। यदि आप इस क्षेत्र में बहुत काम करने जा रहे हैं, तो मैं आपको एक ठोस पाठ्यपुस्तक चुनने की सलाह देता हूं। शायद गूगल कार की प्रसिद्धि के एस थ्रुन द्वारा प्रोबेलिस्टिक रोबोटिक्स जैसा कुछ । (मैंने इसे देर रात के कार्यान्वयन के लिए एक उपयोगी संदर्भ पाया है)।

* रोबोट ऑपरेटिंग सिस्टम (आरओएस) में कई पीएफ-आधारित अनुमानक उपलब्ध हैं । हालाँकि, इन्हें इनडोर उपयोग के लिए अनुकूलित किया गया है। कण फिल्टर मल्टी-मोडल पीडीएफ के साथ सौदा करते हैं, जो मानचित्र-आधारित स्थानीयकरण ( इस दरवाजे या उस दरवाजे के पास मैं हूं ) से हो सकता है। मेरा मानना है कि अधिकांश आउटडोर कार्यान्वयन (विशेषकर जो जीपीएस का उपयोग कर सकते हैं, कम से कम रुक-रुक कर) विस्तारित कलमन फ़िल्टर (ईकेएफ) का उपयोग कर सकते हैं । मैंने अंतर ड्राइव के साथ एक आउटडोर, ग्राउंड रोवर के लिए विस्तारित कलामन फ़िल्टर का सफलतापूर्वक उपयोग किया है।

— जोश वन्डर हुक
स्रोत

(1) मैं कण फिल्टर के लिए "स्पष्ट" कनेक्शन नहीं देखता। (२) यदि अन्य प्रश्न / सूत्र हैं जो मेरे प्रश्न से मिलते-जुलते हैं, तो कृपया उनके लिए एक लिंक दिखाएं। (३) मैं ईकेएफ के जिस्ट को समझता हूं, और निश्चित रूप से एक का उपयोग करने के लिए स्विच करूंगा ... अगर मुझे वास्तविक रूप से पहली बार में राज्य संक्रमण पता था (जो मेरे प्रश्न का एक बड़ा हिस्सा है)। (४) कैमरे और लिडार के साथ एक राज्य के अनुमान में सुधार करने का विचार अमूर्त में ठंडा है, लेकिन यह मेरी जरूरत के दायरे से बाहर है। संदर्भ के लिए धन्यवाद, यद्यपि।

— रोबज

कण फिल्टर एक गैर-रैखिक अनुमानक है। मैं जल्द ही लिंक / Refs अपडेट करूंगा। आईएमयू, गायरो और ओडोमेट्री के लिए राज्य के संक्रमण साहित्य में बड़े पैमाने पर शामिल हैं (रेफ 1 सहित)। फिर, मैं जल्द ही कुछ संदर्भों को अपडेट करूंगा।

— जोश वेंडर हुक

@ रोबोज ने ओपी को बड़े पैमाने पर संपादित किया। टिप्पणियों के जवाब के लिए मानक अभ्यास के बारे में निश्चित नहीं है, इसलिए मैंने पोस्ट में उतनी ही जानकारी जोड़ी जितनी मैं ले सकता हूं।

— जोश वेंडर हुक

7

आप ज्यादातर सामान्य मामलों में समस्या को सरल कर सकते हैं:

बहुत सारे "कमर्शियल ग्रेड" IMus (जैसे Xsens) में बहुत शोरगुल होता है। गति प्राप्त करने के लिए उन्हें फ्यूज़ करने से भी परेशान न हों, ओडोमेट्री पहले से ही परिमाण का क्रम बेहतर है। IMU प्रदान करने वाला एकमात्र उपयोगी डेटा पिच और रोल है, और कुछ हद तक हेडिंग (अगला बिंदु देखें)
IMU से हेडिंग भरोसेमंद नहीं है। यह मैग्नेटोमीटर का उपयोग करता है, और फेरोमैग्नेटिक द्रव्यमान के पास विशाल बहाव (हमारे मामले में 25 मीटर से अधिक 25 डिग्री) दिखाएगा, जैसे कि आप दीवारों के निर्माण में पा सकते हैं। हमने इसे हल करने के लिए IMU हेडिंग का उपयोग किया है, लेकिन एक हेडिंग पूर्वाग्रह का अनुमान लगाते हैं।
यदि आप बाहर हैं, तो यह मत भूलिए कि 10 डिग्री के झुकाव पर 10 मीटर की यात्रा करने से एक्स और वाई में समान परिवर्तन नहीं होता है और फ्लैट इलाके में 10 मीटर की यात्रा करने की तुलना में। यह आमतौर पर Z का अनुमान लगाकर हिसाब लगाया जाता है, लेकिन मुझे लगता है कि इसका अनुमान अलग तरह से लगाया जा सकता है।
जीपीएस भी एक झूठ बोल कुतिया है, आमतौर पर उच्च-बहुपथ वातावरण में। प्लस लो-ग्रेड (और यहां तक कि कुछ स्थितियों में उच्च-ग्रेड) जीपीएस में बहुत गलत मानक विचलन की रिपोर्ट करने की प्रवृत्ति होती है। हमने यह जांचने के लिए कुछ सरल ची-स्क्वायर परीक्षणों का उपयोग किया है कि क्या किसी विशेष जीपीएस माप को एकीकृत किया जाना चाहिए (यानी यह जांचना कि यह एक निश्चित बिंदु तक वर्तमान फ़िल्टर अनुमान से मेल खाता है), जिससे हमें अच्छे परिणाम मिले।

हमारे लिए "विशिष्ट" समाधान अहंकार-गति अनुमान प्राप्त करने के लिए ओडोमेट्री + आईएमयू का उपयोग करना है और फिर एक्स, वाई, जेड और हेडिंग बायस को सही करने के लिए जीपीएस का उपयोग करना है।

यहां एक ईकेएफ कार्यान्वयन है जिसका हमने बड़े पैमाने पर उपयोग किया है। यदि आपको IMU के अभिविन्यास का अनुमान लगाने की आवश्यकता है (अर्थात यदि इसमें पहले से निर्मित फ़िल्टर नहीं है), तो आप इन दो फ़िल्टर: UKF और EKF पर भी उपयोग कर सकते हैं ।

— sylvain.joyeux
स्रोत

तो आपने अपने ईकेएफ राज्य में प्रमुख पूर्वाग्रह का अनुमान लगाया है? जिज्ञासा से बाहर, वह काम कितना अच्छा था?

— रोब्ज ऑग