Indoor large-scale cooperative positioning method for heterogeneous cluster robots

Abstract

The application provides an indoor large-range cooperative positioning method for heterogeneous cluster robots, the cluster robots are divided into navigation robots and working robots, the navigation robots have upward projection devices and can project specific optical beacons to the top of a task scene space to provide auxiliary positioning services for the working robots; the navigation robots and the working robots both have upward visual positioning modules, the working robots can realize autonomous positioning by observing the optical beacons; and considering the problem of limited coverage of the optical beacons under the requirement of indoor large-range cooperative positioning, a motion control method of the navigation robots is provided; further considering the problem that the existence of a convex on a roof projection surface can cause distortion of the optical beacons, corresponding projection position optimization logic is provided.

Description

Technical Field

[0001] This invention relates to the field of robot positioning technology, and in particular to a method for large-scale indoor collaborative positioning of heterogeneous swarm robots. Background Technology

[0002] Achieving autonomous positioning of swarm robots inside buildings is a prerequisite for their successful execution of specific indoor tasks.

[0003] Because building exterior walls shield electromagnetic signals, satellite positioning indoors is highly unstable and can even fail completely. Therefore, mainstream indoor positioning methods typically employ WiFi, ultrasonic, infrared, or visual positioning. However, when using infrared sensors or vision to obtain relative positional information and then integrating this information with algorithms, the angular resolution is low due to the layout of infrared sensors and wall diffuse reflection. This method is also susceptible to ambient light interference and faces the problem of mutual interference between infrared signals in large-scale swarms. Ultrasonic positioning offers high accuracy and long range, but it also faces the challenge of slow signal attenuation, especially in enclosed spaces. When applied to large-scale robot swarms, repetitive ultrasonic signals can cause the technology to fail. Furthermore, the size and weight of ultrasonic sensors are too large for centimeter-level robots, making this solution unsuitable for swarm robots from a hardware installation perspective.

[0004] To address this, researchers have proposed an upward projection method. For example, Chinese patent application CN112601060A discloses an active shared projection surface perception system and method for desktop swarm robots. In this method, each robot projects an isosceles triangle with an inner circle onto a top projection board. The robot acquires images of all the markers within its top field of view using CMOS vision sensors. By identifying the position and direction of all the markers in the image, the relative position and heading of its neighbors relative to itself are calculated. However, this method has the following problems: the coordinates and heading calculated by each robot are in its own local coordinate system, and there is no correlation between the coordinate systems of the robots. This means that when the swarm robots are performing a task, the central controller cannot know the overall position and heading of the swarm robots in the global coordinate system, thus hindering effective overall decision-making and control.

[0005] Furthermore, in large-scale indoor scenarios, we also found that due to the limited indoor height, the upward projection method has a limited coverage area for the projected markers. If a fisheye camera is used to expand the image acquisition range, severe image distortion will occur, leading to positioning failure. In addition, when the indoor ceiling area is large, the top plane often has various beams, decorations, lights, and pipes, etc. If the projected markers are projected onto these objects, image distortion will also occur, leading to positioning failure. Summary of the Invention

[0006] To address the problems of existing technologies, this invention proposes a large-scale indoor collaborative localization method for heterogeneous swarm robots. In this method, the swarm robots are divided into navigation robots and working robots. The navigation robot has an upward projection device that can project specific optical beacons onto the top of the task scene space, providing auxiliary localization services for the working robots. Both the navigation robot and the working robots have upward visual localization modules, with the working robots able to achieve autonomous localization by observing the optical beacons. Furthermore, considering the limited coverage of optical beacons in large-scale indoor collaborative localization, a motion control method for the navigation robot is proposed. Finally, considering the problem of optical beacon distortion caused by protrusions on the roof projection surface, corresponding projection position optimization logic is proposed.

[0007] The technical solution of this invention is as follows:

[0008] A method for large-scale indoor cooperative localization of heterogeneous swarm robots includes the following steps:

[0009] Step 1: In an indoor task scenario, the navigation robot selects the first desired projection position and projects a customized optical beacon onto the top of the task scenario space from the first desired projection position; the optical beacon has at least two key points and can form a beacon coordinate system as a reference coordinate system through the key points;

