- Email: [email protected]
Double-level ball-riding robot balancing: From system design, modeling, controller synthesis, to performance evaluation
Mechatronics 24 (2014) 519–532
Contents lists available at ScienceDirect
Mechatronics journal homepage: www.elsevier.com/locate/mechatronics
Double-level ball-riding robot balancing: From system design, modeling, controller synthesis, to performance evaluation Kanjanapan Sukvichai ⇑, Manukid Parnichkun Mechantronics, Asian Institute of Technology, P.O. Box 4, Klong Luang, Pathumthani 12120, Thailand
a r t i c l e
i n f o
Article history: Received 9 September 2013 Accepted 10 June 2014 Available online 14 July 2014 Keywords: Ball-riding robot Non-linear system Sensor fusion Linear Quadratic Regulator plus Integral
a b s t r a c t In this research, a new robot, double-level ball-riding robot, is introduced. The robot consists of an upper ball-riding subsystem and a lower ball-riding subsystem. The robot’s dynamics model can be considered separately in two identical planes. Euler–Lagrange equation of motion is applied in order to obtain the dynamics model. Motors are included in the robot’s model. The model is then linearized. The robot’s parameters are identified. The robot’s prototype is manufactured and assembled. Linear Quadratic Regulator with Integral (LQR + I) controller is proposed and applied in order to balance both levels of the robot. The complementary and orientation transformation are used to fuse sensors in order to obtain robot leaning angles. Balancing performance of the developed double-level ball-riding robot is evaluated by simulations and experiments. The results show efficient control performance of LQR + I controller. Ó 2014 Elsevier Ltd. All rights reserved.
1. Introduction Balancing of unstable robot is one of the most challenge topics for researchers in the field of dynamics control. Ball-riding robot or BallBot is an interesting topic from both mechanism and a controller design aspects since the robot is usually a non-linear unstable system. It requires appropriate controller to make the robot become stable. The pioneer work of BallBot was introduced by Lauwers et al. [1] from Carnegie Mellon University (CMU) in 2005. This BallBot used concept of inverted mouse driving mechanism and it was controlled by LQR + PI algorithm. In 2008, Peng et al. [2] and Ching-Wen and Ching-Chih [3] introduced ball-riding robots which changed the driving wheels from regular solid fixed wheels, which were used in CMU BallBot, to omni-directional wheels. Kumagai and Ochiai [4] demonstrated cooperative BallBots for the first time. These BallBots could perform more complicated tasks such as object carrying using human-robot or multi-robot cooperation. Fankhauser and Gwerder [5] introduced a service BallBot which could move in high speed. Lotfiani et al. [6] focused on a trajectory tracking motion of a single level robot. Although, these BallBots could be balanced successfully, all the BallBots still have only single level. Double-level ball-riding robot is a system which contains two cooperative Ballbots. An upper level Ballbot rides on top of a lower level Ballbot. There is a limitation when constructing a tall ⇑ Corresponding author. Tel.: +66 2 524 6054; fax: +66 2 524 6432. E-mail address: [email protected] (K. Sukvichai). http://dx.doi.org/10.1016/j.mechatronics.2014.06.003 0957-4158/Ó 2014 Elsevier Ltd. All rights reserved.
single-level Ballbot which has large moment of inertia since it requires large actuators in order to balance the Ballbot. Multi-level ball-riding robot can solve this problem. By placing a short Ballbot on top of another short Ballbot, the robot will be taller. The number of levels depends on the required height. Since the short Ballbot has small moment of inertia, it requires only small actuators. Furthermore, a multi-level ball-riding robot is more flexible than a single-level ball-riding robot. By controlling the motion of each level cooperatively, a very stable system can be achieved. It can even perform the motion like robot manipulator. This concept can be extended to a building which can resist earthquake. Last but not least important in dynamic control course, it is an excellent platform to test control performance of different control algorithms. In this research a double-level ball-riding robot is introduced. The mechanism is designed and developed. Both levels of the robot are then controlled by using an optimal controller. 2. Mechanic design Each level of the robot is designed by using inverted mouse mechanism. The mathematical model can be derived easily since the sagittal and coronal planes of the robot are decoupled. Each level of the robot is driven by four driving wheels attached to 100 W DC motors through belt and pulley transmission. Driving wheels are placed in perpendicular to the ball surface at a distance from the ball center. The inverted mouse mechanism of each level of the robot is shown in Fig. 1. In the original design, the robot
520
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Driving Motor Encoder
The Euler–Lagrange equation of motion is used to derive dynamics model of the double-level ball-riding robot. Friction in the system is assumed small and negligible. The dynamics equation is obtained using Eq. (1).
d @L @L ¼ f NP dt @ q_ @q
Wheel
Fig. 1. Driving mechanism of double-level ball-riding robot.
ð1Þ
Lagrangian, L, is the difference between total kinetic energy, K, and total potential energy, P, of the robot and is expressed as P P L¼ K P. The value of L is shown in Appendix A. By solving Eq. (1), the dynamics equations of the robot are obtained as shown in Eqs. (2)–(4). d @L @L ¼ mu rb k2 Sinðb þ hÞb_ 2 2mu rb k2 Sinðb þ hÞb_ h_ dt @ /_ @/ ml rb k1 SinðhÞh_ 2 mu r b lSinðhÞh_ 2 mu rb k2 Sinðb þ hÞh_ 2
wheels are made from nylon tubes. Even though the nylon-tube wheels can be used to drive the robot, however there is a problem of large friction between the ridden ball and the nylon-tube wheels when the ridden ball moves in perpendicular to the wheel rotating axis. Furthermore, since the ridden ball always deforms caused by the robot weight, the ball cannot be considered as a solid spherical ball. The ball deformation results in change of the contact type between the ball and wheels from point contacts to area contacts. The increasing friction creates nonlinear behavior. This effect is more severe when the upper-level subsystem is placed on top of the lower-level subsystem. By introducing omni-directional wheels, these problems are solved. Each omni-directional wheel has two degree of freedoms. Wheel rollers help reduce friction between the ball and the wheel. The friction then becomes small. Omni-directional wheels are selected as robot driving wheels. Two identical ball-riding subsystems are used for both lower and upper levels. However the ridden ball of the upper subsystem is attached to the chassis of the lower subsystem. The double-level ball-riding robot is shown in Fig. 2.
€ þ ml rb k1 CosðhÞ€h 2mw r b lSinðhÞh_ 2 þ mu rb k2 Cosðb þ hÞb þ mu rb lCosðhÞ€h þ mu rb k2 Cosðb þ hÞ€h þ 2mw r b lCosðhÞ€h 2€ 2€ 2€ € þ mb r 2 / € þ ib / b þ ml r b / þ mu r b / þ 4mw r b /
þ 2mw r b rw CosðcÞCosðhÞ€h þ 2mw r b rw CosðcÞCosðb þ hÞ€h þ 2mw r 2b CosðcÞCosðhÞ€h þ 2mw r 2b CosðcÞCosðb þ hÞ€h € þ 2mw r 2 CosðcÞCosðb þ hÞb € þ 2mw r b rw CosðcÞCosðb þ hÞb b 2mw r b rw CosðcÞSinðhÞh_ 2 2mw r b rw CosðcÞSinðb þ hÞh_ 2 2mw r 2b CosðcÞSinðhÞh_ 2 2mw r 2b CosðcÞSinðb þ hÞh_ 2 4mw r b rw CosðcÞSinðb þ hÞb_ h_ 4mw r 2b CosðcÞSinðb þ hÞb_ h_ 2mw r 2b CosðcÞSinðb þ hÞb_ 2 2mw rb rw CosðcÞSinðb þ hÞb_ 2 þ
€ 2iw r2 €h 2iw r b €h 2iw r2b / 2b 2 rw rw rw
ð2Þ
3. Robot Dynamics model 3.1. Mechanical model Dynamics model of the double-level ball-riding robot in x–z and y–z planes are identical because of the symmetry of the robot’s structure. The lower and upper balls have the same radius. The dynamics model of each plane is derived from the projection image as shown in Fig. 3.
Fig. 2. Lower (left) and upper (right) subsystems.
Fig. 3. Double-level ball-riding robot assembly (Left) and projection image of the robot in y–z plane (Right).
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
d @L @L € þ 2iw b € ¼ mu gk2 Sinðb þ hÞ þ mu k2 lSinðbÞh_ 2 þ iu b dt @ b_ @b 2€ € þ 4mw rb r w b € þ 2mw r 2 b € þ iu €h þ 2mw r 2 b þ mu k b 2
b
Then, the generalized force vector is considered. The actuating torques of the BallBot system are applied at the driving wheels. These torques are applied to control all the state variables. From the velocity Jacobian matrix, the generalized forces vector is obtained as expressed in Eq. (5).
w
2 þ 2iw €h þ mu k2 €h þ 2mw r 2b €h þ 4mw r b rw €h þ 2mw r 2w €h € þ mu k2 lCosðbÞ€h þ mu r b k2 Cosðb þ hÞ/
€ þ 2mw rb r w CosðcÞCosðb þ hÞ/ 2 € þ 2mw r CosðcÞCosðb þ hÞ/ þ 2mw lrw CosðcÞCosðbÞ€h
*
f NP
b
þ 2mw lrb CosðcÞCosðbÞ€h 2mw lrw CosðcÞSinðbÞh_ 2 2mw lrb CosðcÞSinðbÞh_ 2 þ 2mw rw gSinðcÞCosðb þ hÞ þ 2mw rb gSinðcÞCosðb þ hÞ þ þ
ð3Þ
sm1 is the input torque from lower motor. sm2 is the input torque from upper motor.
3.2. DC motor model
sL ¼ IR
dxm K me K xm þ m v in dt R R
ð6Þ
where
b
– – – – – – –
2 € þ 2mw r 2 b € € € € € þ 4mw r b r w b w þ il h þ iu h þ 4iw h þ k1 ml h 2€ 2€ 2€ € þ mu l h þ 2mu k2 lCosðbÞh þ mu k2 h þ 2mw l h € þ 4mw r 2 h€ þ 8mw rb r w h€ þ 4mw r 2 h€ þ ml rb k1 CosðhÞ/ b
ð5Þ
For simplicity, armature coil resistance is assumed very small and negligible. The motor dynamics is expressed as shown in Eq. (6).
d @L @L ¼ ml gk1 SinðhÞ mu glSinðhÞ mu gk2 Sinðb þ hÞ dt @ h_ @h 2mw glSinðhÞ mu k2 lSinðbÞb_ 2 2mu k2 lSinðbÞb_ h_ € þ 2iw b € þ mu k2 lCosðbÞb € þ mu k2 b € þ 2mw r 2 b € þ iu b 2
2 3 sm1 rb 6 7 ¼ 4 ðsm1 þ sm2 Þ 5 rw sm2
where – –
2iw r2b €h 4iw rb €h þ rw r 2w
€ 4iw rb b € 2iw r 2b b þ 2 rw rw
521
w
€ þ mu rb k2 Cosðb þ hÞ/ € þ mu r b lCosðhÞ/ € þ 2mw r b r w CosðcÞCosðhÞ/ € þ 2mw r b lCosðhÞ/
sL is the load torque of the driving motor. xm is the motor angular velocity. K m is the motor torque constant. K me is multiplication of back emf and torque constant. R is armature coil resistance. IR is motor inertia. v in is input voltage.
€ þ 2mw r b r w CosðcÞCosðb þ hÞ/ 2 € € þ 2mw r CosðcÞCosðhÞ/ þ 2mw r 2 CosðcÞCosðb þ hÞ/
Angular velocities of motors are then transformed into the robot variables as shown in Eq. (7) and (8).
þ 4mw lrw CosðcÞCosðbÞ€h þ 4mw lrb CosðcÞCosðbÞ€h € þ 2mw lr b CosðcÞCosðbÞb € þ 2mw lrw CosðcÞCosðbÞb
w_ 1 ¼
b
b
rb _ _ h_ ¼ xm1 ð/ hÞ rw rb _ ¼ xm2 w_ 2 ¼ þ 1 ðb_ þ hÞ rw
2mw lrw CosðcÞSinðbÞb_ 2 2mw lr b CosðcÞSinðbÞb_ 2 4mw lrw CosðcÞSinðbÞb_ h_ 4mw lr b CosðcÞSinðbÞb_ h_ þ 2mw r w gSinðcÞCosðhÞ þ 2mw r w gSinðcÞCosðb þ hÞ þ 2mw r b gSinðcÞCosðhÞ þ 2mw r b gSinðcÞCosðb þ hÞ € 2iw rb / € 4iw r b b € 4iw r 2 €h 8iw r b €h 2iw r 2 b € 2iw r 2b / þ 2b þ þ 2b þ 2 rw rw rw rw rw rw ð4Þ
where T
– q ¼ ½/; h; b . – / is the angle of the lower ball. – h is the angle of the lower level subsystem measured from z-axis. – b is the angle of the upper level subsystem measured from center axis of the lower level subsystem. – c is the angle from robot’s vertical axis and vector from center of ball to center of the wheel. – w1 ; w2 are angles of the lower and upper wheels respectively. – r b is the radius of the ridden ball. – r w is the radius of the driving wheels. – k1 is the length from center of the lower ball to center of gravity of the lower level subsystem. – k2 is the length from center of the upper ball to center of gravity of the upper level subsystem. – l is the length from center of the lower ball to center of the upper ball.
ð7Þ ð8Þ
Thus, the required torques for the lower- and upper-level subsystems are expressed in Eqs. (9) and (10) respectively.
rb € rb K me r b _ / þ IR / þ 1 €h rw rw R rw K me r b Km þ þ 1 h_ þ v in1 R rw R
sm1 ¼ IR
rb € þ IR r b þ 1 €h þ K me rb þ 1 b_ þ1 b rw rw R rw K me r b K m þ þ 1 h_ þ v in2 R rw R
sm2 ¼ IR
ð9Þ
ð10Þ
3.3. Robot’s state-space model Double-level ball-riding robot can be expressed by a state-space model as shown in Eq. (11). Matrix D is defined as disturbance matrix. It takes into account the disturbance force from friction, disturbance and uncertainty. Friction in this research is assumed to be a Coulomb friction because in the real operation the ridden ball is operated at a small speed, thus the viscous friction is negligible. The friction occurs at the contacts of the lower ridden ball and the lower driving wheels. The friction of the upper ridden ball and the upper driving wheels is considered as the internal force. This term is considered separately from the robot dynamic model.
522
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532 *
€ þ Cðq; qÞ _ þ GðqÞ þ D ¼ f NP MðqÞq
ð11Þ
where
2
3 M11 M12 M13 6 7 M ¼ 4 M21 M22 M23 5; M31 M32 M33
2
3 C1 6 7 C ¼ 4 C2 5; C3
2
3 G1 6 7 G ¼ 4 G2 5 G3
M11 ¼ ib þ m1 r 2b þ 2iw r 2d M12 ¼ m2 rb CosðhÞ þ m3 r b Cosðb þ hÞ 2iw r2d 2iw r d þ2m6 ðr w þ rb ÞCosðcÞðCosðhÞ þ Cosðb þ hÞÞ M13 ¼ m3 rb Cosðb þ hÞ þ 2m6 ðrw þ r b ÞCosðcÞCosðb þ hÞ
there exists also an uncontrollable rotation around the vertical axis. The small wheels on left and right planes at some instants do not contact the ridden ball at the same time, which make the forces from wheels deviate from the center of the ball. Heading motion occurs as shown in Fig. 4. Wheel misalignment from manufacturing and assembling also creates heading motion. Robot’s weight changes a point of contact between the ridden ball and the ground to an area of contact. Contact points on basketball surface are uniform with the estimated density of 126,000 points per m2 as shown in Fig. 5. Each contact point in the contact area creates friction. Thus, the estimated friction torque can be determined following Eq. (12) when the contact area is considered as a circle.
M21 ¼ M12 M22 ¼ il þ iu þ 4iw þ m4 þ m3 k2 þ 4mw ðr b þ r w Þ2 þ 4iw r2d þ 8iw r d þ2m3 lCosðbÞ þ 4ðm5 þ m6 ÞlCosðcÞCosðbÞ M23 ¼ iu þ 2iw þ m3 k2 þ 2mw ðr b þ r w Þ2 þ 2iw r2d þ 4iw r d þm3 lCosðbÞ þ 2ðm5 þ m6 ÞlCosðcÞCosðbÞ M31 ¼ M13 M32 ¼ M23 M33 ¼ iu þ 2iw þ m3 k2 þ 2mw ðr b þ r w Þ2 þ 2iw r2d þ 4iw r d G1 ¼ 0 G2 ¼ m2 gSinðhÞ þ 2ðm5 þ m6 ÞgSinðcÞCosðhÞ þ2ðm5 þ m6 ÞgCosðb þ hÞ m3 gSinðb þ hÞ G3 ¼ m3 gSinðb þ hÞ þ 2ðm5 þ m6 ÞgSinðcÞCosðb þ hÞ
2 3
sfb ¼ q lmRobot gR where – – – – –
q is the ratio of area of all contact points and the contact area l is the friction coefficient between the ball and the ground mRobot is the total weight of the robot system g is the gravitational acceleration R is the radius of contact area
If torque from the driving wheels is greater than the ground friction torque, the robot rotates around the vertical exist as explained in Eq. (13). An experiment of robot’s heading motion is conducted in the last section. *
C1 ¼ m3 r b Sinðb þ hÞb_ 2 2m3 r b Sinðb þ hÞb_ h_ m2 r b SinðhÞh_ 2 þm3 r b Sinðb þ hÞh_ 2 2m6 ðrw þ r b ÞCosðcÞðSinðhÞ þ Sinðb þ hÞÞh_ 2 4m6 ðr w þ r b ÞCosðcÞSinðb þ hÞb_ h_ þ KRme ðr d Þ2 /_ 2m6 ðr w þ r b ÞCosðcÞSinðb þ hÞb_ 2 KRme ðr d Þðr d þ 1Þh_ C2 ¼ m3 lSinðbÞb_ 2 2m3 lSinðbÞb_ h_ 2ðm5 þ m6 ÞlCosðcÞSinðbÞb_ 2 2ðm5 þ m6 ÞlCosðcÞSinðbÞb_ h_ KRme ðr d Þ2 /_ þ2 K me ðr d Þðrd þ 1Þh_ þ K me ðr d Þðr d þ 1Þb_ R
ð12Þ
*
*
*
ðf wheel1 D r w1 f wheel2 D r w2 Þ sfb ¼ ib Ub where *
*
– f wheel1 ; f wheel2 are force vectors from wheels in planes 1 and 2 respectively * * – D r w1 ; D r w2 are the shortest distant from force to the vertical axis – Ub is the ball angular acceleration along the vertical axis
R
C3 ¼ m3 lSinðbÞh_ 2 2ðm5 þ m6 ÞlCosðcÞSinðbÞh_ 2 þ K me ðr d Þðr d þ 1Þb_ þ K me ðr d Þðr d þ 1Þh_ R
R
where
m1 ¼ ðmb þ ml þ mu þ 4mw Þ m2 ¼ ml k1 þ mu l þ 2mw l m3 ¼ mu k2 2
2
m4 ¼ ml k1 þ mu l þ 2mw l
2
m5 ¼ mw rw m6 ¼ mw rb rb rd ¼ rw 3.4. Heading motion Theoretically, the developed invert mouse driving mechanism allows the robot to translate only on x–y plane, however, heading motion around z-axis (vertical axis) may occur due to several factors including the wheels misalignment, omni-directional wheel characteristic, ground friction, etc. In this section, robot heading motion is considered. An omni-directional wheel consists of small polycarbonate auxiliary wheels installed on two planes at 13 mm gap. Even though, these small wheels rotate freely when the ridden ball is moving in perpendicular to the wheel rotating axis, but
ð13Þ
Fig. 4. Force diagram showing heading motion.
523
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
3 0 0 7 6 0 0 7 6 7 6 v in1 6 0 2 0 3 2 37 B¼6 ; u¼ 7 1 0 7 6 v in2 K 7 6 1 m 4 4 M ðr d Þ KRm 4 1 5 M1 1 5 5 ðr d Þ R 0 1 2
M is the mass matrix evaluated at equilibrium point.
D1 ¼
K me 2 r R d
D2 ¼
K me 2 rd þ rd R
5. Implementation of the double-level ball-riding robot
Fig. 5. Contact points inside a contact area of a basketball.
5.1. Robot hardware and electrical circuit design The heading motion of the robot mainly occurs at the lower sub-system since the upper ridden ball is fixed to the lower frame. 4. Linear approximation of dynamic equation The robot’s state-space model is then linearized around the equilibrium point where all the state variables and control input signal are equal to zero or the robot is at upright position. Eq. (11) can be written as Eq. (14).
8 9 2 3 v in1 > > < = K 7 € ¼ M 1 ðqÞ ðrd Þ m 6 _ GðqÞ q 4 ðv in1 þ v in2 Þ 5 Cðq; qÞ > > R : ; v in2
ð14Þ
Let,
T x ¼ / h b /_ h_ b_ ¼ ½ x1
x2
x3
x4
x5
x6 T
Therefore, 3 x4 7 6 x5 7 6 7 6 7 6 x 6 8 9 7 , f ðx; v in1 ; v in2 Þ € ¼ x_ ¼ 6 2 3 q 7 6 v > > in1 6 < =7 6 6 1 7 _ GðqÞ 7 5 4 M ðqÞ ðr d Þ KRm 4 ðv in1 þ v in2 Þ 5 Cðq; qÞ > > : ; v in2 2
ð15Þ
The linearized dynamics model is obtained by using Taylor series expansion around equilibrium point as shown in Eq. (16).
x_ ffi
@ @ f ðx; v in1 ; v in2 Þx þ f ðx; v in1 ; v in2 Þv in1 @x @ v in1 @ þ f ðx; v in1 ; v in2 Þv in2 @ v in2
ð16Þ
The linearized dynamics model at the equilibrium point can be expressed by Eq. (17)
x_ ¼ Ax þ Bu where,
ð17Þ
In order to design the robot controller efficiently, robot parameters are required. Firstly, the parameters are considered and identified. Body of the double-level ball-riding robot consists of 4 parts. The first part is the ridden ball which is made from rubber with medium–high stiffness. The second part is the driving wheel which has the body made from plastic and its wheels are made from synthesis rubber. The third part is the lower level of the robot which includes the upper ball into its body. The last part is the upper level of the robot which does not include the ball. The robot body frame is made from aluminum grade 6061. Shafts and bearings are made from steel, while the pulleys are made from aluminum. The ridden ball is a standard basketball which is solid enough to be used in the ball-riding robot [7]. Robot parameters are determined by using SolidWorks program. By providing proper part materials, the parameters are finally identified. Photos of robot parts are shown in Fig. 6. Motor parameters are identified from experiments. Two identical 100-W DC motors and an optical encoder with resolution of 1440 pulses per revolution are attached to a support plate with driving belt as shown in Fig. 7. First motor is driven by constant current, The back electromotive force voltage is measured at the second motor. Motor speed is measured by the encoder via microcontroller board. The robot parameters are shown in Table 1. After substitution of robot parameters from Table 1 into Eq. (17), the robot’s state and input matrices become Eq. (18) and (19) respectively.
3 0 0 0 1 0 0 7 60 0 0 0 1 0 7 6 7 60 0 0 0 0 1 7 6 A¼6 7 6 0 173:90 5:197 0:579 0:681 0:022 7 7 6 4 0 67:407 9:359 0:173 0:168 0:042 5 0 77:699 49:605 0:225 0:048 0:226 2
2
0
0
3
7 6 0 0 7 6 7 6 7 6 0 0 7 B¼6 6 1:885 0:058 7 7 6 7 6 4 0:563 0:112 5 0:733
0:605
ð18Þ
ð19Þ
524
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Fig. 6. Parts of double-level ball-riding robot.
half of s-plane. Therefore, the robot is considered as an unstable system. A 32-bit ARM controller is selected as the main controller for the robot. This controller operates at 72 MHz and has variety of peripheral interfaces. A motor driver which is full-bridge DC motor driver can drive the motor with continuous current up to 40 A at 24 V. An Inertia Measurement Unit or IMU which consists of accelerometer (ADXL345) and gyro sensor (ITG3200) is used to measure the robot’s tilt angle. Upper level-and lower-level subsystem can communicate between themselves by using wireless module from Nordic Semiconductor Company which has carrier frequency at 2.4 GHz and can handle up to the maximum of 32 bytes. Robot controller and other electrical circuit are shown in Fig. 8.
Fig. 7. Experiment set-up for motor parameters identification.
Table 1 Robot parameters. Parameter
Value
Unit
Km K me R rb rw k1 k2
0.00291737 0.04454 1.64 0.1210 0.0258 0.1474 0.1211 45 0.5081 0.1099 0.0545 9.0508 8.8567 1030.483106 15.2493106 266508.430106 235425.849106
kg cm/A V kg cm/(A rad/s) Ohms m m m m degree m kg kg kg kg kg m2 kg m2 kg m2 kg m2
c l mb mw ml mu ib iw il iu
All the state variables of the robot are checked and found observable and controllable. However, after determine the eigen values of its characteristic equation, some of system poles are on the right
5.2. Robot software The robot controller is programmed with C language. The program also includes robot’s tilt angle estimation and balancing control. Robot’s tilt angles from vertical z axis along x and y direction are required in balancing control of the robot. Accelerometer and gyro sensor are used to determine the robot’s tilt angles. Data from these two sensors have to be fused in order to obtain the accurate leaning angles. There are many filter algorithms that can manipulate and attenuate the noise. Complementary filter is simple and good filter that can improve the quality of the measured angle. The rates of change of angle from gyro sensor are fused to the data from the accelerometer by using concept of combination of low pass and high pass filters. The acceleration data is passed through the low pass filter to remove measurement noise in the sensor. The rates of change of angle from gyro sensor are fed to numerical integration algorithm to change the rates to numerical angles then passed through the high pass filter. The filtered data are then fused together and the robot tilt angles are estimated. The block diagram of the complementary filter is shown in Fig. 9. In order to use complementary filter to estimate the robot tilt angles, information from both gyro sensor and accelerometer must be transformed to world coordinate. Although, roll, pitch and yaw angular velocity are obtained from the gyro sensors, they are measured respect to the sensor coordinate. Unlike gyro sensor, accelerometer is used to measure gravity
525
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Wireless Module Motor Driver
Main Controller
Fig. 10. Diagram of the world and sensor coordinates.
IMU
The angular velocity which is measured respect to the sensor coordinate is transformed to the angular velocity respect to the world coordinate by using Eq. (22). Fig. 8. Controller board and electrical circuit.
2
acceleration which always measured respect to the world coordinate. Thus, these two measured data cannot be fused immediately. The data from gyro sensor must be transformed to the world coordinate using direct cosine method [8]. Rotation matrix of the robot coordinate respect to the world coordinate Rzyx is determined as shown in Fig. 10. Subscript R and W represent the sensor and the world coordinates respectively. The last row of the rotation matrix contains information about angles of the sensor respect to the world z-axis which is an important information used to estimate robot’s tilt angles. By defining Rzyx ð3Þ matrix from the last row of the rotation matrix, thus,
Rzyx ð3Þ ¼ ½ sinðhÞ sinð/Þ cosðhÞ cosð/Þ cosðhÞ ¼ ½ r zx
rzy
r zz ð20Þ
In order to fuse gyro data, the relationship of rate of change of angle from sensor coordinate and world coordinate is required. The relationship is the cross product of the measured angular velocity of the * sensor x and the last row element vector of the rotation matrix as shown in Eq. (21).
2 *
r zx
3
* 6 7 r_ ¼ x 4 rzy 5
r zz
ð21Þ
3 2 3 rzx ½n þ 1 K c r zx ½n þ ð1 K c Þr xacc ½n 6 7 6 7 4 r zy ½n þ 1 5 ¼ 4 K c r zy ½n þ ð1 K c Þr yacc ½n 5 rzz ½n þ 1 K c r zz ½n þ ð1 K c Þr zacc ½n
ð22Þ
Then, information from both sensors is fused by using complementary filter [9]. It can be expressed in Eq. (23).
2
rzx ½n
3
2
r zx ½n 1 þ r zy ½n 1dw r zz ½n 1dh
3
6 7 6 7 4 r zy ½n 5 ¼ 4 r zy ½n 1 r zx ½n 1dw þ r zz ½n 1d/ 5 r zz ½n 1 þ r zx ½n 1dh r zy ½n 1d/ rzz ½n
ð23Þ
where – – – –
K c is the complementary gain. dw is the yaw angular velocity of the gyro sensor. dh is the pitch angular velocity of the gyro sensor. d/ is the roll angular velocity of the gyro sensor.
These three axis component values are then normalized and used to estimate the robot’s tilt angles in pitch and roll direction as shown in Eqs. (24) and (25) respectively.
0
1
rzx ½n þ 1 B C /est ½n þ 1 ¼ tan1 @qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiA 2 r zy ½n þ 1 þ r 2zz ½n þ 1 r zy ½n þ 1 hest ½n þ 1 ¼ tan1 r zz ½n þ 1
ð24Þ
ð25Þ
Contact point location on the ridden ball is determined from the wheel encoders. Resolution of the wheel encoders is multiplied by gear transmission ratio and also by four when the signal is captured using built-in quadrature encoder capture module inside the ARM microcontroller. The resolution of the ridden ball contact point location is 0.01 radians. 6. Control algorithm
Fig. 9. Block diagram of the complementary filter.
In this research, LQR + I is proposed to control and balance the developed double-level ball-riding robot. LQR + I is an optimal controller that feeds all state variables and error integration through the optimal gains to produce the optimal response. LQR + I is the
526
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
result of adding integration of error as another state variable for LQR in order to achieve superior steady-state performance. Integral R term is added with LQR by defining the state variable ze ¼ e dt. The state-space representation of the robot system is then modified as shown in Eq. (26).
A 0 xe B x_ e x_ x_ 0 ¼ ¼ þ u ¼ AX þ Bu X_ ¼ _ze C 0 ze 0 x x0
ð26Þ
where – C is the output matrix. – x0 is the exogenous input vector. The control law is now expressed in Eq. (27).
u ¼ KX ¼ ½ K f
Ki
xe
¼ K f ðx x0 Þ K i C
ze
Z
ðx x0 Þdt ð27Þ
The optimal gain matrix K f and K i can be determined by solving the algebraic Riccati equation of nominal model of the robot. Block diagram of the controlled system is shown in Fig. 11. By applying backward Euler method to Eq. (27), the discrete LQR + I control law can be implemented by using Eq. (28).
2
u½n ¼ u½n 1 þ ½ K f
e/ ½n e/ ½n 1
3
7 6 eh ½n eh ½n 1 7 6 7 6 7 6 e ½n e ½n 1 b b 7 6 6 ðe ½n 2e ½n 1 þ e ½n 2Þ=Dt 7 7 6 / / / 7 6 7 K i 6 6 ðeh ½n 2eh ½n 1 þ eh ½n 2Þ=Dt 7 7 6 6 ðeb ½n 2eb ½n 1 þ eb ½n 2Þ=Dt 7 7 6 7 6 e/ ½nDt 7 6 7 6 eh ½nDt 5 4 eb ½nDt ð28Þ
where – u½n ¼ ½ v in1 ½n v in2 ½n T – Dt is the sampling time. – e/ ; eh ; eb are the error between robot state variables and x0 . Control signals denoted by v in1 and v in2 are sent to the pairs of lower and upper motor drivers which drive the robot in the same direction respectively. Control signals for x and y direction motions are determined the same way.
simulation has the same mass, inertia and height as the doublelevel robot in order to investigate the behaviors of the two robots at very similar conditions. Both robots are controlled by the conventional LQR. Disturbance is applied at the top of the robot body to generate a change of robot leaning angle. Robot leaning angles of the single-level and the double-levels robot are shown in Figs. 12 and 13 respectively. From the simulation results, robot leaning angle has large fluctuation when the disturbance is applied to the single-level robot. Leaning angle of the double-level robot has smaller fluctuation. Ground contact point locations of both robots are show in Fig. 14. Ground contact point position of the double-level robot deviates much less than the single-level robot. The result is because in the double-level robot the actuating motors from both lower-level and upper-level subsystems work simultaneously to suppress the effect from disturbance and keep the robot stability while the single-level robot has only actuating motors from one robot on the ridden ball to compensate the effect from disturbance. From the simulation results, the double-level ball-riding robot is more stable than the single-level ball-riding robot. 7.2. Comparison of LQR and LQR + I LQR performance on double-level ball-riding robot was evaluated in our previous work [10]. It could control the robot when the robot is operated in a controlled environment. In the real operation, the gyro sensor and accelerometer always have output biases. The estimated leaning angle which is obtained from sensors fusions shows the drift. The optimal controller LQR and LQR + I are simulated and compared in this section. Biases from both sensors and saturation of the motors are added to the simulation in order to obtain the performances of LQR and LQR + I. In the first simulation, the robot has to keep its location at the origin point while the sensor signal of robot leaning angle has a drift. From the simulation results, both controllers can maintain the robot stability. The robot controlled by LQR + I can keep it position around the original position while the robot controlled by LQR has location drift from the original point as shown in Fig. 15. The second simulation is set up in order to obtain the effect of the static bias when the robot is driven to a desired location. Fig. 16. shows the result when the robot moves to the desired location when a small bias is applied to the robot while Fig. 17. shows the result when a large bias is applied to the robot. From the simulations, LQR + I is more robust than the conventional
7. Simulation and experimental results 7.1. Comparison of double-level and single-level ball-riding robots In this section, a single-level and a double-level ball-riding robots are tested and compared. The single-level robot in the
Fig. 11. Block diagram of LQR + I.
Fig. 12. Leaning angle of single-level ball-riding robot.
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Fig. 13. Leaning angle of double-level ball-riding robot.
LQR when the robot is in the presence of bias and motor saturation. 7.3. Full robot simulation Control performance of LQR + I on the double-level ball-riding robot is tested by simulation on MATLAB Simulink firstly. The linearized state-space representation of the robot in one plane is considered because of its symmetric property between x–z and y–z planes. The desired operating point is very close to the robot equilibrium point where the robot is in upright position and stationary. All the robot’s state variables are obtained from the robot’s model in MATLAB simulink. Gaussian noise is added to the states and output. LQR + I is implemented to control the robot. The simulation result of the robot’s leaning angle in y–z plane is shown in Fig. 18. Even though the leaning angles of the lower and upper levels fluctuate around zero at upright position due to measurement noise, but the robot can still keep its balancing. The result of the angle of the ridden ball is shown in Fig. 19. The result shows that the robot moves back and forth in order to maintain its balancing even in the presence of noise. By combining the results from two planes, the robot position is shown in Fig. 20.
Fig. 14. Ground contact point location of the robots.
527
Fig. 15. Comparison of LQR and LQR + I with sensor drif.
7.4. Balancing experimental result Lower and upper levels are assembled together. The upper ridden ball is fixed to the lower level by using high strength epoxy. Each level subsystem has been tested for its balancing performances before assembling. Each level subsystem has enough power to maintain its balancing while carrying the other subsystem on top. Fully assembled robot is shown in Fig. 21. Discrete LQR + I control algorithm is implemented to the main microcontroller. Experiments are setup in order to test the balancing performance of the double-level ball-riding robot. Tilt angles in both x–z and y–z plane of each level subsystem are measured and transmitted to the other level subsystem via wireless communication. Wheel positions are obtained via optical encoders. Control signal from LQR + I algorithm is calculated and transmitted to the motor driver. State variables are recorded by computer via serial communication. In the experiments, state-weighting matrix (Q) is defined by assigning more weight to the leaning angles of both levels and their error integration than the other states because balancing of the robot is the primary objective than robot position. This criteria is applied for both x–z and y–z planes. From the experiments, the double-level ball-riding robot can maintain its balancing by using
Fig. 16. Comparison of LQR and LQR + I with small sensor bias.
528
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Fig. 17. Comparison of LQR and LQR + I with large sensor bias.
Fig. 20. Simulation result of robot position.
Fig. 18. Simulation results of robot leaning angles on y–z plane using LQR + I.
Fig. 21. The lower- (left) and upper- (right) level subsystems.
The leaning angles of the robot obtained from real experiments show similarity to the simulation results as shown in Fig. 24. The leaning angles of the robot always fluctuate around zero at upright position and the robot can maintain its balancing. Although the robot can keep its balancing but the angle of the ridden ball shows some drift from the zero angle which means the robot moves from the origin point due to the effect of some surface slip. Control signal is recorded and shown in Fig. 25. The control signal does not saturate and has the maximum duty cycle of 40%. The positions of the robot from 5 experiments are shown in Fig. 26. Even though, the robot has some drifts in its position, but the robot tries to come back to the origin point every times. Priority of the robot location is lower than the robot’s leaning angles based on the defined weighting matrices.
Fig. 19. Angle of the ridden ball.
LQR + I controller. Robot can keep its upright position all the time as shown in Fig. 22. The robot’s leaning angles in y–z plane from the experiment is shown in Fig. 23.
7.5. Tracking experimental result In this section, the experiments are setup in order to obtain the robot tracking performance of a double level robot. The stateweighting matrix, Q, is redefined in this experiment because the position of the robot is more focused than the one in the previous experiment. By varying the Q matrix, the feedback gains change.
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
529
Fig. 22. Photo taken from the experiment.
Fig. 23. Robot leaning angles on y–z plane using LQR + I from experiment.
Fig. 24. Angle of the ridden ball from experiment.
The robot is required to move along the desired trajectory. The experimental results are shown from Figs. 27–30. The robot position along x-axis and the desired trajectory are shown in Fig. 27. From the experiment, the robot moves forward and backward in order to keep its stability similar to the results from the
experiment in Section 7.4. At the same time, the robot tries to move along the desired trajectory with position error due to some disturbance and uncertainty. Location of the ground contact point of the robot on x–y plane is shown in Fig. 28. From the result, the robot can move to the desired location with some error. The error
530
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Fig. 25. Control signal of the lower-level robot.
Fig. 28. Ridden ball ground contact location on x–y plane.
Fig. 26. Robot positions from 5 experiments. Fig. 29. Comparison of leaning angles of lower-level robot from experiments in Sections 7.4 and 7.5.
Fig. 27. Ridden ball ground contact location along x-direction. Fig. 30. Control signal of lower-level robot in position tracking experiment.
occurs because the robot loses its stability when it tries to move as shown in Fig. 29. The result shows the comparison between leaning angle of the lower-level robot when the stability is more concerned than the position. Leaning angle of the lower-level robot
when position is more concerned has fluctuation with twice magnitude compared to the robot when position is less concerned. From the experimental result, even though the robot can move along the desired trajectory but it loses its stability easily compared with the previous experiment in Section 7.4.
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
531
Fig. 31. Photos from heading motion experiment.
Control signal of the motor has larger magnitude and higher frequency than the control signal for the previous experiment due to position and leaning angle control. Control signal still does not saturate as shown in Fig. 30.
7.6. Heading motion experimental result In this section, the experiment is set up in order to measure heading motion of the robot. The state-weighting matrix, Q, is the same as the one in the tracking experiment in order to keep the robot’s position. Heading angle of the robot around the vertical
axis is measured by using IMU sensor. The photos from the experiment are shown in Fig. 31. From the experiment, heading of the lower sub-system changes all the time during the experiment while heading of the upper sub-system does not change. The upper sub-system does not change its heading because the ridden ball is fixed to the lower frame. Heading direction of the lower sub-system fluctuates within a small angle. Even though, heading direction of the lower sub-system is uncontrollable, however, it fluctuates around zero angles in small amplitude with about zero mean. From the experiment, heading direction of the lower subsystem changes to one direction when the robot moves in one direction and changes to the opposite direction when the robot moves in opposite direction. This behavior repeats in the experiment. It happens because of non-uniform surface of basketball and ground characteristics. Mean of heading angle of the lower sub-system is 0.000439 radians with standard deviation of 0.0229 radians. According to the experiment result, the heading direction of the robot is small and can be negligible. The measured heading angle is shown in Fig. 32. 8. Conclusion
Fig. 32. Heading angle around vertical axis of lower sub-system.
Double-level ball-riding robot was introduced in this research. Mechanism design and hardware architecture of the robot were explained. Dynamics model of the robot was obtained by Euler– Lagrange equation of motion. Dynamics model of DC motor was derived and included in the robot’s dynamics model. The model was then linearized around its equilibrium point where robot was in upright position and stationary. Real robot was manufactured. The robot and motor parameters were then identified. ARM 32-bit microprocessor was selected as the robot main
532
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
controller because of its performance. Data from gyro sensor and accelerometer were fused together by using direct cosine method and complementary filter in order to estimate the robot’s leaning angle. In this research, a single-level and double-level ball-riding robots are explored and compared. The double-level ball-riding robot has shown a better performance than a single-level ball-riding robot with the presence of disturbance. The double-level ball-riding robot could reject the disturbance because the two level subsystems work cooperatively together. LQR + I was introduced and proposed in this research to control the robot in order to overcome the sensor drift and motor saturation problems. LQR + I has shown more robust than the conventional LQR. Balancing performance of the robot was tested by simulation in noisy environment in order to imitate the real system. LQR + I controller has shown a good balancing performance in both simulation and real experiments. The robot’s leaning angles fluctuate around equilibrium point with small deviation. Robot moved around the origin point during balancing. Both simulated and experimental data have shown similar results that the robot could keep its balancing. The experiment results have shown position drift due to surface slip. Trajectory tracking performance was setup and experimented. The experimental results have shown that the robot could track the desired trajectory and maintain its balancing stability at the same time. Balancing stability and position control were traded off in the experiment. The robot would lose its stability easier than the balancing stability experiment because of the lower weight. Position error occurred because of disturbance from contact slip and model uncertainty. Heading angle of the ball-riding robot may change because of wheels misalignment and omni-directional wheel characteristic. The heading angle is uncontrollable because of the limitation of the designed mechanism, however, the change of heading angle is very small and negligible as seen from the experiment. Suppression of the disturbance and uncertainty using robust controller such as a sliding mode controller or H1 controller is the main focus in our future research. Appendix A Lagrange equation is a function that describes a dynamic system using energy approach concept. The Lagrangian, L, in Eq. (1) is the difference between potential energy, P, and kinetic energy, P P K, or L ¼ K P. The total potential energy of the double-level ball-riding robot is shown in Eq. (29) while the total kinetic energy is shown in Eq. (30). X
P ¼ gmb r b þ gmw ðrb ðr b þ r w ÞSinðc þ hÞÞ þ gml ðr b þ k1 CosðhÞÞ þ gmw ðr b þ lCosðhÞ ðr b þ r w ÞSinðc þ ðb þ hÞÞÞ þ gmw ðr b þ lCosðhÞ þ ðr b þ r w ÞSinðc ðb þ hÞÞÞ þ gmu ðr b þ lCosðhÞ þ k2 Cosðb þ hÞÞ þ gmw ðr b þ ðr b þ r w ÞSinðc hÞÞ ð29Þ
X
K ¼ 0:5ib /_ 2 þ 0:5mb r 2b /_ 2 þ 0:5iw
!2 _ rb ð/_ hÞ h_ rw
_ 2Þ þ 0:5mw ððrb þ rw Þ2 Cosðc hÞ2 h_ 2 þ ððrb þ rw ÞSinðc hÞh_ þ rb /Þ 2 _ 2Þ þ 0:5il h_ 2 þ 0:5ml ðk1 SinðhÞ2 h_ 2 þ ðk1 CosðhÞh_ þ r b /Þ
_ 2 þ 0:5mw ððlSinðhÞh_ ðrb þ rw ÞSinðc þ ðb þ hÞÞðb_ þ hÞÞ _ þ rb /Þ _ 2Þ þ ðlCosðhÞh_ þ ðr b þ rw ÞCosðc þ ðb þ hÞÞðb_ þ hÞ _ 2 þ 0:5mu ððlSinðhÞh_ k2 Sinðb þ hÞðb_ þ hÞÞ _ 2 þ 0:5iu ðb_ þ hÞ _ 2 þ 0:5mw ððlSinðhÞh_ þ ðrb þ rw ÞSinðc ðb þ hÞÞðb_ þ hÞÞ _ þ rb /Þ _ 2Þ þ ðlCosðhÞh_ þ ðr b þ rw ÞCosðc ðb þ hÞÞðb_ þ hÞ _ þ rb /Þ _ 2Þ þ ðlCosðhÞh_ þ k2 Cosðb þ hÞðb_ þ hÞ 2 rb _ 2 þ ððrb þ rw ÞSinðc þ hÞh_ þ rb /Þ _ 2Þ ðb_ þ hÞ þ 0:5iw 1 rw !2 _ rb ð/_ hÞ h_ þ 0:5mw ððrb þ rw Þ2 Cosðc þ hÞ2 h_ 2 þ 0:5iw rw 2 rb _ 2 þ 0:5iw 1 ðb_ þ hÞ ð30Þ rw References [1] Lauwers T, Kantor G, Hollis R. One is enough! In: 12th Int’l symp. on robotics research. Robotics Institute, Carnegie Mellon University, San Francisco, USA; 2005. [2] Peng Y-F, Chiu C-H, Tsai W-R, Chou M-H. Design of an Omni-directional spherical robot: using fuzzy control. In: Proceedings of the international multiconference of engineers and computer scientists 2009, vol 1. Hong Kong; 2008. [3] Ching-Wen L, Ching-Chih T. Dynamic modeling and sliding-mode control of a ball robot with inverse mouse-ball drive. In: SICE annual conference. The University Electro-Communications, Japan; 2008. [4] Kumagai M, Ochiai T. Development of a robot balancing on a ball. In: International conference on control, automation and systems. Seoul, Korea; 2008. [5] Fankhauser P, Gwerder C. Modeling and control of a Ballbot. Zurich, Switzerland: Autonomous Systems Lab, Swiss Federal Institute of Technology Zurich; 2010. [6] Lotfiani A, Keshmiri M, Danesh, M. Dynamic analysis and control synthesis of a spherical wheeled robot (Ballbot). In: First RSI/ISM International Conference on Robotics and Mechatronics (ICRoM). Sharif University of Technology, Tehran, Iran; 2013. [7] Endo T, Nakamura Y. An omnidirectional vehicle on a basketball. In: 12th international conference on advanced robotics, 2005. ICAR ’05. Proceedings, University of Tokyo, Japan; 2005. [8] Premerlani W, Bizard P. Direction cosine matrix IMU: theory; 2009. [9] Euston M, Coote P, Mahony R, Kim J, Hamel T. A complementary filter for attitude estimation of a fixed-wing UAV. In: International conference on intelligent robots and systems (IROS 2008). 2008; p. 340–345. [10] Sukvichai K, Parnichkun M. Design and balancing control of double-level ballriding robot. In: TRS conference on robotics and industrial technology. Mahidol University, Thailand; 2012.
Contents lists available at ScienceDirect
Mechatronics journal homepage: www.elsevier.com/locate/mechatronics
Double-level ball-riding robot balancing: From system design, modeling, controller synthesis, to performance evaluation Kanjanapan Sukvichai ⇑, Manukid Parnichkun Mechantronics, Asian Institute of Technology, P.O. Box 4, Klong Luang, Pathumthani 12120, Thailand
a r t i c l e
i n f o
Article history: Received 9 September 2013 Accepted 10 June 2014 Available online 14 July 2014 Keywords: Ball-riding robot Non-linear system Sensor fusion Linear Quadratic Regulator plus Integral
a b s t r a c t In this research, a new robot, double-level ball-riding robot, is introduced. The robot consists of an upper ball-riding subsystem and a lower ball-riding subsystem. The robot’s dynamics model can be considered separately in two identical planes. Euler–Lagrange equation of motion is applied in order to obtain the dynamics model. Motors are included in the robot’s model. The model is then linearized. The robot’s parameters are identified. The robot’s prototype is manufactured and assembled. Linear Quadratic Regulator with Integral (LQR + I) controller is proposed and applied in order to balance both levels of the robot. The complementary and orientation transformation are used to fuse sensors in order to obtain robot leaning angles. Balancing performance of the developed double-level ball-riding robot is evaluated by simulations and experiments. The results show efficient control performance of LQR + I controller. Ó 2014 Elsevier Ltd. All rights reserved.
1. Introduction Balancing of unstable robot is one of the most challenge topics for researchers in the field of dynamics control. Ball-riding robot or BallBot is an interesting topic from both mechanism and a controller design aspects since the robot is usually a non-linear unstable system. It requires appropriate controller to make the robot become stable. The pioneer work of BallBot was introduced by Lauwers et al. [1] from Carnegie Mellon University (CMU) in 2005. This BallBot used concept of inverted mouse driving mechanism and it was controlled by LQR + PI algorithm. In 2008, Peng et al. [2] and Ching-Wen and Ching-Chih [3] introduced ball-riding robots which changed the driving wheels from regular solid fixed wheels, which were used in CMU BallBot, to omni-directional wheels. Kumagai and Ochiai [4] demonstrated cooperative BallBots for the first time. These BallBots could perform more complicated tasks such as object carrying using human-robot or multi-robot cooperation. Fankhauser and Gwerder [5] introduced a service BallBot which could move in high speed. Lotfiani et al. [6] focused on a trajectory tracking motion of a single level robot. Although, these BallBots could be balanced successfully, all the BallBots still have only single level. Double-level ball-riding robot is a system which contains two cooperative Ballbots. An upper level Ballbot rides on top of a lower level Ballbot. There is a limitation when constructing a tall ⇑ Corresponding author. Tel.: +66 2 524 6054; fax: +66 2 524 6432. E-mail address: [email protected] (K. Sukvichai). http://dx.doi.org/10.1016/j.mechatronics.2014.06.003 0957-4158/Ó 2014 Elsevier Ltd. All rights reserved.
single-level Ballbot which has large moment of inertia since it requires large actuators in order to balance the Ballbot. Multi-level ball-riding robot can solve this problem. By placing a short Ballbot on top of another short Ballbot, the robot will be taller. The number of levels depends on the required height. Since the short Ballbot has small moment of inertia, it requires only small actuators. Furthermore, a multi-level ball-riding robot is more flexible than a single-level ball-riding robot. By controlling the motion of each level cooperatively, a very stable system can be achieved. It can even perform the motion like robot manipulator. This concept can be extended to a building which can resist earthquake. Last but not least important in dynamic control course, it is an excellent platform to test control performance of different control algorithms. In this research a double-level ball-riding robot is introduced. The mechanism is designed and developed. Both levels of the robot are then controlled by using an optimal controller. 2. Mechanic design Each level of the robot is designed by using inverted mouse mechanism. The mathematical model can be derived easily since the sagittal and coronal planes of the robot are decoupled. Each level of the robot is driven by four driving wheels attached to 100 W DC motors through belt and pulley transmission. Driving wheels are placed in perpendicular to the ball surface at a distance from the ball center. The inverted mouse mechanism of each level of the robot is shown in Fig. 1. In the original design, the robot
520
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Driving Motor Encoder
The Euler–Lagrange equation of motion is used to derive dynamics model of the double-level ball-riding robot. Friction in the system is assumed small and negligible. The dynamics equation is obtained using Eq. (1).
d @L @L ¼ f NP dt @ q_ @q
Wheel
Fig. 1. Driving mechanism of double-level ball-riding robot.
ð1Þ
Lagrangian, L, is the difference between total kinetic energy, K, and total potential energy, P, of the robot and is expressed as P P L¼ K P. The value of L is shown in Appendix A. By solving Eq. (1), the dynamics equations of the robot are obtained as shown in Eqs. (2)–(4). d @L @L ¼ mu rb k2 Sinðb þ hÞb_ 2 2mu rb k2 Sinðb þ hÞb_ h_ dt @ /_ @/ ml rb k1 SinðhÞh_ 2 mu r b lSinðhÞh_ 2 mu rb k2 Sinðb þ hÞh_ 2
wheels are made from nylon tubes. Even though the nylon-tube wheels can be used to drive the robot, however there is a problem of large friction between the ridden ball and the nylon-tube wheels when the ridden ball moves in perpendicular to the wheel rotating axis. Furthermore, since the ridden ball always deforms caused by the robot weight, the ball cannot be considered as a solid spherical ball. The ball deformation results in change of the contact type between the ball and wheels from point contacts to area contacts. The increasing friction creates nonlinear behavior. This effect is more severe when the upper-level subsystem is placed on top of the lower-level subsystem. By introducing omni-directional wheels, these problems are solved. Each omni-directional wheel has two degree of freedoms. Wheel rollers help reduce friction between the ball and the wheel. The friction then becomes small. Omni-directional wheels are selected as robot driving wheels. Two identical ball-riding subsystems are used for both lower and upper levels. However the ridden ball of the upper subsystem is attached to the chassis of the lower subsystem. The double-level ball-riding robot is shown in Fig. 2.
€ þ ml rb k1 CosðhÞ€h 2mw r b lSinðhÞh_ 2 þ mu rb k2 Cosðb þ hÞb þ mu rb lCosðhÞ€h þ mu rb k2 Cosðb þ hÞ€h þ 2mw r b lCosðhÞ€h 2€ 2€ 2€ € þ mb r 2 / € þ ib / b þ ml r b / þ mu r b / þ 4mw r b /
þ 2mw r b rw CosðcÞCosðhÞ€h þ 2mw r b rw CosðcÞCosðb þ hÞ€h þ 2mw r 2b CosðcÞCosðhÞ€h þ 2mw r 2b CosðcÞCosðb þ hÞ€h € þ 2mw r 2 CosðcÞCosðb þ hÞb € þ 2mw r b rw CosðcÞCosðb þ hÞb b 2mw r b rw CosðcÞSinðhÞh_ 2 2mw r b rw CosðcÞSinðb þ hÞh_ 2 2mw r 2b CosðcÞSinðhÞh_ 2 2mw r 2b CosðcÞSinðb þ hÞh_ 2 4mw r b rw CosðcÞSinðb þ hÞb_ h_ 4mw r 2b CosðcÞSinðb þ hÞb_ h_ 2mw r 2b CosðcÞSinðb þ hÞb_ 2 2mw rb rw CosðcÞSinðb þ hÞb_ 2 þ
€ 2iw r2 €h 2iw r b €h 2iw r2b / 2b 2 rw rw rw
ð2Þ
3. Robot Dynamics model 3.1. Mechanical model Dynamics model of the double-level ball-riding robot in x–z and y–z planes are identical because of the symmetry of the robot’s structure. The lower and upper balls have the same radius. The dynamics model of each plane is derived from the projection image as shown in Fig. 3.
Fig. 2. Lower (left) and upper (right) subsystems.
Fig. 3. Double-level ball-riding robot assembly (Left) and projection image of the robot in y–z plane (Right).
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
d @L @L € þ 2iw b € ¼ mu gk2 Sinðb þ hÞ þ mu k2 lSinðbÞh_ 2 þ iu b dt @ b_ @b 2€ € þ 4mw rb r w b € þ 2mw r 2 b € þ iu €h þ 2mw r 2 b þ mu k b 2
b
Then, the generalized force vector is considered. The actuating torques of the BallBot system are applied at the driving wheels. These torques are applied to control all the state variables. From the velocity Jacobian matrix, the generalized forces vector is obtained as expressed in Eq. (5).
w
2 þ 2iw €h þ mu k2 €h þ 2mw r 2b €h þ 4mw r b rw €h þ 2mw r 2w €h € þ mu k2 lCosðbÞ€h þ mu r b k2 Cosðb þ hÞ/
€ þ 2mw rb r w CosðcÞCosðb þ hÞ/ 2 € þ 2mw r CosðcÞCosðb þ hÞ/ þ 2mw lrw CosðcÞCosðbÞ€h
*
f NP
b
þ 2mw lrb CosðcÞCosðbÞ€h 2mw lrw CosðcÞSinðbÞh_ 2 2mw lrb CosðcÞSinðbÞh_ 2 þ 2mw rw gSinðcÞCosðb þ hÞ þ 2mw rb gSinðcÞCosðb þ hÞ þ þ
ð3Þ
sm1 is the input torque from lower motor. sm2 is the input torque from upper motor.
3.2. DC motor model
sL ¼ IR
dxm K me K xm þ m v in dt R R
ð6Þ
where
b
– – – – – – –
2 € þ 2mw r 2 b € € € € € þ 4mw r b r w b w þ il h þ iu h þ 4iw h þ k1 ml h 2€ 2€ 2€ € þ mu l h þ 2mu k2 lCosðbÞh þ mu k2 h þ 2mw l h € þ 4mw r 2 h€ þ 8mw rb r w h€ þ 4mw r 2 h€ þ ml rb k1 CosðhÞ/ b
ð5Þ
For simplicity, armature coil resistance is assumed very small and negligible. The motor dynamics is expressed as shown in Eq. (6).
d @L @L ¼ ml gk1 SinðhÞ mu glSinðhÞ mu gk2 Sinðb þ hÞ dt @ h_ @h 2mw glSinðhÞ mu k2 lSinðbÞb_ 2 2mu k2 lSinðbÞb_ h_ € þ 2iw b € þ mu k2 lCosðbÞb € þ mu k2 b € þ 2mw r 2 b € þ iu b 2
2 3 sm1 rb 6 7 ¼ 4 ðsm1 þ sm2 Þ 5 rw sm2
where – –
2iw r2b €h 4iw rb €h þ rw r 2w
€ 4iw rb b € 2iw r 2b b þ 2 rw rw
521
w
€ þ mu rb k2 Cosðb þ hÞ/ € þ mu r b lCosðhÞ/ € þ 2mw r b r w CosðcÞCosðhÞ/ € þ 2mw r b lCosðhÞ/
sL is the load torque of the driving motor. xm is the motor angular velocity. K m is the motor torque constant. K me is multiplication of back emf and torque constant. R is armature coil resistance. IR is motor inertia. v in is input voltage.
€ þ 2mw r b r w CosðcÞCosðb þ hÞ/ 2 € € þ 2mw r CosðcÞCosðhÞ/ þ 2mw r 2 CosðcÞCosðb þ hÞ/
Angular velocities of motors are then transformed into the robot variables as shown in Eq. (7) and (8).
þ 4mw lrw CosðcÞCosðbÞ€h þ 4mw lrb CosðcÞCosðbÞ€h € þ 2mw lr b CosðcÞCosðbÞb € þ 2mw lrw CosðcÞCosðbÞb
w_ 1 ¼
b
b
rb _ _ h_ ¼ xm1 ð/ hÞ rw rb _ ¼ xm2 w_ 2 ¼ þ 1 ðb_ þ hÞ rw
2mw lrw CosðcÞSinðbÞb_ 2 2mw lr b CosðcÞSinðbÞb_ 2 4mw lrw CosðcÞSinðbÞb_ h_ 4mw lr b CosðcÞSinðbÞb_ h_ þ 2mw r w gSinðcÞCosðhÞ þ 2mw r w gSinðcÞCosðb þ hÞ þ 2mw r b gSinðcÞCosðhÞ þ 2mw r b gSinðcÞCosðb þ hÞ € 2iw rb / € 4iw r b b € 4iw r 2 €h 8iw r b €h 2iw r 2 b € 2iw r 2b / þ 2b þ þ 2b þ 2 rw rw rw rw rw rw ð4Þ
where T
– q ¼ ½/; h; b . – / is the angle of the lower ball. – h is the angle of the lower level subsystem measured from z-axis. – b is the angle of the upper level subsystem measured from center axis of the lower level subsystem. – c is the angle from robot’s vertical axis and vector from center of ball to center of the wheel. – w1 ; w2 are angles of the lower and upper wheels respectively. – r b is the radius of the ridden ball. – r w is the radius of the driving wheels. – k1 is the length from center of the lower ball to center of gravity of the lower level subsystem. – k2 is the length from center of the upper ball to center of gravity of the upper level subsystem. – l is the length from center of the lower ball to center of the upper ball.
ð7Þ ð8Þ
Thus, the required torques for the lower- and upper-level subsystems are expressed in Eqs. (9) and (10) respectively.
rb € rb K me r b _ / þ IR / þ 1 €h rw rw R rw K me r b Km þ þ 1 h_ þ v in1 R rw R
sm1 ¼ IR
rb € þ IR r b þ 1 €h þ K me rb þ 1 b_ þ1 b rw rw R rw K me r b K m þ þ 1 h_ þ v in2 R rw R
sm2 ¼ IR
ð9Þ
ð10Þ
3.3. Robot’s state-space model Double-level ball-riding robot can be expressed by a state-space model as shown in Eq. (11). Matrix D is defined as disturbance matrix. It takes into account the disturbance force from friction, disturbance and uncertainty. Friction in this research is assumed to be a Coulomb friction because in the real operation the ridden ball is operated at a small speed, thus the viscous friction is negligible. The friction occurs at the contacts of the lower ridden ball and the lower driving wheels. The friction of the upper ridden ball and the upper driving wheels is considered as the internal force. This term is considered separately from the robot dynamic model.
522
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532 *
€ þ Cðq; qÞ _ þ GðqÞ þ D ¼ f NP MðqÞq
ð11Þ
where
2
3 M11 M12 M13 6 7 M ¼ 4 M21 M22 M23 5; M31 M32 M33
2
3 C1 6 7 C ¼ 4 C2 5; C3
2
3 G1 6 7 G ¼ 4 G2 5 G3
M11 ¼ ib þ m1 r 2b þ 2iw r 2d M12 ¼ m2 rb CosðhÞ þ m3 r b Cosðb þ hÞ 2iw r2d 2iw r d þ2m6 ðr w þ rb ÞCosðcÞðCosðhÞ þ Cosðb þ hÞÞ M13 ¼ m3 rb Cosðb þ hÞ þ 2m6 ðrw þ r b ÞCosðcÞCosðb þ hÞ
there exists also an uncontrollable rotation around the vertical axis. The small wheels on left and right planes at some instants do not contact the ridden ball at the same time, which make the forces from wheels deviate from the center of the ball. Heading motion occurs as shown in Fig. 4. Wheel misalignment from manufacturing and assembling also creates heading motion. Robot’s weight changes a point of contact between the ridden ball and the ground to an area of contact. Contact points on basketball surface are uniform with the estimated density of 126,000 points per m2 as shown in Fig. 5. Each contact point in the contact area creates friction. Thus, the estimated friction torque can be determined following Eq. (12) when the contact area is considered as a circle.
M21 ¼ M12 M22 ¼ il þ iu þ 4iw þ m4 þ m3 k2 þ 4mw ðr b þ r w Þ2 þ 4iw r2d þ 8iw r d þ2m3 lCosðbÞ þ 4ðm5 þ m6 ÞlCosðcÞCosðbÞ M23 ¼ iu þ 2iw þ m3 k2 þ 2mw ðr b þ r w Þ2 þ 2iw r2d þ 4iw r d þm3 lCosðbÞ þ 2ðm5 þ m6 ÞlCosðcÞCosðbÞ M31 ¼ M13 M32 ¼ M23 M33 ¼ iu þ 2iw þ m3 k2 þ 2mw ðr b þ r w Þ2 þ 2iw r2d þ 4iw r d G1 ¼ 0 G2 ¼ m2 gSinðhÞ þ 2ðm5 þ m6 ÞgSinðcÞCosðhÞ þ2ðm5 þ m6 ÞgCosðb þ hÞ m3 gSinðb þ hÞ G3 ¼ m3 gSinðb þ hÞ þ 2ðm5 þ m6 ÞgSinðcÞCosðb þ hÞ
2 3
sfb ¼ q lmRobot gR where – – – – –
q is the ratio of area of all contact points and the contact area l is the friction coefficient between the ball and the ground mRobot is the total weight of the robot system g is the gravitational acceleration R is the radius of contact area
If torque from the driving wheels is greater than the ground friction torque, the robot rotates around the vertical exist as explained in Eq. (13). An experiment of robot’s heading motion is conducted in the last section. *
C1 ¼ m3 r b Sinðb þ hÞb_ 2 2m3 r b Sinðb þ hÞb_ h_ m2 r b SinðhÞh_ 2 þm3 r b Sinðb þ hÞh_ 2 2m6 ðrw þ r b ÞCosðcÞðSinðhÞ þ Sinðb þ hÞÞh_ 2 4m6 ðr w þ r b ÞCosðcÞSinðb þ hÞb_ h_ þ KRme ðr d Þ2 /_ 2m6 ðr w þ r b ÞCosðcÞSinðb þ hÞb_ 2 KRme ðr d Þðr d þ 1Þh_ C2 ¼ m3 lSinðbÞb_ 2 2m3 lSinðbÞb_ h_ 2ðm5 þ m6 ÞlCosðcÞSinðbÞb_ 2 2ðm5 þ m6 ÞlCosðcÞSinðbÞb_ h_ KRme ðr d Þ2 /_ þ2 K me ðr d Þðrd þ 1Þh_ þ K me ðr d Þðr d þ 1Þb_ R
ð12Þ
*
*
*
ðf wheel1 D r w1 f wheel2 D r w2 Þ sfb ¼ ib Ub where *
*
– f wheel1 ; f wheel2 are force vectors from wheels in planes 1 and 2 respectively * * – D r w1 ; D r w2 are the shortest distant from force to the vertical axis – Ub is the ball angular acceleration along the vertical axis
R
C3 ¼ m3 lSinðbÞh_ 2 2ðm5 þ m6 ÞlCosðcÞSinðbÞh_ 2 þ K me ðr d Þðr d þ 1Þb_ þ K me ðr d Þðr d þ 1Þh_ R
R
where
m1 ¼ ðmb þ ml þ mu þ 4mw Þ m2 ¼ ml k1 þ mu l þ 2mw l m3 ¼ mu k2 2
2
m4 ¼ ml k1 þ mu l þ 2mw l
2
m5 ¼ mw rw m6 ¼ mw rb rb rd ¼ rw 3.4. Heading motion Theoretically, the developed invert mouse driving mechanism allows the robot to translate only on x–y plane, however, heading motion around z-axis (vertical axis) may occur due to several factors including the wheels misalignment, omni-directional wheel characteristic, ground friction, etc. In this section, robot heading motion is considered. An omni-directional wheel consists of small polycarbonate auxiliary wheels installed on two planes at 13 mm gap. Even though, these small wheels rotate freely when the ridden ball is moving in perpendicular to the wheel rotating axis, but
ð13Þ
Fig. 4. Force diagram showing heading motion.
523
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
3 0 0 7 6 0 0 7 6 7 6 v in1 6 0 2 0 3 2 37 B¼6 ; u¼ 7 1 0 7 6 v in2 K 7 6 1 m 4 4 M ðr d Þ KRm 4 1 5 M1 1 5 5 ðr d Þ R 0 1 2
M is the mass matrix evaluated at equilibrium point.
D1 ¼
K me 2 r R d
D2 ¼
K me 2 rd þ rd R
5. Implementation of the double-level ball-riding robot
Fig. 5. Contact points inside a contact area of a basketball.
5.1. Robot hardware and electrical circuit design The heading motion of the robot mainly occurs at the lower sub-system since the upper ridden ball is fixed to the lower frame. 4. Linear approximation of dynamic equation The robot’s state-space model is then linearized around the equilibrium point where all the state variables and control input signal are equal to zero or the robot is at upright position. Eq. (11) can be written as Eq. (14).
8 9 2 3 v in1 > > < = K 7 € ¼ M 1 ðqÞ ðrd Þ m 6 _ GðqÞ q 4 ðv in1 þ v in2 Þ 5 Cðq; qÞ > > R : ; v in2
ð14Þ
Let,
T x ¼ / h b /_ h_ b_ ¼ ½ x1
x2
x3
x4
x5
x6 T
Therefore, 3 x4 7 6 x5 7 6 7 6 7 6 x 6 8 9 7 , f ðx; v in1 ; v in2 Þ € ¼ x_ ¼ 6 2 3 q 7 6 v > > in1 6 < =7 6 6 1 7 _ GðqÞ 7 5 4 M ðqÞ ðr d Þ KRm 4 ðv in1 þ v in2 Þ 5 Cðq; qÞ > > : ; v in2 2
ð15Þ
The linearized dynamics model is obtained by using Taylor series expansion around equilibrium point as shown in Eq. (16).
x_ ffi
@ @ f ðx; v in1 ; v in2 Þx þ f ðx; v in1 ; v in2 Þv in1 @x @ v in1 @ þ f ðx; v in1 ; v in2 Þv in2 @ v in2
ð16Þ
The linearized dynamics model at the equilibrium point can be expressed by Eq. (17)
x_ ¼ Ax þ Bu where,
ð17Þ
In order to design the robot controller efficiently, robot parameters are required. Firstly, the parameters are considered and identified. Body of the double-level ball-riding robot consists of 4 parts. The first part is the ridden ball which is made from rubber with medium–high stiffness. The second part is the driving wheel which has the body made from plastic and its wheels are made from synthesis rubber. The third part is the lower level of the robot which includes the upper ball into its body. The last part is the upper level of the robot which does not include the ball. The robot body frame is made from aluminum grade 6061. Shafts and bearings are made from steel, while the pulleys are made from aluminum. The ridden ball is a standard basketball which is solid enough to be used in the ball-riding robot [7]. Robot parameters are determined by using SolidWorks program. By providing proper part materials, the parameters are finally identified. Photos of robot parts are shown in Fig. 6. Motor parameters are identified from experiments. Two identical 100-W DC motors and an optical encoder with resolution of 1440 pulses per revolution are attached to a support plate with driving belt as shown in Fig. 7. First motor is driven by constant current, The back electromotive force voltage is measured at the second motor. Motor speed is measured by the encoder via microcontroller board. The robot parameters are shown in Table 1. After substitution of robot parameters from Table 1 into Eq. (17), the robot’s state and input matrices become Eq. (18) and (19) respectively.
3 0 0 0 1 0 0 7 60 0 0 0 1 0 7 6 7 60 0 0 0 0 1 7 6 A¼6 7 6 0 173:90 5:197 0:579 0:681 0:022 7 7 6 4 0 67:407 9:359 0:173 0:168 0:042 5 0 77:699 49:605 0:225 0:048 0:226 2
2
0
0
3
7 6 0 0 7 6 7 6 7 6 0 0 7 B¼6 6 1:885 0:058 7 7 6 7 6 4 0:563 0:112 5 0:733
0:605
ð18Þ
ð19Þ
524
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Fig. 6. Parts of double-level ball-riding robot.
half of s-plane. Therefore, the robot is considered as an unstable system. A 32-bit ARM controller is selected as the main controller for the robot. This controller operates at 72 MHz and has variety of peripheral interfaces. A motor driver which is full-bridge DC motor driver can drive the motor with continuous current up to 40 A at 24 V. An Inertia Measurement Unit or IMU which consists of accelerometer (ADXL345) and gyro sensor (ITG3200) is used to measure the robot’s tilt angle. Upper level-and lower-level subsystem can communicate between themselves by using wireless module from Nordic Semiconductor Company which has carrier frequency at 2.4 GHz and can handle up to the maximum of 32 bytes. Robot controller and other electrical circuit are shown in Fig. 8.
Fig. 7. Experiment set-up for motor parameters identification.
Table 1 Robot parameters. Parameter
Value
Unit
Km K me R rb rw k1 k2
0.00291737 0.04454 1.64 0.1210 0.0258 0.1474 0.1211 45 0.5081 0.1099 0.0545 9.0508 8.8567 1030.483106 15.2493106 266508.430106 235425.849106
kg cm/A V kg cm/(A rad/s) Ohms m m m m degree m kg kg kg kg kg m2 kg m2 kg m2 kg m2
c l mb mw ml mu ib iw il iu
All the state variables of the robot are checked and found observable and controllable. However, after determine the eigen values of its characteristic equation, some of system poles are on the right
5.2. Robot software The robot controller is programmed with C language. The program also includes robot’s tilt angle estimation and balancing control. Robot’s tilt angles from vertical z axis along x and y direction are required in balancing control of the robot. Accelerometer and gyro sensor are used to determine the robot’s tilt angles. Data from these two sensors have to be fused in order to obtain the accurate leaning angles. There are many filter algorithms that can manipulate and attenuate the noise. Complementary filter is simple and good filter that can improve the quality of the measured angle. The rates of change of angle from gyro sensor are fused to the data from the accelerometer by using concept of combination of low pass and high pass filters. The acceleration data is passed through the low pass filter to remove measurement noise in the sensor. The rates of change of angle from gyro sensor are fed to numerical integration algorithm to change the rates to numerical angles then passed through the high pass filter. The filtered data are then fused together and the robot tilt angles are estimated. The block diagram of the complementary filter is shown in Fig. 9. In order to use complementary filter to estimate the robot tilt angles, information from both gyro sensor and accelerometer must be transformed to world coordinate. Although, roll, pitch and yaw angular velocity are obtained from the gyro sensors, they are measured respect to the sensor coordinate. Unlike gyro sensor, accelerometer is used to measure gravity
525
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Wireless Module Motor Driver
Main Controller
Fig. 10. Diagram of the world and sensor coordinates.
IMU
The angular velocity which is measured respect to the sensor coordinate is transformed to the angular velocity respect to the world coordinate by using Eq. (22). Fig. 8. Controller board and electrical circuit.
2
acceleration which always measured respect to the world coordinate. Thus, these two measured data cannot be fused immediately. The data from gyro sensor must be transformed to the world coordinate using direct cosine method [8]. Rotation matrix of the robot coordinate respect to the world coordinate Rzyx is determined as shown in Fig. 10. Subscript R and W represent the sensor and the world coordinates respectively. The last row of the rotation matrix contains information about angles of the sensor respect to the world z-axis which is an important information used to estimate robot’s tilt angles. By defining Rzyx ð3Þ matrix from the last row of the rotation matrix, thus,
Rzyx ð3Þ ¼ ½ sinðhÞ sinð/Þ cosðhÞ cosð/Þ cosðhÞ ¼ ½ r zx
rzy
r zz ð20Þ
In order to fuse gyro data, the relationship of rate of change of angle from sensor coordinate and world coordinate is required. The relationship is the cross product of the measured angular velocity of the * sensor x and the last row element vector of the rotation matrix as shown in Eq. (21).
2 *
r zx
3
* 6 7 r_ ¼ x 4 rzy 5
r zz
ð21Þ
3 2 3 rzx ½n þ 1 K c r zx ½n þ ð1 K c Þr xacc ½n 6 7 6 7 4 r zy ½n þ 1 5 ¼ 4 K c r zy ½n þ ð1 K c Þr yacc ½n 5 rzz ½n þ 1 K c r zz ½n þ ð1 K c Þr zacc ½n
ð22Þ
Then, information from both sensors is fused by using complementary filter [9]. It can be expressed in Eq. (23).
2
rzx ½n
3
2
r zx ½n 1 þ r zy ½n 1dw r zz ½n 1dh
3
6 7 6 7 4 r zy ½n 5 ¼ 4 r zy ½n 1 r zx ½n 1dw þ r zz ½n 1d/ 5 r zz ½n 1 þ r zx ½n 1dh r zy ½n 1d/ rzz ½n
ð23Þ
where – – – –
K c is the complementary gain. dw is the yaw angular velocity of the gyro sensor. dh is the pitch angular velocity of the gyro sensor. d/ is the roll angular velocity of the gyro sensor.
These three axis component values are then normalized and used to estimate the robot’s tilt angles in pitch and roll direction as shown in Eqs. (24) and (25) respectively.
0
1
rzx ½n þ 1 B C /est ½n þ 1 ¼ tan1 @qffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiffiA 2 r zy ½n þ 1 þ r 2zz ½n þ 1 r zy ½n þ 1 hest ½n þ 1 ¼ tan1 r zz ½n þ 1
ð24Þ
ð25Þ
Contact point location on the ridden ball is determined from the wheel encoders. Resolution of the wheel encoders is multiplied by gear transmission ratio and also by four when the signal is captured using built-in quadrature encoder capture module inside the ARM microcontroller. The resolution of the ridden ball contact point location is 0.01 radians. 6. Control algorithm
Fig. 9. Block diagram of the complementary filter.
In this research, LQR + I is proposed to control and balance the developed double-level ball-riding robot. LQR + I is an optimal controller that feeds all state variables and error integration through the optimal gains to produce the optimal response. LQR + I is the
526
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
result of adding integration of error as another state variable for LQR in order to achieve superior steady-state performance. Integral R term is added with LQR by defining the state variable ze ¼ e dt. The state-space representation of the robot system is then modified as shown in Eq. (26).
A 0 xe B x_ e x_ x_ 0 ¼ ¼ þ u ¼ AX þ Bu X_ ¼ _ze C 0 ze 0 x x0
ð26Þ
where – C is the output matrix. – x0 is the exogenous input vector. The control law is now expressed in Eq. (27).
u ¼ KX ¼ ½ K f
Ki
xe
¼ K f ðx x0 Þ K i C
ze
Z
ðx x0 Þdt ð27Þ
The optimal gain matrix K f and K i can be determined by solving the algebraic Riccati equation of nominal model of the robot. Block diagram of the controlled system is shown in Fig. 11. By applying backward Euler method to Eq. (27), the discrete LQR + I control law can be implemented by using Eq. (28).
2
u½n ¼ u½n 1 þ ½ K f
e/ ½n e/ ½n 1
3
7 6 eh ½n eh ½n 1 7 6 7 6 7 6 e ½n e ½n 1 b b 7 6 6 ðe ½n 2e ½n 1 þ e ½n 2Þ=Dt 7 7 6 / / / 7 6 7 K i 6 6 ðeh ½n 2eh ½n 1 þ eh ½n 2Þ=Dt 7 7 6 6 ðeb ½n 2eb ½n 1 þ eb ½n 2Þ=Dt 7 7 6 7 6 e/ ½nDt 7 6 7 6 eh ½nDt 5 4 eb ½nDt ð28Þ
where – u½n ¼ ½ v in1 ½n v in2 ½n T – Dt is the sampling time. – e/ ; eh ; eb are the error between robot state variables and x0 . Control signals denoted by v in1 and v in2 are sent to the pairs of lower and upper motor drivers which drive the robot in the same direction respectively. Control signals for x and y direction motions are determined the same way.
simulation has the same mass, inertia and height as the doublelevel robot in order to investigate the behaviors of the two robots at very similar conditions. Both robots are controlled by the conventional LQR. Disturbance is applied at the top of the robot body to generate a change of robot leaning angle. Robot leaning angles of the single-level and the double-levels robot are shown in Figs. 12 and 13 respectively. From the simulation results, robot leaning angle has large fluctuation when the disturbance is applied to the single-level robot. Leaning angle of the double-level robot has smaller fluctuation. Ground contact point locations of both robots are show in Fig. 14. Ground contact point position of the double-level robot deviates much less than the single-level robot. The result is because in the double-level robot the actuating motors from both lower-level and upper-level subsystems work simultaneously to suppress the effect from disturbance and keep the robot stability while the single-level robot has only actuating motors from one robot on the ridden ball to compensate the effect from disturbance. From the simulation results, the double-level ball-riding robot is more stable than the single-level ball-riding robot. 7.2. Comparison of LQR and LQR + I LQR performance on double-level ball-riding robot was evaluated in our previous work [10]. It could control the robot when the robot is operated in a controlled environment. In the real operation, the gyro sensor and accelerometer always have output biases. The estimated leaning angle which is obtained from sensors fusions shows the drift. The optimal controller LQR and LQR + I are simulated and compared in this section. Biases from both sensors and saturation of the motors are added to the simulation in order to obtain the performances of LQR and LQR + I. In the first simulation, the robot has to keep its location at the origin point while the sensor signal of robot leaning angle has a drift. From the simulation results, both controllers can maintain the robot stability. The robot controlled by LQR + I can keep it position around the original position while the robot controlled by LQR has location drift from the original point as shown in Fig. 15. The second simulation is set up in order to obtain the effect of the static bias when the robot is driven to a desired location. Fig. 16. shows the result when the robot moves to the desired location when a small bias is applied to the robot while Fig. 17. shows the result when a large bias is applied to the robot. From the simulations, LQR + I is more robust than the conventional
7. Simulation and experimental results 7.1. Comparison of double-level and single-level ball-riding robots In this section, a single-level and a double-level ball-riding robots are tested and compared. The single-level robot in the
Fig. 11. Block diagram of LQR + I.
Fig. 12. Leaning angle of single-level ball-riding robot.
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Fig. 13. Leaning angle of double-level ball-riding robot.
LQR when the robot is in the presence of bias and motor saturation. 7.3. Full robot simulation Control performance of LQR + I on the double-level ball-riding robot is tested by simulation on MATLAB Simulink firstly. The linearized state-space representation of the robot in one plane is considered because of its symmetric property between x–z and y–z planes. The desired operating point is very close to the robot equilibrium point where the robot is in upright position and stationary. All the robot’s state variables are obtained from the robot’s model in MATLAB simulink. Gaussian noise is added to the states and output. LQR + I is implemented to control the robot. The simulation result of the robot’s leaning angle in y–z plane is shown in Fig. 18. Even though the leaning angles of the lower and upper levels fluctuate around zero at upright position due to measurement noise, but the robot can still keep its balancing. The result of the angle of the ridden ball is shown in Fig. 19. The result shows that the robot moves back and forth in order to maintain its balancing even in the presence of noise. By combining the results from two planes, the robot position is shown in Fig. 20.
Fig. 14. Ground contact point location of the robots.
527
Fig. 15. Comparison of LQR and LQR + I with sensor drif.
7.4. Balancing experimental result Lower and upper levels are assembled together. The upper ridden ball is fixed to the lower level by using high strength epoxy. Each level subsystem has been tested for its balancing performances before assembling. Each level subsystem has enough power to maintain its balancing while carrying the other subsystem on top. Fully assembled robot is shown in Fig. 21. Discrete LQR + I control algorithm is implemented to the main microcontroller. Experiments are setup in order to test the balancing performance of the double-level ball-riding robot. Tilt angles in both x–z and y–z plane of each level subsystem are measured and transmitted to the other level subsystem via wireless communication. Wheel positions are obtained via optical encoders. Control signal from LQR + I algorithm is calculated and transmitted to the motor driver. State variables are recorded by computer via serial communication. In the experiments, state-weighting matrix (Q) is defined by assigning more weight to the leaning angles of both levels and their error integration than the other states because balancing of the robot is the primary objective than robot position. This criteria is applied for both x–z and y–z planes. From the experiments, the double-level ball-riding robot can maintain its balancing by using
Fig. 16. Comparison of LQR and LQR + I with small sensor bias.
528
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Fig. 17. Comparison of LQR and LQR + I with large sensor bias.
Fig. 20. Simulation result of robot position.
Fig. 18. Simulation results of robot leaning angles on y–z plane using LQR + I.
Fig. 21. The lower- (left) and upper- (right) level subsystems.
The leaning angles of the robot obtained from real experiments show similarity to the simulation results as shown in Fig. 24. The leaning angles of the robot always fluctuate around zero at upright position and the robot can maintain its balancing. Although the robot can keep its balancing but the angle of the ridden ball shows some drift from the zero angle which means the robot moves from the origin point due to the effect of some surface slip. Control signal is recorded and shown in Fig. 25. The control signal does not saturate and has the maximum duty cycle of 40%. The positions of the robot from 5 experiments are shown in Fig. 26. Even though, the robot has some drifts in its position, but the robot tries to come back to the origin point every times. Priority of the robot location is lower than the robot’s leaning angles based on the defined weighting matrices.
Fig. 19. Angle of the ridden ball.
LQR + I controller. Robot can keep its upright position all the time as shown in Fig. 22. The robot’s leaning angles in y–z plane from the experiment is shown in Fig. 23.
7.5. Tracking experimental result In this section, the experiments are setup in order to obtain the robot tracking performance of a double level robot. The stateweighting matrix, Q, is redefined in this experiment because the position of the robot is more focused than the one in the previous experiment. By varying the Q matrix, the feedback gains change.
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
529
Fig. 22. Photo taken from the experiment.
Fig. 23. Robot leaning angles on y–z plane using LQR + I from experiment.
Fig. 24. Angle of the ridden ball from experiment.
The robot is required to move along the desired trajectory. The experimental results are shown from Figs. 27–30. The robot position along x-axis and the desired trajectory are shown in Fig. 27. From the experiment, the robot moves forward and backward in order to keep its stability similar to the results from the
experiment in Section 7.4. At the same time, the robot tries to move along the desired trajectory with position error due to some disturbance and uncertainty. Location of the ground contact point of the robot on x–y plane is shown in Fig. 28. From the result, the robot can move to the desired location with some error. The error
530
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
Fig. 25. Control signal of the lower-level robot.
Fig. 28. Ridden ball ground contact location on x–y plane.
Fig. 26. Robot positions from 5 experiments. Fig. 29. Comparison of leaning angles of lower-level robot from experiments in Sections 7.4 and 7.5.
Fig. 27. Ridden ball ground contact location along x-direction. Fig. 30. Control signal of lower-level robot in position tracking experiment.
occurs because the robot loses its stability when it tries to move as shown in Fig. 29. The result shows the comparison between leaning angle of the lower-level robot when the stability is more concerned than the position. Leaning angle of the lower-level robot
when position is more concerned has fluctuation with twice magnitude compared to the robot when position is less concerned. From the experimental result, even though the robot can move along the desired trajectory but it loses its stability easily compared with the previous experiment in Section 7.4.
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
531
Fig. 31. Photos from heading motion experiment.
Control signal of the motor has larger magnitude and higher frequency than the control signal for the previous experiment due to position and leaning angle control. Control signal still does not saturate as shown in Fig. 30.
7.6. Heading motion experimental result In this section, the experiment is set up in order to measure heading motion of the robot. The state-weighting matrix, Q, is the same as the one in the tracking experiment in order to keep the robot’s position. Heading angle of the robot around the vertical
axis is measured by using IMU sensor. The photos from the experiment are shown in Fig. 31. From the experiment, heading of the lower sub-system changes all the time during the experiment while heading of the upper sub-system does not change. The upper sub-system does not change its heading because the ridden ball is fixed to the lower frame. Heading direction of the lower sub-system fluctuates within a small angle. Even though, heading direction of the lower sub-system is uncontrollable, however, it fluctuates around zero angles in small amplitude with about zero mean. From the experiment, heading direction of the lower subsystem changes to one direction when the robot moves in one direction and changes to the opposite direction when the robot moves in opposite direction. This behavior repeats in the experiment. It happens because of non-uniform surface of basketball and ground characteristics. Mean of heading angle of the lower sub-system is 0.000439 radians with standard deviation of 0.0229 radians. According to the experiment result, the heading direction of the robot is small and can be negligible. The measured heading angle is shown in Fig. 32. 8. Conclusion
Fig. 32. Heading angle around vertical axis of lower sub-system.
Double-level ball-riding robot was introduced in this research. Mechanism design and hardware architecture of the robot were explained. Dynamics model of the robot was obtained by Euler– Lagrange equation of motion. Dynamics model of DC motor was derived and included in the robot’s dynamics model. The model was then linearized around its equilibrium point where robot was in upright position and stationary. Real robot was manufactured. The robot and motor parameters were then identified. ARM 32-bit microprocessor was selected as the robot main
532
K. Sukvichai, M. Parnichkun / Mechatronics 24 (2014) 519–532
controller because of its performance. Data from gyro sensor and accelerometer were fused together by using direct cosine method and complementary filter in order to estimate the robot’s leaning angle. In this research, a single-level and double-level ball-riding robots are explored and compared. The double-level ball-riding robot has shown a better performance than a single-level ball-riding robot with the presence of disturbance. The double-level ball-riding robot could reject the disturbance because the two level subsystems work cooperatively together. LQR + I was introduced and proposed in this research to control the robot in order to overcome the sensor drift and motor saturation problems. LQR + I has shown more robust than the conventional LQR. Balancing performance of the robot was tested by simulation in noisy environment in order to imitate the real system. LQR + I controller has shown a good balancing performance in both simulation and real experiments. The robot’s leaning angles fluctuate around equilibrium point with small deviation. Robot moved around the origin point during balancing. Both simulated and experimental data have shown similar results that the robot could keep its balancing. The experiment results have shown position drift due to surface slip. Trajectory tracking performance was setup and experimented. The experimental results have shown that the robot could track the desired trajectory and maintain its balancing stability at the same time. Balancing stability and position control were traded off in the experiment. The robot would lose its stability easier than the balancing stability experiment because of the lower weight. Position error occurred because of disturbance from contact slip and model uncertainty. Heading angle of the ball-riding robot may change because of wheels misalignment and omni-directional wheel characteristic. The heading angle is uncontrollable because of the limitation of the designed mechanism, however, the change of heading angle is very small and negligible as seen from the experiment. Suppression of the disturbance and uncertainty using robust controller such as a sliding mode controller or H1 controller is the main focus in our future research. Appendix A Lagrange equation is a function that describes a dynamic system using energy approach concept. The Lagrangian, L, in Eq. (1) is the difference between potential energy, P, and kinetic energy, P P K, or L ¼ K P. The total potential energy of the double-level ball-riding robot is shown in Eq. (29) while the total kinetic energy is shown in Eq. (30). X
P ¼ gmb r b þ gmw ðrb ðr b þ r w ÞSinðc þ hÞÞ þ gml ðr b þ k1 CosðhÞÞ þ gmw ðr b þ lCosðhÞ ðr b þ r w ÞSinðc þ ðb þ hÞÞÞ þ gmw ðr b þ lCosðhÞ þ ðr b þ r w ÞSinðc ðb þ hÞÞÞ þ gmu ðr b þ lCosðhÞ þ k2 Cosðb þ hÞÞ þ gmw ðr b þ ðr b þ r w ÞSinðc hÞÞ ð29Þ
X
K ¼ 0:5ib /_ 2 þ 0:5mb r 2b /_ 2 þ 0:5iw
!2 _ rb ð/_ hÞ h_ rw
_ 2Þ þ 0:5mw ððrb þ rw Þ2 Cosðc hÞ2 h_ 2 þ ððrb þ rw ÞSinðc hÞh_ þ rb /Þ 2 _ 2Þ þ 0:5il h_ 2 þ 0:5ml ðk1 SinðhÞ2 h_ 2 þ ðk1 CosðhÞh_ þ r b /Þ
_ 2 þ 0:5mw ððlSinðhÞh_ ðrb þ rw ÞSinðc þ ðb þ hÞÞðb_ þ hÞÞ _ þ rb /Þ _ 2Þ þ ðlCosðhÞh_ þ ðr b þ rw ÞCosðc þ ðb þ hÞÞðb_ þ hÞ _ 2 þ 0:5mu ððlSinðhÞh_ k2 Sinðb þ hÞðb_ þ hÞÞ _ 2 þ 0:5iu ðb_ þ hÞ _ 2 þ 0:5mw ððlSinðhÞh_ þ ðrb þ rw ÞSinðc ðb þ hÞÞðb_ þ hÞÞ _ þ rb /Þ _ 2Þ þ ðlCosðhÞh_ þ ðr b þ rw ÞCosðc ðb þ hÞÞðb_ þ hÞ _ þ rb /Þ _ 2Þ þ ðlCosðhÞh_ þ k2 Cosðb þ hÞðb_ þ hÞ 2 rb _ 2 þ ððrb þ rw ÞSinðc þ hÞh_ þ rb /Þ _ 2Þ ðb_ þ hÞ þ 0:5iw 1 rw !2 _ rb ð/_ hÞ h_ þ 0:5mw ððrb þ rw Þ2 Cosðc þ hÞ2 h_ 2 þ 0:5iw rw 2 rb _ 2 þ 0:5iw 1 ðb_ þ hÞ ð30Þ rw References [1] Lauwers T, Kantor G, Hollis R. One is enough! In: 12th Int’l symp. on robotics research. Robotics Institute, Carnegie Mellon University, San Francisco, USA; 2005. [2] Peng Y-F, Chiu C-H, Tsai W-R, Chou M-H. Design of an Omni-directional spherical robot: using fuzzy control. In: Proceedings of the international multiconference of engineers and computer scientists 2009, vol 1. Hong Kong; 2008. [3] Ching-Wen L, Ching-Chih T. Dynamic modeling and sliding-mode control of a ball robot with inverse mouse-ball drive. In: SICE annual conference. The University Electro-Communications, Japan; 2008. [4] Kumagai M, Ochiai T. Development of a robot balancing on a ball. In: International conference on control, automation and systems. Seoul, Korea; 2008. [5] Fankhauser P, Gwerder C. Modeling and control of a Ballbot. Zurich, Switzerland: Autonomous Systems Lab, Swiss Federal Institute of Technology Zurich; 2010. [6] Lotfiani A, Keshmiri M, Danesh, M. Dynamic analysis and control synthesis of a spherical wheeled robot (Ballbot). In: First RSI/ISM International Conference on Robotics and Mechatronics (ICRoM). Sharif University of Technology, Tehran, Iran; 2013. [7] Endo T, Nakamura Y. An omnidirectional vehicle on a basketball. In: 12th international conference on advanced robotics, 2005. ICAR ’05. Proceedings, University of Tokyo, Japan; 2005. [8] Premerlani W, Bizard P. Direction cosine matrix IMU: theory; 2009. [9] Euston M, Coote P, Mahony R, Kim J, Hamel T. A complementary filter for attitude estimation of a fixed-wing UAV. In: International conference on intelligent robots and systems (IROS 2008). 2008; p. 340–345. [10] Sukvichai K, Parnichkun M. Design and balancing control of double-level ballriding robot. In: TRS conference on robotics and industrial technology. Mahidol University, Thailand; 2012.