Vehicle pose data updating method and device and vehicle
By combining advanced assisted driving maps and vehicle perception data, optimized models and predict vehicle postures are solved, and data diversity of high-precision maps and map-free autonomous driving technology are achieved, and low-cost and high-precision postures are achieved Prediction and security improvements.
Patent Information
- Application Number
- CN202411516408.4
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-10-29
- Publication Date
- 2024-11-29
- Estimated Expiration
- 2044-10-29
AI Technical Summary
The existing technology is difficult to find a balance between high-precision maps and navigation maps. High-precision maps are costly and have long update cycles. The mapless autonomous driving technology based on navigation maps faces data distribution and diversity problems, resulting in complex road situations. There are security risks.
By obtaining coordinate data and vehicle perception data of advanced assisted driving maps, performing odometer calculations based on map matching, building an optimization model, combining data residuals of inertial measurement units and global navigation satellite systems, predicting vehicle posture data to achieve low Cost, high-precision pose prediction.
It realizes high-precision prediction of vehicle position data, reduces mapping costs, and enhances positioning accuracy and safety in complex road situations.
Smart Images

Figure CN119048602A_ABST
Abstract
Description
Technical Field
[0001] The present application relates to the field of navigation and assisted driving technology, and more specifically, to a method and device for updating vehicle posture data, and a vehicle. Background Art
[0002] With the continuous evolution of technology and continuous improvement of products, advanced driver assistance services are increasingly recognized by the majority of users. Among them, automobile products equipped with NOA have been released one after another. However, when high-level intelligent driving enters the open roads of cities, the map products behind it present a new trend. Generally speaking, the use of high-precision maps puts forward higher requirements for their freshness, and also brings about an increase in costs. However, it is difficult to make up for the limitations of vehicle-side perception by relying solely on navigation maps. Therefore, high-assisted maps (advanced driver assistance maps) came into being as an intermediate product between navigation maps and high-precision maps.
[0003] In the existing technology, the fusion positioning technology based on the semantic features of high-precision maps uses a high-precision map engine to broadcast 3D semantic features such as road markings, traffic lights, traffic signs, and telephone poles. The images collected by the camera are preprocessed and output through convolutional neural network training. The data obtained by the sensor is associated with the map data to output the map relative position. Finally, a multi-source fusion filtering positioning model is established based on GNSS / INS combined navigation and wheel speedometer. This method is based on a relatively mature filtering theory and is simple and fast to implement, but it is not suitable for high-precision maps. Figure 1 In addition, it places too high demands on perception, regulation, and positioning, which go beyond engineering reality. At the same time, because the filtering model has to satisfy the first-order Markov assumption and does not take historical information into consideration, it cannot effectively assist in current state estimation in some challenging scenarios.
[0004] Based on end-to-end "mapless" autonomous driving technology, it does not rely on pre-made high-precision maps, but is based on navigation maps, directly from the vehicle's sensor input (such as cameras, lidar, millimeter-wave radar, etc.) for real-time environmental perception and decision control. The core of this technology is to use deep learning models to convert sensor data into vehicle driving behaviors, including but not limited to path planning, obstacle avoidance, speed control, etc. Although this method has stronger generalization and does not rely on high-precision maps, it can reduce dependence on map data collection and updates, thereby reducing costs, but it faces fatal defects in data distribution and diversity, and there are safety risks in unknown or complex road conditions.
[0005] Therefore, the present application provides a method, device and vehicle for updating vehicle posture data to solve one of the above-mentioned technical problems. Summary of the invention
[0006] The purpose of this application is to provide a method, device and vehicle for updating the posture data of a vehicle, which can solve at least one of the technical problems mentioned above. The specific application is as follows: According to a specific implementation of the present application, in a first aspect, the present application provides a method for updating posture data of a vehicle, comprising: Acquire first coordinate data based on an advanced driver assistance map, the first coordinate data including first coordinate point data and first semantic label data, each of which is labeled based on the first semantic label data; acquire second coordinate data perceived by the vehicle, the second coordinate data including second coordinate point data and second semantic label data, each of which is labeled based on the second semantic label data; use the first coordinate data as a target point cloud and the second coordinate data as a source point cloud, perform odometer calculation based on map matching on the target point cloud and the source point cloud, and obtain first pose data; construct an optimization model, the optimization model consisting of odometer residuals, inertial measurement unit IMU pre-integration residuals, and global navigation satellite system GNSS data residuals; wherein the odometer residuals are constructed based on the first pose data; predict the motion offset of the vehicle according to the second pose data currently output by the optimization model, and obtain target pose data of the vehicle at the next moment, the target pose data being used to characterize the pose of the vehicle at the next moment.
[0007] In one implementation, the odometer calculation method based on map matching includes: ; Wherein, the first posture data is based on and Characterization, represents the optimal rotation amount, represents the optimal translation amount, k represents the time mark, S represents the current feature point set, v represents the second coordinate data mark, w represents the third coordinate data mark, Represents the horizontal coordinate of the second coordinate data at time k, Represents the ordinate of the second coordinate data at time k, Represents the horizontal coordinate of the first coordinate data at time k, represents the ordinate of the first coordinate data at time k, r represents the rotation amount of the current frame, t is the translation amount of the current frame, and R represents the rotation matrix of r.
[0008] In one embodiment, the optimization model includes: ;in, represents the IMU pre-integration residual, represents the odometer residual, Represents the GNSS data residual, represents the state vector, , indicating that the state vector considers n number of postures, , , express The three posture components at the moment, , represents the set of natural numbers, represents the Huber kernel function, represents the information matrix of the IMU factor, represents the IMU pre-integrated observation, represents the information matrix of the matching odometer, represents the matching odometry observation, Represents the information matrix of RTK-GNSS, Represents the RTK-GNSS observation.
[0009] In one embodiment, before performing odometer calculation based on map matching on the target point cloud and the source point cloud, the method also includes: obtaining position information output by the wheel speed encoder of the vehicle and posture information output by the IMU; fusing the position information with the posture information to obtain third posture data; taking the first coordinate data as the target point cloud and the second coordinate data as the source point cloud, and performing odometer calculation based on map matching on the target point cloud and the source point cloud, including: taking the first coordinate data as the target point cloud, the second coordinate data as the source point cloud, and the third posture data as the prior posture data, and performing odometer calculation based on map matching on the target point cloud and the source point cloud.
[0010] In one embodiment, before constructing the optimization model, the method also includes: for each coordinate point data group calibrated as the same point by the first pose data, label matching the first semantic label data and the second semantic label data corresponding to each coordinate point data group; wherein, in each coordinate point data group, each coordinate point data group includes a first coordinate point data and a second coordinate point data, and each coordinate point data group corresponds to a first semantic label data and a second semantic label data; in response to the proportion of the target coordinate point data group in each coordinate point data group not exceeding a preset proportion, using the third pose data as the first pose data to construct an odometer residual; wherein, the target coordinate point data group is a coordinate point data group that matches the first semantic label data and the second semantic label data.
[0011] In one embodiment, obtaining the second coordinate data perceived by the vehicle includes: obtaining visual data based on a fisheye camera configured on the vehicle; performing target detection and semantic feature extraction on the visual data, and projecting the extracted data to a vehicle coordinate system based on an inverse perspective transformation to obtain the second coordinate data.
[0012] In one embodiment, the fisheye camera includes a first camera configured on the front side of the vehicle, a second camera configured on the left side of the vehicle, a third camera configured on the right side of the vehicle, and a fourth camera configured on the rear side of the vehicle; the acquisition of visual data based on the fisheye camera configured on the vehicle includes: determining an obstacle point with a minimum distance from the vehicle; determining a target camera with a minimum distance from the obstacle point, the target camera being the first camera, the second camera, the third camera, or the fourth camera; in response to the target camera being the first camera or the fourth camera, acquiring visual data based on the second camera and the third camera; in response to the target camera being the second camera or the third camera, acquiring visual data based on the first camera and the fourth camera.
[0013] According to a specific implementation of the present application, in a second aspect, the present application provides a vehicle posture data updating device, comprising: An acquisition unit is used to acquire first coordinate data obtained based on an advanced driver assistance map, wherein the first coordinate data includes first coordinate point data and first semantic label data, and each of the first coordinate point data is labeled based on the first semantic label data; and a processing unit is used to use the first coordinate data as a target point cloud and the second coordinate data as a source point cloud, perform odometer calculation based on map matching on the target point cloud and the source point cloud, and obtain first pose data; and a processing unit is used to use the first coordinate data as a target point cloud and the second coordinate data as a source point cloud, perform odometer calculation based on map matching on the target point cloud and the source point cloud, and obtain first pose data; and a processing unit is used to use the first coordinate data as a target point cloud and the second coordinate data as a source point cloud, perform odometer calculation based on map matching on the target point cloud and the source point cloud, and obtain first pose data; and Based on the first posture data, an odometer residual is constructed; based on the posture information output by the inertial measurement unit IMU of the vehicle, an IMU pre-integration residual is constructed; based on the positioning data output by the real-time differential positioning global navigation satellite system RTK-GNSS, a GNSS data residual is constructed; based on the odometer residual, the IMU pre-integration residual, and the GNSS data residual, an optimization model is constructed; and according to the second posture data currently output by the optimization model, the motion offset of the vehicle is predicted to obtain the target posture data of the vehicle at the next moment, and the target posture data is used to characterize the posture of the vehicle at the next moment.
[0014] According to a specific implementation of the present application, in a third aspect, the present application provides a vehicle for executing any one of the methods described in the first aspect.
[0015] Compared with the prior art, the above-mentioned application of the embodiment of the present application has at least the following beneficial effects: The present application provides a method, device and vehicle for updating the posture data of a vehicle. The method obtains the coordinate data and semantic labels in the global coordinate system from the advanced driver assistance map on the one hand, and obtains the coordinate data and semantic labels in the vehicle body coordinate system from the vehicle perception on the other hand, and then performs coordinate fusion through the odometer matching method to obtain the first posture data. On this basis, by considering various influences, the odometer residual, the inertial measurement unit IMU pre-integration residual, and the global navigation satellite system GNSS data residual are designed respectively, and then an optimization model is constructed through multiple residuals to output the second posture data. On this basis, the second posture data is used to perform high-precision motion offset prediction to output the posture of the vehicle at the next moment. Among them, the accuracy of the posture prediction is guaranteed by the fusion of the vehicle-side data and the map-side data, and the posture is enhanced by the optimization model on this basis, so that the posture prediction maintains a high accuracy. At the same time, the data uses the advanced driver assistance map, which has a lower mapping cost than the method of using high-precision maps for data fusion and posture prediction. In summary, the present application provides a low-cost, high-precision posture prediction application. BRIEF DESCRIPTION OF THE DRAWINGS
[0016] Figure 1 A flow chart of a method for updating posture data of a vehicle according to an embodiment of the present application is shown; Figure 2 A flow chart of a method for performing odometer calculation based on map matching on a target point cloud and a source point cloud is shown; Figure 3 A flow chart of a method for verifying the first pose data is shown; Figure 4 A flow chart of a method for obtaining second coordinate data perceived by a vehicle is shown; Figure 5 A flow chart of a method for acquiring visual data based on a fisheye camera configured on a vehicle is shown; Figure 6 A factor graph for characterizing the processing logic of the optimization model of the present application is shown; Figure 7 A unit block diagram of a vehicle posture data updating device according to an embodiment of the present application is shown. DETAILED DESCRIPTION
[0017] In order to make the purpose, technical application and advantages of this application clearer, the present application will be further described in detail below in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of this application, rather than all the embodiments. Based on the embodiments in this application, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of this application.
[0018] The terms used in the embodiments of the present application are only for the purpose of describing specific embodiments, and are not intended to limit the present application. The singular forms "a", "said" and "the" used in the embodiments of the present application and the appended claims are also intended to include plural forms, and unless the context clearly indicates other meanings, "multiple" generally includes at least two.
[0019] It should be understood that the term "and / or" used in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship.
[0020] It should be understood that, although the terms first, second, third, etc. may be used to describe in the embodiments of the present application, these descriptions should not be limited to these terms. These terms are only used to distinguish the descriptions. For example, without departing from the scope of the embodiments of the present application, the first may also be referred to as the second, and similarly, the second may also be referred to as the first.
[0021] As used herein, the words "if" and "if" may be interpreted as "at the time of" or "when" or "in response to determining" or "in response to detecting", depending on the context. Similarly, the phrases "if it is determined" or "if (stated condition or event) is detected" may be interpreted as "when it is determined" or "in response to determining" or "when detecting (stated condition or event)" or "in response to detecting (stated condition or event)", depending on the context.
[0022] It should also be noted that the term "includes", "comprising" or any other variation thereof is intended to cover non-exclusive inclusion, so that a commodity or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such commodity or device. In the absence of more restrictions, the elements defined by the sentence "comprising a ..." do not exclude the existence of other identical elements in the commodity or device including the elements.
[0023] It should be particularly noted that any symbols and / or numbers in the specification that are not marked in the accompanying drawings are not drawing marks.
[0024] The optional embodiments of the present application are described in detail below with reference to the accompanying drawings.
[0025] The embodiment provided in this application is an embodiment of a method for updating vehicle posture data.
[0026] Combine the following Figure 1 The embodiments of the present application are described in detail.
[0027] Figure 1 A flow chart of a method for updating the posture data of a vehicle according to an embodiment of the present application is shown. Figure 1 As shown, the process includes the following steps S101 to S105.
[0028] In step S101, first coordinate data based on an advanced driver assistance map is obtained.
[0029] The first coordinate data includes first coordinate point data and first semantic label data, and each first coordinate point data is labeled based on the first semantic label data.
[0030] In the above embodiment, the advanced driver assistance Figure 1 Generally refers to maps that serve the L3 and below autonomous driving systems of intelligent connected vehicles. It is a type of navigation electronic map. On the basis of the road attributes of traditional navigation electronic maps, it adds traffic facilities, traffic signs, and road slope, curvature and other attribute information necessary for advanced driving assistance functions, which can help realize advanced driving assistance functions. Compared with high-precision maps, advanced assisted driving maps have lower mapping costs, and can achieve the same or even better level as high-precision maps in assisted driving functions.
[0031] In step S102, second coordinate data perceived by the vehicle is obtained.
[0032] The second coordinate data includes second coordinate point data and second semantic label data, and each second coordinate point data is labeled based on the second semantic label data.
[0033] In the present application, the execution order of step S101 and step S102 is not fixed. For example, step S102 may be executed first and then step S101, or step S101 and step S102 may be executed simultaneously. The above-mentioned execution of step S101 first and then step S102 is only an exemplary embodiment.
[0034] In step S103, the first coordinate data is used as the target point cloud, and the second coordinate data is used as the source point cloud. The target point cloud and the source point cloud are subjected to odometer calculation based on map matching to obtain the first pose data.
[0035] For example, the first pose data can be represented by a rotation amount and a translation amount.
[0036] Furthermore, it is considered to improve the pose accuracy by constructing an optimization model. The residuals considered by the optimization model may include odometer residuals, inertial measurement unit (IMU) pre-integration residuals, and global navigation satellite system (GNSS) data residuals.
[0037] In step S104, an optimization model is constructed.
[0038] The optimization model consists of odometer residuals, IMU pre-integration residuals, and GNSS data residuals.
[0039] In one implementation, the odometer residual may be constructed using the first pose data.
[0040] In one implementation, the IMU pre-integration residual may be obtained through the attitude information output by the IMU.
[0041] In one implementation, the GNSS data residual may be obtained through navigation data provided by a global navigation satellite system, wherein the global navigation satellite system may be, for example, a global navigation satellite system based on real-time kinematic (RTK).
[0042] In step S105, the motion deviation of the vehicle is predicted according to the second posture data currently output by the optimization model to obtain the target posture data of the vehicle at the next moment, and the target posture data is used to characterize the posture of the vehicle at the next moment.
[0043] The vehicle posture data updating method provided by the present application obtains the coordinate data and semantic labels in the global coordinate system from the advanced driver assistance map on the one hand, and obtains the coordinate data and semantic labels in the vehicle body coordinate system from the vehicle perception on the other hand, and then performs coordinate fusion through the odometer matching method to obtain the first posture data. On this basis, by considering various influences, the odometer residual, the inertial measurement unit IMU pre-integration residual, and the global navigation satellite system GNSS data residual are designed respectively, and then the optimization model is constructed through multiple residuals to output the second posture data. On this basis, the second posture data is used to perform high-precision motion offset prediction to output the posture of the vehicle at the next moment. Among them, the accuracy of the posture prediction is guaranteed by the fusion of the vehicle-side data and the map-side data, and the posture is enhanced by the optimization model on this basis, so that the posture prediction maintains a high accuracy. At the same time, the data uses the advanced driver assistance map, which has a lower mapping cost than the method of using high-precision maps for data fusion and posture prediction. In summary, the present application provides a low-cost, high-precision posture prediction application.
[0044] For example, the odometer calculation method based on map matching includes .
[0045] Among them, the first pose data is based on and Characterization, represents the optimal rotation amount, represents the optimal translation amount, k represents the time mark, S represents the current feature point set, v represents the second coordinate data mark, w represents the third coordinate data mark, Represents the horizontal coordinate of the second coordinate data at time k, Represents the ordinate of the second coordinate data at time k, Represents the horizontal coordinate of the first coordinate data at time k, represents the ordinate of the first coordinate data at time k, r represents the rotation amount of the current frame, t is the translation amount of the current frame, and R represents the rotation matrix of r.
[0046] For example, the optimization model is as follows: .
[0047] in, represents the IMU pre-integration residual, represents the odometer residual, Represents the GNSS data residual, represents the state vector, , indicating that the state vector considers n number of postures, , , express The three posture components at the moment, , represents the set of natural numbers, represents the Huber kernel function, represents the information matrix of the IMU factor, represents the IMU pre-integrated observation, represents the information matrix of the matching odometer, represents the matching odometry observation, Represents the information matrix of RTK-GNSS, Represents the RTK-GNSS observation.
[0048] In this application, the odometer calculation based on map matching can also be accelerated by providing a priori posture.
[0049] Figure 2 A flow chart of a method for calculating the odometer based on map matching of a target point cloud and a source point cloud is shown, Figure 2 As shown, the process includes the following steps S201 to S203.
[0050] In step S201, the position information output by the wheel speed encoder of the vehicle and the attitude information output by the IMU are obtained.
[0051] In step S201, the position information and the posture information are fused to obtain third posture data.
[0052] In step S203, the first coordinate data is used as the target point cloud, the second coordinate data is used as the source point cloud, and the third posture data is used as the prior posture data, and the odometer calculation based on map matching is performed on the target point cloud and the source point cloud.
[0053] This application provides a priori posture through third posture data, which saves computing costs and accelerates odometer calculation, thereby improving the timeliness of posture prediction.
[0054] In this application, the first pose data is used to characterize the rotation and translation required to move from the first coordinate point data to the second coordinate point data at the same point, which is equivalent to establishing a matching relationship between the first coordinate point data and the second coordinate point data. On this basis, further consideration is given to performing semantic matching through the semantic label data corresponding to the same point under different coordinate point data to further describe the accuracy of the first pose data.
[0055] Figure 3 A flow chart of a method for verifying the first pose data is shown. Figure 3 As shown, the process includes the following steps S301, S302a, and S302b.
[0056] In step S301, for each coordinate point data group calibrated as the same point position by the first pose data, label matching is performed on the first semantic label data and the second semantic label data corresponding to each coordinate point data group.
[0057] Among them, each coordinate point data group includes a first coordinate point data and a second coordinate point data, and each coordinate point data group corresponds to a first semantic label data and a second semantic label data.
[0058] In step S302a, in response to the fact that the proportion of the target coordinate point data group in each coordinate point data group does not exceed the preset proportion, the third posture data is used as the first posture data to construct the odometer residual.
[0059] The target coordinate point data set is a coordinate point data set that matches the first semantic label data and the second semantic label data.
[0060] In step S302b, in response to the proportion of the target coordinate point data group in each coordinate point data group exceeding the preset proportion, the first pose data is retained and the odometer residual is constructed using the first pose data.
[0061] For example, the preset ratio may be set to, for example, 95%.
[0062] In this application, before constructing the optimization model, the matching between the semantic label data realizes the verification of the first pose data. When the target coordinate point data group accounts for less than 95% of the coordinate point data group, it means that the first pose data is not descriptive enough and not accurate enough. At this time, the third pose data is used and RTK-GNSS is called for repositioning.
[0063] In the present application, the vehicle is equipped with a fisheye camera. For example, visual data can be acquired through the fisheye camera, and second coordinate data can be obtained through a series of processing.
[0064] Figure 4 A flow chart of a method for obtaining the second coordinate data perceived by the vehicle is shown. Figure 4 As shown, the process includes the following steps S401 and S402.
[0065] In step S401 , visual data is acquired based on a fisheye camera disposed on a vehicle.
[0066] In step S402, target detection and semantic feature extraction are performed on the visual data, and based on the inverse perspective transformation, the extracted data is projected to the vehicle coordinate system to obtain second coordinate data.
[0067] For example, the fisheye camera may include a first camera configured at the front side of the vehicle, a second camera configured at the left side of the vehicle, a third camera configured at the right side of the vehicle, and a fourth camera configured at the rear side of the vehicle. The present application can obtain visual data that meets the requirements by setting the fisheye camera call rules.
[0068] Figure 5 A flow chart of a method for acquiring visual data based on a fisheye camera configured on a vehicle is shown. Figure 5 As shown, the process includes the following steps S501 to S503b.
[0069] In step S501, the obstacle point with the shortest distance from the vehicle is determined.
[0070] In step S502, a target camera having the shortest distance to the obstacle point is determined, where the target camera is the first camera, the second camera, the third camera, or the fourth camera.
[0071] For example, the target camera with the shortest distance to the obstacle point can be determined according to the comparison result between the nearest point output by the drivable area detection (freespace) and the size of the body contour point in the body coordinate system.
[0072] In step S503a, in response to the target camera being the first camera or the fourth camera, visual data is acquired based on the second camera and the third camera.
[0073] In step S503b, in response to the target camera being the second camera or the third camera, visual data is acquired based on the first camera and the fourth camera.
[0074] As some other feasible embodiments, in response to the target camera being the first camera, visual data may be acquired based on the fourth camera, or in response to the target camera being the fourth camera, visual data may be acquired based on the first camera.
[0075] In this application, the camera with the shortest distance from the obstacle point is avoided to ensure that there is a sufficient distance between the camera and the obstacle point to obtain visual data with sufficient field of view, thereby ensuring that vehicle perception and processing are easy to obtain.
[0076] The method provided in this application utilizes an advanced assisted driving map produced by a vehicle equipped with a panoramic fisheye camera, IMU and other sensors. The vehicle calls the fisheye camera interface to extract semantic features according to certain rules based on the real-time perception results, so as to prepare more computing resources to call the factor graph optimization model to complete the pose estimation.
[0077] In one implementation, a factor graph optimization model can be constructed through the gtsam library to establish a batch optimization cost function, and then all historical information and current information are jointly optimized to generate a six-degree-of-freedom posture, thereby achieving a low-cost, low-latency, and high-precision autonomous driving fusion positioning.
[0078] The purpose of this application is to provide a high-precision fusion positioning based on advanced assisted driving maps to overcome the high cost problem brought about by positioning based on high-precision maps, and to solve the data missing diversity problem existing in the map-free intelligent driving technology based on navigation maps. At the same time, without adding new sensors to the vehicle, the vehicle-side posture can be obtained by calling the fisheye camera on demand and combining the factor graph optimization, thereby maintaining low cost and low computing power and improving positioning accuracy.
[0079] Preferably, an implementation scheme of the present application is shown below, including steps one to seven.
[0080] 1. Receiving map semantic features. The advanced driver assistance map engine receives map elements such as lane lines, crosswalks, ground signs, and stop lines broadcast within 2km in front of the vehicle and 500m behind the vehicle, and stores their contour point coordinates and semantic label IDs. The broadcast frequency is 50hz, and the contour points output in the frame are regarded as a frame of 2D point cloud. According to certain clustering rules, each cluster carries semantic label information.
[0081] 2. Fisheye camera detection. The panoramic fisheye camera installed on the vehicle can control and call the fisheye camera interface required for detection according to certain rules based on the perception results of the self-vehicle drivable area detection (that is, select some cameras that need to obtain visual data from multiple fisheye cameras). For the visual image information, the deep learning model YOLOV8 can be used to detect the above-mentioned target objects and extract semantic features in real time, and project them onto the ground plane in the vehicle coordinate system through inverse perspective transformation to extract contour coordinate points and semantic label IDs. Fisheye camera calling rules: 1. If both the front and rear fisheye cameras are blocked, only the left and right fisheye cameras are called. 2. If the front fisheye camera is blocked, only the rear fisheye camera is called. 3. If the rear fisheye camera is blocked, only the front fisheye camera is called. Among them, the occlusion condition can be obtained by comparing the nearest point output by the self-vehicle drivable area detection with the size of the body contour point in the body coordinate system. The establishment of the body coordinate system follows the right-hand rule, with the X-axis pointing to the vehicle's forward direction, the Y-axis pointing to the left side of the vehicle, and the Z-axis pointing to the sky of the vehicle.
[0082] 3. Inertial solution: Based on the initial absolute positioning data provided by RTK-GNSS, the position information output by the wheel speed encoder and the attitude information output by the IMU are fused to obtain more accurate and reliable attitude and position information, which can provide a priori posture for the subsequent map matching process.
[0083] 4. Semantic feature association and matching. The semantic feature point contour coordinate point set received by the advanced driver assistance map engine (i.e., the first coordinate data mentioned above in this application) is used as the target point cloud, and the semantic feature contour point set detected in real time by the fisheye camera is used as the source point cloud (i.e., the second coordinate data mentioned above in this application). The predicted prior value is provided by inertial solution, and the Generalized-lterative Closest Point (GICP) algorithm is called to realize the odometer calculation based on map matching. The calculation formula is as follows: Indicates. Among them, represents the optimal rotation amount, represents the optimal translation amount, k represents the time mark, S represents the current feature point set, v represents the second coordinate data mark, w represents the third coordinate data mark, Represents the horizontal coordinate of the second coordinate data at time k, Represents the ordinate of the second coordinate data at time k, Represents the horizontal coordinate of the first coordinate data at time k, represents the ordinate of the first coordinate data at time k, r represents the rotation of the current frame, t is the translation of the current frame, and R represents the rotation matrix of r. is the position of the current feature point in the vehicle body coordinate system, is the closest point matching the feature in the global coordinate system.
[0084] 5. Matching accuracy judgment. After the relative position and posture relationship between the vehicle and the map is obtained through S4 matching, the matching accuracy can be further verified through the semantic tag ID. If the accuracy is less than 95%, the prior position and posture are used, and RTK-GNSS is called for repositioning.
[0085] 6. Constructing the optimization model. Generally speaking, the window optimization model with basic structure is constructed by Indicates. Among them, is the residual, is the Jacobian of the residual with respect to the state quantity, is the information matrix, for The transposed matrix of is the optimization variable.
[0086] On this basis, this application considers the residuals of matching the relative posture of the odometer and the optimization variables (i.e. the odometer residuals mentioned in this application), the residuals of the IMU pre-integration and the optimization variables (i.e. the IMU pre-integration residuals mentioned in this application), and the residuals corresponding to the prior factors formed by RTK-GNSS (i.e. the GNSS data residuals mentioned in this application), specifically involving the residual construction of each factor and the calculation of the Jacobian matrix. Furthermore, as a feasible embodiment, consider performing frame-by-frame optimization by sliding window, for example, setting the optimization model to ;in, represents the IMU pre-integration residual, represents the odometer residual, Represents the GNSS data residual, represents the state vector, , indicating that the state vector considers n number of postures, , , express The three posture components at the moment, , represents the set of natural numbers, represents the Huber kernel function, represents the information matrix of the IMU factor, represents the IMU pre-integrated observation, represents the information matrix of the matching odometer, represents the matching odometry observation, Represents the information matrix of RTK-GNSS, Represents the RTK-GNSS observation.
[0087] 7. Posture release. The optimization model outputs the pose of the current frame in real time, and with reference to the vehicle kinematic model, calculates the vehicle's motion offset from the current time to the latest pose, and releases the predicted vehicle pose data at the next moment.
[0088] In this application, the processing logic of the optimization model can be represented by a factor graph.
[0089] Figure 6 A factor graph for characterizing the processing logic of the optimization model of the present application is shown.
[0090] For example, Figure 6 As shown, the present application considers the residuals of matching the relative pose of the odometer and the optimization variables (i.e., the odometer residuals mentioned above in the present application), the residuals of the IMU pre-integration and the optimization variables (i.e., the IMU pre-integration residuals mentioned above in the present application), and the residuals corresponding to the prior factors formed by RTK-GNSS (i.e., the GNSS data residuals mentioned above in the present application), and sets the matching odometer factor, RTK-GNSS factor, and IMU and integration factor respectively. The optimization model performs graph optimization in a window sliding manner, and updates the pose frame by frame from left to right. In addition, the sliding window contains multiple poses arranged in a delayed sequence, specifically including optimized poses and poses to be optimized. The optimized poses adjacent to the pose to be optimized can be used as reference poses for pose optimization. Among them, each IMU pre-integration factor is used to intervene in two poses to be optimized, each matching mileage factor is used to intervene in two poses to be optimized, and each TTK-GNSS factor is used to intervene in one pose to be optimized. In this application, in order to maintain real-time performance during graph optimization, the dimension of the optimization variable is controlled to a certain extent, which corresponds to the processing of the sliding window and the output of the sliding window.
[0091] The present application also provides an apparatus embodiment that is consistent with the above embodiment, which is used to implement the method steps of the above embodiment. The explanation based on the same name meaning is the same as the above embodiment, and has the same technical effect as the above embodiment, which will not be repeated here.
[0092] Figure 7 A unit block diagram of a vehicle posture data updating device according to an embodiment of the present application is shown.
[0093] like Figure 7 As shown, the vehicle posture data updating device includes an acquisition unit 701 and a processing unit 702. The acquisition unit 701 acquires first coordinate data obtained based on the advanced driver assistance map, the first coordinate data includes first coordinate point data and first semantic label data, and each first coordinate point data is labeled based on the first semantic label data. And is used to acquire second coordinate data perceived by the vehicle, the second coordinate data includes second coordinate point data and second semantic label data, and each second coordinate point data is labeled based on the second semantic label data.
[0094] Processing unit 702 is used to use the first coordinate data as the target point cloud and the second coordinate data as the source point cloud, and perform odometer calculation based on map matching on the target point cloud and the source point cloud to obtain the first pose data. It is used to construct the odometer residual based on the first pose data. It is used to construct the IMU pre-integration residual based on the pose information output by the inertial measurement unit IMU of the vehicle. It is used to construct the GNSS data residual based on the positioning data output by the real-time differential positioning global navigation satellite system RTK-GNSS. It is used to construct an optimization model based on the odometer residual, the IMU pre-integration residual, and the GNSS data residual. And it is used to predict the motion offset of the vehicle according to the second pose data currently output by the optimization model, and obtain the target pose data of the vehicle at the next moment, and the target pose data is used to characterize the pose of the vehicle at the next moment.
[0095] In one implementation, the first pose data is obtained by using the following formula based on the odometer calculation of map matching: .
[0096] Among them, the first pose data is based on and Characterization, represents the optimal rotation amount, represents the optimal translation amount, k represents the time mark, S represents the current feature point set, v represents the second coordinate data mark, w represents the third coordinate data mark, Represents the horizontal coordinate of the second coordinate data at time k, Represents the ordinate of the second coordinate data at time k, Represents the horizontal coordinate of the first coordinate data at time k, represents the ordinate of the first coordinate data at time k, r represents the rotation amount of the current frame, t is the translation amount of the current frame, and R represents the rotation matrix of r.
[0097] In one embodiment, the optimization model includes: .in, represents the IMU pre-integration residual, represents the odometer residual, Represents the GNSS data residual, represents the state vector, , indicating that the state vector considers n number of postures, , , express The three posture components at the moment, , represents the set of natural numbers, represents the Huber kernel function, represents the information matrix of the IMU factor, represents the IMU pre-integrated observation, represents the information matrix of the matching odometer, represents the matching odometry observation, Represents the information matrix of RTK-GNSS, Represents the RTK-GNSS observation.
[0098] In one embodiment, the acquisition unit 701 is also used to: acquire the position information output by the wheel speed encoder of the vehicle, and the posture information output by the IMU. The processing unit 702 is also used to: fuse the position information with the posture information to obtain third posture data. The processing unit 702 uses the first coordinate data as the target point cloud, the second coordinate data as the source point cloud, and performs odometer calculation based on map matching on the target point cloud and the source point cloud in the following manner: using the first coordinate data as the target point cloud, the second coordinate data as the source point cloud, and the third posture data as the prior posture data, and performing odometer calculation based on map matching on the target point cloud and the source point cloud.
[0099] In one embodiment, the processing unit 702 is also used to: for each coordinate point data group calibrated as the same point position by the first pose data, respectively match the first semantic label data corresponding to each coordinate point data group with the second semantic label data. Among them, in each coordinate point data group, each coordinate point data group includes a first coordinate point data and a second coordinate point data, and each coordinate point data group corresponds to a first semantic label data and a second semantic label data. In response to the proportion of the target coordinate point data group in each coordinate point data group not exceeding the preset proportion, the third pose data is used as the first pose data to construct the odometer residual. Among them, the target coordinate point data group is a coordinate point data group that matches the first semantic label data and the second semantic label data.
[0100] In one embodiment, the processing unit 702 obtains the second coordinate data perceived by the vehicle, including: obtaining visual data based on a fisheye camera configured on the vehicle, performing target detection and semantic feature extraction on the visual data, and projecting the extracted data to the vehicle coordinate system based on an inverse perspective transformation to obtain the second coordinate data.
[0101] In one embodiment, the fisheye camera includes a first camera configured on the front side of the vehicle, a second camera configured on the left side of the vehicle, a third camera configured on the right side of the vehicle, and a fourth camera configured on the rear side of the vehicle. The processing unit 702 acquires visual data based on the fisheye camera configured on the vehicle in the following manner: determine the obstacle point with the shortest distance from the vehicle. Determine a target camera with the shortest distance from the obstacle point, the target camera being the first camera, the second camera, the third camera, or the fourth camera. In response to the target camera being the first camera or the fourth camera, acquire visual data based on the second camera and the third camera. In response to the target camera being the second camera or the third camera, acquire visual data based on the first camera and the fourth camera.
[0102] Regarding the device in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.
[0103] The present application also provides a system embodiment that is consistent with the above embodiment, which is used to implement the method steps of the above embodiment. The explanation based on the same name meaning is the same as the above embodiment, and has the same technical effect as the above embodiment, which will not be repeated here.
[0104] The system provided by the present application can be divided into three parts: a sensor data preprocessing module, a semantic feature association matching module, and a factor graph optimization model construction module. Among them, the sensor data preprocessing module mainly processes data-level data to obtain target-level positioning results. On the one hand, it receives contour point data such as lane lines, crosswalks, ground signs and stop lines within a certain range of high-assisted map engine data broadcast. On the other hand, the fisheye camera interface after inverse perspective transformation is called according to certain rules to detect the above semantic features in real time through the neural network model. At the same time, the vehicle inertial estimation is completed based on the initial absolute position provided by GPS, wheel speed encoder and IMU, which can be used as prior information to help narrow the search range and improve the accuracy of data association. The semantic feature association matching module first extracts contour points from the semantic features detected by the fisheye camera in real time, and then associates them with the semantic features of the advanced assisted driving map from the two dimensions of semantic labels and coordinate values, and further uses the GICP matching algorithm to obtain the relative position and posture of the vehicle current frame and the local advanced assisted driving map. Among them, the more accurate prior estimation provided by the inertial estimation result is used to assist the data association and matching process. The factor graph optimization construction module is mainly a method of tightly coupling visual-inertial odometry, which mainly obtains the globally consistent posture of the vehicle by jointly optimizing the visual-map matching odometry factor, IMU pre-integration factor, and RTK-GNSS factor.
[0105] The core technical points of this application are mainly concentrated in the following aspects. 1) Low-cost sensor integration: Using low-cost sensors such as panoramic fisheye cameras and IMUs carried by urban taxis to achieve high-precision positioning, reducing dependence on expensive equipment. 2) Cloud crowdsourcing light map production: Through cloud crowdsourcing, a large number of low-cost vehicles collect data to produce light maps, which improves the coverage and update frequency of map data. 3) Real-time semantic feature extraction: The vehicle calls the fisheye camera interface to extract semantic features based on the real-time perception results, providing key information for subsequent pose estimation. 4) Factor graph optimization model: Construct a factor graph optimization model, integrate map matching factors, RTK-GNSS factors and IMU pre-integration factors, and improve the accuracy of pose estimation. 5) Joint optimization of historical and current information: Through batch optimization cost functions, historical information and current information are combined for joint optimization to generate accurate poses with six degrees of freedom. 6) Modular system design: The system is divided into sensor data preprocessing module, semantic feature association matching module and factor graph optimization model construction module, which realizes clear division of functions and efficient collaboration. 7) Prior information assistance: The DR inference results based on GPS, wheel speed encoder and IMU are used as prior information to improve the accuracy of data association and the efficiency of the matching process. 8) GICP matching algorithm: The GICP algorithm is used to achieve relative pose matching between the current frame of the vehicle and the local high-assisted map, which improves the accuracy and robustness of the matching. 9) Fusion structure design: The visual-map matching odometer factor, IMU pre-integration factor and RTK-GNSS factor are comprehensively considered to achieve a tightly coupled visual-inertial odometer. 10) Sliding window optimization: Through the sliding window technology, the real-time update of the optimization model and the optimization of the key frame pose are achieved, ensuring the real-time and stability of the system. These core technical points together constitute the high-precision fusion positioning of this application, realizing low-cost, low-latency, and high-precision autonomous driving positioning technology, which has broad application prospects and market potential.
[0106] The present application has the following beneficial effects. 1) Cost-effectiveness: By using low-cost panoramic fisheye cameras, IMU and other sensors carried by urban taxis, and cooperating with light maps produced by cloud crowdsourcing, the hardware cost of the high-precision positioning system is greatly reduced. 2) Real-time performance: The vehicle calls the fisheye camera interface in time to extract semantic features according to the real-time perception results, ensuring the response speed and real-time performance of the system. 3) Optimization algorithm: The batch optimization cost function established based on the factor graph improves the accuracy and reliability of positioning by jointly optimizing historical information and current information. 4) Modular design: The system is designed as a sensor data preprocessing module, a semantic feature association matching module and a factor graph optimization model construction module. Each module has a clear division of labor and is easy to maintain and upgrade. 5) Data association accuracy: Through the prior information provided by DR calculation, the search range is narrowed and the accuracy of data association is improved. 6) Robustness: The system can use real-time detection and map matching of semantic features to maintain stable positioning performance even in urban canyons or indoor environments where GPS signals are limited. 7) Flexibility: By calling the fisheye camera on demand, the system can flexibly adapt to different driving scenarios and conditions. 8) Scalability: Light maps produced by cloud crowdsourcing can be continuously updated and expanded to support more types of road and environmental information. 9) Autonomy: The system can realize autonomous calculation and optimization of vehicle-side posture without adding new sensors to the vehicle, reducing dependence on external systems. 10) Low latency: Through the optimization and real-time update of sliding windows, the system can quickly publish posture data to meet the high requirements of autonomous driving for low latency. 11) Balance between high precision and low computing power: While maintaining high-precision positioning, the optimization algorithm reduces the demand for computing resources and is suitable for vehicle platforms with limited computing power.
[0107] The present application also provides a vehicle that is consistent with the above-mentioned embodiment, which is used to implement the method steps of the above-mentioned embodiment. The explanation based on the same name meaning is the same as that of the above-mentioned embodiment, and has the same technical effect as that of the above-mentioned embodiment, which will not be repeated here.
[0108] Although operations are described in a particular order in the drawings, this should not be understood as requiring that the operations be performed in the particular order shown or in serial order, or that all shown operations be performed to achieve desired results. In certain circumstances, multitasking and parallel processing may be advantageous.
[0109] The methods and devices of the present application can be implemented using standard programming techniques, using rule-based logic or other logic to implement various method steps. It should also be noted that the words "device" and "module" used herein and in the claims are intended to include implementations using one or more lines of software code and / or hardware implementations and / or devices for receiving input.
[0110] The foregoing description of the implementation of the present application has been given for the purpose of illustration and description. The foregoing description is not exhaustive nor is it intended to limit the present application to the exact form disclosed, and various variations and modifications may exist according to the above teachings or may be obtained from the practice of the present application. These embodiments are selected and described in order to illustrate the principles of the present application and its practical application, so that those skilled in the art can utilize the present application in various embodiments and various modifications suitable for the specific purpose contemplated.
[0111] Regarding the device in the above embodiment, the specific manner in which each module performs operations has been described in detail in the embodiment of the method, and will not be elaborated here.
[0112] It can be further understood that, unless otherwise specified, “connection” includes a direct connection without other components between the two, and also includes an indirect connection with other components between the two.
[0113] It is further understood that, although the operations are described in a specific order in the drawings in the embodiments of the present application, it should not be understood as requiring the operations to be performed in the specific order or serial order shown, or requiring the execution of all the operations shown to obtain the desired results. In certain environments, multitasking and parallel processing may be advantageous.
[0114] Those skilled in the art will readily appreciate other implementations of the present application after considering the specification and practicing the application disclosed herein. The present application is intended to cover any modification, use or adaptation of the present application, which follows the general principles of the present application and includes common knowledge or customary technical means in the field of the present application that are not disclosed in the present application. The specification and embodiments are only considered as exemplary, and the true scope and spirit of the present application are indicated by the following scope of rights.
[0115] It should be understood that the present application is not limited to the precise structures described above and shown in the drawings, and that various modifications and changes may be made without departing from the scope thereof. The scope of the present application is limited only by the scope of the appended claims.
[0116] The above embodiments are only used to illustrate the technical application of the present application, rather than to limit it. Although the present application has been described in detail with reference to the aforementioned embodiments, a person of ordinary skill in the art should understand that the technical application described in the aforementioned embodiments can still be modified, or some of the technical features therein can be replaced by equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical application to deviate from the spirit and scope of the technical application of each embodiment of the present application.
Claims
1. A method for updating vehicle posture data, characterized in that: include: Acquire first coordinate data obtained based on an advanced driver assistance map, where the first coordinate data includes first coordinate point data and first semantic label data, and each of the first coordinate point data is labeled based on the first semantic label data; Acquire second coordinate data perceived by the vehicle, the second coordinate data including second coordinate point data and second semantic label data, each of the second coordinate point data being annotated based on the second semantic label data; Taking the first coordinate data as the target point cloud and the second coordinate data as the source point cloud, performing odometer calculation based on map matching on the target point cloud and the source point cloud to obtain the first pose data; Constructing an optimization model, wherein the optimization model is composed of an odometer residual, an inertial measurement unit IMU pre-integration residual, and a global navigation satellite system GNSS data residual; wherein the odometer residual is constructed based on the first posture data; According to the second posture data currently output by the optimization model, the motion deviation of the vehicle is predicted to obtain the target posture data of the vehicle at the next moment, and the target posture data is used to characterize the posture of the vehicle at the next moment.
2. The method according to claim 1, characterized in that The odometer calculation method based on map matching includes: ; Wherein, the first posture data is based on and Characterization, represents the optimal rotation amount, represents the optimal translation amount, k represents the time mark, S represents the current feature point set, v represents the second coordinate data mark, w represents the third coordinate data mark, Represents the horizontal coordinate of the second coordinate data at time k, Represents the ordinate of the second coordinate data at time k, Represents the horizontal coordinate of the first coordinate data at time k, represents the ordinate of the first coordinate data at time k, r represents the rotation amount of the current frame, t is the translation amount of the current frame, and R represents the rotation matrix of r.
3. The method according to claim 1, characterized in that The optimization model includes: ; in, represents the IMU pre-integration residual, represents the odometer residual, Represents the GNSS data residual, represents the state vector, , indicating that the state vector takes into account n number of postures, , , express The three posture components at the moment, , represents the set of natural numbers, represents the Huber kernel function, represents the information matrix of the IMU factor, represents the IMU pre-integrated observation, represents the information matrix of the matching odometer, represents the matching odometry observation, Represents the information matrix of RTK-GNSS, Represents the RTK-GNSS observation.
4. The method according to claim 1, characterized in that Before performing odometer calculation based on map matching on the target point cloud and the source point cloud, the method further includes: Obtaining position information output by a wheel speed encoder of the vehicle and attitude information output by the IMU; The position information is fused with the posture information to obtain third posture data; The method of using the first coordinate data as a target point cloud and the second coordinate data as a source point cloud, and performing odometer calculation based on map matching on the target point cloud and the source point cloud, includes: The first coordinate data is used as a target point cloud, the second coordinate data is used as a source point cloud, and the third posture data is used as prior posture data, and an odometer calculation based on map matching is performed on the target point cloud and the source point cloud.
5. The method according to claim 4, characterized in that Before constructing the optimization model, the method further includes: For each coordinate point data group calibrated as the same point position by the first posture data, the first semantic label data and the second semantic label data corresponding to each coordinate point data group are respectively matched; wherein, in each coordinate point data group, each coordinate point data group includes a first coordinate point data and a second coordinate point data, and each coordinate point data group corresponds to a first semantic label data and a second semantic label data; In response to the proportion of the target coordinate point data group in the coordinate point data groups not exceeding a preset proportion, using the third posture data as the first posture data to construct an odometer residual; The target coordinate point data set is a coordinate point data set that matches the first semantic label data and the second semantic label data.
6. The method according to claim 1, characterized in that The obtaining of the second coordinate data perceived by the vehicle includes: Acquiring visual data based on a fisheye camera configured on the vehicle; Target detection and semantic feature extraction are performed on the visual data, and based on an inverse perspective transformation, the extracted data is projected to a vehicle coordinate system to obtain the second coordinate data.
7. The method according to claim 6, characterized in that The fisheye camera includes a first camera disposed on the front side of the vehicle, a second camera disposed on the left side of the vehicle, a third camera disposed on the right side of the vehicle, and a fourth camera disposed on the rear side of the vehicle; The obtaining of visual data based on a fisheye camera configured on the vehicle includes: determining an obstacle point having a minimum distance from the vehicle; Determine a target camera having the shortest distance from the obstacle point, wherein the target camera is the first camera, the second camera, the third camera, or the fourth camera; In response to the target camera being the first camera or the fourth camera, acquiring visual data based on the second camera and the third camera; In response to the target camera being the second camera or the third camera, visual data is acquired based on the first camera and the fourth camera.
8. A vehicle posture data updating device, characterized in that: include: an acquisition unit, for acquiring first coordinate data obtained based on an advanced driver assistance map, the first coordinate data including first coordinate point data and first semantic label data, each of which is labeled based on the first semantic label data; and for acquiring second coordinate data perceived by the vehicle, the second coordinate data including second coordinate point data and second semantic label data, each of which is labeled based on the second semantic label data; A processing unit, configured to use the first coordinate data as a target point cloud and the second coordinate data as a source point cloud, perform an odometer calculation based on map matching on the target point cloud and the source point cloud to obtain first pose data; and to construct an odometer residual based on the first pose data; Used to construct an IMU pre-integration residual based on the attitude information output by the inertial measurement unit IMU of the vehicle; Used to construct GNSS data residuals based on the positioning data output by the real-time differential positioning global navigation satellite system RTK-GNSS; Used to construct an optimization model based on the odometer residual, the IMU pre-integration residual, and the GNSS data residual; And it is used to predict the motion deviation of the vehicle according to the second posture data currently output by the optimization model to obtain the target posture data of the vehicle at the next moment, and the target posture data is used to characterize the posture of the vehicle at the next moment.
9. A vehicle comprising a memory, a processor and a computer program stored in the memory, characterized in that: The processor executes the computer program to implement the method according to any one of claims 1 to 7.
Citation Information
Patent Citations
Automatic driving vehicle positioning method and device, electronic equipment and storage medium
CN114782914A
Laser radar and inertial navigation fusion positioning method and related equipment
CN116929377A
Vehicle pose determination method and device, electronic equipment and storage medium
CN117745812A
Distortion removal method and device for point cloud, electronic equipment and readable storage medium
CN118608435A
Parameterization method for point cloud data and map construction method
US20240078750A1
Cited By
Vehicle calibration method and device, electronic equipment and storage medium
CN119356884A
Boundary-free intelligent mowing robot based on visual fusion positioning
CN120215488A
