A vehicle positioning method, device and equipment

Abstract

The application provides a vehicle positioning method, device and equipment, the method comprising: during the cross-layer driving of a target vehicle, if the target vehicle drives to the junction position of a slope and a first flat layer, determining the initial layer height of the first flat layer based on the sensor data of the target vehicle; determining whether there is an associated flat layer of the first flat layer based on the initial layer height and the stored reference layer height of each flat layer; if not, storing the initial layer height as the reference layer height of the first flat layer, and if yes, storing the reference layer height of the associated flat layer as the reference layer height of the first flat layer; if the target vehicle drives on the first flat layer, correcting the current layer height determined based on the sensor data to obtain a target pseudo layer height based on the reference layer height of the first flat layer; selecting target map data matched with the target pseudo layer height from the built map data, and positioning the target vehicle based on the target map data. Through the technical scheme of the application, accurate positioning of the vehicle can be automatically completed when the target vehicle drives across layers.

Description

Technical Field

[0001] This application relates to the field of mapping and positioning technology, and in particular to a vehicle positioning method, device and equipment. Background Technology

[0002] Memory parking, as an important application in the field of intelligent driving, enables functions such as autonomous vehicle navigation and autonomous parking. Memory parking refers to the system's ability to assist the driver in driving the vehicle from the starting point to the ending point of the pre-defined route and parking it in a space that has been memorized by the intelligent driving system after the driver has manually driven the vehicle along the pre-defined route.

[0003] Mapping and localization technology is one of the key technologies for achieving memory-based parking. This technology comprises two stages: teaching mapping and real-time localization. In the teaching mapping stage, the driver maneuvers the vehicle, and the intelligent driving system records the vehicle's trajectory as the teaching path and scans the surrounding scene to create a map. In the real-time localization stage, once the vehicle enters the mapped area, the intelligent driving system automatically locates the vehicle, providing precise positional information for the vehicle's autonomous driving functions.

[0004] However, in multi-level or underground parking garages, vehicles inevitably have to travel between levels. When vehicles travel between levels, intelligent driving systems cannot automatically and accurately locate the vehicle, nor can they provide precise location information. Summary of the Invention

[0005] This application provides a vehicle positioning method, the method comprising:

[0006] During the cross-level driving process of the target vehicle, if the target vehicle drives to the boundary between the ramp and the first level, the initial level height of the first level is determined based on the sensor data of the target vehicle.

[0007] Based on the initial floor height and the reference floor height of each stored floor, determine whether there is an associated floor of the first floor, wherein the difference between the reference floor height of the associated floor and the initial floor height is less than a first threshold; if not, store the initial floor height as the reference floor height of the first floor; if yes, store the reference floor height of the associated floor as the reference floor height of the first floor.

[0008] If the target vehicle is driving on the first level, the current level determined by the sensor data is corrected based on the reference level of the first level to obtain the target pseudo level of the first level.

[0009] Select target map data that matches the target pseudo-layer height from the existing map data, where the difference between the calibration layer height of the target map data and the target pseudo-layer height is less than a second threshold.

[0010] The target vehicle is located based on the target map data.

[0011] This application provides a vehicle positioning device, the device comprising:

[0012] The determination module is used to determine the initial height of the first level when the target vehicle travels across levels and reaches the boundary between the ramp and the first level, based on the sensor data of the target vehicle; based on the initial height and the reference height of each level that has been stored, determine whether there is an associated level of the first level, wherein the difference between the reference height of the associated level and the initial height is less than a first threshold; if not, store the initial height as the reference height of the first level; if so, store the reference height of the associated level as the reference height of the first level.

[0013] The correction module is used to correct the current floor height determined by the sensor data based on the reference floor height of the first floor if the target vehicle is driving on the first floor, so as to obtain the target pseudo floor height of the first floor.

[0014] The selection module is used to select target map data that matches the target pseudo-layer height from the existing map data, wherein the difference between the calibration layer height of the target map data and the target pseudo-layer height is less than a second threshold.

[0015] The positioning module is used to locate the target vehicle based on the target map data.

[0016] This application provides an in-vehicle terminal device, including: a processor and a machine-readable storage medium, wherein the machine-readable storage medium stores machine-executable instructions that can be executed by the processor; the processor is used to execute the machine-executable instructions to implement the above-described vehicle positioning method.

[0017] This application provides a vehicle, including:

[0018] A camera is used to acquire panoramic image data and send the panoramic image data to a processor;

[0019] An IMU sensor is used to acquire acceleration information and angular velocity information, and to send the acceleration information and angular velocity information to a processor;

[0020] Wheel speed sensor is used to acquire wheel speed and send the wheel speed to processor;

[0021] The processor is used to implement the above-mentioned vehicle positioning method based on the surround view image data, the acceleration information, the angular velocity information, and the wheel speed.

[0022] Wherein, the vehicle includes a vehicle equipped with an intelligent driving system; or,

[0023] The vehicles include those equipped with autonomous driving systems.

[0024] This application provides a machine-readable storage medium storing machine-executable instructions that can be executed by a processor; wherein the processor is used to execute the machine-executable instructions to implement the above-described vehicle positioning method.

[0025] This application provides a computer program product, which includes a computer program that, when executed by a processor, implements the vehicle positioning method described above.

[0026] As can be seen from the above technical solutions, in this embodiment, when the target vehicle is traveling across floors, if the target vehicle is traveling on the first floor, target map data can be selected based on the target pseudo-floor height of the first floor, and the target vehicle can be located based on the target map data. That is, the target vehicle is located by filtering accurate map data based on the target pseudo-floor height. In this way, when the target vehicle is traveling across floors, the accurate positioning of the vehicle can be automatically completed, providing precise location information for the vehicle's autonomous driving function. For positioning similar scenarios on different floors, the floor height of the target vehicle is estimated using the target pseudo-floor height, and the initial positioning pose is determined by combining map and visual observation, ensuring that the initial positioning is accurate and effective, and effectively preventing mismatches.

[0027] For example, as the target vehicle travels through the first level, the accumulated error of the sensor data increases (sensor data is obtained recursively, and the error gradually increases with the recursion distance). Therefore, the error in determining the current level height from the sensor data becomes increasingly larger. Based on this, the current level height determined by the sensor data is corrected using the reference level height of the first level to obtain a target pseudo-level height. The accuracy of the target pseudo-level height is greater than that of the current level height. When selecting target map data based on the target pseudo-level height, accurate and reliable target map data can be selected, thereby improving the accuracy of target vehicle positioning. Attached Figure Description

[0028] Figure 1A This is a flowchart illustrating a vehicle positioning method according to one embodiment of this application;

[0029] Figure 1B This is a flowchart illustrating a map construction method according to one embodiment of this application;

[0030] Figure 2 This is a schematic diagram of the sensors of the teaching vehicle / target vehicle in one embodiment of this application;

[0031] Figure 3 This is a schematic diagram of the mapping and positioning process in one embodiment of this application;

[0032] Figure 4 This is a flowchart illustrating the teaching mapping process in one embodiment of this application;

[0033] Figure 5 This is a schematic diagram of dual-threshold ramp detection in one embodiment of this application;

[0034] Figure 6 This is a schematic diagram illustrating the principle of the pseudo-layer height algorithm in one embodiment of this application;

[0035] Figure 7 This is a flowchart illustrating the real-time positioning process in one embodiment of this application;

[0036] Figure 8 This is a schematic diagram of the vehicle positioning device in one embodiment of this application;

[0037] Figure 9 This is a hardware structure diagram of an in-vehicle terminal device according to one embodiment of this application. Detailed Implementation

[0038] This application proposes a vehicle positioning method that can be applied to an in-vehicle terminal device deployed on the target vehicle. The in-vehicle terminal device supports an intelligent driving system to enable intelligent driving of the target vehicle. For example, the target vehicle can be a vehicle equipped with an intelligent driving system (such as a regular vehicle equipped with an intelligent driving system or an automatic parking system, where the intelligent driving system assists the driver in completing vehicle positioning). Alternatively, the target vehicle can also be a vehicle equipped with an autonomous driving system (i.e., a vehicle that does not require a driver, where the autonomous driving system independently completes vehicle positioning), such as a robot, logistics vehicle, or driverless passenger vehicle; the type of target vehicle is not limited.

[0039] See Figure 1A The diagram shown is a flowchart of the method, which may include:

[0040] Step 101: During the cross-level driving process of the target vehicle, if the target vehicle reaches the boundary between the ramp and the first level, the initial level height of the first level is determined based on the sensor data of the target vehicle.

[0041] Step 102: Based on the initial floor height and the reference floor height of each stored floor, determine whether there is an associated floor of the first floor, where the difference between the reference floor height of the associated floor and the initial floor height is less than a first threshold; if not, the initial floor height of the first floor can be stored as the reference floor height of the first floor; if so, the reference floor height of the associated floor can be stored as the reference floor height of the first floor.

