Subplanner Algorithm to Escape from Local Minima for Artificial Potential Function Based Robotic Path Planning

Han Ul Yoon; Dong-Wook Lee

doi:10.5391/IJFIS.2018.18.4.263

Original Article

Split Viewer

International Journal of Fuzzy Logic and Intelligent Systems 2018; 18(4): 263-275

Published online December 31, 2018

https://doi.org/10.5391/IJFIS.2018.18.4.263

Subplanner Algorithm to Escape from Local Minima for Artificial Potential Function Based Robotic Path Planning

Han Ul Yoon, and Dong-Wook Lee

¹Robotics R&D Group, Korea Institute of Industrial Technology, Ansan, Korea, ²Department of Robotics and Virtual Engineering, Korea University of Science and Technology, Daejon, Korea

Correspondence to :
Dong-Wook Lee, (dwlee@kitech.re.kr)

Received: August 11, 2018; Revised: November 1, 2018; Accepted: November 13, 2018

This is an Open Access article distributed under the terms of the Creative Commons Attribution Non-Commercial License (http://creativecommons.org/licenses/by-nc/3.0/) which permits unrestricted noncommercial use, distribution, and reproduction in any medium, provided the original work is properly cited.

Abstract

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

This paper proposes a subplanner algorithm, which will be referred to as attractive force rotation and restoration, to compensate the local minima problem under the artificial potential based planner. The key role of the proposed subplanner is to build a navigating path for escaping from the local minima by rotating and restoring the attractive force. A mobile manipulator was adopted as our robotic application to substantiate the capability of the proposed subplanner under various simulation scenarios in which the mobile manipulator finds a path to a target object without collision against stationary/moving obstacles. The simulation results substantiated that the compensated artificial potential based planner successfully found a path to guide the mobile manipulator to the target object in the presence of the local minima.

Keywords: Path planning, Mobile manipulator, Artificial potential based planner, Attractive force rotation and restoration algorithm

1. Introduction

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

A robotic path planning problem is to find a path that is a continuous mapping from an initial configuration to a goal configuration with a given time interval [1]. Accordingly, solving a path planning problem means that we want to find a continuous curve which lies in free configuration space and connects the initial configuration and the goal configuration [2, 3]. This problem is often solved by a combination of computational geometry, heuristic search algorithms, and game-theoretic decision-making [1, 4, 5]. The applications of the robotic path planning have been explored in a broad range of field such as mobile robots [6–8], aerial robots [9, 10], fork-type lifters [11, 12], humanoid robot [13, 14], etc.

In path planning, a planner normally refers to a method or an algorithm connecting one configuration to the other configuration. The most challenging aspect of a planner design is that a configuration space usually has a high dimensionality. The high dimensional complexity in the configuration space does not allow us to find a solution by using insights from the explicit geometric representation of the free configuration space [2, 15]. Due to the curse of dimensionality, path planning in the high dimensional configuration space usually yields problematic issues, e.g., narrow passage problem [16]. Canny and Reif proved that the problem of designing a path planning algorithm in high dimensional configuration space becomes NP-hard [17].

Artificial potential based planner (APBP) is a compact but strong way to design a planner. Generally, the APBP consists of attractive potential and repulsive potential functions; both potential functions are differentiable real-valued function. These two potential functions generate a force field in which a current configuration is forced to be attracted by a goal configuration and repulsed by non-free configurations [2, 3]. One advantage of the APBP is that it eventually guides an initial configuration to a goal configuration. The other advantage is that the two potential functions as well as the corresponding force field can be defined on a workspace as a function of the control points of a robotic system, which in turn, give us geometrical insights about the movement of the control points. However, the critical drawback of the APBP is that it fails to find a path if a configuration falls into local minima (at where the force field is vanished) [18].

To mitigate the weak point of the APBP, we propose a subplanner algorithm to escape from the local minima while a path is being planned for a robotic system under the APBP. The proposed subplanner algorithm partitions the configuration space of the robotic system based on the kinematic features and detects the local minima in the partitioned configuration spaces. Then, it plans a navigating path by manipulating the direction of the attractive force to escape from the local minima. Compared to existing works presented in [19–21] as current state-of-the-art, our approach initiates the use of kinematic feature of the robotic system and deals with the higher dimensional configuration space.

To explain our approach, throughout this paper, we consider the path planning problem of a specific robotic application in which a mobile manipulator robot is instructed to reach at and touch a target object by its end effector while navigating through an area where multiple moving and stationary obstacles are deployed. The organization of this paper is as follows: in Section 2, the APBP to solve a robotic path planning problem is introduced. In Section 3, we discuss the proposed subplanner algorithm to escape from local minima. Section 4 presents simulation results to substantiate the capability of the proposed subplanner. Section 5 will be the conclusion of this paper.

2. Artificial Potential Based Path Planning for a Mobile Manipulator

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

2.1 A Mobile Manipulator and its Configuration Space

Figure 1 shows a mobile manipulator model that will be considered as our application throughout this paper. The mobile manipulator consists of two main parts: a mobile base part and four-links (three joints) elbow manipulator part. The base frame of the manipulator is attached on the top-center of the mobile base; hence, the configuration of the mobile manipulator, denoted by q, is

(1)q=[x,y,θ1,θ2,θ3,θ4]T,

where [x, y, θ₁]^T corresponds to the position and orientation of the mobile base, and [θ₂, θ₃, θ₄]^T represents the rotations of manipulator joint. Accordingly, the configuration space of the mobile manipulator is

(2)Q⊆ℝ×ℝ×S×S×S×S=ℝ2×T4.

2.2 A Mobile Manipulator Path Planning: Problem Definition

Let q, , , and be a configuration, a configuration space, a free configuration space, and an obstacle-occupied configuration space, respectively. For our mobile manipulator, given an initial time t₀ and a final time t_f, we want to find a path γ that is a continuous mapping

(3)γ:[t0,tf]→Q,

which satisfies

γ(s)∈Qfree,∀s∈[t0,tf] and γ(t0)=qinit, γ(tf)=qgoal,

where . Note that the role of the APBP is to assign a free configuration q to γ(s); thus

(4)γ(s)←q and q∈Qfree.

2.3 Designing an Artificial Potential Based Planner

2.3.1 Setting the control points of a mobile manipulator in a workspace

A term “control points” refers to the points of our mobile manipulator on where the artificial potentials have an effect. To set the control points, we start with defining p_i(q) that maps a given configuration q in a configuration space onto a three dimensional position vector p_i in a workspace

(5)pi(q): ℝ2×T4→ℝ3.

Now, we want to set five control points on the mobile manipulator as follows: i) the origin of the mobile base frame, ii) the origin of the joint#1, iii) the origin of the joint#2, iv) the origin of the joint#3, and v) the origin of the end effector. Given q, we can specify p₁(q) through p₅(q) with the Denavit-Hartenberg (DH) parameters of the mobile manipulator (see Figure 1)

