A method, system and storage medium for ensuring multi-robot arrival consistency

Abstract

The present invention provides a method, a system and a storage medium for ensuring the arrival consistency of multiple robots. The method steps include: Step S1: Establish a calibration site and calculate the transformation matrix of the pose of each robot under the reference map; Step S2: Establish a working site and calculate the pose of the robot and the working site and their transformation matrices in the reference map coordinate system map respectively; Step S3: When the robot navigates to the working site, each robot generates a new end point for navigation and commands the robot to go. Thereby, multiple robots can complete the precise docking between the robot docking mechanism and the site docking mechanism under the condition of using the same map.

Description

Technical Field

[0001] The present invention relates to robot navigation calibration technology, and in particular, to a method and system for ensuring the consistency of multiple robots arriving at a destination, and a storage medium. Background Art

[0002] With the advancement of Industry 4.0 and intelligent manufacturing, mobile robots are increasingly widely used in production lines, warehousing, and logistics. Multi-robot cooperation has become the key to improving efficiency.

[0003] In the industrial logistics scenario, there are situations where multiple robots need to separately transport goods to the same station. In this scenario, the docking mechanisms of each robot need to successfully dock with the docking mechanism of the station before subsequent freight operations can be carried out. Therefore, in this process, in order to enable each robot to accurately complete the freight docking, theoretically, as long as the positioning position of the robot, the position of the docking mechanism of the target station, and the position of the docking mechanism of the robot itself are known, and even each robot can use the same map, accurate navigation and docking can be achieved.

[0004] However, in reality, both the robots and the docking mechanisms at the stations will have different differences due to manufacturing and installation errors. For example, the installation positions of the positioning devices of each robot in its robot coordinate system are inconsistent, or the installation positions of the docking mechanisms of each robot in its robot coordinate system are inconsistent. As a result, these situations will cause multiple robots to be unable to navigate to the same point based on the same map to complete the accurate docking between the robot and the station. Summary of the Invention

[0005] Therefore, the main object of the present invention is to provide a method and system for ensuring the consistency of multiple robots arriving at a destination, and a storage medium, to solve the problems mentioned in the background art, so that multiple robots can complete the accurate docking between the robot docking mechanism and the station docking mechanism under the condition of using the same map.

[0006] To achieve the above object, according to one aspect of the present invention, a method for ensuring the consistency of multiple robots arriving at a destination is provided. The steps include:

[0007] Step S1: Establish a calibration station and calculate the transformation matrix of the positioning pose of each robot in the reference map. ;

[0008] Step S2: Establish a working station, and calculate the pose and its transformation matrix of the robot and the working station in the reference map coordinate system map respectively. , ;

[0009] Step S3: When the robot navigates to the working station At this time, each robot according to Calculate to update Generate a new end point for navigation and instruct the robot to go there.

[0010] In a possible preferred embodiment, in step S1, the calculation steps of the transformation matrix include:

[0011] In step S11, set a marker a at the docking point A of the calibration site; set a marker b at the docking point B of the robot; set an identification device in the scene to cover the scene including the positions where the markers a and b are located;

[0012] In step S12, when the robot identifies the marker a, obtain the pose of the docking point A in the reference map coordinate system, use it as the calibration site, plan a calibration navigation path, and instruct the robot to go there;

[0013] In step S13, the identification device identifies the marker b, and obtain the pose of the docking point B in the external identification coordinate system L and its transformation matrix ;

[0014] In step S14, the identification device identifies the marker a, consider that the robot coincides with the calibration site, regard the position where the robot is located as the real vehicle coordinate system R, and obtain the pose of the robot in the external identification coordinate system L and its transformation matrix ;

[0015] In step S15, according to the installation position of the docking point B on the robot, determine the pose of the docking point B in the ideal vehicle coordinate system I and its transformation matrix , where the calibration site on the reference map is the ideal vehicle coordinate system I;

[0016] In step S16, calculate to obtain the transformation matrix of the real vehicle coordinate system R in the ideal vehicle coordinate system I for the calibrated pose .

[0017] In a possible preferred embodiment, in step S2, the calculation steps of the transformation matrices , include:

[0018] In step S21, debug the robot and move its docking point to coincide with the working site;

[0019] In step S22, record the current pose of the robot in the reference map coordinate system map and its transformation matrix as , and correspondingly record the pose of the working site and its transformation matrix in the reference map as .

[0020] In a possible preferred embodiment, the method for ensuring the consistency of multi-robot arrival further includes the following steps:

[0021] Step S23 combines the poses of the workstations obtained by all robots and their transformation matrices in the same reference map.