[0042] Step 103: If the target vehicle is traveling on the first level, the current level determined by the sensor data is corrected based on the reference level of the first level to obtain the target pseudo level of the first level.

[0043] Step 104: Select target map data that matches the target pseudo-layer height from the existing map data, and the difference between the calibration layer height of the target map data and the target pseudo-layer height is less than the second threshold.

[0044] Step 105: Locate the target vehicle based on the target map data.

[0045] The process of determining the boundary position between the ramp and the first leveling layer of the target vehicle includes: determining the pitch angle corresponding to the sensor data of the target vehicle at each moment; if the pitch angle corresponding to the sensor data at the first moment reaches the first angle threshold, then searching backward from the first moment to the second moment, and the pitch angle corresponding to the sensor data at the second moment reaches the second angle threshold; the second angle threshold is less than the first angle threshold; and determining the position of the target vehicle at the second moment as the boundary position between the ramp and the first leveling layer.

[0046] Determining the pitch angle corresponding to the sensor data of the target vehicle at each moment includes: if the sensor data includes longitudinal acceleration, then determining the pitch angle corresponding to the sensor data based on the longitudinal acceleration and the wheel acceleration of the target vehicle; wherein, the pitch angle corresponding to the sensor data is determined using the following formula: Alternatively, if the sensor data includes lateral acceleration, longitudinal acceleration, and axial acceleration, the pitch angle corresponding to the sensor data is determined based on the lateral acceleration, longitudinal acceleration, axial acceleration, and the wheel acceleration of the target vehicle; wherein, the pitch angle corresponding to the sensor data is determined using the following formula: θ pitch Indicates the pitch angle, a lon a represents longitudinal acceleration. vel Let g represent the acceleration of the wheel, and a represent the acceleration due to gravity. lat a represents lateral acceleration. up It represents axial acceleration.

[0047] For example, if the target vehicle is traveling on the first level, locating the target vehicle based on the target map data includes: during initial positioning of the target vehicle, acquiring surround view image data of the target vehicle, determining visual feature points of the current position based on the surround view image data, and performing initial positioning of the target vehicle based on the visual feature points of the current position and the visual feature points of multiple trajectory points in the target map data. Alternatively, during follow-up positioning of the target vehicle, acquiring surround view image data of the target vehicle, determining visual semantic features of the current position based on the surround view image data, and performing follow-up positioning of the target vehicle based on the visual semantic features of the current position and the visual semantic features of multiple trajectory points in the target map data.

[0048] For example, when the target vehicle is driving on the first level, the first level may have special markings such as lane lines and speed bumps. Visual feature points refer to the detection and description of key points in image processing. These key points have unique properties in the image, such as corners and edges, and can represent certain specific areas or objects in the image. Based on this, visual feature points of special markings such as lane lines and speed bumps can be extracted. For example, the difference of Gaussian (DoG) operator can be used to extract visual feature points. There are no restrictions on the extraction method of these visual feature points.

[0049] When the target vehicle travels on the first level, the first level may contain special markings such as lane lines and speed bumps. Visual semantic features refer to the semantic description of an object in an image. For example, identifying the location of a lane line in an image and labeling that location with the visual semantic meaning of "lane line," or identifying the location of a speed bump in an image and labeling that location with the visual semantic meaning of "speed bump." In summary, visual semantic features can identify the location of special markings in an image and label that location with visual semantic meaning, without any limitations.

[0050] For example, if the target vehicle is traveling on a slope, first map data matching the slope can be selected from the existing map data. Based on this, when performing initial or follow-up positioning of the target vehicle using the first map data matching the slope, the vehicle's surround view image data is acquired, and the visual feature points of the current position are determined based on the surround view image data. Based on the visual feature points of the current position and the visual feature points of multiple trajectory points in the first map data, the target vehicle is initially or followed up. That is, both initial and follow-up positioning utilize visual feature points for positioning.

[0051] As can be seen from the above technical solutions, in this embodiment, when the target vehicle is traveling across floors, if the target vehicle is traveling on the first floor, target map data can be selected based on the target pseudo-floor height of the first floor, and the target vehicle can be located based on the target map data. That is, the target vehicle is located by filtering accurate map data based on the target pseudo-floor height. In this way, when the target vehicle is traveling across floors, the accurate positioning of the vehicle can be automatically completed, providing precise location information for the vehicle's autonomous driving function. For positioning similar scenarios on different floors, the floor height of the target vehicle is estimated using the target pseudo-floor height, and the initial positioning pose is determined by combining map and visual observation, ensuring that the initial positioning is accurate and effective, and effectively preventing mismatches.

[0052] For example, as the target vehicle travels through the first level, the accumulated error of the sensor data increases (sensor data is obtained recursively, and the error gradually increases with the recursion distance). Therefore, the error in determining the current level height from the sensor data becomes increasingly larger. Based on this, the current level height determined by the sensor data is corrected using the reference level height of the first level to obtain a target pseudo-level height. The accuracy of the target pseudo-level height is greater than that of the current level height. When selecting target map data based on the target pseudo-level height, accurate and reliable target map data can be selected, thereby improving the accuracy of target vehicle positioning.

[0053] This application proposes a map construction method that can be applied to an in-vehicle terminal device deployed in a teaching vehicle. The in-vehicle terminal device supports an intelligent driving system to enable intelligent driving of the teaching vehicle. For example, the teaching vehicle can be a vehicle equipped with an intelligent driving system (such as a regular vehicle equipped with an intelligent driving system or an automatic parking system, where the intelligent driving system assists the driver in locating the vehicle). Alternatively, the teaching vehicle can also be a vehicle equipped with an autonomous driving system (i.e., a vehicle that does not require a driver, where the autonomous driving system independently completes vehicle location), such as a robot, logistics vehicle, or driverless passenger vehicle; the type of teaching vehicle is not limited.

[0054] See Figure 1B The diagram shown is a flowchart of the method, which may include:

[0055] Step 111: During the teaching vehicle's cross-level travel, if the teaching vehicle travels to the boundary between the ramp and the second level, the initial level height of the second level is determined based on the sensor data of the teaching vehicle.

[0056] Step 112: Based on the initial floor height and the reference floor height of each stored floor, determine whether there is an associated floor of the second floor, where the difference between the reference floor height of the associated floor and the initial floor height is less than a first threshold; if not, the initial floor height of the second floor can be stored as the reference floor height of the second floor; if so, the reference floor height of the associated floor can be stored as the reference floor height of the second floor.

[0057] Step 113: If the teaching vehicle is driving on the second level, the current level determined by the sensor data is corrected based on the reference level of the second level to obtain the calibrated level of the second level.

[0058] Step 114: Establish map data for the teaching vehicle on the second level. Based on the calibration height of the second level, associate the existing map data of the second level with the calibration height.

[0059] The process of determining the boundary position between the ramp and the second leveling layer of the teaching vehicle includes: determining the pitch angle corresponding to the sensor data of the teaching vehicle at each moment; if the pitch angle corresponding to the sensor data at the third moment reaches the first angle threshold, then starting from the third moment, searching backward to the fourth moment, and the pitch angle corresponding to the sensor data at the fourth moment reaches the second angle threshold; the second angle threshold is less than the first angle threshold; and determining the position of the teaching vehicle at the fourth moment as the boundary position between the ramp and the second leveling layer.

[0060] For example, determining the pitch angle corresponding to the sensor data of the teaching vehicle at each moment may include, but is not limited to: if the sensor data includes longitudinal acceleration, then the pitch angle corresponding to the sensor data is determined based on the longitudinal acceleration and the wheel acceleration of the teaching vehicle; or, if the sensor data includes lateral acceleration, longitudinal acceleration and axial acceleration, then the pitch angle corresponding to the sensor data is determined based on the lateral acceleration, longitudinal acceleration, axial acceleration and the wheel acceleration of the teaching vehicle.

[0061] For example, if the teaching vehicle is traveling on the second level, establishing map data for the teaching vehicle on the second level may include: if the distance traveled by the teaching vehicle from the preset initial position is less than a preset distance threshold, then acquiring surround view image data of the teaching vehicle, determining visual feature points of the current position based on the surround view image data; and establishing map data for the teaching vehicle on the second level based on the visual feature points of the current position, which is used for initial positioning of the target vehicle. Alternatively, if the distance traveled by the teaching vehicle from the preset initial position is not less than the preset distance threshold, then acquiring surround view image data of the teaching vehicle, determining visual semantic features of the current position based on the surround view image data; and establishing map data for the teaching vehicle on the second level based on the visual semantic features of the current position, which is used for following and positioning of the target vehicle.

