აქსელერომეტრი და გირო გაკვეთილი: 3 ნაბიჯი

2024 ავტორი: John Day | [email protected]. ბოლოს შეცვლილი: 2024-01-30 10:22

შესავალი

ეს სახელმძღვანელო განკუთვნილია ყველასთვის, ვინც დაინტერესებულია გამოიყენოს ამაჩქარებლები და გიროსკოპები, ასევე კომბინირებული IMU მოწყობილობები (ინერტული საზომი ერთეული) მათ ელექტრონიკის პროექტებში

ჩვენ დავფარავთ:

• რას ზომავს ამაჩქარებელი?
• რას ზომავს გიროსკოპი (აკა გირო)?
• როგორ გადავიყვანოთ ანალოგურ-ციფრულ (ADC) მაჩვენებლებს, რომლებსაც იღებთ ამ სენსორიდან ფიზიკურ ერთეულებად (ეს იქნება g ამაჩქარებლისთვის, deg/s გიროსკოპისთვის)
• როგორ გავაერთიანოთ აქსელერომეტრის და გიროსკოპის მაჩვენებლები, რათა მიიღოთ ზუსტი ინფორმაცია თქვენი მოწყობილობის მიწიერ სიბრტყესთან შედარებით

მთელი სტატიის განმავლობაში შევეცდები მათემატიკა მინიმუმამდე დავიყვანო. თუ იცით რა არის Sine/Cosine/Tangent, მაშინ უნდა გესმოდეთ და გამოიყენოთ ეს იდეები თქვენს პროექტში, არ აქვს მნიშვნელობა რა პლატფორმას იყენებთ: Arduino, Propeller, Basic Stamp, Atmel chips, Microchip PIC და ა.

არიან ადამიანები, რომლებიც თვლიან, რომ თქვენ გჭირდებათ რთული მათემატიკა, რათა გამოიყენოთ IMU ერთეული (რთული FIR ან IIR ფილტრები, როგორიცაა კალმანის ფილტრები, პარკები-მაკკლელანის ფილტრები და სხვა). თქვენ შეგიძლიათ შეისწავლოთ ეს ყველაფერი და მიაღწიოთ მშვენიერ, მაგრამ რთულ შედეგებს. საგნების ახსნის ჩემი გზა მოითხოვს მხოლოდ მათემატიკას. მე უბრალოების დიდი მორწმუნე ვარ. მე ვფიქრობ, რომ მარტივი სისტემა უფრო ადვილია გაკონტროლება და მონიტორინგი, გარდა ამისა ბევრ ჩადგმულ მოწყობილობას არ აქვს ძალა და რესურსი განსახორციელებლად რთული ალგორითმები, რომლებიც საჭიროებენ მატრიცის გამოთვლებს.

მაგალითისთვის გამოვიყენებ ახალ IMU ერთეულს, Acc_Gyro Accelerometer + Gyro IMU. ჩვენ გამოვიყენებთ ამ მოწყობილობის პარამეტრებს ჩვენს ქვემოთ მოცემულ მაგალითებში. ეს ერთეული არის კარგი მოწყობილობა დასაწყებად, რადგან ის შედგება 2 მოწყობილობისგან:

- LIS331AL (მონაცემთა ფურცელი) - ტრიაქსიალური 2G ამაჩქარებელი - LPR550AL (მონაცემთა ფურცელი) - ორმაგი ღერძიანი ნაბიჯი და როლი, 500 deg/sec გიროსკოპი

ისინი ერთად წარმოადგენენ თავისუფლების ინერციული გაზომვის 5 გრადუსს. ახლა ეს მშვენიერი სახელია! მიუხედავად ამისა, ლამაზი სახელის მიღმა არის ძალიან სასარგებლო კომბინირებული მოწყობილობა, რომელსაც ჩვენ დავფარავთ და დეტალურად განვმარტავთ ამ სახელმძღვანელოში.

ნაბიჯი 1: ამაჩქარებელი

ამ ერთეულის გასაგებად ჩვენ დავიწყებთ ამაჩქარებლით. აქსელერომეტრებზე ფიქრისას ხშირად სასარგებლოა კუბის ფორმის ყუთის გამოსახვა ბურთით შიგნით. თქვენ შეიძლება წარმოიდგინოთ სხვა რამ, როგორიც არის ნამცხვარი ან დონატი, მაგრამ მე წარმომიდგენია ბურთი:

თუ ჩვენ ავიღებთ ამ ყუთს იმ ადგილას, სადაც არ არის გრავიტაციული ველი, ან სხვა მინდვრები, რამაც შეიძლება გავლენა მოახდინოს ბურთის პოზიციაზე - ბურთი უბრალოდ მიედინება ყუთის შუაგულში. თქვენ შეგიძლიათ წარმოიდგინოთ, რომ ყუთი არის კოსმოსურ სივრცეში, შორს ნებისმიერი კოსმოსური სხეულისგან, ან თუ ასეთი ადგილის პოვნა ძნელია, წარმოიდგინეთ თუნდაც კოსმოსური ხომალდი, რომელიც ბრუნავს პლანეტის გარშემო, სადაც ყველაფერი უმძიმეს მდგომარეობაშია. ზემოთ მოყვანილი სურათიდან ხედავთ, რომ ჩვენ თითოეულ ღერძს მივანიჭებთ წყვილ კედელს (ჩვენ ამოვიღეთ კედელი Y+, რათა შევხედოთ ყუთის შიგნით). წარმოიდგინეთ, რომ თითოეული კედელი წნევის მიმართ მგრძნობიარეა. თუ მოულოდნელად გადავიტანთ ყუთს მარცხნივ (დავაჩქარებთ აჩქარებით 1 გ = 9.8 მ/წ^2), ბურთი მოხვდება კედელს X-. ჩვენ ვზომავთ წნევის ძალას, რომელსაც ბურთი მიმართავს კედელზე და გამოვყოფთ -1 გ მნიშვნელობას X ღერძზე.

