Visually marked map pose acquisition method, robot positioning method and system

Abstract

The present invention discloses a method for obtaining a map pose of a visual marker, a robot positioning method and a system. The method for obtaining a map pose of a visual marker comprises the following steps: step 1: setting a visual marker in a robot walking space; step 2: performing laser SLAM mapping and simultaneously obtaining constraints between the robot and the set visual marker; step 3: adding the constraints between the visual robot and the set visual marker obtained in step 2 to the SLAM backend optimization problem in step 2; step 4: iteratively solving the least squares problem obtained in step 3; step 5: obtaining the optimal pose of the visual marker in the map; the present invention adopts visual markers to assist a mobile robot in mapping and positioning, thereby improving the accuracy and robustness of the robot in constructing a large map and improving the accuracy of the mobile robot's mapping.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot positioning, and in particular relates to a method for acquiring map posture of visual markers, a robot positioning method and a system. Background Art

[0002] There are three common types of maps for mobile robots: scale maps, topological maps, and semantic maps. Scale maps are widely used because they have real physical dimensions and can accurately and intuitively describe map information. Grid maps, feature maps, and point cloud maps are the three most widely used scale maps. Occupancy grid maps are often used in laser SLAM; feature maps are more commonly used in visual SLAM; and point cloud maps are often used to construct three-dimensional maps. Occupancy grid maps rasterize the map, using the state of each grid to represent obstacles in the map. The depth information from the lidar sensor is converted into a probability value in the grid, which then represents the state of the grid: occupied, free, and unknown.

[0003] Localization has become one of the most popular research areas in mobile robotics. It is a key component of SLAM technology for mobile robots and is crucial for achieving intelligent robotics. There are many approaches to robot localization, which can be broadly categorized from a sensor perspective as those based on visual sensors and laser sensors. Visual sensors can obtain rich image texture information and are relatively low-cost. However, their drawbacks are significant lighting effects, high computational complexity, and a certain degree of influence from surface texture. Laser sensors, primarily lidar, are characterized by their ability to obtain accurate depth information and easily and accurately capture environmental features surrounding the robot for localization. However, their cost and limited feature information make it prone to mislocalization due to similar features in environments with simple geometric features, such as long corridors. Using specific visual markers can significantly increase the accuracy of visual localization. By combining the visual markers with the robot's position, the robot's position within the map can be inferred. Combining these with lidar, a commonly used localization method, can overcome the limitation of lidar's limited feature information and achieve accurate localization even in the absence of environmental features.

[0004] Therefore, it is very important to obtain the position of visual markers in the map. The traditional manual map annotation method requires actual measurement of the position of visual markers in the map. Although it is relatively accurate, it is difficult to deploy, and each time the environment changes, it needs to be re-annotated, which affects the overall efficiency and even affects the positioning efficiency of robots based on visual markers. Summary of the Invention

[0005] The present invention aims to solve the problem of manually measuring the position of visual markers on a map, and further provides a method for obtaining the map pose of visual markers, as well as a robot positioning method and system. The method automatically obtains the precise pose of visual markers on a grid map and uses the obtained visual markers to continuously correct the robot's positioning during navigation, reducing positioning errors and achieving precise robot positioning.

[0006] The technical solutions provided by the present invention are as follows:

[0007] In one aspect, a method for obtaining a map pose of a visual marker includes:

[0008] Step 1: Set visual markers in the robot's walking space;

[0009] Step 2: Use the robot to move in the walking space, perform laser SLAM mapping, and obtain the constraints between the robot and the set visual markers;

[0010] Among them, the backend optimization problem used in the laser SLAM mapping part is described by the least squares problem;

[0011]

[0012] in, represents the pose of the i-th submap, represents the pose of the j-th frame radar scan, ∑ij represents the covariance matrix of the j-th frame scan pose under the ith submap, ξ ij Represents the pose of the j-th frame scanning point in the i-th sub-map coordinate system.

[0013] The constraints between the robot and the visual marker are specifically expressed as: the position of the visual marker in the robot coordinate system And by posture The covariance matrix

[0014]

[0015]

[0016] σ(a,b) represents the covariance of a and b;

[0017] Step 3: Add the constraints between the visual robot and the set visual marker obtained in step 2 to the SLAM backend optimization problem in step 2, and transform the least squares problem described in step 2 into a least squares problem with visual marker constraints;

[0018] Step 4: Iteratively solve the least squares problem obtained in step 3, using the initial position of the robot obtained by the IMU as the initial value, and obtain the unique optimal solution Ξr* , the optimal solution Ξr * Contains the optimal pose set of the visual marker in the robot coordinate system

[0019] IMU is the abbreviation of Inertial Measurement Unit, which is an inertial measurement unit;

[0020] Use the LM method to iteratively solve the least squares problem in step 3;

[0021] Step 5: Set the optimal pose of the visual marker obtained in step 4 at all times in the robot coordinate system Transformed into a set of poses of visual markers in the map through TF Then for all elements in the set Find the average value and get the best pose of the visual marker in the map

[0022] The full name of TF is Transformations Frames, which is a coordinate transformation framework;

[0023] The robot is a robot with vision and laser.

[0024] The use of visual markers to assist mobile robots in mapping and positioning improves the accuracy and robustness of robots when building large maps, reduces the frequency and complexity of loops, and improves the accuracy of mobile robot mapping.

[0025] Furthermore, the least squares problem with visual labeling constraints is as follows:

[0026]

[0027]

[0028]

[0029]

