Path approximation for planar motion of a ground vehicle
The generation of a planar trajectory for use by a vehicle in guiding its position is described. The trajectory is generated by identifying points representing the trajectory, fairing the identified points to generate a set of faired points, and computing a set of polynomial segments from the faired points. The segments form a curve representing the trajectory, where the curve is C 2 -smooth, and the curve does not exceed the steering capability of the vehicle. The segments may comprise cubic B-spline segments. If the identified points are not substantially equidistant, then an auxiliary curve approximating the trajectory is generated, and a set of substantially equidistant points on the auxiliary curve is selected as the identified points.
1. A computer-implemented method for generating a planar trajectory for use by a vehicle in guiding its position, the method comprising:
(a) determining whether a plurality of points representing the trajectory are at least substantially equidistant;
(b) if the plurality of points are determined to not be at least substantially equidistant:
generating an auxiliary curve approximating the trajectory;
identifying a plurality of substantially equidistant points from the auxiliary curve; and
fairing the identified plurality of substantially equidistant points to generate a plurality of faired points;
(c) if the plurality of points are determined to be at least substantially equidistant, fairing the plurality of points to generate the plurality of faired points; and
(d) computing by a processor, a plurality of polynomial segments from the plurality of faired points, wherein the polynomial segments form a curve representing the trajectory, the curve is C 2 -smooth, and the curve does not exceed the steering capability of the vehicle.
2. The method of claim 1 , wherein the polynomial segments comprise cubic B-spline segments.
3. The method of claim 1 , wherein distances between neighboring points of the plurality of substantially equidistant points are within 5% of each other.
4. The method of claim 3 , wherein the polynomial segments comprise cubic B-spline segments.
5. The method of claim 1 , further comprising:
identifying a segment associated with a current position;
computing a trajectory deviation value based upon a distance of the current position from the segment; and
providing a course correction signal for the vehicle based upon the trajectory deviation value.
6. The method of claim 5 , wherein computing the trajectory deviation value comprises:
identifying coordinates of the current position;
computing the trajectory deviation value associated with the distance of the current position from the associated segment based upon a point of intersection between the normals of the endpoints of the segment and a line projected between the current position and the point of intersection.
7. A non-transitory computer-readable storage medium comprising computer-executable instructions for representing a planar trajectory for use by a vehicle in guiding its position, the instructions for:
(a) determining whether a plurality of points representing the trajectory are at least substantially equidistant;
(b) if the plurality of points are determined to not be at least substantially equidistant:
generating an auxiliary curve approximating the trajectory;
identifying a plurality of substantially equidistant points from the auxiliary curve; and
fairing the identified plurality of substantially equidistant points to generate a plurality of faired points;
(c) if the plurality of points are determined to be at least substantially equidistant, fairing the plurality of points to generate the plurality of faired points; and
(d) computing a plurality of polynomial segments from the plurality of faired points, wherein the polynomial segments form a curve representing the trajectory, the curve is C 2 -smooth, and the curve does not exceed the steering capability of the vehicle.
8. The computer-readable storage medium of claim 7 , wherein the polynomial segments comprise cubic B-spline segments.
9. The computer-readable storage medium of claim 7 , wherein distances between neighboring points of the plurality of substantially equidistant points are within 5% of each other.
10. The computer-readable storage medium of claim 9 , wherein the polynomial segments comprise cubic B-spline segments.
11. The computer-readable storage medium of claim 7 , further comprising instructions for:
identifying a segment associated with a current position;
computing a trajectory deviation value based upon a distance of the current position from the segment; and
providing a course correction signal for the vehicle based upon the trajectory deviation value.
12. The computer-readable storage medium of claim 11 , wherein computing the trajectory deviation value comprises:
identifying coordinates of the current position;
computing the trajectory deviation value associated with the distance of the current position from the associated segment based upon a point of intersection between the normals of the endpoints of the segment and a line projected between the current position and the point of intersection.
13. A navigation apparatus for use with a vehicle, the apparatus comprising:
the computer-readable medium of claim 7 ;
processing logic for executing instructions stored on the computer-readable medium; and
a directional control apparatus for controlling the direction of vehicle movement based upon the curve formed by the computed segments.
14. A vehicle for following a trajectory, the vehicle comprising:
the navigation apparatus of claim 13 ;
a navigation signal receiver coupled to the processing logic; and
an inertial sensor coupled to the processing logic.