Track planning in three-dimensional search space with space-time artificial potential fields

Abstract

This document describes trajectory search in a three-dimensional (3D) search space with space-time artificial potential fields. An example system includes a processor that obtains an initial pose, a target pose, and an obstacle map of an environment. The processor uses a parking trajectory algorithm to determine a trajectory by searching in a space-time artificial potential field. The trajectory includes a series of 3D waypoints including two-dimensional (2D) position coordinates and time coordinates to navigate from an initial pose toward a target pose. Operations of the primary vehicle are then controlled to maneuver along the trajectory using either an aided driving system or an autonomous driving system. In this manner, the path search is performed throughout the 3D search space without iterating between two 2D search planes. This allows motion planning for autonomous parking to determine an optimal trajectory in a single iteration, in particular in the presence of stationary and dynamic objects.

Description

Background Art

[0001] Some vehicles provide autonomous or automatic parking to assist the driver. However, motion planning (e.g., a driving path for navigating in a parking environment to park in a specific space or respond to a summon request) is often computationally expensive due to the large search space and complex environment (e.g., a combination of moving and stationary objects). For example, motion planning may involve iterative use of both a path planner and a velocity planner to provide a collision-free trajectory in a complex parking environment, and making decisions (e.g., yielding, offsetting, overtaking) to handle static and dynamic objects. Therefore, the computational time to perform motion planning in many parking environments may be too long and not fully optimized for practical implementation. Summary of the invention

[0002] This document describes the technology and system for performing trajectory planning in a three-dimensional (3D) search space with a space-time artificial potential field. The example system includes a processor that obtains an initial pose (e.g., a source node), a target pose (e.g., a target node), and an obstacle map of a parking environment or a similar road. The processor can also obtain a reference path. The processor then uses a parking trajectory algorithm and an obstacle map (and a reference map, if provided) to determine the trajectory by searching in the space-time artificial potential field. The trajectory includes a series of 3D waypoints to navigate from the initial pose to the target pose. Each waypoint includes two-dimensional (2D) position information or coordinates and time information or coordinates. The processor uses an assisted driving system or an autonomous driving system to control the operation of the main vehicle to manipulate along the trajectory to the target pose. In this way, the example system performs a trajectory search in a 3D search space with a space-time artificial potential field without iterating between two 2D search planes (e.g., a position 2D plane and a time-distance plane). This allows motion planning for autonomous parking maneuvers to determine the optimal trajectory in a single iteration or a small number of iterations, particularly in parking environments that include both static and dynamic objects.

[0003] This document also describes methods performed by the systems summarized above and other configurations set forth herein, as well as computer-executable instructions and devices for performing these methods.

[0004] This summary introduces simplified concepts related to trajectory planning in a 3D search space with a space-time artificial potential field in the detailed description and drawings. This summary is not intended to identify essential features of the claimed subject matter, nor is it intended to determine the scope of the claimed subject matter. BRIEF DESCRIPTION OF THE DRAWINGS

[0005] Details of one or more aspects of trajectory planning in a 3D search space with a space-time artificial potential field are described in this document with reference to the following figures. The same numbers are generally used throughout the figures to refer to similar features and components: Figure 1 An example environment is shown in which a parking system performs trajectory planning in a 3D search space with a space-time artificial potential field in accordance with the techniques of this disclosure; Figure 2 An example configuration of a vehicle having a parking system that performs trajectory planning in a 3D search space with a space-time artificial potential field is shown; Figure 3 An example flow chart of the described techniques and systems for performing trajectory planning in a 3D search space with a space-time artificial potential field is shown; Figure 4 An example technique for determining a repulsive potential for trajectory planning in a 3D search space with a space-time artificial potential field is shown; Figure 5-1 to Figure 5-4 An example 3D search space for trajectory planning is shown; Figure 6-1 to Figure 6-4 An example space-time artificial potential field for trajectory planning is shown; Figure 7-1 to Figure 7-14 shows an example trajectory of a vehicle with a parking system using a parking trajectory algorithm in a 3D search space with a space-time artificial potential field; and Figure 8 An example method for a parking system to perform trajectory planning in a 3D search space with a space-time artificial potential field is shown. DETAILED DESCRIPTION Overview

[0006] Some vehicles provide autonomous or automatic parking and summoning functionality. Many autonomous or automatic parking systems use a parking path algorithm to generate a parking path (or summoning path) and a speed profile to navigate the host vehicle to a selected parking space (or respond to a summoning request). However, motion planning for autonomous parking and summoning is a complex task due to the relatively large search space around the selected parking space, non-holonomic constraints on vehicle motion (e.g., the vehicle cannot move directly sideways but must move forward or backward to achieve lateral movement), and avoidance of nearby obstacles (e.g., parked vehicles, moving vehicles, pedestrians, and other objects).

[0007] Some parking systems use an iterative combination of a path planner and a speed planner to generate a parking trajectory. In the path planner and at each search, these parking systems can predict the location of an object and perform a bias on it (e.g., provide a distance offset) to generate a 2D path. The speed planner then generates a speed profile based on a cost function associated with avoiding nearby objects and reaching a desired destination (e.g., a parking spot). Since the speed planner may change the speed profile of the host vehicle based on a new or updated 2D path, the predicted location of the object may also change, and the motion planner must determine another speed profile. In such parking systems, the influence and action of the path planner and the speed planner on each other result in an iterative path-speed algorithm. After a relatively large number of iterations, these parking systems may eventually reach convergence of the 2D path and speed profile.

[0008] In contrast, this document describes techniques and systems for trajectory planning in a single 3D search space with a space-time artificial potential field, without iterating between a path planner and a speed planner. For example, a parking system obtains an initial posture (e.g., a source node), a target posture (e.g., a target node), and an obstacle map of a parking environment. The parking system can also receive a reference path for a trajectory. The parking system uses a parking trajectory algorithm, an obstacle map, and a reference path (if provided) to determine a trajectory by searching in a space-time artificial potential field. The trajectory includes a series of 3D waypoints, including 2D position coordinates and time coordinates, to navigate the main vehicle from the initial posture toward the target posture. The main vehicle is then controlled to maneuver along the trajectory toward the target posture. In this way, the described techniques and systems perform trajectory search in the entire 3D search space without iterating between two 2D search planes (e.g., position 2D plane and time-distance plane). This allows motion planning for autonomous parking, particularly in a parking environment including static and dynamic objects, to determine the best trajectory in a single iteration or a relatively small number of iterations.

[0009] This is just one example of the described techniques and systems for trajectory planning in a 3D search space with a space-time artificial potential field. This document describes other examples and implementations. Operating Environment

