A vehicle continuous positioning method, device, electronic equipment and storage medium

Abstract

The present application proposes a method, device, electronic device and storage medium for continuous positioning of a vehicle. The method includes: by combining point cloud data and environmental images, it is possible to more accurately identify environmental features, reduce misjudgment and interference of dynamic object feature points, and thus improve positioning accuracy in dynamic scenes. Point cloud data can dynamically adjust the feature point set to adapt to environmental changes, and through multi-sensor fusion algorithm optimization, improve data processing efficiency, adapt to high real-time requirements in industrial scenes, maintain positioning stability and accuracy, and indirectly reflect the vehicle load status through point cloud data and driving information; an adaptive adjustment mechanism designed for vehicle load changes, adjusts data fusion weights, and optimizes positioning and motion control strategies; and continuously optimizes the real-time path through a path smoothing algorithm, which can significantly reduce trajectory drift and interruption phenomena, and ensure smooth positioning of the target vehicle.

Description

Technical Field

[0001] The present invention relates to the technical field of vehicle positioning, and in particular to a vehicle continuous positioning method, device, electronic equipment and storage medium. Background Art

[0002] With the advancement of Industry 4.0, special frame vehicles are increasingly used in logistics, assembly, warehousing and other scenarios. Traditional positioning methods rely on LiDAR or monocular vision SLAM (Simultaneous Localization and Mapping) technology, but in dynamic environments, highly occluded areas and scenes with complex trajectories, the reliability of a single sensor is low, and it is prone to positioning drift, trajectory discontinuity and other problems.

[0003] In dynamic scenes, the existing technology has a large amount of misjudgment and interference in the feature points of dynamic objects, resulting in reduced positioning accuracy and discontinuous trajectories. Load changes of industrial frame vehicles (such as carrying heavy objects and uneven loads) will affect the rigid structure and motion characteristics of the vehicle, and the existing technology fails to perform adaptive optimization for load changes. In complex environments (such as turns and narrow passages), the trajectory is prone to drift or interruption, and smooth positioning cannot be guaranteed. Some technologies rely only on a single or limited sensor (such as monocular vision or lidar), and have deficiencies in data fusion and real-time performance. The existing technology does not combine the geometric characteristics of the frame vehicle for motion constraint modeling, and cannot effectively use the structural information of the frame vehicle to optimize the SLAM algorithm. Summary of the invention

[0004] The present application provides a vehicle continuous positioning method, device, electronic device and storage medium to solve the above technical problems.

[0005] A vehicle continuous positioning method provided in an embodiment of the present application includes: acquiring point cloud data, an environmental image and driving information of a target vehicle driving area; obtaining a driving map based on the point cloud data and the environmental image, and obtaining position and posture data of the target vehicle based on the point cloud data, the environmental image and the driving information; in the driving map, extracting the position and posture data of the target vehicle at consecutive moments, recording it as a real-time path, continuously optimizing the real-time path according to a path smoothing algorithm, obtaining the position of the target vehicle, and continuously positioning the target vehicle.

[0006] In one embodiment of the present application, a driving map is obtained based on the point cloud data and the environment image, including: processing the environment image through a deep learning semantic segmentation network to obtain a semantic image, wherein the semantic image includes image feature points; marking the dynamic image feature points in the semantic image as black pixels, and marking the static image feature points in the semantic image as white pixels to obtain a marked semantic image; removing the black pixels in the marked semantic image to obtain a screened semantic image, and filtering the feature points of the screened semantic image by pre- or real-time establishment of a framework vehicle geometry template library to obtain a target semantic image; constructing an initial map of the target area through the target semantic image and the point cloud data, and removing the external points of the initial map to obtain a driving map.

[0007] In one embodiment of the present application, obtaining the position and posture data of the target vehicle according to the point cloud data, the environment image and the driving information includes:

[0008] A vehicle state vector of a target vehicle is defined, wherein the vehicle state vector includes a vehicle position, direction, and speed; initial posture data of the target vehicle is predicted based on the vehicle state vector and the driving information through a kinematic model; the initial posture data is corrected based on the point cloud data and the environmental image to obtain corrected posture data; whether a load change of the target vehicle occurs is identified through the driving information, and the posture data of the target vehicle is obtained based on the load change result.

[0009] In one embodiment of the present application, it is identified whether the target vehicle has a load change through the driving information, and the posture data of the target vehicle is obtained according to the load change result, including: if it is identified that the target vehicle has no load change, the corrected posture data is used as the posture data of the target vehicle; if it is identified that the target vehicle has a load change, the driving information that causes the load change of the target vehicle is recorded as load change driving information, and the point cloud data corresponding to the load change driving information is recorded as load change point cloud data; the load change amount of the load change point cloud data is calculated, and the fusion weight of the load change driving information is adjusted according to the load change amount, and the fusion weight of the load change point cloud data is adjusted according to the load change amount; the posture data of the target vehicle is obtained according to the load change driving information, the load change point cloud data, the fusion weight of the load change driving information, and the fusion weight of the load change point cloud data.

[0010] In one embodiment of the present application, whether there is a load change on the target vehicle is identified through the driving information, including: when the target vehicle is unloaded, defining the acceleration of the target vehicle in an unloaded vehicle state, recorded as an acceleration reference value; calculating the center of gravity offset of the target vehicle according to the acceleration of the target vehicle in the driving information and the acceleration reference value; if the center of gravity offset is greater than or equal to a preset center of gravity offset threshold, determining that there is a load change on the target vehicle; if the center of gravity offset is less than the preset center of gravity offset threshold, determining that there is no load change on the target vehicle.

[0011] In one embodiment of the present application, the real-time path is continuity optimized according to a path smoothing algorithm, including: extracting the current moment position, current moment speed, previous moment position and previous moment speed of the target vehicle; identifying the path type of the real-time path, the path type including a straight-ahead scenario, a turning scenario and a narrow channel scenario; establishing a trajectory continuity constraint function according to the current moment position, current moment speed, previous moment position and previous moment speed of the target vehicle and the path type, the trajectory continuity constraint function including a straight-ahead scenario trajectory continuity constraint function, a turning scenario trajectory continuity constraint function and a narrow channel scenario trajectory continuity constraint function; and performing continuity optimization on the real-time path according to the trajectory continuity constraint function.

[0012] In one embodiment of the present application, a trajectory continuity constraint function is established according to the current moment position, current moment speed, previous moment position and previous moment speed of the target vehicle and the path type, including: if the path type is a straight scene, the straight scene trajectory continuity constraint function is established according to the current moment position, current moment speed, previous moment position and previous moment speed of the target vehicle and the straight speed smoothing weight; if the path type is a turning scene, the turning scene trajectory continuity constraint function is established according to the current moment position, current moment speed, previous moment position and previous moment speed of the target vehicle, the turning position smoothing weight and the turning speed smoothing weight; if the path type is a narrow channel scene, the narrow channel scene trajectory continuity constraint function is established according to the current moment position, current moment speed, previous moment position and previous moment speed of the target vehicle, the narrow channel position smoothing weight and the narrow channel speed smoothing weight.

