A mobile robot multi-radar cooperative mapping method, device, equipment and medium

Abstract

This invention discloses a method, apparatus, device, and medium for multi-radar collaborative mapping of a mobile robot. The method includes: constructing a robot coordinate system and constructing a radar coordinate system corresponding to each radar; and establishing a rotation matrix P based on the relative positional relationship between each radar coordinate system and the robot coordinate system. j Translation matrix T j Multiple lidar sensors simultaneously acquire environmental point cloud data, and the point cloud coordinates in the lidar coordinate system are transformed to the robot coordinate system. The transformed point cloud coordinates are then substituted into the designed loss function F(S). i Iteratively solve for the robot's optimal pose ξ at the current moment. * The invention generates motion paths based on the robot's pose at different times, and completes map construction as the robot moves. It can process point cloud data from multiple LiDAR sensors in real time and effectively, overcoming the problem of occlusion in single-LiDAR mapping and improving map accuracy.

Description

Technical Field

[0001] This invention relates to the field of mobile robot localization and mapping, and in particular to a method, apparatus, device and medium for multi-radar collaborative mapping of mobile robots. Background Technology

[0002] Simultaneous Localization and Mapping (SLAM) is a key technology for intelligent mobile robots to achieve autonomous localization and navigation. High-precision environmental maps can provide crucial and effective environmental information for the navigation of mobile robots, guiding them to move within a suitable space.

[0003] For some mobile robots with special structures, designers cannot install LiDAR in suitable locations. For example, when installing LiDAR on a gliding mobile robot, the radar can only be placed on the side of the robot, inevitably resulting in a large area of ​​the radar's field of view being obstructed by the robot's body. This leads to a significant reduction in localization and mapping accuracy. To solve this problem, multiple LiDARs can be installed around the mobile robot, compensating for the lack of radar field of view by increasing the number of radars. Currently, commonly used 2D LiDAR SLAM algorithms on mobile robot platforms typically only support point cloud data from a single LiDAR as input signals. Summary of the Invention

[0004] In order to at least partially solve one of the technical problems existing in the prior art, the present invention aims to provide a method, apparatus, device and medium for multi-radar collaborative mapping of mobile robots based on point cloud data fusion.

[0005] The first technical solution adopted in this invention is:

[0006] A multi-radar collaborative mapping method for mobile robots includes the following steps:

[0007] Construct the robot's coordinate system and the radar coordinate system corresponding to each radar; the robot is equipped with multiple lidars.

[0008] Based on the relative positional relationship between each radar coordinate system and the robot coordinate system, a rotation matrix P is established. j Translation matrix T j ;

[0009] Multiple lidar sensors simultaneously acquire environmental point cloud data and transform the point cloud coordinates from the lidar coordinate system to the robot coordinate system; for the point cloud coordinates S in lidar j at time i... ji According to the rotation matrix P j Translation matrix T j Transform to the robot coordinate system to obtain the point cloud coordinates S j ′i ;

[0010] Substitute the point cloud coordinates transformed to the robot coordinate system into the designed loss function F(S) i Iteratively solve for the robot's optimal pose ξ at the current moment. * ;

[0011] The motion path is generated based on the robot's pose at different times calculated, and the map is built as the robot moves.

[0012] Furthermore, the construction of the robot coordinate system includes:

[0013] Take any point on the chassis symmetry axis of the mobile robot as the origin O of the robot coordinate system. Following the right-hand rule, let the y-axis of the coordinate system be along the robot's forward direction and the z-axis be along the vertically upward direction, thus constructing the robot coordinate system (x...). o ,y o ,z o ), and the coordinate system (x o ,y o ,z o ( ) serves as the reference coordinate system for the mobile robot.

[0014] Furthermore, the construction of the radar coordinate system corresponding to each radar includes:

[0015] Obtain the geometric center of each radar as the origin O of the coordinate system. j For the j-th lidar, its origin is denoted as O. j Following the right-hand coordinate system rule, let the z-axis of the coordinate system be vertically upward, and let the x and y axes of the coordinate system lie in the same plane, thus constructing a radar coordinate system (x... j ,y j ,z j ).

[0016] Furthermore, based on the relative positional relationship between each radar coordinate system and the robot coordinate system, a rotation matrix P is established. j Translation matrix T j ,include:

[0017] Since each radar is fixed in place after being installed on the robot body, the origin of the radar coordinate system is O. j Translation matrix T between the robot coordinate system origin O and the robot coordinate system origin. j Fixed, therefore in Let O be the coordinates of the robot's coordinate system origin in the radar coordinate system;

