Path planning and operational domain navigation with GNSS error prediction

Abstract

The invention provides path planning and operational domain navigation with GNSS error prediction. A computer includes a processor and a memory including instructions executable by the processor to receive global navigation satellite system (GNSS) error predictions corresponding to respective potential paths between a vehicle location and a specified destination. The memory includes instructions to identify a lowest cost path from the potential paths based on path costs determined from respective GNSS error predictions for the potential paths. A propulsion subsystem and / or a steering subsystem of the vehicle is controlled to operate the vehicle along the lowest cost path.

Description

Technical Field

[0001] This disclosure relates to path planning and operational domain navigation utilizing global navigation satellite system error prediction. Background Art

[0002] Computers can be used to operate systems including vehicles, robots, unmanned aerial vehicles, and / or object tracking systems. Data can be acquired by sensors and processed using a computer to determine the position of the system relative to objects in the environment surrounding the system. The computer can use the position to determine a trajectory for moving the system in the environment. The computer can then determine control data to be transmitted to system components to control the components to move according to the determined trajectory. Summary of the Invention

[0003] Robots and / or vehicles can use GNSS (Global Navigation Satellite System) as an input for navigation, path planning, and determining which computer-controlled vehicle operations (such as advanced driver assistance system (ADAS) features), if any, are available. Systems (including vehicles, robots, unmanned aerial vehicles, etc.) can operate by acquiring sensor data (including GNSS data) about the environment surrounding the system and processing the sensor data to determine a path on which to operate the system or a part of the system. The sensor data can be processed to determine the pose of the system, where "pose" specifies the position and orientation of an object (such as the system and / or its components). The system pose can be determined based on a six-degree-of-freedom (DoF) pose, which includes x, y, and z position coordinates, and roll, pitch, and yaw rotation coordinates relative to the x, y, and z axes, respectively. The six DoF pose can be determined relative to a global coordinate system such as a Cartesian coordinate system, where points can be specified according to latitude, longitude, and altitude or some other x, y, and z axes.

[0004] GNSS data typically can provide pose resolution ranging from less than 0.3 meters to over 3.0 meters. The availability of ADAS features depends on the available pose resolution. ADAS, which includes various driver assistance technologies (DAT), are computer-implemented or controlled features that assist vehicle driving and parking operations. Examples of ADAS include forward proximity detection, lane departure detection, blind spot detection, brake actuation, adaptive cruise control, lane keeping assistance, speed control, and / or steering control. With better pose resolution, additional and / or enhanced ADAS features can be provided.

[0005] The techniques described herein consider GNSS error prediction to select a path with the lowest overall GNSS error from alternative potential paths, thereby maximizing the availability of computer-controlled vehicle operations. A cost function is used to calculate the cost of each potential path based on the corresponding GNSS error prediction, and the potential path with the lowest cost is selected.

[0006] This disclosure provides a system that includes a computer having a processor and a memory. The memory includes instructions executable by the processor to receive GNSS error predictions corresponding to respective potential paths between a vehicle location and a specified destination. The instructions can include identifying a lowest-cost path from the potential paths based on a path cost determined from the respective GNSS error predictions of the potential paths, and controlling a propulsion subsystem and / or a steering subsystem of the vehicle to operate the vehicle along the lowest-cost path.

[0007] Instructions for calculating the path cost can include instructions for calculating an operating domain cost, a function cost, and a margin cost. The instructions can further include summing at least the calculated operating domain cost, the calculated function cost, and the calculated margin cost.

[0008] Instructions for calculating the operating domain cost can include instructions for dividing a potential path into a plurality of segments and multiplying the distance of each segment by a domain weight and the GNSS error prediction corresponding to the segment to determine an operating weighted distance. The instructions can further include summing the operating weighted distances of the plurality of segments.

[0009] Instructions for calculating the function cost can include instructions for dividing a potential path into a plurality of segments and determining for each segment which one of a plurality of predetermined ranges the GNSS error prediction corresponding to the segment lies within. Multiplying the distance of each segment by a weighting factor corresponding to the determined range to calculate a function weighted distance, and summing the function weighted distances of the plurality of segments.

[0010] Instructions for calculating the margin cost can include instructions for dividing a potential path into a plurality of segments and multiplying the distance of each segment by a margin factor calculated based on the GNSS error prediction corresponding to the segment to determine a margin weighted distance. The instructions further include summing the margin weighted distances of the plurality of segments.

[0011] Instructions for calculating the margin factor can include instructions for calculating the probability that a GNSS error distance will exceed a selected distance threshold based on the GNSS error prediction; dividing the probability by a desired probability to determine a probability weight; and raising the probability weight to a selected power.

[0012] Instructions for calculating the probability can include instructions for integrating a normal distribution function using the GNSS error prediction as a standard deviation.

[0013] Instructions for calculating functional cost and margin cost may include instructions for dividing a potential path into multiple segments. The instructions include: for each segment, determining which of a plurality of predetermined ranges the GNSS error prediction corresponding to the segment lies in; and multiplying the distance of each segment by a weighting factor corresponding to the determined range to determine a functional weighted distance. The instructions further include: multiplying the distance of each segment by a margin factor calculated based on the GNSS error prediction corresponding to the segment to determine a margin weighted distance, and summing the functional weighted distances and the margin weighted distances of the multiple segments.

