Over-the-horizon path planning method, system and equipment
Through time synchronization and global world model construction, combined with Kalman filtering and kinematic models, the problem of insufficient beyond-visual-range perception of on-board sensors is solved, high-precision path planning is achieved, and the safety and efficiency of the assisted driving system are improved.
Patent Information
- Application Number
- CN202510852767.5
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2025-06-24
- Publication Date
- 2025-10-17
AI Technical Summary
Existing on-board sensors are unable to achieve advance perception and path planning of beyond-visual-range scenarios. The spatiotemporal synchronization errors between beyond-visual-range information and on-board data are large, resulting in distortion of the fused world model and large deviations between the predicted trajectory and the actual trajectory.
By acquiring time-stamped beyond-horizon information and the world model of the ego-vehicle coordinate system in real time, and using cloud-based timestamps for time synchronization, a global world model is constructed. Clock offset and drift corrections are performed based on Kalman filtering. Combined with the kinematic model of angular velocity compensation, the motion trajectory of beyond-horizon targets and the ego-vehicle is predicted, thereby optimizing the ego-vehicle's driving trajectory.
It achieves high-precision time synchronization between beyond-visual-range targets and the ego vehicle, ensuring the accuracy and clarity of the global world model, providing a safer and more efficient trajectory planning solution, and improving the real-time performance and reliability of the assisted driving system.
Smart Images
Figure CN120800375A_ABST
Abstract
Description
TECHNICAL FIELD
[0001] The embodiment of the application relates to the technical field of intelligent driving, in particular to a path planning method, system and equipment based on over-the-horizon. BACKGROUND
[0002] The existing auxiliary driving system mainly relies on vehicle-mounted sensors such as laser radar, millimeter wave radar, camera and the like for environment perception, but the perception distance thereof is usually limited to 200-500 meters, and the over-the-horizon, i.e. more than 1 kilometer scene, cannot be coped with for the advance planning requirement.
[0003] In the expressway or elevated road scene, the traffic accident or congestion 2 kilometers away in front cannot be detected by the vehicle-mounted sensor, and the vehicle adjusts the path only after entering the congestion area, so that the decision lags, resulting in low traffic efficiency.
[0004] And the vehicle that changes lanes transversely across multiple lanes, such as the vehicle at the expressway merging entrance, cannot be predicted in advance by the vehicle-mounted sensor when in the over-the-horizon range, so that the decision of changing lanes of the vehicle is delayed, and the collision risk is increased.
[0005] In addition, the over-the-horizon equipment such as the unmanned aerial vehicle and the image / positioning data transmitted by the V2X networked vehicle have the time stamp deviation and the space coordinate system difference with the sensor data of the ego vehicle such as the ego vehicle positioning and the real-time road condition, and the traditional fusion algorithm cannot be calibrated in real time, so that the world model after fusion is distorted.
[0006] And the traditional path planning algorithm does not consider the dynamic motion compensation of the over-the-horizon target, so that the predicted trajectory has a large deviation from the actual scene. SUMMARY
[0007] In order to solve the problems in the prior art that the perception distance of the vehicle-mounted sensor is limited, the over-the-horizon scene cannot be perceived and planned in advance, and the time and space synchronization error of the over-the-horizon information and the vehicle-mounted data is large, resulting in the distortion of the world model after fusion and further resulting in the large deviation of the predicted trajectory from the actual scene, the embodiment of the application provides a path planning method, system and equipment based on over-the-horizon.
[0008] According to one object of the embodiment of the application, a path planning method based on over-the-horizon is provided, which comprises the following steps:
[0009] real-time acquisition of over-the-horizon information marked with a first time stamp and over-the-horizon positioning information;
[0010] real-time acquisition of an ego vehicle coordinate system world model marked with a second time stamp and ego vehicle positioning information;
[0011] time synchronization of the first time stamp and the second time stamp with a cloud time stamp;
[0012] construct a global world model according to the over-the-horizon information, the over-the-horizon positioning information, the ego-vehicle coordinate system world model, the ego-vehicle positioning information, and the cloud map data;
[0013] predict a motion trajectory of the over-the-horizon target and a motion trajectory of the ego vehicle within a preset time according to the global world model;
[0014] optimize a driving trajectory of the ego vehicle according to the predicted motion trajectory of the over-the-horizon target and the motion trajectory of the ego vehicle.
[0015] In some optional implementations, the first timestamp and the second timestamp are time-synchronized with a cloud timestamp, specifically including:
[0016] According to a dynamic clock offset estimation algorithm of Kalman filtering, the clock offset and clock drift of the first timestamp and the second timestamp are corrected in real time, so that the first timestamp and the second timestamp are time-synchronized with the cloud timestamp.
[0017] In some optional implementations, the clock offset and clock drift of the first timestamp and the second timestamp are corrected in real time, specifically including:
[0018] According to the calculated clock offset and clock drift between the first timestamp and the second timestamp at the last moment and the reference timestamp at the last moment, the clock offset and clock drift between the first timestamp and the second timestamp at the current moment and the reference timestamp at the current moment are predicted respectively;
[0019] The predicted clock offset and clock drift between the first timestamp and the second timestamp at the current moment and the reference timestamp at the current moment are respectively compensated to the first timestamp and the second timestamp at the current moment, so as to correct the first timestamp and the second timestamp at the current moment.
[0020] In some optional implementations, the global world model is constructed, specifically including:
[0021] According to the over-the-horizon positioning information and the ego-vehicle positioning information, the over-the-horizon coordinates and the ego-vehicle coordinates are converted into a global coordinate system in which the cloud map is located;
[0022] The over-the-horizon information and the ego-vehicle coordinate system world model are fused into the global coordinate system to generate a global world model.
[0023] In some optional implementations, the motion trajectory of the over-the-horizon target and the motion trajectory of the ego vehicle within a preset time are predicted, specifically including:
[0024] According to the global world model, a current motion state of the over-the-horizon target and a current motion state of the ego vehicle are acquired;
[0025] Based on the angular velocity compensation-based kinematic model, in combination with Kalman filtering, a motion trajectory of the over-the-horizon target within a preset time and a motion trajectory of the ego vehicle within the preset time are respectively calculated.
[0026] In some optional implementations, the angular velocity compensation-based kinematic model is:
[0027] x(t)=x0+v·Δt·cos(θ+ωΔt);
[0028] y(t)=y0+v·Δt·sin(θ+ωΔt);
[0029] where (x0, y0) represents position coordinates of the over-the-horizon target or the ego vehicle at a current moment, v represents a speed of the over-the-horizon target or the ego vehicle at the current moment, θ represents a heading angle of the over-the-horizon target or the ego vehicle at the current moment, ω represents an angular velocity of the over-the-horizon target or the ego vehicle at the current moment, Δt represents a time interval predicted by the over-the-horizon target or the ego vehicle, and (x(t), y(t)) represents position coordinates of the over-the-horizon target or the ego vehicle after Δt time.
[0030] In some optional implementations, the ego vehicle driving trajectory is optimized according to the predicted motion trajectory of the over-the-horizon target and the motion trajectory of the ego vehicle, and specifically includes:
[0031] The overlapping relationship between the predicted motion trajectory of the over-the-horizon target and the motion trajectory of the ego vehicle is judged, if there is an overlapping case, the motion trajectory of the ego vehicle is re-planned and updated to the global world model, otherwise, the motion trajectory of the ego vehicle is unchanged.
[0032] In some optional implementations, the over-the-horizon information is road environment information in front of the ego vehicle acquired through an over-the-horizon information acquisition module.
[0033] According to another object of the embodiment of the application, an over-the-horizon-based path planning system is provided, which includes an over-the-horizon information acquisition module, an ego vehicle and a cloud server, the over-the-horizon information acquisition module and the ego vehicle are respectively in communication connection with the cloud server;
[0034] The over-the-horizon information acquisition module is configured to acquire road environment information in front of the ego vehicle, and send the road environment information marked with a first timestamp and positioning information of the over-the-horizon information acquisition module to the cloud server.
[0035] The ego vehicle is used for sending the ego vehicle coordinate system world model marked with a second timestamp and ego vehicle positioning information to a cloud server, and receiving a predicted motion trajectory of the over-the-horizon target and a motion trajectory of the ego vehicle sent by the cloud server, and optimizing the ego vehicle driving trajectory according to the predicted motion trajectory of the over-the-horizon target and the motion trajectory of the ego vehicle.
[0036] The cloud server stores a cloud map, and the cloud server is used for time synchronizing the first timestamp and the second timestamp with a cloud timestamp, constructing a global world model according to the road environment information, positioning information of the over-the-horizon information collection module, the ego vehicle coordinate system world model, the ego vehicle positioning information and the cloud map data, predicting a motion trajectory of the over-the-horizon target within a preset time and a motion trajectory of the ego vehicle within the preset time according to the global world model, and sending the predicted motion trajectory of the over-the-horizon target and the motion trajectory of the ego vehicle to the ego vehicle.
[0037] According to another purpose of the embodiment of the present application, a path planning device based on over-the-horizon is provided, comprising a processor, a memory, a communication interface and a communication bus, the processor, the memory and the communication interface complete mutual communication through the communication bus.
[0038] The memory is used for storing at least one executable instruction, and the executable instruction makes the processor execute the operation of the path planning method based on over-the-horizon.
[0039] Compared with the prior art, the present application has the following advantages:
[0040] The path planning method based on over-the-horizon provided by the present application uses the over-the-horizon information collection module to collect road environment information far beyond the sensor range of the ego vehicle, combines the cloud map, over-the-horizon positioning information and ego vehicle positioning information to construct a global world model which is time-synchronized and space-synchronized, realizes the prediction of the driving trajectory of the over-the-horizon target and the ego vehicle based on the global world model, and optimizes the driving trajectory of the ego vehicle according to the predicted trajectory, thereby ensuring the real-time and reliability of the trajectory prediction and providing a safer and more efficient trajectory planning solution for the auxiliary driving system.
[0041] Moreover, the over-the-horizon device, the ego vehicle and the cloud are real-time time compensated in the global world model, high-precision time synchronization of the over-the-horizon device, the ego vehicle and the cloud is realized, and thus the precision and clarity of the global world model are ensured.
[0042] The above description is only a summary of the technical solutions of the embodiments of the present application, in order to enable the technical means of the embodiments of the present application to be more clearly understood, and can be implemented according to the content of the specification, and in order to enable the above and other purposes, characteristics and advantages of the embodiments of the present application to be more apparent and easy to understand, the specific embodiments of the present application are described below. BRIEF DESCRIPTION OF DRAWINGS
[0043] The accompanying drawings are included to provide a further understanding of the application and are incorporated herein and constitute a part of the detailed description. It should be noted that the accompanying drawings illustrate only exemplary embodiments of the application and therefore should not be considered to limit the scope of the application. In the drawings:
[0044] Figure 1 A flowchart of a path planning method based on over-the-horizon provided by an embodiment of the present application is shown.
[0045] Figure 2 A structural block diagram of a path planning system based on over-the-horizon provided by an embodiment of the present application is shown.
[0046] Figure 3 A structural schematic diagram of a path planning device based on over-the-horizon provided by an embodiment of the present application is shown. DETAILED DESCRIPTION
[0047] Exemplary embodiments of the present application will be described in more detail below with reference to the accompanying drawings. Although exemplary embodiments of the present application are shown in the drawings, it should be understood that the present application can be implemented in various forms and should not be limited by the embodiments described herein.
[0048] The embodiments of the present application are to solve the problems in the prior art that the sensing distance of the vehicle-mounted sensor is limited, the early sensing and path planning of the over-the-horizon scene cannot be realized, the space-time synchronization error of the over-the-horizon information and the vehicle-mounted data is large, the fused world model is distorted, and the predicted trajectory and the actual deviation are large. A path planning method based on over-the-horizon is provided.
[0049] Figure 1 A flowchart of a path planning method based on over-the-horizon provided by an embodiment of the present application is shown.
[0050] In this embodiment, it should be noted that a path planning method based on over-the-horizon is realized based on the data of an over-the-horizon information collection module, a self-vehicle end and a cloud server.
[0051] In the implementation process of the path planning method based on the over-the-horizon in this embodiment, the network time of the cloud server is taken as a global clock source, and at the start of the over-the-horizon information collection module, the ego vehicle and / or the cloud server, and at the whole minute or the preset time point, the network time of the over-the-horizon information collection module, the ego vehicle and the cloud server is aligned through the PPS or gPTP of the GNSS, so as to ensure that the over-the-horizon information collection module, the ego vehicle and the cloud server work under a unified time axis.
[0052] Then, the over-the-horizon information collection module and the ego vehicle are compensated in real time, so as to ensure that the over-the-horizon information collection module and the ego vehicle are synchronized in real time with the cloud server.
[0053] Meanwhile, the coordinates of the over-the-horizon information collection module and the ego vehicle are converted, so that the over-the-horizon information collection module and the ego vehicle are in the same coordinate system as the cloud server.
[0054] Finally, based on the same coordinate system, the information data of the over-the-horizon information collection module and the ego vehicle are analyzed, so as to predict the driving trajectory of the over-the-horizon target and the ego vehicle, and to optimize the driving trajectory of the ego vehicle according to the predicted trajectory, while ensuring the real-time and reliability of the trajectory prediction, a safer and more efficient trajectory planning solution is provided for the auxiliary driving system.
[0055] Therefore, the path planning method based on the over-the-horizon provided in this embodiment, as shown in Figure 1 includes the following steps:
[0056] S10. Real-time acquisition of over-the-horizon information marked with a first timestamp and over-the-horizon positioning information.
[0057] In this step, the over-the-horizon information is the road environment information in front of the ego vehicle acquired by the over-the-horizon information collection module; and the over-the-horizon positioning information is the positioning information of the over-the-horizon information collection module.
[0058] And the over-the-horizon information collection module sends the acquired road environment information in front of the ego vehicle and the positioning information of the over-the-horizon information collection module to the cloud server. The corresponding time is also sent during the data sending process.
[0059] Therefore, this step is specifically:
[0060] The cloud server acquires in real time the road environment information in front of the ego vehicle acquired by the over-the-horizon information collection module and the positioning information of the over-the-horizon information collection module marked with the first timestamp.
[0061] The over-the-horizon information collection module includes an image sensor, a millimeter wave radar or a laser radar.
[0062] The road environment information includes: front road obstacles, lane lines, traffic events, etc.
[0063] The image sensor, the millimeter wave radar or the laser radar is based on a UAV, a V2X networked vehicle or a road side unit.
[0064] S20. Real-time acquisition of the ego vehicle coordinate system world model marked with a second timestamp and ego vehicle positioning information.
[0065] In this step, the ego vehicle coordinate system world model is constructed by taking the vehicle body as the coordinate origin and collecting information through the whole vehicle sensor.
[0066] The ego vehicle sends the corresponding time in the process of sending the ego vehicle coordinate system world model and the ego vehicle positioning information to the cloud server.
[0067] Therefore, this step is specifically: the cloud server real-time acquisition of the ego vehicle coordinate system world model marked with a second timestamp and ego vehicle positioning information.
[0068] S30. Time synchronization of the first timestamp and the second timestamp with the cloud timestamp.
[0069] In this step, in view of the communication delay between the over-the-horizon information acquisition module, the ego vehicle end and the cloud server, therefore, the cloud server real-time compensates the first timestamp and the second timestamp after receiving the road environment information in front of the ego vehicle sent by the over-the-horizon information acquisition module marked with the first timestamp and the positioning information of the over-the-horizon information acquisition module, the ego vehicle sent the ego vehicle coordinate system world model marked with the second timestamp and the ego vehicle positioning information, so that the first timestamp and the second timestamp are real-time synchronized with the cloud timestamp.
[0070] S40. Constructing a global world model according to the over-the-horizon information, the over-the-horizon positioning information, the ego vehicle coordinate system world model, the ego vehicle positioning information and the cloud map data.
[0071] In this step, the over-the-horizon positioning information and the ego vehicle coordinate system world model are converted into the UTM coordinate system unified with the cloud server.
[0072] Based on the time synchronization in step S30, the over-the-horizon information, the ego vehicle coordinate system world model and the cloud map data stored by the cloud server are fused, thereby constructing a global world model in the UTM coordinate system.
[0073] S50. According to the global world model, predicting the motion trajectory of the over-the-horizon target within a preset time and the motion trajectory of the ego vehicle within a preset time.
[0074] In this step, the beyond-visual-range information is analyzed based on the global world model, a beyond-visual-range target is determined, and a motion trajectory of the beyond-visual-range target within a preset time is predicted.
[0075] Meanwhile, the ego vehicle coordinate system world model is analyzed based on the global world model, and a motion trajectory of the ego vehicle within a preset time is predicted.
[0076] S60. The ego vehicle driving trajectory is optimized according to the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the ego vehicle.
[0077] In this step, it is judged whether the predicted motion trajectory of the beyond-visual-range target overlaps with the motion trajectory of the ego vehicle within a preset time based on the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the ego vehicle, and the ego vehicle driving trajectory is optimized according to the judgment result.
[0078] The path planning method based on beyond-visual-range provided in this embodiment uses the beyond-visual-range information collection module to collect road environment information far beyond the sensor range of the ego vehicle, combines the cloud map, beyond-visual-range positioning information, ego vehicle coordinate system world model and ego vehicle positioning information, constructs a time-synchronous and space-synchronous global world model, realizes spatio-temporal consistency fusion of the beyond-visual-range information and the ego vehicle coordinate system world model, and provides a precise spatio-temporal reference for subsequent motion prediction and trajectory planning; the global world model is used to realize prediction of the driving trajectory of the beyond-visual-range target and the ego vehicle, and the ego vehicle driving trajectory is optimized according to the predicted trajectory, which ensures the real-time and reliability of trajectory prediction and provides a safer and more efficient trajectory planning solution for the assisted driving system.
[0079] The global world model realizes real-time time compensation for the beyond-visual-range device, the ego vehicle and the cloud, realizes high-precision time synchronization of the beyond-visual-range device, the ego vehicle and the cloud, and thus ensures the precision and clarity of the global world model.
[0080] This embodiment as a preferred embodiment optimizes the specific implementation scheme of time synchronization of the first timestamp and the second timestamp with the cloud timestamp.
[0081] In this embodiment, the first timestamp and the second timestamp are time-synchronized with the cloud timestamp, specifically including:
[0082] According to the dynamic adjustment process noise, the dynamic clock offset estimation algorithm of Kalman filtering is used to correct the clock offset and clock drift of the first timestamp and the second timestamp in real time, so that the first timestamp and the second timestamp are time-synchronized with the cloud timestamp.
[0083] In the embodiment, in view of the communication delay between the over-the-horizon information collection module, the ego vehicle side and the cloud server, therefore, after receiving the road environment information in front of the ego vehicle sent by the over-the-horizon information collection module marked with the first timestamp, the positioning information of the over-the-horizon information collection module, the ego vehicle coordinate system world model sent by the ego vehicle marked with the second timestamp and the ego vehicle positioning information, the cloud server compensates the first timestamp and the second timestamp in real time through the dynamic clock offset estimation algorithm of Kalman filtering according to the dynamic adjustment process noise, so as to realize real-time correction of the clock offset and clock drift of the first timestamp and the second timestamp, and make the first timestamp and the second timestamp time-synchronized with the cloud timestamp.
[0084] The embodiment as a preferred embodiment, real-time correction of the clock offset and clock drift of the first timestamp and the second timestamp, specifically includes:
[0085] According to the clock offset and clock drift between the calculated first timestamp and second timestamp of the last moment and the reference timestamp of the last moment, the clock offset and clock drift between the first timestamp and the second timestamp of the current moment and the reference timestamp of the current moment are respectively predicted;
[0086] The predicted clock offset and clock drift between the first timestamp and the second timestamp of the current moment and the reference timestamp of the current moment are respectively compensated to the first timestamp and the second timestamp of the current moment, so as to correct the first timestamp and the second timestamp of the current moment.
[0087] In the embodiment, the reference timestamp of the last moment is the time information of the cloud server of the last moment, or the reference timestamp of the current moment is the time information of the cloud server of the current moment.
[0088] The clock offset represents the instantaneous deviation of the first timestamp or the second timestamp from the reference timestamp, and the clock drift represents the deviation change rate of the first timestamp or the second timestamp from the reference timestamp.
[0089] The embodiment realizes real-time correction of the first timestamp and the second timestamp based on the reference timestamp respectively.
[0090] In the embodiment, the clock offset and clock drift of the first timestamp and the second timestamp are corrected in real time, and the specific implementation process is:
[0091] Initialization: set the state vector, including the initial clock offset and the initial clock drift; at the same time, determine the state transition matrix F, the observation matrix H, the process noise Q and the observation noise R.
[0092] Prediction: According to the state transition matrix F and the state vector of the last time, the state vector of the current time is predicted; according to the state transition matrix F, the covariance matrix of the last time and the process noise Q, the covariance matrix of the current time is predicted.
[0093] Update: Calculate the Kalman gain K according to the predicted covariance matrix of the current time, and weigh the credibility of the predicted value and the measured value through the Kalman gain K.
[0094] Wherein, the measured value is the current measured clock offset, and the predicted value is the clock offset obtained by the prediction step. The difference between the measured value and the predicted value is combined with the Kalman gain K to correct the predicted state vector, and a more accurate state vector is obtained; the covariance matrix is updated according to the corrected state vector.
[0095] Continuous iteration: constantly repeat the above prediction and update steps, with the passage of time, the fusion of historical state prediction and real-time measurement is used to continuously dynamically correct the clock offset and clock drift between the first timestamp and the second timestamp and the cloud timestamp of the cloud server, and output more robust clock synchronization results.
[0096] In this embodiment, it needs to be clear that during the driving of the vehicle, the data interaction is continuously carried out between the over-the-horizon information collection module, the ego vehicle and the cloud server, and the Kalman clock synchronization algorithm is running in real time, so as to ensure that the clocks of each device are always accurately synchronized.
[0097] In this embodiment, the specific execution steps of the dynamic clock offset estimation algorithm based on Kalman filtering are as follows:
[0098] State vector initialization, including initial clock offset and initial clock drift;
[0099] The initial clock offset is the initial difference between the first timestamp of the over-the-horizon information collection module or the second timestamp of the ego vehicle and the cloud timestamp of the cloud server when the over-the-horizon information collection module, the ego vehicle and / or the cloud server is started.
[0100] The initial clock drift is the rate of change of the deviation between the first timestamp of the over-the-horizon information collection module or the second timestamp of the ego vehicle and the cloud timestamp of the cloud server with time.
[0101] Covariance matrix initialization: the covariance of the initial clock offset is set to 1e-3, and the covariance of the initial clock drift is set to 1e-5.
[0102] Fixed parameter configuration:
[0103] State transition matrix F = [[1, At], [0, 1]], used to describe the change of clock offset with time;
[0104] The observation matrix H = [[1, 0]] indicates that the value is directly related to the clock offset;
[0105] The process noise Q = diag([1e-6, 1e-8]) describes the uncertainty that is not covered by the model. Q[0, 0] is set to a small value (1e-6) for the initial clock offset because the offset changes less frequently. Q[1, 1] is set to a smaller value (1e-8) for the initial clock drift because the drift changes slowly and stably.
[0106] The observation noise R = [[0.1]] describes the unreliability of the measurement. The larger the value, the less accurate the measurement, and the more the subsequent filtering will rely on the predicted value.
[0107] Prediction step:
[0108] State prediction: Based on the state transition matrix F, the clock state at the current time is extrapolated from the clock state at the previous time.
[0109] Covariance prediction: The covariance is passed to the current time through the state transition matrix F, and after superimposing the process noise Q, it ensures that the uncertainty of the prediction includes the model error.
[0110] Update step:
[0111] Calculate the Kalman gain K through the observation matrix H and the observation noise R to dynamically balance the reliability of prediction and measurement.
[0112] State correction: The error correction of the measurement is fused to correct the predicted state.
[0113] Covariance correction: Realize the dynamic update of uncertainty.
[0114] Iteration step:
[0115] The output state and covariance of the current period are taken as the input state and covariance of the next period to form a closed-loop recursion.
[0116] In addition, in the automatic driving scene, the vehicle is in a dynamic driving state, and the synchronization interval Δt in the state transition matrix F is adjusted according to the motion state of the vehicle. If the vehicle is accelerating, Δt may be reduced to track the clock change more timely. If the vehicle is driving at a constant speed, Δt remains relatively stable.
[0117] The embodiment is a preferred embodiment, and the specific construction process of the global world model is optimized.
[0118] In the embodiment, the global world model is constructed, specifically including:
[0119] The over-the-horizon coordinates and the ego coordinates are converted into the global coordinate system in which the cloud map is located according to the over-the-horizon positioning information and the ego positioning information.
[0120] Fuse the over-the-horizon information and the ego vehicle coordinate system world model into the global coordinate system to generate a global world model.
[0121] In this embodiment, the specific implementation scheme for constructing the global world model includes:
[0122] Convert the longitude and latitude of the over-the-horizon positioning information into UTM coordinates;
[0123] Convert the ego vehicle coordinate system world model into UTM coordinates;
[0124] The cloud map data of the cloud server is based on the UTM coordinate system.
[0125] Based on the time synchronization achieved in the above step S30, the over-the-horizon information marked with the corrected first timestamp and the ego vehicle coordinate system world model marked with the corrected second timestamp are fused with the cloud map data, thereby constructing a global world model based on the UTM coordinate system.
[0126] This embodiment, as a preferred embodiment, optimizes the prediction scheme for the motion trajectory of the over-the-horizon target and the motion trajectory of the ego vehicle within a preset time.
[0127] In this embodiment, the motion trajectory of the over-the-horizon target and the motion trajectory of the ego vehicle within a preset time are predicted, specifically including:
[0128] According to the global world model, obtain the current motion state of the over-the-horizon target and the current motion state of the ego vehicle;
[0129] Based on the kinematic model of angular velocity compensation, combined with Kalman filtering, the motion trajectory of the over-the-horizon target within a preset time and the motion trajectory of the ego vehicle within a preset time are calculated respectively.
[0130] In the step, the kinematic model of angular velocity compensation is:
[0131] x(t)=x0+v·Δt·cos(θ+ωΔt);
[0132] y(t)=y0+v·Δt·sin(θ+ωΔt);
[0133] Where (x0, y0) represents the position coordinates of the over-the-horizon target or the ego vehicle at the current time, v represents the speed of the over-the-horizon target or the ego vehicle at the current time, θ represents the heading angle of the over-the-horizon target or the ego vehicle at the current time, ω represents the angular velocity of the over-the-horizon target or the ego vehicle at the current time, Δt represents the time interval predicted by the over-the-horizon target or the ego vehicle, and (x(t), y(t)) represents the position coordinates of the over-the-horizon target or the ego vehicle after Δt time.
[0134] In the trajectory prediction process of the embodiment, the angular velocity ω is introduced to compensate for the nonlinear motion of the curve, and the above-mentioned angular velocity compensation kinematics model is used to predict the motion trajectory of the beyond-visual-range target and the ego vehicle within a preset time, so as to improve the prediction accuracy under complex road conditions.
[0135] In the embodiment, the preset time is set to 2S, and the state variables such as the position coordinates of the beyond-visual-range target or the ego vehicle at the current time, the speed of the beyond-visual-range target or the ego vehicle at the current time, the heading angle of the beyond-visual-range target or the ego vehicle at the current time, the angular velocity of the beyond-visual-range target or the ego vehicle at the current time, and the time interval of the beyond-visual-range target or the ego vehicle prediction are input into the above-mentioned angular velocity compensation kinematics model. The angular velocity compensation kinematics model will output the positioning information after multi-source data fusion, and generate a complete global world model containing a 2-second prediction trajectory by combining Kalman filtering.
[0136] The embodiment is a preferred embodiment, and the optimization process of the ego vehicle driving trajectory is optimized.
[0137] In the embodiment, the ego vehicle driving trajectory is optimized according to the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the ego vehicle, and specifically includes:
[0138] The overlap relationship between the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the ego vehicle is judged. If there is an overlap, the motion trajectory of the ego vehicle is replanned and updated into the global world model, otherwise, the motion trajectory of the ego vehicle remains unchanged.
[0139] In the embodiment, the cloud server sends the complete global world model containing the prediction trajectory of the preset time to the ego vehicle side, the ego vehicle judges whether the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the ego vehicle overlap, if so, the motion trajectory of the ego vehicle is replanned and updated into the global world model, otherwise, the motion trajectory of the ego vehicle remains unchanged.
[0140] In the embodiment, the beyond-visual-range target can be a fixed target or a moving target.
[0141] According to the motion trajectory of the beyond-visual-range target, the beyond-visual-range road environment state in front of the ego vehicle can be judged. If the beyond-visual-range road environment in front of the ego vehicle is ideal, the motion trajectory of the ego vehicle is maintained to ensure smooth driving; if the beyond-visual-range road environment in front of the ego vehicle has an accident or a special event, but the navigation path remains unchanged, the ego vehicle is planned to change lanes to the optimal traffic lane in advance to improve the traffic efficiency and safety of the ego vehicle; the variable scene: the beyond-visual-range road environment in front of the ego vehicle has an accident or a special event, resulting in a change of the navigation path, a path switching request is sent, and after the user confirms, the navigation path and the global world model are updated to adapt to complex road conditions flexibly.
[0142] Therefore, the path planning method based on the over-the-horizon provided in the embodiment can realize the prediction of the driving trajectory of the over-the-horizon target and the ego vehicle based on the global world model of time synchronization and space synchronization, and optimize the driving trajectory of the ego vehicle according to the predicted trajectory, thereby ensuring the real-time and reliability of the trajectory prediction and providing a safer and more efficient trajectory planning solution for the auxiliary driving system.
[0143] In some optional embodiments, the embodiment of the present application further discloses an over-the-horizon-based path planning system.
[0144] As shown in the Figure 3 The over-the-horizon-based path planning system comprises an over-the-horizon information acquisition module 100, an ego vehicle 200, and a cloud server 300, and the over-the-horizon information acquisition module 100 and the ego vehicle 200 are in communication connection with the cloud server 300.
[0145] The over-the-horizon information acquisition module 100 is configured to acquire the road environment information in front of the ego vehicle, and send the road environment information marked with a first timestamp and the positioning information of the over-the-horizon information acquisition module 100 to the cloud server 300.
[0146] The ego vehicle 200 is configured to send the ego vehicle coordinate system world model marked with a second timestamp and the ego vehicle positioning information to the cloud server 300, receive the predicted motion trajectory of the over-the-horizon target and the predicted motion trajectory of the ego vehicle sent by the cloud server 300, and optimize the driving trajectory of the ego vehicle according to the predicted motion trajectory of the over-the-horizon target and the predicted motion trajectory of the ego vehicle 200.
[0147] The cloud server 300 stores a cloud map, and the cloud server 300 is configured to perform time synchronization of the first timestamp and the second timestamp with a cloud timestamp, construct a global world model according to the road environment information, the positioning information of the over-the-horizon information acquisition module 100, the ego vehicle coordinate system world model, the ego vehicle positioning information, and the cloud map data, predict the motion trajectory of the over-the-horizon target within a preset time and the motion trajectory of the ego vehicle 200 within the preset time according to the global world model, and send the predicted motion trajectory of the over-the-horizon target and the predicted motion trajectory of the ego vehicle 200 to the ego vehicle 200.
[0148] In the embodiment, it is necessary to make clear that the over-the-horizon information acquisition module 100 and the ego vehicle 200 take the network time of the cloud server 300 as a global clock source, and at the start of the over-the-horizon information acquisition module 100, the ego vehicle 200, and / or the cloud server 300, and at the whole point or the preset time point, the network time of the over-the-horizon information acquisition module 100, the ego vehicle 200, and the cloud server 300 is aligned through the PPS of the GNSS or the gPTP, so as to ensure that the over-the-horizon information acquisition module 100, the ego vehicle 200, and the cloud server 300 work under a unified time axis.
[0149] The beyond-visual-range information acquisition module 100 includes an image sensor, a millimeter-wave radar or a laser radar.
[0150] The road environment information includes: road obstacles ahead, lane lines, traffic events, etc.
[0151] Image sensors, millimeter-wave radars or lidars are based on drones, V2X connected vehicles or roadside units.
[0152] The world model of the ego-vehicle coordinate system is constructed with the ego-vehicle body as the coordinate origin and the information collected by the vehicle sensors.
[0153] In this embodiment, the specific implementation process of the cloud server 300 synchronizing the first timestamp and the second timestamp with the cloud timestamp, the specific implementation process of constructing a global world model based on road environment information, the positioning information of the beyond-visual-range information acquisition module 100, the vehicle coordinate system world model, the vehicle positioning information and the cloud map data, and the specific implementation process of predicting the motion trajectory of the beyond-visual-range target within a preset time and the motion trajectory of the vehicle 200 within a preset time based on the global world model, refer to the calculation process in the above-mentioned beyond-visual-range path planning method, and will not be repeated in this embodiment.
[0154] Similarly, the specific implementation process of the vehicle 200 optimizing the vehicle's driving trajectory based on the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the vehicle 200 can be found in the optimization process of the vehicle's driving trajectory in the above-mentioned beyond-visual-range path planning method, which will not be repeated in this embodiment.
[0155] In some optional embodiments, the present invention further discloses a path planning device based on beyond visual range.
[0156] Figure 3 A schematic structural diagram of a beyond-visual-range path planning device provided by an embodiment of the present invention is shown.
[0157] like Figure 3 As shown, the beyond-visual-range path planning device includes: a processor 410 , a memory 420 , a communication interface 430 and a communication bus 440 . The processor 510 , the memory 420 and the communication interface 430 communicate with each other through the communication bus 440 .
[0158] In the embodiment, the memory 420 is configured to store at least one executable instruction, and the executable instruction is configured to enable the processor 420 to perform operations of the LoS-based path planning method according to any of the above embodiments. The communication interface 430 is configured to communicate with network elements such as clients or other servers. The processor 410 is configured to execute the program 450, and specifically, perform the steps of the LoS-based path planning method according to the above embodiments.
[0159] Specifically, the program 450 can include program codes including computer executable instructions.
[0160] The processor 410 can be a central processing unit (CPU), or an application specific integrated circuit (ASIC), or one or more integrated circuits configured to implement embodiments of the present application. The one or more processors of the LoS-based path planning device can be processors of the same type, such as one or more CPUs, or processors of different types, such as one or more CPUs and one or more ASICs.
[0161] The memory 420 is configured to store the program 450. The memory 420 can include a high-speed RAM memory, and can further include a non-volatile memory such as at least one disk memory.
[0162] The program 450 can be specifically invoked by the processor 410 to enable the LoS-based path planning device to perform operations of the LoS-based path planning method according to any of the above embodiments.
[0163] In some optional embodiments, the embodiments of the present application further disclose a computer readable storage medium, and the storage medium stores at least one executable instruction. When the executable instruction is run on the LoS-based path planning device / apparatus, the LoS-based path planning device / apparatus performs steps of the LoS-based path planning method according to any of the above method embodiments.
[0164] The specific implementation process of the LoS-based path planning method according to the embodiments is described in the above method embodiments, and will not be repeated here.
[0165] In the description provided herein, numerous specific details are set forth. However, it is understood that embodiments of the application can be practiced without these specific details. In other instances, well-known methods, structures and techniques have not been described in detail in order to avoid obscuring the understanding of this description. Like reference numerals refer to like elements throughout. Similarly, while operations can be depicted in the drawings in a particular order, this should not be understood as requiring or
[0166] It is understood by those skilled in the art that modules in the apparatus of the embodiments can be adapted and placed in one or more apparatuses other than the embodiments. Modules or units or components in the embodiments can be combined into one module or unit or component, and further can be divided into multiple sub-modules or sub-units or sub-components. Except that at least some of such features and / or processes or units are mutually exclusive.
[0167] It is noted that the foregoing examples have been provided merely for the purpose of explanation and are in no way to be construed as limiting of the present application. While the application has been described with reference to preferred embodiments, it is understood that the words which have been used herein are words of description, and that details of the preferred embodiments are not intended to limit the scope of the application. Changes can be made to the embodiments in form and detail without departing from the spirit and the scope of the application. The application as claimed is intended to be fully covered by the following claims. In the claims, any reference signs placed between parentheses shall not be construed as limiting the claim. The word comprising does not exclude the presence of elements or steps not listed in a claim. The word "a" or "an" preceding an element does not exclude the presence of a plurality of such elements. The application can be implemented by means of both hardware and software, and any combinations thereof. In a unit claim, any reference signs placed between parentheses shall not be construed as limiting the claim. The use of the word "at least" followed by a list of one or more items does not exclude additional such items. The use of the words "one" or "the" to refer to an element or an action of the process does not exclude the presence of a plurality of such elements or actions. The word "first", "second", "third", and the like in the description does not necessarily indicate any order. It is understood that they are used to name different elements, and do not imply a certain order or position of the elements. The preceding description is not intended to limit the application. The scope of the application is defined by the appended claims.
Claims
1. A path planning method based on beyond visual range, characterized in that: The following steps are involved: Acquiring in real time beyond-horizon information and beyond-horizon positioning information marked with a first timestamp; Obtaining the world model of the vehicle coordinate system and the vehicle positioning information marked with the second timestamp in real time; Synchronize the first timestamp and the second timestamp with the cloud timestamp; Constructing a global world model based on the beyond-visual-range information, the beyond-visual-range positioning information, the vehicle coordinate system world model, the vehicle positioning information, and the cloud map data; Predicting the motion trajectory of a beyond-visual-range target and the motion trajectory of the ego vehicle within a preset time based on the global world model; The vehicle's trajectory is optimized based on the predicted trajectory of the beyond-visual-range target and the vehicle's trajectory.
2. The beyond-horizon path planning method according to claim 1, wherein: Synchronizing the first timestamp and the second timestamp with the cloud timestamp specifically includes: According to the dynamic adjustment process noise, the clock offset and clock drift of the first timestamp and the second timestamp are corrected in real time through the dynamic clock offset estimation algorithm of the Kalman filter, so that the first timestamp and the second timestamp are synchronized with the time of the cloud timestamp.
3. The beyond-horizon path planning method according to claim 2, wherein: Correcting the clock offset and clock drift of the first timestamp and the second timestamp in real time, specifically including: respectively predicting the clock offset and clock drift between the first timestamp and the second timestamp at the current moment and the reference timestamp at the current moment based on the calculated clock offset and clock drift between the first timestamp and the second timestamp at the previous moment and the reference timestamp at the previous moment; The clock offset and clock drift between the predicted first and second timestamps at the current moment and the reference timestamp at the current moment are compensated to the first and second timestamps at the current moment respectively, so as to correct the first and second timestamps at the current moment.
4. The beyond-visual-range path planning method according to claim 1, wherein: Build a global world model, including: Converting the beyond-visual-range coordinates and the vehicle coordinates into a global coordinate system where the cloud map is located according to the beyond-visual-range positioning information and the vehicle positioning information; The beyond-horizon information and the ego-vehicle coordinate system world model are integrated into the global coordinate system to generate a global world model.
5. The beyond-horizon path planning method according to claim 1, wherein: Predict the trajectory of beyond-visual-range targets and the trajectory of the ego vehicle within a preset time, specifically including: According to the global world model, obtaining the current motion state of the beyond-visual-range target and the current motion state of the ego vehicle; Based on the kinematic model of angular velocity compensation and combined with Kalman filtering, the motion trajectory of the beyond-visual-range target within a preset time and the motion trajectory of the ego vehicle within a preset time are calculated respectively.
6. The beyond-horizon path planning method according to claim 5, characterized in that: The kinematic model based on angular velocity compensation is: x(t)=x0+v·Δt·cos(θ+ωΔt); y(t)=y0+v·Δt·sin(θ+ωΔt); Where (x0, y0) represents the current position of the BVR target or ego vehicle, v represents the current velocity of the BVR target or ego vehicle, θ represents the current heading angle of the BVR target or ego vehicle, ω represents the current angular velocity of the BVR target or ego vehicle, Δt represents the predicted time interval of the BVR target or ego vehicle, and (x(t), y(t)) represents the position of the BVR target or ego vehicle after Δt.
7. The beyond-horizon path planning method according to claim 1, wherein: Optimizing the vehicle's trajectory based on the predicted trajectory of the beyond-visual-range target and the vehicle's trajectory specifically includes: The predicted overlap between the trajectory of the beyond-visual-range target and the trajectory of the ego vehicle is determined. If there is overlap, the trajectory of the ego vehicle is replanned and updated to the global world model. Otherwise, the trajectory of the ego vehicle remains unchanged.
8. The beyond-visual-range path planning method according to claim 1, wherein: The beyond-visual-range information is the road environment information in front of the vehicle acquired by the beyond-visual-range information acquisition module.
9. A path planning system based on beyond visual range, characterized in that: include: A beyond-visual-range information acquisition module, a self-vehicle, and a cloud server, wherein the beyond-visual-range information acquisition module and the self-vehicle are respectively in communication with the cloud server; The beyond-visual-range information acquisition module is used to collect road environment information in front of the vehicle, and send the road environment information marked with a first timestamp and positioning information of the beyond-visual-range information acquisition module to the cloud server; The ego vehicle is configured to send the ego vehicle coordinate system world model marked with a second timestamp and the ego vehicle positioning information to a cloud server, receive the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the ego vehicle sent by the cloud server, and optimize the ego vehicle driving trajectory according to the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the ego vehicle; The cloud server stores a cloud map, and the cloud server is used to synchronize the first timestamp and the second timestamp with the cloud timestamp; construct a global world model based on the road environment information, the positioning information of the beyond-visual-range information acquisition module, the vehicle coordinate system world model, the vehicle positioning information, and the cloud map data; predict the motion trajectory of the beyond-visual-range target and the motion trajectory of the vehicle within a preset time based on the global world model; and send the predicted motion trajectory of the beyond-visual-range target and the motion trajectory of the vehicle to the vehicle.
10. A path planning device based on beyond visual range, characterized in that: include: A processor, a memory, a communication interface, and a communication bus, wherein the processor, the memory, and the communication interface communicate with each other via the communication bus; The memory is used to store at least one executable instruction, and the executable instruction enables the processor to perform the operation of the beyond-visual-range path planning method as described in any one of claims 1-8.