[0062] For example, if the teaching vehicle is traveling on a slope, map data of the teaching vehicle on the slope can also be created. For instance, creating map data of the teaching vehicle on the slope may include, but is not limited to: if the distance traveled by the teaching vehicle from a preset initial position is less than a preset distance threshold, then acquiring surround view image data of the teaching vehicle, determining visual feature points of the current position based on the surround view image data; and creating map data of the teaching vehicle on the slope based on the visual feature points of the current position. This map data can be used for initial positioning of the target vehicle. Alternatively, if the distance traveled by the teaching vehicle from a preset initial position is not less than a preset distance threshold, then acquiring surround view image data of the teaching vehicle, determining visual feature points of the current position based on the surround view image data; and creating map data of the teaching vehicle on the slope based on the visual feature points of the current position. This map data can be used for following and positioning of the target vehicle.

[0063] The technical solutions described above in the embodiments of this application will be explained below in conjunction with specific application scenarios.

[0064] Mapping and localization technology comprises two phases: teaching mapping and real-time localization. In the teaching mapping phase, a driver navigates the vehicle, and the intelligent driving system records the vehicle's trajectory as the teaching path and scans the surrounding scene to create a map. In the real-time localization phase, once the vehicle enters the mapped area, the intelligent driving system automatically locates the vehicle, providing precise location information for its autonomous driving functions. However, in multi-level parking lots or underground parking garages, when a vehicle travels across levels, the intelligent driving system cannot automatically and accurately locate the vehicle, thus failing to provide precise location information.

[0065] For example, high scene similarity across different floors can lead to mismatches in the initial localization, resulting in the inability to automatically and accurately locate the vehicle. Crossing floors requires traversing ramps, which often lack sufficient visual semantic information, further hindering automatic vehicle localization. Furthermore, the limited computing resources of in-vehicle terminal devices prevent the construction of complex maps and the use of resource-intensive localization algorithms, thus hindering the automatic and accurate localization of vehicles based on complex maps and algorithms.

[0066] To address the above findings, this application proposes a vehicle localization method that enables cross-floor memory parking mapping and localization even with limited computing resources in the vehicle terminal device. For localization issues in similar scenarios across different floors, pseudo-floor height is used to estimate the floor height of the vehicle's location. This, combined with map and visual observation, determines the initial localization pose, effectively preventing false matching. To address the issue of limited feature sets in ramp scenarios, pitch angles are calculated using IMU (Inertial Measurement Unit) gravity constraints, and a multi-threshold approach is employed to improve ramp detection accuracy. In ramp scenarios, richer feature points are used for mapping and localization. In level scenarios, more accurate and efficient visual semantics are used for mapping and localization.

[0067] For example, mapping and localization technology may include two stages: teaching mapping and real-time localization. For ease of distinction, the vehicle in the teaching mapping stage is referred to as the teaching vehicle, and the vehicle in the real-time localization stage is referred to as the target vehicle. The teaching vehicle and the target vehicle may be the same or different.

[0068] The teaching vehicle / target vehicle may include onboard terminal equipment, which enables vehicle positioning. (See also: [link to related documentation]). Figure 2 As shown, the teaching vehicle / target vehicle also includes the following sensors.

[0069] Surround-view cameras: Taking four cameras as an example, these four cameras are the left-side camera, right-side camera, front-side camera, and rear-side camera. Of course, the number of cameras can be more or less. For example, two cameras at the front and rear. Or a camera with eight lenses mounted on the roof of the vehicle. Surround-view cameras are used to provide visual observation of the environment around the vehicle. For example, the cameras can collect surround-view image data around the vehicle and send the surround-view image data to the on-board terminal equipment.

[0070] Positioning sensor: The positioning sensor can be a GPS (Global Positioning System) sensor or a BeiDou sensor; there are no restrictions. Taking a GPS sensor as an example, the positioning sensor provides satellite positioning signals and can output the absolute pose of the teaching vehicle / target vehicle. In other words, it can send the absolute pose of the teaching vehicle / target vehicle to the onboard terminal equipment.

[0071] IMU sensor / wheel speed sensor. The IMU sensor can be a 3-axis IMU sensor or a 6-axis IMU sensor, used to provide motion information of the teaching vehicle / target vehicle, such as acceleration information and angular velocity information. That is, it can send the acceleration information and angular velocity information of the teaching vehicle / target vehicle to the on-board terminal equipment.

[0072] Wheel speed sensors are used to provide motion information of the teaching vehicle / target vehicle. For example, wheel speed sensors are used to detect wheel speed and can send the wheel speed of the teaching vehicle / target vehicle to the on-board terminal equipment.

[0073] For example, the data output by the IMU sensor (such as acceleration information and angular velocity information) and the data output by the wheel speed sensor (such as wheel speed) can be referred to as sensor data, i.e. vehicle motion data.

[0074] For example, see Figure 3 The diagram shown is a schematic of the mapping and positioning process.

[0075] For the mapping and localization process, the input data includes data provided by the camera (such as surround view image data around the teaching vehicle / target vehicle), data provided by the GPS sensor (such as the absolute pose of the teaching vehicle / target vehicle), data provided by the IMU sensor (such as the acceleration and angular velocity information of the teaching vehicle / target vehicle), and data provided by the wheel speed sensor (such as the wheel speed of the teaching vehicle / target vehicle).

[0076] After receiving input data, the vehicle-mounted terminal equipment can perform data preprocessing. During preprocessing, operations such as time synchronization and invalid data filtering can be performed. Time synchronization refers to associating data from the same moment in time, such as associating surround view image data, absolute pose, acceleration information, angular velocity information, and wheel speed from the same moment. Invalid data filtering refers to removing invalid or redundant data.

[0077] In mapping mode, the vehicle-mounted terminal device can perform teaching mapping based on input data to obtain map data. In positioning mode, the vehicle-mounted terminal device can achieve real-time positioning based on input data and map data to obtain positioning results, which can represent the real-time location of the target vehicle. Before achieving real-time positioning, teaching mapping needs to be completed for the same road segment; that is, teaching mapping is performed before real-time positioning.

[0078] In mapping mode, the vehicle-mounted terminal device comprehensively utilizes the observation information from various sensors to extract and generate necessary elements in the scene, and completes map construction after fusion processing. In positioning mode, the vehicle-mounted terminal device combines the existing map with the observation information from various sensors to calculate the vehicle's current position and attitude.

[0079] First, the teaching mapping process. After the driver clicks the "Start Mapping" button, the teaching vehicle activates the teaching mapping function. See also... Figure 4 The diagram shown is a flowchart of the teaching mapping process. The teaching mapping process can include six steps: odometer calculation, slope detection, pseudo-layer height estimation, mapping method selection, feature point extraction and fusion, and semantic extraction and fusion. These steps are explained below.

[0080] 1. Odometer Calculation. For the odometer calculation process, the input data can be sensor data (such as acceleration and angular velocity information output by IMU sensors, and wheel speed output by wheel speed sensors, etc.), and the output data can be odometer data. Odometer data can include the vehicle's pose, i.e., position and attitude, and the attitude includes at least the pitch angle, i.e., the pitch angle corresponding to the teaching vehicle.

[0081] During the odometer calculation process, based on sensor data (such as acceleration and angular velocity information output by IMU sensors, and wheel speed output by wheel speed sensors), the relative pose of the teaching vehicle at different times (i.e., odometer data) can be calculated recursively. This relative pose may include pitch angle.

[0082] For a 3-axis IMU sensor, it can output the lateral acceleration, longitudinal acceleration, and yaw rate of the teaching vehicle, but it does not directly measure the pitch and roll rates. For a 6-axis IMU sensor, although the output data also includes yaw acceleration, pitch rate, and roll rate, the pitch and roll angles calculated by the odometer will gradually diverge after a long period of recursion, which is not conducive to slope detection.

[0083] To address this, gravity constraints are introduced in this embodiment to provide observations of pitch and roll angles and suppress their divergence. Gravity constraints provide absolute observations of pitch and roll angles, effectively suppressing divergence in these two degrees of freedom and ensuring that the pitch angle calculated by the odometer is consistent with the actual value. This ensures the feasibility of slope detection and pseudo-floor height estimation when based on this pitch angle.

[0084] In one possible implementation, if the sensor data includes longitudinal acceleration, the pitch angle of the teaching vehicle can be determined based on the longitudinal acceleration and the wheel acceleration of the teaching vehicle. For example, for a 3-axis IMU sensor, the sensor data output by the IMU sensor may include longitudinal acceleration, so the pitch angle and roll angle of the teaching vehicle can be determined using the following formula (1).

[0085]

[0086] In one possible implementation, if the sensor data includes lateral acceleration, longitudinal acceleration, and axial acceleration, the pitch angle of the teaching vehicle can be determined based on the lateral acceleration, longitudinal acceleration, axial acceleration, and the wheel acceleration of the teaching vehicle. For example, for a 6-axis IMU sensor, the sensor data output by the IMU sensor may include lateral acceleration, longitudinal acceleration, and axial acceleration. Therefore, the pitch angle and roll angle of the teaching vehicle can be determined using the following formula (2).