[0010] Figure 1 An example environment 100 is shown in which a parking system 120 of a vehicle 102 (e.g., a host vehicle, an ego vehicle) performs trajectory planning in a 3D search space with a space-time artificial potential field in accordance with the techniques of this disclosure. In the depicted environment 100, the vehicle 102 is in a parking lot or other environment with multiple parking spaces. In other implementations, the environment 100 may be a road with parking spaces on one side of the road. Figure 1 100 is shown parallel to the path of travel of vehicle 102. Parking spaces may also be at an angle to the path of travel of vehicle 102, or may be perpendicular to the path of travel of vehicle 102. Environment 100 includes stationary objects 112 (e.g., other vehicles parked in some of the parking spaces, pillars, curbs, and barriers) and moving objects 114 (e.g., pedestrians and other vehicles navigating in environment 100). Environment 100 may also include available spaces 110 that are not occupied.

[0011] Although shown as a passenger truck, the vehicle 102 may represent other types of motor vehicles (e.g., cars, automobiles, motorcycles, buses, tractors, semi-trailers), watercraft (e.g., boats), or aircraft (e.g., airplanes). The vehicle 102 includes one or more sensors 118 and a parking system 120. In the depicted environment 100, the sensors 118 are mounted to the front, center, and rear of the vehicle 102, or integrated into the front, center, and rear of the vehicle 102. As described in more detail below, the sensors 118 may include camera systems, radar systems, lidar systems, ultrasonic systems, and positioning systems. The sensors 118 may provide sensor data about stationary objects 112 and moving objects 114 to the parking system 120 (e.g., as an obstacle map).

[0012] Additionally, parking system 120 or another component of vehicle 102 may use sensors 118 to obtain initial pose 104 and / or target pose 106 (e.g., parked in available space 110) of vehicle 102. Sensors 118 may also be used to generate an obstacle map of environment 100 including stationary objects 112 and moving objects 114.

[0013] In the depicted implementation, the sensor 118 is mounted on the front of the vehicle 102 and can provide sensor data for building an obstacle map. The sensor 118 can detect nearby objects or parking-space features from any exterior surface of the vehicle 102. For example, a vehicle manufacturer can integrate a radar system, a lidar system, a camera, or an ultrasonic sensor into a bumper, a side mirror, a headlight, or any other interior or exterior location where an object (e.g., a stationary object 112, a moving object 114) needs to be detected. In some cases, the vehicle 102 includes multiple sensors and / or sensor types (such as radar systems and cameras) that provide a larger instrument field of view or improved detection of nearby objects. In general, the vehicle manufacturer can design the position of the sensor 118 to provide a specific field of view that includes an area of ​​interest. Example fields of view include a 180-degree field of view, one or more 90-degree fields of view, etc., which can overlap or be combined into a field of view of a specific size.

[0014] The parking system 120 can provide assisted or autonomous driving to the driver of the vehicle 102. For example, the parking system 120 can identify a selected parking spot (e.g., the available space 110) and generate a trajectory 108 to navigate from the initial pose 104 to the target pose 106 proximate to the selected parking spot. In some implementations, the parking system 120 can then provide the parking path 116 to the assisted or autonomous driving system to park the vehicle 102 in the available space 110.

[0015] The parking system 120 may include a motion planner 122. The parking system 120 and the motion planner 122 may be implemented using hardware, software, firmware, or a combination thereof. The parking system 120 may also include a parking space selector that may identify the available space 110 and select the available space 110 or another parking space for the vehicle 102. In other implementations, the driver may provide input to the parking system 120 to select a desired parking space.

[0016] The motion planner 122 can determine a trajectory 108, which includes a position path plan and a velocity plan for navigating the vehicle 102 from the initial pose 104 to the target pose 106 (e.g., a position close to the available space 110) while avoiding collisions with stationary objects 112 and moving objects 114. The trajectory 108 can also include a path plan and / or velocity plan for navigating in the environment 100 to find the available space 110 or to leave the environment 100. The trajectory 108 includes a series of 3D waypoints between the initial pose 104 and the target pose 106, wherein each waypoint indicates 2D position information or coordinates and time information or coordinates. The motion planner 122 uses a space-time artificial potential field to plan a trajectory in a 3D search space without iterating between a path planner and a velocity planner in different 2D planes. For example, the motion planner 122 can use a variant of the A star (A*) or hybrid A* algorithm to determine the trajectory 108. In other implementations, the motion planner 122 may use Dijkstra, anytime A*, D*, D*Lite, or similar algorithms. Transportation Configuration

[0017] Figure 2 An example configuration of a vehicle 102 with a parking system 120 that performs trajectory planning in a 3D search space with a space-time artificial potential field is shown. Figure 1As described in , the vehicle 102 includes sensors 118 and a parking system 120, which may include a parking space selector 212, a global planner 214, a motion planner 122, and a parking planner 216. In addition, the vehicle 102 may include one or more communication devices 202, one or more processors 204, a computer readable storage medium (CRM) 206, and a control interface 218 to one or more vehicle-based systems (including one or more autonomous driving systems 220).

[0018] The communication device 202 may include a sensor interface and a vehicle-based system interface. For example, when the sensor 118 and / or various components of the parking system 120 are integrated into the vehicle 102, the sensor interface and the vehicle-based system interface may transmit data (e.g., radar data, distance calculations) via a communication bus of the vehicle 102.

[0019] The processor 204 (e.g., an energy processing unit or an electronic control unit) can be a microprocessor or a system on a chip. The processor 204 executes instructions stored in the CRM 206, one or more disks, memories, or other non-transitory computer-readable storage media. For example, the processor 204 can process sensor data from the sensor 118 and execute instructions loaded from the CRM 206 to cause them to generate an obstacle map of the parking environment, determine a trajectory 108 (the trajectory 108 can be referred to as a cruise trajectory) for driving toward a parking space, satisfying a summon request, or navigating in the parking environment. The instructions can cause the processor 204 to be configured to generate a trajectory 108 for at least one automotive system, the trajectory 108 including path planning and speed planning. For example, the processor 204 executes instructions on the CRM 206 to control the autonomous driving system 216 to operate the vehicle 102 using the trajectory 108 to approach a selected parking space based on the sensor data.

[0020] The parking system 120 may be stored in the CRM 206. As described above, the parking system 120 may include a parking space selector 212, a global planner 214, a motion planner 122, and a parking planner 216. The parking space selector 212 may identify available spaces or select a parking space (e.g., an optimal parking space) for the vehicle 102. The selected parking space may be presented to the driver of the vehicle 102 on a display (e.g., an overlay on a photo or video feed of the parking environment or a graphical representation of the parking environment). The parking space selector 212 may also determine nearby available spaces and present them to the driver of the vehicle 102 on a video display. The driver may then select a space in which the parking system 120 will park the vehicle 102.

