Optimal motion planning and control of a

0 downloads 0 Views 4MB Size Report
May 27, 2016 - robot using dynamic programming approach: simulation and ... table includes optimal control inputs for every state in the admissible region.
ARTICLE IN PRESS

JID: MECH

[m5G;May 27, 2016;16:20]

Mechatronics 0 0 0 (2016) 1–11

Contents lists available at ScienceDirect

Mechatronics journal homepage: www.elsevier.com/locate/mechatronics

Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results M. Roozegar, M.J. Mahjoob∗, M. Jahromi Center for Mechatronics and Intelligent Machines, School of Mechanical Engineering, College of Engineering, University of Tehran, Tehran, Iran

a r t i c l e

i n f o

Article history: Received 25 August 2015 Revised 28 April 2016 Accepted 16 May 2016 Available online xxx Keywords: Optimal motion planning and control Spherical mobile robot Dynamic programming Nonholonomic robots Obstacle avoidance Image processing

a b s t r a c t Optimal motion planning and control of a nonholonomic spherical mobile robot is studied. Dynamic Programming (DP) as a direct and online approach is used to navigate the robot in an environment with/without obstacles. The optimal trajectory, which corresponds to the minimum cost, is determined in the case of presence of obstacles in the environment, and the robot can move towards the target optimally, without colliding with obstacles. DP yields optimal control inputs in a closed-loop form. In fact, a traditional control system is no longer needed to track the obtained trajectory since the resulted DP table includes optimal control inputs for every state in the admissible region. The effect of different final states and alternative intervals of the allowable state and control values are also studied. Results from several simulations show that the proposed method enables the robot to find an optimal trajectory from any given state towards a predefined target. An experimental setup is designed wherein a real spherical robot is driven according to the developed algorithm. A vision system monitors the robot and outputs the location/orientation of the robot at each step via image processing. Experimental results are then compared with simulations to validate the model and evaluate the control strategy. © 2016 Published by Elsevier Ltd.

1. Introduction Spherical mobile robot basically consists of a ball-shaped outer shell including its driving mechanism to roll the sphere. As compared to usual wheeled robots or walking robots, a spherical rolling robot offers certain advantages. The major advantage due to the spherical shape is that it is not overturned. Therefore, it can face obstacle cluttered environment and irregular surfaces. Such robots can thus survive in harsh or dangerous environments such as outer planets, deserts and earthquake ruins for exploration or reconnaissance tasks [1–5]. Different constructions have been suggested for a spherical robot [6]. From control point of view, spherical robot is a kind of nonholonomic system; the number of control inputs is smaller than the dimensions of the state space. This makes the motion planning and control problem difficult [7]. The stable full-state tracking problem was investigated for nonholonomic wheeled mobile robots under output-tracking control laws [8]. An adaptive output feedback tracking controller for nonholonomic mobile robots was proposed to guarantee that the tracking errors are confined to an arbitrar-



Corresponding author. E-mail address: [email protected] (M.J. Mahjoob).

ily small ball [9]. Control schemes, based on the flatness property and the concept of virtual vehicles, for a group of nonholonomic kinematic mobile robots for maintaining a desired formation along a time-parameterized path were proposed in [10]. Obstacle avoidance policies were introduced for cluster space control of nonholonomic multirobot systems [11]. In fact, spherical robots are not yet popular due to the complexity involved in motion planning and control of nonholonomic robots. However, researchers have done significant works for navigation and control of these robots. Li and Canny proposed a threestep algorithm for motion planning of a sphere rolling on a flat surface [12]. In the first step of the algorithm, the position coordinates of the sphere are converged to the desired values. Next, two of the three orientation coordinates are converged applying Lie Bracket-like motion. A polhode is used to converge the last orientation coordinate in the third step. Two algorithms were presented for partial and complete reconfiguration of the sphere by using individual control inputs in [13]. In the first algorithm, spherical triangles are utilized to bring the sphere to the desired position and orientation. The second strategy generates a trajectory comprised of straight lines and circular arc segments. Two simple and effective algorithms for reconfiguration of the sphere are given in [14,15]. One algorithm that uses spherical trigonometric concepts is similar to the second step of the Li and Canny algorithm. Another

http://dx.doi.org/10.1016/j.mechatronics.2016.05.002 0957-4158/© 2016 Published by Elsevier Ltd.

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

JID: MECH 2

ARTICLE IN PRESS

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