[0087]

[0088] In formulas (1) and (2), θ pitch θ represents the pitch angle corresponding to the teaching vehicle. roll This indicates the roll angle corresponding to the teaching vehicle. lon a represents longitudinal acceleration. lat a represents lateral acceleration. up These three accelerations represent the axial acceleration, which can be determined by sensor data output from the IMU sensor.

[0089] a vel This represents wheel acceleration, which can be determined from the wheel speed data output by the wheel speed sensor. For example, the wheel speed can be determined based on the wheel speed data, and the wheel acceleration can be obtained by differentiating the wheel speeds.

[0090] g represents gravitational acceleration, which can be a fixed acceleration value.

[0091] 2. Slope Detection. For the slope detection process, the input data can be the pitch angle of the teaching vehicle, and the output data can be the slope detection result, which indicates whether it is a slope or a level.

[0092] For example, during ramp detection, if the pitch angle of the teaching vehicle is greater than a threshold, the location of the teaching vehicle can be considered a ramp. However, since pitch angle estimation is affected by noise and fluctuates, the single-threshold detection method has poor stability and is prone to false ramp detection.

[0093] To address this issue, this embodiment employs a dual-threshold ramp detection method. See [link / reference] Figure 5 The diagram illustrates a dual-threshold ramp detection method. When the pitch angle reaches threshold B for multiple consecutive frames, the teaching vehicle is determined to have entered the ramp. Using this time period as a baseline, all moments exceeding threshold A are searched forward and backward, and the teaching vehicle's position at these moments is considered a ramp. This dual-threshold ramp detection method effectively filters out false ramp detections caused by pitch angle estimation errors, thus accurately detecting ramps.

[0094] In one possible implementation, the pitch angle corresponding to the sensor data of the teaching vehicle at each moment can be obtained based on odometer calculation. Based on this, if the pitch angle corresponding to the sensor data at the third moment reaches a first angle threshold (e.g., threshold B), then the search proceeds backward from the third moment to the fourth moment, and the pitch angle corresponding to the sensor data at the fourth moment reaches a second angle threshold (e.g., threshold A). That is, the fourth moment is the first moment after the third moment where the pitch angle equals the second angle threshold. The search proceeds backward from the third moment to the fifth moment, and the pitch angle corresponding to the sensor data at the fifth moment reaches the second angle threshold. That is, the fifth moment is the first moment before the third moment where the pitch angle equals the second angle threshold.

[0095] Based on this, the position of the teaching vehicle between the fifth and fourth moments can be considered the slope position. Assuming the position of the teaching vehicle at the fifth moment is denoted as position 5, and the position at the fourth moment as position 4, then the position between position 5 and position 4 is the slope position.

[0096] For example, the first angle threshold (such as threshold B) can be configured based on experience, and the second angle threshold (such as threshold A) can be configured based on experience, provided that the second angle threshold is less than the first angle threshold.

[0097] Clearly, the above method can be used to obtain all ramp locations. In multi-level or underground parking garages, when the demonstration vehicle travels across levels, all locations except ramp locations are level locations. Therefore, the location between two ramp locations can be considered a level location. For example, the location between location 5 and location 4 is a ramp location, and the location between location 3 and location 2 is also a ramp location. When the demonstration vehicle travels from location 4 to location 3, the location between location 4 and location 3 is a level location, and location 4 is the boundary between the ramp and the level (i.e., the starting position of the level), while location 3 is the boundary between the ramp and the level (i.e., the ending position of the level). Similarly, when the demonstration vehicle travels from location 3 to location 4, the location between location 3 and location 4 is a level location, and location 3 is the boundary between the ramp and the level (i.e., the starting position of the level), while location 4 is the boundary between the ramp and the level (i.e., the ending position of the level).

[0098] In summary, the ramp detection process yields ramp detection results, which can indicate whether each location is a ramp location or a leveling location. For the boundary between a ramp and a leveling location, the ramp detection results can also indicate whether that location is the boundary between the ramp and the leveling location.

[0099] 3. Pseudo-story height estimation. For the pseudo-story height estimation process, the input data can be sensor data, slope detection results, and GPS data, and the output data can be the pseudo-story height, denoted as the calibration story height, i.e., the calibration pseudo-story height.

[0100] For example, in an outdoor setting, the GPS sensor can output the absolute altitude of the teaching vehicle's location. However, once indoors, the GPS signal is blocked by buildings and disappears, preventing the GPS sensor from outputting the absolute altitude. After this, the absolute altitude of the teaching vehicle can be determined based on the sensor data. However, since the sensor data is used to recursively derive the absolute altitude, the altitude error gradually increases with the recursion distance.

[0101] For example, see Figure 6 The diagram shown illustrates the principle of the pseudo-floor height algorithm. For ease of description, the principle of the pseudo-floor height algorithm is explained by demonstrating the driving process of a teaching vehicle going from the outside, passing a ramp to floor -1, then from floor -1, passing a ramp to floor -2, then from floor -2, passing a ramp to floor -1, and finally from floor -1, passing a ramp to the ground.

[0102] against Figure 6 The first image is an uncorrected diagram of the height variation. After the GPS signal disappears, the height of the teaching vehicle can be recursively calculated based on sensor data. As the recursion distance increases, the height gradually diverges, and after the teaching vehicle returns to the ground, a significant height error appears between the actual value and the calculated value.

[0103] against Figure 6 The second image is a schematic diagram illustrating the correction of the leveling height recursion error. The height of the leveling position is corrected using the ramp detection results; that is, the leveling height remains unchanged, ensuring that the height error is only caused by the ramp recursion error. Clearly, compared to the first image, this method can correct some of the height error.

[0104] For example, if the ramp detection results determine that locations A and B belong to the same level, and location A is the starting point of that level, then based on the height x of location A, the height of all locations from A to B is set to height x. In other words, the height of all locations on the level is height x. This way, the height of all locations on the level no longer changes, meaning the height is no longer altered based on sensor data. Clearly, after this processing, the height error caused by sensor data in the leveling process can be corrected.

[0105] against Figure 6 The third image is a diagram illustrating the relationship between the -1 floor height. For example, you can search for related floors based on a height difference threshold; when passing through the -1 floor twice, the corresponding height of that floor will be associated.

[0106] For example, when the teaching vehicle passes through level -1 for the first time, the height of level -1, x1, can be recorded. When the teaching vehicle passes through level -1 for the second time, the height of level -1, x2, can be recorded. If the absolute value of the difference between height x1 and height x2 is less than the height difference threshold, it means that height x1 and height x2 correspond to the same level.

[0107] against Figure 6 The fourth image is a diagram illustrating the relationship between the -1 floor height correction and the ground height. The associated floor will be corrected based on the height of the first pass. After the -1 floor height is corrected, the height difference between the two passes will be less than the height difference threshold, therefore, the ground height will also be associated.

[0108] For example, the height x1 of the second passage through the -1 layer can be adjusted based on the height x1 of the first passage through the -1 layer. In other words, the height x2 is replaced with the height x1, thereby adjusting the height of the second passage through the -1 layer.

[0109] against Figure 6 The fifth image is a diagram illustrating ground height correction. After the ground height is linked, it will be corrected according to the initial height value, thus eliminating height errors between floors.

[0110] Of course, this application is not limited to two-story scenarios. For indoor scenarios involving floors across ramps, the above method can be used to calculate the pseudo-floor height to eliminate the cumulative error of sensor data between floors.

[0111] When using the above method for correction, the corrected floor height is not completely consistent with the actual height. Therefore, the corrected floor height can be called pseudo-floor height. For example, as can be seen from the principle of the pseudo-floor height algorithm, when passing through the ramp of the corresponding floor for the first time, the sensor data recursion error is not corrected, but this error is relatively small and will not affect the accuracy of the teaching mapping process and the real-time positioning process.

[0112] For example, based on the principle of the pseudo-layer height algorithm, the calibration layer height can be determined in the following way:

[0113] If the GPS signal is valid, the GPS sensor can output the absolute height of the teaching vehicle's location. Thus, if the teaching vehicle travels to a level, the reference floor height of that level can be determined based on the height output by the GPS sensor, and the reference floor height of that level can be stored. There are no restrictions on this process.

[0114] If the GPS signal is invalid, then if the teaching vehicle travels to the level, it can include:

[0115] Step S11: During the teaching vehicle's cross-level travel, if the teaching vehicle reaches the boundary between the ramp and the level, the initial level height of that level is determined based on the sensor data of the teaching vehicle. For ease of distinction, the level in the teaching mapping process is referred to as the second level, which can be any level.

