Επιταχυνσιόμετρο & γυροσκόπιο: 3 βήματα

2024 Συγγραφέας: John Day | [email protected]. Τελευταία τροποποίηση: 2024-01-30 08:39

Εισαγωγή

Αυτός ο οδηγός απευθύνεται σε όλους όσους ενδιαφέρονται να χρησιμοποιήσουν επιταχυνσιόμετρα και γυροσκόπια καθώς και συνδυασμένες συσκευές IMU (μονάδα αδρανειακής μέτρησης) στα ηλεκτρονικά τους έργα

Θα καλύψουμε:

• Τι μετρά ένα επιταχυνσιόμετρο;
• Τι μετρά ένα γυροσκόπιο (γνωστός και ως γυροσκόπιο);
• Πώς να μετατρέψετε αναλογικές σε ψηφιακές (ADC) αναγνώσεις που λαμβάνετε από αυτούς τους αισθητήρες σε φυσικές μονάδες (αυτές θα ήταν g για επιταχυνσιόμετρο, deg/s για γυροσκόπιο)
• Πώς να συνδυάσετε τις μετρήσεις του επιταχυνσιόμετρου και του γυροσκοπίου για να λάβετε ακριβείς πληροφορίες σχετικά με την κλίση της συσκευής σας σε σχέση με το επίπεδο γείωσης

Σε όλο το άρθρο θα προσπαθήσω να κρατήσω τα μαθηματικά στο ελάχιστο. Εάν γνωρίζετε τι είναι το Sine/Cosine/Tangent τότε θα πρέπει να είστε σε θέση να κατανοήσετε και να χρησιμοποιήσετε αυτές τις ιδέες στο έργο σας ανεξάρτητα από την πλατφόρμα που χρησιμοποιείτε: Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC κ.λπ.

Υπάρχουν άνθρωποι εκεί έξω που πιστεύουν ότι χρειάζεστε πολύπλοκα μαθηματικά για να χρησιμοποιήσετε μια μονάδα IMU (σύνθετα φίλτρα FIR ή IIR όπως φίλτρα Kalman, φίλτρα Parks-McClellan κλπ). Μπορείτε να ερευνήσετε όλα αυτά και να επιτύχετε υπέροχα αλλά πολύπλοκα αποτελέσματα. Ο τρόπος που εξηγώ τα πράγματα απαιτούν απλώς βασικά μαθηματικά. Είμαι πολύ πιστός στην απλότητα. Νομίζω ότι ένα απλό σύστημα είναι πιο εύκολο να ελεγχθεί και να παρακολουθείται, εκτός από πολλές ενσωματωμένες συσκευές δεν έχουν τη δύναμη και τους πόρους να εφαρμόσουν πολύπλοκους αλγόριθμους που απαιτούν υπολογισμούς μήτρας.

Θα χρησιμοποιήσω ως παράδειγμα μια νέα μονάδα IMU, το Acc_Gyro Accelerometer + Gyro IMU. Θα χρησιμοποιήσουμε τις παραμέτρους αυτής της συσκευής στα παρακάτω παραδείγματά μας. Αυτή η μονάδα είναι μια καλή συσκευή για αρχή γιατί αποτελείται από 2 συσκευές:

- LIS331AL (φύλλο δεδομένων) - τριαξονικό επιταχυνσιόμετρο 2G - LPR550AL (φύλλο δεδομένων) - βήμα και ρολό διπλού άξονα, γυροσκόπιο 500 deg/sec

Μαζί αντιπροσωπεύουν μια μονάδα αδρανειακής μέτρησης 5 βαθμών ελευθερίας. Τώρα είναι ένα φανταχτερό όνομα! Παρ 'όλα αυτά, πίσω από το φανταχτερό όνομα βρίσκεται μια πολύ χρήσιμη συσκευή συνδυασμού που θα καλύψουμε και θα εξηγήσουμε λεπτομερώς σε αυτόν τον οδηγό.

Βήμα 1: Το επιταχυνσιόμετρο

Για να κατανοήσουμε αυτήν τη μονάδα θα ξεκινήσουμε με το επιταχυνσιόμετρο. Όταν σκέφτεστε για επιταχυνσιόμετρα είναι συχνά χρήσιμο να απεικονίσετε ένα κουτί σε σχήμα κύβου με μια μπάλα μέσα του. Μπορεί να φανταστείτε κάτι άλλο σαν μπισκότο ή ντόνατ, αλλά εγώ φαντάζομαι μια μπάλα:

Εάν πάρουμε αυτό το κουτί σε ένα μέρος χωρίς πεδία βαρύτητας ή για το θέμα αυτό χωρίς άλλα πεδία που μπορεί να επηρεάσουν τη θέση της μπάλας - η μπάλα απλά θα επιπλέει στη μέση του κουτιού. Μπορείτε να φανταστείτε ότι το κιβώτιο βρίσκεται στο διάστημα πολύ μακριά από οποιοδήποτε κοσμικό σώμα, ή αν ένα τέτοιο μέρος είναι δύσκολο να βρεθεί φανταστείτε τουλάχιστον ένα διαστημικό σκάφος που περιφέρεται γύρω από τον πλανήτη όπου όλα βρίσκονται σε κατάσταση βαρύτητας. Από την παραπάνω εικόνα μπορείτε να δείτε ότι αναθέτουμε σε κάθε άξονα ένα ζευγάρι τοίχων (αφαιρέσαμε τον τοίχο Y+ για να κοιτάξουμε μέσα στο κουτί). Φανταστείτε ότι κάθε τοίχος είναι ευαίσθητος στην πίεση. Αν μετακινήσουμε ξαφνικά το κουτί προς τα αριστερά (το επιταχύνουμε με επιτάχυνση 1g = 9,8m/s^2), η μπάλα θα χτυπήσει στον τοίχο X-. Στη συνέχεια μετράμε τη δύναμη πίεσης που εφαρμόζει η σφαίρα στον τοίχο και εξάγουμε μια τιμή -1g στον άξονα Χ.

Λάβετε υπόψη ότι το επιταχυνσιόμετρο θα ανιχνεύσει πραγματικά μια δύναμη που κατευθύνεται προς την αντίθετη κατεύθυνση από το διάνυσμα επιτάχυνσης. Αυτή η δύναμη συχνά ονομάζεται αδρανειακή δύναμη ή πλασματική δύναμη. Ένα πράγμα που πρέπει να μάθετε από αυτό είναι ότι ένα επιταχυνσιόμετρο μετρά την επιτάχυνση έμμεσα μέσω μιας δύναμης που εφαρμόζεται σε έναν από τους τοίχους του (σύμφωνα με το μοντέλο μας, μπορεί να είναι ελατήριο ή κάτι άλλο σε επιταχυνσιόμετρα πραγματικής ζωής). Αυτή η δύναμη μπορεί να προκληθεί από την επιτάχυνση, αλλά όπως θα δούμε στο επόμενο παράδειγμα δεν προκαλείται πάντα από επιτάχυνση.

Αν πάρουμε το μοντέλο μας και το βάλουμε στη Γη, η μπάλα θα πέσει στον τοίχο Ζ και θα ασκήσει δύναμη 1g στον κάτω τοίχο, όπως φαίνεται στην παρακάτω εικόνα:

Σε αυτήν την περίπτωση το κουτί δεν κινείται, αλλά εξακολουθούμε να έχουμε ένδειξη -1g στον άξονα Ζ. Η πίεση που έχει ασκήσει η μπάλα στον τοίχο προκλήθηκε από μια δύναμη βαρύτητας. Θεωρητικά θα μπορούσε να είναι ένας διαφορετικός τύπος δύναμης - για παράδειγμα, αν φανταστείτε ότι η μπάλα μας είναι μεταλλική, η τοποθέτηση ενός μαγνήτη δίπλα στο κουτί θα μπορούσε να μετακινήσει τη μπάλα έτσι ώστε να χτυπήσει έναν άλλο τοίχο. Αυτό ειπώθηκε μόνο για να αποδείξει ότι στην ουσία το επιταχυνσιόμετρο μετρά τη δύναμη και όχι την επιτάχυνση. Απλώς συμβαίνει ότι η επιτάχυνση προκαλεί μια αδρανειακή δύναμη που συλλαμβάνεται από τον μηχανισμό ανίχνευσης δύναμης του επιταχυνσιόμετρου.

Ενώ αυτό το μοντέλο δεν είναι ακριβώς ο τρόπος κατασκευής ενός αισθητήρα MEMS, είναι συχνά χρήσιμο στην επίλυση προβλημάτων που σχετίζονται με το επιταχυνσιόμετρο. Υπάρχουν πράγματι παρόμοιοι αισθητήρες που έχουν μεταλλικές μπάλες στο εσωτερικό τους, ονομάζονται διακόπτες κλίσης, ωστόσο είναι πιο πρωτόγονοι και συνήθως μπορούν να πουν μόνο εάν η συσκευή είναι κεκλιμένη σε κάποιο εύρος ή όχι, όχι η έκταση της κλίσης.

Μέχρι τώρα έχουμε αναλύσει την έξοδο του επιταχυνσιόμετρου σε έναν μόνο άξονα και αυτό είναι το μόνο που θα έχετε με τα επιταχυνσιόμετρα ενός άξονα. Η πραγματική αξία των τριαξονικών επιταχυνσιόμετρων προέρχεται από το γεγονός ότι μπορούν να ανιχνεύσουν αδρανειακές δυνάμεις και στους τρεις άξονες. Ας επιστρέψουμε στο μοντέλο του κουτιού μας και ας περιστρέψουμε το κουτί 45 μοίρες προς τα δεξιά. Η μπάλα θα αγγίξει 2 τοίχους τώρα: Z- και X- όπως φαίνεται στην παρακάτω εικόνα:

Οι τιμές του 0,71 δεν είναι αυθαίρετες, είναι στην πραγματικότητα μια προσέγγιση για το SQRT (1/2). Αυτό θα γίνει πιο ξεκάθαρο καθώς παρουσιάζουμε το επόμενο μοντέλο μας για το επιταχυνσιόμετρο.

Στο προηγούμενο μοντέλο έχουμε σταθεροποιήσει τη δύναμη της βαρύτητας και περιστρέψαμε το φανταστικό μας κουτί. Στα 2 τελευταία παραδείγματα αναλύσαμε την έξοδο σε 2 διαφορετικές θέσεις κουτιού, ενώ το διάνυσμα δύναμης παρέμεινε σταθερό. Αν και αυτό ήταν χρήσιμο για την κατανόηση του τρόπου με τον οποίο το επιταχυνσιόμετρο αλληλεπιδρά με τις εξωτερικές δυνάμεις, είναι πιο πρακτικό να εκτελούμε υπολογισμούς αν στερεώσουμε το σύστημα συντεταγμένων στους άξονες του επιταχυνσιόμετρου και φανταστούμε ότι το διάνυσμα δύναμης περιστρέφεται γύρω μας.

Παρακαλώ ρίξτε μια ματιά στο παραπάνω μοντέλο, διατήρησα τα χρώματα των αξόνων για να κάνετε μια νοητική μετάβαση από το προηγούμενο μοντέλο στο νέο. Απλώς φανταστείτε ότι κάθε άξονας στο νέο μοντέλο είναι κάθετος στις αντίστοιχες όψεις του κουτιού στο προηγούμενο μοντέλο. Το διάνυσμα R είναι το διάνυσμα δύναμης που μετρά το επιταχυνσιόμετρο (θα μπορούσε να είναι είτε η δύναμη της βαρύτητας είτε η αδρανειακή δύναμη από τα παραπάνω παραδείγματα ή ένας συνδυασμός και των δύο). Τα Rx, Ry, Rz είναι προβολή του διανύσματος R στους άξονες X, Y, Z. Παρακαλώ προσέξτε την ακόλουθη σχέση:

R^2 = Rx^2 + Ry^2 + Rz^2 (εξ. 1)

που είναι βασικά το ισοδύναμο του Πυθαγόρειου θεωρήματος σε 3D.

Θυμηθείτε ότι λίγο νωρίτερα σας είπα ότι οι τιμές του SQRT (1/2) ~ 0,71 δεν είναι τυχαίες. Εάν τα συνδέσετε στον παραπάνω τύπο, αφού θυμηθείτε ότι η δύναμη της βαρύτητάς μας ήταν 1 g, μπορούμε να επαληθεύσουμε ότι:

1^2 = (-SQRT (1/2))^2 + 0^2 + (-SQRT (1/2))^2

απλά αντικαθιστώντας R = 1, Rx = -SQRT (1/2), Ry = 0, Rz = -SQRT (1/2) στην εξίσωση 1

Μετά από ένα μακροσκελές προοίμιο θεωρίας πλησιάζουμε στα επιταχυνσιόμετρα της πραγματικής ζωής. Οι τιμές Rx, Ry, Rz σχετίζονται στην πραγματικότητα γραμμικά με τις τιμές που θα βγάλει το επιταχυνσιόμετρο πραγματικής ζωής και που μπορείτε να χρησιμοποιήσετε για την εκτέλεση διαφόρων υπολογισμών.

Πριν φτάσουμε εκεί, ας μιλήσουμε λίγο για τον τρόπο με τον οποίο τα επιταχυνσιόμετρα θα μας παραδώσουν αυτές τις πληροφορίες. Τα περισσότερα επιταχυνσιόμετρα χωρίζονται σε δύο κατηγορίες: ψηφιακά και αναλογικά. Τα ψηφιακά επιταχυνσιόμετρα θα σας δώσουν πληροφορίες χρησιμοποιώντας ένα σειριακό πρωτόκολλο όπως το I2C, SPI ή το USART, ενώ τα αναλογικά επιταχυνσιόμετρα θα εξάγουν ένα επίπεδο τάσης εντός ενός προκαθορισμένου εύρους που πρέπει να μετατρέψετε σε ψηφιακή τιμή χρησιμοποιώντας μια μονάδα ADC (αναλογικό σε ψηφιακό μετατροπέα). Δεν θα μπω σε πολλές λεπτομέρειες για το πώς λειτουργεί το ADC, εν μέρει επειδή είναι τόσο εκτενές θέμα και εν μέρει επειδή διαφέρει από τη μια πλατφόρμα στην άλλη. Ορισμένοι μικροελεγκτές θα έχουν ενσωματωμένες μονάδες ADC, ορισμένες από αυτές θα χρειάζονται εξωτερικά στοιχεία για να εκτελέσουν τις μετατροπές ADC. Ανεξάρτητα από τον τύπο μονάδας ADC που χρησιμοποιείτε, θα καταλήξετε με μια τιμή σε ένα συγκεκριμένο εύρος. Για παράδειγμα, μια μονάδα ADC 10 -bit θα εξάγει μια τιμή στην περιοχή 0..1023, σημειώστε ότι 1023 = 2^10 -1. Μια μονάδα ADC 12-bit θα εξάγει μια τιμή στην περιοχή 0..4095, σημειώστε ότι 4095 = 2^12-1.