[0022] In a possible preferred embodiment, the identification device is a laser identification device, and the markers a and b are reflective column markers.

[0023] In a possible preferred embodiment, the identification device is an image recognition device, and the corresponding markers a and b are any one of graphic markers and QR code markers.

[0024] In a possible preferred embodiment, the markers a and b are respectively arranged in pairs on both sides of the docking point A and the docking point B, and respectively maintain a preset positional layout relationship with the docking point A and the docking point B.

[0025] In a possible preferred embodiment, in step S1, the calculation steps of the transformation matrix include:

[0026] Step S11 sets the marker a at the docking point A of the calibration site; sets a pre-site near the calibration site; sets the marker b at the docking point B of the robot; sets an identification device in the scene to cover the scene where the markers a and b are located;

[0027] Step S12 when the robot recognizes the marker a, obtains the pose of the docking point A in the reference map coordinate system, uses it as the calibration site, plans a calibration navigation path to go from the pre-site, and makes the robot go;

[0028] Step S13 the identification device recognizes the marker b, and obtains the pose of the docking point B in the external recognition coordinate system L and its transformation matrix ;

[0029] Step S14 the identification device recognizes the marker a, considers the robot to coincide with the calibration site, regards the position where the robot is located as the real vehicle coordinate system R, and obtains the pose of the robot in the external recognition coordinate system L and its transformation matrix ;

[0030] Step S15 determines the pose of the docking point B in the ideal vehicle coordinate system I and its transformation matrix according to the installation position of the docking point B on the robot, where the calibration site on the reference map is the ideal vehicle coordinate system I;

[0031] Step S16 calculates , obtain the transformation matrix of the pose of the real vehicle coordinate system R relative to the ideal vehicle coordinate system I .

[0032] To achieve the above object, according to another aspect of the present invention, there is also provided a system for ensuring the consistency of multi-robot arrival, which includes: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the system implements the steps of the method for ensuring the consistency of multi-robot arrival described in any one of the above.

[0033] To achieve the above object, according to another aspect of the present invention, there is also provided a computer-readable storage medium storing a computer program, where when the computer program is executed, the steps of the method for ensuring the consistency of multi-robot arrival described in any one of the above are implemented.

[0034] Through the method, system, and storage medium for ensuring the consistency of multi-robot arrival provided by the present invention, a consistency calibration scheme is ingeniously proposed to calibrate a relative coordinate transformation relationship between the real vehicle coordinate system and the ideal vehicle coordinate system. With the action of this coordinate transformation, it can support multiple robots of the same specification. Even if there are inconsistencies in the installation positions of the positioning devices, when going to each work site, accurate docking point navigation positions can be obtained based on the same reference map after coordinate transformation, so that the docking mechanisms of the robots and the docking mechanisms of the target work sites can be accurately docked.

[0035] In addition, based on the calibration scheme of this solution, when establishing N work sites, it can also support N robots to complete the establishment of all work sites simultaneously at one time, instead of establishing N work sites for each robot separately. Therefore, the implementation workload is greatly reduced, the implementation cycle is shortened, and it can ensure that multi-robots can meet the consistency docking accuracy requirements when facing the docking mechanisms of the same work site. BRIEF DESCRIPTION OF THE DRAWINGS

[0036] The drawings constituting a part of this application are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

[0037] Figure 1 is a schematic diagram of the steps of the method for ensuring the consistency of multi-robot arrival according to the present invention;

[0038] Figure 2 is a schematic diagram of the position reference relationship between the reflective columns 1 to 2 and the calibration site and the pre-site in the method for ensuring the consistency of multi-robot arrival according to the present invention;

[0039] Figure 3In the method for ensuring the consistency of multi-robot arrival in the present invention, it is a schematic diagram of the installation positions of the reflective columns 1 to 4 and the external lidar, and the position reference relationship between the robot and the front station;

[0040] Figure 4 In the method for ensuring the consistency of multi-robot arrival in the present invention, it is a reference schematic diagram of the process of gradually approaching between the docking point of the calibration station and the docking point of the robot during the docking at the calibration station;

[0041] Figure 5 In the method for ensuring the consistency of multi-robot arrival in the present invention, the original trajectory is the trajectory directly from the "point to be navigated" to the "working station", and the new trajectory is the trajectory from the point to be navigated to the new end point Reference schematic diagram of trajectory comparison;

[0042] Figure 6 It is a schematic diagram of the structure of the system for ensuring the consistency of multi-robot arrival in the present invention. Detailed implementation manners