[0013] The vehicle continuous positioning device provided in an embodiment of the present application includes: an acquisition module, which is used to acquire point cloud data, an environmental image and driving information of a target vehicle's driving area; a calculation module, which is used to obtain a driving map based on the point cloud data and the environmental image, and to obtain the position and posture data of the target vehicle based on the point cloud data, the environmental image and the driving information; a continuous positioning module, which is used to extract the position and posture data of the target vehicle at consecutive moments in the driving map, record it as a real-time path, continuously optimize the real-time path according to a path smoothing algorithm, obtain the position of the target vehicle, and continuously position the target vehicle.

[0014] An embodiment of the present application provides an electronic device, including a processor, a memory and a communication bus; the communication bus is used to connect the processor and the memory; the processor is used to execute a computer program stored in the memory to implement the above-mentioned vehicle continuous positioning method.

[0015] The beneficial effects of the present application are as follows: by combining point cloud data and environmental images, environmental features can be identified more accurately, and the misjudgment and interference of dynamic object feature points can be reduced, thereby improving the positioning accuracy in dynamic scenes. Point cloud data can dynamically adjust the feature point set to adapt to environmental changes. Through multi-sensor fusion algorithm optimization, data processing efficiency is improved to adapt to the high real-time requirements in industrial scenarios, and positioning stability and accuracy can be maintained. The vehicle load status can be indirectly reflected through point cloud data and driving information; an adaptive adjustment mechanism designed for vehicle load changes adjusts data fusion weights and optimizes positioning and motion control strategies; the real-time path is continuously optimized through a path smoothing algorithm, which can significantly reduce trajectory drift and interruption phenomena, and ensure smooth positioning of the target vehicle.

[0016] It should be understood that the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the present application. BRIEF DESCRIPTION OF THE DRAWINGS

[0017] The drawings herein are incorporated into the specification and constitute a part of the specification, showing embodiments consistent with the present application, and together with the specification, are used to explain the principles of the present application. Obviously, the drawings described below are only some embodiments of the present application, and for those of ordinary skill in the art, other drawings can be obtained based on these drawings without creative work. In the drawings:

[0018] Figure 1 is a flow chart of a vehicle continuous positioning method shown in an exemplary embodiment of the present application;

[0019] Figure 2is a real-time environment architecture diagram of a vehicle continuous positioning method shown in an exemplary embodiment of the present application;

[0020] Figure 3 is a block diagram of a vehicle continuous positioning device shown in an exemplary embodiment of the present application;

[0021] Figure 4 It is a structural schematic diagram of an electronic device shown in an exemplary embodiment of the present application. DETAILED DESCRIPTION

[0022] The following will describe the implementation methods of the present application with reference to the accompanying drawings and preferred embodiments. Those skilled in the art can easily understand other advantages and effects of the present application from the contents disclosed in this specification. The present application can also be implemented or applied through other different specific implementation methods, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present application. It should be understood that the preferred embodiments are only for illustrating the present application, not for limiting the scope of protection of the present application.

[0023] It should be noted that the illustrations provided in the following embodiments are only schematic illustrations of the basic concept of the present application, and thus the drawings only show components related to the present application rather than being drawn according to the number, shape and size of components in actual implementation. In actual implementation, the type, quantity and proportion of each component may be changed at will, and the component layout may also be more complicated.

[0024] In the following description, a large number of details are discussed to provide a more thorough explanation of the embodiments of the present application. However, it is obvious to those skilled in the art that the embodiments of the present application can be implemented without these specific details. In other embodiments, well-known structures and devices are shown in the form of block diagrams rather than in detail to avoid making the embodiments of the present application difficult to understand.

[0025] The embodiments of the present application respectively propose a vehicle continuous positioning method, a vehicle continuous positioning device, and an electronic device, and these embodiments will be described in detail below.

[0026] See also Figure 1 , Figure 1 FIG. 1 shows a flow chart of a vehicle continuous positioning method according to an embodiment of the present application. Figure 1 As shown, the method at least includes steps S110 to S130, which are described in detail as follows:

[0027] Step S110, obtaining point cloud data of the target vehicle's driving area, environmental images, and driving information of the target vehicle.

[0028] As an example, the target vehicle's driving area can be delineated by setting a preset radius with the vehicle as the center of the circle. The point cloud data of the surrounding environment is collected by the lidar to obtain the outline, distance and shape information of objects in the environment. The point cloud data is collected at a fixed period, such as 100ms (milliseconds), to obtain the point cloud data of the target vehicle's driving area; the environmental image is collected by a monocular camera, and the driving information of the target vehicle is collected by an IMU (Inertial Measurement Unit). The point cloud data, the environmental image and the driving information of the target vehicle are fused in real time to construct a high-precision environmental map to continuously locate the target vehicle.

[0029] As an example, multi-sensor fusion refers to the integration of data from multiple sensors, such as lidar, binocular camera, and inertial measurement unit, to improve the accuracy and robustness of environmental perception. Fusion algorithms usually use methods such as extended Kalman filtering or particle filtering to jointly optimize multi-source data. The IMU pre-integration algorithm is an algorithm that combines high-frequency IMU data, such as velocity and angular velocity, with low-frequency sensor data, such as camera or lidar, to estimate inter-frame pose changes and compensate for positioning errors caused by sensor frequency differences, such as compensating for motion blur in visual and lidar data. IMU data is updated at a high frequency, such as 100 Hz (hertz), to provide inter-frame pose changes.

[0030] As an example, the target vehicle may be a special frame vehicle, which is a special vehicle used in industrial transportation, assembly or logistics scenarios, usually with a modular frame structure and multi-load adaptability. The vehicle usually has low-speed and high-precision movement capabilities, and is suitable for rail transportation, narrow channel operations, and material handling tasks in high-dynamic environments.

[0031] As an example, visual SLAM is used to extract, match and optimize image feature points to achieve position tracking and mapping in a dynamic environment. Visual SLAM refers to a technology that simultaneously builds an environmental map and locates itself based on data collected by cameras and lidar.

[0032] Step S120, obtaining a driving map according to the point cloud data and the environment image, and obtaining position and posture data of the target vehicle according to the point cloud data, the environment image and the driving information.

[0033] In one embodiment of the present application, a driving map is obtained based on point cloud data and an environmental image, including: processing the environmental image through a deep learning semantic segmentation network to obtain a semantic image, the semantic image including image feature points; marking the dynamic image feature points in the semantic image as black pixels, and recording the static image feature points in the semantic image as white pixels to obtain a marked semantic image; removing the black pixels in the marked semantic image to obtain a screened semantic image, and filtering the feature points of the screened semantic image by pre- or real-time establishment of a framework vehicle geometry template library to obtain a target semantic image; constructing an initial map of the target area through the target semantic image and point cloud data, and removing the external points of the initial map to obtain a driving map. Dynamic feature point removal combined with the framework vehicle geometry template library can effectively reduce errors in dynamic environments. By designing a dynamic feature point removal algorithm, interference feature points caused by dynamic objects (such as pedestrians, vehicles, etc.) can be effectively removed, thereby improving the accuracy and stability of map construction, thereby obtaining a more accurate high-precision map.