[0030] Among them, Ξm is the pose collection of the sub-map, Ξr represents the pose of the robot obtained by each sensor of the robot at the j-th frame scanning time with the map as the coordinate system, where is the position of the j-th frame radar scan with the map as the coordinate system, is the position of the visual marker at the jth frame radar scanning moment with the map as the coordinate system, The pose calculated by the odometer at the time of the j-th radar scan with the map as the coordinate system; represents the pose of the i-th submap, represents the position of the j-th frame radar scanning moment, ξij represents the position of the robot in the i-th sub-map coordinate system at the time of the j-th radar scan, and e is the vector error function, which is used to represent and The obtained robot's position in the i-th sub-map coordinate system is ij The degree of fit, ∑ij represents the covariance matrix of the j-th frame scan pose under the i-th sub-map; for The inverse matrix of the corresponding rotation matrix is, represents the translation vector in the pose of the i-th submap, It represents the translation vector of the robot's position in the map coordinate system given by each sensor at the j-th frame radar scanning moment. Represents the rotation angle around the z-axis in the pose of the i-th submap, It represents the rotation angle around the z-axis of the robot's position in the map coordinate system given by each sensor at the j-th radar scanning moment.

[0031] Furthermore, the process of obtaining the constraints between the robot and the set visual marker is as follows:

[0032] Step A1, performing distortion correction on the image information of the collected visual marker;

[0033] After the camera collects the image information of the visual marker, it describes the distortion through the first few terms of the Taylor series expansion around the center point of the image. The correction formula for radial distortion is as follows:

[0034] (x,y)=(1+k1r 2 +k2r 4 )(x0,y0)

[0035] Where (x0, y0) is the position of the image distortion point in the pixel coordinate system, (x, y) is the position after distortion correction, k1 and k2 are the radial distortion coefficients obtained in step 1 using the Zhang Zhengyou calibration method, and r is the radial distance of the image point;

[0036] The corrected point (x, y) can accurately express the length and width of the image texture.

[0037] Step A2, visual mark ID recognition by placing black and white squares;

[0038] Use the OpenCV library to detect the corner points of each black and white square, and obtain the recognized visual marker ID by comparing the recognized placement position with the marker library;

[0039] Step A3: Calculate the position and posture of the visual markers based on the length and width of the black and white squares;

[0040] Step A4: Set a pose window and obtain the pose of the visual marker within the window in the camera coordinate system;

[0041] The angle θ of the visual marker in the camera coordinate system and the position of the visual marker observed by the camera (x a ,y a ) constitutes the pose of the visual marker in the camera coordinate system Set a fixed window W c And distance limit, that is, the pose that does not meet the requirements is not used for constraint calculation. The formula is as follows:

[0042]

[0043]

[0044] Among them, x min Indicates the minimum value of the visual marker pose in the x-axis direction in the camera coordinate system, x max Indicates the maximum value of the visual marker pose in the x-axis direction in the camera coordinate system, min Indicates the minimum value of the visual marker pose in the y-axis direction in the camera coordinate system, y max Indicates the maximum value of the visual marker pose in the y-axis direction in the camera coordinate system, θ min Indicates the minimum angle of the visual marker pose around the z-axis in the camera coordinate system, θ max They represent the maximum rotation angle of the visual marker pose around the z-axis in the camera coordinate system;

[0045] Step A5: The position of the visual marker obtained through the window in the camera coordinate system Perform Kalman filter smoothing;

[0046] Prevent interference from misidentification;

[0047] Step A6: Mark the smoothed visual position in the robot coordinate system and covariance matrix as constraints.

[0048] The TF transformation operation is performed on the known robot model. The essence of TF transformation is to solve the transformation matrix between the known coordinate axes and mark the smoothed visual pose in the camera coordinate system. Transform to obtain the position of the visual marker in the robot coordinate system The TF transformation is used from the ROS operating system library.

[0049] Furthermore, the specific process of step A3 is as follows:

[0050] Step A31, calculating the distance using the camera perspective principle;

[0051] When the digital information carried by the visual marker is obtained, the distance d of the visual marker relative to the camera is calculated based on the change in the side length of the visual marker relative to the standard library and the principle of camera focal length:

[0052]

[0053] Where d is the distance between the center point of the visual marker and the camera, f is the focal length of the camera obtained during calibration, w is the known standard size of a single square of the visual marker, and s is the size of a single square of the visual marker in the pixel coordinate system;

[0054] Step A32, calculating the rotation angle by comparing the distance between the two corner points;

[0055] Based on step A31, calculate the distances from the two adjacent corner points of a black square in the same visual marker to the camera. Calculate the angle θ of the visual marker relative to the camera coordinate system based on the difference in distances:

[0056] Δd=d1-d2

[0057]

[0058]

[0059] Among them, Δd is the difference in depth distance between two adjacent corner points of a black square and the camera, d1 and d2 represent the depth distances from the corner points at different positions to the center point of the camera, and p w is the distance between the two corner points projected on the pixel plane, θ p is the angle between the two corner points and the camera center point, w r is the true distance between the two corner points obtained by the cosine theorem; θ is the angle of the visual marker in the camera coordinate system derived from the cosine formula.

[0060] On the other hand, a robot positioning method based on visual markers, when the robot navigates and loads the map, will simultaneously load the pose of the visual marker in the robot coordinate system obtained by the above-mentioned visual marker map pose acquisition method into the map, and then based on the visual marker image information obtained by the robot from the camera in real time, obtain the real-time constraint between the robot and the visual marker, and by loading the known pose of the visual marker in the map, solve the pose of the robot in the map.