[0043] In order to enable those skilled in the art to better understand the technical solutions of the present invention, the following will clearly and completely describe the specific technical solutions of the present invention in conjunction with embodiments, so as to help those skilled in the art further understand the present invention. Obviously, the embodiments described in this case are only a part of the embodiments of the present invention, rather than all the embodiments. It should be noted that for those of ordinary skill in the art, without departing from the concept of the present invention and without conflict with each other, the embodiments in this application and the features in the embodiments can be combined with each other. Based on the embodiments of the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the scope of disclosure and protection of the present invention.

[0044] In addition, in the description, claims and drawings of the present invention, terms such as "first", "second", "S1", "S2", etc. are used to distinguish similar objects, and do not necessarily describe a specific order or sequence. It should be understood that the features used in this way can be interchanged under appropriate circumstances, so that the embodiments of the present invention described here can be implemented in an order other than those described here. At the same time, the stages described in each step are not forced to be implemented in the same step. It should be understood that the implementation order of the content in each step can be adjusted and interchanged without violating the inventive concept, so that the step embodiments of the present invention described here can be implemented in an order other than those described here. In addition, the terms "include" and "have" in the present invention and any variations thereof are intended to cover non-exclusive inclusion. Unless otherwise clearly specified and limited, the terms "set", "arrange", "install", "connect", "couple" should be understood in a broad sense. For example, it can be a fixed connection, a detachable connection, or an integral connection; it can be a mechanical connection or an electrical connection; it can be directly connected or indirectly connected through an intermediate medium, and it can be the communication inside two components. For those of ordinary skill in the art, the specific meanings of the above terms in this case can be understood according to the specific situation in combination with the prior art.

[0045] In order to enable multiple robots to complete the precise docking between the robot docking mechanism and the station docking mechanism under the condition of using the same map, as Figures 1 to 5 shown, the present invention provides a method for ensuring the consistency of multi-robot arrival, and its exemplary steps include:

[0046] Step S1: Establish a calibration station and calculate the transformation matrix of the position and pose of each robot in the reference map .

[0047] Specifically, due to the inconsistent installation positions of the positioning devices of each robot in its robot coordinate system, and the inconsistent installation positions of the docking mechanism positions of each robot in its robot coordinate system, multiple robots cannot navigate to the same point based on the same map to achieve precise docking between the robot and the station.

[0048] Therefore, the present invention conceives a calibration method for robots to calibrate a coordinate transformation relationship. For example: In this example, it is assumed that the working station on the reference map stored in the robot is the ideal vehicle coordinate system I, and the calibrated station after transformation is the real vehicle coordinate system R. Then this transformation relationship is expressed as: When the docking mechanisms of the robot and the calibration station coincide, the transformation matrix of the position and pose of the real vehicle coordinate system R in the ideal vehicle coordinate system I , can be expressed as:

[0049] Formula (1)

[0050] Wherein in the formula: R represents the real vehicle coordinate system, L represents the external recognition coordinate system, M represents the docking mechanism coordinate system, and I represents the ideal vehicle coordinate system. Then the pose of the real vehicle coordinate in the ideal vehicle coordinate system is: the pose transformation matrix of the docking mechanism in the ideal vehicle coordinate system , the pose transformation matrix of the recognition device in the docking mechanism coordinate system and the pose transformation matrix of the real vehicle in the external recognition coordinate system multiplied together.

[0051] In order to obtain the transformation matrix parameters required for the above-related calibration, the specific example process is as follows.

[0052] - Establish a calibration site, as Figures 2 to 3 shown, and its example steps include:

[0053] Step S11 Set a marker a at the docking point A of the calibration site and a marker b at the docking point B of the robot; set a recognition device in the scene to cover the scene where the markers a and b are located.

[0054] Step S12 When the robot recognizes the marker a, obtain the pose of the docking point A in the reference map coordinate system, use it as the calibration site, plan a calibration navigation path from the front site to go, and let the robot go.

[0055] For example, in this example, the markers a and b are illustrated by reflective posts. As shown in the appendix Figure 2 shown, after placing the reflective post 1 and the reflective post 2 on the ground. The robot recognizes the positions of the reflective post 1 and the reflective post 2 in the current reference map coordinate system according to its current positioning position in the reference map coordinate system and . Take the midpoint coordinates of the positions and to construct the position of the "calibration site", that is, the docking point A. Take and 's normal direction as the orientation of the site. In addition, in order to facilitate planning the navigation path, it is optional to set a front site near the calibration site.

