Intelligent mobile platform trajectory optimization and automatic mapping method, system and storage medium

Abstract

The application relates to a smart mobile platform trajectory optimization and automatic mapping method, a system and a storage medium, wherein the smart mobile platform trajectory optimization method comprises the following steps: acquiring a first pose corresponding to a last frame radar timestamp, and combining measurement data collected by a sensor to obtain a second pose of the smart mobile platform corresponding to a current frame radar timestamp; the sensor is arranged on the smart mobile platform, and the smart mobile platform moves in a current environment; based on the measurement data and the second pose, a retroreflector column coordinate in a world coordinate system is extracted; after confirming that the trajectory of the smart mobile platform covers the current environment, the poses corresponding to all radar timestamps and the retroreflector column coordinates in the world coordinate system are jointly optimized based on a joint optimization model to obtain an optimized trajectory. Through the application, the trajectory error is reduced through a joint optimization method, so that the effect of reducing the map construction error is achieved.

Description

Technical Field

[0001] This application relates to the field of autonomous positioning and navigation for intelligent mobile platforms, and in particular to a method, system, and storage medium for trajectory optimization and automatic mapping of intelligent mobile platforms. Background Technology

[0002] Intelligent mobility platforms are vehicles equipped with numerous sensors that can navigate autonomously and perform different functions by integrating different task platforms. They have been widely used in warehousing and manufacturing industries. Currently, intelligent mobility platforms need to determine their coordinates based on the current environment map and several positioning markers within their activity range to achieve autonomous navigation. Therefore, comprehensive and accurate map construction is crucial for realizing intelligent mobility.

[0003] Because accumulated errors during large-scale map construction can lead to map incomplete closure, closure detection and optimization are typically performed first to achieve map closure. Existing technologies, such as the method proposed by Jing Yongbo et al. in "Robots, Map Generation Methods, Electronic Devices, and Storage Media," utilize additional visual sensors to detect pre-measured markers and optimize the trajectory by adding constraints to these markers to reduce loop closure distance. This results in a more accurate grid map, which is then used for positioning and navigation. However, relying solely on adding markers as constraints still presents the problem of large map construction errors.

[0004] There is currently no effective solution to the problem of large map construction errors caused by inaccurate trajectories in related technologies. Summary of the Invention

[0005] This embodiment provides an automatic trajectory optimization and automatic mapping method, system, and storage medium for an intelligent mobile platform to solve the problem of large map construction errors caused by inaccurate trajectories in related technologies.

[0006] Firstly, this embodiment provides a method for optimizing the trajectory of an intelligent mobile platform, including:

[0007] The first pose corresponding to the radar timestamp of the previous frame is obtained, and the second pose corresponding to the radar timestamp of the current frame is estimated by combining the measurement data collected by the sensor; the sensor is set on the intelligent mobile platform, and the intelligent mobile platform moves in the current environment;

[0008] Based on the measurement data and the second pose, extract the coordinates of the reflector in the world coordinate system;

[0009] After confirming that the trajectory of the intelligent mobile platform covers the current environment, a joint loop optimization is performed on the poses corresponding to all radar timestamps and the coordinates of the reflector pillars in the world coordinate system based on the joint optimization model to obtain the optimized trajectory.

[0010] In some embodiments, obtaining the first pose corresponding to the radar timestamp of the previous frame and estimating the second pose of the intelligent mobile platform corresponding to the radar timestamp of the current frame in combination with the measurement data collected by the sensors includes:

[0011] Obtain the first pose corresponding to the radar timestamp of the previous frame, and extract the corresponding first velocity information from the measurement data;

[0012] By performing linear interpolation on the measurement data corresponding to the times before and after the radar timestamp of the current frame, the second speed information of the intelligent mobile platform corresponding to the radar timestamp of the current frame is obtained.

[0013] The second pose is estimated by performing integral calculations based on the first pose, the first velocity information, and the second velocity information.

[0014] In some embodiments, the method further includes:

[0015] The second pose is optimized by using the pl-ICP point cloud matching method to obtain the second optimized pose corresponding to the radar timestamp of the current frame;

[0016] Verify whether the second optimized pose meets the preset error threshold. If it does not meet the preset error threshold, discard the data of the second optimized pose.

[0017] In some embodiments, extracting the reflector coordinates in the world coordinate system based on the measurement data and the second pose includes:

[0018] By filtering the radar data of two adjacent timestamps in the measurement data, the initial coordinates of the reflective column in the radar coordinate system are detected and extracted.

[0019] Based on the initial coordinates of the reflector and the second pose, the coordinates of the reflector in the world coordinate system are calculated.

[0020] In some embodiments, the method further includes:

[0021] If both the position and attitude in the first pose are zero, then the second pose corresponding to the radar timestamp of the current frame is taken as the origin of the world coordinate system.

