A high-precision repositioning method and system for swarm robots based on error compensation

Abstract

The application discloses a high-precision repositioning method and system for a cluster robot based on error compensation, and the positioning method comprises the following steps: obtaining a homogeneous transformation matrix from a scanner to the end of a mechanical arm; taking a point to be moved to by the scanner as a target point, obtaining a transformation matrix of the target point to a corresponding tracker space according to a homogeneous transformation matrix of the tracker to a global space in a static state; performing space division on the tracker space to obtain a mechanical arm base position space, and driving a chassis to move into the mechanical arm base position space; obtaining a homogeneous transformation matrix of the target point at the mechanical arm base, obtaining a six-dimensional vector of the homogeneous transformation matrix of the target point at the mechanical arm base, and moving the scanner to the target point through the mechanical arm; constructing a least square problem according to the homogeneous transformation matrix of the target point at the mechanical arm base, and iteratively solving to obtain an optimized homogeneous transformation matrix from the scanner to the tracker. The application solves the problem of accurate positioning in a large workspace.

Description

Technical Field

[0001] This invention relates to the field of robot positioning technology, and in particular to a high-precision repositioning method and system for swarm robots based on error compensation. Background Technology

[0002] As a key component of an aircraft's aerodynamic shape and structural stress, the assembly quality of aircraft skin directly affects flight safety, performance, and lifespan. Skin inspection and assembly are characterized by their large area, numerous components, and high precision requirements for mating. Taking a large passenger aircraft as an example, its skin is composed of numerous panels, involving tens of thousands of fasteners and connection points. Currently, skin assembly quality inspection mainly relies on manual labor. However, as aerospace manufacturing develops towards higher quality and efficiency, the requirements for skin mating precision, flatness, and the compliance of fastener installation are increasingly stringent, making traditional manual inspection methods insufficient to meet the demands of modern production.

[0003] Secondly, before final assembly and delivery of the aircraft, a comprehensive inspection of the skin's appearance and joints is required to ensure there are no assembly defects. However, many of these inspection points are highly similar, making them difficult to distinguish with the naked eye. When faced with a large number of similar inspection points, inspectors are prone to judgment fatigue, leading to low inspection efficiency and difficulty in ensuring consistency and accuracy. This inefficient inspection mode not only prolongs the aircraft delivery cycle but also increases labor costs. Furthermore, the large number and wide distribution of aircraft skin inspection points, coupled with prolonged continuous work, easily leads to eye fatigue and distraction. Even experienced inspectors cannot avoid errors and omissions in high-intensity, long-term inspection work. Any oversight at any inspection point can potentially create structural or aerodynamic hazards, affecting the overall safety and reliability of the aircraft. These problems indicate that traditional manual inspection methods are no longer adequate for the stringent precision and efficiency requirements of modern aerospace manufacturing when faced with a wide range of highly similar inspection tasks, becoming a significant factor restricting the quality and delivery schedule of aircraft skin assembly. Summary of the Invention

[0004] This invention provides a high-precision repositioning method and system for swarm robots based on error compensation, in order to solve the technical problems mentioned in the background art.

[0005] To achieve the above objectives, the technical solution of the present invention is implemented as follows:

[0006] This invention provides a high-precision relocalization method for swarm robots based on error compensation, comprising the following steps:

[0007] S1. Mark points on the work site, and fit the homogeneous transformation matrix from the scanner to the tracker based on the position of the marked points. Then, solve the homogeneous transformation matrix from the scanner to the end of the robotic arm based on this homogeneous transformation matrix.

[0008] S2. Set up a global space and obtain the homogeneous transformation matrix from the tracker to the global space in a static state;

[0009] S3. Take the point that the scanner needs to move to as the target point p. Based on the homogeneous transformation matrix from the tracker to the global space in the static state, transform the target point p in the global space to the corresponding tracker space to obtain the transformation matrix from the target point p to the corresponding tracker space.

[0010] S4. Divide the space of the tracker to obtain the position space of the robotic arm base, and drive the chassis to move into the position space of the robotic arm base;

[0011] S5. Transfer the transformation matrix from the target point p to the corresponding tracker space to the robotic arm space to obtain the homogeneous transformation matrix of the target point p on the robotic arm base. Obtain the six-dimensional vector of the homogeneous transformation matrix of the target point p on the robotic arm base, and move the scanner to the target point p through the robotic arm according to the six-dimensional vector.

[0012] S6. Based on the homogeneous transformation matrix of the target point p on the robot arm base, construct a least-squares problem and solve iteratively to obtain the optimized homogeneous transformation matrix from the scanner to the tracker. This completes the relocation of the cluster robots.

[0013] Furthermore, step S1 specifically includes the following steps:

[0014] S11. Markings are made on the work site using a scanner. The tracker obtains the scanner's marked points when it looks at the scanner. The tracker acquires the positions of the scanner's marked points and fits the homogeneous transformation matrix from the scanner to the tracker based on these positions. ;

[0015] S12. Based on the scanner coordinates measured by the tracker and the end-effector coordinates given by the robotic arm, calculate the homogeneous transformation matrix from the scanner to the end-effector of the robotic arm. ;

[0016] S12 specifically includes the following steps:

[0017] S121. First, drive the robotic arm to move N times, where N>3; given N sets of homogeneous transformation matrices from the end effector of the robotic arm to the base of the robotic arm. And N sets of homogeneous transformation matrices from scanners to trackers ;

[0018] S122. Construct the homogeneous transformation matrix from the tracker to the robotic arm base. The constraints to be satisfied are as follows:

[0019] ;