[0014] Instructions for calculating path cost may include instructions for calculating the cost based at least on the potential path distance and the vehicle speed. The instructions may further include instructions for determining the GNSS error prediction based on historical data at a specific location and time.

[0015] Disclosed herein is a method for vehicle navigation path planning, the method including receiving a GNSS error prediction corresponding to a respective potential path between a vehicle location and a designated destination. The method includes identifying a lowest cost path from the potential paths based on a path cost determined according to the respective GNSS error prediction of the potential paths, and controlling a propulsion subsystem and / or a steering subsystem of the vehicle to operate the vehicle along the lowest cost path.

[0016] Calculating the path cost may include: calculating an operating domain cost, a functional cost, and a margin cost; and summing at least the calculated operating domain cost, the calculated functional cost, and the calculated margin cost.

[0017] Calculating the operating domain cost may include: dividing the potential path into multiple segments and multiplying the distance of each segment by a domain weight and the GNSS error prediction corresponding to the segment to determine an operating weighted distance. The method may include summing the operating weighted distances of the multiple segments.

[0018] Calculating the functional cost may include dividing the potential path into multiple segments. The method may include: for each segment, determining which of a plurality of predetermined ranges the GNSS error prediction corresponding to the segment lies in; and multiplying the distance of each segment by a weighting factor corresponding to the determined range to calculate a functional weighted distance. The method may include summing the functional weighted distances of the multiple segments.

[0019] Calculating the margin cost may include: dividing the potential path into multiple segments and multiplying the distance of each segment by a margin factor calculated based on the GNSS error prediction corresponding to the segment to determine a margin weighted distance. The method may include summing the margin weighted distances of the multiple segments.

[0020] Calculating a margin factor can include: calculating a probability that a GNSS error distance will exceed a selected distance threshold based on GNSS error prediction; dividing the probability by an expected probability to determine a probability weight; and raising the probability weight to a selected power.

[0021] Calculating the probability can include integrating a normal distribution function using the GNSS error prediction as a standard deviation.

[0022] Calculating a functional cost and a margin cost can include dividing a potential path into a plurality of segments. The method can further include: determining, for each segment, which of a plurality of predetermined ranges a GNSS error prediction corresponding to the segment is in; and multiplying a distance of each segment by a weighting factor corresponding to the determined range to determine a functional weighted distance. The distance of each segment can be multiplied by a margin factor calculated based on the GNSS error prediction corresponding to the segment to determine a margin weighted distance. The method can include summing the functional weighted distances and the margin weighted distances of the plurality of segments.

[0023] Calculating a path cost can include calculating a cost based at least on a potential path distance and a vehicle speed. The method can further include determining a GNSS error prediction based on historical data at a specific location and time. BRIEF DESCRIPTION OF THE DRAWINGS

[0024] Figure 1 is a block diagram of an example vehicle sensing system.

[0025] Figure 2 is an illustration of an example map having a plurality of potential paths between a vehicle location and a designated destination.

[0026] Figure 3 is an illustration of an example potential path including a plurality of segments between a vehicle location and a designated destination.

[0027] Figure 4 is a block diagram of an example path planning system using a cost function based on GNSS error.

[0028] Figure 5 is a flowchart of an example process for identifying a lowest cost path based on GNSS error prediction. DETAILED DESCRIPTION

[0029] Figure 1It is an illustration of an example system 100. System 100 includes a vehicle 110 that can be operated by a user and / or under the control of a computing device 115, which may include one or more vehicle electronic control units (ECUs) or computers known as such, and the ECU or computer may include additional hardware, software, and / or programming as described herein. The computing device 115 can receive data regarding the operation of the vehicle 110 from sensors 116. Instead of or in combination with the control of a human user, the computing device 115 can operate the vehicle 110 or its components. System 100 may also include a remote (i.e., external to the vehicle) server computer 120 that can communicate with the vehicle 110 via a network 130.

[0030] The computing device 115 can include one or more processors and one or more memory devices known as such. In addition, the memory includes one or more forms of computer-readable media and stores instructions that can be executed by the processor to perform various operations (including the operations disclosed herein). For example, the computing device 115 can include programming to operate one or more of vehicle braking, propulsion (e.g., controlling the acceleration of the vehicle 110 by controlling one or more of an internal combustion engine, an electric motor, a hybrid engine, etc.), steering, climate control, interior lights, and / or exterior lights, etc., and to determine whether and when the computing device 115 (rather than a human operator) controls such operations.

[0031] The computing device 115 can include more than one computing device, for example, controllers, ECUs, etc. included in the vehicle 110 for monitoring and / or controlling various vehicle subsystems (e.g., a propulsion subsystem 112, a braking subsystem 113, a steering subsystem 114, etc.), or, for example, communicatively coupled to the more than one computing device via a vehicle communication bus as further described below. The computing device 115 is generally arranged to communicate on a vehicle communication network (e.g., including a bus in the vehicle 110, such as a Controller Area Network (CAN), etc.). The vehicle network can additionally or alternatively include known wired or wireless communication mechanisms, such as Ethernet or other communication protocols.