[0056] Then, install the reflective post 3 and the reflective post 4 on the robot body, and set the center positions of the reflective post 3 and the reflective post 4 as the position of the vehicle body docking mechanism, that is, the docking point B, and its normal direction as the orientation of the vehicle body docking mechanism.

[0057] At the same time, in the calibration scene, set a recognition device. For example, in this example, it is illustrated by an external lidar. Its installation position example is as Figure 3As shown, so that its recognition range covers the scenarios where markers a and b are located.

[0058] - Calculate the calibration result, and its example steps include:

[0059] In step S13, the recognition device recognizes marker b, and obtains the pose of docking point B in the external recognition coordinate system L and its transformation matrix ;

[0060] For example: By using an external lidar to recognize the reflective columns 3 and 4 in the environment, the pose of the docking mechanism of the robot body in the external lidar coordinate system and its transformation matrix can be obtained .

[0061] In step S14, the recognition device recognizes marker a, assumes that the robot coincides with the calibration site, and regards the position where the robot is located as the real vehicle coordinate system R, so as to obtain the pose of the robot in the external recognition coordinate system L and its transformation matrix .

[0062] For example: Since the calibration site is established at the center position of the reflective columns 1 and 2, after the robot body navigates there, by using an external lidar to recognize the reflective columns 1 and 2 in the environment, the pose of the real vehicle in the external lidar coordinate system and its transformation matrix can be obtained . That is, it is assumed that the robot coincides with the calibration site at this time, so the position where the robot is located is the real position, regarded as the real vehicle coordinate system R.

[0063] It is worth mentioning that since the calibration site is in the middle of the reflective columns 1 and 2, the external lidar can recognize the reflective columns 1 and 2 to obtain the pose of the calibration site in the external lidar coordinate system, and the robot navigates to the calibration site. Therefore, in an ideal situation, when the navigation error between the vehicle and the calibration site is 0, the robot is at the calibration site, so the external lidar can recognize the reflective columns 1 and 2 to obtain .

[0064] When the navigation error is not 0, at this time, it is assumed that the navigation error of the robot is fixed , , , the pose of the calibration site is . The following compares two calculation methods:

[0065] 1) Add the pose of the calibration site and the navigation error and regard it as the pose of the real vehicle under the external lidar :

[0066] At this time, the pose of the real vehicle in the external recognition coordinate system is the site pose + navigation error = , and according to formula (1), we get Since the vehicle body docking mechanism coincides with the external docking mechanism at this time, the calibration result should be , so strictly using the real vehicle pose for calibration will introduce navigation errors.

[0067] 2) Regarding the calibration site pose as the real vehicle's pose under the external laser :

[0068] At this time, the real vehicle's pose in the external recognition coordinate system is , and according to formula (1), we get . Since the vehicle body docking mechanism coincides with the external docking mechanism at this time, the calibration result conforms to the actual situation. Therefore, directly using the calibration site pose as the real vehicle pose for calculation will include the navigation error in the calibration result.

[0069] So, in the above real vehicle's pose in the external recognition coordinate system , in this example, we choose to directly use the pose of the "calibration site" recognized by the external lidar in the external laser coordinate system as the real position of the robot to avoid introducing navigation errors.

[0070] Step S15 determines the pose of docking point B in the ideal vehicle coordinate system I and its transformation matrix according to the installation position of docking point B on the robot , where the calibration site on the reference map is the ideal vehicle coordinate system I.

[0071] For example, in actual calibration, the pose of the docking mechanism of the robot in the ideal vehicle coordinate system and the corresponding transformation matrix can be directly obtained from the design drawings.

[0072] Step S16 substitutes the above data into formula 1 to calculate , and then we can obtain the transformation matrix of the calibrated pose of the real vehicle coordinate system R in the ideal vehicle coordinate system I .

[0073] Considering that during the calibration process of multi-robot consistency, since the robots to be calibrated are generally of the same type, their is the same. And since the external reflective columns 1 and 2 and the external lidar are arranged in a fixed calibration scene, during the calibration process is also the same. However, due to manufacturing or installation errors of the actual vehicle body docking mechanisms, they are generally different. Therefore, after calibration, each robot obtains a coordinate transformation that suits itself.

[0074] In addition, due to the positioning devices of each robot, their installation positions in their respective robot coordinate systems are inconsistent, while the position of the "calibration site" in the map is fixed. Therefore, when navigating to the "calibration site", if the installation positions of the positioning devices are different, the positions of the real vehicle after reaching the "calibration site" will also be different. Therefore, the transformation matrix calibrated by the present invention can overcome the influence brought by the inconsistent installation positions of the positioning devices.