[0020] Wherein, the homogeneous transformation matrix Homogeneous transformation matrix The calculation formulas are as follows:

[0021] ;

[0022] ;

[0023] in, , Let each represent a homogeneous transformation matrix. , of Rotation matrix, , Let each represent a homogeneous transformation matrix. The translation position;

[0024] S123. Using the camera calibration algorithm Tsai, obtain the optimal homogeneous transformation matrix that satisfies the constraints in S122. The specific calculation formula is as follows:

[0025] ;

[0026] in, This represents the homogeneous transformation matrix that needs to be calculated. of Rotation matrix, This represents the homogeneous transformation matrix that needs to be calculated. of Displacement matrix;

[0027] S124. Based on the optimal homogeneous transformation matrix Solving for the homogeneous transformation matrix from the scanner to the robotic arm's end effector yields the solution. The calculation formula is as follows:

[0028] .

[0029] Furthermore, step S2 specifically includes the following steps:

[0030] S21. For markers or features at the work site, a global space is pre-defined. In global space Scan the interior to obtain a point set. ;

[0031] S22, Set of Points The homogeneous transformation matrix from the currently scanned points to the global space is obtained by registering with a predefined standard model using the ICP algorithm. Then, based on the homogeneous transformation matrix Inverse kinematics is used to solve the homogeneous transformation matrix from the tracker to the global space in the static state. The specific calculation formula is as follows:

[0032] ;

[0033] in, This represents the homogeneous transformation matrix from the tracker's initial position to the global space.

[0034] Furthermore, step S4 specifically includes the following steps:

[0035] S41. The tracking space is divided into a mesh based on the robotic arm extension constraint to obtain the position space of the robotic arm base;

[0036] S42. Construct a discrete set of feasible poses for the scanner chassis on the xy plane of the robotic arm base position space, and name it the candidate pose set. In the candidate pose set Add a compensation amount to the x-axis point coordinates. Complete the construction of grid G, obtain the feasible position points of the scanner chassis on the xy plane based on grid G, and issue instructions to move the scanner chassis to the feasible position points;

[0037] S43. After the scanner chassis moves to the feasible position point, determine whether the scanner's position information is lost. If so, rotate the scanner chassis to reacquire the scanner's position information; otherwise, proceed directly to S5.

[0038] Furthermore, step S41 specifically includes the following steps:

[0039] S411. Determine the cell step size S for mesh generation based on the diameter of the robotic arm. The calculation formula is as follows:

[0040] ;

[0041] in, Indicates the diameter of the robotic arm; Therefore The side length of the square whose diameter is the inscribed circle; Indicates the safety margin factor;

[0042] S412. Determine the extent of grid G, including the effective working area of ​​grid G ​​and the starting point of grid G;

[0043] S413. Within the range of the grid G ​​defined in S412, generate a series of points along the x-axis with a unit step size S. The number of points... for:

[0044] ;

[0045] in, , These represent the x-axis coordinates of the end point and the start point of grid G, respectively.

[0046] Based on quantity Generate a series of point coordinates on the x-axis, as follows:

[0047] ;

[0048] in, This represents the coordinates of the i-th point on the x-axis;

[0049] S414. Within the range of the grid G ​​defined in S412, generate a series of points along the y-axis with a unit step size S. The number of points... for:

[0050] ;

[0051] in, , These represent the y-axis coordinates of the end point and the start point of grid G, respectively;

[0052] Based on quantity Generate a series of point coordinates on the y-axis, as follows:

[0053] ;

[0054] in, This represents the coordinates of the j-th point on the y-axis;

[0055] A series of point coordinates generated on the x and y axes constitute a two-dimensional space, which is used as the position space of the robotic arm base.

[0056] Furthermore, step S42 specifically includes the following steps:

[0057] S421. Based on a series of point coordinates generated on the x and y axes, construct a discrete set of feasible poses for the scanner chassis in the xy plane, and name it the candidate pose set. The calculation formula is as follows:

[0058] ;

[0059] S422, in the candidate pose set Add a compensation amount to the x-axis point coordinates. This ensures that the robotic arm base remains between the scanner and the tracker at the given position, preventing the robotic arm from obstructing the scanner, thus completing the construction of mesh G, as shown in the following expression:

[0060] ;

[0061] S423. Construct all feasible location points of the scanner chassis on the xy plane based on grid G. The expression is as follows:

[0062] ;

[0063] S424, at all feasible locations Select one of the feasible locations and issue a command to move the scanner chassis to that location.

[0064] Furthermore, step S5 specifically includes the following steps:

[0065] S51. Transfer the transformation matrix from the target point p to the corresponding tracker space to the robotic arm space, and based on the homogeneous transformation matrix from the robotic arm base to the tracker... Obtain the homogeneous transformation matrix of target point p on the robot arm base. ;

[0066] Homogeneous transformation matrix from robotic arm base to tracker The calculation formula is as follows:

[0067] ;

[0068] in, Represents the homogeneous transformation matrix from the end effector of the robotic arm to the base of the robotic arm;

[0069] Homogeneous transformation matrix of target point p on the robot arm base The calculation formula is as follows:

[0070] ;

[0071] S52. Based on the homogeneous transformation matrix from the target point p to the robot arm base. And the homogeneous transformation matrix from the scanner to the robotic arm end effector. Calculate the homogeneous transformation matrix from the end effector to the robot arm base when the scanner reaches the target point p. The calculation formula is as follows:

[0072] ;

[0073] S53. Obtain the homogeneous transformation matrix from the target point p to the base of the robotic arm. A six-dimensional vector {x,y,z,A,B,C}, where the homogeneous transformation matrix is... The expression for the rotation matrix is ​​as follows:

[0074] ;

[0075] in, Represents the homogeneous transformation matrix The rotation matrix, The parameter in the u-th row and v-th column of the rotation matrix is ​​represented, and u and v take values ​​from 1 to 3; A, B, and C represent the Euler angles in the x, y, and z axes, respectively.

[0076] Homogeneous transformation matrix The Euler angles are (A, B, C), and the formulas for calculating A, B, C are as follows:

[0077] ;

[0078] ;

[0079] ;

[0080] S54. Using a six-dimensional vector {x,y,z,A,B,C} and calling the robotic arm displacement interface, the robotic arm is displaced to the target point, i.e., the homogeneous transformation matrix. The corresponding point.

[0081] Furthermore, step S6 specifically includes the following steps:

[0082] S61. Transform the homogeneous matrix As the target pose, the homogeneous transformation matrix from the target point p to the robot arm base. As the current pose; construct rotation and translation errors based on the target pose and the current pose;

[0083] The formula for calculating rotational error is as follows:

[0084] ;

[0085] in, express Rotation error in the form of a rotation matrix; The rotation error is represented in the form of a rotation vector; T represents the transpose of the matrix. Indicates the current rotation matrix; Represents the rotation angle of the rotation vector; This represents the element in the 3rd row and 3rd column within the rotation error.

[0086] The formula for calculating translation error is as follows:

[0087] ;

[0088] in, Indicates translation error. Representing the target matrix The position vector; Represents the current position vector;

[0089] Based on rotational error and translation error Construct the current pose error value The calculation formula is as follows:

[0090] ;

[0091] S62, Set a pose increment This makes the current pose error value Add pose increment Then, the current pose error value Reduce; based on pose increment and pose increment The least squares problem is constructed as follows:

[0092] ;

[0093] Where L represents the total error of the k-th iteration; Let represent the geometric Jacobian matrix of the robotic arm's end effector. The geometric Jacobian matrix represents a six-dimensional vector {x,y,z,A,B,C}.

[0094] S63. Solve the least squares problem to obtain the final output, which is the optimized homogeneous transformation matrix from scanner to tracker. This completes the relocation of the cluster robots.

[0095] Furthermore, S63 specifically includes the following steps:

[0096] S631. Use the obtained pose increment Update the current pose estimate To obtain the pose estimation for the next round. The calculation formula is as follows:

[0097] ;

[0098] in, This represents mapping a 6-dimensional vector to a 4x4 three-dimensional rigid body Lie algebraic motion matrix;

[0099] S632, Estimate the pose for the next round. Updated to the end effector of the robotic arm; pose estimation at this point. Updated to the homogeneous transformation matrix from scanner to tracker. ;

[0100] S633, Calculate the updated error ,if If the error is less than the preset error threshold, stop the iteration and obtain the final output; otherwise, let... Return to S63 and continue iterating until the updated pose error value is less than the preset error threshold; after convergence, the final output is the optimized homogeneous transformation matrix from scanner to tracker. The calculation formula is as follows:

[0101] .

[0102] The present invention also discloses a swarm robot system that uses a high-precision repositioning method for swarm robots for positioning, including two omnidirectional mobile vehicles, a tracker, a scanner and a robotic arm;

[0103] The scanner is mounted on one of the omnidirectional mobile units via a robotic arm and is used to scan the target object; the tracker is mounted on another omnidirectional mobile unit and is used to track and acquire the scanning trajectory of the scanner and the movement trajectory of the omnidirectional mobile unit at the bottom of the scanner. The scanner chassis is located at the bottom of the omnidirectional mobile unit on which the scanner is located.

[0104] The beneficial effects of this invention are:

[0105] 1. The high-precision repositioning method for swarm robots disclosed in this invention integrates calibration and continuous error compensation mechanisms. These mechanisms mean that after the initial calibration, no further calibration is required, enabling continuous observation, continuous compensation, and continuous movement. Traditional methods typically separate system calibration from online control, leading to the continuous accumulation of calibration errors during operation. This invention breaks this traditional paradigm by integrating system calibration parameters (including the homogeneous transformation matrix) into the online control system. Or homogeneous transformation matrix ) and real-time observation data (including homogeneous transformation matrix) Homogeneous transformation matrix This method involves iterative optimization. Instead of relying on a one-time, offline, precise calibration, it treats calibration as a continuous online process, dynamically compensating for and correcting the system's inherent calibration errors (referring to errors introduced by calibration), chassis errors, robotic arm errors, and pose uncertainties caused by chassis movement through iterative calculations.

[0106] 2. This invention breaks through the limitations of traditional workspace and greatly expands spatial capabilities. The successful application of this method makes the working range of the scanner fixed at the end of the robotic arm no longer limited to its own field of view, but extended to any point in the entire global space that overlaps with the visible area of ​​the tracker.

[0107] Furthermore, this invention, combined with a tracker station, provides the scanner with an efficient and precise global operating space, enabling the scanner to operate across its entire global space.

[0108] 3. This invention does not rely on the absolute precision of the scanner chassis, because the robotic arm only needs to move to the grid G ​​to reach the corresponding position. It does not require high precision; rather, the position after displacement is calculated. This provides better support for the robotic arm's end-effector pose arrival. Traditional systems heavily rely on the high-precision positioning of the moving chassis; any drift or slippage of the chassis will directly lead to end-effector operation failure. This method completely reduces the stringent requirements for chassis precision.

[0109] Furthermore, this invention achieves true "visual servoing": the working logic of this invention is transformed from "open-loop control based on absolute coordinates" to "closed-loop feedback control based on real-time observation", which makes the swarm robot system have extremely high fault tolerance and robustness to chassis errors.