[0032] The computing device 115 can transmit messages to and / or receive messages from various devices in the vehicle (e.g., controllers, actuators, sensors (including sensors 116), etc.) via the vehicle network. Alternatively or additionally, in the case where the computing device 115 actually includes multiple devices, the vehicle communication network can be used for communication between the devices represented as the computing device 115 in this disclosure. In addition, as mentioned below, various controllers or sensing elements (such as sensors 116) can provide data to the computing device 115 via the vehicle communication network.

[0033] Additionally, the computing device 115 may be configured to communicate with a remote server computer 120 (such as a cloud server) via a vehicle-to-infrastructure (V2I) interface over the network 130, as described below, the interface including hardware, firmware, and software that permit the computing device 115 to communicate with the remote server computer 120 over a network 130 such as a wireless Internet or a cellular network. The V2X interface 111 may thus include a processor, memory, transceiver, etc. configured to utilize various wired and / or wireless networking technologies (e.g., cellular, Bluetooth Low Energy (BLE), Ultra-Wideband (UWB), peer-to-peer communication, UWB-based radar, IEEE 802.11, and / or other wired and / or wireless packet networks or technologies). The computing device 115 may be configured to communicate with other vehicles using, for example, a vehicle-to-vehicle (V2V) network formed on the basis of a mobile ad-hoc network among nearby vehicles 110 or formed through an infrastructure-based network via the V2X (vehicle-to-everything) interface 111 (e.g., according to wireless communication including cellular communication (C-V2X) cellular, dedicated short-range communication (DSRC), and / or similar communication). The computing device 115 also includes non-volatile memory such as known. The computing device 115 may record data by storing it in the non-volatile memory for later retrieval and transmission to the server computer 120 or the user mobile device via the vehicle communication network and the vehicle-to-infrastructure (V2X) interface 111.

[0034] As already mentioned, the instructions stored in the memory and executable by the processor of the computing device 115 typically include programming for operating one or more vehicle 110 components (e.g., braking, steering, propulsion, etc.). Using the data received in the computing device 115 (e.g., sensor data from the sensors 116, the server computer 120, etc.), the computing device 115 may make various determinations and / or control various vehicle 110 components and / or operations. For example, the computing device 115 may include programming to control vehicle 110 operating behaviors (e.g., the physical manifestations of the vehicle 110 operation), such as speed, acceleration, deceleration, steering, etc., as well as strategic behaviors (e.g., generally controlling the operating behavior in a manner intended to achieve an efficient traversal of a route), such as the distance between vehicles and / or the amount of time between vehicles, lane changes, the minimum gap between vehicles, the left-turn across-path minimum, the time to arrival at a specific location, and the shortest time from arrival to crossing an intersection (without traffic lights).

[0035] Each of subsystems 112, 113, 114 may include a respective processor and memory and / or one or more actuators. Subsystems 112, 113, 114 may be programmed and connected to a vehicle 110 communication bus, such as a Controller Area Network (CAN) bus or a Local Interconnect Network (LIN) bus, to receive instructions from a computing device 115 and control the actuators based on the instructions.

[0036] The sensor 116 may include a variety of known devices to provide data via the vehicle communication bus. For example, a radar fixed to the front bumper (not shown) of the vehicle 110 may provide the distance from the vehicle 110 to the next vehicle in front of the vehicle 110, or a GNSS sensor disposed in the vehicle 110 may provide the geographical coordinates of the vehicle 110. The distance provided by the radar and / or other sensors 116 and / or the geographical coordinates provided by the GNSS sensor may be used by the computing device 115 to operate the vehicle 110.

[0037] The vehicle 110 is generally a land-based vehicle 110 having three or more wheels, for example, a passenger vehicle, a light truck, etc. The vehicle 110 includes one or more sensors 116, a V2X interface 111, a computing device 115, and one or more subsystems 112, 113, 114. The sensors 116 may collect data related to the vehicle 110 and the operating environment of the vehicle 110. By way of example and not limitation, the sensors 116 may include, for example, an altimeter, a camera, lidar, radar, ultrasonic sensors, infrared sensors, pressure sensors, accelerometers, gyroscopes, temperature sensors, pressure sensors, Hall sensors, optical sensors, voltage sensors, current sensors, mechanical sensors (such as switches), etc. The sensors 116 may be used to sense the operating environment of the vehicle 110. For example, the sensors 116 may detect phenomena such as weather conditions (rainfall, external ambient temperature, etc.), road grade, road position (e.g., using road edges, lane markings, etc.), or the position of a target object (such as a neighboring vehicle). The sensors 116 may also be used to collect data, including dynamic vehicle data related to the operation of the vehicle 110, such as speed, yaw rate, steering angle, engine speed, brake pressure, oil pressure, power levels applied to the subsystems 112, 113, 114 in the vehicle 110, connectivity between components, and the accurate and timely performance of the components of the vehicle 110.

