Mobile robot multi-radar collaborative mapping method, device and equipment and medium

Abstract

The invention discloses a mobile robot multi-radar cooperative mapping method, device and equipment and a medium, and the method comprises the steps: constructing a robot coordinate system, and constructing a radar coordinate system corresponding to each radar; establishing a rotation matrix Pj and a translation matrix Tj according to the relative position relationship between each radar coordinate system and the robot coordinate system; a plurality of laser radars respectively collect environment point cloud data at the same moment, and point cloud coordinates in a radar coordinate system are converted to a robot coordinate system; substituting the point cloud coordinates converted to the coordinate system of the robot into a designed loss function F (Si), and iteratively solving the optimal pose Xi * of the robot at the current moment; and generating a motion path according to the calculated poses of the robot at different moments, and completing map construction along with the movement of the robot. According to the invention, the point cloud data collected by a plurality of laser radars can be effectively processed in real time, the problem that the mapping view of a single laser radar is blocked is solved, and the accuracy of a map is improved.

Description

Technical Field

[0001] The present invention relates to the field of mobile robot positioning and mapping, and in particular to a mobile robot multi-radar collaborative mapping method, device, equipment and medium. Background Art

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

[0003] For some mobile robots with special structures, designers cannot install the LiDAR in a suitable position. For example, when installing a LiDAR on an auxiliary mobile robot, the LiDAR can only be placed on the side of the robot, and inevitably the radar field of view will be blocked by a large area of ​​the robot body, which will significantly reduce the positioning and mapping accuracy. To solve this problem, multiple LiDARs can be installed around the mobile robot to compensate for the lack of radar field of view by increasing the number of LiDARs. At present, the commonly used two-dimensional laser SLAM algorithms on mobile robot platforms usually only support point cloud data from a single LiDAR as input signals. Summary of the invention

[0004] In order to solve at least one of the technical problems existing in the prior art to a certain extent, the purpose of the present invention is to provide a mobile robot multi-radar collaborative mapping method, device, equipment and medium based on point cloud data fusion.

[0005] The first technical solution adopted by the present invention is:

[0006] A mobile robot multi-radar collaborative mapping method comprises the following steps:

[0007] Construct the robot coordinate system and the radar coordinate system corresponding to each radar; there are multiple laser radars installed on the robot;

[0008] According to the relative position relationship between each radar coordinate system and the robot coordinate system, the rotation matrix P is established j and the translation matrix T j ;

[0009] Multiple laser radars collect environmental point cloud data at the same time and transform the point cloud coordinates in the radar coordinate system to the robot coordinate system; for the point cloud coordinate S in radar j at the i-th moment ji , according to the rotation matrix P j and the translation matrix T j Convert to the robot coordinate system to get the point cloud coordinates S j ′i ;

[0010] Substitute the point cloud coordinates converted to the robot coordinate system into the designed loss function F(S i ), iteratively solve the optimal posture ξ of the robot at the current moment * ;

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

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

[0013] Get any point on the chassis symmetry axis of the mobile robot as the origin O of the robot coordinate system. According to the right-hand coordinate system rule, let the y-axis of the coordinate system be along the robot's forward direction and the z-axis of the coordinate system be along the vertical upward direction to construct the robot coordinate system (x o ,y o ,z o ), the coordinate system (x o ,y o ,z o ) is used as the reference coordinate system of the mobile robot.

[0014] Furthermore, constructing a radar coordinate system corresponding to each radar includes:

[0015] Get the geometric center of each radar as the origin of the coordinate system O j , for the jth laser radar, its coordinate origin is recorded as O j According to the right-hand coordinate system rule, let the z-axis of the coordinate system be vertically upward, and the x- and y-axes of the coordinate system be in the same plane, and construct the radar coordinate system (x j ,y j ,z j ).

[0016] Furthermore, the rotation matrix P is established according to the relative position relationship between each radar coordinate system and the robot coordinate system. j and the translation matrix T j ,include:

[0017] Since each radar is fixed after being installed on the robot body, the origin of the radar coordinate system is O j The translation matrix T between the robot coordinate system origin O j Fixed, so in is the coordinate of the origin O of the robot coordinate system in the radar coordinate system;