[0018] Since each lidar is mounted at a different angle on the horizontal plane, it is necessary to determine the lidar coordinate system (x). j,y j ,z j ) and robot coordinate system (x o ,y o ,z o The rotation angle θ between ) j For each radar fixed to the robot's body, there exists a corresponding θ. j Then the rotation matrix

[0019] Furthermore, the transformation of point cloud coordinates from the radar coordinate system to the robot coordinate system includes:

[0020] Determine the radar coordinate system (x) j ,y j ,z j ) and robot coordinate system (x o ,y o ,z o Rotation matrix R between ) j Translation matrix T j Then, according to the relative transformation formula of coordinates in different coordinate systems, the point cloud coordinates S in the radar coordinate system can be determined. ji Point cloud coordinates S′ converted to robot coordinate system ji =R j S ji +T j .

[0021] Furthermore, the point cloud coordinates transformed to the robot coordinate system are substituted into the designed loss function F(S). i Iteratively solve for the robot's optimal pose ξ at the current moment. * ,include:

[0022] The current pose of the mobile robot is calculated based on point cloud data from multiple LiDAR sensors at the same time; the loss function is:

[0023]

[0024] In the formula, M(S) i (ξ) is the map occupancy probability index in the classic Hector-SLAM algorithm. The larger the value of this index, the higher the probability of ξ. * The closer to the robot's true pose; Δξ is the difference between the robot's pose at the current moment and the robot's pose at the previous moment; m is the number of LiDARs, and n is the LiDAR resolution;

[0025] To solve for the loss function F(S) i The optimal ξ when )→0 * First, M(S) jiPerform a first-order Taylor expansion at ξ, then use the Gauss-Newton method to obtain the optimal Δξ. Add the ξ from the previous time step to Δξ to obtain the ξ at the current time step. * .

[0026] Furthermore, the step of generating motion paths based on the calculated robot poses at different times, and completing map construction as the robot moves, includes:

[0027] As the robot moves, its current pose ξ is calculated in real time at a preset frequency. * ξ is calculated at each time step. * That is, the point cloud data at that moment is output as the obstacle edge to the map image, and the continuous ξ * This refers to the robot's movement trajectory. The continuously output point cloud data, when mapped onto an image, forms the constructed environmental map.

[0028] The second technical solution adopted in this invention is:

[0029] A mobile robot multi-radar collaborative mapping device, comprising:

[0030] The coordinate system construction module is used to construct the robot's coordinate system and the radar coordinate system corresponding to each radar; the robot is equipped with multiple radars.

[0031] The matrix calculation module is used to establish the rotation matrix P based on the relative positional relationship between each radar coordinate system and the robot coordinate system. j Translation matrix T j ;

[0032] The coordinate transformation module is used to collect environmental point cloud data from multiple lidars at the same time and transform the point cloud coordinates in the lidar coordinate system to the robot coordinate system.

[0033] The pose calculation module is used to substitute the point cloud coordinates transformed into the robot coordinate system into the designed loss function F(S). i Iteratively solve for the robot's optimal pose ξ at the current moment. * ;

[0034] The map building module is used to generate motion paths based on the robot's pose at different times calculated, and to complete map building as the robot moves.

[0035] The third technical solution adopted in this invention is:

[0036] An electronic device includes a processor and a memory, wherein the memory stores at least one instruction, at least one program, a code set, or an instruction set, and the at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by the processor to realize a multi-radar collaborative mapping method for a mobile robot as described above.

[0037] The fourth technical solution adopted in this invention is:

[0038] A computer-readable storage medium storing at least one instruction, at least one program, code set, or instruction set, wherein the at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by a processor to implement a multi-radar collaborative mapping method for a mobile robot as described above.

[0039] The fifth technical solution adopted in this invention is:

[0040] A computer program product or computer program includes computer instructions stored in a computer-readable storage medium. A processor of a computer device can read the computer instructions from the computer-readable storage medium and execute the computer instructions to cause the computer device to perform the method described above.

[0041] The present invention has the following advantages and beneficial effects:

[0042] 1) The present invention provides a multi-radar collaborative mapping method for mobile robots based on point cloud data fusion, which uses multi-radar point cloud data for mapping and overcomes the problem of inaccurate positioning caused by the obstruction of radar field of view when mapping with a single radar in some scenarios.

[0043] 2) This invention does not restrict the direction of radar installation, making it easy to apply to various mobile robot bodies with special mechanical structures.

[0044] 3) Compared with existing technologies, this invention is not complicated, does not require high hardware computing power, and is easy to implement on various mobile robot platforms. Attached Figure Description