[0061] Furthermore, the robot's position in the map According to the following formula:

[0062]

[0063] Where Δθ represents the direction of the visual marker in the map coordinate system and the orientation of the visual marker in the robot coordinate system The difference is the rotation angle of the robot coordinate system relative to the map coordinate system. Substituting it into the rotation matrix formula can obtain the two-dimensional rotation matrix R that transforms the map coordinate system to the robot coordinate system. Taking the inverse matrix, we can obtain the two-dimensional rotation matrix R that transforms the robot coordinate system to the map coordinate system. -1 , multiply the position of the visual marker in the robot coordinate system by Then use the position of the visual coordinate system in the map coordinate system minus The position of the robot coordinate system in the map coordinate system is obtained by reverse calculation = -Δθ, which means the robot’s position in the map is obtained

[0064] In another aspect, a system for acquiring map poses using visual markers includes:

[0065] Visual marker setting module: sets visual markers in the robot's walking space;

[0066] Mapping and constraint acquisition module: Use the robot to move in the walking space to perform laser SLAM mapping and obtain the constraints between the robot and the set visual markers;

[0067] Among them, the back-end optimization problem used in the laser SLAM mapping part is described by the least squares problem. The constraints between the robot and the visual marker are: the position of the visual marker in the robot coordinate system and by The covariance matrix

[0068] Problem conversion module: Add the constraints between the visual robot and the set visual markers obtained in the mapping and constraint acquisition module to the SLAM backend optimization problem description obtained in the mapping and constraint acquisition module, and convert the least squares problem described in the mapping and constraint acquisition module into a least squares problem with visual marker constraints;

[0069] Iterative solution module: Iteratively solves the least squares problem converted by the problem conversion module, using the initial robot pose obtained by the IMU as the initial value to obtain a unique optimal solution. The optimal solution includes the optimal pose solution set of the visual marker in the robot coordinate system;

[0070] Visual marker pose acquisition module: Based on the optimal pose set of the visual marker at all times in the robot coordinate system obtained by the iterative solution module, it is transformed into a pose set of the visual marker in the map through TF, and then all elements in the pose set are averaged to obtain the optimal pose of the visual marker in the map;

[0071] The robot is a robot with vision and laser.

[0072] In another aspect, a robot positioning system based on visual markers includes:

[0073] Map loading unit: when the robot navigates and loads the map, the position and posture of the visual marker in the robot coordinate system obtained by the above-mentioned visual marker map posture acquisition method are simultaneously loaded into the map;

[0074] Constraint acquisition unit: Based on the visual marker image information obtained by the robot from the camera in real time, the real-time constraints between the robot and the visual marker are obtained;

[0075] Posture loading and solving unit: By loading the known postures of visual markers in the map, the posture of the robot in the map is solved.

[0076] In another aspect, a computer-readable storage medium stores a computer program, wherein the computer program is invoked by a processor to implement:

[0077] The steps of the above-mentioned method for acquiring map pose of visual markers or the steps of the above-mentioned robot positioning method based on visual markers.

[0078] In another aspect, a robot includes a robot body and a camera 2, a display screen 3, an armrest 4, a laser radar 14, and an ultrasonic sensor 5 disposed on the robot body;

[0079] The camera 2, display screen 3, armrest 4, laser radar 14 and ultrasonic sensor 5 are all connected to the control module of the robot body;

[0080] The control module of the robot body applies the above-mentioned map pose acquisition method based on visual markers to obtain the map pose or applies the above-mentioned robot positioning method based on visual markers to perform positioning;

[0081] The chassis drive module of the robot adopts an independent suspension structure driven by a hub motor differential, which is connected to the robot's carrying platform through a connecting rod 9 and a spring 10. The independent suspension structure includes a first differential motor drive wheel 6 and a second differential motor drive wheel 7, and four auxiliary wheels 8 respectively installed on both sides of the chassis. The first differential motor drive wheel 6 and the second differential motor drive wheel 7 are fixed on a rotating pin 12. The four auxiliary wheels 8 are respectively fixed on the four support points of the robot's chassis. The auxiliary wheels 8 are suspended and supported on the connecting rod 9. The spring 10 connects the connecting rod 9 and the robot's chassis 11 through the rotating pin 12. The entire connecting rod supports the robot's carrying platform 13.

[0082] The drive chassis utilizes an independent suspension design with in-wheel motor differential drive. Connected to the vehicle body via connecting rods 9 and springs 10, each drive wheel is independently connected to a spring via a pivot pin. Any bouncing of any wheel due to ground impact is cushioned by springs above the wheel, ultimately distributing the vibration evenly across the supporting links without affecting the other wheels, effectively preventing tilt and vibration caused by uneven surfaces. This structure operates smoothly, compactly, and reliably. It adapts to ground height, ensuring effective contact between the drive wheels and the ground, resisting impact with obstacles and enabling multi-story elevator crossings with excellent obstacle-crossing capabilities. Furthermore, four universal wheels 8 assist in steering, providing coordinated movement and greater directional flexibility, enabling free steering even in confined spaces.

[0083] Beneficial effects

[0084] The present invention adopts cartographer as the main method of SLAM. Cartographer is a graph-based backend optimization method that can greatly reduce the impact of cumulative errors on map construction.

[0085] The present invention uses visual markers to assist mobile robots in mapping and positioning, which improves the accuracy and robustness of the robot when building large maps, reduces the frequency and complexity of loops, and improves the accuracy of mobile robot mapping.

