"मैनुअल" जोड़तोड़

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

विकास का सार यही है। सामान्य रूप से विचार करें, कहें, 5-अक्ष मैनिपुलेटर (कीनेमेटिक संरचनात्मक आरेख नीचे दिए गए आंकड़े में दिखाया गया है)। अंतरिक्ष में चलते समय इसकी क्षमताएं विस्तृत होती हैं: यह 5 निर्देशांक में गति करने में सक्षम होता है, और एक ही समय में, अर्थात्। तीन XYZ कुल्हाड़ियों के साथ 3 निर्देशांक और उनमें से दो के आसपास रोटेशन। इस मामले में, जोड़तोड़ का काम करने वाला तत्व काम करने की जगह में किसी भी स्थिति पर कब्जा करने में सक्षम है और साथ ही काम करने वाले निकाय के आवश्यक अभिविन्यास को बनाए रखता है (अपने स्वयं के अक्ष के आसपास काम करने वाले तत्व के रोटेशन की गिनती नहीं करता है)।



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


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

1. मैनिपुलेटर का विकास
2. हाथ आंदोलन पर नज़र रखने के लिए एक उपकरण का विकास
3. सब कुछ

संक्षेप में प्रत्येक चरण के बारे में:

1. यहां सब कुछ तुच्छ है ... सीएडी में, हम संरचना को डिज़ाइन करते हैं, भागों और निर्माण के चित्र बनाते हैं, फिर इकट्ठा होते हैं। फिर हम कीनेमेटीक्स की गणना करते हैं और अपने स्वयं के सॉफ़्टवेयर लिखते हैं, जो ड्राइव में निम्न-स्तरीय कॉल छिपाएगा।
2. इस स्तर पर, हम सेंसर के प्रकार का निर्धारण करते हैं, एक सर्किट आरेख डिजाइन करते हैं, एक मुद्रित सर्किट बोर्ड बनाते हैं, अंतरिक्ष में स्थिति का निर्धारण करने के लिए एक एल्गोरिथ्म विकसित करते हैं, सॉफ्टवेयर लिखते हैं।
3. दरअसल, पिछले चरणों के लिए क्या था - मालिक को आज्ञाकारिता बनाने के लिए। यहाँ मैं जोड़तोड़ के कार्य क्षेत्र में निर्देशांक परिवर्तित करने के लिए एक एल्गोरिथ्म विकसित करता हूं।

1. मैनिपुलेटर का विकास

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

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




3D मॉडल को CAD "Compass 3D" में बनाया गया था। भागों सामग्री - प्लास्टिक भागों के लिए 4 मिमी प्लाईवुड और पीएलए। प्लाईवुड से भागों को एक लेजर मशीन पर काट दिया गया था, प्लास्टिक से भागों को 3 डी प्रिंटर पर मुद्रित किया गया था। डायनामिक्स एक्सएक्स -12 डिजिटल सर्वोस का उपयोग अक्ष ड्राइव के रूप में किया जाता है। मैनिपुलेटर की उपस्थिति एक विले कीट से मिलती है, जिससे उन्हें "कोमार" उपनाम मिला।

कीनेमेटीक्स का गणितीय विवरण

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

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



आवश्यक स्थिति एक त्रिज्या वेक्टर द्वारा दर्शाया गया है $$$\ vec {r}$ । वेक्टर क्यों है यह समझाने के लायक है $$$\ vec {r}$ निकला हुआ किनारा बिंदु के लिए तैयार है, और आरओ के अंतिम बिंदु तक नहीं। चूंकि कामकाजी निकाय का अभिविन्यास हमें पहले से ज्ञात है (मैं खुद इसे कुछ आवश्यकताओं के अनुसार निर्धारित करता हूं), यह आवश्यक है कि निकला हुआ किनारा सही जगह पर हो, एक वेक्टर के रूप में इंगित किया गया हो $$$\ vec {r}$ । यह वेक्टर त्रिज्या वेक्टर से घटाकर प्राप्त किया जाता है $$$\ vec {R}$ PO के अंतिम बिंदु पर आकृष्ट किया गया है, इसका उन्मुखीकरण वेक्टर है $$$\ vec {v_0}$ , बीएसके के सापेक्ष, अर्थात्:

$$

$$\ vec {r} = \ vec {R} - \ vec {v_0}$$

निकला हुआ किनारा बिंदु के वांछित स्थान पर संक्रमण पर विचार करें। यह ए , बी और सी के लिंक ए और बी (मैंने उन्हें वैक्टर के रूप में दर्शाया) के रोटेशन द्वारा किया जाता है । बेस कॉर्डिनेट सिस्टम (BSC) की उत्पत्ति को Hinge B के बिंदु पर रखा गया है । काज रोटेशन के अक्ष को Z अक्ष के साथ निर्देशित किया जाता है, अक्ष B और C को Z के लंबवत निर्देशित किया जाता है ।

जब सभी औपचारिकताएं पूरी हो जाती हैं, तो इस बिंदु पर आते हैं। उलटे कीनेमेटिक्स समस्या को हल करने के लिए, मैनिपुलेटर डिज़ाइन की सादगी के कारण मैंने एक ज्यामितीय दृष्टिकोण का उपयोग किया। ज्यामिति से यह देखा जा सकता है कि वेक्टर $$$\ vec {r}$ लिंक वैक्टर के योग के बराबर $$$\ vec {a}$ और $$$\ vec {b}$ । कोणों $$$$ थीटा$ । $$$\ Gamma_1$ । $$$\ Gamma_2$ - क्रमशः ए , बी और सी लिंक के रोटेशन के कोण।
वैक्टर द्वारा बंधे एक त्रिकोण पर विचार करें $$$\ vec {r}$ । $$$\ vec {a}$ और $$$\ vec {b}$ । इस त्रिभुज से, कोसाइन प्रमेय द्वारा, हम कोण पाते हैं $$$\ अल्फा$ और $$$\ बीटा$ । वैक्टर की लंबाई के बराबर होने दें:

$$

$$| \ vec {r} | = r \ quad | \ vec {a} | = a \ quad | \ ve {{b} = = b$$

हम वांछित कोणों के संबंध में कोसाइन प्रमेय लिखते हैं:

$$

$$b ^ 2 = a ^ 2 + r ^ 2-2ar \ cos (\ alpha) \\ r ^ 2 = a ^ 2 + b ^ 2-2ab \ cos (\ beta)$$

कोण व्यक्त करें $$$\ अल्फा$ और $$$\ बीटा$ :

$$

$$\ Alpha = \ arccos \ left (\ frac {a + 2 + r ^ 2-b ^ 2} {2ar} \ right) \ quad \ Beta = \ arccos \ left (\ frac {a 2 + b 2) -r ^ 2} {2ab} \ right)$$

इसे ज्यामितीय आरेख से देखा जा सकता है:

$$

$$\ theta = \ arctan = \ left (\ frac {r_y} {r_x} \ right) \ quad \ omega = \ arcsin = \ left (\ frac {r_z) {r} का दाईं ओर$$

तब:

$$

$$\ gamma_1 = \ omega + \ Alpha \ quad \ gamma_2 = 180 ^ \ circ- \ beta$$

अंत में, लिंक के रोटेशन कोणों के लिए एक रैखिक समन्वय से संक्रमण सूत्रों द्वारा किया जाता है:

$$

$$\ theta = arctan \ left (\ frac {r_y} {r_x} \ right) \\ \ Gamma_1 = arcsin \ left (\ frac {r_z} {r} \ right) + arosos \ left (\ frac) a ^ 2 + r ^ 2-b ^ 2} {2ab} \ right) \\ \ gamma_2 = 180 ^ \ circ-arccos \ left (\ frac {a 2 + b ^ 2-r ^ 2} {2ar} \ right)$$

