Development of directional algorithm for threewheel omnidirectional autonomous mobile robot

Nội dung text: Development of directional algorithm for threewheel omnidirectional autonomous mobile robot

1. Vietnam Journal of Science and Technology 59 (3) (2021) 345-356 doi:10.15625/2525-2518/59/3/15583 DEVELOPMENT OF DIRECTIONAL ALGORITHM FOR THREE- WHEEL OMNIDIRECTIONAL AUTONOMOUS MOBILE ROBOT Le Phuong Truong1, *, Huan Liang Tsai2, Huynh Cao Tuan1 1Faculty of Mechatronics and Electronics, Lac Hong University, Bien Hoa, Viet Nam 2Department of Electrical Engineering, Da-Yeh University, Chang-Hua 51591, Taiwan *Emails: lephuongtruong@lhu.edu.vn or letruonglhu@gmail.com Received: 12 October 2020; Accepted for publication: 11 February 2021 Abstract. An omnidirectional algorithm has been developed to control autonomous mobile robots with three wheels. The implementation system consists of three planet DC motors with a rated power of 80 W for three wheels, three encoders for speed feedback, one encoder for distance feedback, and one digital compass sensor for angle feedback. The main system with an STM32F407 microcontroller is designed for directional control of wheels based on the signal received from compass sensor and encoder, and then controls three subsystems to adjust the steering speed of each wheel. The subsystem is built to control only one DC motor for each wheel with the built-in proportional integral derivative controller (PID) algorithm by an STM32F103 microcontroller. Furthermore, the directional control algorithm is developed for three omnidirectional wheels and a PID algorithm is designed to control the speed of DC motor for each wheel. It can be seen from the results obtained that the proposed system has several advantages: (1) automatically adjusting the angle and position; (2) erasing the sensor for tracking line of the automobile robot; (3) being self-developed; (4) ensuring reliable accuracy. Keywords: directional algorithm, three wheels, PID control, mobile robot. Classification numbers: 4.1.2; 4.10.2. 1. INTRODUCTION The international ABU Asia-Pacific Robot Contest (ABU Robocon) is a scientific challenge to apply the advanced research for both development and implementation of mobile robots with the preset task in the fields of automation, electronics, computer vision and image processing, as well as mechatronics system. The motion of autonomous mobile robot with high accuracy is very important. Furthermore, the wheel of autonomous robot is either Mecanum wheel or Omni wheel because it is designed for movement with omni-direction and flexibility. The structure of the two wheels is shown in Fig. 1. In this work, the Omni directional wheels are applied to create the motion of mobile robot system. The advanced DC motors are utilized to control wheel torques independently and thus are capable of simultaneous and independent control over their rotational and translational motion. In recent years, several researches were focused on the Mecanum and Omni wheels as listed in Table 1.
2. Le Phuong Truong, Huan Liang Tsai, Huynh Cao Tuan (a) (b) Figure 1. Wheel types: (a) Mecanum wheel, (b) Omni wheel. Table 1. The comparison of Mecanum and Omni wheels. Wheel type Algorithms Hardware Direction Wheel References control number Mecanum Closed-loop Mitsubishi Cooney et al. control and dead- M16C/62 Directional 4 2004 [1] reckoning for microcontroller navigation Kinematic None Directional 4 Guo et al. 2016 equation [2] None Nucleo-F411 Directional 4 Yamada et al. RE 2017 [3] PSO based Fuzzy- On-board Directional 3 Feng et al. 2010 PI computer [4] Fuzzy-PI MRL Robo Cup Directional 4 Hashemi et al. SSL 2011 [5] Side-slip angle None Directional 4 Li et al. 2016 [6] estimation method Model predictive PIC32 family of Directional 3 Jessivaldo control/filtered Smith Microcontroller Santos et al. predictor 2017 [7] Barrel-shaped Arduino two-wheel 3 TatsuroTerakaw friction-driven Mega2560 driven Omni- a et al. 2019 [8] directional PID, directional STM32F- Directional, 3 Proposed Omni algorithm family MCUs position system In addition, the Mecanum and Omni wheels have been controlled based on the motion equation. Several authors have demonstrated the accuracy of motion equation for the Omni- directional control. Table 2 shows the motion equation with 3 and 4 Omni wheels. 346
3. Development of directional algorithm for three-wheel omnidirectional autonomous mobile robot Table 2. The motion equation with three- and four-wheel Omni platform. Type Motion equation Descriptions References x(t) = Ax(t) + B[u(t) + Ksgn(x(t))] ut()is the continuous Santos et al. y()() t= Cx t control input applied 2017 [7] T with the zero order. utututut()()()()=  123  Bv , Bvn are viscous Three- 22 3 jKBtv 2 + 00 wheel 2MR rM friction relative to v and a 22 v . 3 jKBtvn n A = −+ 002 2MRa rM B , Bvn viscous friction 3bjK22 B w 002 + JRan rI relative to  and . cos()cos() CCC,, Coulomb 0 − v vn  MM friction relative to , iK −1()()sinsin B = t and RMMM a I is robot’s inertial bbb n momentum; is angle; IIInnn M is robot’s mass; b Cv − 00 is robot’s radius . M r is wheels radius; l is C K= o −n 0, C = I M reduction; Ra is C armature resistances; o 0 − w k . torque constants. In t q = k(q)v with Galicki. ( ,qq )12 is stands for the 2019 [9] T −1 location of the center of k(q) = j (q) j(q) j(q) Four- the vehicle; q3 is the − s1 − c1 d wheel angle made between the − s − c d j(q) = 2 2 line joining the robot − s3 − c3 d center with the first wheel center and the − s − c d 4 4 horizontal line; si = sin[ q3 + (i −1) / 2] d is distance from the c = cos[q + (i −1) / 2] center of the robot to i 3 each of the wheel center. T 3 q = q1, q2 , q3  R c' =cos( q + ( i − 1) / 2; i 3 ' si =sin( q3 + ( i − 1) / 2. T 3 qqqqR= (,,)123 In this study, the proportional integral derivative controller (PID) algorithm is applied for the speed control of each wheel, angle and distance algorithms are applied to control directional 347
4. Le Phuong Truong, Huan Liang Tsai, Huynh Cao Tuan position using both STM32F407 and STM32F103 MCUs, respectively. The main part of this paper includes a description of the system configuration in section 2, followed by section 3 revealing the development of directional algorithm and section 4 showing the diagram of algorithm and hardware development. Discussions and brief conclusions are represented in section 5 and section 6, respectively. 2. SYSTEM DESCRIPTION Figure 2 shows the schematic diagram of the three-wheel Omni directional system which consists of a mechanical system, the main control interface circuitry with a STM32F407 MCU, three sub control systems with a STM32F103 MCU-based interface circuitry, and a C## programming platform. The three encoder subsystems can give feedback on the speed of each wheel under practical operation conditions for the PID control through an interface circuitry built- in with a STM32F107 MCU. In addition, a compass sensor is built for measuring the angular motion of system and an encoder transfers the distance data through an interface circuitry of STM32F407 MCU-based main system which is configured in the C## program. STM32F407MCU Main system Compass sensor Digital PID control Digital Control angle and distance signal signal algorithm Encorder Distance Wheel STM32F103 MCU 4h Sub system Digital PID Digital PID signal control signal control Encorder 0h Wheel Wheel 0h 4h STM32F103 MCU STM32F103 MCU Sub system 1 Sub system 2 Figure 2. The schematic diagram of the three-wheel Omni directional system. The main interface circuitry is designed with the self-developed algorithm for both angle and distance control to calculate the speed and angle of each wheel based on the feedback data of the compass sensor and encoder system. Furthermore, the interface circuitry of the control sub-system is the built-in PID algorithm through an STM32F103 MCU for the speed control of each wheel. 3. DEVELOPMENT OF OMNIDIRECTIONAL WHEELS AND ALGORITHM 3.1. Omnidirectional wheels The omnidirectional wheels are designed on the basis of alternating passive rollers around the circumference which are perpendicular to the turning direction (see Fig. 3.). The result is that the wheel can be driven with full force, but will also slide laterally with great ease. 348
5. Development of directional algorithm for three-wheel omnidirectional autonomous mobile robot Figure 3. Prototype of Omnidirectional wheels. 3.2. Equation of motion Omni-directional drives have been realized in several innovative ways such as the Newton’s laws of motion (Kalmár et al. [10]) and the equation of motion (Tamás Kalmár [11]) as follows: Az(t) + Bz(t) = C()v(t) (1) T T where z(t) = x y   is the state vector and v(t) = v1 v2 v3 is the control input vector, and A, B, C are respectively given by m 00 m A = 00 (2) h 00 L 3 00 2 3 B = 00 (3) 2 3 L 00 and −sin − sin −  sin +  33 (4) C = cos − cos −  − cos +  33 1 1 1 The state variables x, y,  are the linear and angular displacements, m is the mass of the 349
6. Le Phuong Truong, Huan Liang Tsai, Huynh Cao Tuan th vehicle, vi is the input voltage of motor to drive the i wheel. The characteristic constants of the motors are denoted by and  . The variable L is the distance. 3.3. Development of the equation of voltage input for each wheel One of the important parameters to develop directional algorithm for Omnidirectional three-wheel is the voltage input of DC motor for each wheel, which can be given by [8 - 11]: V1 = d sin d + + (5) 3 V2 = d cos d + − (6) 3 and V3 = d cos d + + (7) 3 st nd rd where V1, V2 , V3 are the voltage input to DC motor of the 1 , 2 and 3 wheels,d is the value of rotation of each wheel,  is the value of rotation of the robot head, d is the angular of robot. In this case, the values of and are in the range of [−1, 1]. 3.4. Development of the equation of rotation angle of autonomous robot It is assumed that at the beginning the autonomous robot has the angle ( ), angular direction ( ), and angle responded from compass sensor ( cs ). When the autonomous robot is moving, the angular motion of autonomous robot could have an angular error (  ),which can be written as:  = cs − (8) Case study 1: The value of =0, the autonomous robot keeps moving. Case study 2: The value of  0, the system needs to control the rotation angle by the equation below: a =−   (9) where a is the angular adjustment. 3.5. PID equation The PID controller in a feedback control loop continuously calculates an error signal e(t) , which is the difference between a desired set point and a measured output variable, and applies a correction based on proportional, integral and derivative terms. The function of PID controller can be written as [12]: 350
7. Development of directional algorithm for three-wheel omnidirectional autonomous mobile robot de()1 t utketTetdt()()()()=++ (10) pd 0 dtT() de() t utketkketdt()()()()=++ (11) pdi 0 dt() where k p , ki , kd are the coefficients corresponding to the proportional, integral and derivative terms. 4. THE DIAGRAM OF ALGORITHM AND HARDWARE DEVELOPMENT 4.1. The diagram of algorithm Start Set-up angular, speed Reset compass sensor Calculate voltage inputs for 3 DC motors of 3 wheels False Current speed = speed Re-calculate Calculate the setup speed rotation angular False True Current angular = angular set up True Keep current speed End Figure 4. The algorithm flowchart of omnidirectional three-wheel. The algorithm of the omnidirectional three-wheel is developed as shown in Fig. 4. First, the angle and speed of the autonomous robot are initialized. The system will calculate voltage inputs for each wheel based on the Eqs. (5-7) and then check the current speed. If false, the voltage for each wheel will be increased or decreased until current speed equals to the desired speed. 351
8. Le Phuong Truong, Huan Liang Tsai, Huynh Cao Tuan Furthermore, the importance of omnidirectional control is the direction of an autonomous robot. In this case, if the current angle of the autonomous robot is not equal to the desired one, the system calculates the rotation angle. 4.2. Hardware implementation The hardware implementation of the proposed system includes a STM32F407 MCU-based main control interface circuitry, a STM32F103 MCU-based control subsystem, four kinds of hardware devices for encoder system, and a compass sensor as shown in Fig. 5. Firstly, three Planet DC motors with the rated 24 V, 60 W, 9000 rpm are built as omnidirectional wheels in the mobile robot. The speed of wheel motor is measured using an encoder with the rated 13 ppr. Secondly, the encoder for feedback on the distance was HEDS 5540 500 implse 3 Kamal ( Omni Wheel direction 0h Encoder for distance feedback Compass sensor Omni Wheel directional 8h Planet DC motor Planet DC motor And encorder And encorder Main-interface circuitry Omni Wheel Sub-interface directional 4h circuitry Planet DC motor Bluetooth HC05 And encorder Figure 5. Experimental set-up. Finally, the compass sensor was used to determine the angle of mobile robot using the MPU 6050 GY-521 ( It contains a Tri-Axis angular rate sensor with a sensitivity up to 131 LSBs/dps in a full-scale range of 250 , 500 , 1000, and 2000dps. Furthermore, the main interface circuitry is a self-developed platform with an embedded STM32F407VGT6 microcontroller (MCU) which consists of the ARM 32-bit Cortex-M4 CPU with a float point unit single precision, embedded memories of 1 MB flash memory and 192 kB SRAM, and three 12-bit ADCs. With the embedded universal synchronous/ asynchronous receiver transmitters (USART) and ST-LINK/V2 interfaces and the sub, the interface circuitry is developed by STM32F103VG using the Cortex-M3 core, at a maximum CPU 352
9. Development of directional algorithm for three-wheel omnidirectional autonomous mobile robot speed of 72 MHz. The portfolio covers from 16 kbytes to 1 Mbyte of Flash with motor control peripherals, USB full-speed interface and CAN. 5. RESULTS AND DISCUSSION The auto mobile robot is desired to operate at an angle of 30o and a motor rate ranging from 2000 to 0 rpm depending on encoder feedback for directionally forwarding and backwarding. 4000 40 Desired position for Adjust angle forward directional Adjust postion 2000 30 Target postion ) degree 0 20 ( Angle Desired position for -2000 backward directional 10 Backward directional Thepositional feedback from encoder -4000 0 0 2 4 6 8 10 12 Time (s) Figure 6. Adjustment of steering angle and position of mobile robot. 60 Caculated Wheel 8h Measured 40 20 ) Wheel 4h Wheel 4h Rpm ( 0 -20 Wheel 0h -40 Wheel 0h -60 Wheel 8h Speedof each wheel -80 -100 0 2 4 6 8 10 12 Time (s) ` Figure 7. Adjustment of steering speed of each wheel. From the results, the auto mobile robot needs 2 seconds to adjust the angle and 6 seconds to increase from 0 to 2000 rpm when moving forward (feedback from the encoder). It takes the same duration for the robot to decrease from 2000 to 0 rpm when moving backward. Figure 7 and Fig. 8 show that the 0 h directional wheel has a mission of adjusting the moving direction of the mobile robot, so the speed of this wheel is lower than that of 4 h and 8 h directional wheels and Fig. 9 show the time respond at difference angles from 30 to 600. Apart from that, as can be seen from 353
10. Le Phuong Truong, Huan Liang Tsai, Huynh Cao Tuan Fig 6, the duration for the auto mobile robot to reduce the speed from maximum value to 0 for each wheel is 0.5 s. 60 Wheel 8h Wheel 0h 50 ) Wheel 4h m p R ( l 40 e e h w h 30 c a e f o d 20 e e p S 10 0 0 1 2 3 4 5 6 Time (s) Figure 8. Speed of each wheel at an angle of 60 Figure 9. Time respond at difference angles. degree. In order to easily assess the precision of the proposed system, the difference between the results of measurement and calculation for omnidirectional autonomous robot output speed and angle is defined as e = xi − xˆi (13) th where xi and xˆi are the i values of measurement and calculation, n is the total number of observations. The results of difference analysis for the omnidirectional output speed for each wheel are depicted in Fig. 10. 6 Wheel 0h 5 Wheel 4h 4 Wheel 8h ) 3 rpm ( 2 1 0 Difirenceof speed -1 -2 -3 -4 0 2 4 6 8 10 12 Time (s) Figure 10. Difference analyses between measurement and calculation results. 354
11. Development of directional algorithm for three-wheel omnidirectional autonomous mobile robot The difference ranges for the omnidirectional output speed of each wheel are listed in Table 3. Furthermore, the corresponding root mean square errors (RMSE) with reference to the simulation are given by [13]. n 2 (xi − xˆi )  = i=1 (14) RMSE n Table 3. Difference analyses of proposed system. items e() rpm  RMSE ()r p m Wheel 0 h -3 5 1.66 Wheel 4 h -3 1 0.59 Wheel 8 h -3.52 3 1.40 Table 3 reveals the RMSE results of omnidirectional output speed of 1.66 rpm, 0.59 rpm and 1.40 rpm for 0 h, 4 h and 8 h wheels, respectively. Based on the confidence in algorithm for omnidirectional devices, the proposed system is validated to have a sufficient degree of precision and confidence in the control ability. 6. CONCLUSIONS This paper presented the self-development of a directional algorithm to control three omnidirectional wheels of autonomous mobile robot system including the encoder system and compass sensor based STM32F407 and STMF103MCU with interface circuitry. From a practical viewpoint, the directional algorithm to control three omnidirectional wheels of autonomous mobile robots has many practical advantages including self-adjust the angle and position; erase the sensor for tracking line of the automobile robot; self-developed; ensure reliable accuracy. In the other way, another important practical implication is that the effectiveness of the low-level controller results in simplification of the high-level one. CRediT authorship contribution statement. Phan Le Phuong Truong: methodology and experiments, Huan Laing Tsai: Reviewing and Editing; Huynh Cao Tuan: Supervision. Declaration of competing interest. The authors declare that, they have no known competing financial interests or personal relationships that could have appeared to influence the work reported in this paper. REFERENCE 1. Cooney J. A., Xu W. L., Bright G. - Visual dead-reckoning for motion control of Mecanum- wheeled mobile robot, Mechatronics 14 (6) (2004) 623-637. 2. Guo S., Jin Y., Bao S., Xi F. F. - Accuracy analysis of omnidirectional mobile manipulator with mecanum wheels, Advances in Manufacturing 4 (4) (2016) 363-370. 3. Yamada N., Komura H., Endo G., Nabae H., Suzumori K. - Spiral mecanum wheel achieving omnidirectional locomotion in step-climbing, In the Proceedings of the IEEE 355
12. Le Phuong Truong, Huan Liang Tsai, Huynh Cao Tuan International Conference on Advanced Intelligent Mechatronics 2017, pp. 1285-1290. DOI: 10.1109/AIM.2017.8014195. 4. Feng H. M., Chen C. Y., Horng J. H. - Intelligent omni-directional vision-based mobile robot fuzzy systems design and implementation, Expert Systems with Applications 37 (2010) 4009-4019. 5. Hashemia E. J., Adidi M. G., Jadidi N. G. - Model-based PI–fuzzy control of four-wheeled omni-directional mobile robots, Robotics and Autonomous Systems 59 (2011) 930-942. 6. Li B., Du H., Li W., Zhang Y. - Side-slip angle estimation based lateral dynamics control for omni-directional vehicles with optimal steering angle and traction/brake torque distribution, Mechatronics 30 (2019) 348-362. 7. Jessivaldo Santos, Andr´e G. S. Concei¸c˜ao, Tito L. M. Santos - Trajectory tracking of Omni-directional Mobile Robots via Predictive Control plus a Filtered Smith Predictor, IFAC Papers OnLine 50 (1) (2017) 10250-10255. 8. Terakawa T., Komori M., Yamaguchi Y., and Nishida Y. - Active omni wheel possessing seamless periphery and omnidirectional vehicle using it, Precision Engineering 56 (2019) 466-475. 9. Galicki M. - Collision-free control of an omni-directional vehicle, Robotics and Autonomous Systems 57 (2009) 889-900. 10. Kalmár T. Nagy, D’Andrea R., Ganguly P. - Near-optimal dynamic trajectory generation and control of an omnidirectional vehicle, RobotAutonSyst 46 (2004) 47-64. 11. TamásKalmá Nagy T. - Real-time trajectory generation for omni-directional vehicles by constrained dynamic inversion, Mechatronics 35 (2016) 44-53. 12. Zulfatman Z., Rahmat M. F. - Application of self-tuning fuzzy PID controller on industrial hydraulic actuator using system identification approach, The International Journal on Smart Sensing and Intelligent Systems 2 (2) (2009) 246-261. 13. Chouder A., Silvestre S., Sadaoui N., Rahmani L. - Modeling and simulation of a grid connected PV system based on the evaluation of main PV module parameters, Simulation Modelling Practice and Theory 20 (1) (2012) 46-58. 356