[0086] The method for obtaining visual markers proposed in the present invention greatly reduces the requirements for the environment and the difficulty of deployment, and greatly improves the robustness of vision-assisted positioning.

[0087] The positioning method using visual markers in navigation proposed by the present invention reduces the dependence on maps and environmental features in positioning, and improves the accuracy, speed and robustness of robot positioning in navigation. BRIEF DESCRIPTION OF THE DRAWINGS

[0088] Figure 1 This is a schematic diagram of the technical concept of the robot positioning method provided by the present invention;

[0089] Figure 2Obtain visual marker diagrams for the robot;

[0090] Figure 3 This is a schematic diagram of the overall structure of the robot;

[0091] Figure 4 This is a schematic diagram of the robot chassis structure;

[0092] Figure 5 It is a coordinate diagram of the robot positioning provided by the present invention. DETAILED DESCRIPTION

[0093] The present invention will be further described below with reference to the accompanying drawings and embodiments.

[0094] Example 1

[0095] The embodiment of the technical solution of the present invention provides a method for obtaining map pose of a visual marker, such as Figure 1 and Figure 2 Shown, including:

[0096] Step 1: Set visual marker 1 in the robot walking space;

[0097] Step 2: Use the robot to move in the walking space, perform laser SLAM mapping, and obtain the constraints between the robot and the set visual markers;

[0098] Among them, the backend optimization problem used in the laser SLAM mapping part is described by the least squares problem;

[0099]

[0100] in, represents the pose of the i-th submap, represents the pose of the j-th frame radar scan, ∑ij represents the covariance matrix of the j-th frame radar scan pose under the ith submap, ξ ij Represents the pose of the j-th frame scanning point in the i-th sub-map coordinate system.

[0101] The constraints between the robot and the visual marker are specifically expressed as: the position of the visual marker in the robot coordinate system And by posture The covariance matrix

[0102]

[0103]

[0104] σ(a,b) represents the covariance of a and b;

[0105] Step 3: Add the constraints between the visual robot and the set visual marker obtained in step 2 to the SLAM backend optimization problem in step 2, and transform the least squares problem described in step 2 into a least squares problem with visual marker constraints:

[0106]

[0107]

[0108]

[0109]

[0110] Among them, Ξm is the pose collection of the sub-map, Ξr represents the pose of the robot obtained by each sensor of the robot at the j-th frame scanning time with the map as the coordinate system, where is the position of the j-th frame radar scan with the map as the coordinate system, is the position of the visual marker at the jth frame radar scanning moment with the map as the coordinate system, The pose calculated by the odometer at the time of the j-th radar scan with the map as the coordinate system; represents the pose of the i-th submap, represents the position of the j-th frame radar scanning moment, ξ ij represents the position of the robot in the i-th sub-map coordinate system at the time of the j-th radar scan, and e is the vector error function, which is used to represent and The obtained robot's position in the i-th sub-map coordinate system is ij The degree of fit, ∑ij represents the covariance matrix of the j-th frame scan pose under the i-th sub-map; for The inverse matrix of the corresponding rotation matrix is, represents the translation vector in the pose of the i-th submap, It represents the translation vector of the robot's position in the map coordinate system given by each sensor at the j-th frame radar scanning moment. Represents the rotation angle around the z-axis in the pose of the i-th submap, It represents the rotation angle around the z-axis of the robot's position in the map coordinate system given by each sensor at the j-th radar scanning moment.

[0111] Step 4: Iteratively solve the least squares problem obtained in step 3, using the initial position of the robot obtained by the IMU as the initial value, and obtain the unique optimal solution Ξr * ,Ξr * Contains the optimal pose solution set of the visual marker in the robot coordinate system

[0112] IMU is the abbreviation of Inertial Measurement Unit, which is an inertial measurement unit;

[0113] Use the LM method to iteratively solve the least squares problem in step 3;

[0114] Step 5: Set the optimal pose of the visual marker obtained in step 4 at all times in the robot coordinate system Transformed into a set of poses of visual markers in the map through TF Then for all elements in the set Find the average value and get the best pose of the visual marker in the map

[0115] The full name of TF is Transformations Frames, which is a coordinate transformation framework;

[0116] The robot is a robot with vision and laser.

[0117] The process of obtaining the constraints between the robot and the set visual markers is as follows:

[0118] Step A1, performing distortion correction on the image information of the collected visual marker;

[0119] After the camera collects the image information of the visual marker, it describes the distortion through the first few terms of the Taylor series expansion around the center point of the image. The correction formula for radial distortion is as follows:

[0120] (x,y)=(1+k1r 2 +k2r 4 )(x0,y0)

[0121] Where (x0, y0) is the position of the image distortion point in the pixel coordinate system, (x, y) is the position after distortion correction, k1 and k2 are the radial distortion coefficients obtained in step 1 using the Zhang Zhengyou calibration method, and r is the radial distance of the image point;

[0122] The corrected point (x, y) can accurately express the length and width of the image texture.

[0123] Step A2: Visual ID recognition by placing black and white squares;

[0124] Use the OpenCV library to detect the corner points of each black and white square, and obtain the recognized visual marker ID by comparing the recognized placement position with the marker library;

[0125] Step A3: Calculate the position and posture of the visual markers based on the length and width of the black and white squares;

[0126] Step A4: Set a pose window and obtain the pose of the visual marker within the window in the camera coordinate system;

[0127] The angle θ of the visual marker in the camera coordinate system and the position of the visual marker observed by the camera (x a ,y a ) constitutes the pose of the visual marker in the camera coordinate system Set a fixed window W c And distance limit, that is, the pose that does not meet the requirements is not used for constraint calculation. The formula is as follows:

[0128]

[0129]

[0130] Among them, x min Indicates the minimum value of the visual marker pose in the x-axis direction in the camera coordinate system, x max Indicates the maximum value of the visual marker pose in the x-axis direction in the camera coordinate system, min Indicates the minimum value of the visual marker pose in the y-axis direction in the camera coordinate system, y max Indicates the maximum value of the visual marker pose in the y-axis direction in the camera coordinate system, θ min Indicates the minimum angle of the visual marker pose around the z-axis in the camera coordinate system, θ max They represent the maximum rotation angle of the visual marker pose around the z-axis in the camera coordinate system;

[0131] Step A5: The position of the visual marker obtained through the window in the camera coordinate system Perform Kalman filter smoothing;

[0132] Prevent interference from misidentification;

[0133] Step A6: Mark the smoothed visual position in the robot coordinate system and covariance matrix as constraints.

[0134] The TF transformation operation is performed on the known robot model. The essence of TF transformation is to solve the transformation matrix between the known coordinate axes and mark the smoothed visual pose in the camera coordinate system. Transform to obtain the position of the visual marker in the robot coordinate system The TF transformation is used from the ROS operating system library.

[0135] The specific process of step A3 is as follows:

[0136] Step A31, calculating the distance using the camera perspective principle;

[0137] When the digital information carried by the visual marker is obtained, the distance d of the visual marker relative to the camera is calculated based on the change in the side length of the visual marker relative to the standard library and the principle of camera focal length:

[0138]

[0139] Where d is the distance between the center point of the visual marker and the camera, f is the focal length of the camera obtained during calibration, w is the known standard size of a single square of the visual marker, and s is the size of a single square of the visual marker in the pixel coordinate system;

[0140] Step A32, calculating the rotation angle by comparing the distance between the two corner points;

[0141] Based on step A31, calculate the distances from the two adjacent corner points of a black square in the same visual marker to the camera. Calculate the angle θ of the visual marker relative to the camera coordinate system based on the difference in distances:

[0142] Δd=d1-d2

[0143]

[0144]

[0145] Among them, Δd is the difference in depth distance between two adjacent corner points of a black square and the camera, d1 and d2 represent the depth distances from the corner points at different positions to the center point of the camera, and p w is the distance between the two corner points projected on the pixel plane, θ p is the angle between the two corner points and the camera center point, w r is the true distance between the two corner points obtained by the cosine theorem; θ is the angle of the visual marker in the camera coordinate system derived from the cosine formula.

[0146] Example 2

[0147] like Figure 5 As shown, a robot positioning method based on visual markers is shown. When the robot navigates and loads a map, the position of the visual marker in the robot coordinate system obtained by the above-mentioned visual marker map pose acquisition method is loaded into the map at the same time. Then, based on the visual marker image information obtained by the robot from the camera in real time, the real-time constraint between the robot and the visual marker is obtained. By loading the known position of the visual marker in the map, the position of the robot in the map is solved.

[0148] The robot's position in the map According to the following formula:

[0149]

[0150] Where Δθ represents the direction of the visual marker in the map coordinate system and the orientation of the visual marker in the robot coordinate system The difference is the rotation angle of the robot coordinate system relative to the map coordinate system. Substituting it into the rotation matrix formula can obtain the two-dimensional rotation matrix R that transforms the map coordinate system to the robot coordinate system. Taking the inverse matrix, we can obtain the two-dimensional rotation matrix R that transforms the robot coordinate system to the map coordinate system. -1 , multiply the position of the visual marker in the robot coordinate system by Then use the position of the visual coordinate system in the map coordinate system minus The position of the robot coordinate system in the map coordinate system is obtained by reverse calculation = -Δθ, which means the robot’s position in the map is obtained

[0151] Example 3

[0152] A visually marked map pose acquisition system, comprising:

[0153] Visual marker setting module: sets visual markers in the robot's walking space;

[0154] Mapping and constraint acquisition module: Use the robot to move in the walking space to perform laser SLAM mapping and obtain the constraints between the robot and the set visual markers;

[0155] Among them, the back-end optimization problem used in the laser SLAM mapping part is described by the least squares problem. The constraints between the robot and the visual marker are: the position of the visual marker in the robot coordinate system and by The covariance matrix

[0156] Problem conversion module: Add the constraints between the visual robot and the set visual markers obtained in the mapping and constraint acquisition module to the SLAM backend optimization problem description obtained in the mapping and constraint acquisition module, and convert the least squares problem described in the mapping and constraint acquisition module into a least squares problem with visual marker constraints;

[0157] Iterative solution module: Iteratively solves the least squares problem converted by the problem conversion module, using the initial robot pose obtained by the IMU as the initial value to obtain a unique optimal solution. The optimal solution includes the optimal pose solution set of the visual marker in the robot coordinate system;

[0158] Visual marker pose acquisition module: Based on the optimal pose set of the visual marker at all times in the robot coordinate system obtained by the iterative solution module, the pose set of the visual marker at all times observed by the optimized robot in the robot coordinate system is transformed into the pose set of the visual marker in the map through TF transformation, and then all elements in the pose set are averaged to obtain the optimal pose of the visual marker in the map;

[0159] The robot is a robot with vision and laser.