[0018] Since each laser radar is installed at a different angle on the horizontal plane, it is necessary to determine the radar coordinate system (x j,y j ,z j ) and the robot coordinate system (x o ,y o ,z o ) between the rotation angle θ j , for each radar fixed on the robot body, there is a corresponding θ j , then the rotation matrix

[0019] Furthermore, the step of converting the point cloud coordinates in the radar coordinate system to the robot coordinate system includes:

[0020] Determine the radar coordinate system (x j ,y j ,z j ) and the robot coordinate system (x o ,y o ,z o ) j and the translation matrix T j Then, according to the relative transformation formula of coordinates in different coordinate systems, the point cloud coordinates S of the radar coordinate system are ji Convert the point cloud coordinates S′ to the robot coordinate system ji =R j S ji +T j .

[0021] Furthermore, the point cloud coordinates converted to the robot coordinate system are substituted into the designed loss function F(S i ), iteratively solve the optimal posture ξ of the robot at the current moment * ,include:

[0022] According to the point cloud data from multiple lidars at the same time, the current position of the mobile robot is calculated; 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, the higher the ξ * The closer it is to the real posture of the robot; Δξ is the difference between the current posture of the robot and the previous posture of the robot; m is the number of laser radars, and n is the resolution of the laser radar;

[0025] To solve the loss function F(S i )→0 * , first M(S ji(ξ+Δξ)) performs a first-order Taylor expansion at ξ, and then uses the Gauss-Newton method to solve for the optimal Δξ. Adding ξ at the previous moment to Δξ gives ξ at the current moment. * .

[0026] Furthermore, the motion path is generated according to the calculated robot postures at different times, and the map is constructed as the robot moves, including:

[0027] As the robot moves, the current position of the robot is calculated in real time according to the preset frequency. * , we can calculate ξ at each moment * , that is, the point cloud data at that moment is output to the map image as the obstacle edge, and the continuous ξ * That is the motion trajectory of the robot, and the continuously output point cloud data mapped to the picture is the constructed environment map.

[0028] The second technical solution adopted by the present invention is:

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

[0030] A coordinate system construction module is used to construct the robot coordinate system and the radar coordinate system corresponding to each radar; there are multiple radars installed on the robot;

[0031] The matrix calculation module is used to establish the rotation matrix P according to the relative position relationship between each radar coordinate system and the robot coordinate system. j and the translation matrix T j ;

[0032] The coordinate conversion module is used for multiple laser radars to collect environmental point cloud data at the same time and convert the point cloud coordinates in the radar coordinate system to the robot coordinate system;

[0033] The pose solving module is used to substitute the point cloud coordinates converted to the robot coordinate system into the designed loss function F(S i ), iteratively solve the optimal posture ξ of the robot at the current moment * ;

[0034] The map construction module is used to generate a motion path based on the calculated robot posture at different times and complete the map construction as the robot moves.

[0035] The third technical solution adopted by the present invention is:

[0036] An electronic device comprises 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 a mobile robot multi-radar collaborative mapping method as described above.

[0037] The fourth technical solution adopted by the present invention is:

[0038] A computer-readable storage medium, wherein at least one instruction, at least one program, a code set or an instruction set is stored in the storage medium, and 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 mobile robot multi-radar collaborative mapping method as described above.

[0039] The fifth technical solution adopted by the present invention is:

[0040] A computer program product or a 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 the processor executes the computer instructions, so that the computer device executes the above method.

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

[0042] 1) The present invention discloses a mobile robot multi-radar collaborative mapping method based on point cloud data fusion, which uses multi-radar point cloud data to build maps, thus overcoming the problem of inaccurate positioning caused by obstruction of the radar field of view when building maps with a single radar in some scenarios.

[0043] 2) The present invention has no restrictions on the direction of radar installation, and is easy to apply to various mobile robot bodies with special mechanical structures.

[0044] 3) Compared with the prior art, the present invention is less complicated, has low requirements on hardware computing power, and is easy to implement on various mobile robot platforms. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the embodiments of the present invention or the drawings of related technical solutions in the prior art are introduced below. It should be understood that the drawings introduced below are only for the convenience of clearly describing 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 work.

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

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

[0048] Figure 3 Schematic diagram of the radar coordinate system and the robot coordinate system in an embodiment of the present invention;

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