algorithm effectively uses the final configuration of the sphere at the singularity of the ZYZ Euler angle description, thus reducing the number of configuration variables. It however uses control inputs in a rotating reference frame. Bhattacharya and Agrawal deduced the first-order mathematical model for a kind of spherical robot from the non-slip constraint and the conservation of angular momentum. The authors studied the trajectory planning based on optimal time and energy strategy, and presented both simulations and experimental results [16,17]. Bicchi et al. [18,19] established a simplified dynamic model for a spherical robot and discussed its motion planning on a surface with obstacles. Joshi and Banavar proposed the kinematics model of a spherical robot using Euler parameters and investigated the path planning problems [20,21]. Two different algorithms for motion planning of a rolling sphere, the circle-based and the generalized Viviani-curve-based ones, were also proposed [22]. Based on Ritz approximation theory, the near-optimal trajectory of a spherical robot was planned with the Gauss-Newton algorithm [5]. Path tracking control of a spherical robot, focusing on the simultaneous converges of both position and orientation, was studied in [4]. Wilson’s eXtended Classifier System was utilized for motion planning of a spherical robot [23,24]. A motion planning strategy, composed of two trivial and one nontrivial maneuver, was devised for a spherical rolling robot driven by two internal rotors [25]. Using computational fluid dynamics, Alizadeh et al. studied the motion of a spherical robot on the surface of water and called it an amphibious robot [26]. Motion analysis of spherical robots has thus been studied in two directions: one is the motion planning of these robots and the other is tracking the desired trajectory [27,28]. In almost all of the previous researches, the optimal trajectory obtained offline should be tracked by control actions. The control inputs were not limited in motion planning and could not be implemented on a real robot. There is also a lack of experimental work for closed-loop control of the spherical robot. In this paper, the direct and online approach of dynamic programming, introduced by Bellman, is used to find the optimal trajectory and closed-loop control of a nonholonomic spherical robot in an environment with/without obstacles. In fact, DP is used to find the optimal trajectory to reach the target without colliding with obstacles. The control inputs have certain bounds in our proposed procedure and could easily be implemented in real world. Here, designing a traditional control system and tracking the obtained optimal trajectory are eliminated since DP yields the optimal control inputs in a feedback form. In other words, in the proposed method, the robot can find the optimal control inputs according to its state at each step and the completed DP table. The effect of presence of obstacles, variation of final states and intervals of the allowable state and control values are also studied. An experimental setup is designed to implement the proposed closedloop algorithm. A vision system is utilized to monitor the state of the robot at each step via image processing. The closed-loop control system implemented here is explained and experimental results are compared with simulations to validate the model and evaluate the control strategy. The outline of the paper is as follows. The kinematic model of the spherical robot is described in the next section. In Section 3, a brief description of DP and its implementation for the spherical robot are presented. Section 4 provides the simulation results. Experimental works and implementation of control loop are discussed in Section 5. 2. Modeling This paper focuses on a simple spherical mobile robot developed using a pendulum-based design. This type of spherical robot uses gravitational force to move. Fig. 1 shows a schematic of the

Fig. 1. Schematic of the intervals of a pendulum-driven spherical robot.

Fig. 2. The fabricated spherical mobile robot.

internals of a kind of spherical robot called Rotundus [6]. This design consists of two perpendicular motors attached to the inside of the sphere. One of them, stepper motor 1 in Fig. 1, is attached to the horizontal axis (the main shaft) that goes through the sphere and changes θ . In the center of the main shaft there is a pendulum that can move with respect to the shaft. When this motor is activated, position of the mass center of the robot changes, and hence, the sphere moves as long as the weight of the pendulum has enough inertia as it is easier for the casing to spin than the pendulum to go around. The pendulum can move in a direction perpendicular to the shaft for changing φ by the other motor (stepper motor 2 in Fig. 1), causing the robot to turn in other directions. Therefore, it has a pendulum with two DOFs [29–31]. Fig. 2 shows the fabricated spherical robot in our laboratory. Consider the motion of a sphere on a flat plate, rolling without slipping. The contact point can be represented in the coordinate system attached to the sphere center as follows. ρ is the radius of the spherical shell.

⎛ ⎞ ρ cosφ cosθ f (θ , φ ) = ⎝ ρ cosφ sinθ ⎠ ρ sinφ

(1)

where ρ ,φ ,θ are variables specifying a point in the spherical coordinates. The contact trajectory on the plate is specified by C = (x,y) in the xyz-coordinates attached to the plane. The contact trajectory on the sphere is indicated by C  = (θ , φ ) in the coordinates attached to the sphere. In Cartesian coordinates, the rotation angle of the sphere with respect to the surface is the angle between two coordinate systems at the contact point (see Fig. 3). This angle is known as the “holonomy angle”, indicated by ψ . The solution of the forward problem in which we seek C with knowledge of C’ is obtained as follows [29,30]:



˙ ˙ ⎪ ⎨x˙ = ρ −θ cos φ sin ψ + φ cos ψ y˙ = ρ θ˙ cos φ cos ψ + φ˙ sin ψ ⎪ ⎩ ˙ ψ = −θ˙ sin φ

(2)

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

ARTICLE IN PRESS

JID: MECH

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

3

Now, consider the following first-order equation of the spherical robot

xˆ˙ = −θˆ˙ cos φ sin ψ + φˆ˙ cos ψ

(5)

where θˆ˙ and φˆ˙ are the control variables. Before the numerical procedure of DP can be applied, the differential equation of the system should be approximated by a difference equation. This can be done by dividing the time interval 0 ≤ t ≤ T (0 ≤ tˆ ≤ 1 ) into N equal increments, t (tˆ). Hence:

Fig. 3. Holonomy angle in spherical robot.

where the input variables are θ and φ . These variables represent a trajectory on the sphere. Since the inputs needed to drive the spherical robot are θ and φ , Eq. (2) corresponds to the forward kinematics of the robot. The robot speed is low enough to neglect the dynamics involved. Thus, the sphere-plate contact point stays always underneath the mass suspended down the pendulum. Therefore, by determining θ and φ , the center of mass, and consequently, the position of the contact point could be determined from Eq. (1). As explained before, the motors position the pendulum inside the sphere to change the mass center for a desired move. Thus, by changing θ and φ , we can find a trajectory which shows the contact point trajectory on the sphere. Then, using Eq. (2), we can find x and y which represent the contact point trajectory on the plate.



















xˆ tˆ + tˆ = xˆ tˆ + tˆ −θˆ˙ tˆ cos φ tˆ sin ψ tˆ + φˆ˙ tˆ cos ψ tˆ

(6) It is assumed that tˆ is small enough so that control and state variables can be approximated by a piecewise-constant function that changes only at instants tˆ = 0, tˆ, 2tˆ, . . . , (N − 1 )tˆ; thus, for tˆ = ktˆ(k = 0, 1, . . . , N − 1 ) knowing that xˆ(ktˆ) is referred to the kth value of x and is denoted by xˆ(k ) ,



xˆ(k + 1 ) = xˆ(k ) + tˆ −θˆ˙ (k ) cos φ (k ) sin ψ (k ) + φˆ˙ (k ) cos ψ (k )

(7) Similar equations are obtained for the other states as follows:



yˆ(k + 1 ) = yˆ(k ) + tˆ θˆ˙ (k ) cos φ (k ) cos ψ (k ) + φˆ˙ (k ) sin ψ (k )

3. Optimal motion planning In this section, the process of motion planning based on DP is briefly clarified. In fact, DP is applied to determine an optimal control function to minimize the cost value. In this method, an optimal policy is found by employing the intuitively appealing concept called the principle of optimality. This principle states that “An optimal policy has the property that whatever the initial state and initial decision are, the remaining decisions must constitute an optimal policy with regard to the state resulting from the first decision.” DP is a computational technique which extends the decisionmaking concept to sequences of decisions which together define an optimal policy and trajectory. The routing problem should be “solved” in a table, where only the consequences of lawful decisions are included. Once the table is completed, the optimal path from any state to the final state can be obtained by entering the table at the appropriate state and reading off the optimal heading at each successive state along the trajectory. The information in the table can also be used to adjust the new optimal trajectory even if we are forced to deviate from the optimal path (or because of errors). This is the most important advantage of using DP in control problems [32,33]. Here, the process of implementing DP for the spherical robot to reach the final state by finding a set of optimal motions is explained. Non- dimensionalized kinematic equations are obtained and used here, for convenience. Assuming tˆ = Tt , xˆ = ρx and yˆ = ρy , we have:

t = T tˆ, x = ρ xˆ, y = ρ yˆ, x˙ =

ρˆ T

˙ y˙ = x,

ρˆ ˙ 1 ˙ ψ = ψˆ˙ , y, T

T

1 1 θ˙ = θˆ˙ , φ˙ = φˆ˙ T T

(3)

where T is the total duration of motion. By substituting these equations in the kinematic model of the spherical robot obtained in Section 2 we have:

xˆ˙ = −θˆ˙ cos φ sin ψ + φˆ˙ cos ψ , yˆ˙ = θˆ˙ cos φ cos ψ + φˆ˙ sin ψ ,

ψˆ˙ = −θˆ˙ sinφ , ψˆ˙ = T ψ˙ , θˆ˙ = T θ˙ , φˆ˙ = T φ˙