[0022] In some of these embodiments, the joint optimization model is:

[0023]

[0024] in, 1≤i, j≤N, where N represents the number of trajectory points, and i and j represent the different radar timestamps; λ and θ represents the distance and angle between the reflective column and the intelligent mobile platform when the reflective column is first detected; i θ j With R i R j These represent the radar's attitude in the world coordinate system at the corresponding time; W i and W j θ represents the radar's position in the world coordinate system at the corresponding moment; ij W ij R ij This indicates a relative conversion relationship.

[0025] Secondly, this embodiment provides an automatic mapping method for an intelligent mobile platform, including:

[0026] Based on the trajectory of the intelligent mobile platform in the current environment obtained from the intelligent mobile platform trajectory optimization method described in the first aspect above, a point cloud map is automatically built.

[0027] Thirdly, this embodiment provides an intelligent mobile platform trajectory optimization device, which is applied to an intelligent mobile platform. The device includes: a pose acquisition module, a reflector coordinate acquisition module, and a joint loop closure optimization module.

[0028] The pose acquisition module is used to acquire the first pose corresponding to the radar timestamp of the previous frame, and to estimate the second pose of the intelligent mobile platform corresponding to the radar timestamp of the current frame by combining the measurement data collected by the sensor; the sensor is set on the intelligent mobile platform, and the intelligent mobile platform moves in the current environment;

[0029] The reflector coordinate acquisition module is used to extract the reflector coordinates in the world coordinate system based on the measurement data and the second pose.

[0030] The joint loop closure optimization module is used to perform joint loop closure optimization on the poses corresponding to all radar timestamps and the coordinates of the reflector pillars in the world coordinate system based on the joint optimization model after confirming that the trajectory of the intelligent mobile platform covers the current environment, so as to obtain the optimized trajectory.

[0031] Fourthly, this embodiment provides an automatic mapping system for an intelligent mobile platform. The system is applied to an intelligent mobile platform and includes: an intelligent mobile platform trajectory optimization device and an automatic mapping module.

[0032] The intelligent mobile platform trajectory optimization device is used to acquire the trajectory of the intelligent mobile platform under the current environment after joint loop optimization;

[0033] The automatic mapping module is used to automatically create a point cloud map based on the trajectory.

[0034] Fifthly, this embodiment provides a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, it implements the intelligent mobile platform trajectory optimization method described in the first aspect or the intelligent mobile platform automatic mapping method described in the second aspect.

[0035] In a sixth aspect, this embodiment provides a storage medium storing a computer program that, when executed by a processor, implements the intelligent mobile platform trajectory optimization method described in the first aspect or the intelligent mobile platform automatic mapping method described in the second aspect.

[0036] Compared with related technologies, the intelligent mobile platform trajectory optimization and automatic mapping method, system, and storage medium provided in this embodiment obtain the first pose corresponding to the radar timestamp of the previous frame and estimate the second pose corresponding to the radar timestamp of the current frame by combining the measurement data collected by the sensor; the sensor is set on the intelligent mobile platform, and the intelligent mobile platform moves in the current environment; based on the measurement data and the second pose, the reflector coordinates in the world coordinate system are extracted; after confirming that the trajectory of the intelligent mobile platform covers the current environment, joint loop closure optimization is performed on the poses corresponding to all radar timestamps and the reflector coordinates in the world coordinate system based on the joint optimization model to obtain the optimized trajectory. This solves the problem of large map construction errors caused by inaccurate trajectories and achieves the effect of reducing trajectory errors through joint optimization methods, thereby reducing map construction errors.

[0037] Details of one or more embodiments of this application are set forth in the following drawings and description to make other features, objects and advantages of this application more readily apparent. Attached Figure Description

[0038] The accompanying drawings, which are included to provide a further understanding of this application and form part of this application, illustrate exemplary embodiments and are used to explain this application, but do not constitute an undue limitation of this application. In the drawings:

[0039] Figure 1 This is a hardware structure block diagram of the terminal of the intelligent mobile platform trajectory optimization method in one embodiment;

[0040] Figure 2 This is a schematic diagram of the trajectory optimization process in existing technologies;

[0041] Figure 3 This is a flowchart of a trajectory optimization method for an intelligent mobile platform in one embodiment;

[0042] Figure 4 This is a flowchart of an automatic mapping method for an intelligent mobile platform in a preferred embodiment;

[0043] Figure 5 This is a comparative schematic diagram showing the trajectory of the intelligent mobile platform after joint loop closure optimization in a preferred embodiment.

[0044] Figure 6 This is a structural block diagram of an intelligent mobile platform trajectory optimization device in one embodiment;

[0045] Figure 7 This is a structural block diagram of an intelligent mobile platform automatic mapping system in one embodiment.

