System for waypoint selection and method of use - Patents.com

Abstract

The waypoint selection system includes a non-transitory computer-readable medium configured to store instructions and a processor coupled to the non-transitory computer-readable medium. The processor is configured to execute instructions for obtaining information regarding a current pose of the vehicle, obtaining a plurality of waypoints between the current pose and a destination pose, and receiving obstacle information. The processor is configured to execute instructions for determining a first waypoint cost for a first waypoint of the plurality of waypoints. The processor is configured to execute instructions for selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition. The processor is configured to execute instructions for outputting the selected first waypoint and all of the plurality of waypoints between the first waypoint and the current pose to the controller in response to the selection of the first waypoint.

Description

[Technical Field]

[0001] SUMMARY The present disclosure relates to a system for waypoint selection and a method of using the system. [Background technology]

[0002] Determining a route for a vehicle robot to travel from one location to another involves plotting waypoints between an initial location and a destination location. Waypoints are intermediate locations that the robot or vehicle is expected to pass through while traveling from the initial location to the destination location.

[0003] Planning a route involves the use of both global and local planning aspects. The global planning aspect involves determining a destination location and identifying waypoints between the initial location and the destination local. The local planning aspect involves planning a route from the initial location to intermediate waypoints determined during global planning. The use of global and local planning aspects allows the robot or vehicle to move from the initial location to the destination location autonomously or with minimal user control. Summary of the Invention

[0004] According to a first exemplary aspect of the present disclosure, a waypoint selection system is provided, comprising: a non-transitory computer-readable medium configured to store instructions; and a processor coupled to the non-transitory computer-readable medium, wherein the processor is configured to execute the instructions to acquire information regarding a current pose of a vehicle; acquire a plurality of waypoints between the current pose of the vehicle and a destination pose; acquire obstacle information; determine a first waypoint cost for a first waypoint of the plurality of waypoints; select the first waypoint in response to the first waypoint cost satisfying a predetermined condition; and output the selected first waypoint and all of the plurality of waypoints between the first waypoint and the current pose to a controller in response to the selection of the first waypoint.

[0005] According to a second exemplary aspect of the present disclosure, there is provided a waypoint selection method including: acquiring information regarding a current pose of a vehicle; acquiring a plurality of waypoints between the current pose of the vehicle and a destination pose; acquiring obstacle information; determining a first waypoint cost for a first waypoint of the plurality of waypoints; selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition; and outputting the selected first waypoint and all waypoints of the plurality of waypoints between the first waypoint and the current pose to a controller in response to the selection of the first waypoint.

[0006] According to a third exemplary aspect of the present disclosure, there is provided a vehicle control system comprising: a non-transitory computer-readable medium configured to store instructions; and a processor coupled to the non-transitory computer-readable medium, wherein the processor is configured to execute the instructions to acquire information about a current pose of the vehicle, acquire a destination pose, acquire obstacle information, acquire information about the vehicle's surroundings, determine a plurality of waypoints between the current pose and the destination pose, determine a first waypoint cost for a first waypoint of the plurality of waypoints, select the first waypoint in response to the first waypoint cost satisfying a predetermined condition, generate a path between the current pose and a pose at the first waypoint in response to the selection of the first waypoint, and output the selected first waypoint and the generated path to a controller in response to the selection of the first waypoint.

[0007] According to a fourth exemplary aspect of the present disclosure, a controller is provided that includes a non-transitory computer-readable medium configured to store instructions; and a processor coupled to the non-transitory computer-readable medium, the processor configured to execute the instructions to: acquire information regarding a current pose of the vehicle, acquire a destination pose, acquire obstacle information, acquire information regarding a surroundings of the vehicle, determine a plurality of waypoints between the current pose and the destination pose, determine a first waypoint cost for a first waypoint of the plurality of waypoints, select the first waypoint in response to the first waypoint cost satisfying a predetermined condition, generate a path between the current pose and a pose at the first waypoint in response to the selection of the first waypoint, and output movement instructions to the vehicle to move along the generated path in response to the selection of the first waypoint. [Brief explanation of the drawings]

[0008] Aspects of the present disclosure are best understood from the following detailed description when read in conjunction with the accompanying drawings. It should be noted that, in accordance with standard industry practice, various features have not been drawn to scale. In fact, dimensions of various features may be arbitrarily expanded or reduced for clarity of illustration. [Figure 1] FIG. 1 is a block diagram of a route determination system according to some embodiments. [Figure 2] FIG. 1 is a schematic diagram of a route including waypoints according to some embodiments. [Figure 3] 1 is a flowchart of a method for selecting a waypoint according to some embodiments. [Figure 4] FIG. 1 is a block diagram of a waypoint selection system according to some embodiments. [Figure 5] FIG. 1 is a schematic diagram of a ratio of a global path to a calculated path according to some embodiments. [Figure 6A] FIG. 10 is a schematic diagram of determining heading angle difference according to some embodiments. [Figure 6B] FIG. 10 is a schematic diagram of determining heading angle difference according to some embodiments. [Figure 6C] FIG. 10 is a schematic diagram of determining heading angle difference according to some embodiments. [Figure 6D] FIG. 10 is a schematic diagram of determining heading angle difference according to some embodiments. [Figure 7] FIG. 1 is a schematic illustration of determining orientation difference according to some embodiments. [Figure 8] FIG. 10 is a schematic diagram of obstacle proximity determination according to some embodiments. [Figure 9] FIG. 1 is a block diagram of a system usable for a waypoint selection system according to some embodiments. DETAILED DESCRIPTION OF THE INVENTION

[0009] The following disclosure provides many different embodiments or examples for implementing different features of the provided subject matter. Below, specific examples of components, values, operations, materials, arrangements, and the like are described to simplify the disclosure. Of course, these are merely examples and are not intended to be limiting. Other components, values, operations, materials, arrangements, and the like are also contemplated. For example, the formation of a first feature above or on a second feature in the following description can include embodiments in which the first and second features are formed in direct contact with each other, and can also include embodiments in which an additional feature may be formed between the first and second features such that the first and second features are not in direct contact with each other. Additionally, the present disclosure may repeat reference numerals and / or letters in various examples. This repetition is for simplicity and clarity and does not, in itself, dictate a relationship between the various described embodiments and / or configurations.

[0010] Additionally, spatially relative terms such as "below," "below," "lower," "above," and "upper" may be used herein for ease of description to describe the relationship of one element or feature to another, as shown in the figures. Spatially relative terms are intended to encompass different orientations of the device during use or operation in addition to the orientation shown in the figures. The device may be oriented in other directions (rotated 90 degrees or to other orientations), and the spatially relative descriptors used herein may be similarly interpreted accordingly. The use of the term "and / or" means that each option can be used individually or in combination with any or all of the other options.