[0010] Step 2: The working robot observes the optical beacon mentioned in Step 1 through a visual positioning module mounted on its top, with the optical axis perpendicular to the top of the task scene space and looking upwards. Based on the image information of the optical beacon, it calculates its own position and orientation in the reference coordinate system to achieve autonomous positioning.

[0011] Step 3: The navigation robot collects the positioning information broadcast by all the working robots. During the movement of the working robots while performing their tasks, the navigation robot calculates the furthest distance between itself and the working robots. Then the robot stops moving, among which This is a preset service radius threshold;

[0012] Step 4: Based on the movement of the working robot, the navigation robot selects the next desired projection position and moves to the next desired projection position to project a customized optical beacon onto the top of the task scene space; then the working robot observes the optical beacon through the visual positioning module mounted on its top, and calculates its own position and orientation in the reference coordinate system based on the optical beacon image information to achieve autonomous positioning.

[0013] Step 5: Repeat steps 3 and 4 until the robot completes its task.

[0014] Furthermore, in step 4, the navigation robot selects the nth desired projection position. The steps are as follows: The current position of the navigation robot Current centroid of the workforce And the current average orientation of the workforce of robots Sure:

[0015]

[0016] Furthermore, in step 4, during the process of the navigation robot moving towards the nth desired projection position, the nth desired projection position is optimized through the following process, where n≥2:

[0017] Step 4.1: During the movement of the navigation robot towards the nth desired projection position, the navigation robot calculates the distance d between its own position and the nth desired projection position. t If d t <d set 2, then it is considered that the navigation robot has moved to the vicinity of the nth desired projection position; d set2 The preset distance threshold;

[0018] Step 4.2: When the navigation robot has moved to the vicinity of the nth desired projection position, the navigation robot stops moving and then optimizes the nth desired projection position using a combination of static and dynamic methods:

[0019] When the navigation robot is stationary, it projects a detection pattern onto the top of the task scene space and observes the projected pattern through a visual positioning module mounted on its top, whose optical axis is perpendicular to the top of the task scene space and looks upward. The navigation robot judges whether there is distortion in the observed projected pattern and whether there is a protrusion in the top area. It makes a judgment based on the protrusion detection result Flag1 at the previous moment, the protrusion detection result Flag2 at the current moment, and the cumulative protrusion amount m. Here, Flag1=0 indicates no protrusion, Flag1=1 indicates a protrusion, and the initial value of Flag1 is 1. Flag2=0 indicates no protrusion, Flag2=1 indicates a protrusion, and the initial value of m is 0.

[0020] When Flag2 = 1 and Flag1 = 1, then m = m + 1. When m is odd, the navigation robot selects a random direction as the direction of movement at the current moment. When m is even, the navigation robot moves along the direction closest to the nth desired projection position.

[0021] If Flag2 = 0, then m = 0, and the navigation robot will move in the direction closest to the nth desired projection position;

[0022] If Flag2 = 1 and Flag1 = 0, then m = 0, the navigation robot returns to the position it was at in the previous moment, and the position of the navigation robot in the previous moment is the optimized nth expected projection position, and the position optimization ends;

[0023] The position optimization ends when the navigation robot moves to the nth desired projection position.

[0024] Furthermore, in step 4, the detection pattern is a rectangular pattern, and the size of the rectangular pattern can cover the size of the optical beacon.

[0025] Furthermore, the optical beacon consists of three circles arranged in an isosceles triangle pattern. A beacon coordinate system X is established with the midpoint M of the base of the triangular beacon as the origin. B O B Y B The X-axis of the beacon coordinate system is perpendicular to the base of the triangular beacon, and its positive X-axis direction points to the vertex N of the triangular beacon; points M and N are key points; the beacon coordinate system is the reference coordinate system.

[0026] Furthermore, in step 2, the process of calculating its own position and orientation in the reference coordinate system based on the optical beacon image information is as follows:

[0027] Step 2.1: First, establish the camera coordinate system: Using the optical center of the camera in the vision positioning module mounted on the top of the robot as the origin, establish the camera coordinate system X. C Y C Z C The positive Y-axis of this coordinate system is aligned with the robot's direction of motion, and the Z-axis coincides with the camera's optical axis.

[0028] Step 2.2: Based on the optical beacon image information, identify the optical beacon in the image and obtain the coordinates of the key points of the optical beacon in the pixel coordinate system;

[0029] Step 2.3: Based on the camera pinhole imaging model, transform the coordinates of the optical beacon key points in the pixel coordinate system to the camera coordinate system;