[0046] In the figure: 600, Intelligent mobile platform trajectory optimization device; 610, Pose acquisition module; 620, Reflector column coordinate acquisition module; 630, Joint loop closure optimization module; 700, Intelligent mobile platform automatic mapping system; 710, Automatic mapping module. Detailed Implementation

[0047] To better understand the purpose, technical solution, and advantages of this application, the application is described and illustrated below in conjunction with the accompanying drawings and embodiments.

[0048] Unless otherwise defined, the technical or scientific terms used in this application shall have the general meaning as understood by one of ordinary skill in the art to which this application pertains. Words such as “a,” “an,” “an,” “the,” “the,” and “these,” used in this application, do not indicate quantitative limitation and may be singular or plural. The terms “comprising,” “including,” “having,” and any variations thereof used in this application are intended to cover non-exclusive inclusion; for example, a process, method, system, product, or device that comprises a series of steps or modules (units) is not limited to the listed steps or modules (units) but may include steps or modules (units) not listed, or may include other steps or modules (units) inherent to such processes, methods, products, or devices. The terms “connected,” “linked,” and “coupled,” used in this application, are not limited to physical or mechanical connections but may include electrical connections, whether direct or indirect. The term “multiple” used in this application refers to two or more. The "and / or" operator describes the relationship between related objects, indicating that three relationships can exist. For example, "A and / or B" can represent three cases: A alone, A and B simultaneously, and B alone. Typically, the character " / " indicates that the objects before and after it are in an "or" relationship. The terms "first," "second," and "third," etc., used in this application are merely for distinguishing similar objects and do not represent a specific ordering of the objects.