अब जब आप निकला हुआ किनारा वांछित स्थिति में ले गए हैं, तो आपको काम करने वाले शरीर को ठीक से उन्मुख करने की आवश्यकता है। ऐसा करने के लिए, आपको वेक्टर के निर्देशांक को जानना होगा $$$\ vec {v}$ निकला हुआ किनारा बिंदु के सापेक्ष, अर्थात्। स्थानीय समन्वय प्रणाली (एलएससी) के आधार पर निर्देशांक - $$${E ’}$ जिसकी शुरुआत मैनिपुलेटर के निकला हुआ किनारा बिंदु पर स्थित है।

$$

$$E '= \ left [\ vec {x}' \ \ vec {y} '\ \ vec {z}' का अधिकार]$$

वेक्टर $$$\ vec {x} '$ लिंक बी द्वारा निर्देशित, वेक्टर $$$\ vec {y} '$ - कुंडा f1 की धुरी के साथ।
एक आधार खोजने के लिए $$$ई ’$ हम बीएसके से संक्रमण मैट्रिक्स को परिभाषित करते हैं (आधार के साथ $$$E_0$ ) और एलएसके। इस मैट्रिक्स को टिका ए , बी और सी में घुमावों के संयोजन द्वारा प्राप्त किया जाता है :

$$

$$E '= CE_0$$

जहाँ

$$

$$C = R_CR_BR_A$$

क्योंकि मैट्रिक्स $$$E_0$ एकल है, फिर:

$$

$$E '= C = R_CR_BR_A$$

मैट्रिक्स $$$सी$ यह एक कोण द्वारा धुरी के चारों ओर रोटेशन मैट्रिसेस को गुणा करके गणना की जाती है।
हम इस मैट्रिक्स की गणना करने वाले फ़ंक्शन को परिभाषित करते हैं:

$$

$$f = axisAngle2rotMat (अक्ष, कोण)$$

अक्ष को वेक्टर के रूप में पिछले घुमावों द्वारा प्राप्त मैट्रिक्स के एक स्तंभ के रूप में लिया जाता है, और कोण पहले से ही ऊपर की गणना करते हैं।

$$

नतीजतन, हम प्राप्त करते हैं:

$$

$$C = C_ {CBA} \ quad E '= C$$

बीएसके में काम कर रहे शरीर के वेक्टर को दें $$$\ vec {v_0}$ । फिर समानता सत्य है:

$$

$$\ vec {v_0} = E '\ vec {v}$$

यहाँ से हम व्यक्त करते हैं $$$\ vec {v}$ - आधार में कामकाजी निकाय के वेक्टर $$$ई ’$ , यानी। निकला हुआ किनारा बिंदु के सापेक्ष:

$$

$$\ vec {v} = E '^ T \ vec {v_0}$$

अब, पीओ वेक्टर को जानते हुए, हम ओरिएंटिंग कुल्हाड़ियों f1 और f2 के रोटेशन कोणों की गणना कर सकते हैं


पीओ को दिए गए स्थान पर ले जाने के लिए, आपको कोनों पर एक घुमाव बनाने की आवश्यकता है $$$\ varphi_1$ और $$$\ varphi_2$ । आंकड़ा दिखाता है कि

$$

$$\ varphi_1 = arctan \ left (\ frac {v_y} {v_z} \ right) \ quad \ varphi_2 = arcsin \ left (\ frac {\ sqrt {v_y ^ 2_ v_z ^ 2}} {V} \ right)$$

जहाँ $$$V = | \ vec {v} | = \ sqrt {v_x ^ 2 + v_y ^ 2 + v_z ^ 2}$ - कामकाजी निकाय की लंबाई,
$$$v_x, v_y, v_z$ - वेक्टर निर्देशांक $$$\ vec {v}$ ।

