A method for indoor positioning of swarm robots based on active beacon

Abstract

The application discloses an indoor positioning method for a swarm robot based on an active beacon, divides the swarm robot into a navigation robot and a working robot, projects a customized optical beacon on the top of a working space by the navigation robot, the optical beacon has at least two key points and can form a beacon coordinate system as a global reference coordinate system through the key points, the working robot vertically observes the optical beacon through a visual positioning module arranged on the top of the working robot and perpendicular to the top of the working space, and the position and direction of the working robot in the global reference coordinate system are calculated according to the image information of the optical beacon, so that the autonomous positioning is realized. The upward projection method is used to effectively solve the mutual shielding problem of the robots, the navigation robot actively projects a specially designed optical beacon pattern on the ceiling, and the autonomous positioning of the swarm robot is realized, so that the problem that the global situation information of the swarm robot cannot be obtained in the indoor positioning of the large-scale swarm robot is effectively solved.

Description

Technical Field

[0001] This invention relates to the field of swarm robot positioning technology, specifically to an indoor positioning method for swarm robots based on active beacons. Background Technology

[0002] Achieving autonomous localization of swarm robots in indoor environments is a prerequisite for their successful execution of specific indoor tasks. Existing autonomous localization methods for swarm robots primarily rely on infrared sensors or vision to obtain relative positional information between individuals, then integrate this relative distance information using algorithms. However, due to the layout of infrared sensors and wall diffuse reflection, these localization methods suffer from low angular resolution and are susceptible to ambient light interference, facing the problem of mutual interference of infrared signals in large-scale swarms. Furthermore, these methods can fail due to mutual occlusion between robots, a problem particularly pronounced in large-scale, dense swarms.

[0003] 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. Summary of the Invention

[0004] To address the problems existing in the prior art, this invention proposes an indoor positioning method for swarm robots based on active beacons. In this method, the swarm robots are divided into navigation robots and working robots. The navigation robots have an upward projection device that can project specific optical beacons onto the top of the workspace to provide auxiliary positioning services for the working robots. The working robots have an upward visual positioning module that can achieve autonomous positioning by observing the optical beacons.

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

[0006] An indoor localization method for swarm robots based on active beacons includes the following steps:

[0007] Step 1: The navigation robot projects a customized optical beacon onto the top of the workspace; the optical beacon has at least two key points and can form a beacon coordinate system as a global reference coordinate system through the key points;

[0008] Step 2: The working robot uses a visual positioning module mounted on its top, with the optical axis perpendicular to the top of the workspace, to observe the optical beacon mentioned in Step 1. Based on the image information of the optical beacon, it calculates its own position and orientation in the global reference coordinate system, thus achieving autonomous positioning.

[0009] 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 global reference coordinate system.

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

[0011] 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.

[0012] 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;

[0013] 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;

[0014] 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 YC Coordinates in;

[0015] 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 global 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 global reference coordinate system.

[0016] Furthermore, in step 2.1, the camera's optical center coincides with the robot's motion center.

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

[0018] 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;

[0019] 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.

[0020] 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.

[0021] 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(un ,v n ).

[0022] 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:

[0023]

[0024]

[0025] The robot's heading angle is:

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

[0027] Beneficial effects

[0028] This invention proposes an indoor positioning method for swarm robots based on active beacons. 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 the direction reference) onto the ceiling to assist the swarm robots in achieving autonomous positioning. This effectively solves the problem of not being able to obtain accurate global situational information of the group in indoor positioning of large-scale swarm robots.

[0029] 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

[0030] 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:

[0031] Figure 1 This is a flowchart of an indoor localization method for swarm robots based on active beacons.

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

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

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

[0035] 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.

[0036] In this embodiment, 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 workspace to provide auxiliary positioning services for the working robots. The working robots have an upward visual positioning module with an optical axis perpendicular to the top of the workspace, which can achieve autonomous positioning by observing the optical beacons.

[0037] Based on the above functional classification, the indoor localization method for swarm robots based on active beacons specifically includes the following steps:

[0038] Step 1: The navigation robot projects a customized optical beacon onto the ceiling above the workspace. The optical beacon has at least two key points and can form a beacon coordinate system as a global reference coordinate system through these key points.

[0039] like Figure 2As 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 global 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.

[0040] 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.

[0041] Step 2: The working robot uses a visual positioning module mounted on its top, with the optical axis perpendicular to the top of the workspace, to observe the optical beacon mentioned in Step 1. Based on the optical beacon information, it calculates its own position and orientation in the global reference coordinate system, thus achieving autonomous positioning.

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

[0043] 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.

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

[0045] 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.

[0046] 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.

[0047] 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 ).

[0048] 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.

[0049] 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:

[0050] 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:

[0051]

[0052] 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.

[0053] 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:

[0054]

[0055] In equation (2) (u k ,v k ) and (x k (y, k, h) represent 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:

[0056]

[0057] 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).

[0058] 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 ).

[0059] 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 O C 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.

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

[0061]

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

[0063] 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.:

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

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

[0066] In this way, the position and heading of the working robots in the global reference coordinate system are calculated. All working robots are calculated in a unified global reference coordinate system and then aggregated to the central controller, which can accurately and quickly obtain the group situation information.

[0067] 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. An indoor localization method for swarm robots based on active beacons, characterized in that: The swarm robots are divided into navigation robots and working robots. The navigation robots are equipped with upward projection devices, which can vertically project specific optical beacons onto the top of the workspace. The optical beacons consist of three circles arranged in an isosceles triangle. A beacon coordinate system is established with the midpoint M of the base of the triangular beacon as the origin. The beacon coordinate system's X-axis is perpendicular to the base of the triangular beacon, with its positive direction pointing towards the vertex N of the triangular beacon; points M and N are key points; the beacon coordinate system is the global reference coordinate system. The working robot has an upward-looking visual positioning module with its optical axis perpendicular to the top of the workspace, enabling it to capture images of the top of the workspace. The method includes the following steps: Step 1: The navigation robot projects a customized optical beacon onto the top of the workspace; Step 2: The robot, using its top-mounted optical axis perpendicular to the top of the workspace and viewed through its upward-looking visual positioning module, observes the optical beacon described in Step 1. Based on the optical beacon image information, it calculates its own position and orientation in the global reference coordinate system, achieving autonomous positioning. The specific process of calculating its own position and orientation in the global 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. 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 beacons in the image and obtain the coordinates of the key points of the optical beacons in the image coordinate system; the process of identifying optical beacons in the image includes beacon vertex detection and beacon information extraction steps: 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: A 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 their coordinates in the image coordinate system. Specifically, the template matching method is used to process all possible beacon vertices obtained in the beacon vertex detection stage to obtain the three vertices of the optical beacon. The base, sides, and coordinates of the three vertices N, J, and Q in the image coordinate system are determined by calculating the pairwise distances between the three vertices. According to the optical beacon design scheme, 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 image coordinate system, the coordinates of the optical beacon keypoints M and N in the image coordinate system are obtained as follows: and ; Step 2.3: Based on the camera pinhole imaging model, determine the coordinates of the optical beacon key points M and N in the image coordinate system. 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; Step 2.4: Project the camera coordinate system onto the plane containing the optical beacon to obtain a two-dimensional coordinate system. The camera's optical center is in the coordinate system Projection position in Two-dimensional coordinate system Origin, obtain the optical beacon key points M and N in two-dimensional coordinate system The coordinates in the middle are divided into and ; Step 2.5: Based on the key points of the optical beacon in the two-dimensional coordinate system 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 global reference coordinate system. for: Two-dimensional coordinate system The rotation angle relative to the beacon coordinate system is the robot's heading angle in the global reference coordinate system. : 。 2. The indoor positioning method for swarm robots based on active beacons according to claim 1, characterized in that: In step 2.1, the optical center of the camera coincides with the center of motion of the robot.

3. The indoor positioning method for swarm robots based on active beacons according to claim 1, 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.

Images