[0110] Among them, open-loop control based on absolute coordinates refers to making the end effector of the robotic arm reach the relevant position based on only one observation, without actively correcting the position of the end effector of the robotic arm subsequently.

[0111] Closed-loop feedback control based on real-time observation refers to continuously tracking the end effector of the robotic arm with a tracker, observing the position error between the end effector and the target point p in real time, and continuously correcting the trajectory of the end effector based on the position error until the end effector reaches the accurate position.

[0112] 4. This invention designs an efficient "instruction-observation-iteration" closed-loop convergence process, namely the iterative process in S6. Furthermore, this invention designs an extremely efficient automated process for the iterative process, namely, each time a better pose estimate is calculated... It immediately issues commands to drive the robotic arm and then obtains the edge transformation matrix via a tracker. This serves as the input for the next iteration. This closed-loop process ensures continuous error convergence and self-correction of the swarm robot system.

[0113] Error continues to converge: Each iteration makes the end effector approach the target point, and through multiple iterations, the final accuracy far exceeds the accuracy of a single motion.

[0114] Self-calibration of swarm robot systems: Automatically shields nonlinear factors such as kinematic calibration errors, backlash, and deformation of the robotic arm, achieving a higher level of "hand-eye coordination".

[0115] 5. The swarm robot system disclosed in this invention utilizes a high-precision repositioning method for swarm robots to achieve positioning, realizing the dynamic expansion of the robotic arm's workspace to the full field of view of the scanner, effectively solving the bottleneck problems of limited workspace for high-precision robotic arms and insufficient absolute positioning accuracy of the mobile chassis. Attached Figure Description

[0116] Figure 1 This is the positioning method of the high-precision repositioning method for swarm robots in this invention;

[0117] Figure 2 This is a schematic diagram of the swarm robot mechanism in an embodiment of the present invention. Detailed Implementation

[0118] To facilitate understanding of the present invention, a more complete description will be given below with reference to the accompanying drawings. Preferred embodiments of the invention are shown in the drawings. However, the invention can be implemented in many other different forms and is not limited to the embodiments described herein. Rather, these embodiments are provided to provide a thorough and complete understanding of the disclosure of the invention.

[0119] It should be noted that when a component is referred to as "fixed" or "set" on another component, it can be directly on or indirectly on the other component. When a component is referred to as "connected" to another component, it can be directly connected to or indirectly connected to the other component.

[0120] It should be understood that the terms "length", "width", "upper", "lower", "front", "rear", "left", "right", "vertical", "horizontal", "top", "bottom", "inner", "outer", etc., indicate the orientation or positional relationship based on the orientation or positional relationship shown in the accompanying drawings. They 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, or be constructed and operated in a specific orientation. Therefore, they should not be construed as limitations on the present invention.

[0121] Furthermore, the terms "first" and "second" are used for descriptive purposes only and should not be construed as indicating or implying relative importance or implicitly specifying the number of technical features indicated. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of that feature. In the description of this invention, "a plurality of" means two or more, unless otherwise explicitly specified.

[0122] Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention pertains. The terminology used herein in the specification of this invention is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention. The term "and / or" as used herein includes any and all combinations of one or more of the associated listed items.

[0123] It should also be noted that in the embodiments of this application, the same reference numerals are used to represent the same component or part. For the same part in the embodiments of this application, the reference numerals may only be used to mark one part or component as an example. It should be understood that the reference numerals are also applicable to other identical parts or components.

[0124] Reference Figure 1 and Figure 2 This application provides a high-precision relocalization method for swarm robots based on error compensation, comprising the following steps:

[0125] S1. Mark points on the work site, and fit the homogeneous transformation matrix from the scanner to the tracker based on the position of the marked points. Then, solve the homogeneous transformation matrix from the scanner to the end of the robotic arm based on this homogeneous transformation matrix.

[0126] S2. Set up a global space and obtain the homogeneous transformation matrix from the tracker to the global space in a static state;

[0127] S3. Take the point that the scanner needs to move to as the target point p. Based on the homogeneous transformation matrix from the tracker to the global space in the static state, transform the target point p in the global space to the corresponding tracker space to obtain the transformation matrix from the target point p to the corresponding tracker space.

[0128] S4. Divide the space of the tracker to obtain the position space of the robotic arm base, and drive the chassis to move into the position space of the robotic arm base;

[0129] S5. Transfer the transformation matrix from the target point p to the corresponding tracker space to the robotic arm space to obtain the homogeneous transformation matrix of the target point p on the robotic arm base. Obtain the six-dimensional vector of the homogeneous transformation matrix of the target point p on the robotic arm base, and move the scanner to the target point p through the robotic arm according to the six-dimensional vector.

[0130] S6. Based on the homogeneous transformation matrix of the target point p on the robot arm base, construct a least-squares problem and solve iteratively to obtain the optimized homogeneous transformation matrix from the scanner to the tracker. This completes the relocation of the cluster robots.

[0131] In some embodiments, S1 specifically includes the following steps:

[0132] S11. Markings are made on the work site using a scanner. The tracker obtains the scanner's marked points when it looks at the scanner. The tracker acquires the positions of the scanner's marked points and fits the homogeneous transformation matrix from the scanner to the tracker based on these positions. ;

[0133] S12. Based on the scanner coordinates measured by the tracker and the end-effector coordinates given by the robotic arm, calculate the homogeneous transformation matrix from the scanner to the end-effector of the robotic arm. .

[0134] In some embodiments, S12 specifically includes the following steps:

[0135] S121. First, drive the robotic arm to move N times, where N>3; given N sets of homogeneous transformation matrices from the end effector of the robotic arm to the base of the robotic arm. And N sets of homogeneous transformation matrices from scanners to trackers ;

[0136] S122. Construct the homogeneous transformation matrix from the tracker to the robotic arm base. The constraints to be satisfied are as follows:

[0137] ;

[0138] Wherein, the homogeneous transformation matrix Homogeneous transformation matrix The calculation formulas are as follows:

[0139] ;

[0140] ;

[0141] in, , Let each represent a homogeneous transformation matrix. , of Rotation matrix, , Let each represent a homogeneous transformation matrix. The translation position;

[0142] S123. Using the camera calibration algorithm Tsai, obtain the optimal homogeneous transformation matrix that satisfies the constraints in S122. The specific calculation formula is as follows:

[0143] ;

[0144] in, This represents the homogeneous transformation matrix that needs to be calculated. of Rotation matrix This represents the homogeneous transformation matrix that needs to be calculated. of Displacement matrix;

[0145] S124. Based on the optimal homogeneous transformation matrix Solving for the homogeneous transformation matrix from the scanner to the robotic arm's end effector yields the solution. The calculation formula is as follows:

[0146]

[0147] In some embodiments, S2 specifically includes the following steps:

[0148] S21. For markers or features at the work site, a global space is pre-defined. In global space Scan the interior to obtain a point set. ;

[0149] S22, Set of Points The homogeneous transformation matrix from the currently scanned points to the global space is obtained by registering with a predefined standard model (such as a standard model of the skin or a standard model of the wing) using the ICP (Iterative Closest Point) algorithm. Then, based on the homogeneous transformation matrix Inverse kinematics is used to solve the homogeneous transformation matrix from the tracker to the global space in the static state. The specific calculation formula is as follows:

[0150]

[0151] in, This represents the homogeneous transformation matrix from the tracker's initial position to the global space.

[0152] In some embodiments, S3 specifically includes the following steps:

[0153] S31. Based on the homogeneous transformation matrix from the tracker to the global space in the static state. Take the point that the scanner needs to move to as the target point p, and obtain the homogeneous transformation matrix from the target point p to the global space. ;

[0154] S32. Determine the homogeneous transformation matrix Whether it is in the initial tracker space; if so, then according to the homogeneous transformation matrix. Transform the target point p to the initial tracker space to obtain the homogeneous transformation matrix from target point p to the tracker. If not, arrange a transfer station to obtain the homogeneous transformation matrix from target point p to the k-th tracker space. The calculation formula is as follows:

[0155]

[0156] in, Let represent the homogeneous transformation matrix from the (i+1)th tracker space to the ith tracker space; Let represent the homogeneous transformation matrix from the i-th tracker space to the (i-1)-th tracker space; This represents the homogeneous transformation matrix from the first tracker space to the initial tracker space; This represents the homogeneous transformation matrix from the initial tracker space to the global space.

[0157] In some embodiments, S4 specifically includes the following steps:

[0158] S41. The tracking space is divided into a mesh based on the robotic arm extension constraint to obtain the position space of the robotic arm base;

[0159] S42. Construct a discrete set of feasible poses for the scanner chassis on the xy plane of the robotic arm base position space, and name it the candidate pose set. In the candidate pose set Add a compensation amount to the x-axis point coordinates. Complete the construction of grid G, obtain the feasible position points of the scanner chassis on the xy plane based on grid G, and issue instructions to move the scanner chassis to the feasible position points;

[0160] S43. After the scanner chassis moves to the feasible position point, determine whether the scanner's position information is lost. If so, rotate the scanner chassis to reacquire the scanner's position information. Theoretically, after rotating one full circle, the scanner can always be found in a certain direction. Once found, stop rotating; otherwise, proceed directly to S5.

[0161] In some embodiments, S41 specifically includes the following steps:

[0162] S411. Determine the cell step size S for mesh generation based on the diameter of the robotic arm. The calculation formula is as follows:

[0163] ;

[0164] in, Indicates the diameter of the robotic arm; Therefore The side length of the square whose diameter is the inscribed circle; In this embodiment, the safety margin factor represents the safety margin coefficient. The value is 2 / 3;

[0165] S412. Determine the extent of grid G, including the effective working area of ​​grid G ​​and the starting point of grid G;

[0166] Specifically, let the effective working area of ​​the tracker in the xy plane be:

[0167] ;

[0168] The grid starting point can be set to ( );

[0169] S413. Within the range of the grid G ​​defined in S412, generate a series of points along the x-axis with a unit step size S. The number of points... for:

[0170] ;

[0171] in, , These represent the x-axis coordinates of the end point and the start point of grid G, respectively.

[0172] Based on quantity Generate a series of point coordinates on the x-axis, as follows:

[0173] ;

[0174] in, This represents the coordinates of the i-th point on the x-axis;

[0175] S414. Within the range of the grid G ​​defined in S412, generate a series of points along the y-axis with a unit step size S. The number of points... for:

[0176] ;

[0177] in, , These represent the y-axis coordinates of the end point and the start point of grid G, respectively;

[0178] Based on quantity Generate a series of point coordinates on the y-axis, as follows:

[0179] ;

[0180] in, This represents the coordinates of the j-th point on the y-axis;

[0181] A series of point coordinates generated on the x and y axes constitute a two-dimensional space, which is used as the position space of the robotic arm base.

[0182] In some embodiments, S42 specifically includes the following steps:

[0183] S421. Based on a series of point coordinates generated on the x and y axes, construct a discrete set of feasible poses for the scanner chassis in the xy plane, and name it the candidate pose set. The calculation formula is as follows:

[0184] ;

[0185] S422, in the candidate pose set Add a compensation amount to the x-axis point coordinates. This ensures that the robotic arm base remains between the scanner and the tracker at the given position, preventing the robotic arm from obstructing the scanner, thus completing the construction of mesh G, as shown in the following expression:

[0186] ;

[0187] S423. Construct all feasible location points of the scanner chassis on the xy plane based on grid G. The expression is as follows:

[0188] ;

[0189] S424, at all feasible locations Select one of the feasible locations and issue a command to move the scanner chassis to that location.

[0190] The mapping relationship between the robotic arm's motion point space and the chassis is as follows:

[0191] ;

[0192] in This represents a cell space (8 points describe a cuboid space), meaning that when the robotic arm's end effector is given a position in this space, the chassis needs to be displaced to the corresponding position. , Indicates the mapping symbol. Allowable chassis tolerances. Specifically, the current diameter of the robotic arm When the height reaches 1.7 meters, the robotic arm allows for a chassis error of approximately 0.79 meters.

[0193] In some embodiments, S5 specifically includes the following steps:

[0194] S51. Transfer the transformation matrix from the target point p to the corresponding tracker space to the robotic arm space, and based on the homogeneous transformation matrix from the robotic arm base to the tracker... Obtain the homogeneous transformation matrix of target point p on the robot arm base. ;

[0195] Homogeneous transformation matrix from robotic arm base to tracker The calculation formula is as follows:

[0196] ;

[0197] in, Represents the homogeneous transformation matrix from the end effector of the robotic arm to the base of the robotic arm;

[0198] Homogeneous transformation matrix of target point p on the robot arm base The calculation formula is as follows:

[0199] ;

[0200] S52. Based on the homogeneous transformation matrix from the target point p to the robot arm base. And the homogeneous transformation matrix from the scanner to the robotic arm end effector. Calculate the homogeneous transformation matrix from the end effector to the robot arm base when the scanner reaches the target point p. The calculation formula is as follows:

[0201] ;

[0202] Alternatively, S51 can be omitted, and the homogeneous transformation matrix from the target point p to the base of the robotic arm can be calculated directly. The calculation formula is as follows:

[0203] ;

[0204] S53. Obtain the homogeneous transformation matrix from the target point p to the robot arm base. A six-dimensional vector {x,y,z,A,B,C}, where the homogeneous transformation matrix is... The expression for the rotation matrix is ​​as follows:

[0205] ;

[0206] in, Represents the homogeneous transformation matrix The rotation matrix, The parameter in the u-th row and v-th column of the rotation matrix; u and v both take values ​​from 1 to 3; A, B, and C represent the Euler angles in the x, y, and z axes, respectively.

[0207] Homogeneous transformation matrix The Euler angles are (A, B, C), and the formulas for calculating A, B, C are as follows:

[0208] ;

[0209] ;

[0210] ;

[0211] Where A, B, and C represent Euler angles; Indicates the angle of rotation about the y-axis;

[0212] S54. Using a six-dimensional vector {x,y,z,A,B,C} and calling the robotic arm displacement interface, the robotic arm is displaced to the target point, i.e., the homogeneous transformation matrix. The corresponding point.

[0213] In some embodiments, S6 specifically includes the following steps:

[0214] S61. Transform the homogeneous matrix As the target pose, the homogeneous transformation matrix from the target point p to the robot arm base. As the current pose; construct rotation and translation errors based on the target pose and the current pose;

[0215] The formula for calculating rotational error is as follows:

[0216] ;

[0217] in, express Rotation error in the form of a rotation matrix; The rotation error is represented in the form of a rotation vector; T represents the transpose of the matrix. Indicates the current rotation matrix; Represents the rotation angle of the rotation vector; This represents the element in the 3rd row and 3rd column within the rotation error.

[0218] The formula for calculating translation error is as follows:

[0219] ;

[0220] in, Indicates translation error. Representing the target matrix The position vector; Represents the current position vector;

[0221] Based on rotational error and translation error Construct the current pose error value The calculation formula is as follows:

[0222] ;

