System for waypoint selection and method of using

A waypoint selection method comprising:

• obtaining information related to a current pose of a vehicle including a combination of location and orientation relative to a reference axis;
• obtaining information related to a destination pose of the vehicle including a combination of location and orientation relative to the reference axis;
• obtaining a plurality of waypoints between the orientation of the vehicle at the location indicated by the current pose and the orientation of the vehicle at the location indicated by the destination pose;
• obtaining obstacle information;
• determining a first waypoint cost for a first waypoint, corresponding to an amount of movement for the vehicle to move from the orientation of the vehicle at the location indicated by the current pose to the orientation of the vehicle at the first waypoint, of the plurality of waypoints;
• selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition;
• generating a path from the current pose to the pose at the first waypoint in response to selecting the first waypoint;
• outputting movement instructions to the vehicle for moving along the generated path:
• wherein determining the first waypoint cost comprising determining the first waypoint cost based on:
• a path length ratio between (1) a direct path between the current pose and a pose at the first waypoint and (2) a calculated path between the current pose and the pose at the first waypoint;
• a number of direction changes along the calculated path between the current pose and the pose at the first waypoint;
• a heading angle from the current pose to the pose at the first waypoint;
• an orientation difference between the current pose and the pose at the first waypoint, with the waypoint cost increasing as it approaches ninety degrees and decreasing as it approaches zero degrees or one hundred eighty degrees;
• a proximity of a closest obstacle to the calculated path;
• and
• a Euclidean distance between the current pose and the pose at the first waypoint.

A vehicle control system comprising:

• a non-transitory computer readable medium configured to store instructions thereon;
• and
• a processor connected to the non-transitory computer readable medium, wherein the processor is configured to execute the instructions for:
• obtaining information related to a current pose of a vehicle including a combination of location and orientation relative to a reference axis;
• obtaining information related to a destination pose of the vehicle including a combination of location and orientation relative to the reference axis;
• obtaining obstacle information;
• obtaining information related to surroundings of the vehicle;
• determining a plurality of waypoints between the orientation of the vehicle at the location indicated by the current pose and the orientation of the vehicle at the location indicated by the destination pose;
• determining a first waypoint cost for a first waypoint, corresponding to an amount of movement for the vehicle to move from the orientation of the vehicle at the location indicated by the current pose to the orientation of the vehicle at the first waypoint, of the plurality of waypoints;
• selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition;
• generating a path between the current pose and a pose at the first waypoint in response to selecting the first waypoint;
• and
• outputting the selected first waypoint and the generated path to a controller in response to selecting of the first waypoint,
• wherein the processor is further configured to execute the instructions for determining the first waypoint cost based on:
• a path length ratio between (1) a direct path between the current pose and a pose at the first waypoint and (2) a calculated path between the current pose and the pose at the first waypoint;
• a number of direction changes along the calculated path between the current pose and the pose at the first waypoint;
• a heading angle from the current pose to the pose at the first waypoint;
• an orientation difference between the current pose and the pose at the first waypoint, with the waypoint cost increasing as it approaches ninety degrees and decreasing as it approaches zero degrees or one hundred eighty degrees;
• a proximity of a closest obstacle to the calculated path;
• and
• a Euclidean distance between the current pose and the pose at the first waypoint,
• wherein the vehicle control system further comprises the controller, the controller is configured to:
• generate instructions for movements of the vehicle from the current pose to the pose at the first waypoint;
• and
• transmit the instructions for movements to the vehicle.

A controller comprising:

• a non-transitory computer readable medium configured to store instructions thereon;
• and
• a processor connected to the non-transitory computer readable medium, wherein the processor is configured to execute the instructions for:
• obtaining information related to a current pose of a vehicle including a combination of location and orientation relative to a reference axis;
• obtaining information related to a destination pose of the vehicle including a combination of location and orientation relative to the reference axis;
• obtaining obstacle information;
• obtaining information related to surroundings of the vehicle;
• determining a plurality of waypoints between the orientation of the vehicle at the location indicated by the current pose and the orientation of the vehicle at the location indicated by the destination pose;
• determining a first waypoint cost for a first waypoint, corresponding to an amount of movement for the vehicle to move from the orientation of the vehicle at the location indicated by the current pose to the orientation of the vehicle at the first waypoint, of the plurality of waypoints;
• selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition;
• generating a path between the current pose and a pose at the first waypoint in response to selecting the first waypoint;
• outputting movement instructions to the vehicle for moving along the generated path in response to selecting of the first waypoint,
• wherein the processor is further configured to execute the instructions for determining the first waypoint cost based on:
• a path length ratio between (1) a direct path between the current pose and a pose at the first waypoint and (2) a calculated path between the current pose and the pose at the first waypoint;
• a number of direction changes along the calculated path between the current pose and the pose at the first waypoint;
• a heading angle from the current pose to the pose at the first waypoint;
• an orientation difference between the current pose and the pose at the first waypoint, with the waypoint cost increasing as it approaches ninety degrees and decreasing as it approaches zero degrees or one hundred eighty degrees;
• a proximity of a closest obstacle to the calculated path;
• and
• a Euclidean distance between the current pose and the pose at the first waypoint.