लेजर स्कैन + ज्ञात मानचित्र के साथ विस्तारित कलमन फ़िल्टर

Question

मैं वर्तमान में स्कूल के लिए एक प्रोजेक्ट पर काम कर रहा हूं, जहां मुझे लेजर स्कैनर के साथ एक पॉइंट रोबोट के लिए विस्तारित कलमन फ़िल्टर लागू करने की आवश्यकता है। रोबोट 0 डिग्री टर्न त्रिज्या के साथ घूम सकता है और आगे बढ़ सकता है। सभी गतियों के टुकड़े-टुकड़े रैखिक (ड्राइव, घुमाव, ड्राइव) हैं।

जिस सिम्युलेटर का हम उपयोग कर रहे हैं वह त्वरण का समर्थन नहीं करता है, सभी गति तात्कालिक है।

हमारे पास एक ज्ञात मानचित्र (पीएनजी छवि) है जिसे हमें स्थानीयकृत करने की आवश्यकता है। हम लेजर स्कैन को अनुकरण करने के लिए छवि में किरणों का पता लगा सकते हैं।

मेरे साथी और मैं गति और सेंसर मॉडल के रूप में थोड़ा भ्रमित हैं जिसका हमें उपयोग करने की आवश्यकता होगी।

अब तक हम राज्य को एक वेक्टर रूप में मॉडलिंग कर रहे हैं ।$(x,y,\theta)$

हम निम्नानुसार अद्यतन समीकरणों का उपयोग कर रहे हैं

void kalman::predict(const nav_msgs::Odometry msg){
    this->X[0] += linear * dt * cos( X[2] ); //x
    this->X[1] += linear * dt * sin( X[2] ); //y
    this->X[2] += angular * dt; //theta

    this->F(0,2) = -linear * dt * sin( X[2] ); //t+1 ?
    this->F(1,2) =  linear * dt * cos( X[2] ); //t+1 ?

    P = F * P * F.t() + Q;

    this->linear = msg.twist.twist.linear.x;
    this->angular = msg.twist.twist.angular.z;
    return;
}

हमने सोचा कि हमारे पास काम करने के लिए सब कुछ था जब तक कि हमने देखा कि हम शुरू करना भूल गए थे Pऔर यह शून्य था, जिसका अर्थ था कि कोई सुधार नहीं हो रहा था। जाहिर तौर पर हमारा प्रचार बहुत सटीक था क्योंकि हमने अभी तक सिस्टम में शोर नहीं किया है।

मोशन मॉडल के लिए हम F के लिए निम्नलिखित मैट्रिक्स का उपयोग कर रहे हैं:

$F = \begin{bmatrix}1 & 0 & -v*\Delta t*sin(\theta) \\ 0 & 1 & v*\Delta t*cos(\theta) \\ 0 & 0 & 1 \end{bmatrix}$

जैसा कि हमारे अद्यतन सूत्रों का जैकबियन है। क्या ये सही है?

सेंसर मॉडल के लिए हम रोबोट , और और रे ट्रेसिंग के बारीक अंतर को ध्यान में रखकर जैकबियन (H) का अनुमान लगा रहे हैं । हमने टीए से बात की जिन्होंने कहा कि यह काम करेगा लेकिन मैं अभी भी अनिश्चित हूं। हमारे प्रोफेसर दूर हैं इसलिए हम उनसे दुर्भाग्य से नहीं पूछ सकते। हम 3 लेजर माप प्रति सुधार कदम का उपयोग कर रहे हैं इसलिए एच एक 3x3 है।$x$$y$$\theta$

दूसरा मुद्दा जहां पी को कैसे इनिशियलाइज़ किया जाए, हमने 1,10,100 की कोशिश की और वे सभी रोबोट को मैप के बाहर (-90, -70) में रखते हैं, जब मैप केवल 50x50 होता है।

हमारे प्रोजेक्ट का कोड यहां पाया जा सकता है: https://github.com/en4bz/kalman/blob/master/src/kalman.cpp

किसी भी सलाह की काफी सराहना की जाती है।

संपादित करें:

इस बिंदु पर मैंने मूल आंदोलन शोर के साथ स्थिर करने के लिए फ़िल्टर प्राप्त किया है, लेकिन कोई वास्तविक आंदोलन नहीं है। जैसे ही रोबोट फ़िल्टर डायवर्ज को बहुत तेज़ी से स्थानांतरित करना शुरू करता है और मानचित्र से बाहर निकलता है।

Answer 1

• लेजर स्कैन और एक ज्ञात मानचित्र के आधार पर स्थानीयकरण के लिए ईकेएफ का उपयोग करना एक बुरा विचार है और यह काम नहीं करेगा क्योंकि ईकेएफ की मुख्य मान्यताओं में से एक मान्य नहीं है: माप मॉडल भिन्न नहीं है।

मेरा सुझाव है कि मोंटे कार्लो लोकलाइज़ेशन (कण फिल्टर) में देखें। यह न केवल आपके माप मॉडल के साथ समस्या का समाधान करेगा, बल्कि वैश्विक स्थानीयकरण (मानचित्र के भीतर प्रारंभिक मुद्रा पूरी तरह से अज्ञात) की भी अनुमति देगा।