გთხოვთ გაითვალისწინოთ, რომ ამაჩქარებელი რეალურად აღმოაჩენს ძალას, რომელიც მიმართულია აჩქარების ვექტორის საპირისპირო მიმართულებით. ამ ძალას ხშირად უწოდებენ ინერციულ ძალას ან ფიქტიურ ძალას. ერთი რამ, რაც თქვენ უნდა ისწავლოთ აქედან არის ის, რომ ამაჩქარებელი არიდებს გაზომვას არაპირდაპირ იმ ძალის მეშვეობით, რომელიც მის ერთ კედელზეა მიმართული (ჩვენი მოდელის მიხედვით, ეს შეიძლება იყოს გაზაფხული ან სხვა რამ რეალურ ცხოვრებაში ამაჩქარებლებში). ეს ძალა შეიძლება გამოწვეული იყოს აჩქარებით, მაგრამ როგორც ვნახავთ შემდეგ მაგალითში ის ყოველთვის არ არის გამოწვეული აჩქარებით.

თუ ჩვენ ავიღებთ ჩვენს მოდელს და დავდებთ დედამიწაზე, ბურთი დაეცემა Z- კედელზე და ქვედა კედელზე გამოიყენებს 1 გ ძალას, როგორც ეს მოცემულია ქვემოთ მოცემულ სურათზე:

ამ შემთხვევაში ყუთი არ მოძრაობს, მაგრამ ჩვენ მაინც ვიღებთ -1 გ – ს კითხვას Z ღერძზე. წნევა, რომელიც ბურთმა მოახდინა კედელზე, გამოწვეული იყო გრავიტაციული ძალით. თეორიულად, ეს შეიძლება იყოს სხვა ტიპის ძალა - მაგალითად, თუ თქვენ წარმოიდგინეთ, რომ ჩვენი ბურთი მეტალისაა, მაგნიტის ყუთის გვერდით დაყენება შეძლებს ბურთის გადატანას ისე, რომ ის სხვა კედელს შეეჯახოს. ეს ითქვა მხოლოდ იმის დასამტკიცებლად, რომ არსებითად აქსელერომეტრი ზომავს ძალას და არა აჩქარებას. ეს ხდება ისე, რომ აჩქარება იწვევს ინერციულ ძალას, რომელიც იპყრობს ამაჩქარებლების ძალის გამოვლენის მექანიზმს.

მიუხედავად იმისა, რომ ეს მოდელი არ არის ზუსტად ის, თუ როგორ არის აგებული 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 გ, ჩვენ შეგვიძლია შევამოწმოთ:

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 მოდულს იყენებთ, თქვენ მიაღწევთ მნიშვნელობას გარკვეულ დიაპაზონში. მაგალითად, 10 ბიტიანი ADC მოდული გამოუშვებს მნიშვნელობას 0..1023 დიაპაზონში, გაითვალისწინეთ, რომ 1023 = 2^10 -1. 12 ბიტიანი ADC მოდული გამოუშვებს მნიშვნელობას 0..4095 დიაპაზონში, გაითვალისწინეთ, რომ 4095 = 2^12-1.

მოდით გადავიდეთ მარტივი მაგალითის გათვალისწინებით, დავუშვათ, რომ 10 ბიტიანი ADC მოდულმა მოგვცა შემდეგი მნიშვნელობები სამი ამაჩქარებელი არხისთვის (ღერძი):

AdcRx = 586 AdcRy = 630 AdcRz = 561

თითოეულ ADC მოდულს ექნება საცნობარო ძაბვა, დავუშვათ ჩვენს მაგალითში ეს არის 3.3V. 10 ბიტიანი adc მნიშვნელობის ძაბვად გადასაყვანად ჩვენ ვიყენებთ შემდეგ ფორმულას:

VoltsRx = AdcRx * Vref / 1023

სწრაფი შენიშვნა: რომ 8 ბიტიანი ADC- ისთვის ბოლო გამყოფი იქნება 255 = 2 ^ 8 -1, ხოლო 12 ბიტიანი ADC- ისთვის ბოლო გამყოფი იქნება 4095 = 2 ^ 12 -1.

ამ ფორმულის გამოყენება სამივე არხზე ვიღებთ:

VoltsRx = 586 * 3.3V / 1023 = ~ 1.89V (ჩვენ ვაგროვებთ ყველა შედეგს 2 ათობითი წერტილში) VoltsRy = 630 * 3.3V / 1023 = ~ 2.03V VoltsRz = 561 * 3.3V / 1023 = 8 1.81V