मैनिप्युलेटर का सॉफ्टवेयर दो स्तरों में विभाजित है, ऊपरी और निचला। ऊपरी स्तर पर एक पुस्तकालय और विभिन्न कमांड विधियों के रूप में मतलाब में लिखा गया है, निचला - एटमेगा 328 न्यूक्लियर पर। ऊपरी स्तर का कार्य माइक्रोकंट्रोलर को भेजे गए कमांड को तैयार करना है। उनका काम टीम को प्राप्त करना है और इसके अनुसार, ड्राइव को सही स्थिति में स्थापित करना है। इस रूप में, नियंत्रण प्रणाली विश्वसनीय (अब तक ...) और उपयोग करने के लिए सुविधाजनक हो गई।


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

लिखित पुस्तकालय के बारे में कुछ शब्द। इसके निपटान में निम्नलिखित कार्य हैं:

1. सीरियल कनेक्शन फ़ंक्शन
2. चल समारोह

• एक बिंदु पर, सभी ड्राइव की निरंतर गति के साथ
• एक बिंदु पर, सभी ड्राइव के अंतिम पड़ाव के साथ (ड्राइव की गति को उसके स्वयं के और सबसे बड़े रोटेशन कोण के अनुपात में चुना जाता है। इस प्रकार, बिंदु के दृष्टिकोण के अंत में, सभी ड्राइव चलना बंद कर देते हैं)
• एक बिंदु पर, प्रत्येक ड्राइव की एक चिकनी गति के साथ (इस मामले में, गति का ग्राफ त्वरण, निरंतर गति और ब्रेकिंग के क्षेत्रों के साथ एक ट्रेपोज़ॉइड जैसा दिखता है)
  
  
  समय सारिणी

  

  
  
  
  
• एक बिंदु पर, कुल्हाड़ियों के रोटेशन और रोटेशन की गति के मैन्युअल रूप से निर्दिष्ट कोणों के अनुसार

फ़ंक्शंस की सूची को निश्चित रूप से विस्तारित किया जा सकता है, लेकिन यह कार्यक्षमता सीधे उस समस्या को हल करने के लिए पर्याप्त थी जिसके लिए मैनिपुलेटर डिज़ाइन किया गया था।

वीडियो मैनिप्युलेटर के संचालन को दर्शाता है।

2. हाथ आंदोलन पर नज़र रखने के लिए एक उपकरण का विकास

अगले चरण पर जाएं। यहां आपको एक विशेष उपकरण डिजाइन करने की आवश्यकता होगी जो वास्तविक समय में आपके हाथ की हथेली में स्थिति को ट्रैक कर सके। साथ ही, इस प्रक्रिया को "मोशन कैप्चर" कहा जाता है। योजना को कार्यान्वित करने के लिए, मैंने तीन सेंसरों के आधार पर स्ट्रैडडाउन जड़त्वीय नेविगेशन प्रणाली (SINS) का उपयोग करने का निर्णय लिया: एक गायरोस्कोप, एक्सेलेरोमीटर और मैग्नेटोमीटर। इन सेंसरों के एक समूह को IMU कहा जाता है - एक सेंसर (अनुवाद में, यह वही SINS है)।

सबसे पहले, मैंने अपनी ज़रूरत की कार्यक्षमता के साथ एक मुद्रित सर्किट बोर्ड डिज़ाइन किया, जिस पर मैंने ये तीन सेंसर लगाए। प्रसंस्करण नियंत्रक के रूप में, मैंने Atmega2560 माइक्रोकंट्रोलर को चुना (या बल्कि जो लिया गया था)। चीनी बाजार में सेंसर ने आम और सस्ते का इस्तेमाल किया। ये सेंसर्ड थ्री-एक्सिस जीरोस्कोप और एक्सेलेरोमीटर MPU6050 और मैग्नेटोमीटर HMC5883l हैं।



अंतरिक्ष में स्थिति की गणना के लिए एल्गोरिथ्म काफी सरल है: पहले, हम स्थानीय समन्वय प्रणाली (एलएससी) के लिए एक आधार के रूप में अभिविन्यास निर्धारित करते हैं, जो सेंसर के साथ जुड़ा हुआ है, और अंततः हाथ की हथेली के साथ जुड़ा हुआ है।