[0050] Figure 5 Schematic diagram of the field of view of the laser radar 1 in an embodiment of the present invention;

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

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

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

[0054] Fig. 9 A schematic diagram of a map constructed when the radar field of view is blocked in an embodiment of the present invention;

[0055] Fig.10 A schematic diagram of a map constructed by a mobile robot multi-radar collaborative mapping method according to an embodiment of the present invention;

[0056] Fig.11 The present invention is a flowchart of the steps of a mobile robot multi-radar collaborative mapping method in an embodiment of the present invention. DETAILED DESCRIPTION

[0057] The embodiments of the present invention are described in detail below, and examples of the embodiments are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements having the same or similar functions. The embodiments described below with reference to the accompanying drawings are exemplary and are only used to explain the present invention, and are not to be construed as limitations of the present invention. For the step numbers in the following embodiments, they are only provided for the convenience of explanation, and the order between the steps is not limited in any way, and the execution order of each step in the embodiment can be adaptively adjusted according to the understanding of those skilled in the art.

[0058] In the description of the present invention, it should be understood that descriptions involving orientations, such as up, down, front, back, left, right, etc., and orientations or positional relationships indicated are based on the orientations or positional relationships shown in the accompanying drawings, and are only for the convenience of describing the present invention and simplifying the description, and do not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation, and therefore should not be understood as a limitation on the present invention.

[0059] In the description of the present invention, "several" means one or more, "more" means more than two, "greater than", "less than", "exceed" etc. are understood as not including the number itself, and "above", "below", "within" etc. are understood as including the number itself. If there is a description of "first" or "second", it is only used for the purpose of distinguishing the technical features, and cannot be understood as indicating or implying the relative importance or implicitly indicating the number of the indicated technical features or implicitly indicating the order of the indicated technical features.

[0060] In the description of the present invention, unless otherwise clearly defined, terms such as setting, installing, connecting, etc. should be understood in a broad sense, and technicians in the relevant technical field can reasonably determine the specific meanings of the above terms in the present invention based on the specific content of the technical solution.

[0061] In response to the existing technical problems, the present invention proposes a mobile robot multi-radar collaborative mapping solution based on point cloud data fusion, which 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 Fig.11 As shown, this embodiment provides a mobile robot multi-radar collaborative mapping method, which performs real-time mapping by fusing multi-radar point cloud data, overcomes the problem of radar field of view being blocked during mapping in traditional single-radar mapping methods, and improves 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; multiple laser radars are installed on the robot.

[0065] As an optional implementation, multiple laser radars are installed and fixed one by one on the body of the mobile robot, ensuring that the radars are installed horizontally on the same plane, and there is no restriction on the installation direction of the radars.

[0066] For a wheeled robot with a symmetrical chassis, any point on the chassis axis of symmetry can be selected as the origin of the robot coordinate system O. A right-handed coordinate system is used, with the y-axis in front of the robot and the z-axis pointing vertically upward. o ,y o ,z o) as the reference coordinate system of the mobile robot. As an optional implementation, the geometric center of the robot body is selected as the origin of the coordinate system O. According to the right-hand coordinate system rule, the y-axis of the coordinate system is along the robot's forward direction, and the z-axis of the coordinate system is along the vertical upward direction. o ,y o ,z o ) is fixed thereafter.

[0067] To establish a radar coordinate system, the geometric center of each radar can be selected as the origin of the coordinate system O j According to the right-hand coordinate system rule, let the coordinate system z-axis be vertically upward, and the coordinate system x and y axes be in the same plane. j ,y j ,z j ) is fixed thereafter. For example, a conventional two-dimensional laser radar is a cylindrical structure, and the geometric center of the cylinder is taken as the origin of the laser radar coordinate system. For the laser radar numbered j, its coordinate origin is recorded as O j The method of this embodiment requires that the z-axis direction of the laser radar coordinate system is vertically upward and complies with the rule of the right-hand coordinate system. The x-axis and y-axis directions of the radar coordinate system are not required. That is, the laser radar can be rotated around the z-axis at any angle and then installed horizontally on the mobile robot body to obtain the corresponding coordinate system (x j ,y j ,z j ).

[0068] S2. According to the relative position relationship between each radar coordinate system and the robot coordinate system, establish the rotation matrix P j and the translation matrix T j .