თითოეულ აქსელერომეტრს აქვს ნულოვანი გ ძაბვის დონე, შეგიძლიათ იპოვოთ ის სპეციფიკაციებში, ეს არის ძაბვა, რომელიც შეესაბამება 0 გ-ს. ხელმოწერილი ძაბვის მნიშვნელობის მისაღებად ჩვენ უნდა გამოვთვალოთ ცვლა ამ დონიდან. ვთქვათ, ჩვენი 0 გ ძაბვის დონე არის VzeroG = 1.65V. ჩვენ გამოვთვლით ძაბვის ცვლას ნულოვანი გ ძაბვიდან შემდეგნაირად:

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

ჩვენ გვაქვს აქსელერომეტრის მაჩვენებელი ვოლტებში, ის ჯერ კიდევ არ არის გ (9.8 მ/წ^2), საბოლოო კონვერტაციის გასაკეთებლად ჩვენ ვიყენებთ ამაჩქარებლის მგრძნობელობას, ჩვეულებრივ გამოხატულ mV/გ. ვთქვათ, ჩვენი მგრძნობელობა = 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.33 გ

ჩვენ, რა თქმა უნდა, შეგვიძლია გავაერთიანოთ ყველა ნაბიჯი ერთ ფორმულაში, მაგრამ მე გავლილი მაქვს ყველა ნაბიჯი იმის გასარკვევად, თუ როგორ მიდიხართ ADC კითხვებიდან ძალის ვექტორულ კომპონენტზე, რომელიც გამოხატულია g- ში.

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

ჩვენ ახლა გვაქვს სამივე კომპონენტი, რომელიც განსაზღვრავს ჩვენი ინერციული ძალის ვექტორს, თუ მოწყობილობა გრავიტაციის გარდა სხვა ძალებს არ ექვემდებარება, შეგვიძლია ვივარაუდოთ, რომ ეს არის ჩვენი გრავიტაციის ძალის ვექტორის მიმართულება. თუ გსურთ გამოთვალოთ მოწყობილობის დახრილობა მიწასთან შედარებით, შეგიძლიათ გამოთვალოთ კუთხე ამ ვექტორსა და Z ღერძს შორის. თუ თქვენ ასევე დაინტერესებული ხართ დახრილობის თითო ღერძის მიმართულებით, შეგიძლიათ ეს შედეგი გაყოთ 2 კომპონენტად: დახრილობა X და Y ღერძებზე, რომელიც შეიძლება გამოითვალოს, როგორც კუთხე გრავიტაციის ვექტორსა და X / Y ღერძებს შორის. ამ კუთხეების გამოთვლა უფრო მარტივია, ვიდრე თქვენ წარმოგიდგენიათ, ახლა, როდესაც ჩვენ გამოვთვალეთ მნიშვნელობები Rx, Ry და Rz. დავუბრუნდეთ ჩვენს ბოლო აქსელერომეტრის მოდელს და გავაკეთოთ დამატებითი ნოტაციები:

კუთხეები, რომლებიც ჩვენ გვაინტერესებს არის კუთხეები X, Y, Z ღერძებსა და ძალის ვექტორს შორის. ჩვენ განვსაზღვრავთ ამ კუთხეებს Axr, Ayr, Azr. R და Rx წარმოქმნილი მართი კუთხის სამკუთხედიდან შეგიძლიათ შეამჩნიოთ, რომ:

cos (Axr) = Rx / R და ანალოგიურად: cos (Ayr) = Ry / R cos (Azr) = Rz / R

ჩვენ შეგვიძლია გამოვაკლოთ Eq.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 მყუდრო = cos (Ayr) = Ry / R cosZ = cos (Azr) = Rz / R

ამ სამეულს ხშირად უწოდებენ მიმართულების კოსინუსს და ის ძირითადად წარმოადგენს ერთეულ ვექტორს (ვექტორი სიგრძით 1), რომელსაც აქვს იგივე მიმართულება, როგორც ჩვენი R ვექტორი. თქვენ შეგიძლიათ მარტივად დაადასტუროთ, რომ:

SQRT (cosX^2 + მყუდრო^2 + cosZ^2) = 1

ეს მშვენიერი თვისებაა, რადგან ის გვათავისუფლებს R ვექტორის მოდულის (სიგრძის) მონიტორინგისგან. ხშირად, თუ ჩვენ მხოლოდ ჩვენი ინერციული ვექტორის მიმართულება გვაინტერესებს, აზრი აქვს მისი მოდულის ნორმალიზებას სხვა გამოთვლების გასამარტივებლად.

ნაბიჯი 2: გიროსკოპი

ჩვენ არ ვაპირებთ შემოვიტანოთ გიროსკოპის ეკვივალენტური მოდელი, როგორც აქსელერომეტრისთვის, არამედ პირდაპირ მეორე აქსელერომეტრის მოდელზე გადავალთ და ჩვენ ვაჩვენებთ რას ზომავს გიროსკოპი ამ მოდელის მიხედვით.

თითოეული გიროსკოპის არხი ზომავს ბრუნვას ერთი ღერძის გარშემო. მაგალითად, 2 ღერძიანი გიროსკოპი ზომავს ბრუნვას X (ან) და X (Y) ღერძების გარშემო. ამ ბრუნვის რიცხვებით გამოსახატავად გავაკეთოთ რამოდენიმე აღნიშვნა. ჯერ განვსაზღვროთ:

Rxz - არის ინერციული ძალის ვექტორის პროექცია XZ სიბრტყეზე Ryz - არის ინერციული ძალის ვექტორის პროექცია YZ სიბრტყეზე