[0034] As an example, the deep learning semantic segmentation network can be PSPNet (Pyramid Scene Parsing Network, a deep learning algorithm for image semantic segmentation). The monocular camera image is processed by PSPNet to generate a semantic image. In the semantic image, image feature points such as edges and corners are extracted by ORB (Oriented FAST and Rotated BRIEF, an algorithm for fast feature point extraction and description). The extracted feature points are used to match the features of static objects and dynamic objects in the environment. In the semantic image, dynamic objects such as pedestrians and vehicles are marked as black pixels, and static objects such as roads and buildings are marked as white pixels. If the feature point is located in the black pixel area (dynamic object), the feature point is removed; if the feature point is located in the white pixel area (static object), the feature point is retained. Based on the frame vehicle geometry template library, the remaining feature points are filtered to ensure that the feature points are evenly distributed and cover the key areas around the vehicle. The filtered feature points and the lidar point cloud data are used to build an initial map of the environment. The point cloud data is matched with the feature points and the RANSAC (Random Sample Conversion) algorithm is used to match the feature points. Consensus, random sample consensus algorithm) algorithm removes outliers and optimizes map accuracy to obtain a driving map. Dynamic feature point removal combined with the framework vehicle geometry template library effectively reduces errors in dynamic environments. By designing a dynamic feature point removal algorithm, it can effectively remove interfering feature points caused by dynamic objects (such as pedestrians, vehicles, etc.), thereby improving the accuracy and stability of map construction, and obtaining a more accurate high-precision map.

[0035] As an example, the framework feature template library is a collection of feature templates built based on the geometric shape of the framework vehicle, which is used to guide the feature matching and optimization of special framework vehicles in visual SLAM. The template library content includes vehicle body constraints, load change models and geometric features.

[0036] In one embodiment of the present application, the newly collected lidar point cloud is matched with the driving map (ICP algorithm). If a new static object is detected, the corresponding point cloud data is added to the driving map; if a dynamic object is detected, its point cloud data is removed to prevent map contamination.

[0037] In one embodiment of the present application, the pose data of the target vehicle is obtained according to the point cloud data, the environmental image and the driving information, including: defining the vehicle state vector of the target vehicle, the vehicle state vector of the vehicle includes the vehicle position, direction and speed; predicting the initial pose data of the target vehicle according to the vehicle state vector and the driving information through the kinematic model; correcting the initial pose data according to the point cloud data and the environmental image to obtain the corrected pose data; identifying whether there is a load change in the target vehicle through the driving information, and obtaining the pose data of the target vehicle according to the load change result. By integrating multi-source data such as LiDAR, monocular camera and IMU, and performing real-time fusion, the advantages of various sensors can be fully utilized, and the shortcomings can be complemented, thereby improving the positioning accuracy.

[0038] As an example, multi-sensor data fusion is performed through the extended Kalman filter of multi-sensor data fusion. The extended Kalman filter is a common method for processing vehicle state estimation of nonlinear systems. It dynamically fuses multi-sensor data (lidar, monocular camera and IMU) through prediction update and observation update. The following is the specific implementation process:

[0039] First, define the vehicle state vector of the target vehicle. The vehicle state vector of the vehicle includes the vehicle position, direction, and speed. The vehicle state vector X is defined as the vehicle's position and speed:

[0040]

[0041] Where X is the vehicle state vector of the target vehicle, (x, y, z) is the three-dimensional spatial position of the target vehicle, ψ is the pitch angle of the target vehicle, θ is the roll angle of the target vehicle, and ψ is the yaw angle of the target vehicle; v x is the speed of the target vehicle in the x direction, v y is the speed of the target vehicle in the y direction, v z is the speed of the target vehicle in the z direction.

[0042] The kinematic model is used to predict the initial position data of the target vehicle based on the vehicle state vector and driving information. The kinematic model is used to predict the vehicle state based on the driving information, i.e., IMU data (acceleration and angular velocity):

[0043] Vehicle state transfer equation:

[0044] X k|k-1 =f(X k-1 )+u k

[0045] Among them, X k|k-1 is the vehicle state prediction at time k, based on the information at time k-1, f(X k-1 ) is the transition relationship from the vehicle state at time k to the vehicle state at time k-1, u k For interference.

[0046] The nonlinear vehicle state transfer function f(X) is:

[0047]

[0048] φ k|k-1 =φ k-1 +ω x Δt,

[0049] θ k|k-1 =θ k-1 +ω y Δt,

[0050] ψ k|k-1 =ψ k-1 +ω z Δt.

[0051] Among them, x k|k-1 is the predicted value of the x coordinate at time k, based on the information at time k-1; y k|k-1 is the predicted value of the x coordinate at time k, based on the information at time k-1; z k|k-1 is the predicted value of the x coordinate at time k, based on the information at time k-1, φ k|k-1 is the predicted roll angle at time k, based on the information at time k-1, θ k|k-1 is the predicted value of the pitch angle at time k, based on the information at time k-1, ψ k|k-1 The predicted value of the yaw angle at time k, based on the information at time k-1, v x is the speed of the target vehicle in the x direction, v y is the speed of the target vehicle in the y direction, v z is the speed of the target vehicle in the z direction, a x is the acceleration of the target vehicle in the x direction, a yis the acceleration of the target vehicle in the y direction, a z is the acceleration of the target vehicle in the z direction, Δt is the time interval, indicating the time difference from time k to time k-1, ω x is the angular velocity of the target vehicle in the x direction, ω y is the angular velocity of the target vehicle in the y direction, ω z is the angular velocity of the target vehicle in the z direction.

[0052] Vehicle state covariance prediction:

[0053]

[0054] P k|k-1 is the predicted covariance matrix of the vehicle state at time k, reflecting the uncertainty of the vehicle state estimation, P k-1 is the vehicle state covariance matrix at time k-1, indicating the uncertainty of the previous vehicle state, F k-1 is the vehicle state transfer matrix, obtained by calculating the Jacobian matrix of f(X), Q k is the process noise covariance matrix, which represents the IMU measurement error.

[0055] Correct the vehicle state based on the feature points extracted by the lidar point cloud and the monocular camera:

[0056] Measurement Model:

[0057] Z k =h(X k )+v k ;

[0058] Among them, Z k is the sensor measurement value, including the laser radar point cloud position and the camera feature point position; h(X k ) is a nonlinear measurement equation that maps the vehicle state vector to the observation space; v k To measure noise.

[0059] Kalman gain calculation:

[0060]

[0061] Among them, K k is the Kalman gain, P k|k-1 is the predicted covariance matrix of the vehicle state at time k, reflecting the uncertainty of the vehicle state estimation, H k is the measurement matrix, and the Jacobian matrix of h(X) is obtained; R k is the measurement noise covariance matrix.

[0062] Among them, the covariance update is:

[0063] P k =(IK k H k ) k|k-1 ;

[0064] P k is the predicted covariance matrix of the vehicle state at time k, reflecting the uncertainty of the vehicle state estimation, I is the unit matrix, K k is the Kalman gain, P k|k-1 is the predicted covariance matrix of the vehicle state at time k, based on the information at time k-1, H k is the measurement matrix, and finds the Jacobian matrix for h(X).

[0065] Vehicle status update:

[0066] X k =X k|k-1 +K k (Z k -h(X k|k-1 ));

[0067] X k is the vehicle state at time k, X k|k-1 is the vehicle state prediction at time k, based on the information at time k-1, f(X k-1 ) is the transition relationship from the vehicle state at time k to the vehicle state at time k-1, u k is the interference term, h(X k|k-1 ) is the observation function, K k is the Kalman gain.