[0049] The method embodiments provided in this example can be executed on a terminal, computer, or similar computing device. For example, it can run on a terminal. Figure 1 This is a hardware structure block diagram of the terminal for the intelligent mobile platform trajectory optimization method in this embodiment. For example... Figure 1 As shown, a terminal may include one or more ( Figure 1 Only one is shown in the diagram. A processor 102 and a memory 104 for storing data are also included. The processor 102 may be, but is not limited to, a microprocessor (MCU) or a programmable logic device (FPGA). The terminal may also include a transmission device 106 for communication functions and an input / output device 108. Those skilled in the art will understand that… Figure 1 The structure shown is for illustrative purposes only and does not limit the structure of the terminal described above. For example, the terminal may also include components that are larger than... Figure 1 The more or fewer components shown, or having the same Figure 1 The different configurations shown are illustrated.

[0050] The memory 104 can be used to store computer programs, such as application software programs and modules, like the computer program corresponding to the intelligent mobile platform trajectory optimization method in this embodiment. The processor 102 executes various functional applications and data processing by running the computer program stored in the memory 104, thereby implementing the above-described method. The memory 104 may include high-speed random access memory and may also include non-volatile memory, such as one or more magnetic storage devices, flash memory, or other non-volatile solid-state memory. In some instances, the memory 104 may further include memory remotely located relative to the processor 102, and these remote memories can be connected to the terminal via a network. Examples of such networks include, but are not limited to, the Internet, corporate intranets, local area networks, mobile communication networks, and combinations thereof.

[0051] The transmission device 106 is used to receive or send data via a network. This network includes a wireless network provided by the terminal's communication provider. In one example, the transmission device 106 includes a Network Interface Controller (NIC), which can connect to other network devices via a base station to communicate with the Internet. In another example, the transmission device 106 can be a Radio Frequency (RF) module used for wireless communication with the Internet.

[0052] In existing technologies, to eliminate accumulated errors during the loop traversal, additional visual sensors are used to detect pre-measured marker points. By adding corresponding constraints to these marker points, the trajectory is optimized to reduce the loop traversal distance. This results in a more accurate grid map, which is then used for positioning and navigation. Figure 2 This is a schematic diagram of the trajectory optimization process in existing technologies, such as... Figure 2 As shown, the left side is the trajectory before loop closure optimization, t1-t9 represent trajectory points. The trajectory point t9 deviates from the actual pose due to accumulated error. D301 and D302 are pre-measured marker points, which are used as constraints for trajectory optimization, resulting in the trajectory on the right after loop closure optimization.

[0053] This embodiment provides a method for optimizing the trajectory of an intelligent mobile platform. Figure 3 This is a flowchart of the method in this embodiment, as follows: Figure 3 As shown, the process includes the following steps:

[0054] Step S310: Obtain the first pose corresponding to the radar timestamp of the previous frame, and estimate the second pose of the intelligent mobile platform corresponding to the radar timestamp of the current frame by combining the measurement data collected by the sensor; the sensor is set on the intelligent mobile platform, and the intelligent mobile platform moves in the current environment.

[0055] Specifically, while moving in the current environment, the intelligent mobile platform detects and collects measurement data of the current environment and its own motion through sensors installed on it. These sensors include radar, inertial measurement units, and wheel speedometers, and the corresponding measurement data includes point cloud data, vehicle angular velocity, and vehicle linear velocity. Furthermore, to better optimize the trajectory, a certain number of reflective pillars are placed as markers in the current environment, without needing to measure the exact distance between the reflective pillar markers beforehand.

[0056] The vehicle speed information corresponding to the current frame timestamp is calculated based on the measurement data of the current frame radar timestamp. Then, based on the first pose corresponding to the previous frame radar timestamp and the measurement data, the second pose corresponding to the current frame radar timestamp is estimated. The pose includes the position and attitude of the vehicle body. In this embodiment, the position and attitude are represented by coordinates and angles in the world coordinate system, respectively.

[0057] Step S320: Based on the measurement data and the second pose, extract the coordinates of the reflector in the world coordinate system.

[0058] Specifically, based on the radar measurement data in the sensor, including the information of reflective pillars in the current environment detected by the radar during movement, the center coordinates of the reflective pillars in the radar coordinate system are obtained. Then, combined with the second pose, the center coordinates of the reflective pillars in the world coordinate system are transformed to obtain the center coordinates of the reflective pillars.

[0059] Step S330: After confirming that the trajectory of the intelligent mobile platform covers the current environment, perform joint loop closure optimization on the pose and reflector coordinates corresponding to all radar timestamps in the world coordinate system based on the joint optimization model to obtain the optimized trajectory.

[0060] Specifically, after confirming on-site that the trajectory of the intelligent mobile platform has covered the current environment, the pose corresponding to each radar timestamp can be estimated sequentially based on the pose of the previous radar timestamp, thereby obtaining the poses corresponding to all radar timestamps in the world coordinate system and the center coordinates of all reflectors. By performing joint loop closure optimization on all poses and reflector coordinates as a whole, the poses of all radar timestamps corresponding to the same reflector are subject to the same constraint, thus obtaining the optimized trajectory of the intelligent mobile platform.

[0061] The above steps differ from existing technologies that use additional sensors to detect and add pre-measured markers as corresponding constraints to optimize the trajectory by reducing the loop closure distance. In this embodiment, the poses of the intelligent mobile platform in the current environment are estimated step by step based on the sensor measurement data, and reflective pillars are added as markers. The coordinates of all reflective pillars in the world coordinate system are extracted. By treating all trajectories and reflective pillars in the current environment as a whole for joint loop closure optimization, the poses of all radar timestamps corresponding to the same detected reflective pillar are subject to the same constraint. Compared with existing technologies, this adds optimization constraints, making the optimized trajectory more accurate. Thus, a more accurate map can be built based on the optimized trajectory. Therefore, it can solve the problem of large map construction errors caused by inaccurate trajectories in existing technologies, and achieve the effect of reducing map construction errors by optimizing the trajectory.

[0062] Furthermore, in this embodiment, the detection of reflective pillars does not require additional sensors. When placing reflective pillars, there is no need to consider the size of the current mapping environment, nor is it necessary to measure the accurate distance between reflective pillar markers in advance. After mapping is completed, the reflective pillars do not need to be fixed in the current environment, which can reduce the workload of scene layout and measuring the specific distance between reflective pillars.

[0063] In some embodiments, obtaining the first pose corresponding to the radar timestamp of the previous frame and estimating the second pose of the intelligent mobile platform corresponding to the radar timestamp of the current frame in combination with the measurement data collected by the sensors includes the following steps:

[0064] Obtain the first pose corresponding to the radar timestamp of the previous frame, and extract the corresponding first velocity information from the measurement data.

[0065] Specifically, the vehicle's first velocity information corresponding to the previous radar timestamp is extracted from the measurement data, including the vehicle's angular velocity and linear velocity. Then, the first pose of the previous radar timestamp is obtained, specifically including the vehicle's position and attitude at the previous radar timestamp.

[0066] By performing linear interpolation on the measurement data corresponding to the times before and after the radar timestamp of the current frame, the second velocity information of the intelligent mobile platform corresponding to the radar timestamp of the current frame is obtained.

[0067] Specifically, the vehicle's angular velocity and linear velocity at the two sampling times before and after the current frame radar timestamp are obtained based on the sampling frequencies of the inertial measurement unit (IMU) and wheel speedometers, respectively. Since the IMU and wheel speedometers generally have different sampling frequencies, the timestamps at the two sampling times before and after the current frame radar timestamp can be obtained separately. By performing linear interpolation on the above measurement data, the second velocity information corresponding to the current frame radar timestamp is obtained, including the vehicle's linear velocity and angular velocity. For example, assuming the current frame radar timestamp is T... B The linear velocity and angular velocity of the vehicle body are ω. B and v B The timestamps before the current frame's radar timestamp are t1 and t3, corresponding to the vehicle's angular velocity ω1 and linear velocity v1, respectively. The timestamps after the current frame's radar timestamp are t2 and t4, corresponding to the vehicle's angular velocity ω2 and linear velocity v2, respectively. The linear interpolation operation is as follows:

[0068]

[0069]

[0070] The second pose is estimated by performing integral calculations based on the first pose, first velocity information, and second velocity information.

[0071] Specifically, assuming the previous radar timestamp is T A The corresponding first pose position is (x A ,y A ), with an attitude of θ A The first velocity information is ω A and v A Combining this with the second velocity information obtained from the linear interpolation in the previous step, the second pose position (x) is estimated. B ,y B ) and attitude θ B The integral is calculated as follows:

[0072]

[0073]

[0074] θ B =θ A +ωΔt;

[0075] Δt=T B -T A .

[0076] Since the actual sampling time interval is short, the movement of the intelligent mobile platform within a short period of time can be regarded as uniform. Therefore, the values ​​of v and w in the above calculation can be taken from the radar timestamp T of the previous frame. AOr the current frame radar timestamp T B Furthermore, the corresponding linear velocity and angular velocity can be obtained by taking the average of the linear velocity and angular velocity corresponding to the two timestamps.

[0077] Furthermore, the above method also includes the following steps:

[0078] The second pose is optimized by using the pl-ICP point cloud matching method to obtain the second optimized pose corresponding to the radar timestamp of the current frame;

[0079] Check whether the second optimized pose meets the preset error threshold. If it does not meet the preset error threshold, discard the data of the second optimized pose.

[0080] Specifically, using the pl-ICP point cloud matching method, the point cloud data of all previous radar frames are used as the matching target, and the point cloud of the current radar frame is used as the point cloud to be matched. Point-to-line point cloud matching is performed to obtain the second optimized pose corresponding to the radar timestamp of the current frame. The second optimized pose also needs to be verified to see if it meets the preset error threshold. The preset error threshold is the error value given by the pl-ICP function. Usually, the preset error threshold is set to 80. If it does not meet the requirement, the corresponding pose data of this radar frame is discarded.

[0081] In this embodiment, the pose corresponding to the radar timestamp of the current frame is estimated based on the pose and measurement data corresponding to the radar timestamp of the previous frame. Then, the pose corresponding to the radar timestamp of the current frame is further optimized by the pl-ICP point cloud matching method, which can obtain a more accurate pose corresponding to the radar timestamp of the current frame.

[0082] In some embodiments, the extraction of reflector coordinates in the world coordinate system based on measurement data and a second pose includes the following steps:

[0083] By filtering the radar data of two adjacent timestamps in the measurement data, the initial coordinates of the reflective column in the radar coordinate system are detected and extracted.

[0084] Based on the initial coordinates and second pose of the reflector, the coordinates of the reflector in the world coordinate system are calculated.

[0085] Specifically, the point cloud data acquired by the radar in two adjacent timestamps are filtered, and the presence of reflective pillars is detected. If a reflective pillar is detected, the initial coordinates (x, y) of the center of the reflective pillar in the radar coordinate system are extracted and calculated. Then, the second pose (x, y) in the world coordinate system corresponding to the current frame's radar timestamp is combined with this initial coordinates. B ,y B ,θ B The coordinates of the reflector are transformed to the world coordinate system to obtain the reflector coordinates (x′, y′). The specific process is as follows:

[0086] x′=xcosθ B -ysinθ B +x B ;

[0087] y′=xsinθ B +ycosθ B +y B .

[0088] After obtaining the coordinates of all reflectors in the world coordinate system, reflectors with similar coordinates within a certain range are considered as the same reflector, based on the approximate location range when the reflectors were set.

[0089] In this embodiment, the coordinates of the reflective column in the radar coordinate system are extracted from the radar data, and then further transformed to the coordinates of the reflective column in the world coordinate system, so that the trajectory of the intelligent mobile platform can be used as a whole for joint loop optimization in the world coordinate system.

[0090] In some embodiments, the method further includes:

[0091] If the position and attitude of the first pose are both zero, then the second pose corresponding to the radar timestamp of the current frame is taken as the origin of the world coordinate system.

[0092] Specifically, the pose corresponding to each radar timestamp is estimated from the position corresponding to the radar timestamp of the previous frame. When the position and attitude in the first pose of the radar timestamp of the previous frame are both zero, it means that there is no radar data in the previous frame and the intelligent mobile platform has not yet started moving. Therefore, the second pose corresponding to the radar timestamp of the current frame is taken as the origin of the world coordinate system, and the world coordinate system is established with this origin.

[0093] In some of these embodiments, the joint optimization model described above is as follows:

[0094]