[0011] The following description is applicable to any non-holonomic robot or vehicle. A non-holonomic robot or vehicle includes a robot or vehicle that cannot turn in a single spot, i.e., has a turning radius greater than zero. The following description refers to a vehicle for simplicity, but one skilled in the art will understand that the present description is applicable to any non-holonomic robot or vehicle. The present description is also applicable to autonomous driving applications.

[0012] Global planning of a route for a vehicle from an initial location to a destination location includes identifying waypoints along the estimated global path. Local planning includes selecting one of the identified waypoints and determining a route from the initial location to the selected waypoint. The selection of waypoints for local planning of a route affects the efficiency of the vehicle's travel to reach the destination location. For example, if the selected waypoint is very close to the initial location, the vehicle may have to change direction multiple times to reach the selected waypoint. This change in direction increases travel time and reduces travel efficiency. If the selected waypoint is far from the initial location, the risk of obstacles between the initial location and the selected waypoint increases, and the processing load for determining a route that avoids more obstacles may increase. Therefore, selecting waypoints that help maximize travel efficiency, or that provide at least a predetermined threshold for travel efficiency, helps maximize the efficiency of the vehicle's travel to reach the destination location.

[0013] Efficiency of movement for a vehicle helps reduce power consumption by the vehicle. For example, if the vehicle is an electric vehicle (EV), improved efficiency of movement improves the battery life of the vehicle. In some embodiments, the vehicle includes an engine. Increasing efficiency of movement reduces fuel consumption by vehicles that include an engine. Using forklifts in a warehouse as an example, efficiency of movement helps improve stock or distribution processes in the warehouse, thereby increasing productivity in logistics operations.

[0014] 1 is a block diagram of a route determination system 100 according to some embodiments. The route determination system 100 can be used to determine a route for a vehicle to travel from an initial location to a destination location. In addition, to travel from the initial location to the destination location, the route determination system 100 can also plan the movement of the vehicle so that the vehicle has a desired orientation when it arrives at the destination location. For example, a forklift in a warehouse not only needs to reach a destination location on a shelf to retrieve an item, but also needs to be facing the shelf at the destination location to increase the efficiency of the movement.

[0015] The route determination system 100 includes an odometry source 110 configured to determine an initial pose. The pose is a combination of location and orientation relative to a reference axis. In some embodiments, the odometry source 110 is also configured to determine the vehicle's current speed. In some embodiments, the odometry source 110 includes at least one sensor, such as an accelerometer, a gyroscope, or another suitable sensor attached to the vehicle. In some embodiments, the odometry source 110 is implemented by a device external to the vehicle that detects the pose of the vehicle. For example, some embodiments monitor the vehicle using a remote sensor, such as a camera, light detection and ranging (LiDAR), or other suitable remote sensor, and determine the vehicle's pose using a processor connected to the remote sensor. The odometry source 110 is configured to output the vehicle's current pose to the controller 130 and the planning module 160. In some embodiments, the current pose is provided to the planning module 160 and the controller 130 wirelessly or independently via a wired connection.

[0016] Route determination system 100 further includes a destination pose input 120. The destination pose input 120 includes a destination location and a desired orientation of the vehicle at the destination location. In some embodiments, the destination pose is received from a user, for example, through a graphical user interface (GUI) or another suitable input / output (I / O) device. In some embodiments, the destination pose is determined by an external device, for example, a processor, configured to determine tasks for the vehicle. The destination pose input 120 is provided to planning module 160. The destination pose input 120 is provided to planning module 160 wirelessly or via a wired connection.

[0017] Route determination system 100 further includes controller 130 configured to provide commands to the vehicle. In some embodiments, controller 130 is included as part of the vehicle. In some embodiments, controller 130 is external to the vehicle. In some embodiments, controller 130 includes a cloud-based controller implemented using one or more servers. Controller 130 is configured to receive a local route and waypoints from planning module 160 and a current pose from odometries source 110. Based on the local route and waypoints, controller 130 is configured to determine a movement that the vehicle should perform to change from a current pose to a destination pose at a destination location or a selected waypoint. Controller 130 is configured to provide commands for both speed and direction of movement for the vehicle. In some embodiments, controller 130 is configured to provide commands to the vehicle wirelessly. In some embodiments, controller 130 is configured to provide commands to the vehicle using a wired connection. In some embodiments, controller 130 is configured to receive the local route and waypoints wirelessly. In some embodiments, controller 130 is configured to receive the local route and waypoints via a wired connection.

[0018] Route determination system 100 further includes a map server 140 configured to store obstacle information and route locations. In some embodiments, map server 140 is included in a memory connected to the vehicle. In some embodiments, map server 140 is external to the vehicle, such as a cloud-based storage unit. Map server 140 includes information about the locations of known obstacles. In some embodiments, map server 140 is updated based on new information from the vehicle and other vehicles as new obstacles are identified or obstacle locations change. In some embodiments, map server 140 includes information about the route, such as roads, aisles in a warehouse, corridors in a building, or other suitable routes the vehicle may travel. In some embodiments, map server 140 further includes information about the locations of other vehicles connected to map server 140. Map server 140 is configured to provide the obstacle information and route locations to planning module 160. In some embodiments, map server 140 transmits the obstacle information and route locations wirelessly. In some embodiments, map server 140 transmits the obstacle information and route locations via a wired connection. In some embodiments, the map server 140 is also connected to the controller 130 for exchanging information with the controller 130 wirelessly or by a wired connection.

[0019] Route determination system 100 further includes sensor 150 configured to detect obstacles near the vehicle. In some embodiments, sensor 150 is a single sensor. In some embodiments, sensor 150 includes multiple sensors. In some embodiments, sensor 150 includes different types of sensors. In some embodiments, sensor 150 is within the vehicle. In some embodiments, sensor 150 is external to the vehicle. In some embodiments, sensor 150 is integrated with odometries source 110. In some embodiments, sensor 150 includes a camera, a LiDAR, or another suitable sensor. Sensor 150 is configured to provide information about the vehicle's surroundings to planning module 160, such as point cloud data, laser scan data, or other suitable data. In some embodiments, sensor 150 provides surrounding information to planning module 160 wirelessly. In some embodiments, sensor 150 provides surrounding information to planning module 160 via a wired connection.