ओरिएंटेशन प्रारंभिक समन्वय प्रणाली (NSC) के सापेक्ष निर्धारित किया जाता है, जो कि पावर-अप के समय अनिवार्य रूप से डिवाइस की प्रारंभिक स्थिति है। एनएससी और एलएससी के ठिकानों में इसके ओर्थ के कॉलम वैक्टर होते हैं:

$$

$$E_0 = \ left [\ vec {X} \ \ vec {Y} \ \ vec {Z} \ right] = \ बाएँ [\ start {array} {cccc} 1 & 0 & 0 \\ 0 & 1 और 0 \\ 0 & 0 & 1 \ _ {अंत} सरणी \ right] \\ E '= \ left [\ vec {x}' \ \ vec {y} '\ \ vec {z}' \ right]$$

वे अनुपात से संबंधित हैं:

$$

$$E '= CE_0$$

जहाँ $$$सी$ एक संक्रमण मैट्रिक्स है, यह अंतरिक्ष में एक घूर्णन मैट्रिक्स भी है। हम एनएससी के आधार को पहचान मैट्रिक्स के रूप में मानते हैं। इसलिये

$$

$$E '= C$$

।
के बाद, त्वरण वेक्टर $$$\ vec {A}$ , जिसे एलएससी के संबंध में जाना जाता है, एनएससी में निर्धारित किया जाना चाहिए। यह एलएससी आधार द्वारा त्वरण स्तंभ वेक्टर को गुणा करके किया जाता है:

$$

$$\ vec {A} = E '\ vec {A}'$$

फिर हम समय के साथ दो बार प्राप्त त्वरण वेक्टर को एकीकृत करते हैं और दूरी प्राप्त करते हैं, अर्थात। निर्देशांक:

$$

$$\ vec {r} = \ iint \ vec {A} dt$$

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


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

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

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

$$

$$q = \ left [q_1 \ q_2 \ q_3 \ q_4 \ right]$$

सामान्यीकृत वेक्टर द्वारा वर्णित रोटेशन की धुरी को जानना $$$\ vec {v} = \ left [v_x \ v_y \ v_z \ right]$ और कोण $$$\ varphi$ (जहां वे ऊपर से आते हैं) आप चतुर्भुज की गणना कर सकते हैं:

$$

$$q_ {v, \ varphi} = \ left [\ cos \ left (\ frac {\ varphi} {2} \ right) \ \ v_x \ sin \ left (\ frac {\ _ varphi} {2 \ _ \ _) \ v_y \ sin \ left (\ frac {\ varphi} {2} \ right) \ \ v_z \ sin \ बाएँ (\ frac {\ varphi} {2} \ right) \ right] \ quad] (*)$$

फिर, चक्र के प्रत्येक पुनरावृत्ति पर केवल गाइरोस्कोप का उपयोग करते हुए, हम अभिव्यक्ति के अनुसार चतुर्धातुक के वर्तमान मूल्य की गणना करेंगे:

$$

$$q_n ^ G = q_ {n-1} q_ {v, d \ varphi}$$

यहां $$$q_n ^ G$ - एक निश्चित समय में चतुर्धातुक। सूचकांक जी शीर्ष पर इंगित करता है कि यह मान गायरो रीडिंग के सापेक्ष मापा जाता है, अर्थात। कोणीय वेग; $$$q_ {n-1}$ - समय में पिछले पल में चतुर्भुज का मूल्य; $$$q_ {v, d \ varphi}$ -एक माप कदम में स्थिति को बदलने, हम एक कोण से स्थिति का असतत परिवर्तन कह सकते हैं $$$d \ varphi$ के लिए $$$dt$ नमूनाकरण अवधि रोटेशन (*) के चतुर्भुज द्वारा व्यक्त की गई है।