[0069] Specifically, since each radar is fixed after being installed on the robot body, the origin of the radar coordinate system O j The translation matrix T between the robot coordinate system origin O j fixed, in is the coordinate of the origin O of the robot coordinate system in the radar coordinate system. Since each laser radar is installed at a different angle on the horizontal plane, it is necessary to determine the radar coordinate system (x j ,y j ,z j ) and the robot coordinate system (x o ,y o ,z o ) between the rotation angle θ j , for each radar fixed on the robot body, there is a corresponding θ j , then the rotation matrix

[0070] S3, multiple laser radars collect environmental point cloud data at the same time, and convert the point cloud coordinates in the radar coordinate system to the robot coordinate system; for the point cloud coordinate S in radar j at the i-th moment ji , according to the rotation matrix P j and the translation matrix T j Convert to the robot coordinate system and get the point cloud coordinates S j ′ i .

[0071] In some embodiments, the radar coordinate system (x j ,y j ,z j ) and the robot coordinate system (x o ,y o ,z o ) j and the 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 Convert the coordinates S' to the robot coordinate system ji =R j S ji +T j .

[0072] S4. Substitute the point cloud coordinates converted to the robot coordinate system into the designed loss function F(S i ), iteratively solve the optimal posture ξ of the robot at the current moment * .

[0073] At the beginning of mapping, each laser radar collects point cloud data at the same frequency, and the point cloud data at the same time are collected As algorithm input. Since the algorithm used in this embodiment needs to process point cloud data input from multiple laser radars at the same time, the loss function used in conventional single-radar mapping cannot be applied. This embodiment of the present invention proposes to use the loss function Where M(S i (ξ)) is the map occupancy probability index in the classic Hector-SLAM algorithm. The larger the value, the higher the ξ * The closer to the robot's true posture. The loss function designed in this embodiment can process point cloud data points numbered 1 to n from radars numbered 1 to m. Specifically, to solve the loss function F(S i )→0 * , you need to first change M(S ji(ξ+Δξ)) performs a first-order Taylor expansion at ξ, and then uses the Gauss-Newton method to solve for the optimal Δξ. Adding ξ at the previous moment to Δξ gives ξ at the current moment. * . Where Δξ is the difference between the robot posture at the current moment and the robot posture at the previous moment.

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

[0075] S5. Generate a motion path according to the calculated robot postures at different times, and complete the map construction as the robot moves.

[0076] As the robot moves, the algorithm calculates the robot's current position in real time at a certain frequency. * , we can calculate ξ at each moment * , that is, the point cloud data at that moment is output to the map image as the obstacle edge, and the continuous ξ * That is the motion trajectory of the robot, and the continuously output point cloud data mapped to the picture is the constructed environment map.

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

[0078] like Figure 1 As shown, this embodiment provides a mobile robot multi-radar collaborative mapping method based on point cloud data fusion, and the specific implementation method includes:

[0079] S101. Determine the robot coordinate system: The purpose of determining the robot coordinate system is to treat the mobile robot as a mass point when building a map to determine the unique position and posture of the robot at the current moment. In principle, any point on the robot 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 the robot coordinate system (x) 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, determine the coordinate system of laser radar m: Since multiple laser radars are installed on the mobile robot, the position and installation angle of each radar are different, so it is necessary to determine the coordinate system of each laser radar. In principle, any point on the laser radar 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 uses the origin of the coordinate system of radar j as jFixed at the geometric center of the radar, according to the rule of the right-hand coordinate system, the vertical upward direction is the z-axis, and the x-axis and y-axis can take any direction of the radar coordinate system (x j ,y j ,z j ), where 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 laser radar m coordinate system to the robot coordinate system: For any radar j among the m laser radars, there is a unique and fixed coordinate transformation relationship {R j ,T j}, R j represents the rotation between the radar j coordinate system and the robot coordinate system, T j represents the translation between the radar j coordinate system and the robot coordinate system. Since the radar j is fixed, {R j ,T j} can be obtained by measurement;