[0116] For example, when the GPS signal is valid, the GPS sensor can output the absolute altitude of the teaching vehicle's location. When the GPS signal is invalid, the absolute altitude at which the GPS signal is invalid is recorded as altitude k1. In subsequent processes, the sensor data is used based on altitude k1 to determine the altitude of the teaching vehicle's location.

[0117] For example, starting from when the GPS signal is invalid, assuming that the height of the teaching vehicle has changed downward by height k2 based on sensor data, then the current height is k1-k2, and so on.

[0118] Assuming height k1 is the height of a certain level, the teaching vehicle will traverse a ramp when traveling down from this level to another level, and the height of this ramp can be calculated. For example, based on the ramp detection results, the start and end positions of the ramp can be determined. Based on the sensor data at the start and end positions of the ramp, the height of the ramp can be determined and denoted as height k3. Therefore, if the teaching vehicle travels to the boundary between the ramp and the level, the initial level height of that level, i.e., k1-k3, is determined.

[0119] Assuming the reference floor height (defined in subsequent steps) of a certain level is k4, when the teaching vehicle travels downhill from this level to another level, it will cross a ramp. Based on sensor data from the start and end positions of the ramp, the height of the ramp can be determined and denoted as height k5. Therefore, if the teaching vehicle reaches the boundary between the ramp and the level, the initial floor height of that level is determined to be k4-k5. If the teaching vehicle travels uphill from this level to another level, the initial floor height of that level is determined to be k4+k5.

[0120] In summary, during the teaching vehicle's cross-level travel, if the teaching vehicle reaches the boundary between the ramp and the second level, the initial level height of the second level can be determined, and there are no restrictions on this process.

[0121] Step S12: Based on the initial floor height of the second flat floor and the reference floor height of each stored flat floor, determine whether there is an associated flat floor for the second flat floor. The difference between the reference floor height of the associated flat floor and the initial floor height of the second flat floor is less than a first threshold. The first threshold can be configured based on experience and is not restricted.

[0122] For example, after obtaining the reference floor height of a certain floor, the reference floor height of that floor can be stored. Based on this, after obtaining the initial floor height of the second floor, the difference between the initial floor height of the second floor and the reference floor height of each floor can be calculated, such as the absolute value of the difference between the initial floor height and the reference floor height of each floor.

[0123] If the difference between the reference floor height of a certain floor and the initial floor height of the second floor is less than a first threshold, then that floor is considered an associated floor of the second floor. If the difference between the reference floor height of all floors and the initial floor height of the second floor is not less than the first threshold, then there is no associated floor of the second floor.

[0124] For example, calculate the difference between the initial floor height of the second floor and the reference floor height of each floor, and find the minimum difference. If the minimum difference is less than a first threshold, the floor corresponding to the minimum difference is considered as the associated floor of the second floor. If the minimum difference is not less than the first threshold, there is no associated floor of the second floor.

[0125] Step S13: If there is no associated floor for the second floor, store the initial floor height of the second floor as its reference floor height. If there is an associated floor for the second floor, store the reference floor height of the associated floor as its reference floor height. Thus, the reference floor height for the second floor is stored.

[0126] Step S14: If the teaching vehicle is driving on the second level, the current level determined by the sensor data is corrected based on the reference level of the second level to obtain the calibrated level of the second level.

[0127] For example, while the teaching vehicle is moving on the second level, the current floor height of its current position can be determined based on sensor data. Based on this, the current floor height can be corrected using a reference floor height of the second level, i.e., the reference floor height of the second level is used as the output floor height for the current position. Clearly, the output floor height at each position on the second level is the reference floor height for the second level. For ease of distinction, the floor height at each position on the second level is designated as the calibration floor height; that is, the second level corresponds to the same calibration floor height. The accuracy of the calibration floor height of the second level is better than the current floor height determined by sensor data.

[0128] 4. Mapping Method Selection. For the mapping method selection process, the input data can be the slope detection results, and the output data is the mapping method, such as feature point extraction and fusion method, or semantic extraction and fusion method.

[0129] For example, to address the issue of insufficient visual semantics in ramp scenes, a scene-specific mapping approach can be adopted. For instance, a preset initial position can be pre-configured. This preset initial position can be the mapping starting point, can be specified by the driver, and can be any position without restriction.

[0130] If the distance traveled by the teaching vehicle from the preset initial position (i.e., the distance the vehicle has already traveled) is less than the preset distance threshold (which can be configured based on experience), then the mapping method is feature point extraction and fusion. That is, regardless of whether the current position is a slope or a flat surface, the feature point extraction and fusion method is used for mapping, i.e., visual feature point mapping is used to ensure that the initial road segment of the mapping uses the visual feature point initial positioning function.

[0131] If the distance traveled by the teaching vehicle from its preset initial position is not less than a preset distance threshold, then: If the current position of the teaching vehicle is a slope, the mapping method is feature point extraction and fusion. Feature point extraction and fusion is used for mapping on slopes, meaning visual feature points are used to create the map at the slope location to fully utilize visual feature information such as wall textures and improve positioning accuracy. If the current position of the teaching vehicle is a level surface, the mapping method is semantic extraction and fusion. Semantic extraction and fusion is used for mapping on level surfaces, meaning visual semantic mapping is used at the level surface location to improve positioning efficiency and accuracy.

[0132] 5. Feature Point Extraction and Fusion. For the feature point extraction and fusion process, the input data can be sensor data and panoramic image data (provided by the camera), and the output data can be map data.

[0133] For example, if the teaching vehicle is traveling on the second level (which can be any level), and if the distance traveled by the teaching vehicle from the preset initial position is less than a preset distance threshold (i.e., the distance is within a preset range), then the surround view image data of the teaching vehicle is acquired, and the visual feature points of the current position are determined based on the surround view image data. Map data of the teaching vehicle on the second level is then built based on the visual feature points of the current position. This map data is used for initial localization of the target vehicle, and there are no restrictions on this mapping process.

[0134] For example, if the teaching vehicle is traveling on a slope, and the distance traveled by the teaching vehicle from the preset initial position is less than a preset distance threshold, then the surround view image data of the teaching vehicle is acquired, and the visual feature points of the current position are determined based on the surround view image data. Map data of the teaching vehicle on the slope is then built based on the visual feature points of the current position, and this map data is used for initial localization of the target vehicle.

[0135] For example, if the teaching vehicle is traveling on a slope, and if the distance traveled by the teaching vehicle from the preset initial position is not less than a preset distance threshold, then the surround view image data of the teaching vehicle is acquired, and the visual feature points of the current position are determined based on the surround view image data. Map data of the teaching vehicle on the slope is then established based on the visual feature points of the current position, and this map data is used for following and locating the target vehicle.

[0136] In summary, if the distance traveled by the teaching vehicle from the preset initial position is less than a preset distance threshold, regardless of whether it is on a level surface or a slope, visual feature points are used to build map data, and this map data is used for initial positioning of the target vehicle. Furthermore, if the teaching vehicle is traveling on a slope, visual feature points are used to build map data regardless of whether the travel distance is less than the preset distance threshold.

[0137] 6. Semantic Extraction and Fusion. For the semantic extraction and fusion process, the input data can be sensor data and surround view image data (provided by the camera), and the output data can be map data.

[0138] For example, if the teaching vehicle is traveling on a second level (which can be any level), and if the distance traveled by the teaching vehicle from its preset initial position is not less than a preset distance threshold, then the surround view image data of the teaching vehicle can be acquired, and the visual semantic features of the current position can be determined based on the surround view image data. Map data of the teaching vehicle on the second level is then built based on the visual semantic features of the current position, and this map data is used for following and locating the target vehicle. There are no restrictions on this mapping process.

[0139] For example, both visual feature point mapping and visual semantic mapping first extract features from a single frame of the image (visual feature point mapping extracts image feature points, while visual semantic mapping extracts image semantic information). Then, the pose of the teaching vehicle is determined based on sensor data, and the extracted features are fused across multiple frames based on the pose to form the corresponding map. There are no restrictions on either visual feature point mapping or visual semantic mapping.

[0140] Of course, in order to improve the smoothness of location switching, the mapping method does not need to strictly follow the division of slope and level. It can be extended appropriately to allow the visual feature point map and the visual semantic map to have a certain overlap.

[0141] For example, for the map data of the teaching vehicle on the second level (established based on visual feature points or visual semantic features), a calibration layer height can also be set for the existing map data of the second level, that is, the existing map data of the second level is associated with the calibration layer height of the second level.

[0142] For example, this calibration layer height is used to filter map data. That is, for each trajectory point of the teaching vehicle on the second level, that trajectory point can correspond to this calibration layer height of the second level.