[0045] To more clearly illustrate the technical solutions in the embodiments of the present invention or the prior art, the following description is provided with accompanying drawings of the relevant technical solutions in the embodiments of the present invention or the prior art. It should be understood that the accompanying drawings described below are only for the purpose of clearly illustrating some embodiments of the technical solutions of the present invention. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0046] Figure 1This is an overall flowchart of a multi-radar collaborative mapping method for mobile robots in an embodiment of the present invention;

[0047] Figure 2 This is a hardware structure diagram of the mobile robot platform in an embodiment of the present invention;

[0048] Figure 3 This is a schematic diagram of the radar coordinate system and robot coordinate system in an embodiment of the present invention;

[0049] Figure 4 This is the pose transformation matrix between the radar coordinate system and the robot coordinate system in this embodiment of the invention;

[0050] Figure 5 This is a schematic diagram of the field of view of lidar 1 in an embodiment of the present invention;

[0051] Figure 6 This is a schematic diagram of the field of view of the lidar 2 in an embodiment of the present invention;

[0052] Figure 7 This is a schematic diagram of the equivalent radar field of view generated in an embodiment of the present invention;

[0053] Figure 8 This is a schematic diagram of the mobile robot trajectory and mapping effect in an embodiment of the present invention;

[0054] Figure 9 This is a schematic diagram of a map constructed when the radar field of view is obstructed in an embodiment of the present invention;

[0055] Figure 10 This is a schematic diagram of the map constructed by the multi-radar collaborative mapping method for mobile robots in an embodiment of the present invention;

[0056] Figure 11 This is a flowchart illustrating the steps of a multi-radar collaborative mapping method for mobile robots in an embodiment of the present invention. Detailed Implementation

[0057] The embodiments of the present invention are described in detail below. Examples of these embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and should not be construed as limiting the present invention. The step numbers in the following embodiments are set only for ease of explanation, and there is no limitation on the order between the steps. The execution order of each step in the embodiments can be adaptively adjusted according to the understanding of those skilled in the art.

[0058] In the description of this invention, it should be understood that the orientation descriptions, such as up, down, front, back, left, right, etc., are based on the orientation or positional relationship shown in the accompanying drawings. They are only for the convenience of describing this invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limiting this invention.

[0059] In the description of this invention, "several" means one or more, "more than" means two or more, "greater than," "less than," and "exceeding" are understood to exclude the stated number, while "above," "below," and "within" are understood to include the stated number. The use of "first" and "second" in the description is merely for distinguishing technical features and should not be construed as indicating or implying relative importance, or implicitly indicating the number of indicated technical features, or implicitly indicating the order of the indicated technical features.

[0060] In the description of this invention, unless otherwise explicitly defined, terms such as "set up," "install," and "connect" should be interpreted broadly, and those skilled in the art can reasonably determine the specific meaning of the above terms in this invention in conjunction with the specific content of the technical solution.

[0061] To address the existing technical problems, this invention proposes a multi-radar collaborative mapping scheme for mobile robots based on point cloud data fusion. This scheme can simultaneously utilize point cloud data from multiple radars with arbitrary installation angles to achieve efficient and high-precision mobile robot positioning and mapping.

[0062] Example 1

[0063] like Figure 11 As shown, this embodiment provides a multi-radar collaborative mapping method for mobile robots. By fusing point cloud data from multiple radars for real-time mapping, it overcomes the problem of radar field of view obstruction during mapping using traditional single-radar mapping methods, thereby improving mapping accuracy. The method specifically includes the following steps:

[0064] S1. Construct the robot coordinate system and the radar coordinate system corresponding to each radar; the robot is equipped with multiple lidars.

[0065] As an optional implementation, multiple lidars are installed and fixed one by one on the body of the mobile robot, ensuring that the lidars are installed horizontally on the same plane, without any restrictions on the installation orientation of the lidars.

[0066] For a wheeled robot with a symmetrical chassis, any point on the chassis's axis of symmetry can be chosen as the origin O of the robot's coordinate system. Using a right-handed coordinate system, the y-axis is defined as the front of the robot, and the z-axis is vertically upward. This coordinate system (x...) o ,y o ,z oThe coordinate system is used as the reference coordinate system for the mobile robot. As an optional implementation, the geometric center of the robot's body is chosen as the origin O of the coordinate system. Following the right-hand rule, the y-axis is aligned with the robot's forward direction, and the z-axis is aligned vertically upwards. The resulting coordinate system (x...) is then used as the reference coordinate system. o ,y o ,z o It remains fixed thereafter.