(4)



ψ (k + 1) = ψ (k ) + tˆ −θˆ˙ (k ) sin φ (k )

φ (k + 1) = φ (k ) + tˆ φˆ˙ (k )

(8)

The state variables are therefore xˆ, yˆ, ψ and φ . The cost to be minimized here is the energy consumed by the robot since the energy source of the robot is inside the spherical shell. According to the references [5,16,17], for this robot, the minimum energy path problem is to minimize the cost functional defined below:

J=

1 2



T

uT Ru dt

(9)

0

where the components of u are control inputs, i.e. uT = [u1 u2 ]. Assuming R = α I, where I is the identity matrix and α is a constant, we have:

J=

1 2



0

T

 α T 2 u1 + u22 dt α u21 + α u22 dt =



2

(10)

0

Since uT = [u1 u2 ] = [ω1 ω2 ] contains the control inputs in this problem, which are the angular velocities of the shaft and pendulum stepper motors related to θ˙ and φ˙ by gear ratios, and α2 is a constant, the cost functional can be defined as follows:

 J=

T 0

(ω12 + ω22 )dt

(11)

For accuracy, instead of defining it in the cost function, only the trajectories with final states close enough, which is defined by the operator, to the desired final state (set point) are acceptable and the completed DP table does not include the other ones. Nonetheless, one can define a new cost function like the following, which takes in the accuracy, too.

 J=

T 0

2 (ω12 + ω22 )dt + λ P (T ) − Pf

(12)

where P(T) is the state of the robot at time T and Pf is the desired final state. Note that λ is the weight corresponding to accuracy. In mathematical analysis, the smoothness of a function is a property measured by the number of its continuous derivatives. In

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

ARTICLE IN PRESS

JID: MECH 4

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

Fig. 4. All acceptable trajectories between initial and final states.

our problem, for smoothness of motion, the cost functional, which should be added to the previous cost, is defined as follows:

4. Simulation Assume that the initial and final states of the robot are P0 =

Js = γ



T 0

y (x )dt

(13)

where γ is the corresponding weight and y (x), the second derivative, represents the variations of the slope of the trajectory, which is used as a criterion for smoothness in the present problem. Next, depending on the problem, the limits for allowable values of the state and control variables should be determined. In fact, the limits on the state variables constitute the admissible region and the limits of the control variables are determined according to the restrictions in experiments and feasibility of implementation in the real world. In order to use DP for motion planning of the robot in an environment with obstacles and avoid colliding with them, the presence of obstacles should be introduced in the admissible region. To this end, we should omit the locations of obstacles from the admissible region and solve the problem for the new environment. The first step in the computational procedure is to find the optimal policy for the last stage. Therefore, all of the allowable control values should be examined at each of the allowable state values. The optimal control for each state is the one which yields the trajectory possessing minimum cost. Afterwards, knowing the optimal policy for all states of the last stage, we consider the next-to-last stage and compute the corresponding optimal policy. Similarly, we solve the problem for other stages and obtain a table including optimal policies, or optimal control variables, for all the admissible state variables. To reduce the required number of calculations substantially, and thereby make the computational process feasible, the admissible state and control values must be quantized first. To avoid collision with obstacles, as mentioned, we omit the “grid” points associated with the obstacle locations from the quantized values of states, and then, apply the DP method to find the optimal controls for all new admissible states. This way, we can determine the optimal trajectory to the target state, which has the minimum cost, without colliding with any of the obstacles.

(x0 , y0 , ψ0 , φ0 ) = (0, 0, 0, 0) and Pf = (x f , y f ) = (1, 1), respectively. Note that there are no constraints on the values of ψ f and φ f . The robot radius is ρ = 0.2 m, the time of motion is assumed to be T = 10 sec, N = 10 (a ten-stage process) and the limits of the state and control variables are as follows:

0 ≤ x(t ) ≤ 1, −

π 6



φ (t ) ≤

π 6

0 ≤ y(t ) ≤ 1, ,

0 ≤ θ˙ (t ) ≤



π 2

,

π 2

≤ ψ (t ) ≤



π 4

π 2

≤ φ˙ (t ) ≤

,

π 4

As stated, to limit the number of calculations and make the computational procedure feasible, state and control values must be quantized. Herein, we use the following quantized values:

x(t ) = 0, 0.1, 0.2, . . . , 1

,

y(t ) = 0, 0.1, 0.2, . . . , 1

,

