(3.4) V i v n t c th c c a robot tính theo cơng th c [11]:
(3.5) (3.6) (3.7)
Mơăhìnhăđ ng h c c a robot hai bánh xích di chuy n trên m t ph ng [10]:
cos sin tan (3.8)
sin cos tan (3.9)
(3.10)
đ aămơăhìnhăt đi m dị line đ năđi m mong mu n l păph ngătrìnhă
sai s :
ng h c sai s c a mơ hình:
cos
cos tan
cos
sin (3.12) V i , là moment qn tính c a mơ hình , là kh i
l ng c a robot
H đ t tr ng thái năđ nh theo tiêu chu n Lyapunov:
(3.13) V i là các h s đi u khi n mang giá tr d ng.
2.2. nhăv ăvƠăđi uăkhi năchoărobotăđiătheoăqu ăđ o:
S d ng thi t b GPSăđ xácăđnh to đ th c t c aăRobot,ăđ xácăđ nhăđúngă
kho ng cách và v n t c di chuy n c a robot, trong h to đ Descartes ta dùng c m bi n la bàn s , Encoder thu tính hi u t căđ t hai bánh xích qua b đi u khi năđ đi u khi nărobotăđiătheoăsaăđ đưăl p..ăS ăđ kh i các thi t b đi u khi nănh ăsauμ
Hình 3.2. S đ kh i các ỏhành ph n đi Ố khi n roboỏ húỏ bùn
Thi tăb ăGPS:
S d ng thi t b GNSS C099 ậFλPăđ xácăđ nh v trí c a Robot. GNSS C099-
FλPăđ c tích h p v i chip ZED-F9P k t h p v iăRTKăđ kh oăsátăđ chính xác c a thi t b GPS này b ng cách tính kho ng cách c a m tăđi m t đ ng d năđ c xác
đ nhătr c. C099-FλPăchoăphépăđánhăgiáăhi u qu v ZED-F9P, mô-đunăđ nh v đ
chính xác cáo U-Blox. Mơ-đunăZED-F9P cung c păđ nh v GNSS nhi uăb ngăt n và
choăng i dùng. B thu GPS h tr RTK,ăđ c g iălƠăắrover”,ăđ căcƠiăđ t trên Robot hút bùn ho c các máy t hành khác. M ng GPS-RTKăc ngăbaoăg m b thuăđ ng yên,
đ c g i là tr măắbase”,ăvƠăliênăk t vô tuy n gi a tr m base và rover
Hình 3.3. S đ kh i C099-F9P Các thơng s chình c a C099-F9P nh ăsauμ
B ng 3.1. Các ỏhơng s chính c a ỏhi ỏ b GPS C099-F9P
Power supply 2.1 mm plug 6 V-12 V DC, USB connection or LiPO cell JST connector
IO voltage 3.3V
USB Micro USB port for GNSS data and power suppl Protocols NMEA, UBX, RTC
Receiver type 184-channel u-blox F9 engine
GPS L1C/A L2C, GLO L1OF L2OF, GAL E1B/C E5b, BDS B1I B2I, QZSS L1C/A L1S L2C, SBAS L1C/A
Nav. update rate RTK up to 20 Hz1 Sensitivity Tracking & Nav -167 dBm
Cold starts -148 dBm Hot starts -157 dBm Reacquisition 160 dBm Serial interfaces 2 UART
1 SPI 1 USB
1 DDC (I2C compliant) Digital I/O Configurable timepulse
EXTINT input for wakeup RTK fix status
GEOFENCE status
Timepulse Configurable: 0.25 Hz to 10 MHz
Protocols NMEA, UBX binary, RTCM v. 3.3, SPARTN v. 2.0 Supply voltage 2.7 V to 3.6 V
Power consumption 68 mA at 3.0 V (continuous) Backup supply 1.65 V to 3.6 V
Thi tăb ăCompass sensor:
S d ng c m bi n la bàn s HMC5883L
C m bi n la bàn s HMC5883Lăcóăkíchăth c nh g n s d ng giao ti p I2C,
đ cădùngăđ đoăt tr ng c aătráiăđ t nh măxácăđ nhăph ngăh ng v iăđ chính
xácălênăđ n 1 ho că2ăđ , c m bi năcóăcáchăđoăriêngăbi t cho t ng tr c và có th k t h p l iăđ tính tốn 3D
Thơng s c a c m bi n la bàn s :
B ng 3.2. Thông s c a c m bi n la bàn s
c tính Conditions Min Typ Max Units
Power Supply Supply Voltage VDD Referenced to AGND VDDIO Referenced to DGND 2.16 1.71 2.5 1.8 3.6 VDD+ 0.1 Volts Volts Average Current Draw
Idle Mode Measurement Mode (7.5 Hz ODR;
No measurement average, MA1:MA0 = 00) VDD = 2.5V,
VDDIO = 1.8V (Dual Supply) VDD = VDDIO = 2.5V (Single Supply) - - 2 100 - - A A
Field Range Full scale (FS) -8 +8 gauss Mag
Dynamic Range
3-bit gain control ±1 ±8 gauss
Sensitivity (Gain) VDD=3.0V, GN=0 to 7, 12-bit ADC 230 1370 LSb/gauss Digital Resolution VDD=3.0V, GN=0 to 7, 1-LSb, 12-bit ADC 0.73 4.35 milli-gauss Noise Floor (Field Resolution) VDD=3.0V, GN=0, No
measurement average, Standard Deviation 100 samples
(See typical performance graphs below)
2 milli-gauss
Linearity ±2.0 gauss input range 0.1 ±% FS Hysteresis ±2.0 gauss input range ±25 ppm
Cross-Axis Sensitivity
Test Conditions: Cross field = 0.5 gauss, Happlied = ±3 gauss
±0.2% %FS/gauss Output Rate
(ODR)
Continuous Measurment Mode Single Measurement Mode
0.75 75 160 Hz Hz Measuremen t Period
From receiving command to data ready
6 ms
Turn-on Time
Ready for I2C commands Analog Circuit Ready for Measurements 200 50 s ms Gain Tolerance
All gain/dynamic range settings ±5 % I2C Address 8-bit read address
8-bit write address
0x3D 0x3C
hex hex I2C Rate Controlled by I2C Master 400 kHz I2C
Hysteresis
Hysteresis of Schmitt trigger inputs on SCL and SDA - Fall
(VDDIO=1.8V) Rise (VDDIO=1.8V) 0.2*V DDIO 0.8*V DDIO Volts Volts
Self Test X & Y Axes Z Axis
±1.16 ±1.08 ±1.08 ±1.08
gauss
X & Y & Z Axes (GN=5) Positive Bias
X & Y & Z Axes (GN=5) Negative Bias 243 -575 575 -243 LSb Sensitivity Tempco TA = -40 to 125°C, Uncompensated Output -0.3 %/°C B ăđi uăkhi n:
- Ch năb ăđi uăkhi n,ăArduinoăUnoăR3ăcóđ yăđ ngo iăviăđ giao ti p v i các module
GNSS C099 ậ F9P, c măbi nălaăbƠnăS ăHMC5883L, encoder
- B đi u khi n s d ng board Arduino Uno R3 v iăviăđi u khi n Atmega328 (8bit)
s ăd ngăth chăanhăcóăchuăkìădaoăđ ngălƠă16ăMHz.
- ArduinoăUnoăcoăt ngăc ngăcóă14ăpină(ngõ)ăraă/ăvƠoăđ căđánhăs ăt ă0ăt iă13ă(trongă đóăcóă6ăpinăPWM,ăđ căđánhăd uă~ătr cămưăs ăc aăpin).ăCóăthêmă6ăpinănh nătínă hi uăanalogăđ căđánhăkíăhi uăt ăA0ă- A5,ă6ăpinănƠyăc ngăcóăth ăs ăd ngăđ cănh ă cácăpinăraă/ăvƠoăbìnhăth ngă(nh ăpină0ă- 13).ă ăcácăpinăđ căđ ăc p,ăpină13ălƠăpină đ căbi tăvìăn iătr căti păv iăLEDătr ngătháiătrênăboard.
Hình 3.4. Board Arduino Uno R3
B ng 3.3. Thông s thi t b đi u khi n
Viăđi uăkhi n ATmega328P
i năápăho tăđ ng 5V
i năápăvƠoăkhuyênădùng 7-12V
i năápăvƠoăgi iăh n 6-20V
Digital I/O pin 14ă(trongăđóă6ăpinăcóăkh ăn ngăb măxung)
PWM Digital I/O Pins 6
Analog Input Pins 6
C ngăđ ădòngăđi nătrênăm iăI/Oăpin 20 mA
C ngăđ ădòngăđi nătrênăm iă3.3Văpin 50 mA
Flash Memory 32 KB (ATmega328P)
0.5ăKBăđ căs ăd ngăb iăbootloader
SRAM 2 KB (ATmega328P) EEPROM 1 KB (ATmega328P) T căđ 16 MHz Chi uădƠi 68.6 mm Chi uăr ng 53.4 mm Tr ngăl ng 25 g
2.3. Thu tătoánăđi uăkhi năRobot: Ch năthu tătoánăđi uăkhi n
đi u khi n robot s d ng thu t toán theoăđu i thu n túy - Pure Pursuit [14] là m t thu t toán theo dõi chuy năđ ng b ngăcáchătínhătốnăđ congăđ di chuy n robot t v trí hi n t i c aănóăđ n v trí m c tiêu. Nó tính tốn l nh v n t căgócăđ di chuy n robot t v trí hi n t i c aănóăđ đ n m t s đi m nhìn v phíaătr c c a robot.
Sauăđó,ăthu t tốn di chuy năđi mănhìnătr cătrênăđ ngăđiăd a trên v trí hi n t i c a
robotăchoăđ năđi m cu i cùng c aăđ ng đi.ăT t c cácăđi m c a thu tătoánălƠăđ ch n m c tiêu có v tríăcáchăxeătr c m t kho ngătrênăđ ngăđi.
Ph ngăphápăti p c n chính c a thu t toán Pure Pursuit lƠăph ngăphápăxácă đ nhăđ cong s đ aărobotăđ năđi m m cătiêu.ă i m m c tiêu này là m tăđi m trên
đ ngăđiăc a robot (path) cách robot m t kho ng,ăkhiăđóăthu t tốn s k t n iăđi m hi n t iăvƠăđi m m c tiêu b ng m t cung.
Hình 3.6. Mơ ỏ hình h c ỏhỐ ỏ ỏoán ỏheo đỐ i ỏhỐ n ỏỐý
V iăđi m (x, y)ălƠătiêuăđi m n m trên qu đ o đưăđnh (path), cáchăđi m g c to đ m t kho ng l. M c tiêu c a thu t toán Pure Pursuit lƠătìmăđ cong cung n i t
đi m g căđ năđi m (x, y). Theo mô t hình h c hình 3.6 ta có:
(3.14) (3.15) (3.16) (3.17) (3.18) (3.19) (3.20)
à đ á í đ í
Trong thu tătoánătheoăđu i thu nătuỦ,ăđ dƠiătiêuăđi m l đ căđ tătr c, và kho ng cách ngang u (c aărobot)ăđ nătiêuăđi m. Kho ngăcáchănƠyăđ c mơ t và tính tốn hình 3.7
Hình 3.7 Mơ ỏ kho ng cách ngang c a roboỏ đên ỏiêỐ đi m
Áp d ngăph ngătrìnhă i m ậ d c (Point ậ Slope) [15] đ tìm qu đ o robot:
( ) ( rr) tan( ) y y x x− = − (3.21) Trongăđóμ (xr, yr): to đ robot : góc xoay c a robot
Chuy năph ngătrìnhăv d ng: ax + by + c = 0 (3.22)
Trongăđóμăa = - tan(); b = 1; c = tan() .(xr - yr) Áp d ng công th c kho ngăcáchăđ ng th ng, ta có:
2 2 ax by c d a b + + = − (3.23)
Thay các thông s và t aăđ c aătiêuăđi m,ătaăđ c:
2 2 l l ax by c u a b + + = + (3.24) Trongăđóμă
(xl, yl): t aăđ tiêuăđi m
+ăTínhătốnăđ cong c aăcungătrịnăđánhălái.
+ Tính tốn v n t c tham chi u m c tiêu bánh xe trái và bánh xe ph i
Tr ng tâm c a robot tách r i v m t hình h căvƠăcáchăđi m m c tiêu m tăđo n l. i m m c tiêu (x, y)ăđ c ràng bu cătrênăđ ngăđi,ăthu tătốnătínhătốnăđ cong c a cung n iăđi m g c v iăđi m m cătiêuăcóăđ dài dây cung l.
L uăđ ăđi uăkhi n
Hình 3.8 L Ố đ ỏhỐ ỏ ỏốnđi Ố khi n robot đi ỏheo line (qỐ đ o)
Di n gi iăl uăđ :
Vi c đi u khi n robot bám theo qu đ o cho tr căđ c th c hi n l năl c các
b c sau:
- Kh iăt oăqu đ o tham chi u, xácăđ nhăđi măđ u,ăđi măcu iăc aăqu ăđ o. Trong
b c này t các to đ tham chi u cho tr c, qu đ o s đ căxácăđ nhăb ngăcáchăn iăcácă
đi măthamăchi uăchoătr c,ăsauăđó ti n hƠnh chia nh các vector tham chi u nh h nă t ng ng v i các ph n t có trong qu đ o. Xácăđ nhăđi mătrênăqu ăđ oăcóăkho ngăcách
g năRobotănh t.ăCácăb căti pătheoălƠăchuy năđi măđ năv ăh ăto ăđ ăc aărobotb ngăGPSăvƠă
Compass sensor, tínhătốnăđ ngăcongăđ ăn iăsuyăv năt căgócărobot,ăc pănh tăv ătríăhi năt iă
chia thƠnh cácăđi m tham chi u v i kho ng cách gi a cácăđi m lƠ0.02măt ngă ng v i v n
t c tham chi uăs ăb 0.2m/s vƠ bán kính quay 0.5m.
Hình 3.9 QỐ đ o roboỏ ỏrên h ỏo đ ỏ ng đ i
S d ng c m bi n la bàn s xácăđ nh góc l ch c a robot so v iăh ng B c. Do tín hi u nh năđ c t c m bi n th c t là t aăđ GPS c aărobotă,ăđ xácăđ nhăđúngă
kho ng cách và v n t c di chuy n c a robot, c n chuy năđ i tín hi u t t aăđ GPS sang h t aăđ Descartes v i góc l chăbanăđ u là góc c a la bàn s .
Trongăđóăcóăcácăthơngăs c năxácăđ nhănh ăsauμ
Xácăđnh kho ng cách c a robot khi bi t t aăđ đ u và cu i d , theo công th c
[13]ănh ăsauμ
(3.25)
or cos for E W line (3.26)
q (3.27)
Trongăđóμă lƠăv ăđ , ălƠăkinhăđ ,ă ălƠăđ ngăđiăng nănh t,ăRălƠăbánăkínhătráiă đ t,ă ă=ălnă(tană( ă/ă4ă+ă )ă/ătană( ă/ă4ă+ă ))ăchênhăl chăqu ăđ oăd ăki n.ăXácă đ nhăh ăs ăgócă
atan (3.28)
Trongăđóμă ălƠăv ăđ ăe,ă ălƠăkinhăđ ă,ă ălƠăđ ngăđiăng nănh tă(<180ă°),ăRălƠă bánăkínhătráiăđ t.
Xácăđ nhăđi măđ năkhiăbi tăh ăs ăgócăvƠăkho ngăcáchă[13]
(3.29)
cos (3.30)
V iμă ă=ălnă(tană( ă/ă4ă+ă ă2/2ă)ă/ătană( ă/ă4ă+ă ă1/2ă))
q (3.31)
sin q (3.32)
(3.33)
Hình 3.10 QỐ đ o roboỏ ỏrên h ỏo đ GPS
- Xácăđ nh v n t c tham chi u d aătrênăđ cong qu đ o. Sau khi kh i t o qu
đ o ti n hƠnh xácăđnh t căđ tham chi u cho robot b ng cách n iăsuyăđ cong qu
đ o thơng qua n i suy bán kính cong r c aă3ăđi m li n k trên qu đ o.ă congăđ c xácăđnh lƠ h s , hình qu đ o tham chi u robot cóλăđi m quay xe v i bán kính cong lƠ r = 0.5, t i cácăđi m nƠyăđ cong c a qu đ o lƠ l n nh t, c n gi i h n
t căđ khi robot di chuy năđ n các đi m tham chi u,ăđ cong s t l ngh ch v i v n t c,ăđ cong cƠng l n, v n t c cƠng nh .
Hình 3.11 th n i sỐy đ cong qu đ o c a robot
Vi căthayăđ i v n t căđ t ng t s làm cho chuy năđ ng c aărobotăkhơngăđ c
m t, có qn tính l n khi gi m t c, vi c gi i h n t căđ tham chi u phù h p v i t c
đ robot ngoài th c t ,ăđ ng th i c năthayăđ i v n t c qu đ o theo m t gia t c nh t
đnh. D a vào v n t c qu đ o và gia t căchoătr c ta có th xácăđnh v n t c tham chi uăchoărobotănh ăđ th n i suy v n t c qu đ o hình 3.12
- S d ng b đi u khi n PID ( khâu t l ) tínhăđ r ngăxungă(PWM)ăđi u khi n t căđ đ ngăc ăbánh trái vƠ bánh ph i.
- Sau khi s d ng gi i thu t Pure Pursuit Adaptive xácăđ nhăđ c v n t c tham chi u c a bánh trái vƠ bánh ph i, ti n hƠnh xây d ng b đi u khi n công xu tăđ ng
c ăchoăm i bánh s d ng b đi u khi n PID, v i tín hi uăđ u vƠo input (setpoint) lƠ
t căđ tham chi u hai bánh xích VL, VR, tín hi uăđi u khi n lƠđ r ng xung (PWM) .
Hình 3.13. B đi u khi n PID Công th c b đi u khi n PID: Công th c b đi u khi n PID:
0 ( ) ( ) ( ) ( ) t p i d de t u t K e t K e t dt K dt = + + (3.34) Trongăđó: P (Proportional) lƠ khâu t l . I (Integral) lƠ khâu tích phân. D (Derivative) lƠ khâu vi phân. e(t) lƠ sai s h th ng
Kp: lƠ h s khâu t l . Ki lƠ h s khâu tích phân.
Kd lƠ h s khâu vi phân. U(t) lƠ tín hi uăđi u khi n.
đi u khi n t c đ đ ng c ăđáp ng v i t căđ tham chi uăđư tính tốnătr c
đó, s d ng hai khâu t l đ t lƠ FF (Feed forward) vƠ FB (Feed back)ătrongăđó, khâu t l FFăđi u khi n công su t ph thu c vào v n t c tham chi uăđư tính tốn t tr c và b sung thêm m t ph n công su tăđ tri tătiêuăquánătínhăkhiăt ngăho c gi m t c g m 2 khâu t l c a v n t c vƠ gia t c là
FF =K vv. tar +K aa. tar (3.35) V i: vtar: v n t c tham chi u
atar: gia t c tham chi u
Kv, Ka l năl t là là h ng s v n t c và gia t c truy n th ng, h ng s này cho bi tăl ngăn ngăl ng c n cho m t v n t c và gia t c m c tiêu nh tăđ nh.
Khâu t l FBăđi u khi n công su tăđ tri t tiêu sai l ch gi a v n t c bánh xe
đoăđ c và v n t c tham chi u:
.( )
p tar mea
FB=K v −v (3.36) V i: vmea: v n t căđoăđ c t encoder
Trongăđóμă
Kp: h ng s t l sai s ph n h i, h ng s càng l n thì sai l chăđ c tri t tiêu càng nhanh.
Khiăđóăđ r ng xung PWM (Pulse-width modulation) c n cung c p cho b
đi u khi năđ ngăc ăcácăbánhăxeălƠă(PWM = FF + FB), m iăđ ngăc ăđ c áp d ngăđ c l p b đi u khi n này.
2.4. Gi iăthu tăđi uăkhi năchoărobotăhútăbùnă
Xácăđ nhăv ătríă hi năt iăRobot (Forward kinematic) B tăđ u Tínhătốnăđ ngăcong (Invert kinemmatic) H ăth ngăláiăRobot
Post = end point
K tăthúc úng Sai N iăsuyăv năt c GPS Compass sensor Kh iăt o
Posμăto ăđ ărobot PathμăQu ăđ oăRobotă Endpointμă i măk tăthúc
Xácăđ nh đi măg nănh t
Xácăđ nh đi măm cătiêu
2.5. K tăqu ămôăph ng:
S d ng thi t b GPSăđ xácăđnh to đ th c t c aăRobot,ăđ xácăđ nhăđúngă
kho ng cách và v n t c di chuy n c a robot, trong h to đ Descartes ta dùng c m bi n la bàn s . K t qu mô ph ngăđ tăđ cănh ăsauμ
Hình 3.16. ỏh ố n ỏ c ỏh c ỏ
Hình 3.18. QỐ đ o mơ ph ngc a robot
Theoănh ăgi i pháp m c 2.3, đ u ra c a bùn ta s d ng ng có kíchăth c 4 inch
đ c n i v i ng ch l p s n trên g t ng caoăh năđáyăb là 4 m, chi u cao robot 750 mm. Khi k t n i đ u ra v i robot, đo n ng này phía trên và bùn s theo c t áp c aăđ căb măra ngồi, đo n ng này có kh iăl ng kho ng 10kg so v i kh iăl ng c a robot là 400kg thì nhăh ng c a nó t i chuy năđ ng c aăRobotălƠăkhơngăđángă
k .ăTheoă[6]ăđ i v i các robot bánh xích có t căđ di chuy năkhôngăv t quá 0.3 m/s khi di chuy nătrênăđ ng cong l călyătơmălƠăkhôngăđángăk , thi t k c a bánh xích c ng giúp robot khơng b tr t theo chi u ngang.
2.6. K tălu n
K t qu mô ph ng cho th y s tr t nhăh ngăđ n qu đ o chuy năđ ng khi robot di chuy n. V iăbiênăđ sai s dò line 18 cm là r t nh so v iăsaăđ v i kích
th c c a robot và sai s nƠyăkhơngăđángăk so v iăkíchăth c c a b ch aăn c làm