संपादित करें: मुझे खेद है कि मैं एक और महत्वपूर्ण बिंदु का उल्लेख करना भूल गया:

• यदि आप ईकेएफ लागू करना चाहते हैं, तो आपके माप मॉडल के साथ-साथ आपके गति मॉडल में केवल गॉसियन शोर शामिल हो सकता है । इसका मतलब है कि आपको अपने माप मॉडल को रूप में लिखने में सक्षम होना चाहिए । यह एक गंभीर सीमा है, क्योंकि यह प्रोबेबिलिस्टिक रोबोटिक्स में बीम_रेन्ज_फाइंडर_मॉडल जैसे थोड़े अधिक जटिल मॉडल की अनुमति नहीं देता है, जो रोबोट के सामने गतिशील वस्तुओं, अमान्य (अधिकतम) माप और पूरी तरह से यादृच्छिक रीडिंग को भी मानता है। आप भाग के लिए कास्टिंग किरणों के साथ फंस जाएंगे और गाऊसी शोर जोड़ ।$z_t = h(x_t) + N(0, Q_t)$$h(x_t)$

Comments

^ ^ "लेजर स्कैन और एक ज्ञात मानचित्र के आधार पर स्थानीयकरण के लिए ईकेएफ का उपयोग करना एक बुरा विचार है" किसने कहा? कृपया संदर्भ प्रदान करें। ईकेएफ सबसे सफल अनुमानक है और स्थानीयकरण की समस्या के लिए ईकेएफ का उपयोग करने के लिए कई कागजात सुझाते हैं। दरअसल, मैं हैरान हूं कि आप क्या कह रहे हैं। ईकेएफ की मुख्य धारणाएं गति और अवलोकन मॉडल रैखिक हैं और शोर गॉसियन है। मुझे नहीं पता कि "माप मॉडल अलग नहीं है।"

— क्रोको

मूल पोस्टर में रेअरट्रिंग का उल्लेख किया गया है जिसका अर्थ है कि वह संभाव्य रोबोटिक्स में "रेंज फाइंडर्स के बीम मॉडल" के समान माप मॉडल का उपयोग करने का इरादा रखता है। यह माप मॉडल जंप प्रदर्शित करता है (कल्पना करें कि एक लेजर बीम मुश्किल से किसी बाधा को मारता है और उसे गायब करता है: यह केवल एक छलांग के लिए एक छोटा घुमाव लेता है जो अलग-अलग नहीं होता है।)

— sebsch

Answer 2

सबसे पहले, आपने इस बात का उल्लेख किया कि आप किस तरह के स्थानीयकरण का उपयोग कर रहे हैं। यह ज्ञात या अज्ञात पत्राचार के साथ है?

अब Matlab में याकूब को प्राप्त करने के लिए आप निम्नलिखित कार्य कर सकते हैं

>> syms x y a Vtran Vrotat t
>> f1 = x + Vtran*t*cos(a);
>> f2 = y + Vtran*t*sin(a);
>> f3 = a + Vrotat*t;
>> input  = [x y a];
>> output = [f1 f2 f3];
>> F = jacobian(output, input)

परिणाम

F =
[ 1, 0, -Vtran*t*sin(a)]
[ 0, 1,  Vtran*t*cos(a)]
[ 0, 0,               1]

एक्सटेंडेड कलमैन फ़िल्टर को लीनियर होने के लिए सिस्टम की आवश्यकता होती है और गॉसियन होने के लिए शोर की आवश्यकता होती है। यहां प्रणाली का अर्थ है गति और अवलोकन मॉडल रैखिक होना चाहिए। लेजर सेंसर रोबोट के फ्रेम से सीमा और कोण को एक लैंडमार्क की ओर देते हैं, इसलिए माप मॉडल रैखिक नहीं है। के बारे में P, यदि आप इसे बड़ा मानते हैं तो आपका ईकेएफ अनुमानक शुरुआत में खराब होगा और यह मापों पर निर्भर करता है क्योंकि पिछले राज्य वेक्टर उपलब्ध नहीं है। हालांकि, एक बार जब आपका रोबोट चलना शुरू कर देता है और संवेदनहीन हो जाता है, तो ईकेएफ बेहतर हो जाएगा क्योंकि यह रोबोट की मुद्रा का अनुमान लगाने के लिए गति और माप मॉडल का उपयोग करता है। यदि आपका रोबोट किसी भी स्थान को महसूस नहीं कर रहा है, तो अनिश्चितता बहुत अधिक बढ़ जाती है। साथ ही, आपको कोण के बारे में सावधान रहने की आवश्यकता है। C ++ में,cos and sinकोण रेडियन में होना चाहिए। इस समस्या के बारे में आपके कोड में कुछ भी नहीं है। यदि आप रेडियन में गणना करते समय कोण के शोर को डिग्री में मान रहे हैं, तो आपको अजीब परिणाम मिल सकते हैं क्योंकि रेडियन में गणना के दौरान शोर के रूप में एक डिग्री को बहुत बड़ा माना जाता है।