ψ (t ) = −1.57, −1.47, −1.37, . . . , 1.57 φ (t ) = −0.52, −0.42, −0.32, . . . , 0.52 , θ˙ (t ) = 0, 0.1, 0.2, . . . , 1.5 , φ˙ (t ) = −0.7, −0.6, −0.5 . . . , 0.7 As mentioned, for accuracy, the completed DP table only includes the trajectories with final states close enough to the desired final state (set point), instead of defining it in the cost function. Fig. 4 shows all these acceptable trajectories form P0 to Pf . Note that the system under study is MIMO (Multi-Input Multi-Output) and nonholonomic with nonlinear equations. Moreover, the state and control variables are not quantized into very small intervals and the number of control inputs is smaller than the dimensions of the state space. These all together make the motion planning quite difficult. In addition, the trajectories depend significantly on the initial and final states, especially the holonomy angle, as well as the cost function defined. Therefore, the obtained trajectories are limited and not very smooth. Fig. 5 represents the smoothest one with the least cost; among all of the obtained trajectories. The difference between the approximated and exact trajectories is that the approximated trajectory is obtained by ignoring the

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

JID: MECH

ARTICLE IN PRESS

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

5

Fig. 5. Comparison between exact and approximated optimal trajectories obtained by DP (without obstacles).

Fig. 7. Comparison between exact and approximated optimal trajectories obtained by DP.

computational grid-point error, but the exact trajectory is obtained by following the sequence of optimal controls by the system. Accordingly, they have the same cost. Here, P (T ) = (0.9919, 1.0012) and P (T ) = (0.9158, 0.9820) are the approximated and exact final states, respectively. Now, assume that the initial and final states of the robot are P0 = (x0 , y0 , ψ0 , φ0 ) = (0, 0, 0, 0) and Pf = (x f , y f , ψ f , φ f ) = (1, 1, 0, 0), respectively. Note that there are constraints on the values of ψ f and φ f in this part. These new constraints make motion planning more difficult. Similarly, the simulated trajectories through the DP process are shown in Figs. 6 and 7. In this case, as mentioned, there are more constraints on the final state and it is more complex to find an optimal trajectory. Thus, in order to increase the accuracy, the allowable state and control values are quantized into smaller intervals. The approximated and exact final states for the optimal trajectory, the smoothest one with the least cost, are P (T ) = (1.0 040, 1.0 020, −0.0296, −0.0 047) and P (T ) = (1.0177, 0.9472, −0.3045, 0), respectively. Note that in this

case, the errors of φ cancel each other by chance, while following the sequence of optimal controls in exact trajectory, and the final value of this variable is exactly equal to 0. In comparison to the case of having no constraints on the values of ψ f and φ f , the cost corresponding to this trajectory is greater than the cost obtained before. Clearly, this happens because of our new constraints. The optimal control inputs for this case are represented in Figs. 8 and 9. Placing two obstacles in (xobs1 , yobs1 ) = (0.4, 0.4) and (xobs2 , yobs2 ) = (0.7, 0.6), the trajectories will be changed to trajectories represented in Figs. 10 and 11. Note that for collision avoidance, as mentioned, the grid points related to the obstacle locations are omitted from the quantized values of the states, i.e. the admissible region, and the DP method is applied afterwards. In Fig. 11, the final states for approximated and exact optimal trajectories are P (T ) = (1.0039, 0.9955, −0.0208, 0.0764) and P (T ) = (1.0181, 0.9375, −0.26, 0.1), respectively.

Fig. 6. All acceptable trajectories between initial and final states.

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

JID: MECH 6

ARTICLE IN PRESS

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

Fig. 8. Optimal angular velocity of the main shaft.

Fig. 9. Optimal angular velocity of pendulum.

Fig. 11. Comparison between exact and approximated optimal trajectories (with obstacles).

The cost of tracing this trajectory (with obstacles) is about 16.934% more than that of without obstacles. The optimal control inputs for this case are represented in Figs. 12 and 13. As it is observed, the optimal controls are completely different from the case without obstacles since the optimal trajectory has been changed. The main point here is that once the DP table has been completed, it could be utilized in any environment even if the robot has not experienced. Generally, there exist many other approaches for motion planning of robots. Recently, an artificial intelligence method, eXtended Classifier System (XCS), was applied to navigate a spherical robot [23, 24]. Based on Reinforcement Learning (RL) algorithm, the robot was trained to find a trajectory between initial and final configurations by interacting with the environment. Fig. 14 shows the final trajectory obtained using XCS for moving from the initial state of P0 = (0, 0, 0, 0) to the final state of Pf = (1, 1, 0, 0). Compared to XCS, DP has the following advantages:

Fig. 10. All acceptable trajectories between initial and final states (with obstacles).

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

JID: MECH

ARTICLE IN PRESS

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