[0038] The server computer 120 generally has features in common with the V2X interface 111 and the computing device 115 of the vehicle 110, such as a computer processor and memory and a configuration for communicating via a network 130, and thus these features will not be described further. The server computer 120 may be used to develop and train software that can be transmitted to the computing device 115 in the vehicle 110.

[0039] Figure 2 FIG. 200 is an illustration of a map showing multiple potential paths 206 and 208 between a vehicle 110 location 202 and a selected destination 204. In an example, the image 200 can be a map downloaded to a computing device 115 in the vehicle 110 via a network 130 from a source such as GOOGLE TM Maps. The computer 115 can use any suitable algorithm for global motion planning to select potential paths, e.g., an incremental search-based planner such as Rapidly-exploring Random Trees (RRT), a graph-based planning method using road structure, a sampling-based method such as Probabilistic RoadMap (PRM) planning, etc. Although two potential paths are shown in the depicted example, many additional potential paths can be determined for evaluation using the disclosed techniques.

[0040] Reference Figure 3 , each potential path (e.g., path 206) can be segmented as part of a lowest cost analysis. For example, the path can be segmented based on a distance threshold of 100 meters to 200 meters. The threshold distance can be decreased or increased according to road characteristics such as curvature. If the path includes a curve, the segment distance can be shortened for the curved portion of the path. Straight flat segments along the path can have an increased segment length. Additionally, the path can be segmented at each intersection. In some examples, a segment can include multiple lanes. Each segment lane can be evaluated separately to facilitate lane-level path planning. Each of segments 1 to 12 has a corresponding distance D1 - D12 and a corresponding GNSS error prediction E1 to E12. Each lane within a segment can have a corresponding lane-level GNSS error prediction. The distances D1 to D12 can be in meters, and the GNSS error predictions E1 to E12 can be in meters or provided as a standard deviation value σ. In an example, the GNSS error prediction can be an average error value for the corresponding segment.

[0041] The GNSS error prediction can be based on historical crowdsourced GNSS error data from vehicles that have traveled in the area of interest. The GNSS error can be determined by comparing Real-Time Kinematic (RTK) with GNSS information. RTK receives a normal signal from GNSS as well as a correction stream to determine a position with, for example, 1 centimeter (cm) position accuracy. As vehicles with RTK combined GNSS capabilities travel on various roads, the determined GNSS errors can be continuously stored as a GNSS error map to the cloud, where, for example, the errors regarding position and time are stored.

[0042] Figure 4FIG. 0 is a block diagram of an example path planning system 400 that uses a GNSS error-based cost function 404. The GNSS error-based cost function 404 receives a GNSS error prediction 402 and provides the cost of each path to a conventional route planning and path planning system 406. For example, the planning system 406 can select a potential path with the lowest cost and / or incorporate the path cost into an additional evaluation and planning layer. For example, a lane-level layer can plan a path from A to B on a discrete (lane node-level) representation of a map. A behavior layer can contain policy logic to make decisions such as lane changes, overtaking, merging, intersection handling, etc. A trajectory generator layer can generate a trajectory from the selected path, which can include position, orientation, speed, angular speed, and acceleration with respect to time. A motion control layer can calculate brake, throttle, and steering commands based on the provided trajectory.

[0043] The GNSS error-based cost function 404 includes a shortest distance cost factor 410, an operating domain cost factor 412, a functional cost factor 414, a margin cost factor 416, and a general cost factor 418. The shortest distance cost factor 410 is the distance of each section. Other general cost factors 418 can include conventional factors such as speed, travel time, minimizing traffic lights, etc. The cost of each path can be calculated by summing these factors for each potential path according to Equation 1.

[0044] Cost of each path = (path distance) + (GNSS error operating domain cost)

[0045] + (GNSS error functional cost and margin cost) + (other general cost factors) (1)

[0046] The GNSS error for the operating domain cost factor 412 can be calculated for each potential path by multiplying the distance of each section by a domain weight and the GNSS error prediction corresponding to the section to determine an operationally weighted distance and summing the operationally weighted distances of multiple sections according to Equation 2. In an example, if the GNSS error of a section exceeds a threshold (e.g., 7 meters), the domain weight is set to a number greater than 1, such as 3; otherwise, the domain weight can be set to 1. The threshold can be selected based on the resolution required for computer-controlled vehicle operation. The domain weight can be empirically selected based on the importance of having available computer-controlled vehicle operation.

[0047]

[0048] The GNSS error functional cost factor 414 is calculated by determining, for each road segment, which of a plurality of predetermined ranges the GNSS error corresponding to the road segment falls within and multiplying the distance of each road segment by a weighting factor corresponding to the determined range to calculate a functionally weighted distance. For example, if the GNSS error prediction for a given road segment is between 0.3 meters and 1.5 meters, the weighting factor is set to 1; if the GNSS error prediction for the road segment is greater than one meter, the weighting factor is set to 3; if the GNSS error prediction for the road segment is greater than five meters, the weighting factor is set to 10. The ranges can be selected based on different required resolutions of, for example, ADAS features, and the weighting factors can be determined empirically, for example. The GNSS error functional cost for each path can be calculated by summing the functionally weighted distances of the road segments according to Equation 3.