[0021] The global planner 214 provides high-level motion planning for the parking system 120. For example, the global planner 214 can provide a reference path to the motion planner 122 (which can also be referred to as a local planner) for navigating to the vicinity of a selected parking space, satisfying a summon request, or leaving the parking environment. The reference path provides an ideal or suggested path plan from the initial pose 104 to the target pose 106. The global planner 214 typically generates the reference path using map data of the parking environment. The map data can be stored locally in the vehicle 102 or obtained from a remote computer system using a communication device. In other implementations, the reference path can be a stored path for a frequently visited parking environment (e.g., a learned or trained trajectory for parking in a specified location at the driver's home or work).

[0022] The motion planner 122 determines a trajectory 108 for navigating from the initial pose 104 to the target pose 106 in the parking environment. As described below, the motion planner 122 can determine a series of 3D waypoints to safely navigate in the parking environment (e.g., avoiding collisions with stationary objects 112 and moving objects 114). The 3D waypoints include 2D position coordinates and time coordinates. The heading of the vehicle can be determined by taking derivatives of the 2D position (e.g., dy / dx or dS / dL). The velocity component can be determined by taking derivatives of the position axis with respect to time (e.g., dx / dt, dy / dt, dS / dt, dL / dt).

[0023] The parking planner 216 determines a parking path (e.g., Figure 1 to navigate from the target pose 106 to the selected parking space (e.g., Figure 1 The parking planner 216 may also determine the type of maneuver (e.g., forward parking, reverse parking, single-turn maneuver, double-turn maneuver) to use to park in the selected parking space. For example, the parking planner 216 may determine based on the parking-space characteristics that the vehicle 102 cannot perform a single-turn maneuver to enter the selected parking space because the entry turn radius is less than the minimum inner turn radius of the vehicle 102. The parking planner 216 may also determine to perform a single-turn or double-turn maneuver based on driver preference or input, followed by a reverse move to park in the selected parking space.

[0024] The vehicle 102 also includes a control interface 218 to one or more vehicle-based systems that, alone or in combination, provide a means for receiving the trajectory 108 to control the vehicle 102. One example of a vehicle-based system to which the control interface 218 provides parking information includes an autonomous driving system 220, which can rely on information output from the parking system 120. For example, the autonomous driving system 220 can rely on data transmitted via the communication device 202 and obtained from the sensors 118 to operate the vehicle 102 along the trajectory 108 toward the target pose 106 in a crowded parking environment. For example, the autonomous driving system 220 can use the data provided by the parking system 120 and / or the sensors 118 to control the operation of the vehicle 102 to navigate close to a selected parking space (and then use the parking path 116 to park in the selected parking space), to satisfy a summon request, or to navigate in a parking environment. Example trajectory planning flow chart

[0025] Figure 3 An example flow chart 300 of the described techniques and systems for trajectory planning in a 3D search space with a space-time artificial potential field is shown. Figure 3 A parking system of the technology can be for example Figure 1 and Figure 2 The output of flowchart 300 is trajectory 306, which can be provided as input to autonomous driving system 220 to operate vehicle 102 along trajectory 306 to navigate in a parking environment (e.g., to approach an available parking space, find an available space, or exit a parking lot).

[0026] At step 304, the parking system 120 or the motion planner 122 obtains input 302 and runs the parking trajectory algorithm in a 3D search space with a space-time artificial potential field. The 3D search space includes two position dimensions and a time dimension. The input 302 includes an initial or current pose of the vehicle 102, a target pose near a destination within a selected parking space or parking environment, and an obstacle map. The initial pose can represent a source node of the parking trajectory algorithm and can be obtained from the positioning system 208, which uses position data to determine the position of the vehicle. The target pose can represent a target node of the parking trajectory algorithm and can be obtained from the parking space selector 212 or another system of the parking system 120. The parking system 120 can also obtain an obstacle map of the environment near, around, and including the initial pose and the target pose. The obstacle map can be obtained from the perception system 210, which uses sensor data to generate and populate the obstacle map. In some implementations, the obstacle map may be a radar occupancy grid map generated from radar data or a similar type of occupancy grid map (eg, an occupancy grid map that fuses data from multiple types of sensors).

[0027] The parking trajectory algorithm utilizes a space-time artificial potential field and a graph search based algorithm to plan the trajectory of the vehicle 102 in the parking environment 306. The parking trajectory algorithm first discretizes the 3D search space into a 3D node array and assigns an artificial potential field value or magnitude to each node using a potential field function. The parking trajectory algorithm determines a trajectory to travel from a current position (eg, initial pose) to a target position with the lowest cost or potential energy.

[0028] There are two common artificial potential fields generated within the parking trajectory algorithm: attractive potential and repulsive potential. In general, the target pose and reference path (if provided) exhibit attractive potential, while obstacles (e.g., moving objects 114, stationary objects 112, and boundaries) produce repulsive potential. The attractive or repulsive potential is a function of the distance between the vehicle 102 and the potential source (e.g., an inverse, linear, quadratic, or exponential relationship). Therefore, the total potential energy U at any point or node within the parking environment is 总 (s, l, t) is determined by the attractive potential energy U 吸引 (s, l, t) and repulsive potential energy U 排斥 The sum of (s, l, t) is determined as shown in equation (1): U 总 (s, l, t) = U 吸引 (s, l, t)+U 排斥 (s, l, t) (1)

[0029] The attractive potential can be generated based on the reference line, the reference path, the target line, and the target node. The reference line can represent the lateral center of the lane or implied lane in which the vehicle 102 is traveling. The reference path can represent the ideal path for the vehicle 102 to follow the marked lanes in the parking environment from the source node to the target node or target line, assuming that there are no obstacles. The target line can represent the lateral (or another direction) line to which the vehicle 102 is to travel to leave the parking environment or navigate the vehicle 102 in the parking environment to search for an available space 110. The attractive potential from the reference path and the target pose are shown in equations (2) and (3), respectively: U 参考路径 (s, l, t) = w 参考路径 d 路径 (2) U 目标 (s, l, t) = w 目标 d 目标 (3) Among them, w 参考路径 and w 目标 denotes weights with the same or different constant values, and d 路径 and d 目标 The Euclidean distance from the current position of the vehicle 102 to the target position or the minimum distance from the current position of the vehicle 102 to the reference path or the target line, respectively.

[0030] Figure 6-1 and Figure 6-2 Example attractive potential fields 600-1 and 600-2 are shown, respectively, for use by motion planner 122 in determining trajectory 306. Figure 6-1 6, graph 600-1 shows the attractive potential field generated by a reference path. The reference path is offset from the S axis. The magnitude of the attractive potential increases linearly based on the lateral (L) distance to the reference path or line.

[0031] exist Figure 6-2 6, graph 600-2 shows the attractive potential field generated by the target line. The target line is a line with an S value of 40. The magnitude of the attractive potential increases linearly based on the distance to the target line along the S axis.

[0032] Repulsive potential energy can be generated from boundaries and obstacles. If used, the repulsive potential energy of the boundary is useful for keeping the vehicle 102 away from the boundary of the parking environment. The repulsive potential energy from the obstacle is shown in equation (4): Among them, w 障碍物 represents a weight having a constant value, d represents the distance between the vehicle 102 and the obstacle, and d cRepresents the collision distance offset for small distances around obstacles, d g represents the gradient distance offset around the obstacle, and C 碰撞 and C 梯度 Represents the cost associated with being within the corresponding distance of the obstacle.

[0033] Figure 4 An example technique for determining repulsive potential in trajectory planning in a space-time field is shown. Specifically, Figure 4 A host vehicle 402 (eg, Figures 1 to 3 406). In the parking trajectory algorithm, the representation of the host vehicle 402 and the obstacle 404 is simplified by using one or more circles. In the example shown, the host vehicle 402 and the obstacle 404 are both represented by three circles 406.

[0034] The distance d, 408 from equation (4) can be determined using equation (5): where s 主 and 障碍物 s coordinates of the centers of the main vehicle 402 and the obstacle 404 respectively; l 主 and l 障碍物 The coordinates of the centers of the host vehicle 402 and the obstacle 404 are represented respectively; and 主 and r 障碍物 Denote the radii of the circle 406 representing the host vehicle 402 and the obstacle 404 , respectively.

[0035] In equations (1) to (5), the position coordinates are provided in station (S or s) and lateral (L or l) dimensions. The S dimension indicates the distance along the path, and the L dimension indicates the vertical offset from the path. Cartesian coordinates can also be used to represent a 2D search space in a global coordinate system or a vehicle coordinate system with an x-axis and a y-axis perpendicular to each other. Similarly, polar coordinates or another position coordinate system can be used for a 2D position search space in a space-time field.

[0036] Collision distance offset d c 416 represents the distance between obstacle boundary 410 and collision circle 412 of obstacle 404. Obstacle boundary 410 represents the approximate boundary of obstacle 404 as shown by circle 406. Collision circle 412 represents the area within which collision with obstacle 404 occurs or may occur. g418 represents the distance between the gradient circle 414 and the obstacle boundary 410. The gradient circle 414 represents the region within which the repulsive potential from the obstacle decreases linearly. In other implementations, the repulsive potential may decrease quadratically, exponentially, or by some other value within the gradient circle 414. The collision cost C 碰撞 420 and gradient cost C 梯度 422 represent the cost of the host vehicle 402 within the collision circle 412 or gradient circle 414 of the obstacle 404, respectively. For example, the collision cost C 碰撞 420 and gradient cost C 梯度 422 may have values ​​of 100 and 25, respectively. In other implementations, different values ​​may be used for the collision cost C 碰撞 420 and gradient cost C 梯度 422 both.

[0037] Figure 6-3 An example repulsive potential field 600-3 is shown that is used by the motion planner 122 to determine the trajectory 306. The graph 600-3 shows the repulsive potential field generated by nine parked vehicles in a parking environment. The magnitude of the repulsive potential field increases as the vehicle 102 approaches the corresponding object(s).

[0038] Figure 6-4 An example space-time artificial potential field 600-4 is shown that is used by the motion planner 122 to determine the trajectory 306 for a single point in time. Specifically, graph 600-4 shows a combination of attractive and repulsive potential energies or fields from graph 600-1, graph 600-2, and graph 600-3.

[0039] The result of the parking trajectory algorithm is trajectory 306. Trajectory 306 includes a series of 3D waypoints including 2D position coordinates and time coordinates to navigate vehicle 102 from an initial attitude toward a target attitude. The 2D position coordinates are represented by two position dimensions (e.g., in the SL plane). The slope of the position coordinates (e.g., dS / dL) indicates the heading of vehicle 102. The position provided by the position coordinates can be represented in a vehicle coordinate system or a global coordinate system. The time coordinate can be used to determine the velocity components of trajectory 306. Specifically, the longitudinal velocity component is indicated by the slope in the ST plane (e.g., dS / dT), and the lateral velocity component is indicated by the slope in the LT plane (e.g., dL / dT).

[0040] At optional step 308, the motion planner 122 optimizes the trajectory 306 to smooth out the path and speed of the vehicle 102. For example, the motion planner 122 may optimize the trajectory 306 using inequality constraints to reduce curvature, acceleration, or any jerkiness.

[0041] As another example, the motion planner 122 can introduce speed penalties to minimize situations where the vehicle 102 travels faster or slower than a reference speed. Speed ​​penalties can also include a maximum speed or a minimum speed that cannot be exceeded. Similarly, the motion planner 122 can also consider kinematic constraints (e.g., steering restrictions) and dynamic constraints (e.g., changes in road elevation that may occur, for example, on a ramp) to introduce other penalties to influence or optimize the trajectory 306. In other implementations, these penalties and constraints can be integrated as part of the parking trajectory algorithm in step 304.

[0042] At step 310, parking system 120 or autonomous driving system 220 executes a horizon waypoint from trajectory 306. The horizon waypoint represents a subset of trajectory 306. For example, the horizon waypoint may represent a two second period or execution time of trajectory 306. As a result, motion planner 122 provides the 3D waypoints required to execute two seconds of trajectory 306, and then replans the remainder of trajectory 306 to account for updated information (e.g., a change in the trajectory of mobile object 114 or the perception of a new object). The horizon waypoint may represent a different execution time (e.g., one second) or distance threshold (e.g., ten meters or ten percent of the total path). In this way, parking system 120 executes a back-off horizon approach by searching for a longer trajectory, but only executing a small portion of it, and then replanning.

[0043] At step 312, the parking system 120 determines whether the target pose has been reached. If not, the parking system 120 returns to step 304, and the motion planner 122 runs the parking trajectory algorithm in the 3D search space with the space-time artificial potential field to re-plan the rest of the trajectory 306. If the target pose has been reached, the motion planning ends.

[0044] Figure 5-1 to Figure 5-4 An example 3D search space 500 for trajectory planning is shown. Specifically, Figure 5-1 306 as an example three-dimensional (3D) search space occupied by obstacles and trajectories 306. Figure 3 3D search space 500 for environment and trajectory planning.

[0045] The 3D search space 500 adds a time (T) dimension to the traditional two-dimensional (2D) search space for trajectory planning. Figure 5-1 to Figure 5-4 As shown, the 2D position search space can be represented by a station (S) dimension indicating the distance along the path and a lateral (L) dimension indicating the vertical offset from the path. Cartesian coordinates can also represent a 2D position search space with an x-axis and a y-axis perpendicular to each other in a global coordinate system or a vehicle coordinate system. Similarly, polar coordinates can be used for a 2D position search space.

[0046] exist Figure 5-1 , the 3D search space 500 includes a longitudinal S axis, a lateral L axis, and a vertical T axis. Stationary objects (e.g., parked vehicles 504) appear as vertical lines relative to the SL plane because their positions are independent of time. If moving objects (e.g., oncoming vehicles 506 and pedestrians 502) are predicted to have uniform speeds, they can appear as straight lines in the space-time field 500. The slope of the line for a moving object represents its predicted speed. Trajectory 306 is shown as a B-spline curve with its 3D waypoints shown as circles.

[0047] exist Figure 5-2 , a 2D SL search space 500-1 is shown. The 2D SL search space 500-1 is a projection of the 3D search space 500 onto the SL plane. The position of the parked vehicle 504 remains as a point, which appears as a rectangle due to the perspective view. In contrast, the positions of the pedestrian 502 and the oncoming vehicle 506 appear as lines due to their predicted movement over time. The trajectory 306 indicates how the vehicle 102 deviates from the reference path 508 when turning to provide a larger offset between it and the oncoming vehicle 506 and the pedestrian 502.

[0048] exist Figure 5-3 , a 2D ST search space 500-2 is shown. The 2D ST search space 500-2 is a projection of the 3D search space 500 on the ST plane. The positions of the parked vehicles 504 are represented as vertical lines because their positions do not change over time. In contrast, the position of the pedestrian 502 is presented as a straight line with a positive slope because the pedestrian 502 is predicted to travel at a uniform speed in the same direction as the vehicle 102. When the oncoming vehicle 506 is expected to travel toward the vehicle 102 at a uniform speed and exceed the vehicle 102, its position is presented as a line with a negative slope. The trajectory 306 indicates the S position of the vehicle 102 over time. The vehicle 102 stops briefly before passing the pedestrian 502 to allow the oncoming vehicle 506 to pass, resulting in the slope of the trajectory 306 being vertical. The instantaneous longitudinal velocity of the vehicle 102 can be determined as the inverse of the instantaneous slope at any point along the trajectory 306.

[0049] exist Figure 5-4, a 2D LT search space 500-3 is shown. The 2D LT search space 500-3 is a projection of the 3D search space 500 onto the LT plane. The positions of the parked vehicles 504 are represented as vertical lines because their positions do not change over time. Similarly, the position of the pedestrian 502 is presented as a vertical line because the lateral position of the pedestrian is predicted to be constant over time (e.g., the pedestrian walks on a straight line with a uniform offset from the reference path 508). Similarly, the position of the oncoming vehicle 506 is presented as a vertical line (which looks like a straight line with a small negative slope due to perspective) because the lateral position of the vehicle is predicted to be constant. The trajectory 306 indicates the L position of the vehicle 102 over time as it turns to make room for the oncoming vehicle 506 and then turns in the opposite lateral direction to pass the pedestrian 502. The instantaneous lateral velocity of the vehicle 102 can be determined as the inverse of the instantaneous slope at any point along the trajectory 306.

[0050] Figure 7-1 to Figure 7-14 An example trajectory 306 of a vehicle having a parking system using a parking trajectory algorithm in a 3D search space with a space-time artificial potential field is shown. The trajectory 306 is shown using a graph 700 showing snapshots of the travel of the vehicle 102 in the parking environment. The graph 700 provides different snapshots of the 2DSL search space 500-1 over time as the vehicle 102 travels toward a target pose. The position of the vehicle 102 is represented using an S axis (e.g., longitudinally forward of the vehicle) and a lateral (L) axis perpendicular to the S axis. As shown in FIG. Figures 5-1 to 5-4 As shown, the environment includes a plurality of stationary objects, specifically nine parked vehicles 504. The parked vehicles 504 define a drivable path in which the vehicle 102 can navigate objects and travel. The environment also includes a number of moving objects, specifically pedestrians 502 and oncoming vehicles 506. The pedestrians 502 are moving slowly in the same direction as the vehicle 102, while the oncoming vehicles 506 are traveling toward the vehicle 102. The reference path 508 is provided to the motion planner 122 to provide additional attractive potential energy.

[0051] exist Figure 7-1 In graph 700-1 of FIG. 700-1 , vehicle 102 is traveling along path 508 in the positive S direction. Graph 700-1 is taken at the beginning of the trajectory, where T equals 0 seconds. Pedestrian 502 is walking in the same direction, but slightly to the right of vehicle 102. Oncoming vehicle 506 is approximately 40 meters in front of vehicle 102 and heading toward the vehicle. At this point, vehicle 102 is traveling at a set reference speed.

[0052] Figure 7-2Graph 702-1 shows the space-time artificial potential field of trajectory 306 used by motion planner 122 to determine the beginning (e.g., T=0 seconds) of trajectory 306. The lowest potential energy value is found along or near reference path 508, where pedestrian 502 causes a local maximum in the potential energy value near vehicle 102.

[0053] exist Figure 7-3 and Figure 7-5 In graphs 700-2 and 700-3 of FIG. 5 , where T equals 4 seconds and 8 seconds, respectively, vehicle 102 is now a short distance behind pedestrian 502. Because vehicle 102 cannot go around and pass pedestrian 502 without a predicted collision with oncoming vehicle 506, motion planner 122 slows vehicle 102 while remaining centered on reference path 508.

[0054] Figure 7-4 and Figure 7-6 Graphs 702-2 and 702-3 of FIG. 1 show the space-time artificial potential field at T equal to 4 seconds and 8 seconds, respectively, used by the motion planner 122 to determine the trajectory 306, which is Figure 7-3 and Figure 7-5 The lowest potential energy values ​​are found along or near the reference path 508, where the pedestrian 502 and oncoming vehicle 506 cause local maxima in the potential energy values ​​near the vehicle.

[0055] exist Figure 7-7 In the diagram 700-4 of FIG. 700-4 , where T equals 11 seconds, the vehicle 102 continues to remain a short distance behind the pedestrian 502. Because it is predicted that the vehicle 102 still cannot safely go around and pass the pedestrian 502, the motion planner 122 maintains a lower speed while staying on the center of the reference path 508.

[0056] Figure 7-8 Graph 702-4 of FIG. 702-4 shows the space-time artificial potential field at T equal to 11 seconds used by the motion planner 122 to determine the trajectory 306, which is Figure 7-7 The lowest potential energy value is found along or near the reference path 508 , where the pedestrian 502 and the oncoming vehicle 506 cause a local maximum in the potential energy value near the vehicle 506 .

[0057] exist Figure 7-9In graph 700-5 of FIG. 700-5 , where T equals 14 seconds, vehicle 102 remains a short distance behind pedestrian 502 as oncoming vehicle 506 passes vehicle 102. Vehicle 102 moves laterally to the right to provide a greater lateral distance between vehicle 102 and oncoming vehicle 506, while still maintaining a lower speed to avoid passing pedestrian 502.

[0058] Figure 7-10 Graph 702-5 of FIG. 702-5 shows the space-time artificial potential field at T equal to 14 seconds used by the motion planner 122 to determine the trajectory 306, which is Figure 7-9 The lowest potential energy value is found along or near the reference path 508 , where the pedestrian 502 causes a local maximum in front of and to the right of the vehicle 102 , while the local maximum caused by the oncoming vehicle 506 is now behind the vehicle 102 .

[0059] exist Figure 7-11 In Figure 700-6 of FIG. 1 , where T equals 18 seconds, vehicle 102 passes pedestrian 502. Once oncoming vehicle 506 has safely passed vehicle 102, motion planner 122 initiates a passing trajectory that includes a path to the left of pedestrian 502 within the drivable lane while also increasing its speed back to the reference speed. In this way, vehicle 102 safely passes pedestrian 502.

[0060] Figure 7-12 Graph 702-6 of FIG. 702-6 shows the space-time artificial potential field at T equal to 18 seconds used by the motion planner 122 to determine the trajectory 306, which is Figure 7-11 The lowest potential energy value is found along or near the reference path 508, where the pedestrian 502 causes a local maximum in front of and to the right of the vehicle 102, while the local maximum caused by the oncoming vehicle 506 is now behind the vehicle 102. The passing trajectory follows the maximum attractive potential energy around the pedestrian 502.

[0061] exist Figure 7-13 In Figure 700-7, where T equals 26 seconds, the vehicle 102 has safely navigated through a parking environment with multiple stationary and moving objects to reach its destination. The trajectory is shown as a B-spline 702, which is defined by a series of position waypoints in the 2D view. For example, the vehicle 102 can now initiate a parking maneuver to park in a selected parking space. As another example, the vehicle 102 can now leave the parking environment to begin traveling on the road.

[0062] Figure 7-14Graph 702-7 of FIG. 702-7 shows the space-time artificial potential field at T equal to 26 seconds used by the motion planner 122 to determine the trajectory 306, which is Figure 7-13 The lowest potential energy value is found along the target line. Example Method

[0063] Figure 8 An example method 800 for a parking system to perform trajectory planning in a 3D search space with a space-time artificial potential field is shown. The method 800 is shown as operations (or actions) being performed, but need not be limited to the order or combination of operations shown herein. In addition, any one or more operations may be repeated, combined, or reorganized to provide other methods. In the following discussion, reference may be made to Figure 1 Environment 100 and Figures 1 to 7-12 The entities detailed in the foregoing are referenced for purposes of example only. The technology is not limited to being performed by one entity or multiple entities.

[0064] At step 802, an initial pose and a target pose of the host vehicle are obtained. For example, the motion planner 122 may obtain the initial pose of the vehicle 102 from a vehicle state estimator. The initial pose may be determined using position data. The initial pose may represent a source node used in a parking trajectory algorithm or an algorithm based on a graph search. The motion planner 122 may obtain a target pose from a parking space selector 212 or a global planner 214. The target pose may represent a target node used in a parking trajectory algorithm and may indicate a location close to a selected parking space, a location along a road in an environment, or an exit of an environment. The target pose may be determined using position data, map data, or other sensor data (e.g., data from a camera system, a radar system, a lidar system, or an ultrasonic sensor system).

[0065] At step 804, an obstacle map of the environment including an initial pose and a target pose is obtained. For example, the motion planner 122 may obtain the obstacle map from a perception system. The obstacle map may be a radar occupancy grid map of the environment 100 or an occupancy grid map centered on a radar. Obstacles in the obstacle map may be represented by bounding boxes, circles, occupancy grids, free space polygons, or any combination thereof. The motion planner 122 may also obtain a reference path.

[0066] At step 806, a trajectory is determined by searching in a space-time artificial potential field. The trajectory includes a series of 3D waypoints including 2D position coordinates and time coordinates for navigating the host vehicle from an initial posture toward a target posture. For example, the motion planner 122 initially runs a parking trajectory algorithm by searching in a 3D search space with a space-time artificial potential field to obtain a series of 3D waypoints including 2D position coordinates (e.g., S and L coordinates) and time coordinates (e.g., T coordinates) for navigating the vehicle 102 from an initial posture toward a target posture. The 3D search space includes a longitudinal dimension, a lateral dimension perpendicular to the longitudinal dimension, and a time dimension.

[0067] The motion planner 122 may use various parking trajectory algorithms that are graph search based algorithms. For example, the parking trajectory algorithm may be a variant of a hybrid A*, A*, or Dijkstra algorithm for finding an optimal path from an initial pose to a target pose using nonholonomic constraints on the movement of the vehicle 102.

[0068] The parking trajectory or graph search-based algorithm uses a space-time artificial potential field to plan a trajectory 306 that avoids collisions with stationary objects 112 and moving objects 114 in the environment 100. The space-time artificial potential field includes a repulsive potential field and an attractive potential field. The corresponding repulsive potential field is a function of the distance between the vehicle 102 and the corresponding stationary object 112 or the corresponding moving object 114 (e.g., a linear, inverse, quadratic, or exponential relationship). The attractive potential field may include a target potential field, which is a function of the distance between the vehicle 102 and the target posture (e.g., a linear, inverse, quadratic, or exponential relationship). The attractive potential field may also include a reference path potential field, which is a function of the lateral distance or absolute distance between the vehicle 102 and the reference path (e.g., a linear, inverse, quadratic, or exponential relationship), where the lateral distance is perpendicular to the reference path.

[0069] The parking trajectory algorithm or graph search based algorithm may also consider or include speed potential to penalize speeds that deviate from a reference speed, exceed a maximum speed threshold, or are less than a minimum speed threshold in the parking trajectory 306. Similarly, a steering penalty may be used to penalize positions in the parking trajectory 306 that require a steering angle that exceeds a steering angle threshold. Trajectory optimization may also be performed on a series of 3D waypoints to smooth positions and speeds together and reduce curvature, acceleration, or jerkiness.

[0070] At step 808, the operation of the host vehicle is controlled using the assisted driving system or the autonomous driving system to maneuver along the trajectory to or toward the target pose. For example, the motion planner 122 can output the trajectory 306 to the autonomous driving system 220. The autonomous driving system 220 can then be used to control the vehicle 102 to navigate along the trajectory 306.

[0071] The motion planner 122 may also perform horizon planning to identify or select a first waypoint of the trajectory 306. The first waypoint includes a subset of 3D waypoints for the vehicle 102 to navigate from the initial attitude to the target attitude. The subset of waypoints may include position coordinates and time coordinates included in a predetermined operation time (e.g., two seconds) or a predetermined distance (e.g., five meters) of the vehicle 102 along the trajectory 306. In response to the autonomous driving system 220 completing the operation of the vehicle 102 along the first waypoint, the motion planner 122 may identify an intermediate attitude at the end of the first waypoint from the subset of position coordinates. The parking trajectory algorithm is then used to determine a second waypoint of the trajectory of the vehicle 102 navigating from the intermediate attitude to the target node. The autonomous driving system 220 may then control the operation of the vehicle 102 to maneuver toward the target attitude along the second waypoint. Example

[0072] In the following sections, examples are provided.

[0073] Example 1. A method, comprising: obtaining an initial posture and a target posture of a main vehicle; obtaining an obstacle map of an environment, the obstacle map comprising the initial posture and the target posture; using a parking trajectory algorithm and the obstacle map to determine a trajectory by searching in a space-time artificial potential field, the trajectory comprising a series of three-dimensional (3D) waypoints, the series of three-dimensional waypoints comprising two-dimensional (2D) position coordinates and time coordinates of the main vehicle to navigate from the initial posture toward the target posture; and using an assisted driving system or an autonomous driving system to control the operation of the main vehicle to maneuver along the trajectory toward the target posture.

[0074] Example 2. The method of Example 1, wherein the method further includes: selecting a first waypoint of a trajectory, the first waypoint including a subset of 3D waypoints for navigating the main vehicle from an initial posture toward a target posture; in response to the assisted driving system or the autonomous driving system completing the operation of the main vehicle along the first waypoint, identifying an intermediate posture located at the end of the first waypoint from the first waypoint; using a parking trajectory algorithm and an obstacle map to determine a second waypoint of the trajectory for navigating the main vehicle from the intermediate posture toward the target posture; and using the assisted driving system or the autonomous driving system to control the operation of the main vehicle along the second waypoint toward the target posture.

[0075] Example 3. The method of Example 2, wherein the subset of 3D waypoints includes position coordinates and time coordinates of predetermined operating times of the host vehicle along the trajectory.

[0076] Example 4. The method of any of the previous examples, wherein: the environment includes one or more stationary objects and one or more moving objects; and the series of 3D waypoints of the trajectory avoids collisions between the host vehicle and the one or more stationary objects and the one or more moving objects.

[0077] Example 5. The method of Example 4, wherein: the parking trajectory algorithm includes a graph search-based algorithm; and the graph search-based algorithm uses a space-time artificial potential field for each of one or more stationary objects and one or more moving objects to avoid collision with one or more stationary objects and one or more moving objects in the environment.

[0078] Example 6. The method of Example 5, wherein the artificial potential field includes a repulsive potential field and at least one attractive potential field; the corresponding repulsive potential field is a function of the distance between the main vehicle and the corresponding stationary object or the corresponding moving object; and at least one attractive potential field includes a target potential field, which is a function of the distance between the main vehicle and the target posture.

[0079] Example 7. The method of Example 6, wherein the at least one attraction potential field further comprises a reference path potential field, the reference path potential field being a function of a lateral distance between the host vehicle and the reference path, the lateral distance being perpendicular to the reference path.

[0080] Example 8. The method of any one of Examples 5 to 7, wherein the graph search-based algorithm further includes at least one of the following: a speed potential used to penalize a speed of the host vehicle in a trajectory that deviates from a reference speed, exceeds a maximum speed threshold, or is less than a minimum speed threshold; a steering penalty used to penalize position coordinates in a 3D waypoint that require a steering angle that exceeds a steering angle threshold.

[0081] Example 9. The method of any one of Examples 5 to 8, wherein the graph search-based algorithm comprises a variant of a hybrid A*, A*, Dijkstra algorithm for finding an optimal trajectory from an initial pose to a target pose.

[0082] Example 10. The method of any of the previous examples, wherein the parking trajectory algorithm searches in a three-dimensional search space, the three-dimensional search space comprising a longitudinal dimension, a lateral dimension perpendicular to the longitudinal dimension, and a time dimension.

[0083] Example 11. The method of any of the previous examples, wherein the method further comprises performing trajectory optimization on the series of 3D waypoints to smooth the series of position coordinates and velocities together.

[0084] Example 12. The method of any of the previous examples, wherein the target pose comprises a selected parking spot, a location near the selected parking spot, a location along a road in the environment, or an exit of the environment.

[0085] Example 13. A method of any of the previous examples, wherein the initial pose is generated by a vehicle state estimator using position data; the target pose is generated by a parking space finder using position data and other sensor data or map data; and the other sensor data includes data from at least one of a camera system, a radar system, a lidar system, or an ultrasonic sensor system.

[0086] Example 14. A system comprising one or more processors configured to perform the method of any of the preceding examples.

[0087] Example 15. A computer-readable storage medium comprising computer-executable instructions which, when executed, cause a processor to perform the method of any one of Examples 1 to 13. Conclusion

[0088] Although various embodiments of the present disclosure are described in the foregoing description and shown in the accompanying drawings, it should be understood that the present disclosure is not limited thereto, but can be implemented in various ways within the scope of the following claims. From the foregoing description, it will be apparent that various changes can be made without departing from the scope of the present disclosure defined by the following claims.

Claims

1. A method, comprising: Obtaining the initial attitude and target attitude of the main vehicle; Obtaining an obstacle map of an environment, wherein the obstacle map includes the initial posture and the target posture; determining, using a parking trajectory algorithm and the obstacle map, a trajectory by searching in a space-time artificial potential field, the trajectory comprising a series of three-dimensional (3D) waypoints comprising two-dimensional (2D) position coordinates and time coordinates of the host vehicle to navigate from the initial pose toward the target pose; as well as Operation of the host vehicle is controlled using an assisted driving system or an autonomous driving system to maneuver along the trajectory toward the target pose.

2. The method according to claim 1, characterized in that The method further comprises: selecting a first waypoint of the trajectory, the first waypoint comprising a subset of the 3D waypoints for navigating the host vehicle from the initial pose toward the target pose; in response to the assisted driving system or the autonomous driving system completing operation of the host vehicle along the first waypoint, identifying from the first waypoint an intermediate pose at a terminal end of the first waypoint; determining a second waypoint of the trajectory for navigating the host vehicle from the intermediate pose toward the target pose using the parking trajectory algorithm and the obstacle map; and Operation of the host vehicle is controlled using the assisted driving system or the autonomous driving system to maneuver along the second waypoint toward the target attitude.

3. The method according to claim 2, characterized in that The subset of the 3D waypoints includes position coordinates and time coordinates of predetermined operating times of the host vehicle along the trajectory.

4. The method according to claim 1, characterized in that: The environment includes one or more stationary objects and one or more moving objects; and The series of 3D waypoints of the trajectory avoids collisions between the host vehicle and the one or more stationary objects and the one or more moving objects.

5. The method according to claim 4, characterized in that: The parking trajectory algorithm includes an algorithm based on a graph search; and The graph search based algorithm uses a space-time artificial potential field for each of the one or more stationary objects and the one or more moving objects to avoid collisions with the one or more stationary objects and the one or more moving objects in the environment.

6. The method according to claim 5, characterized in that: The artificial potential field includes a repulsive potential field and at least one attractive potential field; The respective repulsive potential field is a function of a distance between the host vehicle and the respective stationary object or the respective moving object; and The at least one attractive potential field includes a target potential field that is a function of a distance between the host vehicle and the target pose.

7. The method according to claim 6, characterized in that The at least one attractive potential field further includes a reference path potential field that is a function of a lateral distance between the host vehicle and a reference path, the lateral distance being perpendicular to the reference path.

8. The method according to claim 5, characterized in that The graph search-based algorithm further includes at least one of the following: a speed potential for penalizing a speed of the host vehicle in the trajectory that deviates from a reference speed, exceeds a maximum speed threshold, or is less than a minimum speed threshold; or A steering penalty is used to penalize the position coordinates of the 3D waypoints that require a steering angle exceeding a steering angle threshold.

9. The method according to claim 5, characterized in that The graph search based algorithm includes a variant of a hybrid A*, A*, Dijkstra algorithm for finding the best trajectory from the initial pose to the target pose.

10. The method according to claim 1, characterized in that The parking trajectory algorithm searches in a three-dimensional search space, which includes a longitudinal dimension, a lateral dimension perpendicular to the longitudinal dimension, and a time dimension.

11. The method according to claim 1, characterized in that The method further includes performing trajectory optimization on the series of 3D waypoints to smooth position coordinates and velocity together.

12. The method according to claim 1, characterized in that The target pose includes a selected parking spot, a location near the selected parking spot, a location along a road in the environment, or an exit of the environment.

13. The method of claim 1, wherein: The initial pose is generated by a vehicle state estimator using position data; The target pose is generated by a parking space finder using the position data and other sensor data or map data; and The other sensor data includes data from at least one of a camera system, a radar system, a lidar system, or an ultrasonic sensor system.

14. A system comprising one or more processors, the one or more processors being configured to: Obtaining the initial attitude and target attitude of the main vehicle; Obtaining an obstacle map of an environment, wherein the obstacle map includes the initial posture and the target posture; determining, using a parking trajectory algorithm and the obstacle map, a trajectory by searching in a space-time artificial potential field, the trajectory comprising a series of three-dimensional (3D) waypoints comprising two-dimensional (2D) position coordinates and time coordinates of the host vehicle to navigate from the initial pose toward the target pose; as well as Operation of the host vehicle is controlled using an assisted driving system or an autonomous driving system to maneuver along the trajectory toward the target pose.

15. The system of claim 14, wherein: The one or more processors are further configured to: selecting a first waypoint of the trajectory, the first waypoint comprising a subset of the 3D waypoints for navigating the host vehicle from the initial pose toward the target pose; in response to the assisted driving system or the autonomous driving system completing operation of the host vehicle along the first waypoint, identifying from the first waypoint an intermediate pose at a terminal end of the first waypoint; determining a second waypoint of the trajectory for navigating the host vehicle from the intermediate pose to the target pose using the parking trajectory algorithm and the obstacle map; as well as Operation of the host vehicle is controlled using the assisted driving system or the autonomous driving system to maneuver along the second waypoint toward the target attitude.

16. The system of claim 15, wherein: The subset of 3D waypoints includes position coordinates and time coordinates of predetermined operating times of the host vehicle along the trajectory.

17. The system of claim 14, wherein: The parking trajectory algorithm searches in a three-dimensional search space, which includes a longitudinal dimension, a lateral dimension perpendicular to the longitudinal dimension, and a time dimension.

18. The system of claim 14, wherein: The environment includes one or more stationary objects and one or more moving objects; the series of 3D waypoints of the trajectory avoiding collisions between the host vehicle and the one or more stationary objects and the one or more moving objects; The parking trajectory algorithm includes an algorithm based on a graph search; and The graph search based algorithm uses a space-time artificial potential field for each of the one or more stationary objects and the one or more moving objects to avoid collisions with the one or more stationary objects and the one or more moving objects in the environment.

19. The system of claim 18, wherein: The artificial potential field includes a repulsive potential field and at least one attractive potential field; The respective repulsive potential field is a function of a distance between the host vehicle and the respective stationary object or the respective moving object; and The at least one attractive potential field includes a target potential field that is a function of a distance between the host vehicle and the target pose.

20. A computer-readable storage medium comprising computer-executable instructions which, when executed, cause a processor to: Obtaining the initial attitude and target attitude of the main vehicle; Obtaining an obstacle map of an environment, wherein the obstacle map includes the initial posture and the target posture; determining a trajectory by searching in a space-time artificial potential field using a parking trajectory algorithm and the obstacle map, the trajectory comprising a series of three-dimensional (3D) waypoints comprising two-dimensional (2D) position coordinates and time coordinates of the host vehicle to navigate from the initial pose toward the target pose; and Operation of the host vehicle is controlled using an assisted driving system or an autonomous driving system to maneuver along the trajectory toward the target pose.

Images