[0075] After the calibration of each robot is completed, in order to ensure the connection consistency between the docking mechanisms of multiple robots and the docking mechanism of the working site under the same map, this solution further proposes a construction plan for the working site.

[0076] Step S2: Establish a working site, and calculate the poses and their transformation matrices of the robot and the working site in the reference map coordinate system map 、 .

[0077] Specifically, in a factory environment where mobile robots pick up and place goods, multiple robots need to dock with the docking mechanism at many points. To enable the docking mechanisms of different mobile robots to reach the same position at the same point, a simple implementation method is to establish points for each vehicle separately. However, if there are M robots and N points, then M*N points need to be established during the implementation process, which will obviously consume a huge amount of workload and require a long implementation cycle.

[0078] Therefore, this solution attempts to solve the above problems by only establishing N points jointly by multiple robots. As shown in Figure 4 , in which the calculation steps of the transformation matrices 、 in step S2 include:

[0079] Step S21: Debug the robot and move its docking point to coincide with the working site;

[0080] Step S22: Record the pose and its transformation matrix of the current robot in the reference map coordinate system map as , and record the pose and its transformation matrix of the working site in the reference map as .

[0081] Specifically, first control the robot to be near the docking mechanism of the working site, and then manually move the robot body so that the docking mechanism of the robot coincides with the docking mechanism of the working site. Record the pose transformation matrix of the robot in the reference map coordinate system map as , and then establish a "working site" in the map, and its pose transformation matrix is:

[0082] Formula (2)

[0083] It should be noted that for the convenience of description, in the formulas in the examples of the present invention, only the pose transformation matrix is calculated. In this example, the "workstation" in the map is saved using the pose. Since the conversion relationship between the pose and its transformation matrix is basic knowledge, it will not be elaborated here. Because for those skilled in the art, the equivalent relationship of mutual conversion between the transformation matrix and the pose in the above formulas and examples, as well as the conversion adjustments required during the implementation process, can be understood. Therefore, when implementing, those skilled in the art can mutually convert and implement according to the control system settings of their own robots.

[0084] Further, in an alternative embodiment, the following can be further executed: Step S23 combines the poses and their transformation matrices of the workstations obtained by all robots in the same reference map.

[0085] Based on the above example, for the "workstation" established by a robot in the map, on this site, this robot and other robots can both make the body docking mechanism and the working point docking mechanism of this robot coincide through the coordinate transformation calibrated by themselves. In this way, it can be realized that after multiple robots jointly establish all the "workstations" under the same positioning map, they are then stitched together into a complete map for all vehicles to share.

[0086] Step S3 When the robot navigates to the workstation each robot generates a new end point for navigation according to and commands the robot to go there.

[0087] Specifically, after multiple robots jointly establish all the "workstations", assuming the pose transformation matrix of the "workstation" is when navigating to this site, a new end point for navigation needs to be generated according to the coordinate transformation calibrated by this robot and its corresponding pose transformation matrix is :

[0088] Formula (3)

[0089] As shown in the appendix Figure 5 the original trajectory is the trajectory directly from the "point to be navigated" to the "workstation", and the new trajectory is the trajectory from the point to be navigated to the new end point .

[0090] With this setting, since multiple vehicles use the same map of the "workstation" and each vehicle has its own calibrated coordinate transformation​​ Therefore, the new end point of the "workstation" after coordinate transformation is the pose when the docking mechanisms of the robot and the workstation coincide. It can be seen that this solution ensures that multiple robots share the same map and that when different robots reach the same "workstation", the vehicle body docking mechanism coincides with the station docking mechanism.

[0091] To verify the feasibility of the above example, assume that multiple robots need to navigate to the same point using the same map and the docking mechanisms reach the same position. Taking 3 robots as an example, they are vehicle A, vehicle B, and vehicle C respectively.

[0092] Calibrate the coordinate transformation for each vehicle first. For vehicle A, there is a positioning laser on the vehicle body for positioning and identification, which can identify the external reflective columns 1 and 2. Assume that the current pose of vehicle A in the map is P Map ( x , y , angle )=[0.3 m ,0.5 m , 18 0 ° ] , and the positions of the reflective columns 1 and 2 identified by vehicle A in the map are P 1 Map ( x , y )=[ 2.0145 m , 1.8039 m ] and P 2 Map ( x , y )=[ 2.0642 m ,-1.6265 m ] , then the calculation method of ""calibration site"" is:

[0093]

[0094] .