[0095] in, 1≤i, j≤N, where N represents the number of trajectory points, and i and j represent the different radar timestamps; λ and θ represents the distance and angle between the reflective column and the intelligent mobile platform when the reflective column is first detected; i θ j With R i R j These represent the radar's attitude in the world coordinate system at the corresponding time; W i and W j θ represents the radar's position in the world coordinate system at the corresponding moment; ij W ij Rij This indicates a relative conversion relationship.

[0096] Specifically, assuming that in the current environment, the intelligent mobile platform ultimately retains the pose with N frames of timestamps, where i and j represent two different radar timestamps in the N poses, and the aforementioned θ i θ j With R i R j These are the numerical and matrix representations of the radar's attitude in the world coordinate system, respectively.

[0097] The joint optimization model in this embodiment uses the minimum error when the reflective column is detected for the first time as a benchmark. By making the distance and angle of the reflective column detected under different radar timestamps as similar as possible, the trajectory of the intelligent mobile platform is optimized.

[0098] This embodiment also provides an automatic mapping method for intelligent mobile platforms, which includes the following steps:

[0099] Based on any of the above embodiments, the intelligent mobile platform trajectory optimization method obtains the trajectory of the intelligent mobile platform moving in the current environment, and automatically builds a point cloud map based on the trajectory after joint loop closure optimization.

[0100] The above steps make the obtained optimized trajectory more accurate, thereby enabling the construction of a more accurate map based on the optimized trajectory. Therefore, it can solve the problem of large map construction errors caused by inaccurate trajectories in the existing technology, and achieve the effect of reducing map construction errors by optimizing the trajectory.

[0101] The present embodiment will now be described and illustrated through preferred embodiments.

[0102] Figure 4 This is a flowchart of the automatic mapping method for an intelligent mobile platform according to a preferred embodiment of the present invention, such as... Figure 4 As shown, the method includes the following steps:

[0103] Step S410: Obtain the first pose corresponding to the radar timestamp of the previous frame, and extract the corresponding first velocity information from the measurement data.

[0104] Step S420: By performing linear interpolation on the measurement data corresponding to the times before and after the radar timestamp of the current frame, the second speed information of the intelligent mobile platform corresponding to the radar timestamp of the current frame is obtained.

[0105] Step S430: Based on the first pose, the first velocity information and the second velocity information, perform integral calculation to estimate the initial pose corresponding to the radar timestamp of the current frame.

[0106] Step S440: Optimize the initial pose using the pl-ICP point cloud matching method, and retain the initial pose that meets the preset error threshold to obtain the second pose corresponding to the radar timestamp of the current frame.

[0107] Step S450: By filtering the radar data of two adjacent timestamps in the measurement data, the initial coordinates of the reflector in the radar coordinate system are detected and extracted; based on the initial coordinates and the second pose, the coordinates of the reflector in the world coordinate system are obtained.

[0108] Step S460: After confirming that the trajectory of the intelligent mobile platform covers the current environment, joint loop closure optimization is performed on the pose and reflector coordinates corresponding to all radar timestamps in the world coordinate system based on the joint optimization model to obtain the optimized trajectory.

[0109] Step S470: Build a point cloud map of the current environment based on the optimized trajectory.

[0110] Figure 5 This is a comparative schematic diagram showing the trajectory of the intelligent mobile platform in this preferred embodiment after joint loop closure optimization, as shown below. Figure 5 As shown, the left side is the point cloud map obtained by loop closure optimization in the prior art, and the right side is the point cloud map obtained by joint loop closure optimization in this preferred embodiment. Taking the upper left corner of the two point cloud maps as the origin, it can be seen that the difference between the point cloud map obtained by joint loop closure optimization on the right and the point cloud map obtained by ordinary loop closure optimization is that the point cloud map on the left has a greater difference as it is further away from the origin, while the point cloud map obtained by joint loop closure optimization on the right is more consistent with the real environment.

[0111] It should be noted that the steps shown in the above process or in the flowchart of the accompanying figures can be executed in a computer system such as a set of computer-executable instructions, and although a logical order is shown in the flowchart, in some cases the steps shown or described may be executed in a different order than that shown here.

[0112] This embodiment also provides an intelligent mobile platform trajectory optimization device, which is used to implement the above embodiments and preferred embodiments; details already described will not be repeated. The terms "module," "unit," "subunit," etc., used below refer to combinations of software and / or hardware that implement a predetermined function. Although the device described in the following embodiments is preferably implemented in software, hardware implementation, or a combination of software and hardware, is also possible and contemplated.

[0113] Figure 6 This is a structural block diagram of the intelligent mobile platform trajectory optimization device 600 of this embodiment. The intelligent mobile platform trajectory optimization device 600 is applied to an intelligent mobile platform, such as... Figure 6As shown, the device includes: a pose acquisition module 610, a reflector coordinate acquisition module 620, and a joint loop closure optimization module 630;

