3D Collision-Free Motion Based on Collision Index
KAO-SHING HWANG and MING-YI JU Dept. of Electrical Engineering, National Chung Cheng University, Chiayi 160, Taiwan, R.O.C. Email:
[email protected]
Abstract A collision-free
motion
planning
method
for
mobile
robots moving
in
three-dimensional workspace is proposed in this article. To simplify the mathematical representation and reduce the computation complexity for collision detection, objects in the workspace are modeled as ellipsoids. By means of applying a series of coordinate and scaling transformations between the robot and the obstacles in the workspace, intersection check is reduced to test whether the point representing the robot falls outside or inside the transformed ellipsoids representing the obstacles. Therefore, the requirement of the computation time for collision detection is reduced drastically in comparison with the computational geometry method, which computes a distance function of the robot segments and the obstacles. As a measurement of the possible occurrence of collision, the collision index, which is defined by projecting conceptually an ellipsoid onto a three-dimensional Gaussian distribution contour, plays a significant role in planning the collision-free path. The method based on reinforcement learning search using the defined collision index for collision-free motion is proposed. A simulation example is given in this article to demonstrate the efficiency of the proposed method. The result shows that the mobile robot can pass through the blocking obstacles and reach the desired final position successfully after several trials. Key words: collision detection, collision index, reinforcement learning.
1
1.
Introduction Motion planning of robots in a time-varying environment is an on-going research
problem. The research efforts are usually geared toward determining efficient methods for collision avoidance among obstacles in a shared environment. Two major areas of these works are navigation and guidance. In general, navigation is a problem on deciding a global path or a route for a robot to follow. Typically, navigation or path planning in a known environment involves identifying a collision-free path through a population of obstacles subject to some constraints, such as the shortest distance. Guidance, sometimes referred as local navigation, is the problem of maneuvering around local (dynamic) obstacles while a robot is moving along a global path. Therefore, the classical collision avoidance algorithm can be stated as follows [1]: Given an arbitrary moving object with a certain degree of freedom, such as a mobile robot, the algorithm should find a continuous collision-free path along which the object can move from an initial configuration to a desired configuration. Two related approaches to efficient collision detection are hierarchical models and bounding representations. A hierarchical model describes an object at various levels of detail.
The collision detection algorithm uses different levels of detail to reduce the
number of components that are examined.
Similarly, bounding representations
approximately model an object with simple primitives, such as ellipses. Some efficient algorithms have been proposed to determine if collision has occurred between the bounding representations, which are the components of the original model examined [2]. The above problem rises to a variety of methods. A general solution using the concept of free-space was presented in [3, 4]. In that approach, each obstacle in an
2
environment is "grown" by the dimensions of a robot. The robot is effectively reduced to a dimensionless point. The algorithm then reduces the complexity of the problems to find a safe path along which the point robot moves among obstacles. Another simple and efficient algorithm was proposed to plan collision-free motions for general manipulators with a known set of stationary obstacles by using the configuration space [5]. One method which used the concept of "freeways" to represent an environment was proposed in [6]. This method defines a spatial space between obstacles as freeways with a centerline (called a "spline") that is used to navigate along the freeways. Alternatively, another approach which employed a proper free-space representation was presented using polytopes [7]. After a global graph search is carried out, a family of feasible trajectories is generated, and a constrained optimization problem is formulated to obtain the final near-optimal trajectory. The other problem in developing a collision avoidance strategy is representing the complicated shapes of a mobile robot and obstacles by means of a generic analytic function to reduce the complexity of the detection algorithm. Geometric models that are often used to represent three-dimensional shapes are based on constructive solid geometry (CSG). The representation of the CSG method is compact, but it requires some computation to identify which entity is in a given location. The boundary model is useful for representing topological information; however, some additional work is still required to get geometrical neighborhoods. Gewali and his colleagues applied the polynomial time algorithm to find the shortest paths in a 3D space among a set of polyhedral obstacles which resemble vertical buildings by means of a fixed number of distinct heights [8]. They also proposed a shortest-path algorithm which has less complexity when each obstacle has an equal height. A strategy for planar path planning has been proposed [9]. This
3
strategy models the objects, including the mobile robot and obstacles, as a set of ellipses and approximates these ellipses by means of polygons. A collision-free trajectory can, thus, be derived by deforming the ellipse of the robot into a circle and processing the coupling transformations on the configuration of the obstacles within the motion. A similar method that adopted a quadtree-type hierarchical representation was proposed in [10]. The method assumes that the objects in the workspace do not undergo rotation. Therefore, the trajectory becomes a polyhedron in the geometrical dimensions. By presenting the time dimension by means of the other dimension, the object representation is built by repeatedly subdividing the time-space dimensions into eight subspaces, called cell, until each cell satisfies some conditions. Based on the cells and the control points, a collision-free path can be planned. Although this approach suffers from a very wide search space, it works well in a sparsely occupied space. Nevertheless, these methods, which approximate the shapes of the objects by means of primitives or polyhedral, are too complex and time-consuming in solving the collision avoidance problem. Artificial potential models for realizing obstacles avoidance have been studied [11, 12]. These schemes apply the addition of attractive and repulsive potentials to providing obstacle avoidance capability. An attractive potential well is generally a bowl shaped energy well which drives the robot to its center if the environment is unobstructed. If the workspace is obstructed, some repulsive potential energy hills are added to the attractive potential well at the location. Volpe and his colleagues adopted superquadric modeling to describe the repulsive potential field by representing an obstacle in terms of superquadrics [12,13]. This method assumes that there is only one obstacle in a workspace, so it is difficult and impractical to implement this scheme in the case of multiple obstacles. Quinlan proposed a method for spatial computation
4
between non-convex obstacles [2]. A hierarchical bounding representation is built, based on spheres, that approximates the object. A search routine examines the hierarchical bounding representation of each object and determines pairs of components to compare with a convex distance algorithm. An efficient approach to directing a mobile robot, such as an under-water robot, moving in a three-dimension working space is described in this paper. The method models the robot and obstacles as several ellipsoids [14,15]. It is assumed that the submarine-like has three degrees of freedom (two rotations and one translation) for changing its pose in the workspace. A learning mechanism for collision-free trajectories based on the reinforcement learning using the collision index, which is analogous to the potential in the potential modeling method, is proposed. The rest of the article is organized as follows: In Section 2, we describe the basic idea of how to simplify the collision detection problem between the objects composed of various geometric shapes into the one where only the spatial relation between a point and several ellipsoids needs to be evaluated. Meanwhile, unlike the bounding modeling schemes, the spatial distance is not the index used to derive a collision avoidance path. Instead, the collision index based on possible collision occurrence is defined.
In
Section
3,
we
describe
in
detail
the
proposed
intelligent
collision-avoidance planning scheme with the reinforcement learning mechanism. Results of the simulation are given in Section 4. Finally, conclusions are drawn, and directions for the related future work are discussed in the last section.
2. Collision Detection Algorithm Efficient collision detection is crucial to collision-free path planning. To reduce the requirement of computation time for intersection check, complex objects are
5
usually modeled as simpler primitives to simplify the mathematical or geometrical representations. An Appropriate primitive should reflect a good balance between the efficiency of primitive-primitive intersection detections and the number of primitives required to adequately represent the world model. In the proposed approach, ellipsoids are selected to represent objects due to the capability of representing a mobile robot’s motion in the direction of its longest axis. Besides, the main advantage of ellipsoid model is that it is very simple both in mathematical and geometrical representations; therefore the computation complexity and computation time can be reduced. Ellipsoids are also used as primitives by other researchers [16, 17]. However, detecting intersection between two ellipsoids is not an easy task. A rapid approach has been proposed in [16] to compute the minimum distance between two separate ellipsoids. Unfortunately, this approach fails in intersecting cases. To solve this problem, a novel method, which converts the intersection check of ellipsoids into an optimization problem, is provided in [17]. But it is time-consuming and limited in real-time applications. To make use of the advantages and overcome the subsistent problems of ellipsoidal representation, an efficient collision detection algorithm is developed in this work for collision-free motion planning. An ellipsoid can be expressed in the parametric form, ⎡ x ⎤ ⎡ rx cos ϕ cosψ ⎤ ⎢ y ⎥ ⎢ r cos ϕ sinψ ⎥ −π /2 ≤ϕ < π /2 ⎢ ⎥=⎢ y ⎥ , where ; − π ≤ψ < π ⎢ z ⎥ ⎢ rz sin ϕ ⎥ ⎢ ⎥ ⎢ ⎥ 1 ⎣1⎦ ⎣ ⎦
(1)
or in the standard quadric expression, 2
2 2 x + y + z = 1. rx2 ry2 rz2
(2)
However, after some transformations, such as rotations or translations, the quadric
6
equation becomes somewhat complicated: E ( x, y , z ) = Ax 2 + By 2 + Cz 2 + Dxy + Eyz + Fzx + Gx + Hy + Iz + J ,
(3)
where the parameters, A~J, depend on the rotational angles and the translated distance.
2.1 Transformations between Ellipsoids
The relative position between a point and an ellipsoid can be easily verified based on Eq. (3). If E ( x, y , z ) > 1 , the point (x, y, z) is outside the ellipsoid; otherwise, it is located inside the ellipsoid. Therefore, the result of Eq. (3) obtained by providing the coordinates of a point as the arguments can be regarded as one of collision occurrence indexes. Therefore, a simple detection algorithm can be derived easily if the mobile robot and the obstacles in the working space can be represented geometrically in the form of a point and several ellipsoids. Unfortunately, the problem encountered here is not that of a point colliding with a set of ellipsoidal obstacles, but one where the robot is also modeled as an ellipsoid. The mathematical solution for the intersection of two ellipsoids is not practical since the computational complexity increases exponentially. Therefore, in order to utilize the simplicity of detecting the geometric relationship between a point and an ellipsoid, a series of transformations must be applied to the robot and obstacles in the workspace. The approach first changes the reference coordinate system from the world coordinate system to the mobile robot’s coordinate system. Then, a scaling operation deforms the moving ellipsoid, which represents the robot, into a unit sphere or a large point while the obstacles are transformed into other ellipsoidal shapes. The problem is, thus, simplified as one between a point and ellipsoids. The mathematical procedure is illustrated in the following.
7
The ellipsoid which represents the mobile robot can be expressed in polar vector form. The vector can be also regarded as a sphere scaled along the x-y-z axes:
x m0
⎡rxm cos ϕ cosψ ⎤ ⎢ r cos ϕ sinψ ⎥ ⎥, = S m x 0 = ⎢ ym ⎢ rzm sin ϕ ⎥ ⎢ ⎥ 1 ⎣ ⎦
where x 0
and S m
⎡cos ϕ cosψ ⎤ ⎢ cos ϕ sinψ ⎥ ⎥ is the unit spherical vector, x 0 = ⎢ ⎢ sin ϕ ⎥ ⎢ ⎥ 1 ⎣ ⎦
⎡ rxm ⎢0 is a scaling transformation, S m = ⎢ ⎢0 ⎢ ⎣0
0 rym 0 0
0 0 rzm 0
−π /2 ≤ϕ