[0082] S401-S40M, laser radar data collection, S501-S50M, laser radar point cloud coordinate conversion: The point cloud data collected by each laser radar is based on the radar coordinate system. To use the point cloud data of multiple radars for mapping at the same time, it is necessary to convert the point clouds in different radar coordinate systems to the same coordinate system. This embodiment chooses to convert all radar point clouds to the robot coordinate system at the same time. According to the previously determined {R j ,T j}, for radar j, the point cloud P in the radar j coordinate system L ={S ji}Convert to the robot coordinate system to get

[0083] S601, S701, point cloud data input algorithm, use optimization algorithm to calculate the optimal value of robot posture change: Since the point cloud data to be processed in this embodiment comes from multiple laser radars with different postures, the common laser mapping algorithms on the market cannot process such inputs, so this embodiment proposes to use the following loss function The loss function takes into account all the 1st to nth point cloud data points from the laser radars numbered 1 to m. This embodiment uses the Gauss-Newton method to solve F(S i )→0;

[0084] S801, iteratively update the output robot posture: according to the robot posture update formula make Solve and update the robot's posture in real time;

[0085] S901. Robot movement 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 position and posture in real time. Based on the output position and posture and the point cloud data set at the current moment, the point cloud data is mapped to the real-time updated environment map to complete the mapping.

[0086] Experimental testing

[0087] This embodiment is a real scene experiment. A wheeled robot equipped with two WLR-719LIDAR single-line laser radars with a scanning frequency of 20Hz is selected as the experimental platform. The two laser radars are installed on both sides of the robot body to ensure that the field of view of the two radars is blocked by a large range. A Raspberry Pi 4B configured with the Ubuntu 18.04 operating system is selected as the main control board to run the program. The experimental scene is: a laboratory in a school.

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

[0089] Establish the laser radar 1, laser radar 2 and robot coordinate system, such as Figure 3 The coordinate system depends on the installation position and attitude of the corresponding laser radar, and once the installation is completed, the coordinate system is fixed.

[0090] By measuring the positional relationship between the origin of the laser radar coordinate system and the origin of the robot coordinate system and the rotation angle between the coordinate systems around the z-axis, the transformation relationship between the laser radar 1 coordinate system, the laser radar 2 coordinate system and the robot coordinate system is determined as follows: Figure 4 shown.

[0091] LiDAR 1 and LiDAR 2 are affected by the visual field occlusion, where the right visual field of LiDAR 1 and the left visual field of LiDAR 2 are blocked by the mobile robot body. Figure 5 and Figure 6 shown.

[0092] The point cloud data of laser radar 1 and laser radar 2 are simultaneously used as input, and the equivalent radar field of view obtained by the algorithm calculation output of this embodiment is as follows: Figure 7 shown.

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

[0094] Complete maps constructed using traditional methods when the lidar field of view is obstructed Fig. 9 shown.

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

[0096] In summary, the present invention proposes a mobile robot multi-radar collaborative mapping method based on point cloud data fusion, which uses multiple single-line laser radars as data acquisition sensors, cooperates with sensor devices such as IMU, connects them to the SOC main control board and carries them on the mobile robot, and can effectively process point cloud data collected from multiple laser radars in real time, overcomes the problem of single laser radar mapping field being blocked, simplifies the action route required by the mobile robot during the mapping process, and improves the accuracy of the map. The present invention is suitable for indoor mobile robot platforms with limited hardware resources.

[0097] Example 2

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

[0099] A coordinate system construction module is used to construct the robot coordinate system and the radar coordinate system corresponding to each radar; there are multiple radars installed on the robot;

[0100] The matrix calculation module is used to establish the rotation matrix P according to the relative position relationship between each radar coordinate system and the robot coordinate system. j and the translation matrix T j ;

[0101] The coordinate conversion module is used for multiple laser radars to collect environmental point cloud data at the same time and convert the point cloud coordinates in the radar coordinate system to the robot coordinate system;

[0102] The pose solving module is used to substitute the point cloud coordinates converted to the robot coordinate system into the designed loss function F(S i ), iteratively solve the optimal posture ξ of the robot at the current moment * ;

[0103] The map construction module is used to generate a motion path based on the calculated robot posture at different times and complete the map construction as the robot moves.

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

[0105] Example 3

[0106] An embodiment of the present invention further provides an electronic device, the electronic device comprising a processor and a memory, the memory storing 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 being loaded and executed by the processor to implement the following Fig.11 A mobile robot multi-radar collaborative mapping method is shown.

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