[0068] In one embodiment of the present application, it is determined whether the target vehicle has a load change through driving information, and the position and posture data of the target vehicle are obtained according to the load change result, including: if it is determined that the target vehicle has no load change, the corrected position and posture data are used as the position and posture data of the target vehicle; if it is determined that the target vehicle has a load change, the driving information that causes the load change of the target vehicle is recorded as load change driving information, and the point cloud data corresponding to the load change driving information is recorded as load change point cloud data; the load change amount of the load change point cloud data is calculated, and the fusion weight of the load change driving information is adjusted according to the load change amount, and the fusion weight of the load change point cloud data is adjusted according to the load change amount; the position and posture data of the target vehicle are obtained according to the load change driving information, the load change point cloud data, the fusion weight of the load change driving information, and the fusion weight of the load change point cloud data. In view of the load change of the frame vehicle, the sensor weight and optimization parameters can be automatically adjusted to ensure the stability and accuracy of the positioning system, and the adaptive adjustment mechanism designed for the load change of the frame vehicle ensures the robustness of the system in multi-load scenarios.

[0069] As an example, load changes have a significant impact on vehicle motion characteristics. By adjusting the fusion weight of sensor data to ensure positioning accuracy and robustness, the target vehicle can be identified through driving information to see whether there is a load change. The target vehicle's position data is obtained based on the load change results:

[0070] First, use IMU data to detect the center of gravity offset or acceleration change caused by the load:

[0071] ΔC=||a IMU -a 参考 ||;

[0072] Among them, ΔC is the center of gravity offset, a IMU is the current IMU acceleration, a 参考 It is the acceleration reference value of the unloaded vehicle state.

[0073] Analyze payload changes using LiDAR point clouds:

[0074]

[0075] ΔW is the load change, i is the i-th point, N is the total number of point clouds, p i is the load change point cloud data, p 参考 Benchmark point cloud data.

[0076] Adjust weights based on load changes:

[0077] When the load increases:

[0078] w LiDAR =w LiDAR +k·ΔWw IMU =w IMU -k·ΔW;

[0079] When the load decreases:

[0080] w LiDAR =w LiDAR -k·ΔWw IMU =w IMU +k·ΔW;

[0081] Among them, w LiDAR is the fusion weight of the load-varying point cloud data, w IMU is the fusion weight of load change driving information, ΔW is the load change, and k is the adjustment coefficient, which is proportional to the load change amplitude.

[0082] w LiDAR +w IMU =1

[0083] The adjusted weights are used for EKF data fusion:

[0084] X k =w LiDAR ·X LiDAR,k +w IMU ·X IMU,k

[0085] X k is the vehicle state at time k, i.e. the position data of the target vehicle, w LiDAR is the fusion weight of the load-varying point cloud data, w IMU is the fusion weight of load change driving information, X LiDAR,k is the load change point cloud data, X IMU,k The system can maintain positioning accuracy and trajectory continuity when the load changes by dynamically adjusting the weights.

[0086] In one embodiment of the present application, whether there is a load change on the target vehicle is identified through driving information, including: when the target vehicle is unloaded, defining the acceleration of the target vehicle in an unloaded vehicle state, recorded as an acceleration reference value; calculating the center of gravity offset of the target vehicle based on the acceleration of the target vehicle in the driving information and the acceleration reference value; if the center of gravity offset is greater than or equal to a preset center of gravity offset threshold, determining that there is a load change on the target vehicle; if the center of gravity offset is less than the preset center of gravity offset threshold, determining that there is no load change on the target vehicle.

[0087] As an example, the preset center of gravity offset is 10%. If the center of gravity offset is greater than or equal to 10%, it is determined that there is a load change on the target vehicle; if the center of gravity offset is less than 10%, it is determined that there is no load change on the target vehicle.

[0088] Step S130, in the driving map, extract the position data of the target vehicle at consecutive moments, record it as a real-time path, perform continuity optimization on the real-time path according to a path smoothing algorithm, obtain the position of the target vehicle, and continuously locate the target vehicle.

[0089] In one embodiment of the present application, the real-time path is optimized for continuity according to the path smoothing algorithm, including: extracting the current moment position, current moment speed, last moment position and last moment speed of the target vehicle; identifying the path type of the real-time path, the path type includes a straight scene, a turning scene and a narrow channel scene; establishing a trajectory continuity constraint function according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle and the path type, the trajectory continuity constraint function includes a straight scene trajectory continuity constraint function, a turning scene trajectory continuity constraint function and a narrow channel scene trajectory continuity constraint function; the real-time path is optimized for continuity according to the trajectory continuity constraint function. The trajectory optimization algorithm can further smooth and optimize the vehicle's driving trajectory, reduce positioning errors and fluctuations, introduce a trajectory continuity constraint model, and ensure that the vehicle maintains smooth positioning in a complex path, avoiding navigation errors caused by positioning mutations; smooth positioning can increase the accuracy of vehicle positioning, and the trajectory continuity constraint model ensures smooth positioning in a complex path, solving the problem of trajectory drift in the prior art.

[0090] In one embodiment of the present application, a trajectory continuity constraint function is established according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle and the path type, including: if the path type is a straight scene, a straight scene trajectory continuity constraint function is established according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle and the straight speed smoothing weight; if the path type is a turning scene, a turning scene trajectory continuity constraint function is established according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle, turning position smoothing weight and turning speed smoothing weight; if the path type is a narrow channel scene, a narrow channel scene trajectory continuity constraint function is established according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle, narrow channel position smoothing weight and narrow channel speed smoothing weight. The real-time path is continuously optimized according to the path smoothing algorithm, and the optimization strategy is adaptively adjusted according to different path scenes, so that the vehicle positioning is more accurate.

[0091] As an example, the real-time path is optimized for continuity according to the path smoothing algorithm. The real-time path includes straight driving, turning and narrow passages. If the path type is a straight driving scene, the straight driving scene trajectory continuity constraint function is established according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle and the straight driving speed smoothing weight:

[0092]

[0093] Among them, J is the straight scene path, pt is the position of the vehicle at time t, p t =[x t ,y t ,z t ],v t is the speed of the vehicle at time t, v t =[v x,t ,v y,t ,v z,t ], λ is the speed smoothing weight, p t-1 is the position of the vehicle at time t-1, v t-1 is the speed of the vehicle at time t-1.

[0094] If the path type is a turning scene, a turning scene trajectory continuity constraint function is established based on the current position, current speed, previous position and speed of the target vehicle, the turning position smoothing weight and the turning speed smoothing weight, increasing the position weight and reducing the speed weight;

[0095] Trajectory continuity constraint function for turning scenarios:

[0096]

[0097] Among them, J 转弯 is the turning scene path, p t =[x t ,y t ,z t ] is the position of the vehicle at time t, v t =[v x,t ,v y,t ,v z,t ]: speed of the vehicle at time t, p t-1 is the position of the vehicle at time t-1, v t-1 is the speed of the vehicle at time t-1, α is the turning position smoothing weight, β is the turning speed smoothing weight, where α>1,β<1: enhance position smoothness and reduce speed change constraints.

[0098] According to the current position, current speed, previous position and speed of the target vehicle, the narrow channel position smoothing weight and the narrow channel speed smoothing weight, the narrow channel scene trajectory continuity constraint function is established to increase the smoothness weight of the position and speed to prevent the path from deviating from obstacles; narrow channel scene trajectory continuity constraint function:

[0099]