Rxz და Rz მიერ ჩამოყალიბებული მართკუთხა სამკუთხედიდან პითაგორას თეორემის გამოყენებით ვიღებთ:

Rxz^2 = Rx^2 + Rz^2 და ანალოგიურად: Ryz^2 = Ry^2 + Rz^2

ასევე გაითვალისწინეთ, რომ:

R^2 = Rxz^2 + Ry^2, ეს შეიძლება გამომდინარეობდეს Eq.1 და ზემოთ განტოლებებიდან, ან ის შეიძლება გამომდინარეობდეს R და Ryz R^2 = Ryz^2 + Rx^2

ჩვენ არ ვაპირებთ ამ ფორმულების გამოყენებას ამ სტატიაში, მაგრამ სასარგებლოა აღვნიშნოთ კავშირი ჩვენს მოდელში არსებულ ყველა მნიშვნელობას შორის.

ამის ნაცვლად ჩვენ განვსაზღვრავთ კუთხეს Z ღერძსა და Rxz, Ryz ვექტორებს შემდეგნაირად:

Axz - არის კუთხე Rxz (პროექცია R XZ სიბრტყეზე) და Z ღერძი Ayz - არის კუთხე Ryz (პროექცია R YZ სიბრტყეზე) და Z ღერძი

ახლა ჩვენ ვუახლოვდებით იმას, რასაც გიროსკოპი ზომავს. გიროსკოპი ზომავს ზემოთ განსაზღვრული კუთხეების ცვლილების სიჩქარეს. სხვა სიტყვებით რომ ვთქვათ, ის გამოუშვებს მნიშვნელობას, რომელიც წრფივად არის დაკავშირებული ამ კუთხეების ცვლილების სიჩქარესთან. ამის ახსნისთვის ვივარაუდოთ, რომ ჩვენ გავზომეთ ბრუნვის კუთხე Y ღერძის გარშემო (ეს იქნება Axz კუთხე) t0 დროს და ჩვენ განვსაზღვრავთ მას როგორც Axz0, შემდეგ ჩვენ გავზომოთ ეს კუთხე მოგვიანებით t1 და ეს იყო Axz1. ცვლილების სიჩქარე გამოითვლება შემდეგნაირად:

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

თუ ჩვენ გამოვხატავთ Axz- ს გრადუსებში, ხოლო დროს წამებში, მაშინ ეს მნიშვნელობა გამოიხატება deg/s- ში. ეს არის ის, რასაც გიროსკოპი ზომავს.

პრაქტიკაში გიროსკოპი (თუ ეს არ არის სპეციალური ციფრული გიროსკოპი) იშვიათად მოგცემთ დეგ/წმ -ში გამოხატულ მნიშვნელობას. ისევე როგორც აქსელერომეტრისთვის, თქვენ მიიღებთ ADC მნიშვნელობას, რომელიც დაგჭირდებათ გადააკეთოთ deg/s– ზე EQ– ის მსგავსი ფორმულის გამოყენებით. 2, რომელიც ჩვენ განვსაზღვრეთ ამაჩქარებლისთვის. მოდით, გავეცნოთ ADC- ს გიროსკოპის კონვერტაციის ფორმულაში (ჩვენ ვივარაუდოთ, რომ ჩვენ ვიყენებთ 10 ბიტიანი ADC მოდულს, 8 ბიტიანი ADC– სთვის შეცვალეთ 1023 255 – ით, 12 ბიტიანი ADC შეცვალეთ 1023 4095 – ით).

RateAxz = (AdcGyroXZ * Vref / 1023 - VzeroRate) / Sensitivity Eq.3 RateAyz = (AdcGyroYZ * Vref / 1023 - VzeroRate) / მგრძნობელობა

AdcGyroXZ, AdcGyroYZ - მიღებული ჩვენი adc მოდულიდან და ისინი წარმოადგენენ არხებს, რომლებიც ზომავს R ვექტორის პროექციის ბრუნვას შესაბამისად XZ სიბრტყეში, რაც ექვივალენტურია იმის თქმა, რომ ბრუნვა გაკეთდა შესაბამისად Y და X ღერძების გარშემო.

Vref - არის ADC საცნობარო ძაბვა, რომელსაც ჩვენ გამოვიყენებთ 3.3V ქვემოთ მოცემულ მაგალითში VzeroRate - არის ნულოვანი ძაბვა, სხვაგვარად რომ ვთქვათ ძაბვა, რომელიც გიროსკოპმა გამოუშვა, როდესაც ის არ ექვემდებარება რაიმე ბრუნვას, Acc_Gyro დაფისთვის ეს არის მაგალითად 1.23V (თქვენ შეგიძლიათ ნახოთ ეს მნიშვნელობები სპეციფიკაციებში) მგრძნობელობა - არის თქვენი გიროსკოპის მგრძნობელობა ის გამოხატულია mV / (deg / s) ხშირად დაწერილი mV / deg / s, ის ძირითადად გეუბნებათ რამდენი mV იქნება გიროსკოპის გამომუშავება იზრდება, თუ თქვენ გაზრდით ბრუნვის სიჩქარეს ერთი დგ/წმ -ით. Acc_Gyro დაფის მგრძნობელობაა მაგალითად 2 მვ/დეგ/წმ ან 0.002 ვ/დეგ/წმ