[0114] The pose acquisition module 610 is used to acquire the first pose corresponding to the radar timestamp of the previous frame, and to estimate the second pose of the intelligent mobile platform corresponding to the radar timestamp of the current frame by combining the measurement data collected by the sensor; the sensor is set on the intelligent mobile platform, and the intelligent mobile platform moves in the current environment.

[0115] The reflector coordinate acquisition module 620 is used to extract the reflector coordinates in the world coordinate system based on measurement data and the second pose.

[0116] The joint loop closure optimization module 630 is used to perform joint loop closure optimization on the pose and reflector coordinates corresponding to all radar timestamps in the world coordinate system based on the joint optimization model after confirming that the trajectory of the intelligent mobile platform covers the current environment, so as to obtain the optimized trajectory.

[0117] The device provided in this embodiment performs joint loop optimization on all trajectories and reflective pillars in the current environment as a whole, so that the poses of all radar timestamps corresponding to the same reflective pillar are subject to the same constraint. Compared with the prior art, the optimization constraint conditions are increased, making the optimized trajectory more accurate. Thus, a more accurate map can be built based on the optimized trajectory. Therefore, it can solve the problem of large map construction errors caused by inaccurate trajectories in the prior art, and achieve the effect of reducing map construction errors by optimizing the trajectory.

[0118] This embodiment also provides an automatic mapping system for intelligent mobile platforms. Figure 7 This is a structural block diagram of the Intelligent Mobile Platform Automatic Mapping System 700, such as... Figure 7 As shown, the system includes: an intelligent mobile platform trajectory optimization device 600 and an automatic mapping module 710;

[0119] The intelligent mobile platform trajectory optimization device 600 is used to acquire the trajectory of the intelligent mobile platform under the current environment after joint loop optimization.

[0120] The automatic mapping module 710 is used to automatically create point cloud maps based on trajectories.

[0121] The system provided in this embodiment can build a more accurate map based on the optimized trajectory, thus solving the problem of large map construction errors caused by inaccurate trajectories in the prior art, and achieving the effect of reducing map construction errors by optimizing the trajectory.

[0122] It should be noted that the above modules can be functional modules or program modules, and can be implemented through software or hardware. For modules implemented through hardware, the above modules can reside in the same processor; or the above modules can be located in different processors in any combination.

[0123] This embodiment also provides a computer device, including a memory and a processor, wherein the memory stores a computer program and the processor is configured to run the computer program to perform the steps in any of the above method embodiments.

[0124] Optionally, the computer device may further include a transmission device and an input / output device, wherein the transmission device is connected to the processor and the input / output device is connected to the processor.

[0125] It should be noted that the specific examples in this embodiment can refer to the examples described in the above embodiments and optional implementations, and will not be repeated in this embodiment.

[0126] Furthermore, in conjunction with the intelligent mobile platform trajectory optimization method or intelligent mobile platform automatic mapping method provided in the above embodiments, this embodiment can also provide a storage medium for implementation. The storage medium stores a computer program; when executed by a processor, the computer program implements any one of the intelligent mobile platform trajectory optimization methods or intelligent mobile platform automatic mapping methods in the above embodiments.

[0127] It should be understood that the specific embodiments described herein are merely illustrative of the application and not intended to limit it. All other embodiments derived by those skilled in the art based on the embodiments provided in this application without inventive effort are within the scope of protection of this application.

[0128] Obviously, the accompanying drawings are merely some examples or embodiments of this application. Those skilled in the art can apply this application to other similar situations based on these drawings without any creative effort. Furthermore, it is understood that although the work done in this development process may be complex and lengthy, for those skilled in the art, certain design, manufacturing, or production modifications made based on the technical content disclosed in this application are merely conventional technical means and should not be considered as insufficient disclosure of this application.

[0129] The term "embodiment" in this application refers to a specific feature, structure, or characteristic described in connection with an embodiment that may be included in at least one embodiment of this application. The appearance of this phrase in various places in the specification does not necessarily imply the same embodiment, nor does it imply that it is mutually exclusive with or independent of other embodiments. It will be clearly or implicitly understood by those skilled in the art that the embodiments described in this application may be combined with other embodiments without conflict.

[0130] The embodiments described above are merely illustrative of several implementation methods of this application, and while the descriptions are relatively specific and detailed, they should not be construed as limiting the scope of patent protection. It should be noted that those skilled in the art can make various modifications and improvements without departing from the concept of this application, and these all fall within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the appended claims.

Claims