[0020] Route determination system 100 further includes a planning module 160. Planning module 160 is configured to receive a current pose from odometries source 110, a destination pose input 120, obstacle information and path locations from map server 140, and surrounding information from sensors 150. Planning module 160 is configured to determine a local path and waypoints between the current pose and the destination pose using the received information. Planning module 160 is configured to output the local path and waypoints to controller 130 wirelessly or through a wired connection. Planning module 160 is implemented using one or more processors. In some embodiments, planning module 160 is part of the vehicle. In some embodiments, planning module 160 is external to the vehicle.

[0021] The planning module 160 includes a global planning module 162. The global planning module 162 is configured to determine global waypoints from the current pose to a destination pose. The global planning module 162 includes a global cost map 163. The global cost map 163 is configured to determine which portions of the map between the current pose and the destination pose are traversable by the vehicle. The global planning module 162 further includes a global planner 164 configured to determine waypoints from the current pose to the destination pose based on information from the global cost map 163 together with information received by the planning module 160. The global planner 164 is configured to set locations for the waypoints. In some embodiments, the global planner 164 is configured to set waypoints at regular intervals between the current pose and the destination pose. In some embodiments, the global planner 164 is configured to set waypoints at predetermined distances from known obstacle locations between the current pose and the destination pose. The global cost map 163 and the global planner 164 are implemented by one or more processors. In some embodiments, global cost map 163 and global planner 164 are implemented by the same processor. In some embodiments, global cost map 163 and global planner 164 are implemented using separate processors.

[0022] Planning module 160 further includes local planning module 165. Local planning module 164 is configured to determine a local path and waypoints from the current pose to a pose at the selected waypoint received from global planning module 162. Local planning module 165 includes a local cost map 166. Local cost map 166 is configured to determine which portions of the map between the current pose and the pose at the selected waypoint are traversable by the vehicle. Details of determining the amount of travel are described in detail below, according to some embodiments. Local planning module 165 further includes a local planner 167 configured to determine a path from the current pose to a pose at the selected waypoint based on information from local cost map 166, information from global planning module 162, and information received by planning module 160. Local planner 167 is configured to set waypoints along the determined path. Local cost map 166 and local planner 167 are implemented by one or more processors. In some embodiments, local cost map 166 and local planner 167 are implemented by the same processor. In some embodiments, local cost map 166 and local planner 167 are implemented using separate processors. In some embodiments, at least one of local cost map 166 or local planner 167 is implemented using the same processor as at least one of global cost map 163 or global planner 164. In some embodiments, local cost map 166, local planner 167, global cost map 163, and global planner 164 are all implemented using separate processors.

[0023] 2 is a schematic diagram of a route 200 including waypoints 205 according to some embodiments. In some embodiments, the route 200 is a global path provided by a global planning module, e.g., global planning module 162 (FIG. 1), to a local planning module, e.g., local planning module 165 (FIG. 1). The route 200 includes an initial location 210 and an initial orientation 215, which together define an initial pose. The route 200 further includes a destination location 220 and a destination orientation 225, which together define a destination pose. There are multiple waypoints 205 between the initial location 210 and the destination location 220. The waypoint 230 is a waypoint between the multiple waypoints 205 selected by a local planner, e.g., by the local planner 167 (FIG. 1), to determine a local path starting from the initial pose.

[0024] Based on route 200, a local planner, e.g., local planner 167 (FIG. 1), determines a local path from the initial pose to waypoint 230, with an orientation specified at waypoint 230. In response to receiving the local path between waypoint 230 and the initial pose and waypoint 205, a controller, e.g., controller 130 (FIG. 1), generates and provides instructions to the vehicle to move from the initial pose to the pose at waypoint 230. Details are provided below regarding how the local cost map selects waypoints, such as waypoint 230, according to some embodiments.

[0025] 3 is a flowchart of a method 300 for selecting a waypoint according to some embodiments. Method 300 can be used to generate a local path, for example, a local path between an initial pose and a pose at waypoint 230 (FIG. 2). In some embodiments, method 300 is implemented using route determination system 100 (FIG. 1).

[0026] Method 300 includes an operation 305 in which a global waypoint is set. The global waypoint is set based on the current pose, the destination pose, map data, and sensor data. In some embodiments, the current pose is provided by an odometry sensor, e.g., odometry sensor 110 (FIG. 1). In some embodiments, the destination pose is provided by a destination pose input, e.g., destination pose input 120 (FIG. 1). In some embodiments, the map data is provided by a map server, e.g., map server 140 (FIG. 1). In some embodiments, the sensor data is provided by a sensor, e.g., sensor 150 (FIG. 1). Based on the received information, a global route for the vehicle to travel from the current pose to the destination pose is determined. In some embodiments, the global route is determined using a global planner, e.g., global planner 164 (FIG. 1). The global waypoint is set along the global route. In some embodiments, the global waypoint is set at regular intervals along the global route. In some embodiments, the global waypoint is set a predetermined distance from a known obstacle location between the current pose and the destination pose.

[0027] Following operation 305, method 300 proceeds to operation 310 or operation 330. Method 300 proceeding to operation 310 provides more accurate results compared to method 300 proceeding to operation 330. However, method 300 proceeding to operation 310 involves a higher processing load compared to method 300 proceeding to operation 330. In some embodiments, the selection of whether to proceed to operation 310 or operation 330 is based on user input. In some embodiments, whether to proceed to operation 310 or operation 330 is determined based on the vehicle's state of charge (SOC). In embodiments in which method 300 proceeds according to the vehicle's SOC, method 300 proceeds to operation 330 in response to the vehicle's SOC falling below a predetermined threshold.

[0028] In operation 310, a waypoint cost is calculated for each waypoint of the global waypoints, for example, using local cost map 166 (FIG. 1). Calculation of waypoint costs is described in more detail below. In some embodiments, operation 310 is limited to only waypoints that are less than or equal to the midpoint between the initial location determined using the current pose and the destination location determined using the destination pose. In some embodiments, operation 310 is limited to only waypoints that are more than 10% of the distance along the global path from the current pose.

[0029] In operation 330, a waypoint cost is calculated for a first one of the global waypoints, for example, using local cost map 166 (FIG. 1). Calculation of waypoint cost is described in more detail below. In some embodiments, the first waypoint is the global waypoint closest to the current pose. In some embodiments, the first waypoint is the global waypoint close to the destination pose. In some embodiments, the first waypoint is the global waypoint closest to the middle of the global path between the current pose and the destination pose. In some embodiments, the first waypoint is specified by a user. In some embodiments, the first waypoint is randomly selected from among the global waypoints.