[0067] To establish a radar coordinate system, the geometric center of each radar can be selected as the origin O of the coordinate system. j Following the right-hand coordinate system rule, let the z-axis of the coordinate system be vertically upward, and let the x and y axes of the coordinate system lie in the same plane. The established coordinate system (x...) j ,y j ,z j This remains fixed thereafter. For example, a conventional two-dimensional lidar has a cylindrical structure, with the geometric center of the cylinder taken as the origin of the lidar coordinate system. For a lidar numbered j, its coordinate origin is denoted as O. j The method in this embodiment requires that the z-axis of the lidar coordinate system be vertically upward and follow the rules of the right-hand coordinate system. It does not impose requirements on the x and y axes of the lidar coordinate system. That is, the lidar can be rotated any angle around the z-axis and then horizontally mounted on the mobile robot body, resulting in the coordinate system (x, y) corresponding to the lidar numbered j. j ,y j ,z j ).

[0068] S2. Based on the relative positional relationship between each radar coordinate system and the robot coordinate system, establish the rotation matrix P. j Translation matrix T j .

[0069] Specifically, since each radar is fixed in place after being mounted on the robot body, the origin O of the radar coordinate system... j Translation matrix T between the robot coordinate system origin O and the robot coordinate system origin. j fixed, in Let O be the coordinates of the robot's coordinate system origin in the radar coordinate system. Since each lidar is mounted at a different angle on the horizontal plane, it is necessary to determine the radar coordinate system (x...). j ,y j ,z j ) and robot coordinate system (x o ,y o ,z o The rotation angle θ between ) j For each radar fixed to the robot's body, there exists a corresponding θ. j Then the rotation matrix

[0070] S3. Multiple lidars simultaneously acquire environmental point cloud data and transform the point cloud coordinates from the lidar coordinate system to the robot coordinate system; for the point cloud coordinates Si in lidar j at time i, ... ji According to the rotation matrix P j Translation matrix T j Transform to the robot coordinate system to obtain the point cloud coordinates S j ′ i .

[0071] In some embodiments, the radar coordinate system (x) is determined. j ,y j ,z j ) and robot coordinate system (x o ,y o ,z o Rotation matrix R between ) j Translation matrix T j Then, according to the relative transformation formula of coordinates in different coordinate systems, it can be known that a point S in the radar coordinate system... ji Coordinates S′ transformed to the robot coordinate system ji =R j S ji +T j .

[0072] S4. Substitute the point cloud coordinates transformed into the robot coordinate system into the designed loss function F(S) i Iteratively solve for the robot's optimal pose ξ at the current moment. * .

[0073] At the start of mapping, each LiDAR collects point cloud data at the same frequency, and the point cloud data at the same time are collected. As input to the algorithm. Since the algorithm used in this embodiment needs to process point cloud data input from multiple LiDARs at the same time, the loss function used in conventional single-radar mapping cannot be applied. This embodiment of the invention proposes to use a loss function. Where M(S) i (ξ) is the map occupancy probability index in the classic Hector-SLAM algorithm. The larger the value of this index, the higher the probability of ξ. * The closer the value is to the robot's true pose, the better. The loss function designed in this embodiment can handle point cloud data points numbered 1 to n from radars numbered 1 to m. Specifically, to solve for the loss function F(S) that makes the value more accurate, the loss function is designed to be more accurate. i The optimal ξ when )→0 * M(S) needs to be set first. jiPerform a first-order Taylor expansion of (ξ+Δξ) at ξ, then use the Gauss-Newton method to obtain the optimal Δξ. Add the ξ from the previous time step to Δξ to obtain the ξ at the current time step. * Where Δξ is the difference between the robot's pose at the current moment and the robot's pose at the previous moment.

[0074] It should be noted that the robot pose ξ0 at time 0 needs to be manually given; in this embodiment, ξ0 = 0.

[0075] S5. Generate motion paths based on the robot poses at different times calculated, and complete map construction as the robot moves.

[0076] As the robot moves, the algorithm calculates the robot's current pose ξ in real time at a certain frequency. * ξ is calculated at each time step. * That is, the point cloud data at that moment is output as the obstacle edge to the map image, and the continuous ξ * This refers to the robot's movement trajectory. The continuously output point cloud data, when mapped onto an image, forms the constructed environment map.

[0077] The above method will be explained in detail below with reference to the accompanying drawings and specific embodiments.

[0078] like Figure 1 As shown, this embodiment provides a multi-radar collaborative mapping method for mobile robots based on point cloud data fusion. Specific implementation methods include:

[0079] S101. Determine the robot coordinate system: The purpose of determining the robot coordinate system is to treat the moving robot as a point mass during mapping to determine its unique pose at the current moment. In principle, any point on the robot's body can be taken as the origin of the robot coordinate system, and the robot coordinate system can be established according to any rules. To simplify the method, this embodiment fixes the origin O of the robot coordinate system at the geometric center of the robot chassis, and establishes a robot coordinate system (x, y) with the robot's forward direction as the y-axis and the vertical upward direction as the z-axis, according to the rules of the right-hand coordinate system. o ,y o ,z o );

[0080] S201-S20M, Determining the Coordinate System of LiDAR m: Since multiple LiDARs are installed on the mobile robot, and each LiDAR has a different position and installation angle, it is necessary to determine the coordinate system of each LiDAR. In principle, any point on the LiDAR body can be taken as the origin of the radar coordinate system, and the radar coordinate system can be established according to any rule. To simplify the method, this embodiment sets the origin O of the coordinate system of radar j to... jFixed at the radar's geometric center, a radar coordinate system is established according to the rules of the right-hand coordinate system, with the vertically upward direction as the z-axis, and the x and y axes taking arbitrary directions (x... j ,y j ,z j Here, the x-axis and y-axis can take any direction, which means that the radar coordinate system and the robot coordinate system can rotate around the z-axis;

[0081] S301-S20M, Determine the coordinate transformation relationship from the LiDAR m coordinate system to the robot coordinate system: For any one of the m LiDARs j, there is a unique and fixed coordinate transformation relationship {R} between its coordinate system and the robot coordinate system. j ,T j}, R j T represents the rotation between the radar j-coordinate system and the robot coordinate system. j Let {R} represent the translation between the radar j coordinate system and the robot coordinate system. Since radar j is fixed, {R} j ,T j This can be obtained through measurement;

