Potential Function. Homework #3. Part I: Star Algorithm

Transcription

1 Homework #3 Potential Function Part I: Star Algorithm I have implemented the Star Algorithm to compute the 2-dimensional configuration space Q starting from the workspace W and the robot shape R. The obstacles QO i in the configuration space are given by the following relationship: QO i = q Q R q WO i If we assume that the robot and the obstacles in the workspace are convex polygons, the Star Algorithm computes the obstacles in the workspace using the normal vectors of each side of the polygons. I have implemented a Matlab function norm = c_norm(tang, dir) that returns the normal vector for each side of a polygon. If the parameter dir is 1, then the direction of the normal vector is inside the convex shape (for the robot). If dir is -1, then the direction of the normal vector is outside (for the obstacles). In order to understand where is the inside area or the outside area of a convex polygon is, we just need to build the cross product between two adjacent sides. If the z-component of the product is positive, the inside direction is the direction given by the sides rotated by If the z-component of the product is negative, the inside direction is the direction given by the sides rotated by The algorithm sorts the sides of the robot and of one obstacle by increasing the angle of the normal vectors. Thereafter it builds the new convex polygon adding all the sides of the robot and of the obstacle in the calculated sequence. In order to define the position of the new obstacle in the configuration space, I define the vertex with the higher x- value as reference point on the robot as and the vertex with the lower x-value as reference point on the obstacle. Since the robot s reference point can touch the obstacle

2 reference point by a given robot s configuration q, the corresponding vertex of the obstacle in the configuration space must be at the position q. I have implemented a Matlab function [X, Y, num] = q_obstacle(robot_x, robot_y, robot_num, robot_n, obstacle_x, obstacle_y, obstacle_num, obstacle_n) that transforms an obstacle of the workspace in the correspondent obstacle in the configuration space. The parameter ending with _num is the number of vertices of the polygon and the parameter ending with _n is the set of the normal vectors to the polygon sides. If the moving robot and the obstacle in the workspace are two convex polygons M and P, we can also say that the corresponding obstacle in the configuration space is given by the Minkowski difference between the obstacle in the workspace and the robot: P M = p m p P, m M When the program starts, you can push the button Draw Obstacle to draw an obstacle and set with the left mouse button the vertices of the obstacles within the white area (robot workspace W). To end the process you can click with the right mouse button somewhere. To draw the robot you can push the button Draw Robot and set with the left mouse button the vertices of the robot within the white area. To end the process, click somewhere with the right mouse button. A black point will be placed on the reference vertex of the robot. To compute the configuration space with the Star Algorithm you can push now the button Compute Q. The obstacles QO i in the configuration space are drawn in yellow. To set the end configuration, push the button End Configuration, and set with the left mouse button the point within the remaining white area (configuration space Q). To calculate the path you can push the button Calculate Path.

3 Part II: Potential function In order to compute the path within the configuration space, I have implemented an algorithm based on potential fields. We define a potential field as the sum of an attractive potential U att and a repulsive potential U rep : U q = U att q + U rep q The attractive potential and its gradient are defined as function of the actual configuration q and the goal configuration q goal : U att q = 1 2 ζ d2 (q, q goal ), d(q, q goal ) d goal d goal ζ d q, q goal 1 2 ζ d 2 goal, d(q, q goal ) > d goal U att q = ζ (q q goal ), d goal ζ q q goal d q, q goal d(q, q goal ) d goal, d(q, q goal ) > d goal

4 This definition is very useful because when the robot is near the goal the velocity decreases linearly (quadratic potential). However, when it is far from the goal (d(q, q goal ) > d goal ) the velocity is constant (conic potential). In the movies you can see very well this behaviour. However, if the robot is very close to the goal its velocity goes to zero and it never reaches the goal. Therefore, I have defined a minimum velocity in my numerical implementation. The repulsive potential and its gradient are defined as a function of the distance D q between the actual configuration and the closest obstacle: U rep q = 1 2 η 1 D q 1 2 Q, D q Q 0, D q > Q U rep q = η 1 Q 1 D q 1 D 2 q D q, D q Q 0, D q > Q An issue of this definition is that if the distance D q is a very small number, the value of U rep q becomes huge. To avoid this problem in my numerical implementation, I have defined a minimum threshold for the distance. To represent the distance, I have used the saturated distance function as in the Homework #2: ρ R q, θ = min d q, q + λ cos θ, sin θ T s. t. q + λ cos θ, sin θ T QO i, if ρ R q, θ < R λ 0, i, otherwise Obviously, infinity does not exist in real computers and therefore I have used a high number. The name of this parameter is R_inf. The name of the parameter, which defines the saturation radius is R, and it corresponds to the parameter Q in the repulsive potential function. I have used the implemented Matlab function rho = sensor(workspace, X, step, x_max, y_max, j_max, i_max, R, R_inf).

5 The parameter step = 1/50 is the dimension in the workspace of the basic step that the robot does during its motion. When the algorithm is running, the configuration point on the robot starts moving to the goal configuration q goal along the direction given by the negated gradient of the potential field. The length of the step is given multiplying the negated gradient by a factor α: q i+1 = q i α i U q i In order to understand whether the algorithm has arrived to a local minimum we define a numerical threshold ϵ. If the magnitude of the gradient is less than the threshold, a minimum is reached: U q i < ε If the actual configuration q i is far from the goal configuration, the algorithm has failed. I notice that the definition of the parameter d goal (d_star), ζ (zeta), η (eta), α (alpha), and ϵ (epsilon) is very important and determines the behaviour of the algorithm. I have set the distance from the goal configuration d goal = 150*step in order to slow down the motion starting 150 steps far from the goal (in the movies this behaviour is easy to recognize). For the attractive potential I have used ζ = 1*step to have a good velocity far from the obstacle. To restrict the influence of the obstacle and to avoid too large repulsive potentials I have used η = step/50. Finally, I have used α = 1 and ϵ = step/20. In order to register a movie of the path planning, there is a check box Movie that has to be checked before pushing the button Calculate Path. Finally, in order to get a 3D graph of the potential function over the whole configuration space Q, you can use the check box 3D Graph (this option could require several minutes).

6