[0095] Among them, The function is to make the site orientation approach the current orientation of the robot. It can be obtained that A 1 Map ( x , y , angle )=[2.0394 m ,0.0887 m , 179.1691 ° ] .

[0096] Set the pose of the "front site" in the map . Since the coordinates of A1 are known, assuming that the "front site" is set at a distance D in the backward direction of the calibration site, then:

[0097] A 2 Map ( x , y , angle ) = [ A 1 Map ( x )- D * cos ( A 1 Map ( angle )), A 1 Map ( x )- D * sin ( A 1 Map ( angle )), A 1 Map ( angle )]

[0098] In this hypothesis, D = 2m, and it can be obtained that A 2 Map ( x , y ,angle )=[0. 0396 m , 0.1177 m , 179.1691 ° ] The control algorithms for navigating from the "frontier site" to the "calibration site" are quite common, such as: PID, LQR, and MPC control methods, which will not be elaborated here.

[0099] As attached Figure 3 As described, by using an external lidar to identify the retroreflective posts 1, 2 and the retroreflective posts 3, 4, the coordinates in the external recognition coordinate system can be obtained as follows:

[0100] P 1 Laser ( x , y )=[ 1.99922 m ,- 1.69147 m ]

[0101] P 2 Laser ( x , y )=[ 1. 8965 m ,1.68587 m ]

[0102] P 3 Laser ( x , y )=[2.06905 m , - 0.190122 m ]

[0103] P 4 Laser ( x , y )=[2.051114 m ,0.200761 m ]

[0104] Since the midpoint coordinates of two positions are taken in the previous step to construct the position of the "calibration station", take and as the normal direction of the station, and assume that the center positions of the reflector columns 3 and 4 are the positions of the vehicle body docking mechanism, and its normal direction is the orientation of the vehicle body docking mechanism. Then, the pose of the station and the pose of the vehicle body docking mechanism in the external recognition coordinate system can be calculated and , and the calculation method is:

[0105]

[0106]

[0107]

[0108]

[0109] Among them, and are used to make the pose orientation close to the station orientation. It can be obtained that:

[0110] A 1 Laser ( x , y , angle ) = [1.9479 m ,-0.0028 m ,178.2639 ° ]

[0111] M laser ( x , y , angle )=[2.0601 m ,0.0053 m ,177.3815 ° ]

[0112] For the convenience of calculation, assume that the pose of the docking mechanism in the ideal vehicle coordinate system is ,

[0113] T Module I = [ cos ( M I ( angle )) - sin ( M I ( angle )) M I ( x ) sin ( M I ( angle )) cos ( M I ( angle )) M I ( y ) 0 0 1 ]

[0114] T Module Laser = [ cos ( M Laser ( angle )) - sin ( M Laser ( angle )) M Laser ( x ) sin ( M Laser ( angle )) cos ( M Laser ( angle )) M Laser ( y ) 0 0 1 ]

[0115] T R Laser = [ cos ( A 1 Laser ( angle )) - sin ( A 1 Laser ( angle )) A 1 Laser ( x ) sin ( A 1 Laser ( angle )) cos ( A 1 Laser ( angle )) A 1 Laser ( y ) 0 0 1 ]

[0116] According to formula (1), the coordinate transformation calibrated for vehicle A can be obtained, denoted as :

[0117] T R I _ A = [ 0.9999 -0.0154 0.1117 0.0154 0.9999 0.0132 0 0 1 ]

[0118] Similarly, the coordinate transformations for vehicles B and C can also be calibrated, denoted as and :

[0119] T R I _ B = [ 0.999 2 -0.0401 0. 08 1 6 0.0401 0.999 2 0.0 2 3 4 0 0 1 ]

[0120] T R I _ C = [ 0.9952 -0.0976 0. 21 17 0.0976 0.9952 0. 0502 0 0 1 ]

[0121] Based on this, the calibration of the coordinate transformation for each vehicle is completed. The following demonstrates the implementation method.

[0122] During implementation, multiple vehicles need to jointly establish a "working station" under the same positioning map. There is an existing "station docking mechanism 1". Manually operate vehicle A to align the docking mechanism of vehicle A with the "station docking mechanism 1", and record the current positioning pose of vehicle A in the map as A 1 _ org Map ( x , y , angle )=[0.1,0.2,10 ° ] , according to formula (2), the transformation matrix corresponding to the pose of "working station 1" in the map is:

[0123] T I Map =[ cos ( A 1 _ org Map ( angle )) - sin ( A 1 _ org Map ( angle )) A 1 _ org Map ( x ) sin ( A 1 _ org Map ( angle )) cos ( A 1 _ org Map ( angle )) A 1 _ org Map ( y ) 0 0 1 ]* ( T R I _ A ) -1 =[ 0.9874 -0.1584 -0.0134 0.1584 0.9874 0.1684 0 0 1 ]

[0124] Convert it into the pose of "working station 1" in the map which is:

[0125] A 1 Map ( x , y , angle )=[-0.0134 m ,0.1684 m ,9.1187 ° ]

[0126] After vehicle A constructs "working station 1", vehicle B and vehicle A share the same map. When vehicle B navigates to "working station 1", a new navigation end point will be generated according to the station position and the transformation matrix calibrated by the current vehicle B The end point pose can be calculated by formula (3):

[0127] T R Map = [ cos ( A 1 Map ( angle )) - sin ( A 1 Map ( angle )) A 1 _ org Map ( x ) sin ( A 1 Map ( angle )) cos ( A 1 Map ( angle )) A 1 _ org Map ( y ) 0 0 1 ]* T R I _ B = [ 0.9985 -0.0555 0.1980 0.0555 0.9985 0.0413 0 0 1 ]