ავიღოთ მაგალითი, დავუშვათ, რომ ჩვენი 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 გრადუსი/წმ

სხვა სიტყვებით რომ ვთქვათ, მოწყობილობა ბრუნავს Y ღერძის გარშემო (ან შეგვიძლია ვთქვათ, რომ ის ბრუნავს XZ სიბრტყეში) 306 გრადუსი/წმ სიჩქარით და X ღერძის გარშემო (ან შეგვიძლია ვთქვათ, რომ ის ბრუნავს YZ სიბრტყეში) სიჩქარით - 94 გრადუსი/წმ გთხოვთ გაითვალისწინოთ, რომ უარყოფითი ნიშანი ნიშნავს იმას, რომ მოწყობილობა საპირისპირო მიმართულებით ბრუნავს ჩვეულებრივი პოზიტიური მიმართულებით. კონვენციით ბრუნვის ერთი მიმართულება დადებითია. კარგი გიროსკოპის სპეციფიკაციის ფურცელი გაჩვენებთ რომელი მიმართულებაა დადებითი, წინააღმდეგ შემთხვევაში თქვენ მოგიწევთ მისი პოვნა მოწყობილობით ექსპერიმენტირებით და აღნიშნით როტაციის რომელი მიმართულება იწვევს ძაბვის გაზრდას გამომავალ ბუდეზე. ეს საუკეთესოდ კეთდება ოსცილოსკოპის გამოყენებით, რადგან როტაციის შეწყვეტისთანავე ძაბვა დაეცემა ნულოვანი მაჩვენებლის დონეზე. თუ თქვენ იყენებთ მულტიმეტრს, თქვენ უნდა შეინარჩუნოთ ბრუნვის მუდმივი სიჩქარე სულ მცირე რამდენიმე წამის განმავლობაში და გაითვალისწინოთ ძაბვა ამ ბრუნვის დროს, შემდეგ შეადაროთ იგი ნულოვანი ძაბვას. თუ ის აღემატება ნულოვან ძაბვას, ეს ნიშნავს რომ ბრუნვის მიმართულება დადებითია.

ნაბიჯი 3: აქსელერომეტრისა და გიროსკოპის შერწყმა

ამ ყველაფრის ერთად შედგენა - ამაჩქარებლისა და გიროსკოპის მონაცემების შერწყმა

თუ თქვენ კითხულობთ ამ სტატიას, თქვენ ალბათ შეიძინეთ ან გეგმავთ IMU მოწყობილობის შეძენას, ან ალბათ აპირებთ ააშენოთ ცალკე ამაჩქარებელი და გიროსკოპული მოწყობილობებიდან.

პირველი ნაბიჯი კომბინირებული IMU მოწყობილობის გამოყენებისას, რომელიც აერთიანებს ამაჩქარებელს და გიროსკოპს, არის მათი საკოორდინატო სისტემების გასწორება. უმარტივესი გზაა აირჩიოთ ამაჩქარებლის საკოორდინატო სისტემა, როგორც საცნობარო კოორდინატთა სისტემა. აქსელერომეტრის მონაცემების უმეტესობა აჩვენებს X, Y, Z ღერძების მიმართულებას ფიზიკური ჩიპის ან მოწყობილობის გამოსახულებასთან შედარებით. მაგალითად აქ არის X, Y, Z ღერძების მიმართულებები, როგორც ეს ნაჩვენებია Acc_Gyro დაფის სპეციფიკაციებში:

შემდეგი ნაბიჯებია:

განსაზღვრეთ გიროსკოპის შედეგები, რომლებიც შეესაბამება RateAxz, RateAyz ღირებულებებს ზემოთ განხილული. დაადგინეთ, საჭიროა თუ არა ეს გამომავალი ინვერსია გიროსკოპის ფიზიკური მდგომარეობის გამო ამაჩქარებელთან შედარებით

ნუ იფიქრებთ, რომ თუ გიროსკოპს აქვს გამომავალი X ან Y, ის შეესაბამება ამაჩქარებლების კოორდინატთა სისტემის ნებისმიერ ღერძს, თუნდაც ეს გამომავალი იყოს IMU ერთეულის ნაწილი. საუკეთესო საშუალებაა მისი გამოცდა. დავუშვათ თქვენ გაქვთ დაფიქსირებული გიროსკოპის პოზიცია ამაჩქარებელთან შედარებით. ვარაუდობენ, რომ გიროსკოპისა და ამაჩქარებლის საზღვრები ერთმანეთის პარალელურია, ანუ თქვენ გიროს ათავსებთ 90 გრადუსზე მრავალჯერადი კუთხით ამაჩქარებლის ჩიპთან შედარებით. თუ თქვენ შეიძენთ IMU– ს გამგეობას, დიდი შანსია, რომ ისინი უკვე ამგვარად იყვნენ განლაგებულნი. ამ სტატიაში ჩვენ არ განვიხილავთ მოდელებს, სადაც გიროსკოპი მოთავსებულია ამაჩქარებელთან შედარებით არარეგულარულ კუთხეში (ვთქვათ 45 ან 30 გრადუსი), თუმცა ეს შეიძლება სასარგებლო იყოს ზოგიერთ პროგრამაში.

