एक चतुर्भुज की ऊंचाई को स्थिर करने की समस्या के उदाहरण का उपयोग करते हुए कई सेंसर के साथ एक जड़त्वीय नेविगेशन प्रणाली (एएनएन) का उपयोग करना

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

सबसे पहले, कुछ पृष्ठभूमि की जानकारी स्पष्ट करने के लिए कि आपको क्या करना है।
एक मल्टीरोट एक उपकरण है जिसमें प्रोपेलर के साथ कई (3, 4, 6, 8) मोटर्स होते हैं, जिनमें से प्रत्येक एक लंबवत समायोज्य कर्षण बनाता है। एक हेलीकाप्टर के विपरीत, यहां स्थिरीकरण पूरी तरह से इलेक्ट्रॉनिक है, और माइक्रोप्रोसेसर इसमें ANN (उड़ान नियंत्रक) का उपयोग कर रहा है।

उड़ान में किन कार्यों को संबोधित करने की आवश्यकता है?

• अभिविन्यास का निर्धारण (जमीन के सापेक्ष तीन अक्षों के साथ कोण) और उनके साथ स्थिरीकरण
• ऊंचाई का निर्धारण और उस पर स्थिरीकरण
• निर्देशांक और दिए गए बिंदुओं पर उड़ान का निर्धारण
• नियंत्रण कक्ष से कमांड प्राप्त करना और मोटर्स को नियंत्रण संकेत जारी करना

हमारे पास हमारे निपटान में क्या है?

फिलहाल, आसान पहुंच में, हमारे पास सेंसर का एक मानक सेट है:

• बहुत अच्छा 3 अक्ष gyroscopes।
• औसत गुणवत्ता के 3-अक्ष त्वरक
• औसत गुणवत्ता के 3-अक्ष मैग्नेटोमीटर
• मध्यम या खराब बैरोमीटर

इस तरह की किट को Arduino- जैसे प्रोसेसर के साथ या अलग स्कार्फ के साथ 70-100 डॉलर में पाया जा सकता है

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

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



एक सोनार बैरोमीटर की सहायता के लिए आ सकता है, जो ऊंचाई को बहुत अधिक सटीकता के साथ मापता है (यहां तक ​​कि जो मैंने $ 5 के लिए खरीदा है वह निर्माता के कथन के अनुसार mm 3 मिमी की सटीकता देता है)। लेकिन सोनार जमीन (2-10 मीटर) से ऊपर केवल कम काम करने में सक्षम है, लंबे समय (200 मीटर तक) के लिए उपाय, सतह की गुणवत्ता के प्रति संवेदनशील है, झुकाव के कोण तक, और एक संकेत खो सकता है।
तीसरा कार्य - निर्देशांक का निर्धारण - उपरोक्त संवेदकों द्वारा किसी भी तरह से हल नहीं किया जाता है। गायरोस्कोप के साथ संयोजन में एक्सेलेरोमीटर रैखिक क्षैतिज त्वरण पैदा कर सकता है, लेकिन दो समस्याएं हैं: विशाल 1 जी वेक्टर (हम जो मापेंगे) की तुलना में लगातार अभिनय कर रहे हैं, और सुधार के लिए सुधार की अनुपस्थिति। तो निर्देशांक का निर्धारण जीपीएस सेंसर का विशेषाधिकार है, और आप यहाँ उच्च सटीकता पर भरोसा नहीं कर सकते हैं।

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

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

चिकनी

अपने वर्तमान रूप में बैरोमीटर पढ़ने से हमें क्या फायदा नहीं है? खैर, सबसे पहले, सिग्नल के मजबूत शोर से मोटर्स पर अनावश्यक नियंत्रण प्रभाव होगा। कम-पास फिल्टर लगाने से, हम शोर कम करते हैं, लेकिन माप की गति खो देते हैं। इसका मतलब यह है कि किसी भी अल्पकालिक गड़बड़ी को नजरअंदाज किया जाएगा, तेज गड़बड़ी को लंबे समय तक देरी के साथ काम किया जाएगा, और सबसे महत्वपूर्ण बात, हमें पीआईडी ​​नियंत्रक के लिए अंतर घटक (डी) नहीं मिलेगा। और सिद्धांत से निम्नानुसार, इस घटक के बिना एक नियामक लक्ष्य मान के आसपास कमजोर रूप से दोलन करने के लिए प्रवण है, जो व्यवहार में मनाया जाता है।