(6)p1(q)=[xy0], p2(q)=[xyd1],p3(q)=[x+a2c1c2y+a2s1c2a2s2+d1],p4(q)=[x+a3c1c2c3-a3c1s2s3+a2c1c2y+a3s1c2c3-a3s1s2s3+a2s1c2a3s2c3+a3c2s3+a2s2+d1],p5(q)=[(x+a4c1c2c3c4-a4c1c2s3s4+a3c1c2c3-a4c1s2s3c4-a4c1s2c3s4-a3c1s2s3+a2c1c2)(y+a4s1c2c3c4-a4s1c2s3s4+a3s1c2c3-a4s1s2s3c4-a4s1s2c3s4-a3s1s2s3+a2s1c2)(a4s2c3c4-a4s2s3s4+a3s2c3+a4c2s3c4+a4c2c3s4+a3c2s3+a2s2+d1)],

where c_i and s_i are shorthand notations for cos θ_i and sin θ_i, respectively.

2.3.2 Attractive potential in workspace

The role of attractive potential is to attract a current control point p_i(q) to a goal control point p_i(q_goal) by yielding a vector field in a workspace . The most common choice of attractive potential function is the combination of conic potential and quadratic potential for two conditions stating whether p_i(q) is close to p_i(q_goal) or not [1, 2].

Let ||·|| denote L₂-norm. We first define the two conditions as

(c1)‖pi(q)-pi(qgoal)‖ >ɛ,

(c2)‖pi(q)-pi(qgoal)‖ ≤ɛ,

where ε is a threshold distance from p_i(q_goal) where a planner switches between conic and quadratic potentials. Now, the attractive potential function U_att_,i can be defined by