Ας προχωρήσουμε εξετάζοντας ένα απλό παράδειγμα, ας υποθέσουμε ότι η μονάδα ADC 10bit μας έδωσε τις ακόλουθες τιμές για τα τρία κανάλια επιταχυνσιόμετρων (άξονες):

AdcRx = 586 AdcRy = 630 AdcRz = 561

Κάθε μονάδα ADC θα έχει τάση αναφοράς, ας υποθέσουμε ότι στο παράδειγμά μας είναι 3,3V. Για να μετατρέψουμε μια τιμή ad 10bit σε τάση, χρησιμοποιούμε τον ακόλουθο τύπο:

VoltsRx = AdcRx * Vref / 1023

Μια γρήγορη σημείωση εδώ: ότι για 8bit ADC το τελευταίο διαχωριστικό θα ήταν 255 = 2 ^ 8 -1 και για 12bit ADC το τελευταίο διαχωριστικό θα ήταν 4095 = 2 ^ 12 -1.

Εφαρμόζοντας αυτόν τον τύπο και στα 3 κανάλια λαμβάνουμε:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (στρογγυλοποιούμε όλα τα αποτελέσματα σε 2 δεκαδικά ψηφία) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = ~ 1.81V

Κάθε επιταχυνσιόμετρο έχει επίπεδο τάσης μηδέν-g, μπορείτε να το βρείτε στις προδιαγραφές, αυτή είναι η τάση που αντιστοιχεί σε 0g. Για να λάβουμε μια υπογεγραμμένη τιμή τάσης πρέπει να υπολογίσουμε τη μετατόπιση από αυτό το επίπεδο. Ας πούμε ότι το επίπεδο τάσης 0g είναι VzeroG = 1.65V. Υπολογίζουμε τις μετατοπίσεις τάσης από την τάση μηδέν-g ως εξής:

DeltaVoltsRx = 1.89V - 1.65V = 0.24V DeltaVoltsRy = 2.03V - 1.65V = 0.38V DeltaVoltsRz = 1.81V - 1.65V = 0.16V

Έχουμε τώρα τις ενδείξεις του επιταχυνσιόμετρου μας σε Volt, δεν είναι ακόμα σε g (9,8 m/s^2), για να κάνουμε την τελική μετατροπή εφαρμόζουμε την ευαισθησία του επιταχυνσιόμετρου, συνήθως εκφρασμένη σε mV/g. Ας πούμε ότι η ευαισθησία μας = 478.5mV/g = 0.4785V/g. Τιμές ευαισθησίας μπορούν να βρεθούν στις προδιαγραφές του επιταχυνσιόμετρου. Για να λάβουμε τις τελικές τιμές δύναμης που εκφράζονται σε g χρησιμοποιούμε τον ακόλουθο τύπο:

Rx = DeltaVoltsRx / Ευαισθησία

Rx = 0.24V / 0.4785V / g = ~ 0.5g Ry = 0.38V / 0.4785V / g = ~ 0.79g Rz = 0.16V / 0.4785V / g = ~ 0.33g

Θα μπορούσαμε φυσικά να συνδυάσουμε όλα τα βήματα σε έναν τύπο, αλλά πέρασα από όλα τα βήματα για να καταστήσω σαφές πώς πηγαίνετε από τις αναγνώσεις ADC σε ένα στοιχείο διανυσματικής δύναμης που εκφράζεται σε g.

Rx = (AdcRx * Vref / 1023 - VzeroG) / Sensitivity (Eq.2) Ry = (AdcRy * Vref / 1023 - VzeroG) / Sensitivity Rz = (AdcRz * Vref / 1023 - VzeroG) / Sensitivity

Έχουμε τώρα και τα 3 στοιχεία που καθορίζουν το διάνυσμα αδρανειακής δύναμης, εάν η συσκευή δεν υπόκειται σε άλλες δυνάμεις εκτός από τη βαρύτητα, μπορούμε να υποθέσουμε ότι αυτή είναι η κατεύθυνση του διανύσματος της δύναμης της βαρύτητάς μας. Εάν θέλετε να υπολογίσετε την κλίση της συσκευής σε σχέση με το έδαφος, μπορείτε να υπολογίσετε τη γωνία μεταξύ αυτού του διανύσματος και του άξονα Ζ. Εάν ενδιαφέρεστε επίσης για την κατεύθυνση κλίσης ανά άξονα, μπορείτε να χωρίσετε αυτό το αποτέλεσμα σε 2 συστατικά: κλίση στον άξονα Χ και Υ που μπορεί να υπολογιστεί ως η γωνία μεταξύ του διανύσματος βαρύτητας και των αξόνων Χ / Υ. Ο υπολογισμός αυτών των γωνιών είναι πιο απλός από όσο νομίζετε, τώρα που έχουμε υπολογίσει τις τιμές για Rx, Ry και Rz. Ας επιστρέψουμε στο τελευταίο μοντέλο επιταχυνσιόμετρου και κάνουμε μερικές πρόσθετες σημειώσεις:

Οι γωνίες που μας ενδιαφέρουν είναι οι γωνίες μεταξύ των αξόνων Χ, Υ, Ζ και του διανύσματος δύναμης R. Θα ορίσουμε αυτές τις γωνίες ως Axr, Ayr, Azr. Μπορείτε να παρατηρήσετε από το τρίγωνο ορθογώνιας γωνίας που σχηματίζεται από R και Rx ότι:

cos (Axr) = Rx / R, και ομοίως: cos (Ayr) = Ry / R cos (Azr) = Rz / R

Μπορούμε να αφαιρέσουμε από την εξίσωση 1 ότι R = SQRT (Rx^2 + Ry^2 + Rz^2).

Μπορούμε να βρούμε τώρα τις γωνίες μας χρησιμοποιώντας τη συνάρτηση arccos () (η αντίστροφη συνάρτηση cos ()):

Axr = arccos (Rx/R) Ayr = arccos (Ry/R) Azr = arccos (Rz/R)

Έχουμε διανύσει πολύ δρόμο για να εξηγήσουμε το μοντέλο του επιταχυνσιόμετρου, απλώς για να καταλήξουμε σε αυτούς τους τύπους. Ανάλογα με τις εφαρμογές σας, μπορεί να θέλετε να χρησιμοποιήσετε τυχόν ενδιάμεσους τύπους που έχουμε παράγει. Θα εισαγάγουμε επίσης σύντομα το μοντέλο γυροσκοπίου και θα δούμε πώς μπορούν να συνδυαστούν τα δεδομένα του επιταχυνσιόμετρου και του γυροσκοπίου για να παρέχουν ακόμη πιο ακριβείς εκτιμήσεις κλίσης.