[0160] Please refer to the above-mentioned methods for the specific implementation process of each module, and will not be elaborated here. It should be understood that the above-mentioned division of functional modules is merely a division of logical functions. In actual implementation, other division methods can be used. For example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not implemented. At the same time, the above-mentioned integrated units can be implemented in the form of hardware or software functional units.

[0161] Example 4

[0162] A robot positioning system based on visual markers, comprising:

[0163] Map loading unit: when the robot navigates and loads the map, the position and posture of the visual marker in the robot coordinate system obtained by the above-mentioned visual marker map posture acquisition method are simultaneously loaded into the map;

[0164] Constraint acquisition unit: Based on the visual marker image information obtained by the robot from the camera in real time, the real-time constraints between the robot and the visual marker are obtained;

[0165] Posture loading and solving unit: By loading the known postures of visual markers in the map, the posture of the robot in the map is solved.

[0166] Example 5

[0167] A computer-readable storage medium stores a computer program, wherein the computer program is called by a processor to implement:

[0168] The steps of the above-mentioned method for acquiring map pose of visual markers or the steps of the above-mentioned robot positioning method based on visual markers.

[0169] When the processor calls the computer program to implement the steps of the above-mentioned method for obtaining the map pose of the visual mark, the following steps are specifically performed:

[0170] A method for obtaining map pose of a visual marker, comprising:

[0171] Step 1: Set visual markers in the robot's walking space;

[0172] Step 2: Use the robot to move in the walking space, perform laser SLAM mapping, and obtain the constraints between the robot and the set visual markers;

[0173] Among them, the back-end optimization problem used in the laser SLAM mapping part is described by the least squares problem; the constraints between the robot and the visual marker are specifically expressed as: the position of the visual marker in the robot coordinate system and by The covariance matrix

[0174] Step 3: Add the constraints between the visual robot and the set visual marker obtained in step 2 to the SLAM backend optimization problem in step 2, and transform the least squares problem described in step 2 into a least squares problem with visual marker constraints;

[0175] Step 4: Iteratively solve the least squares problem in step 3, using the initial position of the robot obtained by the IMU as the initial value, and obtain the unique optimal solution Ξr * ,Ξr * Contains the optimal pose solution set of the visual marker in the robot coordinate system

[0176] Step 5: Set the optimal pose of the visual marker obtained in step 4 at all times in the robot coordinate system Transformed into a set of poses of visual markers in the map through TF Then for all elements in the set Find the average value and get the best pose of the visual marker in the map

[0177] The robot is a robot with vision and laser.

[0178] When the processor calls the computer program to implement the steps of the robot positioning method based on visual markers, it specifically executes:

[0179] When the robot loads the map during navigation, the pose of the visual marker in the robot coordinate system obtained by the above-mentioned visual marker map pose acquisition method is loaded into the map at the same time. Then, based on the visual marker image information obtained by the robot from the camera in real time, the real-time constraint between the robot and the visual marker is obtained. By loading the known pose of the visual marker in the map in the yaml file, the robot's pose in the map is solved.

[0180] Example 6

[0181] like Figure 3As shown, a robot includes a robot body and a camera 2, a display screen 3, an armrest 4, a laser radar 14, and an ultrasonic sensor 5 arranged on the robot body;

[0182] The camera 2, display screen 3, armrest 4, laser radar 14 and ultrasonic sensor 5 are all connected to the control module of the robot body;

[0183] The control module of the robot body obtains the map pose using the map pose acquisition method of the visual marker or performs positioning using the robot positioning method based on the visual marker;

[0184] The chassis drive module of the robot adopts an independent suspension structure driven by a hub motor differential, which is connected to the robot's carrying platform through a connecting rod 9 and a spring 10. The independent suspension structure includes a first differential motor drive wheel 6 and a second differential motor drive wheel 7, and four auxiliary wheels 8 respectively installed on both sides of the chassis. The first differential motor drive wheel 6 and the second differential motor drive wheel 7 are fixed on a rotating pin 12. The four auxiliary wheels 8 are respectively fixed on the four support points of the robot's chassis. The auxiliary wheels 8 are suspended and supported on the connecting rod 9. The spring 10 connects the connecting rod 9 and the robot's chassis 11 through the rotating pin 12. The entire connecting rod supports the robot's carrying platform 13.

[0185] The robot is made of an aluminum alloy body. The display screen 3 is used to complete the human-computer interaction process during the use of the robot. One-click interaction can be completed through the display screen 3. The armrest 4 is used to push the robot or perform manual mapping when the power is off. The laser radar 14 is located above the chassis, in front of the robot. The shell leaves a gap in the plane where the laser radar 14 is located, so that the laser radar 14 can collect 180-degree depth information in front of the robot. Under the chassis, ultrasonic sensors 5 are placed in five directions in front of the robot for obstacle avoidance during autonomous navigation of the robot. The different information collected by the ultrasonic sensor 5 at the five positions is used to determine the movement plan that the robot needs to make.

[0186] See Figure 4 As shown, in order to achieve flexible movement of the robot in specific scenarios and large-angle steering in a small space, the system as a whole is driven by differential motor drive wheels 6 and 7. By controlling the motor rotation rate, the differential motor drive wheels 6 and 7 are fixed on the rotating pin 12, and the different speeds of the drive wheels 6 and 7 are controlled to enable the robot to complete rotation in any direction. The four auxiliary wheels 8 are respectively fixed on the four support points of the chassis to maintain the balance of the robot. The auxiliary wheels 8 are suspended and supported on the connecting rod 9. The spring 10 connects the connecting rod 9 and the lower chassis 11 of the robot through the rotating pin 12. Finally, the entire connecting rod supports the carrying platform 13.