गणना में अगला कदम एक्सेलेरोमीटर और मैग्नेटोमीटर सेंसर की एक जोड़ी से डेटा के आधार पर रोटेशन मैट्रिक्स को ढूंढना है। विशेष रूप से, हम गुरुत्वाकर्षण वेक्टर और पृथ्वी के चुंबकीय क्षेत्र के इंडक्शन वेक्टर पर विचार करते हैं, जो ऊपर वर्णित है, पृथ्वी और संबंधित एनएससी के संबंध में स्थिर हैं। यहाँ थीसिस यह है: एलएससी और एनएससी के आधार पर गुरुत्वाकर्षण और प्रेरण के मूल्यों को जानते हुए, हम एनएससी से एलएससी और अंतरिक्ष में रोटेशन के quaternion के लिए संक्रमण मैट्रिक्स (रोटेशन) की गणना कर सकते हैं ।

द्वारा निरूपित करें $$$\ vec {G}$ गुरुत्वाकर्षण के सामान्यीकृत प्रारंभिक वेक्टर। इसे निम्नानुसार लिया जा सकता है:

$$

$$\ vec {G} = {\ left [0 \ 0 \ 1 \ right]} ^ $$$

के माध्यम से $$$\ vec {H}$ सामान्यीकृत प्रारंभिक चुंबकीय प्रेरण वेक्टर को निरूपित करें:

$$

$$\ vec {H} = {\ left [H_x \ H_y \ H_z \ right]} ^ T$$

एक तीसरा वेक्टर भी आवश्यक है। $$$\ vec {K}$ जो इन दोनों के साथ जुड़ा हुआ है। यह वेक्टर गुणन द्वारा प्राप्त किया जाता है $$$\ vec {H}$ पर $$$\ vec {G}$ :

$$

$$\ vec {K} = {\ left [K_x \ K_y \ K_z \ right]} ^ T = \ vec {H} \ टाइम्स \ vec {G} = {\ बाएँ [H_y \ -H_d \ _ \ _]}} ^ $$$

अब मैट्रिक्स बनता है $$$M_0$ जो प्रारंभिक स्थिति की विशेषता है, अर्थात् एनएससी, प्रारंभिक समय में, एलएससी एनएससी के साथ मेल खाता है। मैट्रिक्स वैक्टर के घटकों से प्राप्त किया जाता है $$$\ vec {H}$ । $$$\ vec {G}$ और $$$\ vec {K}$ :

$$

$$M_0 = \ left [\ vec {H} \ \ vec {G} \ \ vec {K} \ right] = \ बाएँ [\ start {array} {cccc} H_x & 0 & H_y \\__y & 0 & -H_x \\ H_z & 1 & 0 \ _ {अंत। सरणी} \ right]$$

इसी तरह, लेकिन पहले से ही लूप के प्रत्येक पुनरावृत्ति पर, एक समान मैट्रिक्स बनाया जाता है $$$M '= \ left [\ vec {h} \ \ vec {g} \ \ vec {k} \ right $$ जहाँ $$$\ vec {h}$ क्या वर्तमान सामान्यीकृत चुंबकीय प्रेरण वेक्टर है, $$$\ vec {g}$ क्या वर्तमान सामान्यीकृत गुरुत्वाकर्षण वेक्टर है, $$$\ vec {k} = \ vec {h} \ टाइम्स \ vec {g}$ । यह मैट्रिक्स LSC की स्थिति की विशेषता है, क्योंकि ये वैक्टर सेंसर की समन्वय प्रणाली में जाने जाते हैं, जो एलएससी के साथ मेल खाता है। फिर, दोनों मैट्रिसेस को जानकर, हम निम्नलिखित समीकरण लिख सकते हैं:

$$

$$E'M '= E_0M_0$$

यह अलग-अलग परिस्थितियों में पाए जाने वाले दो मेट्रिक्स को जोड़ता है। एक अभिव्यक्ति दी $$$E '= CE_0$ और क्या आधार $$$E_0$ क्या पहचान मैट्रिक्स है, हम प्राप्त करते हैं:

$$

$$CM '= M_0$$

यहां से हम संक्रमण मैट्रिक्स को व्यक्त करते हैं $$$सी$ :