एकीकृत

ठीक है, बैरोमीटर छोड़ दो और एक्सेलेरोमीटर ले लो। ऐसा लगता है कि सब कुछ सरल है - हम जेड अक्ष के साथ मूल्य से निरंतर 1 जी को घटाते हैं और रैखिक ऊर्ध्वाधर त्वरण प्राप्त करते हैं। हम इसे दो बार एकीकृत करते हैं (वास्तव में, माप चक्र में संक्षेप) और वेग और सापेक्ष विस्थापन प्राप्त करते हैं। पीआईडी ​​नियंत्रक के लिए, ये tidbits हैं, उनके साथ आप एक अच्छा गतिशील मॉडल बना सकते हैं। लेकिन यहां, सब कुछ उतना अच्छा नहीं है जितना हम चाहते हैं। तंत्र का झुकाव Z अक्ष पर त्वरण वेक्टर A के प्रक्षेपण में बदलाव का कारण बनेगा। मोटर से कंपन या तापमान में परिवर्तन संवेदनशीलता में "बदलाव" का कारण बन सकता है, और हमारा 1G स्थिरांक अब वास्तविकता के अनुरूप नहीं होगा। लेकिन यहां तक ​​कि पूरी तरह से गतिहीन उपकरण और ठीक 1G सेट के मामले में, कोई भी सेंसर शोर पैदा करता है। लेकिन दोहरे एकीकरण के दस सेकंड के भीतर भी एक छोटी सी गलती एक हाथी के आकार तक बढ़ जाती है, और अब हम 10 मीटर / सेकंड की गति और 20 मीटर की ऊंचाई देखते हैं (हालांकि हम जमीन से बाहर नहीं आए हैं)।

पूरक फ़िल्टर

सरल तरीके से समझाने के लिए - इस फिल्टर को अलग-अलग सेंसरों द्वारा मापी गई दो मात्राओं पर लागू किया जाता है, और उनमें से एक को सही करता है ताकि यह धीरे-धीरे दूसरे तक पहुंच जाए। मापने के चक्र में, फ़िल्टर एक सरल सूत्र द्वारा कार्यान्वित किया जाता है:



इस मामले में, ए 1 पर ए 2 का प्रभाव उनके बीच के अंतर के आनुपातिक है और गुणांक कश्मीर (बड़ा, कमजोर प्रभाव) द्वारा निर्धारित किया जाता है।
यदि आप इस फ़िल्टर को एक्सेलेरोमीटर द्वारा पाई गई ऊँचाई और बैरोमीटर के रीडिंग पर लागू करते हैं, तो आपको एक दिलचस्प बात मिलती है: एक्सीलरोमीटर के बहाव को बैरोमीटर द्वारा लगातार समायोजित किया जाएगा, और बैरोमीटर की रीडिंग को सुचारू किया जाएगा (क्योंकि A2 के लिए यह फ़िल्टर लो-पास फ़िल्टर की तरह काम करता है)। लेकिन केवल दूसरा इंटीग्रल सही किया जाएगा, और पहले वाला शांति से अनंत को "बहाव" देगा, और परिणामस्वरूप, छोटे गुणांक के कारण बैरोमीटर बस स्थिति को प्रभावित करने में सक्षम नहीं होगा।
यह फिल्टर जाइरोस्कोप + एक्सेलेरोमीटर की एक जोड़ी के लिए महान काम क्यों करता है? क्योंकि वहां हम पहले इंटीग्रल को सही करते हैं, और अंत में यह "फ्लोट दूर" करना बंद कर देता है जब एक चक्र के दौरान सुधार की मात्रा एकीकरण के दौरान एक ही चक्र में जोड़े गए गायरोस्कोप त्रुटि के मूल्य के बराबर होती है।

पीआईडी ​​नियंत्रक बचाव के लिए आता है