[0030] Step 2.4: Project the camera coordinate system onto the plane containing the optical beacon to obtain the two-dimensional coordinate system X.C O C Y C The camera's optical center is in coordinate system X. C O C Y C Projection position in C R is a two-dimensional coordinate system X C O C Y C Origin, obtain the key points of the optical beacon in the two-dimensional coordinate system X. C O C Y C Coordinates in;

[0031] Step 2.5: Based on the key points of the optical beacon in the two-dimensional coordinate system X C O C Y C The coordinates in the middle are used to deduce the result. C The coordinates of R in the beacon coordinate system represent the robot's position in the reference coordinate system; the two-dimensional coordinate system X... C O C Y C The rotation angle relative to the beacon coordinate system is the robot's heading angle in the reference coordinate system.

[0032] Furthermore, in step 2.2, the process of identifying optical beacons in the image includes beacon vertex detection and beacon information extraction:

[0033] Beacon vertex detection: The image acquired by the visual positioning module is processed into grayscale, and beacon vertex detection is performed based on Canny operator edge detection and Hough transform circle recognition to obtain all possible beacon vertices in the robot's field of vision;

[0034] Beacon information extraction: The template matching method is used to process all possible beacon vertices obtained in the beacon vertex detection stage to obtain the key points of the optical beacon and the coordinates of the key points in the pixel coordinate system.

[0035] Furthermore, in the beacon vertex detection stage, a pixel threshold for the circular vertex of the optical beacon is set according to the size of the projection component of the navigation robot. If the radius of the circle obtained by edge detection based on the Canny operator and circle recognition based on Hough transform is smaller than the pixel threshold, the circle is regarded as noise and deleted.

[0036] Furthermore, in the beacon information extraction stage, a template matching method is used to process all possible beacon vertices obtained in the beacon vertex detection stage, resulting in three vertices of the optical beacon. The base, sides, and the coordinates of vertices N, J, and Q in the pixel coordinate system are determined by calculating the pairwise distances between these three vertices. According to the optical beacon design, the origin M of the beacon coordinate system is the midpoint of the base of the triangular beacon, and M = (J + Q) / 2. Pointing in the positive X-axis direction Pointing in the positive Y-axis direction, based on the coordinates of the three vertices in the pixel coordinate system, the coordinates of the optical beacon keypoints M and N in the pixel coordinate system are obtained as follows: P M(u m ,v m )and P N(u n ,v n ).

