A.3.1
Continuous Obstacle Checking Problem
The cost function that identified if the robot would collide with any of the obstacles worked most of the time, but on occasion would hit obstacles with the back part of the robot while coming around turns. This stemmed from a bug in the cost function that would calculate the cost of the robot’s footprint at a number of arbitrary positions along a given trajectory, but only mark the trajectory as invalid if the final position was in collision. The positions that resulted in a collision were the intermediate positions between the original position and the final predicted position.
Figure A.2: End Path Scenario - Each arrowhead marks the robot position as it moves from left to right. As the robot approaches, it slows down until it reaches just outside the goal radius. Then it moves laterally without being able to enter the circle until it happens to cross the radius due to noise.
A.3.2
The End Path Problem
Using the forward point method, the alignment methods work reasonably well in the general case. However, when the robot nears the goal, these cost functions become trickier. Once the robot is within df p of the goal, the forward point is beyond the goal, meaning higher
costs for points closer to the goal.
Let us consider the GoalAlignment cost specifically. In order to get around the problem, the ROS local planner sets the weight of the GoalAlignment cost to zero when the point x, y is within df p of the goal. This eliminates the problem within that radius, but causes problems
just outside of it, stemming from the difference between the robot’s location at planning time (x, y), and the predicted location (xτ, yτ). The weight is only set to zero when the starting
location is within the radius, not when the predicted location is within the radius. Hence, paths that would move the robot into the radius from outside of it are more costly.
The resulting behavior ranges from annoying to failure-ridden. The worst case scenario is that the robot is not able to reach its goal. When the robot is just outside the radius, any path that enters within the radius is going to be scored higher than those that remain outside the radius. This leads to the behavior seen in Figure A.2. The robot moves laterally around the goal, only moving closer to the goal if the robot happens to enter the radius.
The annoying feature of this is that the robot slows down as it approaches the radius. This can also be seen in Figure A.2. Trajectories at full speed would move the robot’s front point into the radius, and thus are scored higher, so that the slower trajectories cost less.
Figure A.3: Angular Problem - The path planner should score a plan that turns toward the goal more preferably, but because of the discretization of the grid, it does not. The initial forward point has the same PathAlignment score as the predicted point, with both falling in the same horizontal band of equal costs.
The Angular Problem
Consider a scenario where the robot must plan a path to a point initially to its right. Based on the alignment metrics used, one would expect the robot to turn right such that the front of the robot is aligned with the shortest path and the goal location. Instead, the robot does some combination of turning and translating. Even with configurations with very high weights on the PathAlignment will not recognize turning toward the goal as being less costly. The problem lies in the discretization of the search space for computational efficiency. If the robot initially starts out with no velocity, then due to the dynamic constraints, the DWA trajectory generation will only consider very small velocities within the robot’s acceleration limits. As a result, the robot’s predicted location at the end of the simulation period τ will be at a point roughly parallel to the robot’s path (when starting 90 deg away). This point can have the same score as the initial position (without any rotation) in the discretized grid. As a result, the robot sees no initial benefit to turning, despite there being actual progress toward reducing the PathAlignment cost.
The Driving Straight Problem
Sometimes even the simplest scenarios present interesting edge cases. Consider a holonomic robot aiming toward a goal directly in front of it. One would expect the robot to move straight forward toward its goal, i.e. v˙x is the maximum allowed speed and ˙vy and ˙vθ are
zero. It seems simple enough. However, the actual behavior includes using the holonomic base to move with maximum values of ˙vx and ˙vy with additional rotational velocity ˙vθ to
ensure the robot stays on path.
It seems bizarre that the robot would chose this complicated maneuver when a simple one would suffice. However, the optimization of costs does not optimize for simplicity. Instead, it moves toward the goal as quickly as it can, due to the GoalDistance cost. In this particular instance, the cause of the problem is the way in which the space of possible trajectories is explored. To encapsulate the robot’s dynamics, there are parameters that specify the minimum and maximum values of ˙vx, ˙vy and ˙vθ and iterating over all combinations of those
ranges with some resolution. However, the overall velocity of the robot when these three velocities are combined can be greater than the magnitude of any individual component velocity. Thus the quickest way to get to the goal (and minimize GoalDistance cost) is to move at a slight angle, translating in both the x and y directions.
This is a valid trajectory, and does in fact move the robot toward the goal in less time than the straight forward approach. However, what it gains in terms of speed efficiency, it loses in legibility. The robot moving at such an angle looks like it may start moving in a different direction. This modifies the robot’s floor pattern and its shape, since driving at an angle change the profile of the robot. The effect on the robot’s tempo/speed is minimal.