[0049] GNSS error functional cost = ∑(road segment distance * weight) (3)

[0050] The GNSS error range or a range such as the GNSS error range can also be used to determine, for example, the positioning level of an ADAS function. For example, an error less than 0.3 meters can achieve an "in-lane" positioning feature; an error between 0.3 meters and 1.5 meters can achieve a "which lane" positioning feature; an error between 1.5 meters and 5 meters can achieve a "which road" positioning level feature. As an example, if the "in-lane" positioning feature is available, the system can be allowed to perform more computer-controlled vehicle operations compared to, for example, road-level positioning where features such as lane changes or lane determination are not allowed.

[0051] The GNSS error margin cost factor 418 can be calculated by multiplying the distance of each road segment by a margin factor calculated based on the GNSS error prediction corresponding to the road segment to determine a margin-weighted distance and summing the margin-weighted distances of the plurality of road segments according to Equation 4.

[0052] GNSS error margin cost = ∑(road segment distance * margin factor) (4)

[0053] Calculating the margin factor (Equation 5) can include: calculating the probability f(x) that the GNSS error distance will exceed a selected distance threshold x based on the GNSS error prediction σ and dividing the probability f(x) by the desired probability to determine a probability weight. The desired probability can be the confidence level that the GNSS error distance will fall below the operational limit corresponding to the point below which the computer can control vehicle operations. The probability weight can be raised to a selected scaling factor power (e.g., 1 / 3 or 1 / 4) in order to adjust the importance of the GNSS error margin cost in the overall cost function.

[0054] Margin factor = (probability / desired probability) 缩放因子 (5)

[0055] In the example, the expected probability can be 10 -8 , which is the occurrence rate that appears 1 time, for example, in 10 8 . This corresponds to 5.73σ. The selected distance threshold x can be an operating limit, for example, 0.57 meters. The occurrence of an error distance above this operating limit may be considered too large for certain functions. This translates to 5.73σ = 0.57, so σ covariance = 0.57 / 5.73 = 0.0994.

[0056] Calculating the probability can include integrating the normal distribution function (Equation 6) using the GNSS error prediction corresponding to the road segment as the standard deviation σ, where the selected distance threshold is x = 0.57 meters and the mean μ = 0, assuming a normal distribution.

[0057]

[0058] In the example, the GNSS error prediction for an example road segment (e.g., D3) can be σ = 0.2. When the road segment GNSS error prediction is σ = 0.2, the probability that the GNSS error is outside the 0.57 - meter threshold is 0.0022. Thus, the margin factor D3:

[0059]

[0060] When directly measuring the error and no covariance is available using Equation 7, the margin factor can also be calculated. The same distribution can be used to find the probability at x = measured error.

[0061] Margin factor = (expected probability / probability at measured x) 缩放因子 (7)

[0062] Figure 5 is regarding Figures 1 to 4 FIG. 500 is a flowchart of a process 500 for vehicle navigation path planning using GNSS error prediction as described. Process 500 can be implemented in a computing device 115 included in vehicle 110. Process 500 includes a plurality of blocks that can be executed in the order shown. Alternatively or additionally, process 500 can include fewer blocks, or can include blocks executed in a different order.

[0063] Process 500 begins at block 502, where computing device 115 in vehicle 110 identifies multiple potential paths between the vehicle location and a selected destination. In an example, computer 115 may use any suitable algorithm for global motion planning to select potential paths, such as, for example, an incremental search-based planner (such as Rapidly-exploring Random Tree (RRT)), a graph-based planning method using road structure, a sampling-based method (such as Probabilistic RoadMap (PRM) planning), etc. As an example, the potential paths may be in the form of a map, such as map 200 downloaded to computing device 115 via network 130, for example, from a source such as GOOGLE Maps.

[0064] At block 504, computing device 115 receives GNSS error predictions corresponding to the respective potential paths between the vehicle location and the specified destination. The GNSS error predictions may be for the locations and times corresponding to each potential path. For example, the GNSS error predictions may be provided as a GNSS error map, where the errors for location and time are stored. Thus, for example, the GNSS error prediction may be the GNSS error from the map at the corresponding location and similar time of day. The GNSS error map may be based on historical GNSS error data collected from vehicles that have traveled in the same area and / or along the same path or portions of the path. The GNSS error may be determined by comparing RTK positioning with GNSS.

[0065] At block 506, computing device 115 calculates the cost of each potential path based on the corresponding GNSS error prediction. The cost function 404 based on GNSS error includes a shortest distance cost factor 410, an operating domain cost factor 412, a function cost factor 414, a margin cost factor 416, and a general cost factor 418. The cost of each path may be calculated by summing these factors for each potential path.

[0066] At block 508, computing device 115 identifies the lowest cost path from the potential paths based on the path costs determined in block 506 according to the respective GNSS error predictions of the potential paths. The lowest cost path also has the lowest overall GNSS error, thus maximizing the usability of computer-controlled vehicle operation.