$$

$$C = {\ left (M_0 ^ {- 1} \ right)} ^ TM '^ T$$

पहला कारक, जैसा कि आप समझ सकते हैं, एल्गोरिथम की बहुत शुरुआत में केवल एक बार गणना की जाती है, और इस प्रक्रिया में पुनरावृत्ति की आवश्यकता नहीं होती है। दूसरा कारक चक्र के प्रत्येक पुनरावृत्ति पर बहुत आसानी से और जबरन नहीं बनता है।

बाद में, जब संक्रमण मैट्रिक्स पाया जाता है, तो हम इसे एक चतुर्धातुक में बदलते हैं $$$q_n ^ ए$ (एक मैट्रिक्स से एक चतुर्भुज में परिवर्तित करने के लिए एल्गोरिथ्म एक सार्वजनिक चीज है और मैं इसे यहां नहीं दूंगा)।

नतीजतन, हम जानते हैं कि दो चतुर्धातुक एक दूसरे से स्वतंत्र रूप से पाए जाते हैं। और परिणामों को संयोजित करने के लिए, मैंने गुणांक के साथ एक सरल पूरक फ़िल्टर लागू किया $$$एफ$ , जो 0 से 1 तक की सीमा से लिया गया है:

$$

$$q_n = Fq_n ^ G + \ left (1-F \ right) q_n ^ A$$

नतीजतन, हमें एक quaternion मिलता है, जिसमें SINS मॉड्यूल के साथ डिवाइस के स्थान की स्थिति के बारे में अद्यतित जानकारी होती है।

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

ठीक है, हमें जो क्वाटर्नेशन दिया गया है, वह ओरिएंटेशन है। योजना के अनुसार आगे क्या है? और फिर एक्सेलेरोमीटर के अनुसार ऑब्जेक्ट के रैखिक निर्देशांक का अनुसरण करता है। ठीक है, यहाँ केवल यह बताना बेहतर है कि आवश्यक सटीकता के साथ एक्सेलेरोमीटर का उपयोग करके निर्देशांक निर्धारित करना असंभव है। यहां तक ​​कि कम से कम स्थिति में एक परिवर्तन खोजें जो पर्याप्त होगा! बेशक, इस समस्या को हल करने के प्रयास किए गए थे, लेकिन वास्तविक समय में निर्देशांक ढूंढना संभव नहीं था। क्यों? क्योंकि शोर सेंसर रीडिंग के दोहरे असतत एकीकरण ने केवल समताप मंडल में, और एक उन्मत्त गति से उड़ान भरी, लेकिन सकारात्मक परिणाम के लिए नहीं। इसलिए, मैंने पैराग्राफ की शुरुआत में इंगित एल्गोरिथ्म को काट दिया जब तक कि अभिविन्यास नहीं मिला, और मैं वहां रुक गया।

3. सब कुछ

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

मैनिपुलेटर के विकास पर आइटम में, पीओ के वेक्टर को मैन्युअल रूप से सेट किया गया था, और उसके बाद ड्राइव के रोटेशन कोण पाए गए थे। तो, यह वेक्टर हाथ की हथेली के उन्मुखीकरण के आधार पर निर्धारित किया जाना चाहिए। आधार के रूप में अभिविन्यास व्यक्त किया जा सकता है $$$ई ’$ चतुष्कोण को एक मैट्रिक्स में परिवर्तित करके इसे खोजना। आधार में इसके अंगों के स्तंभ वैक्टर होते हैं, इसलिए, हम तीन वैक्टर प्राप्त करते हैं $$$\ vec {x} '$ । $$$\ vec {y} '$ और $$$\ vec {z} ’$ । वेक्टर को अभिविन्यास के रूप में लेना सुविधाजनक होगा $$$\ vec {x} '$ , क्योंकि मैंने मैनिपुलेटर की ऐसी प्रारंभिक स्थिति ग्रहण की थी, जब सभी लिंक और पीओ आधार समन्वय प्रणाली के एक्सजेड विमान में स्थित हैं, और पीओ उस क्षण क्षैतिज है, अर्थात्। वेक्टर $$$\ vec {v_0}$ शुरुआत में, यह बीएसके और एनएससी के एक्स अक्ष के साथ गठबंधन किया गया है (हम एसआईएनएस मॉड्यूल की प्रारंभिक स्थिति की स्थिति रखते हैं ताकि एनएससी मैनिपुलेटर के बीएसके के साथ मेल खाता है)। इस मामले में, मैट्रिक्स का पहला कॉलम $$$ई ’$ सदिश समान $$$\ vec {v_0}$ । फिर हम इसे पीओ की लंबाई के साथ सामान्य करते हैं और मैनिपुलेटर के आधार समन्वय प्रणाली के सापेक्ष आवश्यक वेक्टर प्राप्त करते हैं। फिर हम पी 1 से सामान्यीकृत निर्देशांक की गणना के अनुक्रम का पालन करते हैं।