[0143] In summary, after the mapping task is completed, the corresponding map data can be output. This map data may include, but is not limited to: the valid GPS location coordinates of the last frame before the mapping started, the teaching trajectory (each trajectory point includes the calibration layer height), the visual feature point map, and the visual semantic map.

[0144] Second, the real-time positioning process. When the target vehicle enters the area covered by the established map, positioning mode can be activated. The positioning process involves two stages: initial positioning and follow-up positioning. Initial positioning refers to positioning the vehicle after it enters the map, without knowing its exact position or orientation; a positioning accuracy of meters is sufficient. Follow-up positioning, on the other hand, is performed with the vehicle's position and orientation known, achieving a positioning accuracy of centimeters.

[0145] See Figure 7 The diagram shown is a flowchart of the real-time positioning process. The real-time positioning process may include odometry calculation, slope detection, pseudo-layer height estimation, initial feature point localization, predicted pose calculation, map selection and cropping, feature point following localization, semantic following localization, etc. These processes are explained below.

[0146] 1. Odometer Calculation. For the odometer calculation process, the input data is sensor data (such as acceleration and angular velocity information output by IMU sensors, and wheel speed output by wheel speed sensors), and the output data can be odometer data, which includes the pitch angle, i.e., the pitch angle corresponding to the target vehicle.

[0147] In one possible implementation, if the sensor data includes longitudinal acceleration, the pitch angle of the target vehicle can be determined based on the longitudinal acceleration and the wheel acceleration of the target vehicle. For example, for a 3-axis IMU sensor, the sensor data output by the IMU sensor may include longitudinal acceleration; therefore, the pitch angle and roll angle of the target vehicle can be determined using the above formula (1).

[0148] In one possible implementation, if the sensor data includes lateral acceleration, longitudinal acceleration, and axial acceleration, the pitch angle of the target vehicle can be determined based on the lateral acceleration, longitudinal acceleration, axial acceleration, and the wheel acceleration of the target vehicle. For example, for a 6-axis IMU sensor, the sensor data output by the IMU sensor may include lateral acceleration, longitudinal acceleration, and axial acceleration. Therefore, the pitch angle and roll angle of the target vehicle can be determined using the above formula (2).

[0149] 2. Slope Detection. For the slope detection process, the input data can be the pitch angle corresponding to the target vehicle, and the output data can be the slope detection result, which can be a slope or a level.

[0150] In one possible implementation, the pitch angle corresponding to the sensor data of the target vehicle at each moment can be obtained based on odometer calculation. Based on this, if the pitch angle corresponding to the sensor data at the first moment reaches a first angle threshold (e.g., threshold B), then the search proceeds backward from the first moment to the second moment, and the pitch angle corresponding to the sensor data at the second moment reaches the second angle threshold (e.g., threshold A). That is, the second moment is the first moment after the first moment where the pitch angle equals the second angle threshold. The search proceeds backward from the first moment to the sixth moment, and the pitch angle corresponding to the sensor data at the sixth moment reaches the second angle threshold. That is, the sixth moment is the first moment before the first moment where the pitch angle equals the second angle threshold. Based on this, the position of the target vehicle between the sixth moment and the second moment can be a slope position.

[0151] Clearly, the above method can be used to obtain all ramp locations. Besides the ramp locations, the remaining locations are leveling locations; the location between two ramp locations can be considered a leveling location.

[0152] In summary, the ramp detection process yields ramp detection results, which can indicate whether each location is a ramp location or a leveling location. For the boundary between a ramp and a leveling location, the ramp detection results can also indicate whether that location is the boundary between the ramp and the leveling location.

[0153] 3. Pseudo-story height estimation. For the pseudo-story height estimation process, the input data can be sensor data, slope detection results, and GPS data, and the output data can be the pseudo-story height, denoted as the target pseudo-story height.

[0154] For example, if the GPS signal is invalid, when the target vehicle drives to the level, it may include:

[0155] Step S21: During the target vehicle's cross-level travel, if the target vehicle reaches the boundary between the ramp and the level, the initial level height of that level is determined based on the target vehicle's sensor data. For ease of distinction, the level during the real-time positioning process is designated as the first level, which can be any level.

[0156] For the method of determining the initial floor height of the first floor, please refer to step S11, which will not be repeated here.

[0157] Step S22: Based on the initial floor height of the first floor and the reference floor height of each stored floor, determine whether there is an associated floor of the first floor. The difference between the reference floor height of the associated floor and the initial floor height of the first floor is less than a first threshold. The first threshold can be configured based on experience and is not restricted.

[0158] For example, after obtaining the reference floor height of a certain floor, this reference floor height can be stored. Based on this, after obtaining the initial floor height of the first floor, the difference between the initial floor height of the first floor and the reference floor height of each floor is calculated. If the difference between the reference floor height of a certain floor and the initial floor height of the first floor is less than a first threshold, then that floor is considered an associated floor of the first floor. If the difference between the reference floor height of all floors and the initial floor height of the first floor is not less than the first threshold, then there are no associated floors of the first floor.

[0159] Step S23: If there is no associated floor for the first floor, then store the initial floor height of the first floor as its reference floor height. If there is an associated floor for the first floor, then store the reference floor height of the associated floor as its reference floor height. Thus, the reference floor height for the first floor is stored.

[0160] Step S24: If the target vehicle is driving on the first level, the current level determined by the sensor data is corrected based on the reference level of the first level to obtain the target pseudo level of the first level.

[0161] For example, as the target vehicle travels along the first level, its current level height can be determined based on sensor data. Then, the current level height can be corrected based on a reference level height of the first level, which is used as the output level height for the current position. Clearly, the output level height at each position on the first level is also the reference level height. For ease of distinction, the output level height at each position on the first level is denoted as the target pseudo-level height; that is, each level corresponds to the same target pseudo-level height. The accuracy of the target pseudo-level height is greater than the current level height determined by the sensor data.

[0162] 4. Initial localization of feature points. For the initial localization of feature points, the input data can be pseudo-layer height estimation results, map data, and surround view image data. The pseudo-layer height estimation results can include the target pseudo-layer height of the first level, and the output data can be the initial localization pose of the target vehicle.

[0163] For example, the memory parking function does not limit the number of teaching maps; that is, the target vehicle can download and store multiple maps. Therefore, based on the nearest valid GPS location corresponding to the current location (denoted as GPS location A), one or more existing map data can be selected from all existing map data, provided that the distance between the GPS location of these existing map data and GPS location A is less than a preset distance threshold. Based on the selected one or more existing map data, initial positioning can be achieved in the following manner.

[0164] When performing initial localization of a target vehicle, if the target vehicle is traveling on a slope (based on slope detection results indicating the current location is on a slope), the first map data matching the slope can be selected from all existing map data. This first map data is then used to perform initial localization of the target vehicle. During the initial localization process, visual feature points are used, and the first map data is built based on these visual feature points.

[0165] For example, surround view image data of the target vehicle can be acquired, and the visual feature points of the current position can be determined based on the surround view image data. If the first map data includes visual feature points of multiple trajectory points, the target vehicle can be initially located based on the visual feature points of the current position and the visual feature points of multiple trajectory points, that is, the trajectory points of the target vehicle's current position can be determined from the multiple trajectory points.

[0166] When initially locating a target vehicle, if the vehicle is traveling on the first level (based on slope detection results), then target map data matching the target pseudo-level height of the first level (determined based on pseudo-level height estimation results) can be selected from all existing map data. This allows for initial positioning of the target vehicle based on the target map data. During this initial positioning process, visual feature points are used, and the target map data is built upon these visual feature points.

[0167] For example, during map construction, each existing map data corresponds to a calibration layer height. Therefore, the difference between the target pseudo-layer height of the first level and the calibration layer height of each existing map data is calculated. Then, existing map data with a difference less than a second threshold are selected. This process filters the existing map data, finding the one that best matches the target pseudo-layer height as the target map data. Clearly, the difference between the calibration layer height of the target map data and the target pseudo-layer height of the first level is less than the second threshold. Thus, when initially locating a target vehicle based on the target map data, accurate and reliable positioning results can be obtained.

[0168] For example, we can acquire surround view image data of the target vehicle and determine the visual feature points of the current position based on the surround view image data. If the target map data includes visual feature points of multiple trajectory points, then we can perform initial positioning of the target vehicle based on the visual feature points of the current position and the visual feature points of multiple trajectory points, that is, determine the trajectory points of the target vehicle's current position from multiple trajectory points.

[0169] In summary, regardless of whether the target vehicle is on a ramp or the first level, the map data (first map data or target map data) can be matched with the visual feature point observations of the current location to obtain the initial pose value of the target vehicle. There are no restrictions on the initial localization process of the target vehicle.

[0170] In the above process, pseudo floor height can be used for filtering. Even if the visual feature points of similar scenes on different floors are not very different, the map data of the corresponding floor can be filtered out using pseudo floor height. This allows the map data to be distinguished during the initial positioning process, avoiding mismatch and enabling successful positioning.