(7)Uatt,i(pi(q))={ɛζ‖pi(q)-pi(qgoal)‖-12ζɛ2,if (c1),12ζ‖pi(q)-pi(qgoal)‖2,if (c2)

where a scaling factor ζ is a small positive constant.

The corresponding attractive force field F_att_,i can be yielded by taking a partial derivative with respect to p_i(q) and negation

(8)Fatt,i(pi(q))=-∇pi(q)Uatt,i(pi(q))={-ɛζ(pi(q)-pi(qgoal))‖pi(q)-pi(qgoal)‖,if (c1),-ζ(pi(q)-pi(qgoal)),if (c2),

which converges to a zero vector as p_i(q) approaches to p_i(q_goal) by switching U_att_,i in Eq. (7) from the conic potential (c1) to the quadratic potential (c2). Figure 2 presents the example of the attractive potential and the corresponding attractive force field in ℝ².

2.3.3 Repulsive potential in workspace

The role of repulsive potential is to repulse a current control points p_i(q) when it approaches toward any j^th obstacle in a workspace . Let o_j denote a point on the j^th obstacle that yields the shortest distance from p_i(q). We first define two conditions stating whether p_i(q) is within δ boundary from the shortest point of the j^th obstacle

(c3)‖pi(q)-oj‖ <δ,

(c4)‖pi(q)-oj‖ ≥δ.

Now, the repulsive potential function U_rep,i,j can be defined as

(9)Urep,i,j(pi(q))={η2(1‖pi(q)-oj‖-1δ)2,if (c3),0,if (c4),

where a scaling factor η is a positive constant. The corresponding repulsive force F_rep_,i,j is equal to

(10)Frep,i,j(pi(q))=-∇pi(q)Urep,i,j(pi(q))={η(1‖pi(q)-oj‖-1δ)1‖pi(q)-oj‖2(pi(q)-oj)‖pi(q)-oj‖,if (c3),0,if (c4).

Figure 3 shows the example of the repulsive potential and the corresponding repulsive force field in ℝ².

2.3.4 Building a path in configuration space by using APBP

To map two artificial forces, F_att_,i and F_rep_,i,j, in a workspace onto a force in a configuration space , we define Jacobian J_i(q) ∈ ℝ^3×6 for i^th control points p_i(q)

(11)Ji(q)=∂pi(q)∂q,

for example

J3(q)=∂p3(q)∂q=[10-a2s1c2-a2c1s20001a2c1c2-a2s1s200000a2c200].

Now, F_att_,i and F_rep_,i,j can be mapped onto via JiT(q)

(12)FQ(q)=∑iJiT(q)Fatt,i(pi(q))+∑j∑iJiT(q)Frep,i,j(pi(q)),

which plans a next configuration q′ at a current configuration q by

(13)q′←q+FQ(q).

To build a path γ, the next configuration is assigned to the path for a time stamp s+Δs

(14)s′←s+Δs, γ(s′)←q′,

then we update the time stamp, the current configuration, and the control points by

(15)s←s′, q←q′, pi(q)←pi(q′).

By repeating a procedure Eq. (12) through Eq. (15) until q is equal to q_goal, we can build a path γ(s) for s ∈ [t₀, t_f ] under the APBP (of course, Eq. (12) requires to calculate Eq. (6) through Eq. (11)). The mobile manipulator path planning algorithm by using the APBP is summarized in Table 1.

3. A Method to Escape from Local Minima

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

3.1 The Existence of Local Minima under the APBP

From Eq. (12), we have seen that F_att_,i and F_rep_,i,j can be mapped onto via JiT(q). By rearranging the right side of Eq. (12), we can obtain

(16)FQ(q)=JiT(q) (∑iFatt,i(pi(q))+∑j∑iFrep,i,j(pi(q))),

which implies that if there exists a configuration q such that

(17)∑iFatt,i(pi(q))+∑j∑iFrep,i,j(pi(q))=[0,0,0]T,

then

(18)FQ(q)=[0,0,0,0,0,0]T.

By recalling Eq. (13), we can know that a next configuration q′ will be the same as q. In other words, a path γ is stopped at the configuration q which means that the APBP fails to build a path. Accordingly, the all control points of the mobile manipulator p_i(q) are trapped in one pose. We refer to these problematic configurations as local minima.

3.2 Detecting Local Minima in Partitioned Configuration Spaces

Suppose that we give a task for a mobile manipulator to grasp an object on the table by its end effector (gripper). If the mobile base part cannot approach to the target object or the elbow manipulator part get stuck at some point, the mobile manipulator certainly fails to complete the given task. In this sense, if positional variations in the mobile base part or/and the elbow manipulator part related to in Eq. (16) are vanished as illustrated in Figure 4, then our mobile manipulator will never reach to a goal configuration. Therefore, we focus on detecting local minima caused by the mobile base part and the elbow manipulator part to investigate whether they are trapped for long iterations, respectively.

To detect those local minima, we first partitions a configuration space into two parts: includes the configuration of the mobile base q_m = [x, y]^T ∈ ℝ² and consists of the configuration of the elbow manipulator . Then, the two types of local minima caused by the mobile base part and/or the elbow manipulator part can be detected easily by investigating the following two conditions. For the local minima caused by the mobile base part, we first project a current configuration q and a goal configuration q_goal in onto the partitioned configuration space ; thus,

q=[x,y,θ1,θ2,θ3,θ4]T→q¯=[x,y,0,0,0,0]T,qgoal=[x*,y*,θ1*,θ2*,θ3*,θ4*]T→q¯goal=[x*,y*,0,0,0,0]T.

Then, we calculate the summation of the attractive force and the repulsive forces and investigate whether

(19)Fatt,1(p1(q¯))+∑jFrep,1,j(p1(q¯))→[0,0,0]T.

If Eq. (19) converges to [0, 0, 0]^T, then it implies that the mobile base part falls into local minima even in the absence of the elbow manipulator part. Next, q and q_goal in is projected onto to detect the local minima caused by the elbow manipulator part

q=[x,y,θ1,θ2,θ3,θ4]T→q¯=[0,0,θ1,θ2,θ3,θ4]T,qgoal=[x*,y*,θ1*,θ2*,θ3*,θ4*]T→q¯goal=[0,0,θ1*,θ2*,θ3*,θ4*]T

This projection corresponds to the translation of the obstacle positions in x-y plane, thus

oj=[xobs,yobs,zobs]T→o¯j=[xobs-x,yobs-y,zobs]T.

Then, we investigate whether

(20)∑i=25Fatt,i(pi(q¯))+∑j∑i=25Frep,i,j(pi(q¯))→[0,0,0]T,

which means that the elbow manipulator part itself eventually traps in the local minima even if the position of the mobile base part is fixed.

3.3 Subplanner Algorithm to Escape from Local Minima: Attractive Force Rotation and Restoration

The existence of local minima is critical issue when we employ the APBP to solve a path planning problem. In general, it is even difficult to visualize or infer the location of local minima in high dimensional configuration space. Fortunately, we could characterize the two types of local minima, which belong to a mobile base and an end effector, by utilizing insights from the kinematic feature of our mobile manipulator.

When a path is planned under the APBP, a current configuration is attracted to a goal configuration by an attractive force F_att_,i. Therefore, if we can design a subplanner algorithm which makes a mobile manipulator to escape from local minima by temporarily changing and restoring the direction of F_att_,i until the positions of the mobile base and the end effector reaches at the point being apart from any obstacles with a safety margin. We call this subplanner algorithm as an attractive force rotation and restoration (AFRORE) algorithm. The algorithmic foundation of the AFRORE algorithm is introduced below by two processes: attractive force rotation (AFRO) and attractive force restoration (AFRE), respectively.

3.3.1 Attractive force rotation

To explain the AFRO, we exemplify a case of which a mobile base part is trapped by a local minimum to show the AFRO illustratively as in Figure 5. For clarity, let us start with defining shorthand notations

∑iFatt,i(pi(q)):=Fatt,∑j∑iFrep,i,j(pi(q)):=Frep,and Fsum=Fatt+Frep.

At a local minimum, F_att is equal to F_rep; hence F_sum becomes [0, 0, 0]^T.

When the mobile manipulator is trapped into the local minimum, we can identify that this local minimum is due to a mobile base part by investigating Eq. (19). A key idea of the AFRO is to keep rotating F_att to one direction, say clockwise by θ_R (note: θ_R is negative value) along the z-axis of a current local frame of the mobile manipulator for an iteration s+Δs ← s

(21)Fatt∣s+Δs←[cθR-sθR0sθRcθR0001] Fatt∣s,

until F_sum guides the control point p₁(q) of the mobile manipulator to the point lying on a δ-boundary from an obstacle at where F_rep vanishes. Figure 5 illustrate the AFRO process for the local minimum caused by the mobile base part.

Similarly, when the elbow manipulator part falls into a local minimum, which can be identified by investigating Eq. (20), the AFRO keeps rotating F_att by θ_R along the y-axis of a current local frame of the mobile manipulator

(22)Fatt∣s+Δs←[cθ1-sθ10sθ1cθ10001] [cθR0sθR010-sθR0cθR] Fatt∣s,

until F_sum leads the control point p₂(q) through p₅(q) of the mobile manipulator to the point lying on the δ-boundary from the obstacle.

Under the AFRO process, the generated F_sum allows both the mobile base part and the elbow manipulator part to contribute to escape local minima. For instance, when Eq. (22) is processed to escape from local minima related to off-ground obstacles and the elbow manipulator part, the generated F_sum acts on all five control points; hence, the mobile base part is also pushed away from the off-ground obstacles. This advantageous characteristic of the AFRO helps and ensures the mobile manipulator to escape from the local minima cased by the mobile base part or/and the elbow manipulator part. The AFRO is followed by an AFRE process introduced below.

3.3.2 Attractive force restoration

Suppose that p₁(q) of the mobile base reaches at a point lying on δ-boundary from the closest obstacle by the AFRO as shown in Figure 5. At this point, if the mobile base traces the circumference of a circle R of which radius is larger than δ toward counter-clockwise direction, then the mobile base never goes into δ-boundary. Therefore, a key idea of AFRE process is that we restore F_att by keep rotating it by θ_L (= −θ_R), along the z-axis of a current local frame of the mobile manipulator

(23)Fatt∣s+Δs←[cθL-sθL0sθLcθL0001] Fatt∣s.

Figure 6 illustrates the AFRE process in which the mobile base follows a circular trajectory lying on the circumference of a circle with a radius R.

The radius R determines the curvature of the circular trajectory and can be defined as a function of θ_L. From Eq. (8) and Eq. (10), we know that F_sum becomes a vector of which size is ||εζ||. AS shown in Figure 6(a), if Eq. (23) is executed 2πθL times while the mobile base is being moved by ||εζ||, then the circumference is completely traced. Therefore, we can calculate R by

(24)‖ɛζ‖·2πθL=2πR⇒R=‖ɛζ‖θL.

During the AFRE process, if a distance between p₁(q) and the closest obstacle becomes greater than αδ, where α is a positive constant to set up a safety margin, then the AFRE is terminated; finally, a path planner is switched to the APBP as the mobile manipulator has successfully escaped from the local minima.

Similarly, when the local minima is caused by the elbow manipulator part, the AFRE is performed along the y-axis of a current local frame of the mobile manipulator

(25)Fatt∣s+Δs←[cθ1-sθ10sθ1cθ10001] [cθR0sθR010-sθR0cθR] Fatt∣s.

If the shortest distance from any control points of the elbow manipulator part, p₂(q) through p₅(q), to the closest obstacle is greater than αδ, then a path planner is switched to the APBP.

Table 2 presents the AFRORE algorithm. The APBP for the mobile manipulator path planning with the subplanner AFRORE for escaping from local minima is summarized in Table 3.

4. Simulation Results

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

The capability of the proposed planner, the APBP with the AFRORE algorithm, was tested under various environmental scenarios. Considering the main role of the mobile manipulator as a co-working robot or a service robot, we created the environmental scenarios in which the mobile manipulator tries to reach a goal configuration to pick up a target object. We set stationary obstacles as well as moving obstacles which can be regarded as moving people, round tables, etc. For all scenarios, the initial and the goal configuration of the mobile manipulator were set to q_init = [5, 50, 0π, 0.5π, 0π, 0π]^T and q_goal = [93.597, 49.245, 0.157π, 0.304π, 0.185π, −0.430π]^T, respectively, which correspond to the end effector position [x,y,z]initT=[5,50,7]T and [x,y,z]goalT=[95,50,6]T. Stationary and moving obstacles are presented as black (black) and green (gray) circles, respectively.

Note that all presented simulation results were also uploaded as video files and can be found at https://www.dropbox.com/s/ea2xqvyqqkzbvew/MobileManiPlanning.html.

4.1 Scenario 1: Stationary On-Ground Obstacles Deployed like Stepping Stones

As illustrated in Figure 7, five stationary obstacles were placed as the stepping stones. In this scenario, we considered an office room in where five round tables are placed. Figure 7(a) shows the mobile manipulator is heading to the goal configuration. Since there existed no local minima, the AFRORE algorithm was not executed. Under APBP, the trace in Figure 7(b)–(d) depicts that the mobile manipulator successfully reached at the goal without collision against the obstacles.

4.2 Scenario 2: Moving-to-Stationary On-Ground Obstacles with a Local Minimum

Figure 8 shows the second scenario. In this scenario, we considered a situation in which five people meet and start chatting at the center of a room. We allowed the obstacles to meet around the center area after the mobile manipulator took off from the initial position. As the obstacle were deployed around the center, they created a local minimum by which the mobile manipulator could be trapped under simple APBP.

In Figure 8(a), the mobile manipulator is moving toward the goal configuration, and the obstacles is gathering around the center. After the mobile manipulator falls into the local minimum, it detects that the local minimum is caused by the mobile base part; therefore, it executed the AFRO of Eq. (21) to escape from the local minimum as shown in Figure 8(b). As the mobile manipulator reaches at the outside of δ-boundary, the AFRO is followed by the AFRE of Eq. (23) by which the mobile manipulator traces a circular trajectory as presented in Figure 8(c). When the distance to the closest obstacle is greater than a safety margin, the planner is switched to the APBP, and Figure 8(d) shows that the mobile manipulator position reaches at the goal.

4.3 Scenario 3: Off-Ground Stationary Obstacle and Moving-to-Stationary On-Ground Obstacles with Local Minima

In the third scenario illustrated in Figure 9, we set an off-ground stationary obstacle (a monkey bar), five on-ground moving-to-stationary obstacles (chatting people), and two on-ground stationary obstacles (round tables). By this environmental setup, local minima for the elbow manipulator part and the mobile base part were created, respectively. Also, the chatting people would meet at the center of a room; in contrast to the second scenario, the gap between chatting people became narrower in this scenario.

Figure 9(a) shows that the elbow manipulator part is trapped in local minima. To escape from this local minima, the AFRO of Eq. (22) and the AFRE of Eq. (25) are activated in sequel after detecting the local minima caused by the elbow manipulator part, which in turn, serves to pass underneath the monkey bar as illustrated in Figure 9(b)–(c). After passing the monkey bar, the mobile base part falls into local minima created by the chatting people. Indeed, there exist multiple local minima due to the narrowly deployed chatting people. Figure 9(d)–(e) shows that the mobile manipulator is escaping from the local minima by performing the AFRO of Eq. (21) and the AFRE of Eq. (23) multiple times. Finally, the planner is switched to the APBP, and the mobile manipulator position reaches at the goal as presented in Figure 9(f).

4.4 Scenario 4: Room Crowded by Moving On-Ground Obstacles with Possible Local Minima

In the fourth scenario illustrated in Figure 10, the mobile manipulator is supposed to reach the goal configuration after passing a room crowded by randomly moving on-ground obstacles. This scenario corresponds to a situation in which the mobile manipulator is performing a given task while people are moving in workplace. Figure 10(a)–(f) shows that the mobile manipulator successfully reaches to the target object. However, even though the cases are not presented here, we note that the mobile manipulator fails to achieve the given task if it is perfectly surrounded by the randomly moving people.

5. Conclusion and Future Works

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

In this paper, we proposed a subplanner algorithm, which was referred to as the AFRORE algorithm, to escape local minima which interferes the APBP to build a desired path. The proposed subplanner algorithm used insight from the kinematics of the robotic system; specifically, it allowed the APBP to overcome weakness by detecting local minima in partitioned configuration spaces and subplanning a path to escape from the detected local minima. The capability of the proposed subplanner was substantiated by the simulation results in which a mobile manipulator reached a target object successfully under various environmental conditions. A limitation of the proposed approach might be that we should priorly have an insight about local minima related to the kinematics of the employed robotic system, which is rather difficult for the highly complex robotic systems.

As future works, the capability of the proposed subplanner algorithm should be investigated for more complex robotic systems such as dual-arm robot, humanoid, etc., of which both kinematics and configuration space has higher complexity. Also, the dynamic tunning of scaling factors in artificial potential functions can be formulated as an optimization problem to find a path with desired criteria, e.g., a shorter but riskier path or a longer but safer path. Lastly, enhancing the main planner and the proposed subplanner by combining with other types of planners, e.g., geometry-based planner or sampling-based planner, to solve more complex problem would be an interesting topic as well.

Acknowledgments

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

This work was supported by the Korea Institute of Industrial Technology under “Development of Soft Robotics Technology for Human-Robot Coexistence Care Robots (No. EO180026).”

Figures

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

Fig. 1.

A mobile manipulator robot with three elbow joints and its DH-parameters.

Fig. 2.

The example of the attractive potential in ℝ²: (a) start [5, 50]^T and goal [95, 50]^T positions, (b) attractive potential U_att, and (c) attractive force field F_att.

Fig. 3.

The example of the repulsive potential in ℝ²: (a) three obstacles (black circles), (b) repulsive potential ∑j=13Urep, and (c) repulsive force field ∑j=13Frep.

Fig. 4.

The mobile manipulator fails to reach a goal position (blue star) as the force field is vanished in the following two cases: (a) local minima cuased by the mobile manipulator part and on-ground obstacles and (b) local minima caused by the elbow manipulator part and an off-ground obstacle.

Fig. 5.

Attractive force rotation (AFRO) process: (a) the mobile base part of a mobile manipulator falls in a local minimum, F_sum = 0, (b) the subplanner keeps rotating F_att clockwise by θ_R, (c) F_att is resurrected, and the mobile manipulator moves along with it, and (d) the mobile manipulator has escaped from a local minimum.

Fig. 6.

Attractive force restoration (AFRE) process: (a) (continued from Figure 5) the subplanner keeps restoring F_att counter-clockwise by θ_L. Consequently, the mobile manipulator moves along with the circumference of a circle with radius R, (b) the planner is switched to the APBP since the distance from the closest obstacle is greater than the predetermined safety margin αδ.

Fig. 7.

Stationary on-ground obstacles deployed like stepping stones: (a) the mobile manipulator is heading to the goal configuration, (b) the mobile manipulator is passing by the second obstacle, (c) the mobile manipulator keeps passing through obstacles without any collision against the obstacles, and (d) the mobile manipulator reached at the goal.

Fig. 8.

Moving-to-stationary on-ground obstacles with a local minimum: (a) the mobile manipulator is moving toward the goal configuration, (b) the mobile base part is trapped by the obstacles and fell into a local minimum. It starts the AFRO, (c) the mobile manipulator has escaped from the local minimum while processing the AFRE, and (d) the mobile manipulator arrived at the goal.

Fig. 9.

Off-ground stationary obstacle and moving-to-stationary onground obstacles with local minima: (a) the elbow manipulator part is trapped in local minima, (b) the elbow manipulator part is moving down and the mobile base part is being pushed away by the AFRO process, (c) the mobile manipulator is passing underneath the monkey bar, (d) the AFRO and the AFRE has been processed in sequel to escape from the first local minima, (e) the mobile manipulator has been escaped from the second local minima by processing the AFRORE, and (f) the mobile manipulator reached at the goal after passing two stationary on-ground obstacles.

Fig. 10.

Room crowded by moving on-ground obstacles with possible local minima: (a) the mobile manipulator is running through people-free area in where the people are moving randomly, (b) the mobile manipulator is avoiding the people under the APBP and stopped by a local minimum in front of its heading direction, (c)– (e) the mobile manipulator has been successfully avoided collision against the randomly moving people by processing the AFRORE multiple times, and (f) the mobile manipulator has been reached at the goal.

Tables

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

Table. 1.

Table 1. APBP for a mobile manipulator path planning.

Given q_init, q_goal, and t₀
• Initialize: q ← q_init and γ(t₀) ← q_init
WHILEq is not q_goal
• Calculate from F_att_,i(p_i(q)) and F_rep_,i,j(p_i(q))	Eq. (12)
• Plan a next configuration:	Eq. (13)
• Increase a time stamp: s′ ← s+Δs	Eq. (14)
• Assign q′ to a path: γ(s′) ← q′	Eq. (14)
• Update: s ← s′, q ← q′, p_i(q) ← p_i(q′)	Eq. (15)
END
• Finalize: t_f ← s and γ(t_f ) ← q_goal
RETURNγ

Table. 2.

Table 2. Subplanner AFRORE for escaping from local minima.

Function: AFRORE(localmin, type)
/ * Atractive Force Rotation * /
WHILElocalmin
SWITCHtype
case
• F_att\|_s_+Δ ← Rot_z(θ_R) · F_att\|_s	Eq. (21)
• Do Eq. (12), Eq. (13), Eq. (14), and Eq. (15)
• checkpoint ← p₁(q)
break
case
• F_att\|_s_+Δ ← Rot_z(θ₁) · Rot_x(θ_R) · F_att\|_s	Eq. (22)
• Do Eq. (12), Eq. (13), Eq. (14), and Eq. (15)
• checkpoint ← {p₂(q), p₃(q), p₄(q), p₅(q)}
break
END
IF min \|\|checkpoint – o_j\|\|≥ δ
localmin ← false
break
ELSE
repeat WHILE
END
/ * Atractive Force Restoration * /
WHILE min \|\|checkpoint – o_j\|\|≤ αδ
SWITCHtype
case
• F_att\|_s_+Δ ← Rot_z(θ_L) · F_att\|_s	Eq. (23)
• Do Eq. (12), Eq. (13), Eq. (14), and Eq. (15)
• checkpoint ← p₁(q)
break
case
• F_att\|_s_+Δ ← Rot_z(θ₁) · Rot_x(θ_L) · F_att\|_s	Eq. (25)
• Do Eq. (12), Eq. (13), Eq. (14), and Eq. (15)
• checkpoint ← {p₂(q), p₃(q), p₄(q), p₅(q)}
break
END
IF min \|\|checkpoint – o_j\|\|≥ αδ
break
ELSE
repeat WHILE
END

Table. 3.

Table 3. APBP for a mobile manipulator path planning with subplanner AFRORE for escaping from local minima.

Given q_init, q_goal, localmin and t₀
• Initialize: q ← q_init, γ(t₀) ← q_init, localmin ← false
/ * Atractive Potential Based Planner * /
WHILEq is not q_goal
• Calculate from F_att_,i(p_i(q)) and F_rep_,i,j(p_i(q))	Eq. (12)
IF
• localmin ← true
• Detect local minimum type: type	Eq. (19) and Eq. (20)
• Run AFRORE(localmin, type)	(see Table 2)
ELSE
• Plan a next configuration:	Eq. (13)
• Increase a time stamp: s′ ← s+Δs	Eq. (14)
• Assign q′ to a path: γ(s′) ← q′	Eq. (14)
• Update: s ← s′, q ← q′, p_i(q) ← p_i(q′)	Eq. (15)
END
• Finalize: t_f ← s and γ(t_f ) ← q_goal
RETURNγ

References

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

Latombe, JC (2003). Robot Motion Planning. Boston, MA: Kluwer Academic Publishers
Choset, H, Lynch, KM, Hutchinson, S, Kantor, G, Burgard, W, Kavraki, LE, and Thrun, S (2005). Principles of Robot Motion: Theory, Algorithms, and Implementations. Cambridge, MA: The MIT Press
Spong, MW, Hutchinson, S, and Vidyasagar, M (2006). Robot Modeling and Control. Hoboken, NJ: John Wiley and Sons
Canny, J 1985. A Voronoi method for the piano-movers problem., Proceedings of IEEE International Conference on Robotics and Automation, St Louis, MO, Array, pp.530-535. http://doi.org/10.1109/ROBOT.1985.1087297
Halperin, D, Salzmann, O, and Sharir, M (2017). Algorithmic motion planning. Handbook of Discrete and Computational Geometry. Boca Raton, FL: CRC Press, pp. 1311-1342
Duchon, F, Babinec, A, Kajan, M, Beno, P, Florek, M, Fico, T, and Jurisica, L (2014). Path planning with modified a star algorithm for a mobile robot. Procedia Engineering. 96, 59-69. https://doi.org/10.1016/j.proeng.2014.12.098
Zhao, R, Lee, DH, and Lee, HK (2015). Fuzzy logic based navigation for multiple mobile robots in indoor environments. International Journal of Fuzzy Logic and Intelligent Systems. 15, 305-314. http://dx.doi.org/10.5391/IJFIS.2015.15.4.305
Kim, DH (2010). Integrated path planning and collision avoidance for an omni-directional mobile robot. International Journal of Fuzzy Logic and Intelligent Systems. 10, 210-217. http://dx.doi.org/10.5391/IJFIS.2010.10.3.210
Bircher, A, Kamel, M, Alexis, K, Burri, M, Oettershagen, P, Omari, S, Mantel, T, and Siegwart, R (2006). Three-dimensional coverage path planning via viewpoint resampling and tour optimization for aerial robots. Autonomous Robots. 40, 1059-1078. https://doi.org/10.1007/s10514-015-9517-1
Alexis, K, Papachristos, C, Siegwart, R, and Tzes, A 2015. Uniform coverage structural inspection path-planning for micro aerial vehicles., Proceedings of 2015 IEEE International Symposium on Intelligent Control, Sydney, Australia, Array, pp.59-64. http://dx.doi.org/10.1109/ISIC.2015.7307280
Feyzabadi, S, and Carpin, S 2014. Risk-aware path planning using hirerachical constrained Markov decision processes., Proceedings of 2014 IEEE International Conference on Automation Science and Engineering, Taipei, Taiwan, Array, pp.297-303. http://dx.doi.org/10.1109/CoASE.2014.6899341
Cho, JH, and Kim, YT (2017). Design of autonomous logistics transportation robot system with fork-type lifter. International Journal of Fuzzy Logic and Intelligent Systems. 17, 177-186. http://dx.doi.org/10.5391/IJFIS.2017.17.3.177
Jaillet, L, and Porta, JM (2016). Path planning with loop closure constraints using an atlas-based RRT. Robotics Research. Cham: Springer, pp. 345-362 https://doi.org/10.1007/978-3-319-29363-920
Wei, Y, Jiang, W, Rahmani, A, and Zhan, Q. (2018) . Motion planning for a humanoid mobile manipulator system. Available https://arxiv.org/abs/1806.07349
Lavalle, S (2006). Planning Algorithms. Cambridge, UK: Cambridge University Press, pp. 154-205
Hsu, D, Kavraki, LE, Latombe, JC, Motwani, R, and Sorkin, S (1998). On finding narrow passages with probabilistic roadmap planners. Robotics: The Algorithmic Perspective: 1998 Workshop on the Algorithmic Foundations of Robotics. Natick, MA: A K Peters, pp. 141-153
Canny, J, and Reif, J 1987. Lower bounds for shortest path and related problems., Proceedings of the 28th Annual Symposium on Foundations of Computer Science, Los Angeles, CA, pp.49-60.
Koren, Y, and Borenstein, J 1991. Potential field methods and their inherent limitations for mobile robot navigation., Proceedings of IEEE International Conference on Robotics and Automation, Sacramento, CA, Array, pp.1398-1404. https://doi.org/10.1109/ROBOT.1991.131810
Sezer, V, and Gokasan, M (2012). A novel obstacle avoidance algorithm: follow the gap method. Robotics and Autonomous Systems. 60, 1123-1134. https://dx.doi.org/10.1016/j.robot.2012.05.021
Ma, Y, Zheng, G, Perruquetti, W, and Qiu, A (2015). Local path planning for mobile robots based on intermediate objectives. Robotica. 33, 1017-1031. http://dx.doi.org/10.1017/S0263574714000186
Singh, Y, Sharma, S, Hatton, D, and Sutton, R (2018). Optimal path planning of unmanned surface vehicles. Indian Journal of Geo Marine Sciences. 47, 1325-1334.

Biographies

Go to

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works
Acknowledgments
Figures
Tables
References
Biographies

Han Ul Yoon received his Ph.D. degree from the Department of Electrical and Computer Engineering at the University of Illinois at Urbana-Champaign in 2014. Dr. Yoon served as a postdoctoral research associate and an assistance professor of instruction in the Department of Mechanical Engineering at Texas A&M University in 2014–2016 and 2017, respectively. He is currently a senior researcher in Robotics R&D Group at the Korea Institute of Industrial Technology (KITECH). He is mainly interested in assistive human-robot interaction, biomechanical analysis and control, and robotic path planning.

E-mail: huyoon@kitech.re.kr

Dong-Wook Lee received his Ph.D. from the Department of Control and Instrumentation Engineering at Chung-Ang University in 2000. Dr. Lee was a research professor at Chung-Ang University from 2002 to 2004. He was a post-doctoral researcher at the University of Tennessee at Knoxville from 2004 to 2005. He is currently a principal researcher in Robotics R&D Group at Korea Institute of Industrial Technology (KITECH). His research interests include android robot, social human-robot interaction, and evolutionary computation.

E-mail: dwlee@kitech.re.kr

Article

Original Article

International Journal of Fuzzy Logic and Intelligent Systems 2018; 18(4): 263-275

Published online December 31, 2018 https://doi.org/10.5391/IJFIS.2018.18.4.263

Subplanner Algorithm to Escape from Local Minima for Artificial Potential Function Based Robotic Path Planning

Han Ul Yoon, and Dong-Wook Lee

¹Robotics R&D Group, Korea Institute of Industrial Technology, Ansan, Korea, ²Department of Robotics and Virtual Engineering, Korea University of Science and Technology, Daejon, Korea

Correspondence to: Dong-Wook Lee, (dwlee@kitech.re.kr)

Received: August 11, 2018; Revised: November 1, 2018; Accepted: November 13, 2018

Abstract

Keywords: Path planning, Mobile manipulator, Artificial potential based planner, Attractive force rotation and restoration algorithm

1. Introduction

2. Artificial Potential Based Path Planning for a Mobile Manipulator

2.1 A Mobile Manipulator and its Configuration Space

(1)

q = {[x, y, θ_{1}, θ_{2}, θ_{3}, θ_{4}]}^{T},

(2)

Q \subseteq ℝ \times ℝ \times S \times S \times S \times S = ℝ^{2} \times T^{4} .

2.2 A Mobile Manipulator Path Planning: Problem Definition

(3)

γ : [t_{0}, t_{f}] \to Q,

which satisfies

γ (s) \in Q_{free}, \forall s \in [t_{0}, t_{f}] and γ (t_{0}) = q_{init}, γ (t_{f}) = q_{goal},

where . Note that the role of the APBP is to assign a free configuration q to γ(s); thus

(4)

γ (s) \leftarrow q and q \in Q_{free} .

2.3 Designing an Artificial Potential Based Planner

2.3.1 Setting the control points of a mobile manipulator in a workspace

(5)

p_{i} (q) : ℝ^{2} \times T^{4} \to ℝ^{3} .

(6)

\begin{array}{l} p_{1} (q) = [\begin{matrix} x \\ y \\ 0 \end{matrix}], p_{2} (q) = [\begin{matrix} x \\ y \\ d_{1} \end{matrix}], \\ p_{3} (q) = [\begin{array}{l} x + a_{2} c_{1} c_{2} \\ y + a_{2} s_{1} c_{2} \\ a_{2} s_{2} + d_{1} \end{array}], \\ p_{4} (q) = [\begin{array}{l} x + a_{3} c_{1} c_{2} c_{3} - a_{3} c_{1} s_{2} s_{3} + a_{2} c_{1} c_{2} \\ y + a_{3} s_{1} c_{2} c_{3} - a_{3} s_{1} s_{2} s_{3} + a_{2} s_{1} c_{2} \\ a_{3} s_{2} c_{3} + a_{3} c_{2} s_{3} + a_{2} s_{2} + d_{1} \end{array}], \\ p_{5} (q) = [\begin{matrix} (x + a_{4} c_{1} c_{2} c_{3} c_{4} - a_{4} c_{1} c_{2} s_{3} s_{4} + a_{3} c_{1} c_{2} c_{3} \\ - a_{4} c_{1} s_{2} s_{3} c_{4} - a_{4} c_{1} s_{2} c_{3} s_{4} - a_{3} c_{1} s_{2} s_{3} \\ + a_{2} c_{1} c_{2}) \\ (y + a_{4} s_{1} c_{2} c_{3} c_{4} - a_{4} s_{1} c_{2} s_{3} s_{4} + a_{3} s_{1} c_{2} c_{3} \\ - a_{4} s_{1} s_{2} s_{3} c_{4} - a_{4} s_{1} s_{2} c_{3} s_{4} - a_{3} s_{1} s_{2} s_{3} \\ + a_{2} s_{1} c_{2}) \\ (a_{4} s_{2} c_{3} c_{4} - a_{4} s_{2} s_{3} s_{4} + a_{3} s_{2} c_{3} + a_{4} c_{2} s_{3} c_{4} \\ + a_{4} c_{2} c_{3} s_{4} + a_{3} c_{2} s_{3} + a_{2} s_{2} + d_{1}) \end{matrix}], \end{array}

where c_i and s_i are shorthand notations for cos θ_i and sin θ_i, respectively.

2.3.2 Attractive potential in workspace

Let ||·|| denote L₂-norm. We first define the two conditions as

(c1)

‖ p_{i} (q) - p_{i} (q_{goal}) ‖ > ɛ,

(c2)

‖ p_{i} (q) - p_{i} (q_{goal}) ‖ \leq ɛ,

where ε is a threshold distance from p_i(q_goal) where a planner switches between conic and quadratic potentials. Now, the attractive potential function U_att_,i can be defined by

(7)

U_{att, i} (p_{i} (q)) = {\begin{array}{l} ɛ ζ ‖ p_{i} (q) - p_{i} (q_{goal}) ‖ - \frac{1}{2} ζ ɛ^{2}, & if (c 1), \\ \frac{1}{2} ζ {‖ p_{i} (q) - p_{i} (q_{goal}) ‖}^{2}, & if (c 2) \end{array}

where a scaling factor ζ is a small positive constant.

The corresponding attractive force field F_att_,i can be yielded by taking a partial derivative with respect to p_i(q) and negation

(8)

\begin{array}{l} F_{att, i} (p_{i} (q)) = - \nabla_{p_{i} (q)} U_{att, i} (p_{i} (q)) \\ = {\begin{array}{l} - \frac{ɛ ζ (p_{i} (q) - p_{i} (q_{goal}))}{‖ p_{i} (q) - p_{i} (q_{goal}) ‖}, & if (c 1), \\ - ζ (p_{i} (q) - p_{i} (q_{goal})), & if (c 2), \end{array} \end{array}

2.3.3 Repulsive potential in workspace

(c3)

‖ p_{i} (q) - o_{j} ‖ < δ,

(c4)

‖ p_{i} (q) - o_{j} ‖ \geq δ .

Now, the repulsive potential function U_rep,i,j can be defined as

(9)

U_{rep, i, j} (p_{i} (q)) = {\begin{array}{l} \frac{η}{2} {(\frac{1}{‖ p_{i} (q) - o_{j} ‖} - \frac{1}{δ})}^{2}, & if (c 3), \\ 0, & if (c 4), \end{array}

where a scaling factor η is a positive constant. The corresponding repulsive force F_rep_,i,j is equal to

(10)

\begin{array}{l} F_{rep, i, j} (p_{i} (q)) = - \nabla_{p_{i} (q)} U_{rep, i, j} (p_{i} (q)) \\ = {\begin{array}{l} η (\frac{1}{‖ p_{i} (q) - o_{j} ‖} - \frac{1}{δ}) \frac{1}{{‖ p_{i} (q) - o_{j} ‖}^{2}} \frac{(p_{i} (q) - o_{j})}{‖ p_{i} (q) - o_{j} ‖}, & if (c 3), \\ 0, & if (c 4) . \end{array} \end{array}

Figure 3 shows the example of the repulsive potential and the corresponding repulsive force field in ℝ².

2.3.4 Building a path in configuration space by using APBP

To map two artificial forces, F_att_,i and F_rep_,i,j, in a workspace onto a force in a configuration space , we define Jacobian J_i(q) ∈ ℝ^3×6 for i^th control points p_i(q)

(11)

J_{i} (q) = \frac{\partial p_{i} (q)}{\partial q},

for example

\begin{array}{l} J_{3} (q) = \frac{\partial p_{3} (q)}{\partial q} \\ = [\begin{matrix} 1 & 0 & - a_{2} s_{1} c_{2} & - a_{2} c_{1} s_{2} & 0 & 0 \\ 0 & 1 & a_{2} c_{1} c_{2} & - a_{2} s_{1} s_{2} & 0 & 0 \\ 0 & 0 & 0 & a_{2} c_{2} & 0 & 0 \end{matrix}] . \end{array}

Now, F_att_,i and F_rep_,i,j can be mapped onto via $J_{i}^{T} (q)$

(12)

F_{Q} (q) = \sum_{i} J_{i}^{T} (q) F_{att, i} (p_{i} (q)) + \sum_{j} \sum_{i} J_{i}^{T} (q) F_{rep, i, j} (p_{i} (q)),

which plans a next configuration q′ at a current configuration q by

(13)

q^{'} \leftarrow q + F_{Q} (q) .

To build a path γ, the next configuration is assigned to the path for a time stamp s+Δs

(14)

s^{'} \leftarrow s + Δ s, γ (s^{'}) \leftarrow q^{'},

then we update the time stamp, the current configuration, and the control points by

(15)

s \leftarrow s^{'}, q \leftarrow q^{'}, p_{i} (q) \leftarrow p_{i} (q^{'}) .

3. A Method to Escape from Local Minima

3.1 The Existence of Local Minima under the APBP

From Eq. (12), we have seen that F_att_,i and F_rep_,i,j can be mapped onto via $J_{i}^{T} (q)$ . By rearranging the right side of Eq. (12), we can obtain

(16)

F_{Q} (q) = J_{i}^{T} (q) (\sum_{i} F_{att, i} (p_{i} (q)) + \sum_{j} \sum_{i} F_{rep, i, j} (p_{i} (q))),

which implies that if there exists a configuration q such that

(17)

\sum_{i} F_{att, i} (p_{i} (q)) + \sum_{j} \sum_{i} F_{rep, i, j} (p_{i} (q)) = {[0, 0, 0]}^{T},

then

(18)

F_{Q} (q) = {[0, 0, 0, 0, 0, 0]}^{T} .

3.2 Detecting Local Minima in Partitioned Configuration Spaces

\begin{array}{l} q = {[x, y, θ_{1}, θ_{2}, θ_{3}, θ_{4}]}^{T} \to \bar{q} = {[x, y, 0, 0, 0, 0]}^{T}, \\ q_{goal} = {[x^{*}, y^{*}, θ_{1}^{*}, θ_{2}^{*}, θ_{3}^{*}, θ_{4}^{*}]}^{T} \to {\bar{q}}_{goal} = {[x^{*}, y^{*}, 0, 0, 0, 0]}^{T} . \end{array}

Then, we calculate the summation of the attractive force and the repulsive forces and investigate whether

(19)

F_{att, 1} (p_{1} (\bar{q})) + \sum_{j} F_{rep, 1, j} (p_{1} (\bar{q})) \to {[0, 0, 0]}^{T} .

\begin{array}{l} q = {[x, y, θ_{1}, θ_{2}, θ_{3}, θ_{4}]}^{T} \to \bar{q} = {[0, 0, θ_{1}, θ_{2}, θ_{3}, θ_{4}]}^{T}, \\ q_{goal} = {[x^{*}, y^{*}, θ_{1}^{*}, θ_{2}^{*}, θ_{3}^{*}, θ_{4}^{*}]}^{T} \to {\bar{q}}_{goal} = {[0, 0, θ_{1}^{*}, θ_{2}^{*}, θ_{3}^{*}, θ_{4}^{*}]}^{T} \end{array}

This projection corresponds to the translation of the obstacle positions in x-y plane, thus

o_{j} = {[x_{obs}, y_{obs}, z_{obs}]}^{T} \to {\bar{o}}_{j} = {[x_{obs} - x, y_{obs} - y, z_{obs}]}^{T} .

Then, we investigate whether

(20)

\sum_{i = 2}^{5} F_{att, i} (p_{i} (\bar{q})) + \sum_{j} \sum_{i = 2}^{5} F_{rep, i, j} (p_{i} (\bar{q})) \to {[0, 0, 0]}^{T},

which means that the elbow manipulator part itself eventually traps in the local minima even if the position of the mobile base part is fixed.

3.3 Subplanner Algorithm to Escape from Local Minima: Attractive Force Rotation and Restoration

3.3.1 Attractive force rotation

\begin{array}{l} \sum_{i} F_{att, i} (p_{i} (q)) : = F_{att}, \\ \sum_{j} \sum_{i} F_{rep, i, j} (p_{i} (q)) : = F_{rep}, \\ and F_{sum} = F_{att} + F_{rep} . \end{array}

At a local minimum, F_att is equal to F_rep; hence F_sum becomes [0, 0, 0]^T.

(21)

{F_{att} ∣}_{s + Δ s} \leftarrow [\begin{matrix} c_{θ_{R}} & - s_{θ_{R}} & 0 \\ s_{θ_{R}} & c_{θ_{R}} & 0 \\ 0 & 0 & 1 \end{matrix}] {F_{att} ∣}_{s},

(22)

{F_{att} ∣}_{s + Δ s} \leftarrow [\begin{matrix} c_{θ_{1}} & - s_{θ_{1}} & 0 \\ s_{θ_{1}} & c_{θ_{1}} & 0 \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} c_{θ_{R}} & 0 & s_{θ_{R}} \\ 0 & 1 & 0 \\ - s_{θ_{R}} & 0 & c_{θ_{R}} \end{matrix}] {F_{att} ∣}_{s},

until F_sum leads the control point p₂(q) through p₅(q) of the mobile manipulator to the point lying on the δ-boundary from the obstacle.

3.3.2 Attractive force restoration

(23)

{F_{att} ∣}_{s + Δ s} \leftarrow [\begin{matrix} c_{θ_{L}} & - s_{θ_{L}} & 0 \\ s_{θ_{L}} & c_{θ_{L}} & 0 \\ 0 & 0 & 1 \end{matrix}] {F_{att} ∣}_{s} .

Figure 6 illustrates the AFRE process in which the mobile base follows a circular trajectory lying on the circumference of a circle with a radius R.

The radius R determines the curvature of the circular trajectory and can be defined as a function of θ_L. From Eq. (8) and Eq. (10), we know that F_sum becomes a vector of which size is ||εζ||. AS shown in Figure 6(a), if Eq. (23) is executed $\frac{2 π}{θ_{L}}$ times while the mobile base is being moved by ||εζ||, then the circumference is completely traced. Therefore, we can calculate R by

(24)

‖ ɛ ζ ‖ \cdot \frac{2 π}{θ_{L}} = 2 π R \Rightarrow R = \frac{‖ ɛ ζ ‖}{θ_{L}} .

Similarly, when the local minima is caused by the elbow manipulator part, the AFRE is performed along the y-axis of a current local frame of the mobile manipulator

(25)

{F_{att} ∣}_{s + Δ s} \leftarrow [\begin{matrix} c_{θ_{1}} & - s_{θ_{1}} & 0 \\ s_{θ_{1}} & c_{θ_{1}} & 0 \\ 0 & 0 & 1 \end{matrix}] [\begin{matrix} c_{θ_{R}} & 0 & s_{θ_{R}} \\ 0 & 1 & 0 \\ - s_{θ_{R}} & 0 & c_{θ_{R}} \end{matrix}] {F_{att} ∣}_{s} .

If the shortest distance from any control points of the elbow manipulator part, p₂(q) through p₅(q), to the closest obstacle is greater than αδ, then a path planner is switched to the APBP.

Table 2 presents the AFRORE algorithm. The APBP for the mobile manipulator path planning with the subplanner AFRORE for escaping from local minima is summarized in Table 3.

4. Simulation Results

The capability of the proposed planner, the APBP with the AFRORE algorithm, was tested under various environmental scenarios. Considering the main role of the mobile manipulator as a co-working robot or a service robot, we created the environmental scenarios in which the mobile manipulator tries to reach a goal configuration to pick up a target object. We set stationary obstacles as well as moving obstacles which can be regarded as moving people, round tables, etc. For all scenarios, the initial and the goal configuration of the mobile manipulator were set to q_init = [5, 50, 0π, 0.5π, 0π, 0π]^T and q_goal = [93.597, 49.245, 0.157π, 0.304π, 0.185π, −0.430π]^T, respectively, which correspond to the end effector position ${[x, y, z]}_{init}^{T} = {[5, 50, 7]}^{T}$ and ${[x, y, z]}_{goal}^{T} = {[95, 50, 6]}^{T}$ . Stationary and moving obstacles are presented as black (black) and green (gray) circles, respectively.

Note that all presented simulation results were also uploaded as video files and can be found at https://www.dropbox.com/s/ea2xqvyqqkzbvew/MobileManiPlanning.html.

4.1 Scenario 1: Stationary On-Ground Obstacles Deployed like Stepping Stones

4.2 Scenario 2: Moving-to-Stationary On-Ground Obstacles with a Local Minimum

4.3 Scenario 3: Off-Ground Stationary Obstacle and Moving-to-Stationary On-Ground Obstacles with Local Minima

4.4 Scenario 4: Room Crowded by Moving On-Ground Obstacles with Possible Local Minima

5. Conclusion and Future Works

Abstract
1. Introduction
2. Artificial Potential Based Path Planning for a Mobile Manipulator
3. A Method to Escape from Local Minima
4. Simulation Results
5. Conclusion and Future Works

Fig 1.

Download