7

Table 1 Real values of the robot parameters. Mass of the pendulum Mass of the spherical shell Radius of the spherical shell Radius of the pendulum

0.639 kg 0.5 kg 200 mm 180 mm

Fig. 12. Optimal angular velocity of the main shaft (with obstacles).

Fig. 16. The spherical robot on the floor covered with a black rubber mat.

– It is much more precise since it is based on exact calculations. – A cost function can be defined depending on the problem. – It is much more reliable in actual situations; XCS may not provide appropriate responses for different initial and/or final configurations. 5. Experimental work Fig. 13. Optimal angular velocity of pendulum (with obstacles).

Fig. 14. Trajectory obtained using XCS.

The fabricated spherical robot was already introduced in Section 2 (see Fig. 2). As mentioned, the selected locomotion mechanism for the robot works on the basis of center of mass displacement. A 2-DOF pendulum changes the position of mass center of the robot. Two stepper motors are included, which turn the main shaft and pendulum independently, making the robot roll and move around. Fig. 15 represents the mechanical components of the robot, such as spherical shell, main shaft and pendulum. The exact values of the robot parameters follow in Table 1. A computerized wireless system is used to command the steppers. An image processing technique is applied to locate the robot where its response to different inputs can be analyzed. The actual position of the robot, the center of the sphere for instance, is found by detecting the landmarks and the spherical robot in each frame. The floor is covered with a black rubber mat to provide perfect rolling and simplify image processing (see Fig. 16). Fig. 17 shows the sequence of image processing. In step 1, the picture received from camera is colored (rgb). For image processing, we need to transform the colored image to grey or black and

Fig. 15. Components of the spherical robot.

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

JID: MECH 8

ARTICLE IN PRESS

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

Fig. 17. The sequence of image processing to find robot position.

Fig. 18. Schematic of the closed-loop control system by DP.

white (step 2). The picture is not uniform enough, namely, some spots are light and some others are dark. Therefore, in the third step we subtract the background image from the original image in order to get a uniform image. Next (step 4), the image is transferred to a binary image where pixels take only 0 or 1 values. Then, noises in the image are removed by deleting the small white areas in step 5. Finally, the centroid of the largest white area shows the position of the robot. The consecutive positions generate the trajectory of the robot. The orientation of the robot can also be found using image processing. Fig. 18 shows the schematic diagram of the closed-loop DPbased control system used in experiment. As stated, the position of the robot, i.e. output/feedback, is obtained by means of a digital camera and image processing. At each stage, the optimal control inputs, determined by the completed DP table, are sent to the robot wirelessly and applied to the motors to make the robot move. Results show that the proposed algorithm is applicable for real-time control of the spherical robot with little error

Fig. 19. Results after tracing a circular path.

compared to the simulation. Fig. 19 represents a typical sample of the experimental results for tracing a circular path using image processing.

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

JID: MECH

ARTICLE IN PRESS

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

9

Fig. 23. Comparison between exact and approximated optimal trajectories (with obstacles). Fig. 20. Comparison between exact and approximated optimal trajectories.

Fig. 24. Comparison between theory and experiment (with obstacles).

Fig. 21. Comparison between theory and experiment.

Fig. 25. Spherical robot moving in a general environment.

Fig. 22. Placing two obstacles in the plane of motion.

Now, suppose that the requested initial and final states of the robot are P0 = (x0 , y0 , ψ0 , φ0 ) = (0, 0, 0, 0), and Pf = (x f , y f ) = (1, 1), respectively. The specified final time is assumed to be T = 15sec, N = 15 (a 15-stage process). To reduce the lateral oscillations of the robot, the limits of control variables are decreased and the duration of motion is increased instead. Fig. 20 shows the simulated optimal trajectories through our DP process. Fig. 21 shows the comparative results of experiments and simulations. Note that the DP table completed in the simulation part is applied in experiments, as mentioned. Hence, knowing the state of the robot at each stage, the optimal control inputs, the main shaft and pendulum angular velocities, are determined according to the DP table.

Placing two obstacles in the plane of motion as depicted in Fig. 22, the optimal trajectories are changed to the trajectories represented in Figs. 23 and 24. In a general environment, where the floor in not covered with the black rubber mat, we face two main problems. First, the robot slips and pure rolling does not occur. Secondly, the image processing is much more difficult; it is hard to recognize the robot over the background. Consequently, the errors increase and the robot deviates from the predefined optimal trajectory. In fact, at each step, the robot finds the optimal control inputs online, according to its current state, i.e. feedback, and the completed DP table. Figs. 25 and 26 represent the results of moving in such an environment, which makes the robot deviate from the predefined optimal path. As it is observed, the experimental result is quite far from the predefined trajectory obtained offline, since the robot

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