[0030] At operation 335, the waypoint cost determined at operation 330 is compared to a waypoint cost threshold, for example, using local planner 167 (FIG. 1). The waypoint cost threshold is a predetermined value set based on user input, the vehicle's SOC, regulations set by a third party such as a company or government, or another suitable criteria. In response to a determination that the waypoint cost is less than the waypoint cost threshold, method 300 proceeds to operation 315. In response to a determination that the waypoint cost is greater than or equal to the waypoint cost threshold, method 300 proceeds to operation 340.

[0031] In operation 340, the waypoint is moved to the next waypoint of the global waypoint, e.g., the second waypoint, using, for example, local cost map 166 (FIG. 1). In some embodiments, the next waypoint is one waypoint that is closer to the current pose than the last waypoint compared to a waypoint cost threshold. In some embodiments, the next waypoint is one waypoint that is closer to the destination pose than the last waypoint compared to a waypoint cost threshold. In some embodiments, the next waypoint is randomly selected from all remaining global waypoints that were not compared to the waypoint cost threshold. Following operation 340, method 300 returns to operation 330, where the cost of the next waypoint determined in operation 340 is calculated. Operations 330, 335, and 340 are repeated until a waypoint is found to have a cost less than the waypoint cost threshold. In response to the comparison between the waypoint costs for all global waypoints and the waypoint cost threshold failing to identify a global waypoint that satisfies the condition of operation 335, method 300 returns to operation 310. Those skilled in the art will appreciate that the waypoint costs for all global waypoints have already been calculated based on the failure of operations 330, 335, and 340 to find any global waypoints with waypoint costs below the waypoint cost threshold. Thus, when method 300 returns to operation 310, the method can immediately proceed to operation 315.

[0032] In operation 315, the global waypoint having the lowest waypoint cost is selected for use by the local planner, e.g., local planner 167 (FIG. 1), to develop the local path. In some embodiments, where method 300 proceeds directly from operation 335 to operation 315, the most recent global waypoint compared to the waypoint cost threshold is selected for use by the local planner.

[0033] In operation 320, a path from the current pose to the selected waypoint is generated. In some embodiments, the path is generated using a local planner, e.g., local planner 167 (FIG. 1). The path is generated based on map data, e.g., from map server 140 (FIG. 1), the current pose, e.g., from odometries source (110), sensor data, e.g., from sensors 150 (FIG. 1), and the selected waypoint.

[0034] In operation 325, the generated path and set of waypoints between the current pose and the selected waypoint are transmitted to a controller for guiding the vehicle to the selected waypoint. In some embodiments, the instructions for transmitting the generated path and set of waypoints are generated by a local planner, e.g., local planner 167 (FIG. 1). In some embodiments, the path and waypoints are transmitted to the controller wirelessly. In some embodiments, the path and waypoints are transmitted to the controller by a wired connection. The controller, e.g., controller 130 (FIG. 1), uses the generated path and set of waypoints to provide velocity commands to the vehicle wirelessly or using a wired connection.

[0035] Method 300 is repeated until the selected waypoint corresponds to the destination pose. In some embodiments, method 300 is repeated once a waypoint is selected and the controller begins guiding the vehicle to the selected waypoint. In some embodiments, method 300 is repeated once the vehicle is within a predetermined distance of the selected waypoint. In some embodiments, method 300 is repeated once the vehicle is within a predetermined arrival time to the selected waypoint. In some embodiments, method 300 is continuously repeated while the vehicle is traveling from the current pose to the destination pose until the destination pose is the selected waypoint.

[0036] In some embodiments, at least one additional operation is included in method 300. For example, in some embodiments, some of the global waypoints are excluded from the waypoint cost calculation based on their proximity to the current pose to reduce processing load. In some embodiments, at least one operation is excluded from method 300. For example, in some embodiments, operations 330, 335, and 340 are excluded, and waypoint costs are calculated for each of the global waypoints regardless of user input or SOC of the vehicle.

[0037] FIG. 4 is a block diagram of a waypoint selection system 400 according to some embodiments. The waypoint selection system 400 is configured to determine a waypoint cost for a waypoint from a current pose. In some embodiments, the waypoint selection system 400 can be used to perform operation 310 and / or operation 330 in method 300 (FIG. 3). The waypoint selection system 400 is implemented using one or more processors and configured to exchange information. In some embodiments, the waypoint selection system 400 is integrated into the local cost map 166 (FIG. 1). The waypoint selection system 400 is configured to output the waypoint cost 495 to a local planner, e.g., the local planner 167 (FIG. 1), wirelessly or using a wired connection. The waypoint selection system 400 is further configured to receive information from an external device wirelessly or using a wired connection.

[0038] The waypoint selection system 400 includes a waypoint cost block 440 configured to determine a waypoint cost for the waypoint under consideration. The waypoint cost corresponds to the amount of movement for the vehicle to move from its current pose to the waypoint pose corresponding to the waypoint under consideration. The waypoint cost block 440 is configured to receive the current pose 410. The current pose is the current position and orientation of the vehicle. In some embodiments, the current pose is determined using a sensor accessible by the waypoint cost block 440. In some embodiments, the current pose is received from an external device, such as the odometries source 110 (FIG. 1). The waypoint cost block 440 is further configured to receive the waypoint pose 420. In some embodiments, the waypoint pose is received from a global planner, such as the global planner 164 (FIG. 1). The waypoint cost block 430 is further configured to receive obstacle information 430. In some embodiments, the obstacle information 430 includes all known obstacles. In some embodiments, the obstacle information 430 includes only obstacles that pose a risk to the vehicle. For example, if an obstacle is located 5 meters above the ground and the vehicle is 2 meters above the ground, in some embodiments the obstacle is excluded from the obstacle information because there is no chance that the vehicle will collide with an obstacle 5 meters above the ground. In some embodiments, obstacle information 430 is stored in a memory accessible by waypoint cost block 440. In some embodiments, obstacle information 430 is received from an external device, such as map server 140 (FIG. 1).

[0039] The waypoint cost block 440 includes a local path function block 450 configured to determine the cost of the waypoint based on a path from the current pose 410 to the waypoint pose 420. The local path function block 450 is configured to determine a path length ratio 452 based on the ratio between a direct path from the current pose 410 to the waypoint pose 420 and a calculated path from the current pose 410 to the waypoint pose. An example of determining the path length ratio 452 is described with reference to FIG. 5.

[0040] FIG. 5 is a schematic diagram of a ratio of a global path to a calculated path according to some embodiments. FIG. 5 includes a vehicle at an initial position 210 having an initial orientation 215. FIG. 5 further includes a destination position 220 and a destination orientation 215. Because the waypoint under consideration is a destination for waypoint cost block 440 (FIG. 4), the destination position 220 and destination orientation 225 are waypoint pose 420 (FIG. 4). FIG. 5 further includes a direct path 510 that extends in a straight line from the initial position 210 to the destination position 220. FIG. 5 further includes a calculated path 520 from the initial position 210 to the destination position 220.