[0037] Furthermore, in step 2.3, based on the camera pinhole imaging model, the coordinates of the optical beacon key points M and N in the pixel coordinate system are... P M(u m ,v m )and P N(u n ,v n Transform to camera coordinate system C M(x m ,y m ,h) and C N(x n ,y n ,h), where h is the perpendicular distance from the camera's optical center to the plane containing the optical beacon; in step 2.4, points M and N are in the two-dimensional coordinate system X C O C Y C The coordinates in the middle are divided into C M(x m ,y m )and C N(x n ,y n In step 2.5, the robot's position in the beacon coordinate system is... B R(x r ,x r )for:

[0038]

[0039]

[0040] The robot's heading angle is:

[0041] heading=arctan2(y n -y m ,x n -x m ).

[0042] Beneficial effects

[0043] This invention proposes a large-scale indoor collaborative localization method for heterogeneous swarm robots. It effectively solves the problem of mutual occlusion between robots by using upward projection. Furthermore, the swarm robots are divided into navigation robots and working robots. The navigation robots actively project specially designed optical beacon patterns (including the origin of the reference coordinate system and direction reference) onto the ceiling to assist the swarm robots in achieving autonomous localization. This effectively solves the problem of not being able to obtain accurate global situational information for large-scale swarm robots in indoor localization. After obtaining the global situational information, the navigation robots move to the desired working area of ​​the swarm according to the next stage of the swarm's task requirements and reproject the optical beacon, further achieving full coverage of the large working area. Moreover, it can optimize the projection position for special situations such as protrusions on the ceiling.

[0044] Additional aspects and advantages of the invention will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the invention. Attached Figure Description

[0045] The above and / or additional aspects and advantages of the present invention will become apparent and readily understood from the description of the embodiments taken in conjunction with the following drawings, in which:

[0046] Figure 1 This is a flowchart of the method.

[0047] Figure 2 For the beacon coordinate system X B O B Y B .

[0048] Figure 3 For the camera coordinate system X C Y C Z C .

[0049] Figure 4 This represents the correspondence between the beacon coordinate system and the camera coordinate system. Detailed Implementation

[0050] The embodiments of the present invention are described in detail below. These embodiments are exemplary and intended to explain the present invention, and should not be construed as limiting the present invention.

[0051] In this embodiment, the swarm robots are divided into navigation robots and working robots. The navigation robots have an upward projection device that can vertically project specific optical beacons to the top of the task scene space to provide auxiliary positioning services for the working robots. Both the navigation robots and the working robots have a visual positioning module with the optical axis perpendicular to the top of the task scene space for upward observation. The working robots can achieve autonomous positioning by observing the optical beacons.

[0052] Based on the above functional classification, the indoor large-scale collaborative localization method for heterogeneous swarm robots specifically includes the following steps:

[0053] Step 1: In an indoor task scenario, the navigation robot selects the first desired projection position and projects a customized optical beacon onto the top of the task scenario space from the first desired projection position; the optical beacon has at least two key points and can form a beacon coordinate system as a reference coordinate system through the key points.

[0054] First expected projection position It depends on the specific task. For example, in a typical cluster formation task, the first desired projection position can be randomly selected. Cluster-coordinated transportation tasks will specify the first desired projection position.

[0055] like Figure 2 As shown, in this embodiment, an RGB spotlight module is installed on the top of the navigation robot to vertically project specific optical beacons onto the top of the workspace. The optical beacon formed in this embodiment consists of three circles arranged in an isosceles triangle. A beacon coordinate system X is established with the midpoint M of the base of the triangular beacon as the origin. B O B Y B The beacon coordinate system's X-axis is perpendicular to the base of the triangular beacon, with its positive direction pointing to vertex N of the triangular beacon. This coordinate system serves as the reference coordinate system for the autonomous localization of the swarm robots, and the projection of the origin of this coordinate system onto the ground represents the position of the navigation robot. Points M and N here are the key points in the optical beacon.

[0056] Of course, optical beacons can also take other shapes to form a pattern, as long as the key points in the beacon can be uniquely identified through the image and a global reference coordinate system can be established through the key points.

[0057] Step 2: The working robot observes the optical beacon mentioned in Step 1 through a visual positioning module mounted on its top, with the optical axis perpendicular to the top of the task scene space and looking upwards. Based on the image information of the optical beacon, it calculates its own position and orientation in the reference coordinate system, thereby achieving autonomous positioning.

[0058] Step 2.1: The working robot first establishes the camera coordinate system:

[0059] Establish the camera coordinate system X with the optical center of the camera in the vision positioning module mounted on the top of the robot as the origin. C Y C Z C The positive Y-axis of this coordinate system is aligned with the robot's motion direction, and the Z-axis coincides with the camera's optical axis. For ease of calculation, the camera's optical center is aligned with the robot's motion center.

[0060] Step 2.2: Next, the visual positioning module acquires an image of the ceiling above the workspace and identifies optical beacons in the image.

[0061] The process of identifying optical beacons here includes beacon vertex detection and beacon information extraction. In order to improve reliability, an information verification step can also be added.

[0062] Beacon Vertex Detection: The images acquired by the visual positioning module are processed into grayscale. Beacon vertex detection is performed based on Canny edge detection and Hough transform circle recognition to obtain all possible beacon vertices in the robot's field of view. To reduce the computational load of the subsequent template matching method, a pixel threshold for the circular vertices of optical beacons is set according to the size of the RGB spotlight module on the top of the navigation robot. Here, we set the pixel threshold to 3 pixels. That is, if the radius of a circle obtained by Canny edge detection and Hough transform circle recognition is less than 3 pixels, the circle is considered noise and deleted.

[0063] Beacon Information Extraction: After beacon vertex detection processing, a template matching method is used to obtain the three vertices of the optical beacon. The base, sides, and coordinates of the three vertices N, J, and Q in the pixel coordinate system are determined by calculating the pairwise distances between the three vertices. This pixel coordinate system has its origin at the top left corner of the image acquired by the visual positioning module, and its X and Y axes are the length and width directions of the image. According to the optical beacon design, the origin M of the beacon coordinate system is the midpoint of the base of the triangular beacon, i.e., M = (J + Q) / 2. Pointing in the positive X-axis direction Pointing in the positive Y-axis direction, based on the coordinates of the three vertices in the pixel coordinate system, we can obtain the coordinates of the optical beacon keypoints M and N in the pixel coordinate system as follows: P M(u m ,v m )and P N(u n ,v n ).

[0064] Information Verification: To prevent identification errors, the geometric relationship that the median and base of an isosceles triangle are perpendicular is also used to verify the identified beacon. Theoretically, if and If they are perpendicular, their inner product is zero. However, in actual beacon recognition, factors such as camera distortion, stray light interference, and blurred contours can lead to errors in beacon vertex identification. In this embodiment, the calculated... and The included angle should be appropriately widened, that is, if and If the included angle is within (80°, 100°), the two are considered perpendicular and the verification is successful; otherwise, the recognition is considered incorrect and the image is re-acquired for beacon recognition and extraction.

[0065] Step 2.3: After identifying the coordinates of the key points of the optical beacon emitted by the navigation robot in the pixel coordinate system, based on the camera pinhole imaging model, the coordinates of the key points of the optical beacon in the pixel coordinate system are transformed to the camera coordinate system to obtain the coordinates of the midpoint M of the beacon's bottom edge and the vertex N of the beacon in the camera coordinate system. C M(x m ,y m ,h) and C N(x n ,y n The specific process is as follows:

[0066] According to the pinhole camera imaging model, the correspondence between a point in the camera coordinate system and its position in the pixel coordinate system is as follows:

[0067]

[0068] In the formula Z C (u,v) represents the distance from the camera's optical center to the plane containing the imaged object; (u,v) represents the coordinates of a point in the pixel coordinate system; (X) represents the distance from the camera's optical center to the plane containing the imaged object. C ,Y C Z C ) represents the coordinates of the corresponding point in the camera coordinate system; T is the 3×3 intrinsic parameter matrix of the camera, which can be obtained using the camera calibration method; (u0, v0) is the representation of the camera optical center in the pixel coordinate system; dx and dy represent the actual physical size of a unit pixel in the X and Y directions in the pixel coordinate system, respectively; f is the focal length of the camera lens.