1. A trajectory optimization method for an intelligent mobile platform, characterized in that, include: The first pose corresponding to the radar timestamp of the previous frame is obtained, and the second pose corresponding to the radar timestamp of the current frame is estimated by combining the measurement data collected by the sensor. The sensor is mounted on the intelligent mobile platform, which moves in the current environment; Based on the measurement data and the second pose, extract the coordinates of the reflector in the world coordinate system; After confirming that the trajectory of the intelligent mobile platform covers the current environment, a joint loop closure optimization is performed on the poses corresponding to all radar timestamps and the coordinates of the reflector pillars in the world coordinate system based on the joint optimization model to obtain the optimized trajectory; the joint optimization model is as follows: ； in, , , N represents the number of trajectory points, and i and j represent the different radar timestamps; and These represent the distance and angle between the reflective column and the intelligent mobile platform when the reflective column is first detected; , and , These represent the radar's attitude in the world coordinate system at the corresponding time. and This indicates the radar's position in the world coordinate system at the corresponding moment; This indicates a relative conversion relationship.

2. The intelligent mobile platform trajectory optimization method according to claim 1, characterized in that, The step of obtaining the first pose corresponding to the radar timestamp of the previous frame and estimating the second pose of the intelligent mobile platform corresponding to the radar timestamp of the current frame by combining the measurement data collected by the sensors includes: Obtain the first pose corresponding to the radar timestamp of the previous frame, and extract the corresponding first velocity information from the measurement data; By performing linear interpolation on the measurement data corresponding to the times before and after the radar timestamp of the current frame, the second speed information of the intelligent mobile platform corresponding to the radar timestamp of the current frame is obtained. The second pose is estimated by performing integral calculations based on the first pose, the first velocity information, and the second velocity information.

3. The intelligent mobile platform trajectory optimization method according to any one of claims 1 or 2, characterized in that, Also includes: The second pose is optimized by using the pl-ICP point cloud matching method to obtain the second optimized pose corresponding to the radar timestamp of the current frame; Verify whether the second optimized pose meets the preset error threshold. If it does not meet the preset error threshold, discard the data of the second optimized pose.

4. The intelligent mobile platform trajectory optimization method according to claim 1, characterized in that, The step of extracting the coordinates of the reflector in the world coordinate system based on the measurement data and the second pose includes: By filtering the radar data of two adjacent timestamps in the measurement data, the initial coordinates of the reflective column in the radar coordinate system are detected and extracted. Based on the initial coordinates of the reflector and the second pose, the coordinates of the reflector in the world coordinate system are calculated.

5. The intelligent mobile platform trajectory optimization method according to claim 1, characterized in that, Also includes: If both the position and attitude in the first pose are zero, then the second pose corresponding to the radar timestamp of the current frame is taken as the origin of the world coordinate system.

6. A method for automatic mapping on an intelligent mobile platform, characterized in that, include: Based on the trajectory of the intelligent mobile platform in the current environment obtained by the intelligent mobile platform trajectory optimization method according to any one of claims 1-5, a point cloud map is automatically built.

7. A trajectory optimization device for an intelligent mobile platform, characterized in that, The intelligent mobile platform trajectory optimization device is applied to an intelligent mobile platform. The device includes: a pose acquisition module, a reflector coordinate acquisition module, and a joint loop closure optimization module. The pose acquisition module is used to acquire the first pose corresponding to the radar timestamp of the previous frame, and to estimate the second pose of the intelligent mobile platform corresponding to the radar timestamp of the current frame by combining the measurement data collected by the sensor; the sensor is set on the intelligent mobile platform, and the intelligent mobile platform moves in the current environment; The reflector coordinate acquisition module is used to extract the reflector coordinates in the world coordinate system based on the measurement data and the second pose. The joint loop closure optimization module is used to perform joint loop closure optimization on the poses corresponding to all radar timestamps and the coordinates of the reflector pillars in the world coordinate system based on the joint optimization model after confirming that the trajectory of the intelligent mobile platform covers the current environment, to obtain the optimized trajectory; the joint optimization model is: ； in, , , N represents the number of trajectory points, and i and j represent the different radar timestamps; and These represent the distance and angle between the reflective column and the intelligent mobile platform when the reflective column is first detected; , and , These represent the radar's attitude in the world coordinate system at the corresponding time. and This indicates the radar's position in the world coordinate system at the corresponding moment; This indicates a relative conversion relationship.

8. An intelligent mobile platform automatic mapping system, characterized in that, The system is applied to an intelligent mobile platform, and the system includes: the intelligent mobile platform trajectory optimization device and the automatic mapping module as described in claim 7; The intelligent mobile platform trajectory optimization device is used to acquire the trajectory of the intelligent mobile platform under the current environment after joint loop optimization; The automatic mapping module is used to automatically create a point cloud map based on the trajectory.

9. A computer device, comprising a memory and a processor, characterized in that, The memory stores a computer program, and the processor is configured to run the computer program to execute the intelligent mobile platform trajectory optimization method according to any one of claims 1 to 5 or the intelligent mobile platform automatic mapping method according to claim 6.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the intelligent mobile platform trajectory optimization method according to any one of claims 1 to 5 or the intelligent mobile platform automatic mapping method according to claim 6.

Images