Αλλά πριν το κάνουμε αυτό, ας κάνουμε μερικές πιο χρήσιμες σημειώσεις:

cosX = cos (Axr) = Rx / R cozy = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

Αυτό το τρίδυμο ονομάζεται συχνά Direction Cosine και αντιπροσωπεύει βασικά το μοναδιαίο διάνυσμα (διάνυσμα με μήκος 1) που έχει την ίδια κατεύθυνση με το διάνυσμά μας R. Μπορείτε εύκολα να επαληθεύσετε ότι:

SQRT (cosX^2 + cozy^2 + cosZ^2) = 1

Αυτή είναι μια ωραία ιδιότητα αφού μας απαλλάσσει από την παρακολούθηση του μέτρου (μήκος) του διανύσματος R. Συχνά, αν μας ενδιαφέρει μόνο η κατεύθυνση του αδρανειακού μας διανύσματος, είναι λογικό να ομαλοποιήσουμε το μέτρο του, ώστε να απλοποιήσουμε άλλους υπολογισμούς.

Βήμα 2: Γυροσκόπιο

Δεν πρόκειται να εισαγάγουμε κανένα αντίστοιχο μοντέλο κουτιού για το γυροσκόπιο όπως κάναμε για το επιταχυνσιόμετρο, αντίθετα θα μεταβούμε κατευθείαν στο δεύτερο μοντέλο επιταχυνσιόμετρου και θα δείξουμε τι μετρά το γυροσκόπιο σύμφωνα με αυτό το μοντέλο.

Κάθε κανάλι γυροσκοπίου μετρά την περιστροφή γύρω από έναν από τους άξονες. Για παράδειγμα, ένα γυροσκόπιο 2 αξόνων θα μετρήσει την περιστροφή γύρω από τους άξονες Χ και Υ (ή μπορεί να πει "περίπου"). Για να εκφράσουμε αυτήν την περιστροφή σε αριθμούς ας κάνουμε μερικές σημειώσεις. Αρχικά ας ορίσουμε:

Rxz - είναι η προβολή του διανύσματος αδρανειακής δύναμης R στο επίπεδο XZ Ryz - είναι η προβολή του διανύσματος αδρανειακής δύναμης R στο επίπεδο ΥΖ

Από το ορθογώνιο τρίγωνο που σχηματίζεται από Rxz και Rz, χρησιμοποιώντας το Πυθαγόρειο θεώρημα παίρνουμε:

Rxz^2 = Rx^2 + Rz^2, και ομοίως: Ryz^2 = Ry^2 + Rz^2

σημειώστε επίσης ότι:

R^2 = Rxz^2 + Ry^2, αυτό μπορεί να προέρχεται από εξισώσεις 1 και άνω, ή μπορεί να προέρχεται από τρίγωνο ορθογώνιας γωνίας που σχηματίζεται από R και Ryz R^2 = Ryz^2 + Rx^2

Δεν πρόκειται να χρησιμοποιήσουμε αυτούς τους τύπους σε αυτό το άρθρο, αλλά είναι χρήσιμο να σημειώσουμε τη σχέση μεταξύ όλων των τιμών στο μοντέλο μας.

Αντ 'αυτού θα ορίσουμε τη γωνία μεταξύ του άξονα Ζ και του Rxz, των διανυσμάτων Ryz ως εξής:

Axz - είναι η γωνία μεταξύ του Rxz (προβολή του R στο επίπεδο XZ) και του άξονα Z Ayz - είναι η γωνία μεταξύ του Ryz (προβολή του R στο επίπεδο YZ) και του άξονα Z

Τώρα πλησιάζουμε σε αυτό που μετρά το γυροσκόπιο. Το γυροσκόπιο μετρά το ρυθμό μεταβολών των γωνιών που ορίζονται παραπάνω. Με άλλα λόγια, θα παράγει μια τιμή που σχετίζεται γραμμικά με το ρυθμό μεταβολής αυτών των γωνιών. Για να το εξηγήσουμε αυτό ας υποθέσουμε ότι έχουμε μετρήσει τη γωνία περιστροφής γύρω από τον άξονα Y (που θα ήταν γωνία Axz) τη στιγμή t0 και την ορίζουμε ως Axz0, στη συνέχεια μετρήσαμε αυτήν τη γωνία σε μεταγενέστερο χρόνο t1 και ήταν Axz1. Ο ρυθμός μεταβολής υπολογίζεται ως εξής:

RateAxz = (Axz1 - Axz0) / (t1 - t0).

Εάν εκφράσουμε το Axz σε μοίρες και τον χρόνο σε δευτερόλεπτα, τότε αυτή η τιμή θα εκφραστεί σε βαθμούς/δευτερόλεπτα. Αυτό μετράει ένα γυροσκόπιο.

Στην πράξη ένα γυροσκόπιο (εκτός αν πρόκειται για ειδικό ψηφιακό γυροσκόπιο) σπάνια θα σας δώσει μια τιμή εκφρασμένη σε βαθμούς/δευτερόλεπτα. Όπως και για το επιταχυνσιόμετρο, θα λάβετε μια τιμή ADC που θα χρειαστεί να μετατρέψετε σε deg/s χρησιμοποιώντας έναν τύπο παρόμοιο με τον εξίσωση. 2 που έχουμε ορίσει για το επιταχυνσιόμετρο. Ας εισαγάγουμε τον τύπο μετατροπής ADC σε deg/s για γυροσκόπιο (υποθέτουμε ότι χρησιμοποιούμε μονάδα ADC 10bit, για ADC 8bit αντικαθιστούμε 1023 με 255, για 12bit ADC αντικαθιστούμε 1023 με 4095).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Eensibility Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / Ευαισθησία

AdcGyroXZ, AdcGyroYZ - λαμβάνονται από τη μονάδα adc μας και αντιπροσωπεύουν τα κανάλια που μετρούν την περιστροφή της προβολής του διανύσματος R στο XZ αντίστοιχα σε επίπεδα YZ, η οποία ισοδυναμεί με την περιστροφή γύρω από τους άξονες Y και X αντίστοιχα.

Vref - είναι η τάση αναφοράς ADC που θα χρησιμοποιήσουμε 3.3V στο παρακάτω παράδειγμα VzeroRate - είναι η τάση μηδενικού ρυθμού, δηλαδή η τάση που εξάγει το γυροσκόπιο όταν δεν υπόκειται σε καμία περιστροφή, για τον πίνακα Acc_Gyro είναι για παράδειγμα 1,23V (μπορείτε να βρείτε αυτές τις τιμές στις προδιαγραφές) Ευαισθησία - είναι η ευαισθησία του γυροσκοπίου σας εκφράζεται σε mV / (deg / s) που συχνά γράφεται ως mV / deg / s, βασικά σας λέει πόσα mV θα η έξοδος του γυροσκοπίου αυξάνεται, εάν αυξήσετε την ταχύτητα περιστροφής κατά ένα deg/s. Η ευαισθησία της πλακέτας Acc_Gyro είναι για παράδειγμα 2mV/deg/s ή 0,002V/deg/s