[0069] When the swarm of robots operates indoors, the ground and ceiling are flat and essentially parallel. Let h be the processing distance from the robot's camera optical center to the ceiling plane where the beacon is located. If the influence of ground undulations on the robot's motion is ignored, then Z... C =h is a constant. Therefore, equation (1) can be simplified to:

[0070]

[0071] In equation (2) (u k ,v k ) and (x k ,y k (h) represents the position of any point K in the pixel coordinate system and its spatial coordinates in the camera coordinate system, respectively; a x =f / dx and a y=f / dy represent the scale factors of the horizontal and vertical axes of the pixel coordinate system, respectively. Performing an inverse transformation on equation (2), we can obtain the corresponding coordinates of any point K in the pixel coordinate system in the camera coordinate system:

[0072]

[0073] Using equation (3), the pixel position identified by the visual positioning module P M(u m ,v m )and P N(u n ,v n This can be converted to the camera coordinate system, and the corresponding coordinates are as follows: C M(x m ,y m ,h) and C N(x n ,y n ,h).

[0074] Step 2.4: Since the optical axis of the visual positioning module is perpendicular to the ceiling plane, and the robot's motion center coincides with the origin of the camera coordinate system, the robot's motion can be mapped to the ceiling plane where the beacon is located, thus simplifying it to two-dimensional planar motion. The two-dimensional coordinate system X obtained by projecting the camera coordinate system onto the ceiling plane is... C O C Y C The robot's center of motion is in coordinate system X. C O C Y C Projection position in C R is the two-dimensional coordinate system X. C O C Y C The origin, points M and N are in the two-dimensional coordinate system X. C O C Y C The coordinates in the middle are divided into C M(x m ,y m )and C N(x n ,y n ).

[0075] Step 2.5: In the two-dimensional coordinate system X C O C Y C In the process, the coordinates of beacon key points M and N were obtained, while the robot's motion center was located in the two-dimensional coordinate system X. C O C Y C Projection position in C R is the two-dimensional coordinate system X. C OC Y C The origin, therefore, allows us to utilize the beacon coordinate system and the two-dimensional coordinate system X. C O C Y C By analyzing the geometric relationships between them, the position and orientation of the working robot in the beacon coordinate system (i.e., the global reference coordinate system) can be calculated.

[0076] In the beacon coordinate system, the robot's position is... B R(x r ,x r )for:

[0077]

[0078] In equation (4), · represents the inner product between vectors.

[0079] Because the robot's heading is specified to be aligned with the positive Y-axis of the camera coordinate system, i.e., aligned with the X-axis of the two-dimensional coordinate system... C O C Y C If the positive Y-axis is parallel, then the robot's heading angle is in the two-dimensional coordinate system X. C O C Y C The rotation angle relative to the beacon coordinate system, i.e.:

[0080] heading=arctan2(y n -y m ,x n -x m (5)

[0081] In equation (5), arctan2 represents the arctangent operation in the four quadrants.

[0082] Step 3: The navigation robot collects the positioning information broadcast by all the working robots. During the movement of the working robots while performing their tasks, the navigation robot calculates the furthest distance between itself and the working robots. Then the robot stops moving, among which This is a preset service radius threshold.

[0083] The field of view of a robot's vision positioning module is limited. If the size and height of the optical beacon are fixed, its effective positioning range is finite and can be determined through actual testing. However, at the edges of the field of view, the beacon imagery exhibits significant distortion, leading to beacon recognition errors. Furthermore, considering the size of the robot swarm, the actual effective positioning area (i.e., the preset service radius threshold) is also limited. The actual distance will be less than the theoretically effective positioning distance.

[0084] Step 4: Based on the movement of the working robot, the navigation robot selects the next desired projection position and moves to the next desired projection position to project a customized optical beacon onto the top of the task scene space; then the working robot observes the optical beacon through the visual positioning module mounted on its top, and calculates its own position and orientation in the reference coordinate system based on the optical beacon image information to achieve autonomous positioning.

[0085] The navigation robot selects the nth desired projection position. The steps are as follows: The current position of the navigation robot Current centroid of the workforce And the current average orientation of the workforce of robots Sure:

[0086]

[0087] Because ceilings inevitably have some protrusions, such as beams, lights, and pipes, these can negatively impact the quality of the optical beacon's projected pattern. Therefore, the navigation robot needs to determine whether the desired projection point meets the projection requirements (by default, the first desired projection position meets the requirements).

[0088] During the movement of the navigation robot towards the nth desired projection position, the nth desired projection position is optimized through the following process, where n≥2:

[0089] Step 4.1: During the movement of the navigation robot towards the nth desired projection position, the navigation robot calculates the distance d between its own position and the nth desired projection position. t If d t <d set 2, then it is considered that the navigation robot has moved to the vicinity of the nth desired projection position; d set2 The preset distance threshold is used here;

[0090] Step 4.2: When the navigation robot has moved to the vicinity of the nth desired projection position, the navigation robot stops moving and then optimizes the nth desired projection position using a combination of static and dynamic methods:

[0091] When the navigation robot is stationary, it projects a detection pattern onto the top of the task scene space. The robot then observes the projected pattern through a visual positioning module mounted on its top, whose optical axis is perpendicular to the top of the task scene space and looks upwards. The navigation robot determines whether there is any distortion in the observed projected pattern, thus identifying any protrusions in the top area. The detection pattern is rectangular, and its size is sufficient to cover the size of the optical beacon.

[0092] The judgment is made based on the bump detection result Flag1 from the previous moment, the bump detection result Flag2 from the current moment, and the cumulative bump amount m. Flag1 = 0 indicates no bump, Flag1 = 1 indicates a bump, and the initial value of Flag1 is 1. Flag2 = 0 indicates no bump, Flag2 = 1 indicates a bump, and the initial value of m is 0.

[0093] When Flag2 = 1 and Flag1 = 1, then m = m + 1. When m is odd, the navigation robot selects a random direction as the direction of movement at the current moment. When m is even, the navigation robot moves along the direction closest to the nth desired projection position.

[0094] If Flag2 = 0, then m = 0, and the navigation robot will move in the direction closest to the nth desired projection position;

[0095] If Flag2 = 1 and Flag1 = 0, then m = 0, the navigation robot returns to the position it was at in the previous moment, and the position of the navigation robot in the previous moment is the optimized nth expected projection position, and the position optimization ends;

[0096] The position optimization ends when the navigation robot moves to the nth desired projection position.

[0097] Step 5: Repeat steps 3 and 4 until the robot completes its task.

[0098] Although embodiments of the present invention have been shown and described above, it is understood that the above embodiments are exemplary and should not be construed as limiting the present invention. Those skilled in the art can make changes, modifications, substitutions and variations to the above embodiments within the scope of the present invention without departing from the principles and spirit of the present invention.

Claims

1. A method for large-scale indoor cooperative localization of heterogeneous swarm robots, characterized in that: The cluster robots are divided into navigation robots and working robots. The navigation robots have an upward projection device that can vertically project specific optical beacons onto the top of the task scene space. The working robots have a visual positioning module with an optical axis perpendicular to the top of the task scene space that allows them to observe upwards and capture images of the top of the work space. The method includes the following steps: Step 1: In an indoor task scenario, the navigation robot selects the first desired projection position and projects a customized optical beacon onto the top of the task scenario space from the first desired projection position; the optical beacon has at least two key points and can form a beacon coordinate system as a reference coordinate system through the key points; Step 2: The working robot observes the optical beacon mentioned in Step 1 through a visual positioning module mounted on its top, with the optical axis perpendicular to the top of the task scene space and looking upwards. Based on the image information of the optical beacon, it calculates its own position and orientation in the reference coordinate system to achieve autonomous positioning. Step 3: The navigation robot collects the positioning information broadcast by all the working robots. During the movement of the working robots while performing their tasks, the navigation robot calculates the furthest distance between itself and the working robots. , Then the robot stops moving, among which This is a preset service radius threshold; Step 4: Based on the movement of the working robot, the navigation robot selects the next desired projection position and moves to it, projecting a customized optical beacon onto the top of the task scene space. Then, the working robot observes the optical beacon using its top-mounted visual positioning module and calculates its position and orientation in the reference coordinate system based on the beacon image information, achieving autonomous positioning. The navigation robot selects the nth desired projection position. The steps are as follows: The current position of the navigation robot The current centroid of the workforce And the current average orientation of the workforce of robots Sure: Step 5: Repeat steps 3 and 4 until the robot completes its task.

2. The indoor large-scale cooperative positioning method for heterogeneous swarm robots according to claim 1, characterized in that: In step 4, during the process of the navigation robot moving towards the nth desired projection position, the nth desired projection position is optimized through the following process: : Step 4.1: During the movement of the navigation robot towards the nth desired projection position, the navigation robot calculates the distance between its own position and the nth desired projection position. ,like If the target position is 1, then the navigation robot is considered to have moved to the vicinity of the nth desired projection position. The preset distance threshold; Step 4.2: When the navigation robot has moved to the vicinity of the nth desired projection position, the navigation robot stops moving and then optimizes the nth desired projection position using a combination of static and dynamic methods: When the navigation robot is stationary, it projects a detection pattern onto the top of the task scene space and observes the projected pattern through a visual positioning module mounted on its top, whose optical axis is perpendicular to the top of the task scene space and looks upward. The navigation robot judges whether there is distortion in the observed projected pattern and whether there is a protrusion in the top area. It makes a judgment based on the protrusion detection result Flag1 at the previous moment, the protrusion detection result Flag2 at the current moment, and the cumulative protrusion amount m. Here, Flag1=0 indicates no protrusion, Flag1=1 indicates a protrusion, and the initial value of Flag1 is 1. Flag2=0 indicates no protrusion, Flag2=1 indicates a protrusion, and the initial value of m is 0. When Flag2 = 1 and Flag1 = 1, then m = m + 1. When m is odd, the navigation robot selects a random direction as the direction of movement at the current moment. When m is even, the navigation robot moves along the direction closest to the nth desired projection position. If Flag2=0, then m=0, and the navigation robot will move along the direction closest to the nth desired projection position; If Flag2 = 1 and Flag1 = 0, then m = 0, the navigation robot returns to the position it was at in the previous moment, and the position of the navigation robot in the previous moment is the optimized nth expected projection position, and the position optimization ends; The position optimization ends when the navigation robot moves to the nth desired projection position.

3. The indoor large-scale cooperative positioning method for heterogeneous swarm robots according to claim 2, characterized in that: In step 4, the detection pattern is a rectangular pattern, and the size of the rectangular pattern can cover the size of the optical beacon.

4. The indoor large-scale collaborative positioning method for heterogeneous swarm robots according to claim 1, characterized in that: The optical beacon consists of three circles arranged in an isosceles triangle. A beacon coordinate system X is established with the midpoint M of the base of the triangular beacon as the origin. B O B Y B The X-axis of the beacon coordinate system is perpendicular to the base of the triangular beacon, and its positive X-axis direction points to the vertex N of the triangular beacon; points M and N are key points; the beacon coordinate system is the reference coordinate system.

5. The indoor large-scale collaborative positioning method for heterogeneous swarm robots according to claim 1 or 4, characterized in that: Step 2, which involves calculating the position and orientation of the device in the reference coordinate system based on the optical beacon image information, is as follows: Step 2.1: First, establish the camera coordinate system: Using the optical center of the camera in the vision positioning module mounted on the top of the robot as the origin, establish the camera coordinate system X. C Y C Z C The positive Y-axis of this coordinate system is aligned with the robot's direction of motion, and the Z-axis coincides with the camera's optical axis. Step 2.2: Based on the optical beacon image information, identify the optical beacon in the image and obtain the coordinates of the key points of the optical beacon in the pixel coordinate system; Step 2.3: Based on the camera pinhole imaging model, transform the coordinates of the optical beacon key points in the pixel coordinate system to the camera coordinate system; Step 2.4: Project the camera coordinate system onto the plane containing the optical beacon to obtain the two-dimensional coordinate system X. C O C Y C The camera's optical center is in coordinate system X. C O C Y C Projection position in For a two-dimensional coordinate system X C O C Y C Origin, obtain the key points of the optical beacon in the two-dimensional coordinate system X. C O C Y C Coordinates in; Step 2.5: Based on the key points of the optical beacon in the two-dimensional coordinate system X C O C Y C The coordinates in the middle are used to deduce the result. The coordinates in the beacon coordinate system represent the robot's position in the reference coordinate system; the two-dimensional coordinate system X... C O C Y C The rotation angle relative to the beacon coordinate system is the robot's heading angle in the reference coordinate system.

6. The indoor large-scale cooperative positioning method for heterogeneous swarm robots according to claim 5, characterized in that: In step 2.2, the process of identifying optical beacons in the image includes beacon vertex detection and beacon information extraction: Beacon vertex detection: The image acquired by the visual positioning module is processed into grayscale, and beacon vertex detection is performed based on Canny operator edge detection and Hough transform circle recognition to obtain all possible beacon vertices in the robot's field of vision; Beacon information extraction: The template matching method is used to process all possible beacon vertices obtained in the beacon vertex detection stage to obtain the key points of the optical beacon and the coordinates of the key points in the pixel coordinate system.

7. The indoor large-scale cooperative positioning method for heterogeneous swarm robots according to claim 6, characterized in that: In the beacon vertex detection stage, a pixel threshold for the circular vertex of the optical beacon is set according to the size of the projection component of the navigation robot. If the radius of the circle obtained by edge detection based on the Canny operator and circle recognition based on Hough transform is smaller than the pixel threshold, the circle is regarded as noise and deleted.

8. The indoor large-scale cooperative positioning method for heterogeneous swarm robots according to claim 6, characterized in that: In the beacon information extraction stage, a template matching method is used to process all possible beacon vertices obtained in the beacon vertex detection stage, resulting in the three vertices of the optical beacon. The base, sides, and the coordinates of vertices N, J, and Q in the pixel coordinate system are determined by calculating the pairwise distances between these three vertices. According to the optical beacon design, the origin M of the beacon coordinate system is the midpoint of the base of the triangular beacon. , Pointing in the positive X-axis direction, Pointing in the positive Y-axis direction, based on the coordinates of the three vertices in the pixel coordinate system, the coordinates of the optical beacon keypoints M and N in the pixel coordinate system are obtained as follows: and .

9. The indoor large-scale collaborative positioning method for heterogeneous swarm robots according to claim 8, characterized in that: In step 2.3, based on the camera pinhole imaging model, the coordinates of the optical beacon key points M and N in the pixel coordinate system are determined. and Transform to camera coordinate system and Where h is the vertical distance from the camera's optical center to the plane containing the optical beacon; in step 2.4, points M and N are in the two-dimensional coordinate system X C O C Y C The coordinates in the middle are divided into and In step 2.5, the robot's position in the beacon coordinate system is... for: Robot heading angle for: 。

Images