[0067] At block 510, computing device 115 controls the vehicle's propulsion subsystem 112 and steering subsystem 114 to operate the vehicle along a lowest-cost path. Computing device 115 determines commands for operating the vehicle's propulsion (e.g., powertrain), braking, and steering components to drive the vehicle along the path. The vehicle path can be described by a polynomial function on which the vehicle (such as vehicle 110) can operate. Sometimes referred to as a path polynomial, the polynomial function can specify the vehicle's position (e.g., according to x, y, and z coordinates) and / or attitude (e.g., roll, pitch, and yaw) over time. That is, the path polynomial can be a polynomial function of degree three or less that describes the vehicle's motion on the ground. The vehicle's motion on the road can be described by a multi-dimensional state vector that includes the vehicle's position, orientation, speed, and acceleration. Specifically, the vehicle motion vector can include positions on x, y, z, yaw, pitch, roll, yaw rate, pitch rate, roll rate, heading speed, and heading acceleration, which can be determined, for example, by fitting a polynomial function to the successive 2D positions relative to the ground included in the vehicle motion vector. Additionally, for example, the path polynomial p(x) is a model that predicts the path as a line depicted by a polynomial equation. The path polynomial p(x) predicts the predetermined upcoming distance x of the path (e.g., measured in meters) by determining the lateral coordinate p:

[0068] p(x) = a0 + a1x + a2x 2 + a3x 3 (8)

[0069] where a0 is an offset, e.g., the lateral distance between the path and the centerline of vehicle 110 at the upcoming distance x, a1 is the heading angle of the path, a2 is the curvature of the path, and a3 is the rate of change of the curvature of the path.

[0070] The path polynomial function can be used to guide vehicle 110 from its current position to another position in the environment around the vehicle while maintaining minimum and maximum limits on lateral and longitudinal accelerations. Vehicle 110 can be operated along the vehicle path by transmitting commands to subsystems 112, 113, 114 to control vehicle propulsion, steering, and braking. After block 510, process 500 ends.

[0071] Computing devices such as those described herein typically each include instructions that can be executed by one or more computing devices such as those identified above and that are for implementing the blocks or steps of the processes described above. For example, the process blocks described above can be embodied as computer-executable instructions.

[0072] Computer-executable instructions can be compiled or interpreted by computer programs created using a variety of programming languages and / or technologies, including but not limited to the following in single or combined form: Java TM , C, C++, Python, Julia, SCALA, Visual Basic, Java Script, Perl, HTML, and the like. Generally, a processor (e.g., a microprocessor) receives instructions, for example, from a memory, a computer-readable medium, etc., and executes these instructions to perform one or more processes including one or more of the processes described herein. A variety of computer-readable media can be used to store such instructions and other data in a file and to transfer such instructions and other data. A file in a computing device is generally a collection of data stored on a computer-readable medium such as a storage medium, random access memory, etc.

[0073] A computer-readable medium (also referred to as a processor-readable medium) includes any non-transitory (i.e., tangible) medium that participates in providing data (e.g., instructions) that can be read by a computer (e.g., by a processor of the computer). Such media can take many forms, including but not limited to non-volatile media and volatile media. Instructions can be transmitted via one or more transmission media, including fiber optics, wire, wireless communication, including internal components that make up a system bus coupled to a processor of a computer. Common forms of computer-readable media include, for example, RAM, PROM, EPROM, FLASH-EEPROM, any other memory chip or cartridge, or any other medium from which a computer can read.

[0074] Unless expressly indicated to the contrary herein, all terms used in the claims are intended to have the ordinary and customary meaning as understood by one of ordinary skill in the art. Specifically, unless the claims recite an express limitation to the contrary, the use of singular articles such as "a," "the," "said," etc. shall be construed to recite one or more of the indicated elements.

[0075] The term "exemplary" is used herein in the sense of representing an example; for example, a candidate for an "exemplary widget" should be construed to refer only to an example of a widget.

[0076] The adverb "about," when modifying a value or result, means that the shape, structure, measurement, value, determination, calculation, etc. may deviate from the exactly described geometric shape, distance, measurement, value, determination, calculation, etc. due to imperfections in materials, machining, manufacturing, sensor measurements, calculations, processing times, communication times, etc.

[0077] In the drawings, like reference numerals indicate like elements. Additionally, some or all of these elements may be varied. With respect to the media, processes, systems, methods, etc. described herein, it should be understood that although the steps or blocks of such processes, etc. have been described as occurring in a sequence according to a particular order, such processes may be practiced by the described steps executed in an order other than that described herein. It should be understood that certain steps may be performed simultaneously, other steps may be added, or certain steps described herein may be omitted. In other words, the description of the processes herein is provided for the purpose of illustrating certain embodiments and should in no way be construed as limiting the claimed invention. Any use herein of "based on" and "responsive to" (including with reference to the media, processes, systems, methods, etc. described herein) indicates a causal relationship and not merely a temporal relationship.