Ας πάρουμε ένα παράδειγμα, ας υποθέσουμε ότι η μονάδα ADC επέστρεψε τις ακόλουθες τιμές:

AdcGyroXZ = 571 AdcGyroXZ = 323

Χρησιμοποιώντας τον παραπάνω τύπο και χρησιμοποιώντας τις παραμέτρους προδιαγραφών του πίνακα Acc_Gyro θα λάβουμε:

RateAxz = (571 * 3.3V/1023 - 1.23V)/(0.002V/deg/s) = ~ 306 deg/s RateAyz = (323 * 3.3V/1023 - 1.23V)/(0.002V/deg/s) = ~ -94 βαθμούς/δευτ

Με άλλα λόγια, η συσκευή περιστρέφεται γύρω από τον άξονα Υ (ή μπορούμε να πούμε ότι περιστρέφεται στο επίπεδο XZ) με ταχύτητα 306 βαθμών/δευτερόλεπτο και γύρω από τον άξονα Χ (ή μπορούμε να πούμε ότι περιστρέφεται σε επίπεδο ΥΖ) με ταχύτητα - 94 βαθμοί/δευτ. Λάβετε υπόψη ότι το αρνητικό πρόσημο σημαίνει ότι η συσκευή περιστρέφεται προς την αντίθετη κατεύθυνση από τη συμβατική θετική κατεύθυνση. Κατά σύμβαση, μία κατεύθυνση περιστροφής είναι θετική. Ένα καλό φύλλο προδιαγραφών γυροσκοπίου θα σας δείξει ποια κατεύθυνση είναι θετική, διαφορετικά θα πρέπει να τη βρείτε πειραματιζόμενοι με τη συσκευή και σημειώνοντας ποια κατεύθυνση περιστροφής έχει ως αποτέλεσμα την αύξηση της τάσης στον πείρο εξόδου. Αυτό γίνεται καλύτερα χρησιμοποιώντας έναν παλμογράφο, καθώς μόλις σταματήσετε την περιστροφή, η τάση θα πέσει πίσω στο επίπεδο μηδενικού ρυθμού. Εάν χρησιμοποιείτε ένα πολύμετρο, θα πρέπει να διατηρήσετε έναν σταθερό ρυθμό περιστροφής για τουλάχιστον μερικά δευτερόλεπτα και να σημειώσετε την τάση κατά τη διάρκεια αυτής της περιστροφής και, στη συνέχεια, να το συγκρίνετε με την τάση μηδενικού ρυθμού. Εάν είναι μεγαλύτερη από την τάση μηδενικού ρυθμού σημαίνει ότι η κατεύθυνση περιστροφής είναι θετική.

Βήμα 3: Συνδυάζοντας το επιταχυνσιόμετρο και το γυροσκόπιο

Συνδυάζοντας τα όλα μαζί - Συνδυάζοντας δεδομένα επιταχυνσιόμετρου και γυροσκοπίου

Αν διαβάζετε αυτό το άρθρο πιθανότατα αποκτήσατε ή σχεδιάζετε να αποκτήσετε μια συσκευή IMU ή πιθανώς σχεδιάζετε να φτιάξετε μια από ξεχωριστές συσκευές επιταχυνσιόμετρου και γυροσκοπίου.

Το πρώτο βήμα στη χρήση μιας συνδυασμένης συσκευής IMU που συνδυάζει ένα επιταχυνσιόμετρο και ένα γυροσκόπιο είναι η ευθυγράμμιση των συστημάτων συντεταγμένων τους. Ο ευκολότερος τρόπος για να το κάνετε είναι να επιλέξετε το σύστημα συντεταγμένων του επιταχυνσιόμετρου ως σύστημα συντεταγμένων αναφοράς. Τα περισσότερα φύλλα δεδομένων επιταχυνσιόμετρου θα εμφανίζουν την κατεύθυνση των αξόνων Χ, Υ, Ζ σε σχέση με την εικόνα του φυσικού τσιπ ή της συσκευής. Για παράδειγμα, εδώ είναι οι κατευθύνσεις των αξόνων Χ, Υ, Ζ όπως φαίνεται στις προδιαγραφές για τον πίνακα Acc_Gyro:

Τα επόμενα βήματα είναι:

Προσδιορίστε τις εξόδους γυροσκοπίου που αντιστοιχούν στις τιμές RateAxz, RateAyz που συζητήθηκαν παραπάνω. Καθορίστε εάν αυτές οι έξοδοι πρέπει να αναστραφούν λόγω της φυσικής θέσης του γυροσκοπίου σε σχέση με το επιταχυνσιόμετρο

Μην υποθέσετε ότι εάν ένα γυροσκόπιο έχει έξοδο με σήμανση X ή Y, θα αντιστοιχεί σε οποιονδήποτε άξονα στο σύστημα συντεταγμένων του επιταχυνσιόμετρου, ακόμη και αν αυτή η έξοδος είναι μέρος μιας μονάδας IMU. Ο καλύτερος τρόπος είναι να το δοκιμάσετε. Υποθέτοντας ότι έχετε καθορίσει τη θέση του γυροσκοπίου σε σχέση με το επιταχυνσιόμετρο. Θεωρείται ότι τα περιγράμματα γυροσκόπιο και επιταχυνσιόμετρο είναι παράλληλα μεταξύ τους, δηλαδή τοποθετείτε το γυροσκόπιο σε γωνία πολλαπλάσια των 90 βαθμών σε σχέση με το τσιπ επιταχυνσιόμετρου. Εάν αποκτήσατε έναν πίνακα IMU, οι πιθανότητες είναι ότι είναι ήδη ευθυγραμμισμένες με αυτόν τον τρόπο. Δεν πρόκειται να συζητήσουμε σε αυτό το άρθρο μοντέλα όπου το γυροσκόπιο τοποθετείται σε ακανόνιστη γωνία σε σχέση με το επιταχυνσιόμετρο (ας πούμε 45 ή 30 μοίρες), αν και αυτό μπορεί να είναι χρήσιμο σε ορισμένες εφαρμογές.

Ακολουθεί μια ακολουθία δειγμάτων για να καθοριστεί ποια έξοδος γυροσκοπίου αντιστοιχεί στην τιμή RateAxz που συζητήθηκε παραπάνω.

- ξεκινήστε από την τοποθέτηση της συσκευής σε οριζόντια θέση. Τόσο οι έξοδοι X όσο και Y του επιταχυνσιόμετρου θα εξάγουν την τάση μηδενικού g (για παράδειγμα για τον πίνακα Acc_Gyro αυτό είναι 1,65V)