[0171] 5. Predicted pose calculation. After the initial localization of the target vehicle is successful, follow-up localization is initiated. The first predicted pose of follow-up localization is provided by the initial localization. Subsequent predicted poses are calculated based on the relative pose estimated by the odometer, based on the localization pose of the previous frame. No restrictions are placed on this process.

[0172] 6. Map selection and cropping, feature point following localization, semantic following localization.

[0173] For example, when tracking and locating a target vehicle, if the target vehicle is driving on a slope, the map selection and capture function can be used to select the first map data that matches the slope from all existing map data, and then track and locate the target vehicle based on the first map data.

[0174] Based on the first map data matched with the ramp, and using the feature point following and positioning function, the surround view image data of the target vehicle is acquired, and the visual feature points of the current position are determined based on the surround view image data. If the first map data includes the visual feature points of multiple trajectory points, the target vehicle can be followed and positioned based on the visual feature points of the current position and the visual feature points of multiple trajectory points.

[0175] For example, when performing follow-up positioning on a target vehicle, if the target vehicle is traveling on the first level, based on the map selection and capture function, target map data that matches the target pseudo-level height of the first level is selected from all existing map data, thereby performing follow-up positioning on the target vehicle based on the target map data.

[0176] Based on target map data that matches the target pseudo-layer height, and using semantic following positioning, surround view image data of the target vehicle is acquired. The visual semantic features of the current location are then determined based on this surround view image data. If the target map data includes visual semantic features of multiple trajectory points, the target vehicle can be positioned by following both the visual semantic features of the current location and the visual semantic features of the multiple trajectory points.

[0177] For example, after obtaining the predicted pose, it is possible to query the map data to see if there is a visual feature point map or a visual semantic map near the predicted pose, and select visual feature point following positioning or visual semantic following positioning accordingly, and finally output a high-precision positioning pose. There are no restrictions on this positioning process.

[0178] As can be seen from the above technical solutions, in this embodiment, during the cross-floor driving of the target vehicle, accurate map data can be filtered based on the target pseudo-floor height to locate the target vehicle, automatically completing the accurate vehicle positioning and providing precise location information for the vehicle's autonomous driving function. For positioning in similar scenarios on different floors, the floor height of the target vehicle is estimated using the target pseudo-floor height, and the initial positioning pose is determined by combining map and visual observation, ensuring accurate and effective initial positioning and effectively preventing mismatches. The driver teaches the vehicle, automatically completing map construction. After teaching, real-time positioning can be performed, providing high-precision pose for autonomous driving. It does not require high-cost sensors such as LiDAR, has low computational requirements, and can achieve autonomous cross-floor memory parking function. The scenario-based mapping and positioning scheme can solve the positioning problem caused by the lack of rich visual semantics on ramps. Visual feature points are used for mapping and positioning in ramp scenarios, making full use of non-ground visual feature information to improve positioning robustness. In flat scenarios, visual semantic mapping and positioning are used, ensuring high-precision positioning output while further reducing dependence on computing resources.

[0179] Based on the same concept as the above method, this application proposes a vehicle positioning device, see [link to relevant documentation]. Figure 8 The diagram shown is a structural schematic of the vehicle positioning device, which may include:

[0180] The determining module 81 is used to determine the initial height of the first level floor based on the sensor data of the target vehicle when the target vehicle travels across levels and reaches the boundary between the ramp and the first level floor; based on the initial height and the reference height of each level floor already stored, determine whether there is an associated level floor of the first level floor, wherein the difference between the reference height of the associated level floor and the initial height is less than a first threshold; if not, store the initial height as the reference height of the first level floor; if so, store the reference height of the associated level floor as the reference height of the first level floor.

[0181] The correction module 82 is used to correct the current floor height determined by the sensor data based on the reference floor height of the first floor if the target vehicle is driving on the first floor, so as to obtain the target pseudo floor height of the first floor.

[0182] The selection module 83 is used to select target map data that matches the target pseudo-layer height from the existing map data, wherein the difference between the calibration layer height of the target map data and the target pseudo-layer height is less than a second threshold.

[0183] The positioning module 84 is used to locate the target vehicle based on the target map data.

[0184] For example, when the determining module 81 determines that the target vehicle has traveled to the boundary position between the ramp and the first level, it is specifically used to: determine the pitch angle corresponding to the sensor data of the target vehicle at each moment; if the pitch angle corresponding to the sensor data at the first moment reaches a first angle threshold, then search backward from the first moment to the second moment, and the pitch angle corresponding to the sensor data at the second moment reaches a second angle threshold; wherein, the second angle threshold is less than the first angle threshold; and determine the position of the target vehicle at the second moment as the boundary position between the ramp and the first level.

[0185] For example, when determining the pitch angle corresponding to the sensor data of the target vehicle at each moment, the determining module 81 is specifically used to: if the sensor data includes longitudinal acceleration, then determine the pitch angle corresponding to the sensor data based on the longitudinal acceleration and the wheel acceleration of the target vehicle; wherein, the pitch angle corresponding to the sensor data is determined using the following formula: Alternatively, if the sensor data includes lateral acceleration, longitudinal acceleration, and axial acceleration, then the pitch angle corresponding to the sensor data is determined based on the lateral acceleration, the longitudinal acceleration, the axial acceleration, and the wheel acceleration of the target vehicle; wherein the pitch angle corresponding to the sensor data is determined using the following formula: θ pitch Indicates the pitch angle, a lon a represents longitudinal acceleration. vel Let g represent the acceleration of the wheel, and a represent the acceleration due to gravity. lat a represents lateral acceleration. up It represents axial acceleration.

[0186] For example, if the target vehicle is driving on the first level, the positioning module 84, when locating the target vehicle based on the target map data, specifically performs the following: During initial positioning of the target vehicle, it acquires the surround view image data of the target vehicle and determines the visual feature points of the current position based on the surround view image data; it performs initial positioning of the target vehicle based on the visual feature points of the current position and the visual feature points of multiple trajectory points in the target map data; or, during follow-up positioning of the target vehicle, it acquires the surround view image data of the target vehicle and determines the visual semantic features of the current position based on the surround view image data; it performs follow-up positioning of the target vehicle based on the visual semantic features of the current position and the visual semantic features of multiple trajectory points in the target map data.

[0187] For example, if the target vehicle is driving on a slope, the selection module 83 selects the first map data that matches the slope from the existing map data. The positioning module 84 is also used to acquire the surround view image data of the target vehicle when performing initial positioning or follow-up positioning of the target vehicle, and determine the visual feature point features of the current position based on the surround view image data; and perform initial positioning or follow-up positioning of the target vehicle based on the visual feature point features of the current position and the visual feature point features of multiple trajectory points in the first map data.

[0188] For example, the determining module 81 is further configured to, during the teaching vehicle's cross-level travel, if the teaching vehicle travels to the boundary between the ramp and the second level, determine the initial level height of the second level based on the sensor data of the teaching vehicle; determine whether there is an associated level of the second level based on the initial level height of the second level and the reference level height of each level that has been stored, such that the difference between the reference level height of the associated level and the initial level height is less than a first threshold; if not, store the initial level height as the reference level height of the second level; if so, store the reference level height of the associated level as the reference level height of the second level; the correction module 82 is further configured to, if the teaching vehicle is traveling on the second level, correct the current level height determined by the sensor data based on the reference level height of the second level to obtain the calibration level height of the second level; the vehicle positioning device further includes: a building module, configured to build map data of the teaching vehicle on the second level, and associate the built map data of the second level with the calibration level height based on the calibration level height of the second level.

[0189] For example, if the teaching vehicle is traveling on the second level, the establishment module, when establishing map data for the teaching vehicle on the second level, specifically performs the following: if the distance traveled by the teaching vehicle from the preset initial position is less than a preset distance threshold, then acquire surround view image data of the teaching vehicle, determine visual feature points of the current position based on the surround view image data; establish map data for the teaching vehicle on the second level based on the visual feature points of the current position, and use the map data for initial positioning of the target vehicle; or, if the distance traveled by the teaching vehicle from the preset initial position is not less than the preset distance threshold, then acquire surround view image data of the teaching vehicle, determine visual semantic features of the current position based on the surround view image data; establish map data for the teaching vehicle on the second level based on the visual semantic features of the current position, and use the map data for following and positioning of the target vehicle.

[0190] Based on the same application concept as the above method, this application proposes a vehicle, including:

[0191] A camera is used to acquire panoramic image data and send the panoramic image data to a processor;

[0192] An IMU sensor is used to acquire acceleration information and angular velocity information, and to send the acceleration information and angular velocity information to a processor;

[0193] Wheel speed sensor is used to acquire wheel speed and send the wheel speed to processor;