აქ მოცემულია ნიმუშის თანმიმდევრობა, რათა დადგინდეს, რომელი გიროსკოპის გამომავალი შეესაბამება ზემოთ განხილულ RateAxz მნიშვნელობას.

- დაიწყეთ მოწყობილობის ჰორიზონტალურ მდგომარეობაში განთავსება. აქსელერომეტრის ორივე X და Y გამოსასვლელი გამოიმუშავებს ნულოვანი გ ძაბვას (მაგალითად Acc_Gyro დაფისთვის ეს არის 1.65V)

- შემდეგ დაიწყეთ მოწყობილობის ბრუნვა Y ღერძის გარშემო, სხვა სათქმელი ის არის, რომ თქვენ ატრიალებთ მოწყობილობას XZ სიბრტყეში, ისე რომ X და Z ამაჩქარებლის მაჩვენებლები იცვლება და Y გამომავალი რჩება უცვლელი. - მოწყობილობის მუდმივი სიჩქარით ბრუნვისას, რომელიც იცვლება გიროსკოპის გამომავალი, სხვა გიროსკოპის გამომავალი უნდა იყოს მუდმივი - გიროსკოპის გამომავალი, რომელიც შეიცვალა Y ღერძის გარშემო ბრუნვისას (ბრუნვა XZ სიბრტყეში) უზრუნველყოფს AdcGyroXZ- ის შეყვანის მნიშვნელობას, საიდანაც ჩვენ ვიანგარიშებთ RateAxz - საბოლოო ნაბიჯი არის იმის უზრუნველყოფა, რომ ბრუნვის მიმართულება შეესაბამება ჩვენს მოდელს, ზოგიერთ შემთხვევაში შეიძლება დაგჭირდეთ RateAxz მნიშვნელობის გადაბრუნება ამაჩქარებელთან შედარებით გიროსკოპის ფიზიკური პოზიციის გამო - კვლავ განახორციელეთ ზემოაღნიშნული ტესტი, ატრიალეთ მოწყობილობა გარშემო Y ღერძი, ამჯერად აკონტროლეთ ამაჩქარებლის X გამომუშავება (ჩვენს მოდელში AdcRx). თუ AdcRx იზრდება (ბრუნვის პირველი 90 გრადუსი ჰორიზონტალური მდგომარეობიდან), მაშინ AdcGyroXZ ასევე უნდა გაიზარდოს. წინააღმდეგ შემთხვევაში თქვენ უნდა შეაბრუნოთ RateAxz, ამის მიღწევა შეგიძლიათ Eq.3 –ში ნიშნის ფაქტორის შემოღებით, შემდეგნაირად:

RateAxz = InvertAxz * (AdcGyroXZ * Vref / 1023 - VzeroRate) / მგრძნობელობა, სადაც InvertAxz არის 1 ან -1

იგივე სატესტო ხელჯოხი უნდა გაკეთდეს RateAyz– ისთვის, მოწყობილობის ბრუნვით X ღერძის გარშემო და თქვენ შეგიძლიათ განსაზღვროთ რომელი გიროსკოპის გამომუშავება შეესაბამება 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 ნაწილში გიროსკოპი)). შემდეგი ჩვენ გავაანალიზებთ ურთიერთობას ამ ღირებულებებს შორის, რაც გამოდგება მოწყობილობის მიდრეკილების უფრო ზუსტი შეფასების მისაღებად მიწიერ სიბრტყესთან შედარებით.

თქვენ შეიძლება ჰკითხოთ საკუთარ თავს ამ მხრივ, თუ ამაჩქარებლის მოდელმა უკვე მოგვცა Axr, Ayr, Azr დახრილობის კუთხეები, რატომ გვინდა შეწუხება გიროსკოპის მონაცემებით? პასუხი მარტივია: აქსელერომეტრის მონაცემებს ყოველთვის არ შეიძლება ენდო 100%. რამდენიმე მიზეზი არსებობს, გახსოვდეთ, რომ ამაჩქარებელი გაზომავს ინერციულ ძალას, ასეთი ძალა შეიძლება გამოწვეული იყოს გრავიტაციით (და იდეალურად მხოლოდ გრავიტაციით), მაგრამ ის ასევე შეიძლება გამოწვეული იყოს აპარატის აჩქარებით (მოძრაობით). შედეგად მაშინაც კი, თუ ამაჩქარებელი შედარებით სტაბილურ მდგომარეობაშია, ის მაინც ძალიან მგრძნობიარეა ვიბრაციისა და მექანიკური ხმაურის მიმართ. ეს არის მთავარი მიზეზი, რის გამოც IMU სისტემების უმეტესობა იყენებს გიროსკოპს ამაჩქარებლის შეცდომების გასასუფთავებლად. მაგრამ როგორ კეთდება ეს? და არის თუ არა გიროსკოპი ხმაურისგან თავისუფალი?

გიროსკოპი არ არის თავისუფალი ხმაურისგან, რადგან ის ზომავს ბრუნვას, ის ნაკლებად მგრძნობიარეა წრფივი მექანიკური მოძრაობების მიმართ, ხმაურის ტიპი, რომელსაც განიცდის ამაჩქარებელი, თუმცა გიროსკოპებს აქვთ სხვა სახის პრობლემები, როგორიცაა მაგალითად დრიფტი (ნულოვანი მაჩვენებლის დაბრუნება არ არის როდესაც ბრუნვა ჩერდება). მიუხედავად ამისა, აქსელერომეტრიდან და გიროსკოპიდან მიღებული მონაცემების საშუალო მაჩვენებლით ჩვენ შეგვიძლია მივიღოთ მოწყობილობის მიმდინარე მიდრეკილების შედარებით უკეთესი შეფასება, ვიდრე მხოლოდ ამაჩქარებლების მონაცემების გამოყენებით.