[0108] The processor may include one or more processing cores. The processor uses various interfaces and lines to connect the various parts of the entire server, and executes various functions of the server and processes data by running or executing instructions, programs, code sets or instruction sets stored in the memory, and calling data stored in the memory. Optionally, the processor can be implemented in at least one hardware form of digital signal processing (DSP), field programmable gate array (FPGA), and programmable logic array (PLA). The processor can integrate one or a combination of a central processing unit (CPU) and a modem. Among them, the CPU mainly processes the operating system and application programs; the modem is used to process wireless communications. It can be understood that the above-mentioned modem may not be integrated into the processor, but implemented separately through a chip.

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

[0110] Example 4

[0111] The embodiment of the present invention further provides a computer-readable storage medium, wherein the storage medium stores at least one instruction, at least one program, code set or instruction set, and the at least one instruction, the at least one program, the code set or instruction set is loaded and executed by a processor to implement the following Fig.11 A mobile robot multi-radar collaborative mapping method is shown.

[0112] Those skilled in the art can understand that all or part of the steps in the various methods of the above embodiments can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium, and the storage medium includes a read-only memory (ROM), a random access memory (RAM), a programmable read-only memory (PROM), an erasable programmable read-only memory (EPROM), a one-time programmable read-only memory (OTPROM), an electronically erasable rewritable read-only memory (EEPROM), a compact disc (CD-ROM) or other optical disc storage, magnetic disk storage, magnetic tape storage, or any other computer-readable medium that can be used to carry or store data.

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

[0114] Example 5

[0115] In some possible implementations, various aspects of the method of the embodiment of the present invention may also be implemented in the form of a program product, which includes a program code. When the program product is run on a computer device, the program code is used to enable the computer device to execute the steps of a mobile robot multi-radar collaborative mapping method according to various exemplary embodiments of the present application described above in this specification. Among them, the executable computer program code or "code" used to execute various embodiments can be written in a high-level programming language 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 the various parts of the present invention can be implemented by hardware, software, firmware or a combination thereof. In the above-mentioned embodiments, a plurality of steps or methods can be implemented by software or firmware stored in a memory and executed by a suitable instruction execution system. For example, if implemented by hardware, as in another embodiment, it can be implemented by any one of the following technologies known in the art or their combination: a discrete logic circuit having a logic gate circuit for implementing a logic function for a data signal, a dedicated integrated circuit having a suitable combination of logic gate circuits, a programmable gate array (PGA), a field programmable gate array (FPGA), etc.

[0117] In the description of this specification, the description with reference to the terms "one embodiment", "some embodiments", "example", "specific example", or "some examples" etc. means that the specific features, structures, materials or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. In this specification, the schematic representations of the above terms do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials or characteristics described may be combined in any one or more embodiments or examples in a suitable manner. In addition, those skilled in the art may combine and combine the different embodiments or examples described in this specification and the features of the different embodiments or examples, without contradiction.

[0118] The above embodiments are only for illustrating the technical concept and features of the present invention, and their purpose is to enable ordinary technicians in the field to understand the content of the present invention and implement it accordingly, and they cannot be used to limit the protection scope of the present invention. Any equivalent changes or modifications made based on the essence of the content of the present invention should be included in the protection scope of the present invention.

Claims

1. A mobile robot multi-radar collaborative mapping method, characterized in that: The following steps are involved: Construct the robot coordinate system and the radar coordinate system corresponding to each radar; there are multiple radars installed on the robot; According to the relative position relationship between each radar coordinate system and the robot coordinate system, the rotation matrix P is established j and the translation matrix T j ; Multiple laser radars collect environmental point cloud data at the same time and convert the point cloud coordinates in the radar coordinate system to the robot coordinate system; Substitute the point cloud coordinates converted to the robot coordinate system into the designed loss function F(S i ), iteratively solve the optimal posture ξ of the robot at the current moment * ; The motion path is generated based on the calculated robot posture at different times, and the map is built as the robot moves.