[0194] The processor is configured to implement the vehicle positioning method of the above example of this application based on the surround view image data, the acceleration information, the angular velocity information and the wheel speed;

[0195] Wherein, the vehicle includes a vehicle equipped with an intelligent driving system; or,

[0196] The vehicles include those equipped with autonomous driving systems.

[0197] Based on the same concept as the above method, this application proposes a vehicle-mounted terminal device, see [link to relevant documentation]. Figure 9 As shown, the vehicle-mounted terminal device includes a processor 91 and a machine-readable storage medium 92, the machine-readable storage medium 92 storing machine-executable instructions that can be executed by the processor 91; the processor 91 is used to execute the machine-executable instructions to implement the vehicle positioning method disclosed in the above example of this application.

[0198] Based on the same concept as the above method, this application also provides a machine-readable storage medium storing a plurality of computer instructions, which, when executed by a processor, can implement the vehicle positioning method disclosed in the above examples of this application.

[0199] The aforementioned machine-readable storage medium can be any electronic, magnetic, optical, or other physical storage device that can contain or store information, such as executable instructions, data, etc. For example, machine-readable storage media can be: RAM (Random Access Memory), volatile memory, non-volatile memory, flash memory, storage drives (such as hard disk drives), solid-state drives, any type of storage disk (such as optical discs, DVDs, etc.), or similar storage media, or combinations thereof.

[0200] Based on the same application concept as the above method, this application embodiment also provides a computer program product, which may include a computer program that, when executed by a processor, implements the vehicle positioning method disclosed in the above examples of this application.

[0201] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, or computer program products. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, embodiments of this application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

[0202] The above description is merely an embodiment of this application and is not intended to limit the scope of this application. Various modifications and variations can be made to this application by those skilled in the art. Any modifications, equivalent substitutions, improvements, etc., made within the spirit and principles of this application should be included within the scope of the claims of this application.

Claims

1. A vehicle positioning method, characterized in that, The method includes: During the cross-level driving process of the target vehicle, if the target vehicle drives to the boundary between the ramp and the first level, the initial level height of the first level is determined based on the sensor data of the target vehicle. Based on the initial floor height and the reference floor height of each stored floor, determine whether there is an associated floor of the first floor, wherein the difference between the reference floor height of the associated floor and the initial floor height is less than a first threshold; if not, store the initial floor height as the reference floor height of the first floor; if yes, store the reference floor height of the associated floor as the reference floor height of the first floor. If the target vehicle is driving on the first level, the current level determined by the sensor data is corrected based on the reference level of the first level to obtain the target pseudo level of the first level. Select target map data that matches the target pseudo-layer height from the existing map data, where the difference between the calibration layer height of the target map data and the target pseudo-layer height is less than a second threshold. The target vehicle is located based on the target map data.

2. The method according to claim 1, characterized in that, The process of determining the position where the target vehicle has traveled to the boundary between the ramp and the first leveling level includes: Determine the pitch angle corresponding to the sensor data of the target vehicle at each moment; If the pitch angle corresponding to the sensor data at the first moment reaches the first angle threshold, then the search continues from the first moment to the second moment, where the pitch angle corresponding to the sensor data at the second moment reaches the second angle threshold; wherein, the second angle threshold is less than the first angle threshold. The location of the target vehicle at the second moment is determined as the boundary between the ramp and the first level.

3. The method according to claim 1, characterized in that, Determining the pitch angle corresponding to the sensor data of the target vehicle at each moment includes: If the sensor data includes longitudinal acceleration, then the pitch angle corresponding to the sensor data is determined based on the longitudinal acceleration and the wheel acceleration of the target vehicle; wherein, the pitch angle corresponding to the sensor data is determined using the following formula: Alternatively, if the sensor data includes lateral acceleration, longitudinal acceleration, and axial acceleration, then the pitch angle corresponding to the sensor data is determined based on the lateral acceleration, the longitudinal acceleration, the axial acceleration, and the wheel acceleration of the target vehicle; wherein the pitch angle corresponding to the sensor data is determined using the following formula: θ pitch The pitch angle is represented by a. lon a represents the longitudinal acceleration. vel The acceleration of the wheel is represented by g, and the acceleration due to gravity is represented by a. lat a represents the lateral acceleration. up This indicates the celestial acceleration.

4. The method according to claim 1, characterized in that, If the target vehicle is driving on the first level, the step of locating the target vehicle based on the target map data includes: When performing initial localization of the target vehicle, the surround view image data of the target vehicle is acquired, and the visual feature point features of the current position are determined based on the surround view image data; the target vehicle is initially localized based on the visual feature point features of the current position and the visual feature point features of multiple trajectory points in the target map data. Alternatively, when performing follow-up positioning on the target vehicle, the 360-degree view image data of the target vehicle is acquired, and the visual semantic features of the current position are determined based on the 360-degree view image data; based on the visual semantic features of the current position and the visual semantic features of multiple trajectory points in the target map data, the target vehicle is followed and positioned.

5. The method according to claim 1, characterized in that, If the target vehicle is traveling on a slope, then the first map data matching the slope is selected from the existing map data. The method further includes: When performing initial positioning or follow-up positioning of the target vehicle, the surround view image data of the target vehicle is acquired, and the visual feature point features of the current position are determined based on the surround view image data; based on the visual feature point features of the current position and the visual feature point features of multiple trajectory points in the first map data, the target vehicle is initially positioned or followed up.

6. The method according to any one of claims 1-5, characterized in that, Before selecting the target map data that matches the target pseudo-layer height from the existing map data, the method further includes: During the teaching vehicle's cross-level travel, if the teaching vehicle travels to the boundary between the ramp and the second level, the initial level height of the second level is determined based on the sensor data of the teaching vehicle. Based on the initial height of the second flat layer and the reference height of each stored flat layer, determine whether there is an associated flat layer of the second flat layer, wherein the difference between the reference height of the associated flat layer and the initial height is less than a first threshold; if not, store the initial height as the reference height of the second flat layer; if so, store the reference height of the associated flat layer as the reference height of the second flat layer. If the teaching vehicle is driving on the second level, the current level determined by the sensor data is corrected based on the reference level of the second level to obtain the calibrated level of the second level. Establish map data of the teaching vehicle on the second level, and associate the established map data of the second level with the calibration level based on the calibration level of the second level.

7. The method according to claim 6, characterized in that, If the teaching vehicle is driving on the second level, the step of establishing map data for the teaching vehicle on the second level includes: If the distance traveled by the teaching vehicle from the preset initial position is less than a preset distance threshold, then the surround view image data of the teaching vehicle is acquired, and the visual feature points of the current position are determined based on the surround view image data; based on the visual feature points of the current position, map data of the teaching vehicle on the second level is established, and this map data is used for initial positioning of the target vehicle; or, If the distance traveled by the teaching vehicle from the preset initial position is not less than a preset distance threshold, then the surround view image data of the teaching vehicle is acquired, and the visual semantic features of the current position are determined based on the surround view image data; based on the visual semantic features of the current position, map data of the teaching vehicle on the second level is established, and the map data is used for following and locating the target vehicle.

8. A vehicle positioning device, characterized in that, The device includes: The determination module is used to determine the initial height of the first level when the target vehicle travels across levels and reaches the boundary between the ramp and the first level, based on the sensor data of the target vehicle; based on the initial height and the reference height of each level that has been stored, determine whether there is an associated level of the first level, wherein the difference between the reference height of the associated level and the initial height is less than a first threshold; if not, store the initial height as the reference height of the first level; if so, store the reference height of the associated level as the reference height of the first level. The correction module is used to correct the current floor height determined by the sensor data based on the reference floor height of the first floor if the target vehicle is driving on the first floor, so as to obtain the target pseudo floor height of the first floor. The selection module is used to select target map data that matches the target pseudo-layer height from the existing map data, wherein the difference between the calibration layer height of the target map data and the target pseudo-layer height is less than a second threshold. The positioning module is used to locate the target vehicle based on the target map data.

9. A vehicle-mounted terminal device, characterized in that, include: A processor and a machine-readable storage medium, the machine-readable storage medium storing machine-executable instructions that can be executed by the processor; The processor is configured to execute machine-executable instructions to implement the method of any one of claims 1-7.

10. A vehicle, characterized in that, include: A camera is used to acquire panoramic image data and send the panoramic image data to a processor; An IMU sensor is used to acquire acceleration information and angular velocity information, and to send the acceleration information and angular velocity information to a processor; Wheel speed sensor is used to acquire wheel speed and send the wheel speed to processor; The processor is configured to implement the method described in any one of claims 1-7 based on the surround view image data, the acceleration information, the angular velocity information, and the wheel speed. Wherein, the vehicle includes a vehicle equipped with an intelligent driving system; or, The vehicles include those equipped with autonomous driving systems.

Images