[0041] The calculated path 520 is calculated to estimate a possible route the vehicle may take to travel from the initial position 210 to the destination position 220. The inability of the vehicle to have a zero turning radius reduces the likelihood that the vehicle will travel in a straight line from the initial position 210 to the destination position 220. Therefore, the calculated path 220 is a more realistic estimate of the route the vehicle will travel. In some embodiments, the calculated path is determined using a Reed-Shepp curve. The Reed-Shepp curve considers the association of a kinematic model with the nonholonomic vehicle. The Reed-Shepp curve indicates the shortest path for the nonholonomic vehicle to travel between points. The current description is not limited to the Reed-Shepp curve, but can be used with any computational method available for nonholonomic vehicles to determine the calculated path 520. The path length ratio is the ratio of the length of the calculated path 520 to the direct path 510. The smaller the ratio, the smaller the difference between the calculated path 520 and the direct path 510. At a value of 1, the calculated path 520 is equal to the direct path 510 .

[0042] Returning to FIG. 4 , local path function block 450 is further configured to determine an estimated direction change count 454 based on the difference between the current pose 410 and the waypoint pose 420. In some embodiments, the estimated direction change count 454 is further determined using a calculated path, such as calculated path 520 ( FIG. 5 ). A direction change includes switching from forward movement to reverse movement or reverse movement to forward movement. For purposes of estimating direction change count 454, turns, such as turning left or right, are not considered direction changes. As the direction change count increases, traveling from the current pose 410 to the waypoint pose 420 becomes less efficient because more time is used in the movement and the distance traveled by the vehicle increases.

[0043] The waypoint cost block 440 further includes an orientation function block 460 configured to determine the cost of the waypoint based on the change in orientation from the current pose 410 to the waypoint pose 420. The orientation function block 460 is configured to determine a heading angle difference 462. An example of determining the heading angle difference is described with reference to Figures 6A-6D.

[0044] 6A-6D are schematic diagrams of determining a heading angle difference according to some embodiments. The heading angle difference is determined by a straight line connecting a current pose, e.g., current pose 410 (FIG. 4), to a waypoint pose, e.g., waypoint pose 420 (FIG. 4). In some embodiments, the straight line corresponds to a direct path 510 (FIG. 5). The angle between the initial orientation 215 and the straight line is used to determine the heading angle difference. An angle ranging from zero degrees (0°) to less than ninety degrees (90°) indicates that forward movement by the vehicle is the preferred direction of travel. An angle ranging from greater than ninety degrees (90°) to one hundred eighty degrees (180°) indicates that reverse movement by the vehicle is the preferred direction of travel. As the angle approaches ninety degrees (90°), movement efficiency decreases and waypoint cost increases. As the angle approaches zero degrees (0°) or one hundred eighty degrees (180°), movement efficiency increases and waypoint cost decreases. At an angle of exactly ninety degrees (90°), both forward and reverse movement are equally prioritized, with the selected direction of movement based on the orientation difference, the obstacle position, the distance between the vehicle and the obstacle, or other suitable parameters.

[0045] 6A is a schematic diagram 600A of a heading angle θa less than ninety degrees (90°) according to some embodiments. The heading angle θa is between a line connecting a current pose, e.g., current pose 410 (FIG. 4), having an initial orientation 215, and a destination pose, e.g., destination pose 420 (FIG. 4), having a destination orientation 225.

[0046] 6B is a schematic diagram 600B of a heading angle θb approximately equal to ninety degrees (90°) according to some embodiments. The heading angle θb is between a line connecting a current pose, e.g., current pose 410 (FIG. 4), having an initial orientation 215, and a destination pose, e.g., destination pose 420 (FIG. 4), having a destination orientation 225.

[0047] 6C is a schematic diagram 600C of a heading angle θc that is greater than ninety degrees (90°) and less than one hundred eighty degrees (180°) according to some embodiments. The heading angle θc is between a line connecting a current pose, e.g., current pose 410 (FIG. 4), having an initial orientation 215, and a destination pose, e.g., destination pose 420 (FIG. 4), having a destination orientation 225.

[0048] 6D is a schematic diagram 600D of a heading angle θd approximately equal to 180 degrees (180°) according to some embodiments. The heading angle θd is between a line connecting a current pose, e.g., current pose 410 (FIG. 4), having an initial orientation 215, and a destination pose, e.g., destination pose 420 (FIG. 4), having a destination orientation 225.

[0049] The waypoint cost associated with Figure 6D is the smallest among Figures 6A to 6D, and the waypoint cost associated with Figure 6B is the highest among Figures 6A to 6D.

[0050] Returning to FIG. 4 , the orientation function block 460 is further configured to determine an orientation difference 464. The orientation difference 464 is the difference between the orientation of the current pose 410 and the orientation of the waypoint pose 420. The orientation difference 464 is determined by calculating a first angle between the orientation of the current pose 410 and a first reference axis and a second angle between the orientation of the waypoint pose 420 and a second reference axis extending parallel to the first reference axis. The difference between the first angle and the second angle corresponds to the orientation difference 464. As the difference between the first angle and the second angle decreases, the efficiency of movement increases and the waypoint cost is reduced. As the difference between the first angle and the second angle increases, the efficiency of movement decreases and the waypoint cost is increased. The maximum difference between the first angle and the second angle is 180 degrees (180°), which has the maximum waypoint cost. An example of determining the orientation difference 464 is shown in FIG. 7.

[0051] FIG. 7 is a schematic diagram 700 of determining an orientation difference according to some embodiments. FIG. 7 includes an initial orientation 215 corresponding to a current pose, e.g., current pose 410 (FIG. 4), and a first reference axis 710. A first angle θs is between the initial orientation 215 and the first reference axis 710. FIG. 7 further includes a destination orientation 225 corresponding to a waypoint pose, e.g., waypoint pose 420 (FIG. 4), and a second reference axis 720. The second reference axis 720 is parallel to the first reference axis 710. A second angle θw is between the destination orientation 225 and the second reference axis 720. An orientation difference, e.g., orientation difference 464 (FIG. 4), is the difference between the first angle θs and the second angle θw.