यह ध्यान देने योग्य है कि हेराफेरी करने वाला, जब आरओ के अभिविन्यास को बदलते हुए, दो मोड में स्थानांतरित करने में सक्षम होता है: पहला - जब निकला हुआ किनारा बिंदु स्थिर होता है, तो दूसरा - जब पीओ का अंतिम बिंदु स्थिर होता है, लेकिन इसका अभिविन्यास मनाया जाता है। दूसरा मोड अधिक दिलचस्प है क्योंकि यहां तक ​​कि रैखिक आंदोलन की अनुपस्थिति में, पीओ के अभिविन्यास का अवलोकन करते हुए, सभी रोटेशन अक्ष अभी भी निकला हुआ किनारा बिंदु को स्थानांतरित करने के लिए शामिल हैं। हालांकि, पहले मोड से दूसरे में संक्रमण वेक्टर को खोजने के लिए वैक्टर को घटाने के केवल एक ऑपरेशन के अलावा होता है $$$\ vec {r}$ (पैरा 1 देखें)।

उपरोक्त सभी ऑपरेशन भी मतलाब वातावरण में प्रोग्राम किए गए थे। सामान्य शब्दों में, कार्यक्रम की संरचना इस प्रकार है:

1. स्टार्टअप पर कैलिब्रेशन। हम पूरे खेत को इसकी मूल स्थिति में सेट करते हैं और बिना किसी निश्चित मात्रा के कच्चे डेटा के लिए SINS मॉड्यूल को छूने, छूने की हिम्मत करते हैं। अंशांकन आवश्यक है, क्योंकि आपको प्रारंभिक ऑफसेट को हटाने की आवश्यकता है जो फ़िल्टर का उत्पादन किया गया (मुझे समझ नहीं आया कि क्यों, और यह ठीक काम किया)।
2. स्क्रिप्ट की शुरुआत। हम सभी पुस्तकालयों और चर को आरंभीकृत करते हैं, पीओ के निकला हुआ किनारा या अंत बिंदु की प्रारंभिक स्थिति निर्धारित करते हैं।
3. साइकिल स्टार्ट। प्रत्येक पुनरावृत्ति पर, हम सेंसर से पूछताछ करते हैं, चतुर्धातुक खोजते हैं, इसे एक मैट्रिक्स में अनुवाद करते हैं, जिसका आधार है $$$ई ’$ , यह आरओ स्थित है, जिसमें आधार है। पहला कॉलम पीओ के अभिविन्यास के रूप में लिया गया है और हम सभी सामान्यीकृत निर्देशांक 1 सेकंड से गणितीय विचारों का उपयोग करते हुए पाते हैं।
4. समय के अंतिम क्षण और वर्तमान में सामान्यीकृत निर्देशांक को जानना, हम कुल्हाड़ियों के रोटेशन के कोणीय वेग की गणना करते हैं।
5. हम जोड़तोड़ के माइक्रोकंट्रोलर को एक डेटा पैकेट भेजते हैं।

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