Title Author Keyword ::: Volume ::: Vol. 18Vol. 17Vol. 16Vol. 15Vol. 14Vol. 13Vol. 12Vol. 11Vol. 10Vol. 9Vol. 8Vol. 7Vol. 6Vol. 5Vol. 4Vol. 3Vol. 2Vol. 1 ::: Issue ::: No. 4No. 3No. 2No. 1

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

Han Ul Yoon, and Dong-Wook Lee

1Robotics R&D Group, Korea Institute of Industrial Technology, Ansan, Korea, 2Department of Robotics and Virtual Engineering, Korea University of Science and Technology, Daejon, Korea
Correspondence to: 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 non-commercial use, distribution, and reproduction in any medium, provided the original work is properly cited.
Abstract

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

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 [68], 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 [1921] 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

### 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

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

where [x, y, θ1]T corresponds to the position and orientation of the mobile base, and [θ2, θ3, θ4]T represents the rotations of manipulator joint. Accordingly, the configuration space of the mobile manipulator is

$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 t0 and a final time tf, we want to find a path γ that is a continuous mapping

$γ:[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

$γ(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 pi(q) that maps a given configuration q in a configuration space onto a three dimensional position vector pi in a workspace

$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 p1(q) through p5(q) with the Denavit-Hartenberg (DH) parameters of the mobile manipulator (see

$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 ci and si 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 pi(q) to a goal control point pi(qgoal) 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 pi(q) is close to pi(qgoal) or not [1, 2].

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

$‖pi(q)-pi(qgoal)‖ >ɛ,$$‖pi(q)-pi(qgoal)‖ ≤ɛ,$

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

$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 Fatt,i can be yielded by taking a partial derivative with respect to pi(q) and negation

$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 pi(q) approaches to pi(qgoal) by switching Uatt,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.

2.3.3 Repulsive potential in workspace

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

$‖pi(q)-oj‖ <δ,$$‖pi(q)-oj‖ ≥δ.$

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

$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 Frep,i,j is equal to

$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.

2.3.4 Building a path in configuration space by using APBP

To map two artificial forces, Fatt,i and Frep,i,j, in a workspace onto a force in a configuration space , we define Jacobian Ji(q) ∈ ℝ3×6 for ith control points pi(q)

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

for example

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

Now, Fatt,i and Frep,i,j can be mapped onto via $JiT(q)$

$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

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

To build a path γ, the next configuration is assigned to the path for a time stamp ss

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

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

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

By repeating a procedure Eq. (12) through Eq. (15) until q is equal to qgoal, we can build a path γ(s) for s ∈ [t0, tf ] 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

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 Fatt,i and Frep,i,j can be mapped onto via $JiT(q)$. By rearranging the right side of Eq. (12), we can obtain

$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

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

then

$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 pi(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 qm = [x, y]T ∈ ℝ2 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 qgoal 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

$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 qgoal 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

$∑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 Fatt,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 Fatt,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, Fatt is equal to Frep; hence Fsum 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 Fatt 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 sss

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

until Fsum guides the control point p1(q) of the mobile manipulator to the point lying on a δ-boundary from an obstacle at where Frep 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 Fatt by θR along the y-axis of a current local frame of the mobile manipulator

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

until Fsum leads the control point p2(q) through p5(q) of the mobile manipulator to the point lying on the δ-boundary from the obstacle.

Under the AFRO process, the generated Fsum 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 Fsum 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 p1(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 Fatt by keep rotating it by θL (= −θR), along the z-axis of a current local frame of the mobile manipulator

$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 Fsum 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

$‖ɛζ‖·2πθL=2πR⇒R=‖ɛζ‖θL.$

During the AFRE process, if a distance between p1(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

$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, p2(q) through p5(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

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 qinit = [5, 50, 0π, 0.5π, 0π, 0π]T and qgoal = [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

### 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

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

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
Fig. 1.

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

Fig. 2.

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

Fig. 3.

The example of the repulsive potential in ℝ2: (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, Fsum = 0, (b) the subplanner keeps rotating Fatt clockwise by θR, (c) Fatt 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 Fatt 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

### Table 1

APBP for a mobile manipulator path planning

 Given qinit, qgoal, and t0 • Initialize: q ← qinit and γ(t0) ← qinit WHILEq is not qgoal • Calculate from Fatt,i(pi(q)) and Frep,i,j(pi(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′, pi(q) ← pi(q′) Eq. (15) END • Finalize: tf ← s and γ(tf ) ← qgoal RETURNγ

### Table 2

Subplanner AFRORE for escaping from local minima

 Function: AFRORE(localmin, type) / * Atractive Force Rotation * / WHILElocalmin SWITCHtype case • Fatt|s+Δ ← Rotz(θR) · Fatt|s Eq. (21) • Do Eq. (12), Eq. (13), Eq. (14), and Eq. (15) • checkpoint ← p1(q) break case • Fatt|s+Δ ← Rotz(θ1) · Rotx(θR) · Fatt|s Eq. (22) • Do Eq. (12), Eq. (13), Eq. (14), and Eq. (15) • checkpoint ← {p2(q), p3(q), p4(q), p5(q)} break END IF min ||checkpoint – oj||≥ δ localmin ← false break ELSE repeat WHILE END / * Atractive Force Restoration * / WHILE min ||checkpoint – oj||≤ αδ SWITCHtype case • Fatt|s+Δ ← Rotz(θL) · Fatt|s Eq. (23) • Do Eq. (12), Eq. (13), Eq. (14), and Eq. (15) • checkpoint ← p1(q) break case • Fatt|s+Δ ← Rotz(θ1) · Rotx(θL) · Fatt|s Eq. (25) • Do Eq. (12), Eq. (13), Eq. (14), and Eq. (15) • checkpoint ← {p2(q), p3(q), p4(q), p5(q)} break END IF min ||checkpoint – oj||≥ αδ break ELSE repeat WHILE END

### Table 3

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

 Given qinit, qgoal, localmin and t0 • Initialize: q ← qinit, γ(t0) ← qinit, localmin ← false / * Atractive Potential Based Planner * / WHILEq is not qgoal • Calculate from Fatt,i(pi(q)) and Frep,i,j(pi(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′, pi(q) ← pi(q′) Eq. (15) END • Finalize: tf ← s and γ(tf ) ← qgoal RETURNγ

References
1. Latombe, JC (2003). Robot Motion Planning. Boston, MA: Kluwer Academic Publishers
2. 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
3. Spong, MW, Hutchinson, S, and Vidyasagar, M (2006). Robot Modeling and Control. Hoboken, NJ: John Wiley and Sons
4. 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
5. 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
6. 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
7. 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
8. 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
9. 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
10. 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
11. 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
12. 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
13. 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
14. 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
15. Lavalle, S (2006). Planning Algorithms. Cambridge, UK: Cambridge University Press, pp. 154-205
16. 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
17. 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.
18. 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
19. 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
20. 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
21. 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

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

December 2018, 18 (4)