[0223] S62, Set a pose increment (one (a vector), such that the current pose error value Add pose increment Then, the current pose error value Reduce; based on pose increment and pose increment The least squares problem is constructed as follows:

[0224] ;

[0225] Where L represents the total error of the k-th iteration; The geometric Jacobian matrix (a six-dimensional vector {x,y,z,A,B,C}) represents the end effector of the robotic arm.

[0226] S63. Solve the least squares problem to obtain the final output, which is the optimized homogeneous transformation matrix from scanner to tracker. This completes the relocation of the cluster robots.

[0227] In some embodiments, S63 specifically includes the following steps:

[0228] S631. Use the obtained pose increment Update the current pose estimate To obtain the pose estimation for the next round. The calculation formula is as follows:

[0229] ;

[0230] in, This represents mapping a 6-dimensional vector to a 4x4 three-dimensional rigid body Lie algebraic motion matrix;

[0231] S632, Estimate the pose for the next round. Updated to the end effector of the robotic arm; pose estimation at this point. Updated to the homogeneous transformation matrix from scanner to tracker. ;

[0232] S633, Calculate the updated error ,if If the error is less than the preset error threshold, stop the iteration and obtain the final output; otherwise, let... Return to S63 and continue iterating until the updated pose error value is less than the preset error threshold; after convergence, the final output is the optimized homogeneous transformation matrix from scanner to tracker. The calculation formula is as follows:

[0233] .

[0234] In another aspect, the present invention provides a swarm robot system that uses a high-precision repositioning method for swarm robots for positioning, including two omnidirectional mobile vehicles, a tracker, a scanner, and a robotic arm;

[0235] The scanner is mounted on one of the omnidirectional mobile units via a robotic arm and is used to scan the target object; the tracker is mounted on another omnidirectional mobile unit and is used to track and acquire the scanning trajectory of the scanner and the movement trajectory of the omnidirectional mobile unit at the bottom of the scanner. The scanner chassis is located at the bottom of the omnidirectional mobile unit on which the scanner is located.

[0236] The above description is merely a specific embodiment of the present invention, but the scope of protection of the present invention is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the technical scope disclosed in the present invention should be included within the scope of protection of the present invention. Furthermore, the technical solutions of the various embodiments of the present invention can be combined with each other, but this must be based on the ability of those skilled in the art to implement them. When the combination of technical solutions is contradictory or cannot be implemented, it should be considered that such a combination of technical solutions does not exist and is not within the scope of protection claimed by the present invention. Therefore, the scope of protection of the present invention should be determined by the scope of the claims.

Claims

1. A high-precision relocalization method for swarm robots based on error compensation, characterized in that, Includes the following steps: S1. Mark points on the work site, and fit the homogeneous transformation matrix from the scanner to the tracker based on the position of the marked points. Then, solve the homogeneous transformation matrix from the scanner to the end of the robotic arm based on this homogeneous transformation matrix. S2. Set up a global space and obtain the homogeneous transformation matrix from the tracker to the global space in a static state; S3. Take the point that the scanner needs to move to as the target point p. Based on the homogeneous transformation matrix from the tracker to the global space in the static state, transform the target point p in the global space to the corresponding tracker space to obtain the transformation matrix from the target point p to the corresponding tracker space. S4. Divide the space of the tracker to obtain the position space of the robotic arm base, and drive the chassis to move into the position space of the robotic arm base; S5. Transfer the transformation matrix from the target point p to the corresponding tracker space to the robotic arm space to obtain the homogeneous transformation matrix of the target point p on the robotic arm base. Obtain the six-dimensional vector of the homogeneous transformation matrix of the target point p on the robotic arm base, and move the scanner to the target point p through the robotic arm according to the six-dimensional vector. S6. Based on the homogeneous transformation matrix of the target point p on the robot arm base, construct a least-squares problem and solve iteratively to obtain the optimized homogeneous transformation matrix from the scanner to the tracker. This completes the relocation of the cluster robots; S2 specifically includes the following steps: S21. For markers or features at the work site, a global space is pre-defined. In global space Scan the interior to obtain a point set. ; S22, Set of Points The homogeneous transformation matrix from the currently scanned points to the global space is obtained by registering with a predefined standard model using the ICP algorithm. Then, based on the homogeneous transformation matrix Inverse kinematics is used to solve the homogeneous transformation matrix from the tracker to the global space in the static state. The specific calculation formula is as follows: in, This represents the homogeneous transformation matrix from the tracker's initial position to the global space; S4 specifically includes the following steps: S41. The tracking space is divided into a mesh based on the robotic arm extension constraint to obtain the position space of the robotic arm base; S42. Construct a discrete set of feasible poses for the scanner chassis on the xy plane of the robotic arm base position space, and name it the candidate pose set. In the candidate pose set Add a compensation amount to the x-axis point coordinates. Complete the construction of grid G, obtain the feasible position points of the scanner chassis on the xy plane based on grid G, and issue instructions to move the scanner chassis to the feasible position points; S43. After the scanner chassis moves to the feasible position point, determine whether the scanner's position information is lost. If so, rotate the scanner chassis to reacquire the scanner's position information; otherwise, proceed directly to S5. S41 specifically includes the following steps: S411. Determine the cell step size S for mesh generation based on the diameter of the robotic arm. The calculation formula is as follows: in, Indicates the diameter of the robotic arm; Therefore The side length of the square whose diameter is the inscribed circle; Indicates the safety margin factor; S412. Determine the extent of grid G, including the effective working area of ​​grid G ​​and the starting point of grid G; S413. Within the range of the grid G ​​defined in S412, generate a series of points along the x-axis with a unit step size S. The number of points... for: in, , These represent the x-axis coordinates of the end point and the start point of grid G, respectively. Based on quantity Generate a series of point coordinates on the x-axis, as follows: in, This represents the coordinates of the i-th point on the x-axis; S414. Within the range of the grid G ​​defined in S412, generate a series of points along the y-axis with a unit step size S. The number of points... for: in, , These represent the y-axis coordinates of the end point and the start point of grid G, respectively; Based on quantity Generate a series of point coordinates on the y-axis, as follows: in, This represents the coordinates of the j-th point on the y-axis; A series of point coordinates generated on the x and y axes constitute a two-dimensional space, which is used as the position space of the robotic arm base.

2. The high-precision relocation method for cluster robots based on error compensation according to claim 1, characterized in that, S1 specifically includes the following steps: S11. Markings are made on the work site using a scanner. The tracker obtains the scanner's marked points when it looks at the scanner. The tracker acquires the positions of the scanner's marked points and fits the homogeneous transformation matrix from the scanner to the tracker based on these positions. ; S12. Based on the scanner coordinates measured by the tracker and the end-effector coordinates given by the robotic arm, calculate the homogeneous transformation matrix from the scanner to the end-effector of the robotic arm. ; S12 specifically includes the following steps: S121. First, drive the robotic arm to move N times, where N>3; given N sets of homogeneous transformation matrices from the end effector of the robotic arm to the base of the robotic arm. And N sets of homogeneous transformation matrices from scanners to trackers ; S122. Construct the homogeneous transformation matrix from the tracker to the robotic arm base. The constraints to be satisfied are as follows: ； Wherein, the homogeneous transformation matrix Homogeneous transformation matrix The calculation formulas are as follows: in, , Let each represent a homogeneous transformation matrix. , A 3x3 rotation matrix, , Let each represent a homogeneous transformation matrix. , The translation position; S123. Using the camera calibration algorithm Tsai, obtain the optimal homogeneous transformation matrix that satisfies the constraints in S122. The specific calculation formula is as follows: in, This represents the homogeneous transformation matrix that needs to be calculated. A 3x3 rotation matrix, This represents the homogeneous transformation matrix that needs to be calculated. A 1*3 displacement matrix; S124. Based on the optimal homogeneous transformation matrix Solving for the homogeneous transformation matrix from the scanner to the robotic arm's end effector yields the solution. The calculation formula is as follows: 。 3. The high-precision relocation method for swarm robots based on error compensation according to claim 2, characterized in that, S42 specifically includes the following steps: S421. Based on a series of point coordinates generated on the x and y axes, construct a discrete set of feasible poses for the scanner chassis in the xy plane, and name it the candidate pose set. The calculation formula is as follows: ； S422, in the candidate pose set Add a compensation amount to the x-axis point coordinates. This ensures that the robotic arm base remains between the scanner and the tracker at the given position, preventing the robotic arm from obstructing the scanner, thus completing the construction of mesh G, as shown in the following expression: ； S423. Construct all feasible location points of the scanner chassis on the xy plane based on grid G. The expression is as follows: ； S424, at all feasible locations Select one of the feasible locations and issue a command to move the scanner chassis to that location.

4. The high-precision relocation method for swarm robots based on error compensation according to claim 3, characterized in that, S5 specifically includes the following steps: S51. Transfer the transformation matrix from the target point p to the corresponding tracker space to the robotic arm space, and based on the homogeneous transformation matrix from the robotic arm base to the tracker... Obtain the homogeneous transformation matrix of target point p on the robot arm base. ; Homogeneous transformation matrix from robotic arm base to tracker The calculation formula is as follows: in, Represents the homogeneous transformation matrix from the end effector of the robotic arm to the base of the robotic arm; Homogeneous transformation matrix of target point p on the robot arm base The calculation formula is as follows: ； S52. Based on the homogeneous transformation matrix from the target point p to the robot arm base. And the homogeneous transformation matrix from the scanner to the robotic arm end effector. Calculate the homogeneous transformation matrix from the end effector to the robot arm base when the scanner reaches the target point p. The calculation formula is as follows: ； S53. Obtain the homogeneous transformation matrix from the target point p to the base of the robotic arm. A six-dimensional vector {x,y,z,A,B,C}, where the homogeneous transformation matrix is... The expression for the rotation matrix is ​​as follows: ； in, Represents the homogeneous transformation matrix The rotation matrix, The parameter in the u-th row and v-th column of the rotation matrix is ​​represented, and u and v take values ​​from 1 to 3; A, B, and C represent the Euler angles in the x, y, and z axes, respectively. Homogeneous transformation matrix The Euler angles are (A, B, C), and the formulas for calculating A, B, C are as follows: S54. Using a six-dimensional vector {x,y,z,A,B,C} and calling the robotic arm displacement interface, the robotic arm is displaced to the target point, i.e., the homogeneous transformation matrix. The corresponding point.

5. The high-precision relocation method for swarm robots based on error compensation according to claim 4, characterized in that, S6 specifically includes the following steps: S61. Transform the homogeneous matrix As the target pose, the homogeneous transformation matrix from the target point p to the robot arm base. As the current pose; construct rotation and translation errors based on the target pose and the current pose; The formula for calculating rotational error is as follows: in ; in, Represents the rotation error in the form of a 3x3 rotation matrix; The rotation error is represented in the form of a rotation vector; T represents the transpose of the matrix. Indicates the current rotation matrix; Represents the rotation angle of the rotation vector; This represents the element in the 3rd row and 3rd column within the rotation error. The formula for calculating translation error is as follows: in, Indicates translation error. Representing the target matrix The position vector; Represents the current position vector; Based on rotational error and translation error Construct the current pose error value The calculation formula is as follows: ； S62, Set a pose increment This makes the current pose error value Add pose increment Then, the current pose error value Reduce; based on pose increment and pose increment The least squares problem is constructed as follows: L= Where L represents the total error of the k-th iteration; Let represent the geometric Jacobian matrix of the robotic arm's end effector. The geometric Jacobian matrix represents a six-dimensional vector {x,y,z,A,B,C}. S63. Solve the least squares problem to obtain the final output, which is the optimized homogeneous transformation matrix from scanner to tracker. This completes the relocation of the cluster robots.

6. The high-precision relocation method for swarm robots based on error compensation according to claim 5, characterized in that, S63 specifically includes the following steps: S631. Use the obtained pose increment Update the current pose estimate To obtain the pose estimation for the next round. The calculation formula is as follows: in, This represents mapping a 6-dimensional vector to a 4x4 three-dimensional rigid body Lie algebraic motion matrix; S632, Estimate the pose for the next round. Updated to the end effector of the robotic arm; pose estimation at this point. Updated to the homogeneous transformation matrix from scanner to tracker. ; S633, Calculate the updated error ,if If the error is less than the preset error threshold, stop the iteration and obtain the final output; otherwise, let... Return to S63 and continue iterating until the updated pose error value is less than the preset error threshold; after convergence, the final output is the optimized homogeneous transformation matrix from scanner to tracker. The calculation formula is as follows: 。

Images