[0078] According to the present invention, there is provided a system having: a computer including a processor and a memory, the memory including instructions executable by the processor to: receive a global navigation satellite system (GNSS) error prediction corresponding to a respective potential path between a vehicle position and a specified destination; identify a lowest cost path from the potential paths based on a path cost determined according to the respective GNSS error prediction of the potential paths; and control a propulsion subsystem and / or a steering subsystem of the vehicle to operate the vehicle along the lowest cost path.

[0079] According to one embodiment, the instructions for calculating the path cost include instructions for: calculating an operating domain cost, a functional cost, and a margin cost; and at least summing the calculated operating domain cost, the calculated functional cost, and the calculated margin cost.

[0080] According to one embodiment, the instructions for calculating the operating domain cost include instructions for: dividing a potential path into a plurality of segments; multiplying the distance of each segment by a domain weight and the GNSS error prediction corresponding to the segment to determine an operating weighted distance; and summing the operating weighted distances of the plurality of segments.

[0081] According to one embodiment, the instructions for calculating the functional cost include instructions for: dividing a potential path into a plurality of segments; determining for each segment which of a plurality of predetermined ranges the GNSS error prediction corresponding to the segment lies within; multiplying the distance of each segment by a weight factor corresponding to the determined range to calculate a functional weighted distance; and summing the functional weighted distances of the plurality of segments.

[0082] According to one embodiment, the instructions for calculating the margin cost include instructions for performing the following operations: dividing a potential path into multiple segments; multiplying the distance of each segment by a margin factor calculated based on a GNSS error prediction corresponding to the segment to determine a margin-weighted distance; and summing the margin-weighted distances of the multiple segments.

[0083] According to one embodiment, the instructions for calculating the margin factor include instructions for performing the following operations: calculating the probability that a GNSS error distance will exceed a selected distance threshold based on a GNSS error prediction; dividing the probability by an expected probability to determine a probability weight; and raising the probability weight to a selected power.

[0084] According to one embodiment, the instructions for calculating the probability include instructions for integrating a normal distribution function using the GNSS error prediction as a standard deviation.

[0085] According to one embodiment, the instructions for calculating the functional cost and the margin cost include instructions for performing the following operations: dividing a potential path into multiple segments; determining for each segment which of a plurality of predetermined ranges the GNSS error prediction corresponding to the segment lies in; multiplying the distance of each segment by a weight factor corresponding to the determined range to determine a functional weight distance; multiplying the distance of each segment by a margin factor calculated based on the GNSS error prediction corresponding to the segment to determine a margin-weighted distance; and summing the functional weight distances and the margin weight distances of the multiple segments.

[0086] According to one embodiment, the instructions for calculating the path cost include instructions for calculating the cost based at least on the potential path distance and the vehicle speed.

[0087] According to one embodiment, the instructions further include instructions for determining a GNSS error prediction based on historical data of a specific location and time.

[0088] According to the present invention, a method for vehicle navigation path planning includes: receiving a GNSS error prediction corresponding to a respective potential path between a vehicle position and a designated destination; identifying a lowest-cost path from the potential paths based on a path cost determined according to the respective GNSS error prediction of the potential paths; and controlling a propulsion subsystem and / or a steering subsystem of the vehicle to operate the vehicle along the lowest-cost path.

[0089] In one aspect of the present invention, calculating the path cost includes: calculating an operating domain cost, a functional cost, and a margin cost; and at least summing the calculated operating domain cost, the calculated functional cost, and the calculated margin cost.

[0090] In one aspect of the present invention, calculating the operation domain cost includes: dividing a potential path into multiple segments; multiplying the distance of each segment by a domain weight and a GNSS error prediction corresponding to the segment to determine an operation weighted distance; and summing the operation weighted distances of the multiple segments.

[0091] In one aspect of the present invention, calculating the function cost includes: dividing a potential path into multiple segments; determining for each segment which of a plurality of predetermined ranges the GNSS error prediction corresponding to the segment is in; multiplying the distance of each segment by a weight factor corresponding to the determined range to calculate a function weighted distance; and summing the function weighted distances of the multiple segments.

[0092] In one aspect of the present invention, calculating the margin cost includes: dividing a potential path into multiple segments; multiplying the distance of each segment by a margin factor calculated based on the GNSS error prediction corresponding to the segment to determine a margin weighted distance; and summing the margin weighted distances of the multiple segments.

[0093] In one aspect of the present invention, calculating the margin factor includes: calculating the probability that the GNSS error distance will exceed a selected distance threshold based on the GNSS error prediction; dividing the probability by an expected probability to determine a probability weight; and raising the probability weight to a selected power.

[0094] In one aspect of the present invention, calculating the probability includes integrating a normal distribution function using the GNSS error prediction as a standard deviation.

[0095] In one aspect of the present invention, calculating the function cost and the margin cost includes: dividing a potential path into multiple segments; determining for each segment which of a plurality of predetermined ranges the GNSS error prediction corresponding to the segment is in; multiplying the distance of each segment by a weight factor corresponding to the determined range to determine a function weighted distance; multiplying the distance of each segment by a margin factor calculated based on the GNSS error prediction corresponding to the segment to determine a margin weighted distance; and summing the function weighted distances and the margin weighted distances of the multiple segments.