लेकिन बैरोमीटर + एक्सेलेरोमीटर की जोड़ी से भी, आप कुछ उपयोगी निकाल सकते हैं यदि आप उनके लिए एक पीआईडी ​​नियंत्रक लागू करते हैं (हाँ, उनके आवेदन का क्षेत्र अत्यंत व्यापक है)।
तो हमारे त्वरक इंटीग्रेटर का मुख्य कमजोर बिंदु क्या है? सूक्ष्म त्रुटि में, जो ऊपर वर्णित विभिन्न कारणों से हो सकता है, जब स्थिर 1 जी को घटाया जाता है। यदि आप फॉर्म में वांछित त्वरण लिखते हैं:



फिर, पूर्वाग्रह मूल्य को समायोजित करके, पहले अभिन्न (गति) और दूसरा (विस्थापन) दोनों को नियंत्रित करना संभव है। तो, हमारे पीआईडी ​​नियंत्रक का लक्ष्य पाया जाता है। लेकिन आपको अभी भी गलती जानने की जरूरत है। हम यह अनुमान लगाते हैं कि पूर्वाग्रह सिस्टम मापदंडों (तापमान, कंपन, आदि) के कुछ स्थिरीकरण के बाद तय किया जाएगा। जब पूर्वाग्रह पाया जाता है, तो एक्सेलेरोमीटर की रीडिंग सच्चाई के बहुत करीब हो जाएगी और आप बैरोमीटर के साथ उन्हें पार करके एक पूरक फ़िल्टर लागू कर सकते हैं। इस फ़िल्टर का सुधार मूल्य वह त्रुटि होगी, जिसमें से PID कंट्रोलर पीछे हट जाएगा (यह लक्ष्य चर को नियंत्रित करके त्रुटि को 0 पर कम करना चाहता है)।
अगला, हम पीआईडी ​​नियंत्रक के सभी तीन घटकों को पाते हैं। आनुपातिक (P) स्वयं त्रुटि है। इंटीग्रल (आई) - बस इसे एकीकृत करें। विभेदक (डी) - सिद्धांत के अनुसार, त्रुटि को अंतर करना आवश्यक है। लेकिन एक बैरोमीटर और इसमें एक एक्सेलेरोमीटर के भयानक शोर हैं। यह अंतर करने के लिए डरावना है, इसलिए हम एक चाल का उपयोग करेंगे - हम डी-घटक के लिए एक्सेलेरोमीटर द्वारा पाए गए नकारात्मक घटक को नहीं लेंगे। चूंकि डी को नियामक में क्षीणन को पेश करने के लिए डिज़ाइन किया गया है, इसलिए यहां गति काफी उपयुक्त है - जितना अधिक है, उतना ही इसे "बुझाने" की आवश्यकता है।

प्रत्येक घटक को अपने गुणांक से गुणा करें, जोड़ें और पूर्वाग्रह प्राप्त करें। लेकिन यहां हम दूसरी चाल लागू करते हैं - हम सीधे त्वरण में पूर्वाग्रह नहीं जोड़ेंगे, लेकिन केवल I-part को जोड़ेंगे। यह वह है जो स्थायी त्रुटि की भयावहता का वर्णन करता है, जो पूर्वाग्रह में धीमी गति से परिवर्तन की हमारी धारणा से मेल खाती है।
हम डी से गुणा करके पी और डी भागों को जोड़ते हैं (चूंकि हमने उन्हें त्वरण से उधार लिया है)।
चूंकि नियामक का मुख्य कार्य त्रुटि के निरंतर घटक को ढूंढना है, इसलिए मैंने इसे कम से कम अल्पकालिक परिवर्तनों को प्रभावित करने के लिए पर्याप्त रूप से "धीरे" स्थापित करने का निर्णय लिया। लेकिन प्रयोगों के लिए एक विस्तृत क्षेत्र बना हुआ है, और सब कुछ वास्तविक सेंसर के व्यवहार से निर्धारित किया जाएगा।

लेकिन जाइरोस्कोप का क्या?