JID: MECH 10

ARTICLE IN PRESS

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11

acteristics. The state of the robot at each step was determined by using a vision system and image processing. Better sensors could be applied to determine the state of the robot more accurately. This way, the error observed in the experiments could be reduced considerably.

References

Fig. 26. Comparison between a predefined trajectory and experimental result in a general environment.

determines optimal policies online, according to its current state at each step and does not track the offline optimal trajectory. According to the experimental results, the proposed algorithm can be applied for real-time control of this nonholonomic robot. In other words, after completing the DP table, all optimal policies are known for all the states in the admissible region. Therefore, the optimal control inputs can be determined according to the current state of the robot, i.e. feedback, and then, applied to the motors, even if the robot is not on the predefined optimal path. Note that in the experiments, the trajectory corresponds to minimum cost since it is obtained by applying the optimal controls at each step using the DP table. Hence, the robot consumes the least energy to reach the final configuration, and as a result, longer running time is attained on a single charge of the battery. The error observed here is mostly because the robot may slip instead of pure rolling. Furthermore, the image processing used for feedback is not much accurate. One can therefore use a material that provides more friction on the spherical shell or surface to make pure rolling feasible. Better sensors can also be employed in order to determine the position/orientation of the robot more accurately. Compared to other methods, the results reveal that this approach is a promising technique for real-time control of a nonholonomic robot. 6. Conclusion Optimal motion planning and control of a nonholonomic spherical mobile robot in an environment with/without obstacles was studied using DP, a direct and online approach. DP was used in a feedback form and the robot found a trajectory, corresponding to the minimum cost while avoiding colliding with obstacles, in the case of presence of obstacles in the environment. The profound advantage of DP is omitting the need of designing a traditional control system for tracking the optimal trajectory. In fact, the completed DP table could be used to determine the optimal control inputs for each state in the admissible region. Results showed that the presented approach is capable of attaining an optimal trajectory in an environment with/without obstacles, from any given position/orientation. The presence of obstacles, effects of diverse final states and different intervals of control inputs and grid points were also considered. The algorithm could be used to optimize other parameters like time or trajectory length (or a combination of them). An experimental setup was designed to validate simulations wherein a real spherical robot was driven according to the developed procedure. Experimental results show that the proposed method is a promising technique compared to other approaches for closed-loop control of a nonholonomic robot due to its char-