- επόμενο ξεκίνημα περιστροφής της συσκευής γύρω από τον άξονα Υ, ένας άλλος τρόπος για να το πείτε είναι ότι περιστρέφετε τη συσκευή σε επίπεδο XZ, έτσι ώστε οι έξοδοι του επιταχυνσιόμετρου Χ και Ζ να αλλάζουν και η έξοδος Υ να παραμένει σταθερή. - ενώ περιστρέφετε τη συσκευή με σταθερή σημείωση ταχύτητας την οποία αλλάζει η έξοδος γυροσκοπίου, οι άλλες έξοδοι γυροσκοπίου πρέπει να παραμένουν σταθερές - η έξοδος γυροσκοπίου που άλλαξε κατά την περιστροφή γύρω από τον άξονα Υ (περιστροφή στο επίπεδο XZ) θα παρέχει την τιμή εισόδου για το AdcGyroXZ, από το οποίο υπολογίζουμε το RateAxz - το τελευταίο βήμα είναι να διασφαλίσουμε ότι η κατεύθυνση περιστροφής αντιστοιχεί στο μοντέλο μας, σε ορισμένες περιπτώσεις μπορεί να χρειαστεί να αντιστρέψετε την τιμή RateAxz λόγω της φυσικής θέσης του γυροσκοπίου σε σχέση με το επιταχυνσιόμετρο - εκτελέστε ξανά την παραπάνω δοκιμή, περιστρέφοντας τη συσκευή γύρω τον άξονα Υ, αυτή τη φορά παρακολουθήστε την έξοδο Χ του επιταχυνσιόμετρου (AdcRx στο μοντέλο μας). Εάν το AdcRx μεγαλώσει (οι πρώτοι 90 μοίρες περιστροφής από την οριζόντια θέση), τότε το AdcGyroXZ πρέπει επίσης να αναπτυχθεί. Διαφορετικά, πρέπει να αντιστρέψετε το RateAxz, μπορείτε να το επιτύχετε εισάγοντας έναν συντελεστή πρόσημου στην εξίσωση 3, ως εξής:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / Ευαισθησία, όπου το InvertAxz είναι 1 ή -1

το ίδιο δοκιμαστικό μπαστούνι μπορεί να γίνει για το RateAyz, περιστρέφοντας τη συσκευή γύρω από τον άξονα Χ και μπορείτε να προσδιορίσετε ποια έξοδο γυροσκοπίου αντιστοιχεί στο RateAyz και εάν πρέπει να αναστραφεί. Μόλις έχετε την τιμή για το InvertAyz, θα πρέπει να χρησιμοποιήσετε τον ακόλουθο τύπο για να υπολογίσετε το RateAyz:

RateAyz = InvertAyz * (AdcGyroYZ * Vref / 1023 - VzeroRate) / Ευαισθησία

Εάν κάνατε αυτές τις δοκιμές στον πίνακα Acc_Gyro θα λάβετε τα ακόλουθα αποτελέσματα:

- ο πείρος εξόδου για το RateAxz είναι GX4 και InvertAxz = -1. - ο πείρος εξόδου για το RateAyz είναι GY4 και InvertAyz = -1

Από αυτό το σημείο και μετά θα θεωρήσουμε ότι έχετε ρυθμίσει το IMU με τέτοιο τρόπο ώστε να μπορείτε να υπολογίσετε τις σωστές τιμές για Axr, Ayr, Azr (όπως ορίζεται στο Μέρος 1. Επιταχυνσιόμετρο) και RateAxz, RateAyz (όπως ορίζεται στο Μέρος 2. Γυροσκόπιο). Στη συνέχεια θα αναλύσουμε τις σχέσεις μεταξύ αυτών των τιμών που αποδεικνύονται χρήσιμες για την ακριβέστερη εκτίμηση της κλίσης της συσκευής σε σχέση με το επίπεδο γείωσης.

Mightσως αναρωτιέστε σε αυτό το σημείο, εάν το μοντέλο επιταχυνσιόμετρου μας έδωσε ήδη γωνίες κλίσης των Axr, Ayr, Azr γιατί θα θέλαμε να ασχοληθούμε με τα δεδομένα του γυροσκοπίου; Η απάντηση είναι απλή: τα δεδομένα του επιταχυνσιόμετρου δεν είναι πάντα αξιόπιστα 100%. Υπάρχουν διάφοροι λόγοι, θυμηθείτε ότι το επιταχυνσιόμετρο μετρά την αδρανειακή δύναμη, μια τέτοια δύναμη μπορεί να προκληθεί από τη βαρύτητα (και ιδανικά μόνο από τη βαρύτητα), αλλά μπορεί επίσης να προκληθεί από επιτάχυνση (κίνηση) της συσκευής. Ως αποτέλεσμα, ακόμη και αν το επιταχυνσιόμετρο βρίσκεται σε σχετικά σταθερή κατάσταση, είναι ακόμα πολύ ευαίσθητο στους κραδασμούς και τον μηχανικό θόρυβο γενικά. Αυτός είναι ο κύριος λόγος για τον οποίο τα περισσότερα συστήματα IMU χρησιμοποιούν γυροσκόπιο για να εξομαλύνουν τυχόν σφάλματα του επιταχυνσιόμετρου. Πώς γίνεται όμως αυτό; Και το γυροσκόπιο είναι απαλλαγμένο από θόρυβο;

Το γυροσκόπιο δεν είναι απαλλαγμένο από θόρυβο ωστόσο επειδή μετρά την περιστροφή είναι λιγότερο ευαίσθητο στις γραμμικές μηχανικές κινήσεις, τον τύπο θορύβου από το οποίο επιταχύνεται το επιταχυνσιόμετρο, ωστόσο τα γυροσκόπια έχουν άλλους τύπους προβλημάτων, όπως π.χ. όταν σταματήσει η περιστροφή). Ωστόσο, με τη μέση τιμή των δεδομένων που προέρχονται από το επιταχυνσιόμετρο και το γυροσκόπιο, μπορούμε να λάβουμε μια σχετικά καλύτερη εκτίμηση της τρέχουσας κλίσης της συσκευής από ό, τι θα λάβαμε χρησιμοποιώντας μόνο τα δεδομένα του επιταχυνσιόμετρου.

Στα επόμενα βήματα θα εισαγάγω έναν αλγόριθμο που εμπνεύστηκε από μερικές ιδέες που χρησιμοποιήθηκαν στο φίλτρο Kalman, ωστόσο είναι πολύ πιο απλό και ευκολότερο να εφαρμοστεί σε ενσωματωμένες συσκευές. Πριν από αυτό ας δούμε πρώτα τι θέλουμε να υπολογίσει ο αλγόριθμός μας. Λοιπόν, είναι η κατεύθυνση του διανύσματος δύναμης βαρύτητας R = [Rx, Ry, Rz] από την οποία μπορούμε να αντλήσουμε άλλες τιμές όπως Axr, Ayr, Azr ή cosX, cozy, cosZ που θα μας δώσει μια ιδέα για την κλίση της συσκευής μας σε σχέση με το επίπεδο γείωσης, συζητάμε τη σχέση μεταξύ αυτών των τιμών στο Μέρος 1. Θα μπορούσε κανείς να πει - δεν έχουμε ήδη αυτές τις τιμές Rx, Ry, Rz από την Εξίσωση 2 στο Μέρος 1; Λοιπόν ναι, αλλά να θυμάστε ότι αυτές οι τιμές προέρχονται από δεδομένα επιταχυνσιόμετρου μόνο, οπότε αν θέλετε να τις χρησιμοποιήσετε απευθείας στην εφαρμογή σας, μπορεί να έχετε περισσότερο θόρυβο από ό, τι μπορεί να ανεχτεί η εφαρμογή σας. Για να αποφύγουμε περαιτέρω σύγχυση, ας επαναπροσδιορίσουμε τις μετρήσεις του επιταχυνσιόμετρου ως εξής:

Racc - είναι το διάνυσμα αδρανειακής δύναμης όπως μετράται με επιταχυνσιόμετρο, το οποίο αποτελείται από τα ακόλουθα στοιχεία (προβολές σε άξονες Χ, Υ, Ζ):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / Sensitivity RyAcc = (AdcRy * Vref / 1023 - VzeroG) / Sensitivity RzAcc = (AdcRz * Vref / 1023 - VzeroG) / Ευαισθησία

Μέχρι στιγμής έχουμε ένα σύνολο μετρημένων τιμών που μπορούμε να λάβουμε καθαρά από τιμές ADC επιταχυνσιόμετρου. Θα ονομάσουμε αυτό το σύνολο δεδομένων "διάνυσμα" και θα χρησιμοποιήσουμε την ακόλουθη σημειολογία.

Racc = [RxAcc, RyAcc, RzAcc]

Επειδή αυτά τα στοιχεία του Racc μπορούν να ληφθούν από δεδομένα επιταχυνσιόμετρου, μπορούμε να το θεωρήσουμε μια είσοδο στον αλγόριθμό μας.

Λάβετε υπόψη ότι επειδή η Racc μετρά τη δύναμη της βαρύτητας, θα έχετε δίκιο εάν υποθέσετε ότι το μήκος αυτού του διανύσματος που ορίζεται ως εξής είναι ίσο ή κοντά στο 1g.

| Racc | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), Ωστόσο, για να είστε σίγουροι ότι έχει νόημα να ενημερώσετε αυτό το διάνυσμα ως εξής:

Racc (κανονικοποιημένο) = [RxAcc/| Racc |, RyAcc/| Racc |, RzAcc/| Racc |].

Αυτό θα διασφαλίσει ότι το μήκος του κανονικοποιημένου διανύσματος Racc είναι πάντα 1.

Στη συνέχεια θα παρουσιάσουμε ένα νέο διάνυσμα και θα το ονομάσουμε

Υπόλοιπο = [RxEst, RyEst, RzEst]

Αυτή θα είναι η έξοδος του αλγορίθμου μας, αυτές είναι διορθωμένες τιμές βάσει δεδομένων γυροσκοπίου και βάσει προηγούμενων εκτιμώμενων δεδομένων.

Δείτε τι θα κάνει ο αλγόριθμός μας: βγάζουμε ένα νέο εκτιμώμενο διάνυσμα Rest. - θεωρούμε ότι το Rest είναι το "καλύτερο στοίχημά" μας για την τρέχουσα θέση της συσκευής.

Ας δούμε πώς μπορούμε να το κάνουμε να λειτουργήσει.

Θα ξεκινήσουμε την ακολουθία μας εμπιστευόμενοι το επιταχυνσιόμετρο και αναθέτοντας:

Ξεκούραση (0) = Racc (0)

Παρεμπιπτόντως, θυμηθείτε το Rest και το Racc είναι διανύσματα, οπότε η παραπάνω εξίσωση είναι απλώς ένας απλός τρόπος για να γράψετε 3 σύνολα εξισώσεων και να αποφύγετε την επανάληψη:

RxEst (0) = RxAcc (0) RyEst (0) = RyAcc (0) RzEst (0) = RzAcc (0)

Στη συνέχεια θα κάνουμε τακτικές μετρήσεις σε ίσα χρονικά διαστήματα Τ δευτερολέπτων και θα λάβουμε νέες μετρήσεις που θα ορίσουμε ως Racc (1), Racc (2), Racc (3) και ούτω καθεξής. Θα εκδίδουμε επίσης νέες εκτιμήσεις ανά χρονικό διάστημα Ανάπαυση (1), Υπόλοιπο (2), Υπόλοιπο (3) και ούτω καθεξής.

Ας υποθέσουμε ότι βρισκόμαστε στο βήμα n. Έχουμε δύο γνωστά σύνολα αξιών που θα θέλαμε να χρησιμοποιήσουμε:

Rest (n -1) - η προηγούμενη εκτίμησή μας, με Rest (0) = Racc (0) Racc (n) - η τρέχουσα μέτρηση του επιταχυνσιόμετρου

Πριν μπορέσουμε να υπολογίσουμε το υπόλοιπο (n), ας εισάγουμε μια νέα μετρημένη τιμή, την οποία μπορούμε να λάβουμε από το γυροσκόπιο μας και μια προηγούμενη εκτίμηση.

Θα το ονομάσουμε Rgyro και είναι επίσης ένα διάνυσμα που αποτελείται από 3 συστατικά:

Rgyro = [RxGyro, RyGyro, RzGyro]

Θα υπολογίσουμε αυτό το διάνυσμα ένα συστατικό τη φορά. Θα ξεκινήσουμε με το RxGyro.

Ας ξεκινήσουμε παρατηρώντας την ακόλουθη σχέση στο μοντέλο γυροσκοπίου μας, από το τρίγωνο ορθογώνιας γωνίας που σχηματίζουν οι Rz και Rxz μπορούμε να εξαγάγουμε ότι:

tan (Axz) = Rx/Rz => Axz = atan2 (Rx, Rz)

Το Atan2 μπορεί να είναι μια συνάρτηση που δεν είχατε χρησιμοποιήσει ποτέ στο παρελθόν, είναι παρόμοια με το atan, εκτός του ότι επιστρέφει τιμές στο εύρος (-PI, PI) σε αντίθεση με το (-PI/2, PI/2) όπως επιστρέφεται από τον atan και χρειάζεται 2 επιχειρήματα αντί για ένα. Μας επιτρέπει να μετατρέψουμε τις δύο τιμές του Rx, Rz σε γωνίες σε όλο το εύρος των 360 μοιρών (-PI σε PI). Μπορείτε να διαβάσετε περισσότερα για το atan2 εδώ.

Γνωρίζοντας λοιπόν το RxEst (n-1) και το RzEst (n-1) μπορούμε να βρούμε:

Axz (n-1) = atan2 (RxEst (n-1), RzEst (n-1)).

Θυμηθείτε ότι το γυροσκόπιο μετρά το ρυθμό μεταβολής της γωνίας Axz. Μπορούμε λοιπόν να εκτιμήσουμε τη νέα γωνία Axz (n) ως εξής:

Axz (n) = Axz (n-1) + RateAxz (n) * T

Θυμηθείτε ότι το RateAxz μπορεί να ληφθεί από τις μετρήσεις ADC του γυροσκοπίου μας. Ένας πιο ακριβής τύπος μπορεί να χρησιμοποιήσει έναν μέσο ρυθμό περιστροφής που υπολογίζεται ως εξής:

RateAxzAvg = (RateAxz (n) + RateAxz (n-1)) / 2 Axz (n) = Axz (n-1) + RateAxzAvg * T

Με τον ίδιο τρόπο μπορούμε να βρούμε:

Ayz (n) = Ayz (n-1) + RateAyz (n) * T

Εντάξει, τώρα έχουμε Axz (n) και Ayz (n). Πού πάμε από εδώ για να αφαιρέσουμε το RxGyro/RyGyro; Από την εξίσωση 1 μπορούμε να γράψουμε το μήκος του διανύσματος Rgyro ως εξής:

| Ρύγυρο | = SQRT (RxGyro^2 + RyGyro^2 + RzGyro^2)

Επίσης επειδή κανονικοποιήσαμε το διάνυσμα Racc, μπορούμε να υποθέσουμε ότι το μήκος του είναι 1 και δεν έχει αλλάξει μετά την περιστροφή, οπότε είναι σχετικά ασφαλές να γράψουμε:

| Ρύγυρο | = 1

Ας υιοθετήσουμε έναν προσωρινό συντομότερο συμβολισμό για τους παρακάτω υπολογισμούς:

x = RxGyro, y = RyGyro, z = RzGyro

Χρησιμοποιώντας τις παραπάνω σχέσεις μπορούμε να γράψουμε:

x = x / 1 = x / SQRT (x^2+y^2+z^2)

Ας διαιρέσουμε τον αριθμητή και τον παρονομαστή του κλάσματος με SQRT (x^2 + z^2)

x = (x / SQRT (x^2 + z^2)) / SQRT ((x^2 + y^2 + z^2) / (x^2 + z^2))

Σημειώστε ότι x / SQRT (x^2 + z^2) = sin (Axz), άρα:

x = sin (Axz) / SQRT (1 + y^2 / (x^2 + z^2))

Τώρα πολλαπλασιάστε τον αριθμητή και τον παρονομαστή του κλάσματος μέσα στο SQRT με z^2

x = sin (Axz) / SQRT (1 + y^2 * z^2 / (z^2 * (x^2 + z^2)))

Σημειώστε ότι z / SQRT (x^2 + z^2) = cos (Axz) και y / z = tan (Ayz), έτσι τελικά:

x = sin (Axz) / SQRT (1 + cos (Axz)^2 * tan (Ayz)^2)

Επιστρέφοντας στο συμβολισμό μας παίρνουμε:

RxGyro = sin (Axz (n)) / SQRT (1 + cos (Axz (n))^2 * tan (Ayz (n))^2)

με τον ίδιο τρόπο το βρίσκουμε

RyGyro = sin (Ayz (n)) / SQRT (1 + cos (Ayz (n))^2 * tan (Axz (n))^2)

Τώρα, τελικά μπορούμε να βρούμε:

RzGyro = Σήμα (RzGyro)*SQRT (1 - RxGyro^2 - RyGyro^2).

Όπου σημάδι (RzGyro) = 1 όταν RzGyro> = 0 και σημάδι (RzGyro) = -1 όταν RzGyro <0.

Ένας απλός τρόπος για να εκτιμήσετε αυτό είναι να λάβετε:

Σημάδι (RzGyro) = Σήμα (RzEst (n-1))

Στην πράξη προσέξτε όταν το RzEst (n-1) είναι κοντά στο 0. Μπορείτε να παραλείψετε εντελώς τη γυροφάση σε αυτήν την περίπτωση και να αναθέσετε: Rgyro = Rest (n-1). Το Rz χρησιμοποιείται ως αναφορά για τον υπολογισμό των γωνιών Axz και Ayz και όταν είναι κοντά στο 0, οι τιμές μπορεί να υπερχειλίσουν και να προκαλέσουν άσχημα αποτελέσματα. Θα είστε στον τομέα των μεγάλων αριθμών κυμαινόμενου σημείου όπου οι εφαρμογές συνάρτησης tan () / atan () ενδέχεται να μην έχουν ακρίβεια.

Ας ανακεφαλαιώσουμε λοιπόν όσα έχουμε μέχρι τώρα, βρισκόμαστε στο βήμα n του αλγορίθμου μας και έχουμε υπολογίσει τις ακόλουθες τιμές:

Racc - τρέχουσες ενδείξεις από το επιταχυνσιόμετρο Rgyro - που λαμβάνονται από το Rest (n -1) και τις τρέχουσες μετρήσεις γυροσκοπίου

Ποιες τιμές χρησιμοποιούμε για τον υπολογισμό της ενημερωμένης εκτίμησης Υπόλοιπο (n); Μάλλον μαντέψατε ότι θα χρησιμοποιήσουμε και τα δύο. Θα χρησιμοποιήσουμε έναν σταθμισμένο μέσο όρο, ώστε:

Ξεκούραση (n) = (Racc * w1 + Rgyro * w2) / (w1 + w2)

Μπορούμε να απλοποιήσουμε αυτόν τον τύπο διαιρώντας τόσο τον αριθμητή όσο και τον παρονομαστή του κλάσματος με το w1.

Ξεκούραση (n) = (Racc * w1/w1 + Rgyro * w2/w1)/(w1/w1 + w2/w1)

και αφού αντικαταστήσουμε το w2/w1 = wGyro παίρνουμε:

Ξεκούραση (n) = (Racc + Rgyro * wGyro) / (1 + wGyro)

Στην παραπάνω forumula, το wGyro μας λέει πόσο εμπιστευόμαστε το γυροσκόπιο μας σε σύγκριση με το επιταχυνσιόμετρο. Αυτή η τιμή μπορεί να επιλεγεί πειραματικά συνήθως οι τιμές μεταξύ 5..20 θα προκαλέσουν καλά αποτελέσματα.

Η κύρια διαφορά αυτού του αλγορίθμου από το φίλτρο Kalman είναι ότι αυτό το βάρος είναι σχετικά σταθερό, ενώ στο φίλτρο Kalman τα βάρη ενημερώνονται μόνιμα με βάση τον μετρημένο θόρυβο των μετρήσεων του επιταχυνσιόμετρου. Το φίλτρο Kalman επικεντρώνεται στο να σας δίνει "τα καλύτερα" θεωρητικά αποτελέσματα, ενώ αυτός ο αλγόριθμος μπορεί να σας δώσει αποτελέσματα "αρκετά καλά" για την πρακτική σας εφαρμογή. Μπορείτε να εφαρμόσετε έναν αλγόριθμο που προσαρμόζει το wGyro ανάλογα με ορισμένους παράγοντες θορύβου που μετράτε, αλλά οι σταθερές τιμές θα λειτουργήσουν καλά για τις περισσότερες εφαρμογές.

Είμαστε ένα βήμα μακριά από τη λήψη των ενημερωμένων εκτιμώμενων τιμών μας:

RxEst (n) = (RxAcc + RxGyro * wGyro) / (1 + wGyro) RyEst (n) = (RyAcc + RyGyro * wGyro) / (1 + wGyro) RzEst (n) = (RzAcc + RzGyro * wGyro) / (1 + wGyro)

Τώρα ας κανονικοποιήσουμε ξανά αυτό το διάνυσμα:

R = SQRT (RxEst (n)^2 + RyEst (n)^2 + RzEst (n)^2)

RxEst (n) = RxEst (n)/R RyEst (n) = RyEst (n)/R RzEst (n) = RzEst (n)/R

Και είμαστε έτοιμοι να επαναλάβουμε τον βρόχο μας ξανά.

Αυτός ο οδηγός εμφανίστηκε αρχικά στο starlino.com, έκανα μερικές ελαφριές επεξεργασίες και τον ξανά δημοσίευσα με άδεια. Ευχαριστώ Starlino!