[0100] Among them, J 狭窄 is the narrow channel path, p t =[x t ,yt ,z t ] is the position of the vehicle at time t, v t =[v x,t ,v y,t ,v z,t ]: speed of the vehicle at time t, p t-1 is the position of the vehicle at time t-1, v t-1 is the speed of the vehicle at time t-1, γ is the narrow channel position smoothing weight, δ is the narrow channel speed smoothing weight, where γ,δ>1: ensure smooth driving.

[0101] As a specific example, a Bezier curve is used to fit the trajectory to ensure that the curve is smooth and satisfies constraints; or a gradient descent method is used to optimize the implementation path and update the position and speed of the trajectory points.

[0102] exist Figure 1 In the technical solution shown, by combining point cloud data and environmental images, environmental features can be identified more accurately, and the misjudgment and interference of dynamic object feature points can be reduced, thereby improving the positioning accuracy in dynamic scenes. Point cloud data can dynamically adjust the feature point set to adapt to environmental changes. Through multi-sensor fusion algorithm optimization, data processing efficiency is improved to adapt to the high real-time requirements in industrial scenarios. The stability and accuracy of positioning can be maintained, and the vehicle load status can be indirectly reflected through point cloud data and driving information; an adaptive adjustment mechanism designed for vehicle load changes adjusts data fusion weights and optimizes positioning and motion control strategies; the real-time path is continuously optimized through a path smoothing algorithm, which can significantly reduce trajectory drift and interruption phenomena and ensure smooth positioning of the target vehicle.

[0103] In one embodiment of the present application, obtaining a driving map includes: collecting point cloud data at the next moment to obtain new point cloud data; if new static feature points are detected in the new point cloud data, the new static feature points are added to the driving map; if new dynamic feature points are detected in the new point cloud data, the new dynamic feature points are removed. Based on multi-sensor data, the environmental map is updated in real time to ensure the accuracy and timeliness of the map. Through the SLAM algorithm, the precise positioning of the vehicle can be achieved, providing a reliable foundation for autonomous driving and navigation.

[0104] In one embodiment of the present application, dynamic feature point removal can also use optical flow method or deep learning model (such as YOLO) to detect dynamic objects and directly remove dynamic points through geometric consistency detection.

[0105] In one embodiment of the present application, the trajectory constraint function may also use a trajectory smoothing method based on a Bezier curve or a Kalman filter.

[0106] In one embodiment of the present application, a low-cost ultrasonic sensor may be used to collect point cloud data, and a depth camera may be used to collect environment images.

[0107] As an example, Figure 2 According to the embodiment of the present application, an implementation environment architecture diagram of vehicle continuous positioning is shown, including a multi-sensor fusion module 210, a framework feature optimization module 220, a trajectory continuity constraint module 230, a SLAM positioning and map update module 240, and a load adaptive adjustment module 250. The multi-sensor fusion module is used to integrate laser radar, monocular camera and inertial measurement unit (IMU), and to fuse multi-source data in real time to build a high-precision environment map; the framework feature optimization module is used to design a dynamic feature point removal algorithm and a trajectory optimization algorithm in combination with the geometric shape of the framework vehicle; the trajectory continuity constraint module is used to introduce a trajectory continuity constraint model to ensure smooth positioning of the vehicle in a complex path; the SLAM positioning and map update module is used to update the environment map in real time and perform vehicle positioning based on multi-sensor data; the load adaptive adjustment module is used to adjust the sensor weights and optimization parameters according to the load changes of the framework vehicle to ensure positioning robustness. By integrating multi-source data such as lidar, monocular camera and IMU, and performing real-time fusion, we can fully utilize the advantages of various sensors and complement their shortcomings, thereby improving positioning accuracy. Multi-sensor fusion can provide more comprehensive and richer environmental information, which helps to build a more accurate and complete environmental map. In the event of failure or interference of a single sensor, other sensors can continue to work and provide effective data support, thereby improving the overall robustness of the system; by designing a dynamic feature point removal algorithm, we can effectively remove interfering feature points caused by dynamic objects (such as pedestrians, vehicles, etc.), thereby improving the accuracy and stability of map construction; the trajectory optimization algorithm can further smooth and optimize the vehicle's driving trajectory, reduce positioning errors and fluctuations, and introduce trajectory The trajectory continuity constraint model can ensure that the vehicle maintains smooth positioning in complex paths and avoid navigation errors caused by sudden positioning changes; smooth positioning can increase the accuracy of vehicle positioning. The trajectory continuity constraint model ensures smooth positioning in complex paths and solves the problem of trajectory drift in existing technologies; based on multi-sensor data, the environmental map is updated in real time to ensure the accuracy and timeliness of the map. Through the SLAM algorithm, the vehicle can be accurately positioned, providing a reliable foundation for autonomous driving and navigation; in response to changes in the frame vehicle load, the sensor weights and optimization parameters can be automatically adjusted to ensure the stability and accuracy of the positioning system. The adaptive adjustment mechanism designed for changes in the frame vehicle load ensures the robustness of the system in multi-load scenarios.

[0108] Figure 3 FIG. 1 is a block diagram of a vehicle continuous positioning device according to an exemplary embodiment of the present application. Figure 3As shown, a vehicle continuous positioning device 300 according to an embodiment of the present application includes: an acquisition module 310, a calculation module 320 and a continuous positioning module 330. The acquisition module is used to acquire point cloud data, an environmental image and driving information of the target vehicle's driving area; the calculation module is used to obtain a driving map according to the point cloud data and the environmental image, and to obtain the position and posture data of the target vehicle according to the point cloud data, the environmental image and the driving information; the continuous positioning module is used to extract the position and posture data of the target vehicle at consecutive moments in the driving map, record it as a real-time path, continuously optimize the real-time path according to the path smoothing algorithm, obtain the position of the target vehicle, and continuously position the target vehicle. By combining point cloud data and environmental images, environmental features can be identified more accurately, and the misjudgment and interference of dynamic object feature points can be reduced, thereby improving the positioning accuracy in dynamic scenes. Point cloud data can dynamically adjust the feature point set to adapt to environmental changes. Through multi-sensor fusion algorithm optimization, data processing efficiency can be improved to adapt to the high real-time requirements in industrial scenarios. The stability and accuracy of positioning can be maintained, and the vehicle load status can be indirectly reflected through point cloud data and driving information; the adaptive adjustment mechanism designed for vehicle load changes adjusts the data fusion weight and optimizes the positioning and motion control strategies; the real-time path is continuously optimized through the path smoothing algorithm, which can significantly reduce the drift and interruption of the trajectory and ensure the smooth positioning of the target vehicle.

[0109] In one embodiment of the present application, the computing module is used to process the environment image through a deep learning semantic segmentation network to obtain a semantic image, which includes image feature points; the dynamic image feature points in the semantic image are marked as black pixels, and the static image feature points in the semantic image are recorded as white pixels to obtain a marked semantic image; the black pixels in the marked semantic image are removed to obtain a screened semantic image, and the screened semantic image is filtered by pre- or real-time establishment of a framework vehicle geometry template library to obtain a target semantic image; the initial map of the target area is constructed through the target semantic image and point cloud data, and the external points of the initial map are removed to obtain a driving map. Dynamic feature point removal is combined with the framework vehicle geometry template library to effectively reduce errors in dynamic environments. By designing a dynamic feature point removal algorithm, interference feature points caused by dynamic objects (such as pedestrians, vehicles, etc.) can be effectively removed, thereby improving the accuracy and stability of map construction, thereby obtaining a more accurate high-precision map.