[0128] At this point, the pose of the new navigation destination is ([[]] . Given the starting pose and the ending pose, there are many ways to perform path planning, such as Bezier curves, polynomial curves, etc. This is prior art and will not be elaborated here. After navigating through this, after multiple vehicles use the same map to jointly establish a "working station", when each vehicle reaches the same "working station", the docking mechanism of each vehicle can be docked with the docking structure of the station to maintain a high-precision docking.

[0129] Further, corresponding to the above method example, such as Figure 6 shown, the present invention also provides a system for ensuring the consistency of multi-robot arrival, which includes: a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the system implements the steps of the method for ensuring the consistency of multi-robot arrival as described in any one of the above.

[0130] Further, corresponding to the above method example, the present invention also provides a computer-readable storage medium storing a computer program, where when the computer program is executed, the steps of the method for ensuring the consistency of multi-robot arrival as described in any one of the above are implemented.

[0131] In summary, through the method, system, and storage medium for ensuring the consistency of multi-robot arrival provided by the present invention, a consistency calibration scheme is ingeniously proposed to calibrate a relative coordinate transformation relationship between the real vehicle coordinate system and the ideal vehicle coordinate system. With the action of this coordinate transformation, it can support multiple robots of the same specification. Even if there are inconsistencies in the installation positions of the positioning devices, when going to each working station, accurate docking point navigation positions can be obtained based on the same reference map after coordinate transformation, so that the docking mechanism of the robot and the docking mechanism of the target working station can be accurately docked.

[0132] In addition, based on the calibration scheme of this solution, when establishing N working stations, it can also support N robots to simultaneously complete the establishment of all working stations at one time, without separately establishing N working stations for each robot. Therefore, the implementation workload is greatly reduced, the implementation cycle is shortened, and it can ensure that multi-robots can meet the consistency docking accuracy requirements when facing the docking mechanism of the same working station.

[0133] The preferred embodiments of the present invention disclosed above are only used to help illustrate the present invention. The preferred embodiments do not describe all the details in detail, nor do they limit the invention to the specific embodiments described. Obviously, many modifications and changes can be made according to the content of this specification. These embodiments are selected and specifically described in this specification to better explain the principles and practical applications of the present invention, so that those skilled in the art can well understand and utilize the present invention. The present invention is only limited by the claims and their full scope and equivalents. Any modifications, equivalent replacements, improvements, etc. made within the spirit and principle of the present invention shall be included within the protection scope of the present invention.

[0134] Those skilled in the art can understand that in addition to implementing the systems, devices, units, and their respective modules provided by the present invention in the form of pure computer-readable program codes, the method steps can be logically programmed to enable the systems, devices, units, and their respective modules provided by the present invention to be implemented in the form of logic gates, switches, application-specific integrated circuits, programmable logic controllers, and embedded microcontrollers, etc. to achieve the same program. Therefore, the systems, devices, and their respective modules provided by the present invention can be regarded as a kind of hardware components, and the modules included therein for implementing various programs can also be regarded as the structures within the hardware components; the modules for implementing various functions can also be regarded as both software programs for implementing methods and the structures within the hardware components.

[0135] In addition, all or part of the steps in the methods of the above embodiments can be completed by a program instructing relevant hardware. The program is stored in a storage medium, including several instructions for enabling a single-chip microcomputer, a chip, or a processor to execute all or part of the steps of the methods described in various embodiments of the present application. The aforementioned storage medium includes: USB flash drives, mobile hard disks, read-only memories (ROMs), random access memories (RAMs), magnetic disks, or optical discs, etc., which can store program codes.

[0136] In addition, any combinations can be made between various different embodiments of the present invention as long as they do not violate the idea of the embodiments of the present invention, and they should also be regarded as the content disclosed in the embodiments of the present invention.

Claims

1. A method for ensuring the consistency of multi-robot arrival, the steps including: Step S1: Establish a calibration site and calculate the transformation matrix of the positioning poses of each robot in the reference map The steps include: Step S11: Set a marker a at the docking point A of the calibration site; set a marker b at the robot docking point B; set an identification device in the scene to cover the scene where the markers a and b are located; Step S12: When the robot recognizes the marker a, obtain the pose of the docking point A in the reference map coordinate system, use it as the calibration site, plan a calibration navigation path, and make the robot go there; The recognition device in step S13 recognizes the marker b, and obtains the pose and its transformation matrix of the docking point B in the external recognition coordinate system L ; Step S14: The recognition device recognizes the marker a, determines that the robot coincides with the calibration site, regards the position of the robot as the real vehicle coordinate system R, and obtains the pose and its transformation matrix of the robot in the external recognition coordinate system L ; Step S15 determines the pose of docking point B and its transformation matrix in the ideal vehicle coordinate system I according to the installation position of docking point B on the robot , where the calibration site on the reference map is the ideal vehicle coordinate system I; Step S16 calculates , and obtains the transformation matrix of the pose of the real vehicle coordinate system R under the ideal vehicle coordinate system I ; Step S2: Establish a working station, and respectively calculate the poses and their transformation matrices of the robot and the working station in the reference map coordinate system map , The steps include: Step S21: Debug the robot and move its docking point to coincide with the working site; Step S22 records the pose of the current robot in the reference map coordinate system map and its transformation matrix as , and correspondingly records the pose of this work site and its transformation matrix in the reference map as ; Step S3: When the robot navigates to the work site each robot calculates to update generate a new destination for navigation and instruct the robot to go there. ​ 2. The method for ensuring the consistency of multi-robot arrival according to claim 1, wherein the steps further include: Step S23 combines the poses of the workstations obtained by all robots and their transformation matrices in the same reference map.

3. The method for ensuring the consistency of multi-robot arrival according to claim 1, wherein the identification device is: a laser identification device, and the markers a and b are: reflective column markers.

4. The method for ensuring the consistency of multi-robot arrival according to claim 1, wherein the identification device is: an image identification device, and the corresponding markers a and b are: any one of a graphic marker and a QR code marker.

5. The method for ensuring the consistency of multi-robot arrival according to claim 1, wherein the markers a and b are respectively arranged in pairs on both sides of the docking point A and the docking point B, and respectively maintain a preset positional layout relationship with the docking point A and the docking point B.

6. The method for ensuring the consistency of multi-robot arrival according to claim 1, wherein the calculation steps of the change matrix in step S1 include: Step S11: Set a marker a at the docking point A of the calibration site; Set a pre-site near the calibration site; Set a marker b at the robot docking point B; set an identification device in the scene to cover the scene where the markers a and b are located; Step S12: When the robot recognizes the marker a, obtain the pose of the docking point A in the reference map coordinate system, use it as the calibration site, plan a calibration navigation path from the pre-site to go there, and make the robot go there; Step S13: The recognition device recognizes marker b, and obtains the pose of docking point B in the external recognition coordinate system L and its transformation matrix ; In step S14, the recognition device recognizes the marker a, determines that the robot coincides with the calibration site, and regards the position of the robot as the real vehicle coordinate system R, so as to obtain the pose and its transformation matrix of the robot in the external recognition coordinate system L ; Step S15 determines the pose of docking point B in the ideal vehicle coordinate system I and its transformation matrix according to the installation position of docking point B on the robot , where the calibration site on the reference map is the ideal vehicle coordinate system I; Step S16 calculates , and obtains the transformation matrix of the pose of the real vehicle coordinate system R under the ideal vehicle coordinate system I .

7. A system for ensuring the consistency of multi-robot arrival, comprising: A memory, a processor, and a computer program stored in the memory and executable on the processor, wherein when the processor executes the computer program, the system implements the steps of the method for ensuring the consistency of multi-robot arrival according to any one of claims 1 to 6.

8. A computer-readable storage medium storing a computer program, wherein when the computer program is executed, the steps of the method for ensuring the consistency of multi-robot arrival according to any one of claims 1 to 6 are implemented.

Images