[0096] In one aspect of the present invention, calculating the path cost includes calculating the cost based at least on the potential path distance and the vehicle speed.

[0097] In one aspect of the present invention, the method includes determining the GNSS error prediction based on historical data at a specific location and time.

Claims

1. A system comprising: A computer comprising a processor and a memory, the memory comprising instructions executable by the processor to: receiving a global navigation satellite system (GNSS) error prediction corresponding to a respective potential path between a vehicle location and a specified destination; identifying a lowest cost path from the potential paths based on path costs determined from the corresponding GNSS error predictions for the potential paths; and A propulsion subsystem and / or a steering subsystem of the vehicle is controlled to operate the vehicle along the least-cost path.

2. The system of claim 1 , wherein the instructions for calculating the path cost include instructions for: Calculate operational domain costs, functional costs, and margin costs; and At least the calculated operating domain cost, the calculated functional cost, and the calculated margin cost are summed.

3. The system of claim 2, wherein the instructions for calculating the operating domain cost include instructions for: Dividing the potential path into a plurality of segments; multiplying the distance for each road segment by a domain weight and a GNSS error prediction corresponding to the road segment to determine an operational weighted distance; as well as The operational weighted distances of the plurality of road segments are summed.

4. The system of claim 2, wherein the instructions for calculating the functional cost include instructions for: Dividing the potential path into a plurality of segments; determining, for each road segment, within which of a plurality of predetermined ranges the GNSS error prediction corresponding to the road segment is located; multiplying the distance of each road segment by a weighting factor corresponding to the determined range to calculate a functionally weighted distance; as well as The function-weighted distances of the plurality of road segments are summed.

5. The system of claim 2, wherein the instructions for calculating the margin cost include instructions for: Dividing the potential path into a plurality of segments; multiplying the distance for each road segment by a margin factor calculated based on a GNSS error prediction corresponding to the road segment to determine a margin-weighted distance; and The margin weighted distances of the plurality of road segments are summed.

6. The system of claim 5, wherein the instructions for calculating the margin factor include instructions for: calculating a probability that a GNSS error distance will exceed a selected distance threshold based on the GNSS error prediction; dividing the probability by the expected probability to determine a probability weight; and The probability weights are exponentiated to a selected power.

7. The system of claim 6, wherein the instructions for calculating the probability include instructions for integrating a normal distribution function using the GNSS error prediction as a standard deviation.

8. The system of claim 2, wherein the instructions for calculating the functional cost and the margin cost include instructions for: Dividing the potential path into a plurality of segments; determining, for each road segment, within which of a plurality of predetermined ranges the GNSS error prediction corresponding to the road segment is located; multiplying the distance of each road segment by a weighting factor corresponding to the determined range to determine a functionally weighted distance; multiplying the distance for each road segment by a margin factor calculated based on a GNSS error prediction corresponding to the road segment to determine a margin-weighted distance; as well as The function weighted distances and the margin weighted distances of the plurality of road segments are summed.

9. The system of any one of claims 1 to 8, wherein the instructions further comprise instructions for determining the GNSS error prediction based on historical data for a specific location and time.

10. A method for vehicle navigation path planning, comprising: receiving GNSS error predictions corresponding to respective potential paths between the vehicle location and a specified destination; identifying a lowest cost path from the potential paths based on path costs determined from the corresponding GNSS error predictions for the potential paths; as well as A propulsion subsystem and / or a steering subsystem of the vehicle is controlled to operate the vehicle along the least-cost path.

11. The method of claim 10, wherein calculating the path cost comprises: Calculate operational domain costs, functional costs, and margin costs; as well as At least the calculated operating domain cost, the calculated functional cost, and the calculated margin cost are summed.

12. The method of claim 11, wherein calculating the operation domain cost comprises: Dividing the potential path into a plurality of segments; multiplying the distance for each road segment by a domain weight and a GNSS error prediction corresponding to the road segment to determine an operational weighted distance; as well as The operational weighted distances of the plurality of road segments are summed.

13. The method of claim 11, wherein calculating the functional cost and the margin cost comprises: Dividing the potential path into a plurality of segments; determining, for each road segment, within which of a plurality of predetermined ranges the GNSS error prediction corresponding to the road segment is located; multiplying the distance of each road segment by a weighting factor corresponding to the determined range to determine a functionally weighted distance; multiplying the distance for each road segment by a margin factor calculated based on a GNSS error prediction corresponding to the road segment to determine a margin-weighted distance; as well as The function weighted distances and the margin weighted distances of the plurality of road segments are summed.

14. The method of claim 13, wherein calculating the margin factor comprises: calculating a probability that a GNSS error distance will exceed a selected distance threshold based on the GNSS error prediction; dividing the probability by the expected probability to determine a probability weight; as well as exponentiating the probability weight to a selected power; and Wherein calculating the probability comprises integrating a normal distribution function using the GNSS error prediction as a standard deviation.

15. The method of any one of claims 10 to 14, further comprising determining the GNSS error prediction based on historical data for a specific location and time.

Images