[0110] As an example, the computing module used for deep learning semantic segmentation network can be PSPNet. The monocular camera image is processed by PSPNet to generate a semantic image. In the semantic image, image feature points such as edges and corners are extracted by ORB (Oriented FAST and Rotated BRIEF, an algorithm for fast feature point extraction and description) algorithm. The extracted feature points are used to match the features of static objects and dynamic objects in the environment. In the semantic image, dynamic objects such as pedestrians and vehicles are marked as black pixels, and static objects such as roads and buildings are marked as white pixels. If the feature point is located in the black pixel area (dynamic object), the feature point is removed; if the feature point is located in the white pixel area (static object), the feature point is retained. Based on the framework vehicle geometry template library, the remaining feature points are filtered to ensure that the feature points are evenly distributed and cover the key areas around the vehicle. The filtered feature points and lidar point cloud data are used to build an initial map of the environment, the point cloud data is matched with the feature points, and the RANSAC (Random Sample Consensus) algorithm is used to remove external points and optimize the map accuracy to obtain a driving map. Dynamic feature point removal combined with the framework vehicle geometry template library can effectively reduce errors in dynamic environments. By designing a dynamic feature point removal algorithm, it can effectively remove interference feature points caused by dynamic objects (such as pedestrians, vehicles, etc.), thereby improving the accuracy and stability of map construction, and obtaining a more accurate high-precision map.

[0111] In one embodiment of the present application, the calculation module is used to define the vehicle state vector of the target vehicle, which includes the vehicle position, direction and speed; the initial posture data of the target vehicle is predicted based on the vehicle state vector and driving information through the kinematic model; the initial posture data is corrected based on the point cloud data and the environmental image to obtain the corrected posture data; the target vehicle is identified through the driving information to see whether there is a load change, and the posture data of the target vehicle is obtained based on the load change result. By integrating multi-source data such as LiDAR, monocular camera and IMU, and performing real-time fusion, the advantages of various sensors can be fully utilized, and their shortcomings can be complemented, thereby improving positioning accuracy.

[0112] As an example, multi-sensor data fusion is performed through the extended Kalman filter of multi-sensor data fusion. The extended Kalman filter is a common method for processing vehicle state estimation of nonlinear systems. It dynamically fuses multi-sensor data (lidar, monocular camera and IMU) through prediction update and observation update. The following is the specific implementation process:

[0113] First, define the vehicle state vector of the target vehicle. The vehicle state vector of the vehicle includes the vehicle position, direction, and speed. The vehicle state vector X is defined as the vehicle's position and speed:

[0114]

[0115] Where X is the vehicle state vector of the target vehicle, (x, y, z) is the three-dimensional spatial position of the target vehicle, φ is the pitch angle of the target vehicle, θ is the roll angle of the target vehicle, and ψ is the yaw angle of the target vehicle; v x is the speed of the target vehicle in the x direction, v y is the speed of the target vehicle in the y direction, v z is the speed of the target vehicle in the z direction.

[0116] The kinematic model is used to predict the initial position data of the target vehicle based on the vehicle state vector and driving information. The kinematic model is used to predict the vehicle state based on the driving information, i.e., IMU data (acceleration and angular velocity):

[0117] Vehicle state transfer equation:

[0118] X k|k-1 =f(X k-1 )+u k

[0119] Among them, X k|k-1 is the vehicle state prediction at time k, based on the information at time k-1, f(X k-1 ) is the transition relationship from the vehicle state at time k to the vehicle state at time k-1, u k For interference.

[0120] The nonlinear vehicle state transfer function f(X) is:

[0121]

[0122] φ k|k-1 =φ k-1 +ω x Δt,

[0123] θ k|k-1 =θ k-1 +ω y Δt,

[0124] ψ k|k-1 =ψ k-1 +ω z Δt.

[0125] Among them, x k|k-1 is the predicted value of the x coordinate at time k, based on the information at time k-1; y k|k-1 is the predicted value of the x coordinate at time k, based on the information at time k-1; z k|k-1 is the predicted value of the x coordinate at time k, based on the information at time k-1, φ k|k-1is the predicted roll angle at time k, based on the information at time k-1, θ k|k-1 is the predicted value of the pitch angle at time k, based on the information at time k-1, ψ k|k-1 The predicted value of the yaw angle at time k, based on the information at time k-1, v x is the speed of the target vehicle in the x direction, v y is the speed of the target vehicle in the y direction, v z is the speed of the target vehicle in the z direction, a x is the acceleration of the target vehicle in the x direction, a y is the acceleration of the target vehicle in the y direction, a z is the acceleration of the target vehicle in the z direction, Δt is the time interval, indicating the time difference from time k to time k-1, ω x is the angular velocity of the target vehicle in the x direction, ω y is the angular velocity of the target vehicle in the y direction, ω z is the angular velocity of the target vehicle in the z direction.

[0126] Vehicle state covariance prediction:

[0127]

[0128] P k|k-1 is the predicted covariance matrix of the vehicle state at time k, reflecting the uncertainty of the vehicle state estimation, P k-1 is the vehicle state covariance matrix at time k-1, indicating the uncertainty of the previous vehicle state, F k-1 is the vehicle state transfer matrix, obtained by calculating the Jacobian matrix of f(X), Q k is the process noise covariance matrix, which represents the IMU measurement error.

[0129] Correct the vehicle state based on the feature points extracted by the lidar point cloud and the monocular camera:

[0130] Measurement Model:

[0131] Z k =h(X k )+v k ;

[0132] Among them, Z k is the sensor measurement value, including the laser radar point cloud position and the camera feature point position; h(X k ) is a nonlinear measurement equation that maps the vehicle state vector to the observation space; v k To measure noise.

[0133] Kalman gain calculation:

[0134]

[0135] Among them, K k is the Kalman gain, P k|k-1 is the predicted covariance matrix of the vehicle state at time k, reflecting the uncertainty of the vehicle state estimation, H k is the measurement matrix, and the Jacobian matrix of h(X) is obtained; R k is the measurement noise covariance matrix.

[0136] Among them, the covariance update is:

[0137] P k =(IK k H k ) k|k-1 ;

[0138] P k is the predicted covariance matrix of the vehicle state at time k, reflecting the uncertainty of the vehicle state estimation, I is the unit matrix, K k is the Kalman gain, P k|k-1 is the predicted covariance matrix of the vehicle state at time k, based on the information at time k-1, H k is the measurement matrix, and finds the Jacobian matrix for h(X).

[0139] Vehicle status update:

[0140]

[0141] X k is the vehicle state at time k, X k|k-1 is the vehicle state prediction at time k, based on the information at time k-1, f(X k-1 ) is the transition relationship from the vehicle state at time k to the vehicle state at time k-1, u k is the interference term, h(X k|k-1 ) is the observation function, K k is the Kalman gain.