[0052] Returning to FIG. 4 , the waypoint cost block 440 is further configured to determine obstacle proximity 470. The waypoint cost block 440 determines obstacle proximity 470 by generating a calculated path between the current pose 410 and the waypoint pose 420, e.g., calculated path 520 ( FIG. 5 ). Next, the waypoint cost block 440 determines the closest obstacle to the calculated path based on the obstacle information 430. The distance between the closest obstacle and the calculated path is determined as obstacle proximity 470. As the distance between the closest obstacle and the calculated path decreases, the waypoint cost increases. In response to determining that the closest obstacle overlaps the calculated path, the waypoint cost block 440 sets the waypoint cost to infinity, and the waypoint under consideration is discarded. In some embodiments, the waypoint cost block 440 generates multiple calculated paths, and obstacle proximity 470 is determined for each of the calculated paths. FIG. 8 is an example of determining obstacle proximity 470 according to some embodiments.

[0053] FIG. 8 is a schematic diagram 800 of determining obstacle proximity according to some embodiments. FIG. 8 includes an initial location 210 corresponding to a current pose, e.g., current pose 410 (FIG. 4), and a destination location 220 corresponding to a waypoint pose, e.g., waypoint pose 420 (FIG. 4). A first calculated path 810 extends from the initial location 210 to the destination location 220. An obstacle 820 closest to the first calculated path 810 is identified based on obstacle information, e.g., obstacle information 430 (FIG. 4). A distance Rd between the closest obstacle 820 and the first calculated path 810 corresponds to an obstacle proximity, e.g., obstacle proximity 470 (FIG. 4). FIG. 8 further includes a second calculated path 830. In some embodiments, another obstacle proximity is calculated based on the distance between the closest object 820 and the second calculated path 830.

[0054] Returning to FIG. 4 , the waypoint cost block 440 is further configured to determine a Euclidean distance 480 between the waypoint pose 420 and the current pose 410. The waypoint cost block 440 is configured to determine the Euclidean distance 480 by determining the length of a line between the current pose 410 and the waypoint pose 420. The waypoint cost associated with the Euclidean distance 480 is based on a comparison between the vehicle's turning radius and the determined Euclidean distance 480. In some embodiments, if the Euclidean distance 480 is less than twice the vehicle's minimum turning radius, the waypoint is determined to have a higher cost, and if the Euclidean distance 480 is equal to or greater than twice the vehicle's minimum turning radius, the waypoint is determined to have a lower cost. The difference in waypoint cost associated with the vehicle's minimum turning radius is associated with a reduction in speed for making turns with a small radius.

[0055] The waypoint cost block 440 is configured to combine the waypoint costs determined by the local path function block 450, the orientation function block 460, the obstacle proximity 470, and the Euclidean distance 480 to determine a total waypoint cost using a formula block 490. The formula block 490 is configured to calculate the total waypoint cost based on a coefficient associated with each component waypoint cost and add the resulting values together. In some embodiments, the coefficients are determined individually based on the vehicle's surrounding environment, the number of obstacles in the obstacle information 430, third-party regulations related to the vehicle's operation, user input, or other suitable criteria. In some embodiments, each of the coefficients is equal so that each component waypoint cost has the same impact on the total waypoint cost. In some embodiments, at least one coefficient is different from at least one other coefficient so that the waypoint cost of at least one component has a greater impact on the total waypoint cost than the waypoint cost of another component. In some embodiments, the formula block 490 is configured to calculate the total waypoint cost using the following equation (1):

number

[0056] The waypoint cost block 440 is configured to output a waypoint cost 495 for the waypoint under consideration. In some embodiments, the waypoint cost block 440 is configured to transmit the waypoint cost 495 wirelessly. In some embodiments, the waypoint cost block 440 is configured to transmit the waypoint cost 495 over a wired connection. In some embodiments, the waypoint cost 495 can be used in performing operation 310 or operation 330 of method 300 (FIG. 3).

[0057] FIG. 9 is a schematic diagram of a system 900 usable for selecting waypoints according to one or more embodiments. The system 900 may further implement a route determination system. The system 900 includes a hardware processor 902 and a non-transitory computer-readable storage medium 904 encoded with, i.e., having stored thereon, computer program code 906, i.e., a set of executable instructions. The computer-readable storage medium 904 is also encoded with instructions 907 for interfacing with external devices. The processor 902 is electrically coupled to the computer-readable storage medium 904 via a bus 908. The processor 902 is also electrically coupled to an I / O interface 910 by the bus 908. A network interface 912 is also electrically connected to the processor 902 via the bus 908. The network interface 912 is connected to a network 914 such that the processor 902 and the computer-readable storage medium 904 can connect to external elements via the network 914. The processor 902 is configured to execute computer program code 906 encoded on the computer-readable storage medium 904 to enable the system 900 to perform some or all of the operations described in the route determination system 100 (FIG. 1), the method 300 (FIG. 3), or the waypoint selection system 400 (FIG. 4).

[0058] In some embodiments, the processor 902 is a central processing unit (CPU), a multiprocessor, a distributed processing system, an application specific integrated circuit (ASIC), and / or other suitable processing unit.

[0059] In some embodiments, computer-readable storage medium 904 is an electronic, magnetic, optical, electromagnetic, infrared, and / or semiconductor system (or apparatus or device). For example, computer-readable storage medium 904 includes a semiconductor or solid-state memory, magnetic tape, a removable computer diskette, a random access memory (RAM), a read-only memory (ROM), a rigid magnetic disk, and / or an optical disk. In some embodiments using an optical disk, computer-readable storage medium 904 includes a compact disk-read-only memory (CD-ROM), a compact disk-read / write (CD-R / W), and / or a digital video disk (DVD).

[0060] In some embodiments, the storage medium 904 stores computer program code 906 configured to cause the system 900 to implement the route determination system 100 (FIG. 1), the method 300 (FIG. 3), or the waypoint selection system 400 (FIG. 4). In some embodiments, the storage medium 904 also stores information necessary to implement the route determination system 100 ( FIG. 1 ), the method 300 ( FIG. 3 ), or the waypoint selection system 400 ( FIG. 4 ), as well as information generated during the implementation of the route determination system 100 ( FIG. 1 ), the method 300 ( FIG. 3 ), or the waypoint selection system 400 ( FIG. 4 ), such as a coefficient parameter 916, a direction change parameter 918, a Euclidean distance parameter 920, a path ratio parameter 922, an obstacle proximity parameter 924, an orientation difference parameter 926, and a heading angle parameter 928, and / or a set of executable instructions for carrying out the implementation of the route determination system 100 ( FIG. 1 ), the method 300 ( FIG. 3 ), or the waypoint selection system 400 ( FIG. 4 ).

[0061] In some embodiments, storage medium 904 stores instructions 907 for interfacing with an external device. The instructions 907 enable processor 902 to generate manufacturing instructions readable by the external device to effectively implement route determination system 100 (FIG. 1), method 300 (FIG. 3), or waypoint selection system 400 (FIG. 4).