მომდევნო ნაბიჯებში მე შემოვიღებ ალგორითმს, რომელიც შთაგონებულია კალმანის ფილტრში გამოყენებული ზოგიერთი იდეით, თუმცა დანერგულ მოწყობილობებზე მისი განხორციელება ბევრად უფრო მარტივი და ადვილია. მანამდე ჯერ ვნახოთ რისი გამოთვლა გვინდა ჩვენს ალგორითმს. ეს არის გრავიტაციული ძალის ვექტორის მიმართულება R = [Rx, Ry, Rz], საიდანაც შეგვიძლია მივიღოთ სხვა მნიშვნელობები, როგორიცაა Axr, Ayr, Azr ან cosX, მყუდრო, cosZ, რომელიც მოგვცემს წარმოდგენას ჩვენი მოწყობილობის დახრილობის შესახებ მიწის სიბრტყესთან შედარებით, ჩვენ განვიხილავთ ამ ღირებულებებს შორის კავშირს 1 ნაწილში. შეიძლება ითქვას - ჩვენ უკვე არ გვაქვს ეს მნიშვნელობები Rx, Ry, Rz Eq.2 ნაწილი 1 -დან? დიახ, მაგრამ გახსოვდეთ, რომ ეს მნიშვნელობები გამომდინარეობს მხოლოდ ამაჩქარებლის მონაცემებიდან, ასე რომ, თუ თქვენ გამოიყენებთ მათ პირდაპირ თქვენს აპლიკაციაში, შეიძლება მიიღოთ მეტი ხმაური, ვიდრე თქვენი აპლიკაცია მოითმენს. დაბნეულობის თავიდან ასაცილებლად მოდით ხელახლა განვსაზღვროთ ამაჩქარებლის გაზომვები შემდეგნაირად:

Racc - არის ინერციული ძალის ვექტორი, რომელიც იზომება ამაჩქარებლით, რომელიც შედგება შემდეგი კომპონენტებისგან (პროექციები X, Y, Z ღერძებზე):

RxAcc = (AdcRx * Vref / 1023 - VzeroG) / მგრძნობელობა RyAcc = (AdcRy * Vref / 1023 - VzeroG) / მგრძნობელობა RzAcc = (AdcRz * Vref / 1023 - VzeroG) / მგრძნობელობა

ჯერჯერობით ჩვენ გვაქვს გაზომილი მნიშვნელობების ერთობლიობა, რომელიც შეგვიძლია მივიღოთ მხოლოდ აქსელერომეტრის ADC მნიშვნელობებით. ჩვენ მონაცემთა ამ ნაკრს ვექტორს ვუწოდებთ და გამოვიყენებთ შემდეგ ნოტაციას.

Racc = [RxAcc, RyAcc, RzAcc]

იმის გამო, რომ Racc- ის ეს კომპონენტები შეიძლება მივიღოთ ამაჩქარებლის მონაცემებიდან, ჩვენ შეგვიძლია ჩავთვალოთ, რომ ეს არის ჩვენი ალგორითმის შეყვანა.

გთხოვთ გაითვალისწინოთ, რომ რადგანაც Racc ზომავს გრავიტაციულ ძალას, თქვენ მართალი იქნებით, თუ ჩათვლით, რომ ქვემოთ მოცემული ვექტორის სიგრძე ტოლია ან ახლოს არის 1 გ.

| რაკი | = SQRT (RxAcc^2 + RyAcc^2 + RzAcc^2), თუმცა დარწმუნებული უნდა იყოს, რომ აზრი აქვს ამ ვექტორის განახლებას შემდეგნაირად:

Racc (ნორმალიზებული) = [RxAcc/| Racc |, RyAcc/| Racc |, RzAcc/| Racc |].

ეს უზრუნველყოფს თქვენი ნორმალიზებული Racc ვექტორის სიგრძეს ყოველთვის 1.

შემდეგ ჩვენ შემოგთავაზებთ ახალ ვექტორს და ჩვენ მას დავარქმევთ

დასვენება = [RxEst, RyEst, RzEst]

ეს იქნება ჩვენი ალგორითმის გამომავალი, ეს არის შესწორებული მნიშვნელობები გიროსკოპის მონაცემებზე დაყრდნობით და წარსულის სავარაუდო მონაცემებზე დაყრდნობით.

აი რას გააკეთებს ჩვენი ალგორითმი: - აქსელერომეტრი გვეუბნება: "თქვენ ახლა ხართ Racc პოზიციაზე" - ჩვენ ვამბობთ "გმადლობთ, მაგრამ ნება მომეცით შევამოწმო", - შემდეგ შეასწორეთ ეს ინფორმაცია როგორც გიროსკოპის მონაცემებით, ასევე წინა დასვენების მონაცემებით და ჩვენ გამოვუშვით ახალი სავარაუდო ვექტორი დანარჩენი. - ჩვენ მიგვაჩნია, რომ დასვენება არის ჩვენი "საუკეთესო ფსონი" მოწყობილობის ამჟამინდელ პოზიციაზე.

