Pašbalansējošs robots - PID kontroles algoritms: 3 soļi

2024 Autors: John Day | [email protected]. Pēdējoreiz modificēts: 2024-01-30 10:51

Šis projekts tika iecerēts, jo man bija interese uzzināt vairāk par vadības algoritmiem un to, kā efektīvi ieviest funkcionālās PID cilpas. Projekts vēl ir izstrādes stadijā, jo vēl nav jāpievieno Bluetooth modulis, kas ļaus kontrolēt robotu no viedtālruņa ar Bluetooth.

Izmantotie N20 līdzstrāvas motori bija salīdzinoši lēti, un līdz ar to tiem ir ievērojama spēle. Tas noved pie neliela raustīšanās, jo motori pārvar “atslābumu”, jo riteņiem tiek piemērots griezes moments. Līdz ar to ir gandrīz neiespējami sasniegt pilnīgi vienmērīgu kustību. Manis uzrakstītais kods ir samērā vienkāršs, taču efektīvi parāda PID algoritma iespējas.

Projekta kopsavilkums:

Robota šasija ir izdrukāta 3D formātā, izmantojot Ender 3 printeri, un tā ir veidota tā, lai to varētu saspiest kopā.

Robotu kontrolē Arduino Uno, kas ņem sensoru datus no MPU6050 un kontrolē līdzstrāvas motorus, izmantojot ārēju motora draiveri. Tas darbojas ar 7,4 V, 1500 mAh akumulatoru. Motora vadītājs to noregulē līdz 5 V, lai darbinātu Arduino, un piegādā motoriem 7,4 V.

Programmatūra tika uzrakstīta no jauna, izmantojot gitHub bibliotēkas “Arduino-KalmanFilter-master” un “Arduino-MPU6050-master”.

Piegādes:

• 3D drukātas detaļas
• Arduino UNO
• MPU6050 6 asu sensors
• DC motora vadītājs
• N20 DC motori (x2)
• 9V akumulators

1. darbība: robota izveide

Drukāšana un montāža

Visai konstrukcijai jābūt piespiežamai, bet es esmu izmantojis superlīmi, lai nostiprinātu detaļas, lai robots būtu pilnīgi stingrs balansēšanas laikā.

Es esmu izstrādājis detaļas Fusion 360 un optimizējis katru daļu drukāšanai bez balstiem, lai nodrošinātu stingrākas pielaides un tīrāku virsmas apdari.

Ender 3 printerim tika izmantoti šādi iestatījumi: 0,16 mm slāņa augstums @ 40% aizpildījums visām detaļām.

2. darbība: 3D drukas robots

Šasija (x1)

Kreisais ritenis (x2)

Kreisais motora korpuss (x2)

Arduino futrālis (x1)

3. darbība: PID kontroles algoritms

Esmu uzrakstījis PID kontroles algoritmu no nulles, izmantojot bibliotēkas “Arduino-KalmanFilter-master” un “Arduino-MPU6050-master” no gitHub.

Algoritma pieņēmums ir šāds:

• Lasiet neapstrādātus datus no MPU6050
• Izmantojiet Kalmana filtru, lai analizētu gan žiroskopa, gan akselerometra datus, lai novērstu žiroskopa rādījumu neprecizitātes sensora paātrinājuma dēļ. Tādējādi tiek atgriezta relatīvi izlīdzināta sensora piķa vērtība grādos līdz divām zīmēm aiz komata.
• Aprēķiniet E kļūdu leņķī, ti: leņķi starp sensoru un iestatīto vērtību.
• Aprēķiniet proporcionālo kļūdu kā (proporcionalitātes konstante x kļūda).
• Aprēķiniet integrālo kļūdu kā darbības summu (integrācijas konstante x kļūda).
• Aprēķiniet atvasinājuma kļūdu kā konstantu kā [(diferenciācijas konstante) x (kļūdas izmaiņas / laika izmaiņas)]
• Summējiet visas kļūdas, lai dotu motoram nosūtāmo apgriezienu skaitu.
• Aprēķiniet motoru pagriešanas virzienu, pamatojoties uz kļūdas leņķa zīmi.
• Cilpa darbosies bezgalīgi un balstīsies uz izvadi, jo ievade mainās. Tā ir atgriezeniskā saite, kurā izvades vērtības tiek izmantotas kā jaunās ievades vērtības nākamajai iterācijai.

Pēdējais solis ir noregulēt PID cilpas Kp, Ki & Kd parametrus.

1. Labs sākumpunkts ir lēnām palielināt Kp, līdz robots svārstās ap līdzsvara punktu un var panākt kritienu.
2. Pēc tam sāciet Kd ar aptuveni 1% Kp vērtības un lēnām palieliniet, līdz svārstības pazūd un robots vienmērīgi slīd, kad tas tiek stumts.
3. Visbeidzot, sāciet ar Ki aptuveni 20% no Kp un mainiet, līdz robots "pārsniedz" noteikto vērtību, lai aktīvi noķertu kritienu un atgrieztos vertikālā stāvoklī.