[0062] System 900 includes an I / O interface 910. I / O interface 910 is coupled to external circuitry. In some embodiments, I / O interface 910 includes a keyboard, keypad, mouse, trackball, trackpad, and / or cursor direction keys for communicating information and commands to processor 902.

[0063] The system 900 also includes a network interface 912 coupled to the processor 902. The network interface 912 enables the system 900 to communicate with a network 914 to which one or more other computer systems are connected. The network interface 912 includes a wireless network interface, such as BLUETOOTH®, Wireless Fidelity (WIFI), Worldwide Interoperability for Microwave Access (WIMAX), General Packet Radio Service (GPRS), or Wideband Code Division Multiple Access (WCDMA®), or a wired network interface, such as ETHERNET, Universal Serial Bus (USB), or IEEE-1394 (Institute of Electrical and Electronics Engineers-1394). In some embodiments, implementations of route determination system 100 (FIG. 1), method 300 (FIG. 3), or waypoint selection system 400 (FIG. 4) are performed in two or more systems 900, and information such as coefficient parameters 916, direction change parameters 918, Euclidean distance parameters 920, path ratio parameters 922, obstacle proximity parameters 924, orientation difference parameters 926, and heading angle parameters 928 are exchanged between the different systems 900 via network 914.

[0064] (Addendum)

[0065] One aspect of the present disclosure relates to a waypoint selection system. The waypoint selection system includes a non-transitory computer-readable medium configured to store instructions. The waypoint selection system further includes a processor connected to the non-transitory computer-readable medium. The processor is configured to execute instructions to acquire information regarding a current pose of the vehicle. The processor is further configured to execute instructions to acquire multiple waypoints between the current pose of the vehicle and a destination pose. The processor is further configured to execute instructions to acquire obstacle information. The processor is further configured to execute instructions to determine a first waypoint cost for a first waypoint of the multiple waypoints. The processor is further configured to execute instructions to select the first waypoint in response to the first waypoint cost satisfying a predetermined condition. The processor is further configured to execute instructions to output the selected first waypoint and all waypoints of the multiple waypoints between the first waypoint and the current pose to a controller in response to the selection of the first waypoint.

[0066] In some embodiments, the processor is further configured to execute instructions for determining a waypoint cost for each of the plurality of waypoints.

[0067] In some embodiments, the processor is further configured to execute instructions to select the first waypoint in response to the first waypoint cost being the lowest waypoint cost of all waypoint costs for the plurality of waypoints.

[0068] In some embodiments, the processor is further configured to execute instructions for selecting the first waypoint in response to the first waypoint cost being less than a predetermined waypoint cost threshold.

[0069] In some embodiments, the processor is further configured to execute instructions for determining a second waypoint cost for a second waypoint of the plurality of waypoints in response to determining that the first waypoint cost is greater than or equal to a predetermined waypoint cost threshold.

[0070] In some embodiments, the processor is further configured to execute instructions to generate, in response to selection of the first waypoint, a path from the current pose to a pose at the first waypoint.

[0071] In some embodiments, the processor is further configured to execute instructions to determine the first waypoint cost based on a path length ratio between (1) a direct path between the current pose and the 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, a proximity of a nearest obstacle to the calculated path, and a Euclidean distance between the current pose and the pose at the first waypoint.

[0072] An aspect of the present specification further relates to a waypoint selection method. The method includes acquiring information about a current pose of a vehicle. The method further includes acquiring a plurality of waypoints between the current pose of the vehicle and a destination pose. The method further includes acquiring obstacle information. The method further includes determining a first waypoint cost for a first waypoint of the plurality of waypoints. The method further includes selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition. The method further includes outputting the selected first waypoint and all waypoints of the plurality of waypoints between the first waypoint and the current pose to a controller in response to the selection of the first waypoint.

[0073] In some embodiments, determining the first waypoint cost includes determining a waypoint cost for each of a plurality of waypoints.

[0074] In some embodiments, selecting the first waypoint includes selecting the first waypoint in response to the first waypoint cost being the lowest waypoint cost of all waypoint costs for the plurality of waypoints.

[0075] In some embodiments, selecting the first waypoint includes selecting the first waypoint in response to the first waypoint cost being less than a predetermined waypoint cost threshold.

[0076] In some embodiments, the method further includes determining a second waypoint cost for a second waypoint of the plurality of waypoints in response to determining that the first waypoint cost is greater than or equal to a predetermined waypoint cost threshold.

[0077] In some embodiments, the method further includes, in response to selecting the first waypoint, generating a path from the current pose to a pose at the first waypoint.

[0078] In some embodiments, determining the first waypoint cost includes determining the first waypoint cost based on a path length ratio between (1) a direct path between the current pose and the 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, a proximity of a nearest obstacle to the calculated path, and a Euclidean distance between the current pose and the pose at the first waypoint.

[0079] One aspect of the present disclosure relates to a vehicle control system. The vehicle control system includes a non-transitory computer-readable medium configured to store instructions. The vehicle control system further includes a processor connected to the non-transitory computer-readable medium. The processor is configured to execute instructions to acquire information about a current pose of the vehicle. The processor is further configured to execute instructions to acquire a destination pose. The processor is further configured to execute instructions to acquire obstacle information. The processor is further configured to execute instructions to acquire information about the vehicle's surroundings. The processor is further configured to execute instructions to determine a plurality of waypoints between the current pose and the destination pose. The processor is further configured to execute instructions to determine a first waypoint cost for a first waypoint of the plurality of waypoints. The processor is further configured to execute instructions to select the first waypoint in response to the first waypoint cost satisfying a predetermined condition. The processor is further configured to execute instructions to generate a path between the current pose and a pose at the first waypoint in response to the selection of the first waypoint. The processor is further configured to execute instructions for, in response to selection of the first waypoint, outputting the selected first waypoint and the generated path to the controller.

[0080] In some embodiments, the processor is further configured to execute instructions for determining a waypoint cost for each of the plurality of waypoints.

[0081] In some embodiments, the processor is further configured to execute instructions to select the first waypoint in response to the first waypoint cost being the lowest waypoint cost of all waypoint costs for the plurality of waypoints.

[0082] In some embodiments, the processor is further configured to execute instructions for selecting the first waypoint in response to the first waypoint cost being less than a predetermined waypoint cost threshold.

[0083] In some embodiments, the processor is further configured to execute instructions to determine the first waypoint cost based on a path length ratio between (1) a direct path between the current pose and the 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, a proximity of a nearest obstacle to the calculated path, and a Euclidean distance between the current pose and the pose at the first waypoint.