[0187] The chassis utilizes an independent suspension design with in-wheel motor differential drive, connected to the vehicle body via connecting rods 9 and springs 10. Each drive wheel is independently connected to a spring via a pivot pin. Any bouncing of any wheel due to ground impact is cushioned by springs above the wheel, ultimately distributing the vibration evenly across the supporting links without affecting the other wheels, effectively preventing tilt and vibration caused by uneven surfaces. This structure operates smoothly, compactly, and reliably, adaptively adapting to ground height to ensure effective contact between the drive wheels and the ground, resisting impact with obstacles and enabling multi-story elevator crossings with excellent obstacle-crossing capabilities. Furthermore, four universal wheels 8 assist in coordinated steering motion, providing greater directional flexibility and enabling free steering even in confined spaces.

[0188] Based on this understanding, the technical solution of the present invention, or the portion that contributes to the prior art, or all or part of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes instructions for causing a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the method described in each embodiment of the present invention. The aforementioned readable storage medium includes various media capable of storing program code, such as a USB flash drive, a mobile hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0189] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and not to limit it. Although the present invention has been described in detail with reference to the above embodiments, ordinary technicians in the field should understand that the specific implementation methods of the present invention can still be modified or replaced by equivalents. Any modification or equivalent replacement that does not depart from the spirit and scope of the present invention should be included in the scope of the claims of the present invention.

Claims

1. A method for obtaining map pose of visual markers, characterized in that: include: Step 1: Set visual markers in the robot's walking space; Step 2: Use the robot to move in the walking space, perform laser SLAM mapping, and obtain the constraints between the robot and the set visual markers; Among them, the back-end optimization problem used in the laser SLAM mapping part is described by the least squares problem; the constraints between the robot and the visual marker are specifically expressed as: the position and posture of the visual marker in the robot coordinate system And by posture The covariance matrix Step 3: Add the constraints between the visual robot and the set visual marker obtained in step 2 to the SLAM backend optimization problem in step 2, and transform the least squares problem described in step 2 into a least squares problem with visual marker constraints; Step 4: Iteratively solve the least squares problem obtained in step 3, using the initial position of the robot obtained by the IMU as the initial value, and obtain the unique optimal solution Ξr * , the optimal solution Ξr * Contains the optimal pose set of the visual marker in the robot coordinate system Step 5: Set the optimal pose of the visual marker obtained in step 4 at all times in the robot coordinate system Transformed into a set of poses of visual markers in the map through TF Then for all elements in the set Find the average value and get the best pose of the visual marker in the map The robot is a robot with vision and laser.

2. The method according to claim 1, characterized in that The least squares problem with visual labeling constraints is as follows: Among them, Ξm is the pose collection of the sub-map, Ξr represents the pose of the robot obtained by each sensor of the robot at the j-th frame scanning time with the map as the coordinate system, where is the position of the j-th frame radar scan with the map as the coordinate system, is the position of the visual marker at the jth frame radar scanning moment with the map as the coordinate system, The pose calculated by the odometer at the time of the j-th radar scan with the map as the coordinate system; represents the pose of the i-th submap, represents the position of the j-th frame radar scanning moment, ξ ij represents the position of the robot in the i-th sub-map coordinate system at the time of the j-th radar scan, and e is the vector error function, which is used to represent and The obtained robot's position in the i-th sub-map coordinate system is ij The degree of fit, ∑ij represents the covariance matrix of the j-th frame scan pose under the i-th sub-map; for The inverse matrix of the corresponding rotation matrix is, represents the translation vector in the pose of the i-th submap, It represents the translation vector of the robot's position in the map coordinate system given by each sensor at the j-th frame radar scanning moment. Represents the rotation angle around the z-axis in the pose of the i-th submap, It represents the rotation angle around the z-axis of the robot's position in the map coordinate system given by each sensor at the j-th radar scanning moment.

3. The method according to claim 1, characterized in that The process of obtaining the constraints between the robot and the set visual markers is as follows: Step A1, performing distortion correction on the image information of the collected visual marker; Step A2: Visual ID recognition by placing black and white squares; Step A3: Calculate the position and posture of the visual markers based on the length and width of the black and white squares; Step A4: Set a pose window and obtain the pose of the visual marker within the window in the camera coordinate system; The angle θ of the visual marker in the camera coordinate system and the position of the visual marker observed by the camera (x a ,y a ) constitutes the pose of the visual marker in the camera coordinate system Set a fixed window W c And distance restrictions, that is, the pose that does not meet the requirements is not used for constraint calculation. The formula is as follows: Among them, x min Indicates the minimum value of the visual marker pose in the x-axis direction in the camera coordinate system, x max Indicates the maximum value of the visual marker pose in the x-axis direction in the camera coordinate system, min Indicates the minimum value of the visual marker pose in the y-axis direction in the camera coordinate system, y max Indicates the maximum value of the visual marker pose in the y-axis direction in the camera coordinate system, θ min Indicates the minimum angle of the visual marker pose around the z-axis in the camera coordinate system, θ max They represent the maximum rotation angle of the visual marker pose around the z-axis in the camera coordinate system; Step A5: The position of the visual marker obtained through the window in the camera coordinate system Perform Kalman filter smoothing; Step A6: Mark the smoothed visual position in the robot coordinate system and covariance matrix as constraints.