2. A mobile robot multi-radar collaborative mapping method according to claim 1, characterized in that: The step of constructing the robot coordinate system comprises: Get any point on the chassis symmetry axis of the mobile robot as the origin O of the robot coordinate system. According to the right-hand coordinate system rule, let the y-axis of the coordinate system be along the robot's forward direction and the z-axis of the coordinate system be along the vertical upward direction to construct the robot coordinate system (x o ,y o , z o ), the coordinate system (x o ,y o , z o ) is used as the reference coordinate system of the mobile robot.

3. The mobile robot multi-radar collaborative mapping method according to claim 1, characterized in that: The step of constructing a radar coordinate system corresponding to each radar includes: Get the geometric center of each radar as the origin of the coordinate system O j , for the jth laser radar, its coordinate origin is recorded as O j According to the right-hand coordinate system rule, let the z-axis of the coordinate system be vertically upward, and the x- and y-axes of the coordinate system be in the same plane, and construct the radar coordinate system (x j ,y j , z j ).

4. The mobile robot multi-radar collaborative mapping method according to claim 1, characterized in that: According to the relative position relationship between each radar coordinate system and the robot coordinate system, a rotation matrix P is established. j and the translation matrix T j ,include: Since each radar is fixed after being installed on the robot body, the origin of the radar coordinate system is O j The translation matrix T between the robot coordinate system origin O j Fixed, so in is the coordinate of the origin O of the robot coordinate system in the radar coordinate system; Since each radar is installed at a different angle on the horizontal plane, it is necessary to determine the radar coordinate system (x j ,y j , z j ) and the robot coordinate system (x o ,y o , z o ) between the rotation angle θ j , for each radar fixed on the robot body, there is a corresponding θ j , then the rotation matrix 5. The mobile robot multi-radar collaborative mapping method according to claim 1, characterized in that: The step of converting the point cloud coordinates in the radar coordinate system to the robot coordinate system includes: Determine the radar coordinate system (x j ,y j , z j ) and the robot coordinate system (x o ,y o , z o ) j and the translation matrix T j Then, according to the relative transformation formula of coordinates in different coordinate systems, the point cloud coordinates S of the radar coordinate system are ji Convert the point cloud coordinates S′ to the robot coordinate system ji =R j S ji +T j .

6. The mobile robot multi-radar collaborative mapping method according to claim 1, characterized in that: Substitute the point cloud coordinates converted to the robot coordinate system into the designed loss function F(S i ), iteratively solve the optimal posture ξ of the robot at the current moment * ,include: According to the point cloud data from multiple radars at the same time, the current position of the mobile robot is calculated; 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 value, the higher the ξ * The closer it is to the real posture of the robot; Δξ is the difference between the current posture of the robot and the previous posture of the robot; m is the number of radars, and n is the radar resolution; To solve the loss function F(S i )→0 * , first M(S ji (ξ+Δξ)) performs a first-order Taylor expansion at ξ, and then uses the Gauss-Newton method to solve for the optimal Δξ. Adding ξ at the previous moment to Δξ gives ξ at the current moment. * .

7. The mobile robot multi-radar collaborative mapping method according to claim 1, characterized in that: The motion path is generated according to the calculated robot postures at different times, and the map is constructed as the robot moves, including: As the robot moves, the current position of the robot is calculated in real time according to the preset frequency. * , we can calculate ξ at each moment * , that is, the point cloud data at that moment is output to the map image as the obstacle edge, and the continuous ξ * That is the motion trajectory of the robot, and the continuously output point cloud data mapped to the picture is the constructed environment map.

8. A mobile robot multi-radar collaborative mapping device, characterized in that: include: A coordinate system construction module is used to construct the robot coordinate system and the radar coordinate system corresponding to each radar; there are multiple radars installed on the robot; The matrix calculation module is used to establish the rotation matrix P according to the relative position relationship between each radar coordinate system and the robot coordinate system. j and the translation matrix T j ; The coordinate conversion module is used for multiple laser radars to collect environmental point cloud data at the same time and convert the point cloud coordinates in the radar coordinate system to the robot coordinate system; The pose solving module is used to substitute the point cloud coordinates converted to the robot coordinate system into the designed loss function F(S i ), iteratively solve the optimal posture ξ of the robot at the current moment * ; The map construction module is used to generate a motion path based on the calculated robot posture at different times and complete the map construction as the robot moves.

9. 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 described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that: The storage medium 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 according to any one of claims 1 to 7.

Images