[0084] In some embodiments, the vehicle control system further includes a controller, the controller configured to generate instructions for moving the vehicle from the current pose to the pose at the first waypoint and to send the instructions for moving to the vehicle.

[0085] One aspect of the present disclosure relates to a controller. The controller includes a non-transitory computer-readable medium configured to store instructions. The controller further includes a processor connected to the non-transitory computer-readable medium. The processor is configured to execute instructions to acquire information about a current pose of the vehicle, acquire a destination pose, acquire obstacle information, and acquire information about the vehicle's surroundings. The processor is further configured to execute instructions to determine a plurality of waypoints between the current pose and the destination pose. The processor is further configured to execute instructions to determine a first waypoint cost for a first waypoint of the plurality of waypoints. The processor is further configured to execute instructions to select the first waypoint in response to the first waypoint cost satisfying a predetermined condition. The processor is further configured to execute instructions to generate a path between the current pose and a pose at the first waypoint in response to the selection of the first waypoint. The processor is further configured to execute instructions to output a movement command to the vehicle to move along the generated path in response to the selection of the first waypoint.

[0086] The foregoing outlines features of some embodiments so that those skilled in the art may better understand aspects of the present disclosure. Those skilled in the art should appreciate that they can readily use this disclosure as a basis for designing or modifying other processes and structures that carry out the same purposes and / or achieve the same advantages as the embodiments introduced herein. Those skilled in the art should also recognize that such equivalent constructions do not depart from the spirit and scope of the present disclosure, and that various changes, substitutions, and alterations can be made herein without departing from the spirit and scope of the present disclosure.

[0087] This application claims the benefit of U.S. Patent Application No. 17 / 472,523, filed September 10, 2021, which is incorporated herein by reference in its entirety.

Claims

1. a non-transitory computer-readable medium configured to store instructions; a processor coupled to the non-transitory computer-readable medium, the processor comprising: Get information about the vehicle's current pose, obtaining a plurality of waypoints between the current pose and a destination pose of the vehicle; Obtain obstacle information, determining a first waypoint cost for a first waypoint of the plurality of waypoints; selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition; In response to the selection of the first waypoint, outputting the selected first waypoint and all waypoints of the plurality of waypoints between the first waypoint and the current pose to a controller. configured to execute the instructions for the processor is further configured to execute the instructions to determine the first waypoint cost based on a number of direction changes along a calculated path between the current pose and a pose at the first waypoint and a comparison of a Euclidean distance between the current pose and the pose at the first waypoint and a turning radius of the vehicle, wherein the direction changes include switching from forward movement to reverse movement of the vehicle or from reverse movement to forward movement, and do not include turning of the vehicle; Waypoint selection system.

2. The waypoint selection system of claim 1 , wherein the processor is further configured to execute the instructions for determining a waypoint cost for each of the plurality of waypoints.

3. 3. The waypoint selection system of claim 2, wherein the processor is further configured to execute the instructions to select the first waypoint in response to the first waypoint cost being the lowest waypoint cost of all waypoint costs for the plurality of waypoints.

4. 2. The waypoint selection system of claim 1, wherein the processor is further configured to execute the instructions to select the first waypoint in response to the first waypoint cost being less than a predetermined waypoint cost threshold.

5. 5. The waypoint selection system of claim 4, wherein the processor is further configured to execute the instructions for determining a second waypoint cost for a second waypoint of the plurality of waypoints in response to determining that the first waypoint cost is greater than or equal to the predetermined waypoint cost threshold.

6. 2. The waypoint selection system of claim 1, wherein the processor is further configured to execute the instructions to generate a path from the current pose to a pose at the first waypoint in response to selection of the first waypoint.

7. The processor: (1) a path length ratio between the direct path between the current pose and the pose at the first waypoint, and (2) 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; and The proximity of the nearest obstacle to the calculated path The waypoint selection system of claim 1 , further configured to execute the instructions for determining the first waypoint cost further based on:

8. Obtaining information about the current pose of the vehicle; acquiring a plurality of waypoints between the current pose and a destination pose of the vehicle; Obtaining obstacle information; determining a first waypoint cost for a first waypoint of the plurality of waypoints; selecting the first waypoint in response to the first waypoint cost satisfying a predetermined condition; in response to selection of the first waypoint, outputting the selected first waypoint and all waypoints of the plurality of waypoints between the first waypoint and the current pose to a controller; Including, determining the first waypoint cost based on a comparison of the number of direction changes along the calculated path between the current pose and a pose at the first waypoint and a Euclidean distance between the current pose and the pose at the first waypoint with a turning radius of the vehicle, wherein the direction changes include a switch from forward movement to reverse movement or from reverse movement to forward movement of the vehicle, but do not include a turn of the vehicle; Waypoint selection method.

9. a non-transitory computer-readable medium configured to store instructions; a processor coupled to the non-transitory computer-readable medium, the processor comprising: Get information about the vehicle's current pose, Get the destination pose, Obtain obstacle information, acquiring information about the surroundings of the vehicle; determining a plurality of waypoints between the current pose and the destination pose; determining a first waypoint cost for a 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 selection of the first waypoint; In response to the selection of the first waypoint, outputting the selected first waypoint and the generated route to a controller. configured to execute the instructions for the processor is further configured to execute the instructions to determine the first waypoint cost based on a number of direction changes along a calculated path between the current pose and a pose at the first waypoint and a comparison of a Euclidean distance between the current pose and the pose at the first waypoint and a turning radius of the vehicle, wherein the direction changes include switching from forward movement to reverse movement of the vehicle or from reverse movement to forward movement, and do not include turning of the vehicle; Vehicle control system.

10. a non-transitory computer-readable medium configured to store instructions; a processor coupled to the non-transitory computer-readable medium, the processor comprising: Get information about the vehicle's current pose, Get the destination pose, Obtain obstacle information, acquiring information about the surroundings of the vehicle; determining a plurality of waypoints between the current pose and the destination pose; determining a first waypoint cost for a 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 selection of the first waypoint; In response to selection of the first waypoint, outputting movement commands to the vehicle to move along the generated path. configured to execute the instructions for the processor is further configured to execute the instructions to determine the first waypoint cost based on a number of direction changes along a calculated path between the current pose and a pose at the first waypoint and a comparison of a Euclidean distance between the current pose and the pose at the first waypoint and a turning radius of the vehicle, wherein the direction changes include switching from forward movement to reverse movement of the vehicle or from reverse movement to forward movement, and do not include turning of the vehicle; controller.

Images