[0142] In one embodiment of the present application, the calculation module is used to use the corrected posture data as the posture data of the target vehicle if it is identified that there is no load change on the target vehicle; if it is identified that there is a load change on the target vehicle, the driving information that causes the load change on the target vehicle is recorded as the load change driving information, and the point cloud data corresponding to the load change driving information is recorded as the load change point cloud data; the load change amount of the load change point cloud data is calculated, and the fusion weight of the load change driving information is adjusted according to the load change amount, and the fusion weight of the load change point cloud data is adjusted according to the load change amount; the posture data of the target vehicle is obtained according to the load change driving information, the load change point cloud data, the fusion weight of the load change driving information, and the fusion weight of the load change point cloud data. In response to the load change of the frame vehicle, the sensor weight and optimization parameters can be automatically adjusted to ensure the stability and accuracy of the positioning system. The adaptive adjustment mechanism designed for the load change of the frame vehicle ensures the robustness of the system in multi-load scenarios.

[0143] As an example, load changes have a significant impact on vehicle motion characteristics. By adjusting the fusion weight of sensor data to ensure positioning accuracy and robustness, the target vehicle can be identified through driving information to see whether there is a load change. The target vehicle's position data is obtained based on the load change results:

[0144] First, use IMU data to detect the center of gravity offset or acceleration change caused by the load:

[0145] ΔC=||a IMU -a 参考 ||;

[0146] Among them, ΔC is the center of gravity offset, a IMU is the current IMU acceleration, a 参考 It is the acceleration reference value of the unloaded vehicle state.

[0147] Analyze payload changes using LiDAR point clouds:

[0148]

[0149] ΔW is the load change, i is the i-th point, N is the total number of point clouds, p i is the load change point cloud data, p 参考 Benchmark point cloud data.

[0150] Adjust weights based on load changes:

[0151] When the load increases:

[0152] w LiDAR =w LiDAR +k·ΔWw IMU =w IMU -k·ΔW;

[0153] When the load decreases:

[0154] w LiDAR =w LiDAR -k·ΔWw IMU =w IMU +k·ΔW;

[0155] Among them, w LiDAR is the fusion weight of the load-varying point cloud data, w IMU is the fusion weight of load change driving information, ΔW is the load change, and k is the adjustment coefficient, which is proportional to the load change amplitude.

[0156] w LiDAR +w IMU =1

[0157] The adjusted weights are used for EKF data fusion:

[0158] X k =w LiDAR ·X LiDAR,k +w IMU ·X IMU,k

[0159] X k is the vehicle state at time k, i.e. the position data of the target vehicle, w LiDAR is the fusion weight of the load-varying point cloud data, w IMU is the fusion weight of load change driving information, X LiDAR,k is the load change point cloud data, X IMU,k The system can maintain positioning accuracy and trajectory continuity when the load changes by dynamically adjusting the weights.

[0160] In one embodiment of the present application, the positioning module is used to define the acceleration of the target vehicle in an unloaded vehicle state when the target vehicle is unloaded, recorded as an acceleration reference value; the center of gravity offset of the target vehicle is calculated based on the acceleration of the target vehicle in the driving information and the acceleration reference value; if the center of gravity offset is greater than or equal to a preset center of gravity offset threshold, it is determined that there is a load change in the target vehicle; if the center of gravity offset is less than the preset center of gravity offset threshold, it is determined that there is no load change in the target vehicle.

[0161] As an example, the preset center of gravity offset is 10%. If the center of gravity offset is greater than or equal to 10%, it is determined that there is a load change on the target vehicle; if the center of gravity offset is less than 10%, it is determined that there is no load change on the target vehicle.

[0162] In one embodiment of the present application, the positioning module is used to extract the current position, current speed, last position and last speed of the target vehicle; identify the path type of the real-time path, which includes straight-ahead scenes, turning scenes and narrow passage scenes; establish a trajectory continuity constraint function according to the current position, current speed, last position and last speed of the target vehicle and the path type, and the trajectory continuity constraint function includes a straight-ahead scene trajectory continuity constraint function, a turning scene trajectory continuity constraint function and a narrow passage scene trajectory continuity constraint function; and optimize the real-time path for continuity according to the trajectory continuity constraint function. The trajectory optimization algorithm can further smooth and optimize the vehicle's driving trajectory, reduce positioning errors and fluctuations, and introduce a trajectory continuity constraint model to ensure that the vehicle maintains smooth positioning in complex paths and avoid navigation errors caused by positioning mutations; smooth positioning can increase the accuracy of vehicle positioning, and the trajectory continuity constraint model ensures smooth positioning in complex paths, solving the problem of trajectory drift in the prior art.

[0163] In one embodiment of the present application, the positioning module is used to establish a straight-line scene trajectory continuity constraint function according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle and the straight-line speed smoothing weight if the path type is a straight-line scene; establish a turning scene trajectory continuity constraint function according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle, turning position smoothing weight and turning speed smoothing weight if the path type is a narrow channel scene; establish a narrow channel scene trajectory continuity constraint function according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle, narrow channel position smoothing weight and narrow channel speed smoothing weight if the path type is a narrow channel scene. The real-time path is continuously optimized according to the path smoothing algorithm, and the optimization strategy is adaptively adjusted according to different path scenes, so that the vehicle positioning is more accurate.

[0164] As an example, the real-time path is optimized for continuity according to the path smoothing algorithm. The real-time path includes straight driving, turning and narrow passages. If the path type is a straight driving scene, the straight driving scene trajectory continuity constraint function is established according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle and the straight driving speed smoothing weight:

[0165]

[0166] Among them, J is the straight scene path, p t is the position of the vehicle at time t, p t =[x t ,y t ,z t],v t is the speed of the vehicle at time t, v t =[v x,t ,v y,t ,v z,t ], λ is the speed smoothing weight, p t-1 is the position of the vehicle at time t-1, v t-1 is the speed of the vehicle at time t-1.

[0167] If the path type is a turning scene, a turning scene trajectory continuity constraint function is established based on the current position, current speed, previous position and speed of the target vehicle, the turning position smoothing weight and the turning speed smoothing weight, increasing the position weight and reducing the speed weight;

[0168] Turning scene trajectory continuity constraint function:

[0169]

[0170] Among them, J 转弯 is the turning scene path, p t =[x t ,y t ,z t ] is the position of the vehicle at time t, v t =[v x,t ,v y,t ,v z,t ]: speed of the vehicle at time t, p t-1 is the position of the vehicle at time t-1, v t-1 is the speed of the vehicle at time t-1, α is the turning position smoothing weight, β is the turning speed smoothing weight, where α>1,β<1: enhance position smoothness and reduce speed change constraints.

[0171] According to the current position, current speed, previous position and speed of the target vehicle, the narrow channel position smoothing weight and the narrow channel speed smoothing weight, the narrow channel scene trajectory continuity constraint function is established to increase the smoothness weight of the position and speed to prevent the path from deviating from obstacles; narrow channel scene trajectory continuity constraint function:

[0172]

[0173] Among them, J 狭窄 is the narrow channel path, p t =[x t ,y t ,z t ] is the position of the vehicle at time t, v t =[v x,t ,vy,t ,v z,t ]: speed of the vehicle at time t, p t-1 is the position of the vehicle at time t-1, v t-1 is the speed of the vehicle at time t-1, γ is the narrow channel position smoothing weight, δ is the narrow channel speed smoothing weight, where γ,δ>1: ensure smooth driving.

[0174] It should be noted that the device provided in the above embodiment and the method provided in the above embodiment belong to the same concept, wherein the specific manner in which each module and unit performs the operation has been described in detail in the method embodiment, and will not be repeated here. In practical applications, the device provided in the above embodiment can distribute the above functions to different functional modules as needed, that is, divide the internal structure of the device into different functional modules to complete all or part of the functions described above, and this is not limited here.