ვნახოთ, როგორ შევძლებთ მის მუშაობას.

ჩვენ ვიწყებთ ჩვენს თანმიმდევრობას ჩვენი აქსელერომეტრის ნდობით და მივანიჭებთ:

დასვენება (0) = Racc (0)

სხვათა შორის დაიმახსოვრე დასვენება და რაკი ვექტორები არიან, ამიტომ ზემოაღნიშნული განტოლება არის მარტივი გზა განტოლების 3 კომპლექტის დასაწერად და განმეორების თავიდან ასაცილებლად:

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

შემდეგ ჩვენ გავაკეთებთ რეგულარულ გაზომვებს თანაბარი დროის ინტერვალით T წამში და მივიღებთ ახალ გაზომვებს, რომლებიც განვსაზღვრავთ როგორც Racc (1), Racc (2), Racc (3) და ასე შემდეგ. ჩვენ ასევე გამოვაქვეყნებთ ახალ შეფასებებს ყოველ ინტერვალში დასვენება (1), დასვენება (2), დასვენება (3) და ასე შემდეგ.

დავუშვათ, ჩვენ n ნაბიჯზე ვართ. ჩვენ გვაქვს ღირებულებების ორი ცნობილი კომპლექტი, რომელთა გამოყენებაც გვსურს:

დასვენება (n -1) - ჩვენი წინა შეფასება, დანარჩენი (0) = Racc (0) Racc (n) - ჩვენი ამჟამინდელი აქსელერომეტრის გაზომვა

სანამ ჩვენ გამოვთვლით დასვენებას (n), შემოვიღოთ ახალი გაზომილი მნიშვნელობა, რომელიც შეგვიძლია მივიღოთ ჩვენი გიროსკოპიდან და წინა შეფასებით.

ჩვენ მას ვუწოდებთ რგიროს და ის ასევე არის ვექტორი, რომელიც შედგება 3 კომპონენტისგან:

Rgyro = [RxGyro, RyGyro, RzGyro]

ჩვენ გამოვთვლით ამ ვექტორს ერთ კომპონენტს ერთდროულად. ჩვენ ვიწყებთ RxGyro– ით.

დავიწყოთ ჩვენს მიმართ გიროსკოპის მოდელში შემდეგი მიმართების დაკვირვებით, Rz და Rxz- ის მიერ წარმოქმნილი მართკუთხა სამკუთხედიდან შეგვიძლია მივიღოთ, რომ:

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

Atan2 შეიძლება იყოს ფუნქცია, რომელიც აქამდე არასოდეს გქონიათ, ის მსგავსია ატანისა, გარდა იმისა, რომ ის აბრუნებს მნიშვნელობებს დიაპაზონში (-PI, PI), განსხვავებით (-PI/2, PI/2) ატანის მიერ დაბრუნებულისაგან და ამას სჭირდება 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) + მაჩვენებელი Axz (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 = ცოდვა (Axz (n)) / SQRT (1 + cos (Axz (n))^2 * tan (Ayz (n))^2)

იგივე გზით ვპოულობთ ამას

RyGyro = ცოდვა (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 = დასვენება (n-1). Rz გამოიყენება როგორც მინიშნება Axz და Ayz კუთხეების გამოსათვლელად და როდესაც ის 0 -თან ახლოს არის, მნიშვნელობებმა შეიძლება გადააჭარბოს და გამოიწვიოს ცუდი შედეგები. თქვენ იქნებით დიდი მოცულობის მცურავი წერტილების რიცხვში, სადაც tan () / atan () ფუნქციის განხორციელებას შეიძლება არ გააჩნდეს სიზუსტე.

მოდით გავიხსენოთ ის, რაც აქამდე გვაქვს, ჩვენ ვართ ჩვენი ალგორითმის n საფეხურზე და გამოვთვალეთ შემდეგი მნიშვნელობები:

Racc - მიმდინარე კითხვები ჩვენი ამაჩქარებელი Rgyro– დან - მიღებული დანარჩენიდან (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)

ზემოხსენებულ ფორმულაში wGyro გვეუბნება, რამდენად ვენდობით ჩვენს გიროსკოპს ამაჩქარებელთან შედარებით. ეს მნიშვნელობა შეიძლება შეირჩეს ექსპერიმენტულად, ჩვეულებრივ, მნიშვნელობები 5..20 შორის გამოიწვევს კარგ შედეგს.

ამ ალგორითმის მთავარი განსხვავება კალმანის ფილტრიდან არის ის, რომ ეს წონა შედარებით ფიქსირებულია, ხოლო კალმანის ფილტრში წონა მუდმივად განახლებულია ამაჩქარებლის გაზომვის ხმაურის საფუძველზე. კალმანის ფილტრი ორიენტირებულია მოგაწოდოთ "საუკეთესო" თეორიული შედეგები, ხოლო ამ ალგორითმს შეუძლია მოგცეთ შედეგები "საკმარისად კარგი" თქვენი პრაქტიკული გამოყენებისთვის. თქვენ შეგიძლიათ დანერგოთ ალგორითმი, რომელიც არეგულირებს 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– ზე, მე გავაკეთე რამდენიმე მსუბუქი რედაქტირება და ხელახლა გამოვაქვეყნე ნებართვით. მადლობა სტარლინო!