[0082] S401-S40M LiDAR data acquisition, S501-S50M LiDAR point cloud coordinate transformation: The point cloud data acquired by each LiDAR is based on its own coordinate system. To use point cloud data from multiple LiDARs for mapping simultaneously, the point clouds from different LiDAR coordinate systems need to be transformed to the same coordinate system. This embodiment chooses to transform all LiDAR point clouds to the robot coordinate system simultaneously, based on the previously determined {R... j ,T j For radar j, the point cloud P in the radar j coordinate system is... L ={S ji Transform to robot coordinate system to obtain

[0083] S601, S701, Point Cloud Data Input Algorithm: An optimization algorithm is used to calculate the optimal value of the robot's pose change. Since the point cloud data to be processed in this embodiment comes from multiple LiDARs with different orientations, commonly available LiDAR mapping algorithms cannot handle this type of input. Therefore, this embodiment proposes using the following loss function... This loss function considers all point cloud data points from lidar sensors numbered 1 to m, from the 1st to the nth. This embodiment uses the Gauss-Newton method to solve for F(S). i The optimal pose change of the robot is Δξ, which is 0.

[0084] S801, Iteratively update the robot pose: Update the robot pose according to the robot pose. make Solve and update robot pose in real time;

[0085] S901. Robot Motion Completes Mapping: This step is similar to the traditional mobile robot mapping method. As the robot moves under human control, the algorithm outputs the robot's current pose in real time. Based on the output pose and the current point cloud data set, the point cloud data is mapped onto the real-time updated environmental map to complete the mapping.

[0086] Experimental Test

[0087] This embodiment is a real-world experiment. A wheeled robot equipped with two WLR-719LIDAR single-line lidars with a scanning frequency of 20Hz was selected as the experimental platform. The two lidars were mounted on opposite sides of the robot's body, ensuring that their respective fields of view were largely obstructed. A Raspberry Pi 4B running Ubuntu 18.04 was selected as the main control board for program execution. The experimental scenario was a laboratory in a school.

[0088] The hardware platform structure diagram of this embodiment is shown below. Figure 2 As shown.

[0089] Establish the coordinate system of LiDAR 1, LiDAR 2, and the robot, such as... Figure 3 As shown. The coordinate system depends on the installation position and orientation of the corresponding lidar, and once installed, the coordinate system remains fixed.

[0090] By measuring the positional relationship between the origin of the lidar coordinate system and the origin of the robot coordinate system, as well as the rotation angles between the coordinate systems around the z-axis, the transformation relationships between the lidar 1 coordinate system, the lidar 2 coordinate system, and the robot coordinate system are determined as follows: Figure 4 As shown.

[0091] LiDAR 1 and LiDAR 2 are affected by field-of-view obstruction. Specifically, the right field of view of LiDAR 1 and the left field of view of LiDAR 2 are obstructed by the mobile robot's body, such as... Figure 5 and Figure 6 As shown.

[0092] Using the point cloud data from both LiDAR 1 and LiDAR 2 as input, the equivalent radar field of view is obtained by the algorithm in this embodiment, as shown below. Figure 7 As shown.

[0093] Manually control the robot to move in the indoor environment, and output the robot's position trajectory and mapping effect in real time, as shown below. Figure 8 As shown.

[0094] A complete map constructed using traditional methods when the LiDAR field of view is obstructed, such as... Figure 9 As shown.

[0095] The complete environment map constructed using this embodiment is as follows: Figure 10 As shown.

[0096] In summary, this invention proposes a multi-radar collaborative mapping method for mobile robots based on point cloud data fusion. It utilizes multiple single-line LiDARs as data acquisition sensors, in conjunction with IMU and other sensor devices, connected to a SOC main control board and mounted on a mobile robot. This method can effectively process point cloud data acquired from multiple LiDARs in real time, overcoming the problem of obstructed field of view in single-LiDAR mapping, simplifying the robot's movement path during mapping, and improving map accuracy. This invention is suitable for indoor mobile robot platforms with limited hardware resources.

[0097] Example 2

[0098] This embodiment provides a multi-radar collaborative mapping device for mobile robots, including:

[0099] The coordinate system construction module is used to construct the robot's coordinate system and the radar coordinate system corresponding to each radar; the robot is equipped with multiple radars.

[0100] The matrix calculation module is used to establish the rotation matrix P based on the relative positional relationship between each radar coordinate system and the robot coordinate system. j Translation matrix T j ;

[0101] The coordinate transformation module is used to collect environmental point cloud data from multiple lidars at the same time and transform the point cloud coordinates in the lidar coordinate system to the robot coordinate system.

[0102] The pose calculation module is used to substitute the point cloud coordinates transformed into the robot coordinate system into the designed loss function F(S). i Iteratively solve for the robot's optimal pose ξ at the current moment. * ;

[0103] The map building module is used to generate motion paths based on the robot's pose at different times calculated, and to complete map building as the robot moves.

[0104] Since this device is a mobile robot multi-radar collaborative mapping device according to an embodiment of the present invention, and the principle of solving the problem by this device is similar to that of the method, the implementation of this device can refer to the implementation process of the above method embodiment, and the repeated parts will not be described again.

[0105] Example 3

[0106] This invention also provides an electronic device, which includes a processor and a memory. The memory stores at least one instruction, at least one program, a code set, or an instruction set. The at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by the processor to achieve the following: Figure 11 This paper presents a multi-radar collaborative mapping method for mobile robots.

[0107] It is understood that the memory may include random access memory (RAM) or read-only memory. Optionally, the memory may include non-transitory computer-readable storage medium. The memory can be used to store instructions, programs, code, code sets, or instruction sets. The memory may include a stored program area and a stored data area, wherein the stored program area may store instructions for implementing an operating system, instructions for at least one function, instructions for implementing the various method embodiments described above, etc.; the stored data area may store data created according to the use of the server, etc.

[0108] A processor may include one or more processing cores. The processor connects to various parts of the server via various interfaces and lines, executing instructions, programs, code sets, or instruction sets stored in memory, and accessing data stored in memory to perform various server functions and process data. Optionally, the processor may be implemented using at least one of the following hardware forms: Digital Signal Processing (DSP), Field-Programmable Gate Array (FPGA), and Programmable Logic Array (PLA). The processor may integrate one or more of the following: Central Processing Unit (CPU) and Modem. The CPU primarily handles the operating system and applications; the modem handles wireless communication. It is understood that the modem may also be implemented as a separate chip without being integrated into the processor.

[0109] Since this electronic device is the electronic device corresponding to the multi-radar collaborative mapping method for mobile robots in this embodiment of the invention, and the principle of solving the problem by this electronic device is similar to that of this method, the implementation of this electronic device can refer to the implementation process of the above method embodiment, and the repeated parts will not be described again.

[0110] Example 4

[0111] This invention also provides a computer-readable storage medium storing at least one instruction, at least one program, a code set, or an instruction set, wherein the at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by a processor to achieve the following: Figure 11 This paper presents a multi-radar collaborative mapping method for mobile robots.

[0112] Those skilled in the art will understand that all or part of the steps in the various methods of the above embodiments can be implemented by a program instructing related hardware. The program can be stored in a computer-readable storage medium, including read-only memory (ROM), random access memory (RAM), programmable read-only memory (PROM), erasable programmable read-only memory (EPROM), one-time programmable read-only memory (OTPROM), electrically-Erasable Programmable Read-Only Memory (EEPROM), compact disc read-only memory (CD-ROM) or other optical disc storage, disk storage, magnetic tape storage, or any other computer-readable medium capable of carrying or storing data.

[0113] Since this storage medium is the storage medium corresponding to a mobile robot multi-radar collaborative mapping method in an embodiment of the present invention, and the principle of the storage medium in solving the problem is similar to that of the method, the implementation of this storage medium can refer to the implementation process of the above method embodiment, and the repeated parts will not be described again.

[0114] Example 5

[0115] In some possible implementations, various aspects of the methods of the embodiments of the present invention can also be implemented as a program product comprising program code that, when run on a computer device, causes the computer device to perform the steps of a mobile robot multi-radar cooperative mapping method according to various exemplary embodiments of this application as described above. The executable computer program code or "code" for performing the various embodiments can be written in high-level programming languages ​​such as C, C++, C#, Smalltalk, Java, JavaScript, Visual Basic, Structured Query Language (e.g., Transact-SQL), Perl, or in various other programming languages.

[0116] It should be understood that various parts of the present invention can be implemented in hardware, software, firmware, or a combination thereof. In the above embodiments, multiple steps or methods can be implemented in software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware, as in another embodiment, it can be implemented using any one or a combination of the following techniques known in the art: discrete logic circuits having logic gates for implementing logical functions on data signals, application-specific integrated circuits (ASICs) having suitable combinational logic gates, programmable gate arrays (PGAs), field-programmable gate arrays (FPGAs), etc.

[0117] In the description of this specification, the references to terms such as "one embodiment," "some embodiments," "example," "specific example," or "some examples," etc., indicate that a specific feature, structure, material, or characteristic described in connection with that embodiment or example is included in at least one embodiment or example of the present invention. In this specification, the illustrative expressions of the above terms do not necessarily refer to the same embodiment or example. Furthermore, the specific features, structures, materials, or characteristics described may be combined in any suitable manner in one or more embodiments or examples. Moreover, without contradiction, those skilled in the art can combine and integrate the different embodiments or examples described in this specification, as well as the features of different embodiments or examples.

[0118] The above embodiments are merely illustrative of the technical concept and features of the present invention, and are intended to enable those skilled in the art to understand the content of the present invention and implement it accordingly. They should not be construed as limiting the scope of protection of the present invention. All equivalent changes or modifications made based on the essence of the content of the present invention should be covered within the scope of protection of the present invention.

Claims

1. A multi-radar collaborative mapping method for mobile robots, characterized in that, Includes the following steps: Construct the robot's coordinate system and the radar coordinate system corresponding to each radar; the robot is equipped with multiple radars. Based on the relative positional relationship between each radar coordinate system and the robot coordinate system, a rotation matrix P is established. j Translation matrix T j ; Multiple lidars simultaneously collect environmental point cloud data and transform the point cloud coordinates from the lidar coordinate system to the robot coordinate system. Substitute the point cloud coordinates transformed to the robot coordinate system into the designed loss function F(S) i Iteratively solve for the robot's optimal pose ξ at the current moment. * ; The motion path is generated based on the robot's pose at different times calculated, and the map is built as the robot moves. The point cloud coordinates transformed to the robot coordinate system are then substituted into the designed loss function F(S). i Iteratively solve for the robot's optimal pose ξ at the current moment. * ,include: The current pose of the mobile robot is calculated based on point cloud data from multiple radars at the same time. The loss function is: In the formula, M(S) i (ξ) is the map occupancy probability index in the classic Hector-SLAM algorithm. The larger the index, the higher the probability of ξ. * The closer to the robot's true pose; Δξ is the difference between the robot's pose at the current moment and the robot's pose at the previous moment; m is the number of radars, and n is the radar resolution; To solve for the loss function F(S) i The optimal ξ when )→0 * First, M(S) ji Perform a first-order Taylor expansion at ξ, then use the Gauss-Newton method to obtain the optimal Δξ. Add the ξ from the previous time step to Δξ to obtain the ξ at the current time step. * .

2. The multi-radar collaborative mapping method for mobile robots according to claim 1, characterized in that, The construction of the robot coordinate system includes: Take any point on the chassis symmetry axis of the mobile robot as the origin O of the robot coordinate system. Following the right-hand rule, let the y-axis of the coordinate system be along the robot's forward direction and the z-axis be along the vertically upward direction, thus constructing the robot coordinate system (x...). o ,y o ,z o ), and the coordinate system (x o ,y o ,z o ( ) serves as the reference coordinate system for the mobile robot.

3. The multi-radar collaborative mapping method for mobile robots according to claim 1, characterized in that, The construction of the radar coordinate system corresponding to each radar includes: Obtain the geometric center of each radar as the origin O of the coordinate system. j For the j-th lidar, its origin is denoted as O. j Following the right-hand coordinate system rule, let the z-axis of the coordinate system be vertically upward, and let the x and y axes of the coordinate system lie in the same plane, thus constructing a radar coordinate system (x... j ,y j ,z j ).

4. The multi-radar collaborative mapping method for mobile robots according to claim 1, characterized in that, The rotation matrix P is established based on the relative positional relationship between each radar coordinate system and the robot coordinate system. j Translation matrix T j ,include: Since each radar is fixed in place after being installed on the robot body, the origin of the radar coordinate system is O. j Translation matrix T between the robot coordinate system origin O and the robot coordinate system origin. j Fixed, therefore in Let O be the coordinates of the robot's coordinate system origin in the radar coordinate system; Since each radar is mounted at a different angle on the horizontal plane, it is necessary to determine the radar coordinate system (x). j ,y j ,z j ) and robot coordinate system (x o ,y o ,z o The rotation angle θ between ) j For each radar fixed to the robot's body, there exists a corresponding θ. j Then the rotation matrix 5. The multi-radar collaborative mapping method for mobile robots according to claim 1, characterized in that, The process of transforming point cloud coordinates from the radar coordinate system to the robot coordinate system includes: Determine the radar coordinate system (x) j ,y j ,z j ) and robot coordinate system (x o ,y o ,z o Rotation matrix R between ) j Translation matrix T j Then, according to the relative transformation formula of coordinates in different coordinate systems, the point cloud coordinates S in the radar coordinate system can be determined. ji Point cloud coordinates S′ converted to robot coordinate system ji =R j S ji +T j .

6. The multi-radar collaborative mapping method for mobile robots according to claim 1, characterized in that, The process of generating motion paths based on the calculated robot poses at different times and constructing maps as the robot moves includes: As the robot moves, its current pose ξ is calculated in real time at a preset frequency. * ξ is calculated at each time step. * That is, the point cloud data at that moment is output as the obstacle edge to the map image, and the continuous ξ * This refers to the robot's movement trajectory. The continuously output point cloud data, when mapped onto an image, forms the constructed environment map.

7. A mobile robot multi-radar collaborative mapping device, characterized in that, include: The coordinate system construction module is used to construct the robot's coordinate system and the radar coordinate system corresponding to each radar; the robot is equipped with multiple radars. The matrix calculation module is used to establish the rotation matrix P based on the relative positional relationship between each radar coordinate system and the robot coordinate system. j Translation matrix T j ; The coordinate transformation module is used to collect environmental point cloud data from multiple lidars at the same time and transform the point cloud coordinates in the lidar coordinate system to the robot coordinate system. The pose calculation module is used to substitute the point cloud coordinates transformed into the robot coordinate system into the designed loss function F(S). i Iteratively solve for the robot's optimal pose ξ at the current moment. * ; The map building module is used to generate motion paths based on the robot's pose at different times calculated, and to complete map building as the robot moves. The point cloud coordinates transformed to the robot coordinate system are then substituted into the designed loss function F(S). i Iteratively solve for the robot's optimal pose ξ at the current moment. * ,include: The current pose of the mobile robot is calculated based on point cloud data from multiple radars at the same time. The loss function is: In the formula, M(S) i (ξ) is the map occupancy probability index in the classic Hector-SLAM algorithm. The larger the index, the higher the probability of ξ. * The closer to the robot's true pose; Δξ is the difference between the robot's pose at the current moment and the robot's pose at the previous moment; m is the number of radars, and n is the radar resolution; To solve for the loss function F(S) i The optimal ξ when )→0 * First, M(S) ji Perform a first-order Taylor expansion of (ξ+Δξ) at ξ, then use the Gauss-Newton method to obtain the optimal Δξ. Add the ξ from the previous time step to Δξ to obtain the ξ at the current time step. * .

8. An electronic device, characterized in that, The electronic device includes a processor and a memory, wherein the memory stores at least one instruction, at least one program, a code set, or an instruction set, and the at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by the processor to implement the method as described in any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that, The storage medium stores at least one instruction, at least one program, code set, or instruction set, wherein the at least one instruction, the at least one program, the code set, or the instruction set is loaded and executed by a processor to implement the method as described in any one of claims 1 to 6.

Images