[0175] See also Figure 4 , Figure 4 is a schematic diagram of a structure of an electronic device shown in an exemplary embodiment of the present application. It should be noted that: Figure 4 The electronic device 400 shown is only an example and should not bring any limitation to the functions and usage area of ​​the embodiments of the present application.

[0176] like Figure 4 As shown, the electronic device 400 includes a processor 401, a memory 402 and a communication bus 403; the communication bus 403 is used to connect the processor 401 and the memory 402; the processor 401 is used to execute the computer program stored in the memory 402 to implement one or more methods as in the above embodiments.

[0177] The electronic device provided in the present application includes a processor, a memory, a transceiver and a communication interface. The memory and the communication interface are connected to the processor and the transceiver and complete communication with each other. The memory is used to store computer programs, the communication interface is used to communicate, and the processor and the transceiver are used to run the computer program so that the electronic device executes each step of the above method.

[0178] In this embodiment, the memory may include a random access memory (RAM), and may also include a non-volatile memory (non-volatile memory), such as at least one disk memory.

[0179] The above-mentioned processor can be a general-purpose processor, including a central processing unit (CPU), a network processor (NP), etc.; it can also be a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components.

[0180] The above embodiments are merely illustrative of the principles and effects of the present application, and are not intended to limit the present application. Anyone familiar with the technology may modify or change the above embodiments without violating the spirit and scope of the present application. Therefore, all equivalent modifications or changes made by a person of ordinary skill in the art without departing from the spirit and technical ideas disclosed in the present application shall still be covered by the claims of the present application.

Claims

1. A vehicle continuous positioning method, characterized in that: The method comprises: Obtaining point cloud data of the target vehicle's driving area, environmental images, and driving information of the target vehicle; Obtaining a driving map according to the point cloud data and the environmental image, and obtaining position and posture data of the target vehicle according to the point cloud data, the environmental image and the driving information; In the driving map, the position data of the target vehicle at continuous moments are extracted and recorded as a real-time path. The real-time path is continuously optimized according to a path smoothing algorithm to obtain the position of the target vehicle, and the target vehicle is continuously positioned.

2. The vehicle continuous positioning method according to claim 1, characterized in that: Obtaining a driving map according to the point cloud data and the environment image, including: Processing the environment image through a deep learning semantic segmentation network to obtain a semantic image, wherein the semantic image includes image feature points; Marking the dynamic image feature points in the semantic image as black pixels, and marking the static image feature points in the semantic image as white pixels, to obtain a marked semantic image; Eliminate black pixels in the marked semantic image to obtain a filtered semantic image, and perform feature point filtering on the filtered semantic image by pre- or real-time establishment of a framework vehicle geometry template library to obtain a target semantic image; An initial map of the target area is constructed using the target semantic image and the point cloud data, and external points of the initial map are removed to obtain a driving map.

3. The vehicle continuous positioning method according to claim 1, characterized in that: Obtaining the position and posture data of the target vehicle according to the point cloud data, the environment image and the driving information, including: Defining a vehicle state vector of a target vehicle, wherein the vehicle state vector of the target vehicle includes a vehicle position, a direction, and a speed; Predicting the initial position data of the target vehicle according to the vehicle state vector and the driving information through a kinematic model; Correcting the initial posture data according to the point cloud data and the environment image to obtain corrected posture data; Whether there is a load change on the target vehicle is identified through the driving information, and the position data of the target vehicle is obtained according to the load change result.

4. The vehicle continuous positioning method according to claim 3, characterized in that: Identifying whether the target vehicle has a load change through the driving information, and obtaining the position and posture data of the target vehicle according to the load change result, including: If it is identified that there is no load change on the target vehicle, the corrected posture data is used as the posture data of the target vehicle; If it is identified that the target vehicle has a load change, the driving information causing the load change of the target vehicle is recorded as load change driving information, and the point cloud data corresponding to the load change driving information is recorded as load change point cloud data; Calculating a load change amount of the load change point cloud data, and adjusting a fusion weight of the load change driving information according to the load change amount, and adjusting a fusion weight of the load change point cloud data according to the load change amount; The position and posture data of the target vehicle are obtained according to the load-change driving information, the load-change point cloud data, the fusion weight of the load-change driving information, and the fusion weight of the load-change point cloud data.

5. The vehicle continuous positioning method according to claim 3, characterized in that: Identifying whether there is a load change on the target vehicle through the driving information includes: In the case where the target vehicle is unloaded, the acceleration of the target vehicle in an unloaded vehicle state is defined and recorded as an acceleration reference value; Calculating a center of gravity offset of the target vehicle according to the acceleration of the target vehicle in the driving information and the acceleration reference value; If the center of gravity offset is greater than or equal to a preset center of gravity offset threshold, it is determined that there is a load change on the target vehicle; if the center of gravity offset is less than the preset center of gravity offset threshold, it is determined that there is no load change on the target vehicle.

6. The vehicle continuous positioning method according to claim 1, characterized in that: Continuously optimizing the real-time path according to a path smoothing algorithm includes: Extract the current position, current speed, last position and last speed of the target vehicle; Identifying a path type of the real-time path, wherein the path type includes a straight-ahead scenario, a turning scenario, and a narrow passage scenario; Establishing a trajectory continuity constraint function according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle and the path type, wherein the trajectory continuity constraint function includes a straight-line scenario trajectory continuity constraint function, a turning scenario trajectory continuity constraint function and a narrow channel scenario trajectory continuity constraint function; The real-time path is optimized for continuity according to the trajectory continuity constraint function.

7. The vehicle continuous positioning method according to claim 6, characterized in that: A trajectory continuity constraint function is established according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle and the path type, including: If the path type is a straight-ahead scenario, a trajectory continuity constraint function for the straight-ahead scenario is established according to the current position, current speed, last position, last speed, and straight-ahead speed smoothing weight of the target vehicle; If the path type is a turning scene, a turning scene trajectory continuity constraint function is established according to the current moment position, current moment speed, last moment position and last moment speed of the target vehicle, a turning position smoothing weight, and a turning speed smoothing weight; If the path type is a narrow channel scene, the narrow channel scene trajectory continuity constraint function is established according to the current moment position, current moment speed, previous moment position and previous moment speed of the target vehicle, the narrow channel position smoothing weight and the narrow channel speed smoothing weight.

8. A vehicle continuous positioning device, characterized in that: The device comprises: An acquisition module is used to acquire point cloud data of the target vehicle's driving area, environmental images, and driving information of the target vehicle; A calculation module, used to obtain a driving map according to the point cloud data and the environmental image, and to obtain position and posture data of the target vehicle according to the point cloud data, the environmental image and the driving information; The continuous positioning module is used to extract the position data of the target vehicle at continuous moments in the driving map, record it as a real-time path, continuously optimize the real-time path according to a path smoothing algorithm, obtain the position of the target vehicle, and continuously position the target vehicle.

9. An electronic device, characterized in that: The electronic device comprises: one or more processors; A storage device for storing one or more programs, when the one or more programs are executed by the one or more processors, enables the electronic device to implement the vehicle continuous positioning method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: A computer program is stored thereon, and the computer program is used to enable a computer to execute the vehicle continuous positioning method as described in any one of claims 1 to 7.

Images