[1] Armour RH, Vincent JFV. Rolling in nature and robotics: a review. J Bionic Eng 2006;3:195–208. [2] Mahboubi S, Seyyed Fakhrabadi MM, Ghanbari A. Design and implementation of a novel spherical mobile robot. J Intell Robot Syst 2012:1–22. [3] Zhan Q, Zhou T, Chen M, Cai S. Dynamic trajectory planning of a spherical mobile robot. In: Proceedings of IEEE conference on robotics, automation and mechatronics; 2006. p. 1–6. [4] Cai Y, Zhan Q, Xi X. Path tracking control of a spherical mobile robot. Mech Mach Theo 2012;51:58–73. [5] Zhan Q, Cai Y, Liu Z. Near-optimal trajectory planning of a spherical mobile robot for environment exploration. In: Proceedings of IEEE conference on robotics, automation and mechatronics; 2008. p. 84–9. [6] Crossley VA. A literature review on the design of spherical robots, Spherical Robot Designs. Pittsburgh, PA 15213: Carnegie Mellon University; 2006. p. 1–6. [7] Schutte AD. Permissible control of general constraint mechanical systems. J Franklin Inst 2010;347:208–27. [8] Wang D, Xu G. Full-state tracking and internal dynamics of nonholonomic wheeled mobile robots. IEEE/ASME Trans Mech 2003;8:203–14. [9] Huang J, Wen C, Wang W, Jiang ZP. Adaptive output feedback tracking control of a nonholonomic mobile robot. Automatica 2014;50:821–31. [10] Ailon A, Zohar I. Control strategies for driving a group of nonholonomic kinematic mobile robots in formation along a time-parameterized path. IEEE/ASME Trans Mech 2012;17:326–36. [11] Mas I, Kitts C. Obstacle avoidance policies for cluster space control of nonholonomic multirobot systems. IEEE/ASME Trans Mech 2012;17:1068–79. [12] Li Z, Canny J. Motion of two rigid bodies with rolling constraint. IEEE Trans Robot Autom 1990;6:62–72. [13] Mukherjee R, Minor MA, Pukrushpan JT. Simple path planning strategies for spherobot: a spherical mobile robot. In: Proceedings of IEEE conference on decision and control; 1999. p. 2132–7. [14] Mukherjee R, Minor MA, Pukrushpan JT. Motion planning for a spherical mobile robot: revisiting the classical ball-plate problem. ASME J Dyn Syst, Meas, Control 2002;124:502–11. [15] Das T, Mukherjee R. Reconfiguration of a rolling sphere: a problem in evolute–involute geometry. ASME J Appl Mech 2006;73:590–7. [16] Bhattacharya S, Agrawal SK. Design, experiments and path planning of a spherical rolling robot. In: Proceedings of IEEE international conference on robotics and automation; 20 0 0. p. 1207–12. [17] Bhattacharya S, Agrawal SK. Spherical rolling robot: a design and path planning studies. IEEE Trans Robot Autom 20 0 0;16:835–9. [18] Marigo A, Bicchi A. A local-local planning algorithm for rolling objects. In: Proceedings of IEEE international conference on robotics and automation; 2002. p. 1759–64. [19] Bicchi A, Balluchi A, Prattichizzo D, Gorelli A. Introducing the “SPHERICLE”: an experimental testbed for research and teaching in nonholonomy. In: Proceedings of IEEE internationalconference on robotics and automation; 1997. p. 2620–5. [20] Joshi VA, Banavar RN. Motion analysis of a spherical mobile robot. Robotica 2008;27:343–53. [21] Joshi VA, Banavar RN, Hippalgaonkar R. Design and analysis of a spherical mobile robot. Mech Mach Theor 2009;45:130–6. [22] Svinin M, Hosoe S. Motion planning algorithms for a rolling sphere with limited contact area. IEEE Trans Robot 2008;24:612–25. [23] Esfandyari MJ, Roozegar M, Shariat Panahi M, Mahjoob MJ. Motion planning of a spherical robot using eXtended classifier systems. In: Proceedings of IEEE 21st Iranian Conference on Electrical Engineering (ICEE); 2013. p. 1–6. [24] Roozegar M, Mahjoob MJ, Esfandyari MJ, Shariat Panahi M. XCS-based reinforcement learning algorithm for motion planning of a spherical mobile robot. Appl Intell 2016:1–11. doi:10.1007/s10489- 016- 0788- 9. [25] Morinaga A, Svinin M, Yamamoto M. A motion planning strategy for a spherical rolling robot driven by two internal rotors. IEEE Trans Robot 2014;30:993–1002. [26] Vahid Alizadeh H, Mahjoob MJ. Quadratic damping model for a spherical mobile robot moving on the free surface of the water. In: Proceeidngs of IEEE international symposium on robotic and sensors environments (ROSE); 2011. p. 125–30. [27] Zheng M, Zhan Q, Liu J, Cai Y. Control of a spherical robot: path following based on nonholonomic kinematics and dynamics. Chin J Aeronaut 2011;24:337–45. [28] Zhan Q, Liu Z, Cai Y. A back-stepping based trajectory tracking controller for a non-chained nonholonomic spherical robot. Chin J Aeronaut 2008;21:472–80. [29] Vahid Alizadeh H, Mahjoob MJ. Effect of incremental driving motion on a vision-based path planning of a spherical robot. In: Proceedings of the second international conference on computer and electrical engineering; 2009. p. 299–303.

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002

JID: MECH

ARTICLE IN PRESS

[m5G;May 27, 2016;16:20]

M. Roozegar et al. / Mechatronics 000 (2016) 1–11 [30] Kamaldar M, Mahjoob MJ, Haeri Yazdi M, Vahid Alizadeh H, Ahmadizadeh S. A control synthesis for reducing lateral oscillations of a spherical robot. In: Proceedings of IEEE international conference on mechatronics; 2011. p. 546–51. [31] Kamaldar M, Mahjoob MJ, Vahid Alizadeh H. Robust speed control of a spherical robot using arx uncertain modeling. In: Proceedings of IEEE international symposium on robotic and sensors environments (ROSE); 2011. p. 196–201.

11

[32] Bellman RE, Kalaba RE. Dynamic programming and modern control theory. New York: Academic Press; 1965. [33] Roozegar M, Mahjoob MJ, Jahromi M. DP-based path planning of a spherical mobile robot in an environment with obstacles. J Franklin Inst 2014;351:4923–38.

Please cite this article as: M. Roozegar et al., Optimal motion planning and control of a nonholonomic spherical robot using dynamic programming approach: simulation and experimental results, Mechatronics (2016), http://dx.doi.org/10.1016/j.mechatronics.2016.05.002