4. The method according to claim 3, characterized in that The specific process of step A3 is as follows: Step A31, calculating the distance using the camera perspective principle; When the digital information carried by the visual marker is obtained, the distance d of the visual marker relative to the camera is calculated based on the change in the side length of the visual marker relative to the standard library and the principle of camera focal length: Where d is the distance between the center point of the visual marker and the camera, f is the focal length of the camera obtained during calibration, w is the known standard size of a single square of the visual marker, and s is the size of a single square of the visual marker in the pixel coordinate system; Step A32, calculating the rotation angle by comparing the distance between the two corner points; Based on step A31, calculate the distances from the two adjacent corner points of a black square in the same visual marker to the camera. Calculate the angle θ of the visual marker relative to the camera coordinate system based on the difference in distances: Δd=d1-d2 Among them, Δd is the difference in depth distance between two adjacent corner points of a black square and the camera, d1 and d2 represent the depth distances from the corner points at different positions to the center point of the camera, and p w is the distance between the two corner points projected on the pixel plane, θ p is the angle between the two corner points and the camera center point, w r is the true distance between the two corner points obtained by the cosine theorem; θ is the angle of the visual marker in the camera coordinate system derived from the cosine formula.

5. A robot positioning method based on visual markers, characterized in that: When the robot loads the map during navigation, the pose of the visual marker in the robot coordinate system obtained by the map pose acquisition method of the visual marker described in any one of claims 1 to 4 is loaded into the map at the same time, and then based on the visual marker image information obtained by the robot from the camera in real time, the real-time constraint between the robot and the visual marker is obtained, and by loading the known pose of the visual marker in the map, the pose of the robot in the map is solved.

6. The method according to claim 5, characterized in that The robot's position in the map According to the following formula: Where Δθ represents the direction of the visual marker in the map coordinate system and the orientation of the visual marker in the robot coordinate system The difference is the rotation angle of the robot coordinate system relative to the map coordinate system. Substituting it into the rotation matrix formula can obtain the two-dimensional rotation matrix R that transforms the map coordinate system to the robot coordinate system. Taking the inverse matrix, we can obtain the two-dimensional rotation matrix R that transforms the robot coordinate system to the map coordinate system. -1 , multiply the position of the visual marker in the robot coordinate system by Then use the position of the visual coordinate system in the map coordinate system minus The position of the robot coordinate system in the map coordinate system is obtained by reverse calculation = -Δθ, which means the robot’s position in the map is obtained 7. A map pose acquisition system for visual marking, characterized in that: include: Visual marker setting module: sets visual markers in the robot's walking space; Mapping and constraint acquisition module: Use the robot to move in the walking space to perform laser SLAM mapping and obtain the constraints between the robot and the set visual markers; Among them, the back-end optimization problem used in the laser SLAM mapping part is described by the least squares problem. The constraints between the robot and the visual marker are: the position of the visual marker in the robot coordinate system And by posture The covariance matrix Problem transformation module: Add the constraints between the visual robot and the set visual markers obtained in the mapping and constraint acquisition module to the SLAM backend optimization problem description obtained in the mapping and constraint acquisition module, and transform the least squares problem described in the mapping and constraint acquisition module; Iterative solution module: Iteratively solves the least squares problem converted by the problem conversion module, using the initial robot pose obtained by the IMU as the initial value to obtain a unique optimal solution. The optimal solution includes the optimal pose solution set of the visual marker in the robot coordinate system; Visual marker pose acquisition module: Based on the optimal pose set of the visual marker at all times in the robot coordinate system obtained by the iterative solution module, it is transformed into a pose set of the visual marker in the map through TF, and then all elements in the pose set are averaged to obtain the optimal pose of the visual marker in the map; The robot is a robot with vision and laser.

8. A robot positioning system based on visual markers, characterized in that: include: A map loading unit: when loading a map during robot navigation, the position of the visual marker in the robot coordinate system obtained by the method for obtaining the map position of the visual marker according to any one of claims 1 to 3 is simultaneously loaded into the map; Constraint acquisition unit: Based on the visual marker image information obtained by the robot from the camera in real time, the real-time constraints between the robot and the visual marker are obtained; Posture loading and solving unit: By loading the known postures of visual markers in the map, the posture of the robot in the map is solved.

9. A computer-readable storage medium, characterized in that: A computer program is stored, which is called by a processor to implement: The steps of the method for acquiring map pose of visual markers as described in claim 1 or the robot positioning method based on visual markers as described in claim 5.

10. A robot, characterized in that: The robot comprises a robot body and a camera (2), a display screen (3), an armrest (4), a laser radar (14), and an ultrasonic sensor (5) arranged on the robot body; The camera (2), display screen (3), armrest (4), laser radar (14) and ultrasonic sensor (5) are all connected to the control module of the robot body; The control module of the robot body applies the map pose acquisition method of the visual marker according to claim 1 to obtain the map pose or applies the robot positioning method based on the visual marker according to claim 5 to perform positioning; The chassis drive module of the robot adopts an independent suspension structure driven by a hub motor differential, and is connected to the robot bearing platform through a connecting rod (9) and a spring (10). The independent suspension structure comprises a first differential motor drive wheel (6) and a second differential motor drive wheel (7) and four auxiliary wheels (8) respectively installed on both sides of the chassis. The first differential motor drive wheel (6) and the second differential motor drive wheel (7) are fixed on a rotating pin (12). The four auxiliary wheels (8) are respectively fixed on four supporting points of the robot chassis. The auxiliary wheels (8) are suspended and supported on the connecting rod (9). The spring (10) connects the connecting rod (9) and the robot chassis (11) through the rotating pin (12). The entire connecting rod supports the robot bearing platform (13).

Images