मैंने ऊपर उल्लेख किया है कि ऊंचाई निर्धारित करने के लिए हमें एक गाइरोस्कोप की भी आवश्यकता होगी। वास्तव में, ऊपर वर्णित एल्गोरिथ्म केवल तभी काम करेगा जब वेक्टर ए (स्थानीय प्रणाली में) जेड अक्ष के साथ बिल्कुल दिखता है। जैसे ही डिवाइस झुकता है, दो अप्रिय चीजें होंगी: जेड अक्ष पर ए का प्रक्षेपण बदल जाएगा और पीआईडी ​​नियंत्रक पूर्वाग्रह के लिए धीरे-धीरे और दर्दनाक रूप से फिर से देखना शुरू कर देगा। । और दूसरा - कोई भी क्षैतिज त्वरण स्थानीय Z अक्ष पर एक नोज़ेरो प्रक्षेपण देना शुरू कर देगा। 45 ° के झुकाव कोण पर, आप समझ नहीं पाएंगे कि त्वरण कहाँ है।
लेकिन जब से हम वैश्विक एक के सापेक्ष स्थानीय प्रणाली के सटीक अभिविन्यास को जानते हैं, न्याय को बहाल करने के लिए कुछ भी मुश्किल नहीं है - बस स्थानीय वेक्टर ए को स्थानीय वेक्टर जी (मूल रूप से एक्सेलेरोमीटर द्वारा पाया गया और ध्यान से गायरोस्कोप द्वारा घुमाया गया) पर प्रोजेक्ट करें, जो हमेशा जमीन में दिखता है।
यह ऑपरेशन सरल है और वेक्टर उत्पाद की परिभाषा से निम्नानुसार है:



यह 1G को घटाने से पहले किया जाना चाहिए।

अब आप कोड और परिणाम देख सकते हैं।

#define ACC_BARO_CMPF 300.0f #define ACC_BARO_P 30.0f #define ACC_BARO_I (ACC_BARO_P * 0.001f) #define ACC_BARO_D (ACC_BARO_P * 0.001f) #define VEL_SCALE ((1.0f — 1.0f/ACC_BARO_CMPF)/1000000.0f) #define ACC_SCALE 9.80665f / acc_1G / 10000.0f err = (alt - BaroAlt)/ACC_BARO_CMPF; // P term of error errI+= err * ACC_BARO_I; // I term of error accZ = (accADC[0]*EstG.VX + accADC[1]*EstG.VY + accADC[2]*EstG.VZ) * InvSqrt(fsq(EstG.VX) + fsq(EstG.VY) + fsq(EstG.VZ)) - errI - acc_1G; // Integrator - velocity, cm/sec vel+= (accZ - err*ACC_BARO_P - vel*ACC_BARO_D) * cycleTime * ACC_SCALE; // Integrator - altitude, cm alt+= vel * cycleTime * VEL_SCALE; // Apply ACC->BARO complementary filter alt-= err; errPrev = err; 

सेंसर डेटा निम्नलिखित चर में आता है:
EstG.V - वेक्टर जी (यह अभिविन्यास खोजने पर पहले प्राप्त किया गया था)
AccADC - एक्सेलेरोमीटर से स्वच्छ डेटा
BaroAlt - एक बैरोमीटर से डेटा सेमी में परिवर्तित
आउटपुट एक्सीजेड, वेल, ऑल्ट है।

जैसा कि आप देख सकते हैं, एल्गोरिथ्म की कम्प्यूटेशनल जटिलता काफी सरल है और Arduino इसे आसानी से "पचाता है" (खासकर यदि आप इसे पूर्णांक अंकगणित में अनुवाद करते हैं, लेकिन फिर कोड खराब पठनीय हो जाता है)।



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

यदि आप एक सोनार को इस एल्गोरिथ्म से जोड़ते हैं (बरोल के बजाय सोनारटेल ले), तो ऊंचाई वक्र लगभग सही दिखता है। इस प्रकार, कम ऊंचाई पर हम एक सोनार का उपयोग करते हैं, जब त्रुटियां होती हैं या माप सीमा के करीब होती है, तो हम एक बैरोमीटर पर स्विच करते हैं (सोनार से विश्वसनीय डेटा की अवधि के दौरान ऊंचाई पर सहमत होने के बाद)।

दुर्भाग्य से, मौसम अभी तक नए एल्गोरिथ्म की उड़ान परीक्षणों की अनुमति नहीं देता है। जैसे ही परिणाम सामने आते हैं, मैं सोनार ग्राफ, पूरे प्रोजेक्ट का डिबग्ड सोर्स कोड और फ्लाइट वीडियो बिछा दूंगा।