Multi-intelligent robot collaborative navigation and mapping method based on adaptive map optimization

Abstract

The invention relates to a multi-intelligent robot collaborative navigation and mapping method based on adaptive map optimization, and belongs to the technical field of intelligent robot environment perception. The method comprises the following steps: constructing an image pyramid for representing a scale space by adopting a Gaussian-driven multi-stage distortion compensation and layered sampling model and converting an original grey-scale map, and then updating a map by adopting a key frame rejection strategy, a local map restoration method based on multiple interpolation and anomaly detection is adopted to optimize and fuse the local maps of the multiple sub-robots to obtain a global map; a frequency-quality joint evaluation leader selection algorithm is adopted to optimize the global map; adjusting the role of the robot by adopting a self-adaptive role conversion map enhancement method, and realizing dynamic role adjustment and task allocation optimization to obtain an enhanced global map; and finally, remodeling a navigation path by adopting a distributed path remodeling and obstacle sharing navigation algorithm to realize collaborative mapping navigation obstacle avoidance of multiple intelligent robots.

Description

Technical Field

[0001] The present invention belongs to the technical field of intelligent robot environment perception, and relates to a multi-intelligent robot collaborative navigation and mapping method based on adaptive map optimization. Background Art

[0002] With the development of robot technology, machine inspection has replaced manual inspection and has become the mainstream method. In complex environments, manual inspection has disadvantages such as repetitive work, time-consuming and labor-intensive, low efficiency, lack of personal safety of inspectors, and susceptibility to weather and environmental influences. Multiple robot inspections have obvious advantages in terms of safety, efficiency, and cost control. Therefore, research on collaborative SLAM (simultaneous localization and mapping) technology is carried out in quadruped robot inspections.

[0003] In the field of autonomous navigation technology, collaborative SLAM technology provides maps and posture information for the robot's autonomous navigation, so the accuracy of map construction will directly affect the navigation accuracy of the robot dog. The current collaborative SLAM technology is usually divided into laser collaborative SLAM and visual collaborative SLAM. Visual collaborative SLAM uses camera sensors to capture surrounding image information. Due to the characteristics of the camera, visual SLAM performs well in texture-rich environments, but does not have good mapping and positioning effects in larger scenes, and has low mapping efficiency. Laser collaborative SLAM uses lidar sensors to directly capture surrounding point cloud information, and has excellent mapping effects, but fails in environments with degraded features.

[0004] Although SLAM technology has achieved certain results, in large-scale scenarios, due to the difficulty of feature extraction and communication delays caused by network problems, visual collaborative SLAM mapping and positioning will fail. Moreover, in collaborative SLAM, the laser collaborative SLAM fails due to the large amount of laser radar data. Therefore, general visual and laser collaborative SLAM is not feasible in complex scenarios. Summary of the invention

[0005] In view of this, the purpose of the present invention is to provide a multi-intelligent robot collaborative navigation and mapping method based on adaptive map optimization, which is used to realize the environmental perception of multi-intelligent robots in complex environments. In view of the difficulties in feature extraction and communication delay caused by network problems in robot collaborative SLAM, the camera image data of robots at different times and the upload of camera image data of multiple robots at the same time are considered, and a Gaussian-driven multi-level distortion compensation and layered sampling model is constructed. A local map repair method based on multiple interpolation and anomaly detection and a frequency-quality joint evaluation leader selection algorithm are adopted to construct a multi-robot leader-follower mechanism.

[0006] In order to achieve the above object, the present invention provides the following technical solutions:

[0007] A multi-intelligent robot collaborative navigation and mapping method based on adaptive map optimization specifically includes the following steps:

[0008] S1: Obtain the original data of the monocular camera carried by the intelligent robot and solve the distortion parameters. Use the Gaussian-driven multi-level distortion compensation and layered sampling model to construct an image pyramid for representing the scale space by converting the original grayscale image and obtain the feature points required for mapping.

[0009] S2: A single robot sends keyframes and map points to the server through the information exchange module, where the information exchange module adopts a keyframe rejection strategy and a local map repair method based on multiple interpolation and anomaly detection;

[0010] The key frame rejection strategy is specifically: if the received key frame is new data, then a new connection is added to the existing map; if it is existing data, then the key frame is deleted;

[0011] The local map repair method based on multiple interpolation and anomaly detection is specifically as follows: firstly, anomaly detection and data cleaning are performed on the received map points, and then the unknown abnormal map points generated by communication problems such as delay and packet loss are compensated by the piecewise polynomial spline fitting method through the polynomial interpolation method and the spline interpolation method respectively; finally, the global map is optimized by the graph optimization method and the BA optimization method;

[0012] S3: A frequency-quality joint evaluation leader selection method is used to evaluate and determine the leader robot based on the sub-robot map update frequency and map quality. After determining the sub-robot leader and follower, an adaptive role transition map enhancement method is used to adjust the robot's role, achieve dynamic role adjustment and task allocation optimization, and finally obtain an enhanced global map.

[0013] S4: A leader-follower mechanism is used to guide multiple intelligent robot agents to navigate and avoid obstacles. A distributed path reshaping and obstacle sharing navigation algorithm is proposed. The attributes of obstacles are described and an obstacle list is uploaded. The attributes of the obstacles include position, size, and angle. The sub-robots reshape the navigation path according to the size and distance of the obstacles in the obstacle list to achieve collaborative mapping, navigation, and obstacle avoidance of multiple intelligent robots.

[0014] Further, step S1 specifically includes the following steps:

[0015] S11: Radial distortion and tangential distortion can be expressed by a polynomial function of the distance L between the pixel and the center of the image. The polynomial function can be expressed as follows after normalization:

[0016]

[0017] Among them, x and y represent the original image coordinates of the monocular camera, X and Y represent the optimized image coordinates of the monocular camera, parameters p1, p2 and p3 represent the degree of radial distortion, and q1 and q2 represent the degree of tangential distortion;

[0018] S12: According to the distortion parameters determined in step S11, the corrected image is Gaussian blurred, the image is smoothed by Gaussian filtering, and the image is convolved using the Gaussian kernel function G(x,y):

[0019]

[0020] Where σ is the standard deviation of the Gaussian kernel;

[0021] For a two-dimensional image, one-dimensional Gaussian filtering is performed on it in the x direction and the y direction respectively; the pixel value of the image is represented as I(x, y), and the pixel value of the image after Gaussian blurring I′(x, y) is represented as:

[0022]

[0023] Among them, (I*G)(x,y) represents the discrete convolution operation of the image I(x,y) and the Gaussian kernel function G(x,y); finally, the Gaussian blurred image is average pooled;

[0024] S13: Determine a Gaussian pyramid by downsampling the images of different scales obtained in step S12, i.e., reduce the size of the image; downsampling can be simply done by taking each n rows and columns, where n is the downsampling factor; this step will generate the next level of the pyramid, and for the downsampled image, repeat the above steps until the required number of pyramid levels is reached or the required image size is reached; combine each downsampled and filtered image into a pyramid; each level of the pyramid is constructed by taking the difference between the image obtained by downsampling the previous level and its Gaussian blurred version.

[0025] Further, step S2 specifically includes the following steps:

[0026] S21: When receiving the key frame sent by the sub-robot, the information exchange module converts the key frame K f The keyframes are stored in the keyframe database, and then the connection between the new keyframe and the existing pose graph of the corresponding server map is established. When one or more sub-robots visit the same location multiple times during the mission, the map contains several K-maps from that location. f ; Although some of the K f For example, K from different perspectives f Added more information to the system, but some other K fmay contain almost the same information, so there is no need to keep all these similar key frames. A key frame rejection strategy is used to randomly select a key frame K from the server map. f , iterate this key frame K in the co-visual map f All adjacent keyframes of K are checked for their observed map points; if the keyframe K is below the predefined percentage threshold T, f If T% of all map points are observed by at least 3 other keyframes, the keyframe and the map points contained in it are considered redundant and deleted from the global map; the threshold can be adjusted adaptively based on historical data; each deletion of redundant keyframes is recorded, and the proportion of redundant keyframes in the past period of time is calculated; if the redundancy ratio is high, the threshold is increased to make the system more tolerant and retain more keyframes; if the redundancy ratio is low, the threshold is reduced to make the system more strict and delete more keyframes;

[0027]

[0028] Among them, R ratio is the redundancy ratio, t r is the number of redundant key frames, t a is the total number of key frames; then set an initial threshold T initial , according to the current redundancy ratio, use the following formula to adjust the threshold:

[0029] T new =T initial +α·(R ratio -T initial ) (6)

[0030] Among them, α is a smoothing factor used to control the sensitivity of adjustment; T new is the new threshold;

[0031] If under a predefined percentage threshold θ, K f If θ% of all map points in are observed by at least 3 other keyframes, then this keyframe and the map points contained in this frame are considered redundant and deleted from the global map;

[0032] S22: Clean the map points and key frames sent by the sub-robot in step S21, check and delete possible outliers or erroneous data, use the interquartile range method to determine the outliers or erroneous data, sort the data, set the value at the 25% position as the first quartile Q1, the value at the 75% position as the third quartile Q3, and calculate the interquartile range I QR :

[0033] I QR =Q3-Q1 (7)

[0034] Secondly, calculate and determine the upper and lower bounds of outlier detection

[0035]

[0036] Among them, k is a pre-set threshold. For each map point (M i ,N i ), if (M i ,N i ) exceeds the specified upper and lower bounds, the point is considered an outlier and deleted;

[0037] S23: When outliers are deleted in step S22, the sub-robot trajectory is fitted to achieve map repair. First, polynomial interpolation is performed to fill in the missing map points and smooth the trajectory. Polynomial interpolation fits a polynomial function through known map points to approximate unknown outlier data map points:

[0038]

[0039] Among them, M i Represents the ordinate of a known map point, N i represents the horizontal coordinate of the known map point, f(M) represents the polynomial interpolation function, n represents the number of known map points, and M represents the vertical coordinate of the unknown map point to be interpolated; the first stage of abnormal map point fitting is obtained through polynomial interpolation;

[0040] S24: According to the coordinates of the abnormal map points in the first stage obtained in step S22 (M i ,N i ), for each adjacent map point Mi and M i+1 , construct a cubic interpolation polynomial function:

[0041] S i (M) = a i (MM i ) 3 +b i (MM i ) 2 +c i (MM i )+d i ,M∈[M i ,M i+1 ] (10)

[0042] Among them, S i (M) represents a cubic interpolation polynomial function; a i , b i , c i , di Represents the coefficients of the cubic spline interpolation polynomial; the coefficients of the cubic spline interpolation polynomial satisfy the requirement that the first and second order derivatives of the interpolation function are continuous and the second order derivative of the interpolation function is zero at the beginning and end:

[0043]

[0044] Among them, S′ i (M i ) represents the first-order derivative of the interpolation function, S″ i (M i ) represents the second-order derivative of the interpolation function, and the map points should satisfy the interpolation conditions:

[0045] S i (M i )=N i ,S i (M i+1 )=N i+1 (12)

[0046] For each interpolation polynomial segment, solve its coefficient a according to the above formula i , b i , c i , d i ,By satisfying the above conditions, a smooth and continuous interpolation function can be obtained,which is used to quadratically fit the unknown map point data, thereby obtaining credible,complete sub-robot map information;

[0047] S25: Using the graph-based optimization method, all optimized parameters are treated as vertices; for each image keyframe, the pose estimated by the front-end visual odometer is taken as one of the vertices, and all map points corresponding to this image keyframe are also taken as vertices; and between the pose vertex and the map point vertex, the corresponding connection relationship is established by constructing edges; the error term is represented by the edge, and the error term here is represented by the reprojection error; wherein, point P is a point in space, and H1 and H2 are observations of point H obtained by the robot at different positions, and the initial posture information of the robot is obtained by calculation through the front-end module. The projection position H2 of point H in the right image can be estimated using the pose and the coordinates of H1, but there is still a certain error between the estimated position H2 and the actual position, and this error is represented by e as the reprojection error. The error e is minimized by adjusting the robot's pose, and the final adjustment result is taken as the optimal pose of the robot; in the back-end optimization process, that is, after BA optimization, redundant data is deleted, and a maximum amount of data that a host can bear is set, thereby improving the real-time performance of the system and enabling the system to run smoothly for a long time.

[0048] Further, step S3 specifically includes the following steps:

[0049] S31: Before the navigation task begins, all sub-robots, i.e. agents, are assigned IDs and connected in a leader-follower manner. The leader robot is mainly responsible for exploring unknown areas, while the follower robot is responsible for further scanning and data refinement of known areas. In the initialization phase of the algorithm, a global leader is declared, and each leader is connected to its direct followers in a hierarchical manner.

[0050] S32: When determining the leader, the sub-robot with the greatest contribution is selected as the leader based on the map update frequency and map quality. For each robot i, the number of map updates within the time window T is N i , then the map update frequency F i It can be expressed as:

[0051]

[0052] S33: Evaluate and score the quality of the sub-robot transmitted map, compare the error between the robot's map and the actual environment, and evaluate the consistency of the local map; check the overlapping parts in the map, evaluate whether the maps of different robots are consistent, whether there are conflicts or inaccurate overlaps; assume that the multiple map data of robot i at position j are {m ij1 ,m ij2 ,...,m ijN};

[0053]

[0054] in, is the average map value at position j, δ ij represents the variance value of the map data at position j; by accumulating these variance values ​​and taking the average, the noise level of robot i can be obtained

[0055]

[0056] Where M represents the number of locations visited by robot i;

[0057] Map Noise Rating Q z,i It can be defined as The map quality is then evaluated by calculating the map smoothness score for each robot and using a high-pass filter H to obtain the high-frequency component h ij ;

[0058] h ij =H(m ij ) (17)

[0059] Calculate the energy E of the high frequency component i :

[0060]

[0061] Map quality rating Q p,i It can be defined as E i The noise score and quality score are combined to calculate the overall quality score Q of each robot. i :

[0062] Q i =ω1Q z,i +ω2Q p,i (19)

[0063] Among them, ω1 and ω2 represent weight parameters and their sum is 1;

[0064] S34: In order to make the frequency and quality scores comparable in the same dimension, they are normalized:

[0065]

[0066] Among them, F min and F max are the lowest and highest update frequencies among all robots respectively; Q min and Q max are the lowest and highest quality scores of all robots, respectively; combined with the normalized frequency and quality rating Calculate the comprehensive contribution score C of each robot i :

[0067]

[0068] Among them, α is a weight parameter with a value between 0 and 1, which is used to adjust the importance of frequency and quality. Finally, the navigation leader robot is determined by the comprehensive contribution score;

[0069] S35: The leader robot is mainly responsible for exploring unknown areas, while the follower robot is responsible for further scanning and data refinement of known areas; there is a robot group Each robot has a state variable that represents its current role (L for leader, F for follower); the dynamic adjustment of roles can be carried out through the following strategies. The first is the area coverage strategy. If a robot R i Area coverage explored When a certain threshold θ is reached, the robot changes from a leader role to a follower role; after becoming a follower, the sub-robot continues to enhance the map of the known area; finally, the sub-robot is optimized for task allocation, and an objective function f is defined TaskTo minimize the task load imbalance of all robots:

[0070]

[0071] in, Indicates robot R i The task load of robot R i The task load is weightedly calculated using the computing resource consumption of the child robot and the size of the scanning area; by dynamically adjusting the role, each robot can adaptively adjust its role according to the environment and task requirements during the mapping process, thereby achieving the purpose of balancing the load and improving mapping efficiency.

[0072] Further, step S4 specifically includes the following steps:

[0073] S41: If the leader sub-robot agent position has not reached the navigation specified position, the navigation task will be started; if the robot has not reached the desired target and the camera detects any obstacle, the properties of the detected obstacle will be stored in the obstacle list , where j:0→n represents the jth obstacle in the list;

[0074] S42: When the follower agent checks whether this particular object has been detected by the leader agent navigating to the same path by calling the obstacle list; if the detected obstacle exists in the obstacle list, if the distance or gap between the existing obstacle in the list and the currently detected obstacle is equal to or less than 0, then the two obstacles are merged together and considered as a single obstacle; otherwise, the currently detected obstacle is added to the obstacle list; then, the agent broadcasts the updated list to other agents; if the detected obstacle does not exist in the obstacle list, it is determined whether the newly detected obstacle is a completely different obstacle or a part of the previously partially detected obstacle; then the coordinates of the obstacle in the global map are passed to the current child robot agent, and the shortest path obstacle avoidance is used for detour navigation so that the agent can actively perform collision avoidance;

[0075] S43: When the agent performs the obstacle avoidance task, it first analyzes the properties of the detected obstacles from the obstacle list, i.e. Includes the distance d between the obstacle and the current agent obstacle , the angle of the obstacle and the size of the obstacle r obstacle ; Then the agent updates the Euclidean distance from the current position to the target, that is, and Γ G ; Afterwards, for each obstacle detected, check whether the obstacle is a potential collision risk, that is, whether it is within the agent's planned navigation trajectory:

[0076]

[0077] Where Δθ is the obstacle avoidance angle, which decreases as the distance of the obstacle from the robot increases and increases as the obstacle size increases. The robot may or may not detect the entire obstacle because the obstacle may be larger than the detection range of the camera sensor. In any case, the robot can see the extreme edges of the obstacle; if the straight line passes through any detected obstacle, the robot calculates new collision-free navigation waypoints based on the available information; finally, the waypoint closest to the calculated shortest path is selected as the robot's temporary goal:

[0078] φ adjusted =φ path ±Δθ (24)

[0079] Among them, φ adjusted is the adjusted path direction, φ path is the current path direction. Specifically, if the obstacle is on the left side of the path, use a positive value, and if it is on the right side, use a negative value. In this way, the robot will dynamically adjust the direction of advance according to the relative position of the obstacle and its size and distance to avoid collision with the obstacle.

[0080] The beneficial effects of the present invention are:

[0081] (1) The present invention uses a monocular camera to collect data for multi-robot visual SLAM, which improves the positioning performance and effectively solves the noise deviation problem in multi-robot SLAM.

[0082] (2) The present invention adopts a piecewise polynomial spline fitting method to spatially align the sensor information of multiple robots, thereby solving the problem of delay and packet loss that occurs during data transmission.

[0083] (3) The present invention uses a leader-follower mechanism guidance model to evaluate and determine the leader robot based on the map update frequency and map quality of the sub-robots, and guides the multi-intelligent robot agents to navigate and avoid obstacles through the leader-follower mechanism.

[0084] Other advantages, objectives and features of the present invention will be described in the following description to some extent, and to some extent, will be obvious to those skilled in the art based on the following examination and study, or can be taught from the practice of the present invention. The objectives and other advantages of the present invention can be realized and obtained through the following description. BRIEF DESCRIPTION OF THE DRAWINGS

[0085] In order to make the purpose, technical solutions and advantages of the present invention more clear, the present invention will be described in detail below in conjunction with the accompanying drawings, wherein:

[0086] Figure 1 It is a flow chart of the multi-intelligent robot navigation and obstacle avoidance method based on adaptive multi-level map optimization of the present invention;

[0087] Figure 2 A schematic diagram of a scenario of an embodiment of the invention. DETAILED DESCRIPTION

[0088] The following describes the embodiments of the present invention by specific examples, and those skilled in the art can easily understand other advantages and effects of the present invention from the contents disclosed in this specification. The present invention can also be implemented or applied through other different specific embodiments, and the details in this specification can also be modified or changed in various ways based on different viewpoints and applications without departing from the spirit of the present invention. It should be noted that the illustrations provided in the following embodiments only illustrate the basic concept of the present invention in a schematic manner, and the following embodiments and features in the embodiments can be combined with each other without conflict.

[0089] See also Figure 1-2 The present invention provides a multi-intelligent robot collaborative navigation and mapping method based on adaptive map optimization. The scenario is shown as follows Figure 2 As shown in the figure, using the monocular camera carried by the intelligent robot, a Gaussian-driven multi-level distortion compensation and hierarchical sampling model is proposed. By converting the original grayscale image, an image pyramid for representing the scale space is constructed. Then, the feature points and map points extracted by the sub-robot are sent to the server through the information exchange module. The map is updated according to the data, and then a local map repair method based on multiple interpolation and anomaly detection is proposed, so as to optimize and fuse the local maps of multiple sub-robots to obtain a global map. In order to optimize the global map, a frequency-quality joint evaluation leader selection algorithm is proposed, and the leader robot is evaluated and determined according to the sub-robot map update frequency and map quality. Then an adaptive role conversion map enhancement method is proposed, which uses this method to adjust the role of the robot, realize dynamic role adjustment and task allocation optimization to obtain an enhanced global map. Finally, the leader-following mechanism is used to guide the multi-intelligent robot agent to navigate and avoid obstacles, and a distributed path reshaping and obstacle sharing navigation algorithm is proposed to reshape the navigation path, so as to realize the collaborative mapping, navigation and obstacle avoidance of multiple intelligent robots.

[0090] like Figure 1 As shown, the present invention provides a multi-intelligent robot collaborative navigation and mapping method based on adaptive map optimization, which specifically includes the following steps:

[0091] S1: Obtain the original data of the monocular camera carried by the intelligent robot and solve the distortion parameters, propose a Gaussian-driven multi-level distortion compensation and layered sampling model, construct an image pyramid for representing the scale space by converting the original grayscale image, and obtain the feature points required for mapping.

[0092] Step S1 specifically includes the following steps:

[0093] S11: Radial distortion and tangential distortion can be expressed by a polynomial function of the distance L between the pixel and the center of the image. The polynomial function can be expressed as follows after normalization:

[0094]

[0095] Among them, x and y represent the original image coordinates of the monocular camera, X and Y represent the optimized image coordinates of the monocular camera, parameters p1, p2 and p3 represent the degree of radial distortion, and q1 and q2 represent the degree of tangential distortion;

[0096] S12: According to the distortion parameters determined in step S11, the corrected image is Gaussian blurred, the image is smoothed by Gaussian filtering, and the image is convolved using the Gaussian kernel function G(x,y):

[0097]

[0098] Where σ is the standard deviation of the Gaussian kernel;

[0099] For a two-dimensional image, one-dimensional Gaussian filtering is performed on it in the x direction and the y direction respectively; the pixel value of the image is represented as I(x, y), and the pixel value of the image after Gaussian blurring I′(x, y) is represented as:

[0100]

[0101] Among them, (I*G)(x,y) represents the discrete convolution operation of the image I(x,y) and the Gaussian kernel function G(x,y); finally, the Gaussian blurred image is average pooled;

[0102] S13: Determine a Gaussian pyramid by downsampling the images of different scales obtained in step S12, i.e., reduce the size of the image; downsampling can be simply done by taking each n rows and columns, where n is the downsampling factor; this step will generate the next level of the pyramid, and for the downsampled image, repeat the above steps until the required number of pyramid levels is reached or the required image size is reached; combine each downsampled and filtered image into a pyramid; each level of the pyramid is constructed by taking the difference between the image obtained by downsampling the previous level and its Gaussian blurred version.

[0103] S2: A single robot sends keyframes and map points to the server through the information exchange module, and proposes a keyframe rejection strategy; if it is new data, a new connection is added to the existing map. If it is existing data, the keyframe is deleted. Considering the delay and packet loss problems that occur during transmission, a local map repair method based on multiple interpolation and anomaly detection is proposed. First, anomaly detection and data cleaning are performed on the received map points, and then the piecewise polynomial spline fitting method is used to compensate for the unknown abnormal map points caused by communication problems such as delay and packet loss through polynomial interpolation and spline interpolation methods respectively; finally, the global map is optimized through the graph optimization method and the BA optimization method.

[0104] Step S2 specifically includes the following steps:

[0105] S21: When receiving the key frame sent by the sub-robot, the information exchange module converts the key frame K f The keyframes are stored in the keyframe database, and then the connection between the new keyframe and the existing pose graph of the corresponding server map is established. When one or more sub-robots visit the same location multiple times during the mission, the map contains several K-maps from that location. f ; Although some of the K f For example, K from different perspectives f Added more information to the system, but some other K f may contain almost the same information, so there is no need to keep all these similar key frames. A key frame rejection strategy is used to randomly select a key frame K from the server map. f , iterate this key frame K in the co-visual map f All adjacent keyframes of K are checked for their observed map points; if the keyframe K is below the predefined percentage threshold T, f If T% of all map points are observed by at least 3 other keyframes, the keyframe and the map points contained in it are considered redundant and deleted from the global map; the threshold can be adjusted adaptively based on historical data; each deletion of redundant keyframes is recorded, and the proportion of redundant keyframes in the past period of time is calculated; if the redundancy ratio is high, the threshold is increased to make the system more tolerant and retain more keyframes; if the redundancy ratio is low, the threshold is reduced to make the system more strict and delete more keyframes;

[0106]

[0107] Among them, R ratio is the redundancy ratio, t r is the number of redundant key frames, t a is the total number of key frames; then set an initial threshold T initial , according to the current redundancy ratio, use the following formula to adjust the threshold:

[0108] T new =T initial +α·(R ratio -T initial ) (6)

[0109] Among them, α is a smoothing factor used to control the sensitivity of adjustment; T new is the new threshold;

[0110] If under a predefined percentage threshold θ, K f If θ% of all map points in are observed by at least 3 other keyframes, then this keyframe and the map points contained in this frame are considered redundant and deleted from the global map;

[0111] S22: Clean the map points and key frames sent by the sub-robot in step S21, check and delete possible outliers or erroneous data, use the interquartile range method to determine the outliers or erroneous data, sort the data, set the value at the 25% position as the first quartile Q1, the value at the 75% position as the third quartile Q3, and calculate the interquartile range I QR :

[0112] I QR =Q3-Q1 (7)

[0113] Secondly, calculate and determine the upper and lower bounds of outlier detection

[0114]

[0115] Among them, k is a pre-set threshold. For each map point (M i ,N i ), if (M i ,N i ) exceeds the specified upper and lower bounds, the point is considered an outlier and deleted;

[0116] S23: When outliers are deleted in step S22, the sub-robot trajectory is fitted to achieve map repair. First, polynomial interpolation is performed to fill in the missing map points and smooth the trajectory. Polynomial interpolation fits a polynomial function through known map points to approximate unknown outlier data map points:

[0117]

[0118] Among them, M i Represents the ordinate of a known map point, N irepresents the horizontal coordinate of the known map point, f(M) represents the polynomial interpolation function, n represents the number of known map points, and M represents the vertical coordinate of the unknown map point to be interpolated; the first stage of abnormal map point fitting is obtained through polynomial interpolation;

[0119] S24: According to the coordinates of the abnormal map points in the first stage obtained in step S22 (M i ,N i ), for each adjacent map point M i and M i+1 , construct a cubic interpolation polynomial function:

[0120] S i (M) = a i (MM i ) 3 +b i (MM i ) 2 +c i (MM i )+d i ,M∈[M i ,M i+1 ] (10)

[0121] Among them, S i (M) represents a cubic interpolation polynomial function; a i , b i , c i , d i Represents the coefficients of the cubic spline interpolation polynomial; the coefficients of the cubic spline interpolation polynomial satisfy the requirement that the first and second order derivatives of the interpolation function are continuous and the second order derivative of the interpolation function is zero at the beginning and end:

[0122]

[0123] Among them, S′ i (M i ) represents the first-order derivative of the interpolation function, S″ i (M i ) represents the second-order derivative of the interpolation function, and the map points should satisfy the interpolation conditions:

[0124] S i (M i )=N i ,S i (M i+1 )=N i+1 (12)

[0125] For each interpolation polynomial segment, solve its coefficient a according to the above formula i , b i , c i , di ,By satisfying the above conditions, a smooth and continuous interpolation function can be obtained,which is used to quadratically fit the unknown map point data, thereby obtaining credible,complete sub-robot map information;

[0126] S25: Using the graph-based optimization method, all optimized parameters are treated as vertices; for each image keyframe, the pose estimated by the front-end visual odometer is taken as one of the vertices, and all map points corresponding to this image keyframe are also taken as vertices; and between the pose vertex and the map point vertex, the corresponding connection relationship is established by constructing edges; the error term is represented by the edge, and the error term here is represented by the reprojection error; wherein, point P is a point in space, and H1 and H2 are observations of point H obtained by the robot at different positions, and the initial posture information of the robot is obtained by calculation through the front-end module, and the projection position H2 of point H in the right image can be estimated using the pose and the coordinates of H1, but there is still a certain error between the estimated position H2 and the actual position, and this error is represented by e as the reprojection error, and the error e is minimized by adjusting the robot's pose, and the final adjustment result is taken as the optimal pose of the robot; in the back-end optimization process, that is, after BA optimization, redundant data is deleted, and a maximum amount of data that a host can bear is set, thereby improving the real-time performance of the system and enabling the system to run smoothly for a long time.

[0127] S3: A frequency-quality joint evaluation leader selection method is proposed to evaluate and determine the leader robot based on the sub-robot map update frequency and map quality. After determining the sub-robot leader and follower, an adaptive role transition map enhancement method is proposed. This method is used to adjust the robot's role, realize dynamic role adjustment and task allocation optimization, and finally obtain an enhanced global map.

[0128] Step S3 specifically includes the following steps:

[0129] S31: Before the navigation task begins, all sub-robots, i.e. agents, are assigned IDs and connected in a leader-follower manner. The leader robot is mainly responsible for exploring unknown areas, while the follower robot is responsible for further scanning and data refinement of known areas. In the initialization phase of the algorithm, a global leader is declared, and each leader is connected to its direct followers in a hierarchical manner.

[0130] S32: When determining the leader, the sub-robot with the greatest contribution is selected as the leader based on the map update frequency and map quality. For each robot i, the number of map updates within the time window T is N i , then the map update frequency F i It can be expressed as:

[0131]

[0132] S33: Evaluate and score the quality of the sub-robot transmitted map, compare the error between the robot's map and the actual environment, and evaluate the consistency of the local map; check the overlapping parts in the map, evaluate whether the maps of different robots are consistent, whether there are conflicts or inaccurate overlaps; assume that the multiple map data of robot i at position j are {m ij1 ,m ij2 ,...,m ijN};

[0133]

[0134] in, is the average map value at position j, δ ij represents the variance value of the map data at position j; by accumulating these variance values ​​and taking the average, the noise level of robot i can be obtained

[0135]

[0136] Where M represents the number of locations visited by robot i;

[0137] Map Noise Rating Q z,i It can be defined as The map quality is then evaluated by calculating the map smoothness score for each robot and using a high-pass filter H to obtain the high-frequency component h ij ;

[0138] h ij =H(m ij ) (17)

[0139] Calculate the energy E of the high frequency component i :

[0140]

[0141] Map quality rating Q p,i It can be defined as E i The noise score and quality score are combined to calculate the overall quality score Q of each robot. i :

[0142] Q i =ω1Q z,i +ω2Q p,i (19)

[0143] Among them, ω1 and ω2 represent weight parameters and their sum is 1;

[0144] S34: In order to make the frequency and quality scores comparable in the same dimension, they are normalized:

[0145]

[0146] Among them, F min and F max are the lowest and highest update frequencies among all robots respectively; Q min and Q max are the lowest and highest quality scores of all robots, respectively; combined with the normalized frequency and quality rating Calculate the comprehensive contribution score C of each robot i :

[0147]

[0148] Among them, α is a weight parameter with a value between 0 and 1, which is used to adjust the importance of frequency and quality. Finally, the navigation leader robot is determined by the comprehensive contribution score;

[0149] S35: The leader robot is mainly responsible for exploring unknown areas, while the follower robot is responsible for further scanning and data refinement of known areas; there is a robot group Each robot has a state variable that represents its current role (L for leader, F for follower); the dynamic adjustment of roles can be carried out through the following strategies. The first is the area coverage strategy. If a robot R i Area coverage explored When a certain threshold θ is reached, the robot changes from a leader role to a follower role; after becoming a follower, the sub-robot continues to enhance the map of the known area; finally, the sub-robot is optimized for task allocation, and an objective function f is defined Task To minimize the task load imbalance of all robots:

[0150]

[0151] in, Indicates robot R i The task load of robot R i The task load is weightedly calculated using the computing resource consumption of the child robot and the size of the scanning area; by dynamically adjusting the role, each robot can adaptively adjust its role according to the environment and task requirements during the mapping process, thereby achieving the purpose of balancing the load and improving mapping efficiency.

[0152] S4: A leader-follower mechanism is used to guide multiple intelligent robot agents to navigate and avoid obstacles. A distributed path reshaping and obstacle sharing navigation algorithm is proposed. The properties of obstacles including position, size and angle are described and an obstacle list is uploaded. The sub-robots reshape the navigation path according to the size and distance of the obstacles in the obstacle list.

[0153] Step S4 specifically includes the following steps:

[0154] S41: If the leader sub-robot agent position has not reached the navigation specified position, the navigation task will be started; if the robot has not reached the desired target and the camera detects any obstacle, the properties of the detected obstacle will be stored in the obstacle list , where j:0→n represents the jth obstacle in the list;

[0155] S42: When the follower agent checks whether this particular object has been detected by the leader agent navigating to the same path by calling the obstacle list; if the detected obstacle exists in the obstacle list, if the distance or gap between the existing obstacle in the list and the currently detected obstacle is equal to or less than 0, then the two obstacles are merged together and considered as a single obstacle; otherwise, the currently detected obstacle is added to the obstacle list; then, the agent broadcasts the updated list to other agents; if the detected obstacle does not exist in the obstacle list, it is determined whether the newly detected obstacle is a completely different obstacle or a part of the previously partially detected obstacle; then the coordinates of the obstacle in the global map are passed to the current child robot agent, and the shortest path obstacle avoidance is used for detour navigation so that the agent can actively perform collision avoidance;

[0156] S43: When the agent performs the obstacle avoidance task, it first analyzes the properties of the detected obstacles from the obstacle list, i.e. Includes the distance d between the obstacle and the current agent obstacle , the angle of the obstacle and the size of the obstacle r obstacle ; Then the agent updates the Euclidean distance from the current position to the target, that is, and Γ G ; Afterwards, for each obstacle detected, check whether the obstacle is a potential collision risk, that is, whether it is within the agent's planned navigation trajectory:

[0157]

[0158] Where Δθ is the obstacle avoidance angle, which decreases as the distance of the obstacle from the robot increases and increases as the obstacle size increases. The robot may or may not detect the entire obstacle because the obstacle may be larger than the detection range of the camera sensor. In any case, the robot can see the extreme edges of the obstacle. If the straight line passes through any detected obstacle, the robot calculates new collision-free navigation waypoints based on the available information; finally, the waypoint closest to the calculated shortest path is selected as the robot's temporary goal:

[0159] φ adjusted =φ path ±Δθ (24)

[0160] Among them, φ adjusted is the adjusted path direction, φ path is the current path direction. Specifically, if the obstacle is on the left side of the path, use a positive value, and if it is on the right side, use a negative value. In this way, the robot will dynamically adjust the direction of advance according to the relative position of the obstacle and its size and distance to avoid collision with the obstacle.

[0161] Finally, it should be noted that the above embodiments are only used to illustrate the technical solution of the present invention rather than to limit it. Although the present invention has been described in detail with reference to the preferred embodiments, those skilled in the art should understand that the technical solution of the present invention can be modified or replaced by equivalents without departing from the purpose and scope of the technical solution, which should be included in the scope of the claims of the present invention.

Claims

1. A multi-intelligent robot collaborative navigation and mapping method based on adaptive map optimization, characterized in that: The method specifically comprises the following steps: S1: Obtain the original data of the monocular camera carried by the intelligent robot and solve the distortion parameters. Use the Gaussian-driven multi-level distortion compensation and layered sampling model to construct an image pyramid for representing the scale space by converting the original grayscale image and obtain the feature points required for mapping. S2: A single robot sends keyframes and map points to the server through the information exchange module, where the information exchange module adopts a keyframe rejection strategy and a local map repair method based on multiple interpolation and anomaly detection; The key frame rejection strategy is specifically: if the received key frame is new data, then a new connection is added to the existing map; if it is existing data, then the key frame is deleted; The local map repair method based on multiple interpolation and anomaly detection is specifically as follows: firstly, anomaly detection and data cleaning are performed on the received map points, and then the piecewise polynomial spline fitting method is used to compensate for the unknown abnormal map points by polynomial interpolation and spline interpolation methods respectively; finally, the global map is optimized by the graph optimization method and the BA optimization method; S3: A frequency-quality joint evaluation leader selection method is used to evaluate and determine the leader robot based on the sub-robot map update frequency and map quality. After determining the sub-robot leader and follower, an adaptive role transition map enhancement method is used to adjust the robot's role, achieve dynamic role adjustment and task allocation optimization, and finally obtain an enhanced global map. S4: A leader-follower mechanism is used to guide multiple intelligent robot agents to navigate and avoid obstacles. A distributed path reshaping and obstacle sharing navigation algorithm is proposed. The properties of obstacles are described and an obstacle list is uploaded. The properties of the obstacles include position, size, and angle. The sub-robots reshape the navigation path according to the size and distance of the obstacles in the obstacle list.

2. The multi-intelligent robot collaborative navigation and mapping method according to claim 1, characterized in that: Step S1 specifically includes the following steps: S11: Radial distortion and tangential distortion are represented by a polynomial function of the distance L between the pixel and the center of the image. The polynomial function is expressed as follows after normalization: Among them, x and y represent the original image coordinates of the monocular camera, X and Y represent the optimized image coordinates of the monocular camera, parameters p1, p2 and p3 represent the degree of radial distortion, and q1 and q2 represent the degree of tangential distortion; S12: According to the distortion parameters determined in step S11, the corrected image is Gaussian blurred, the image is smoothed by Gaussian filtering, and the image is convolved using the Gaussian kernel function G(x,y): Where σ is the standard deviation of the Gaussian kernel; For a two-dimensional image, one-dimensional Gaussian filtering is performed on it in the x direction and the y direction respectively; the pixel value of the image is represented as I(x, y), and the pixel value of the image after Gaussian blurring I′(x, y) is represented as: Among them, (I*G)(x,y) represents the discrete convolution operation of the image I(x,y) and the Gaussian kernel function G(x,y); finally, the Gaussian blurred image is average pooled; S13: Determine a Gaussian pyramid by downsampling the images of different scales obtained in step S12; downsampling is performed by taking each n rows and columns, where n is the downsampling factor; this step will generate the next level of the pyramid, and for the downsampled image, repeat the above steps until the required number of pyramid levels is reached or the required image size is reached; combine each downsampled and filtered image into a pyramid; each level of the pyramid is constructed by the difference between the image obtained by downsampling the previous level and its Gaussian blurred version.

3. The multi-intelligent robot collaborative navigation and mapping method according to claim 2, characterized in that: Step S2 specifically includes the following steps: S21: When receiving the key frame sent by the sub-robot, the information exchange module converts the key frame K f The keyframes are stored in the keyframe database, and then the connection between the new keyframe and the existing posture graph of the corresponding server map is established. When one or more sub-robots visit similar positions multiple times during the task, a keyframe K is randomly selected from the server map using the keyframe rejection strategy. f , iterate this key frame K in the co-visual map f All adjacent keyframes of K are checked for their observed map points; if the keyframe K is below the predefined percentage threshold T, f If T% of all map points are observed by at least 3 other keyframes, the keyframe and the map points contained in it are considered redundant and deleted from the global map; the threshold is adaptively adjusted according to historical data; each deletion of redundant keyframes is recorded, and the proportion of redundant keyframes in the past period of time is calculated; if the redundancy ratio is high, the threshold is increased; if the redundancy ratio is low, the threshold is reduced; Among them, R ratio is the redundancy ratio, t r is the number of redundant key frames, t a is the total number of key frames; then set an initial threshold T initial , according to the current redundancy ratio, use the following formula to adjust the threshold: T new =T initial +α·(R ratio -T initial ) (6) Among them, α is a smoothing factor used to control the sensitivity of adjustment; T new is the new threshold; If under a predefined percentage threshold θ, K f If θ% of all map points in are observed by at least 3 other keyframes, then this keyframe and the map points contained in this frame are considered redundant and deleted from the global map; S22: Clean the map points and key frames sent by the sub-robot in step S21, check and delete abnormal values ​​or erroneous data, use the interquartile range method to determine abnormal values ​​or erroneous data, sort the data, set the value at the 25% position as the first quartile Q1, the value at the 75% position as the third quartile Q3, and calculate the interquartile range I QR : I QR =Q3-Q1(7) Secondly, calculate and determine the upper bound Δ and lower bound ▽ of outlier detection: Among them, k is a pre-set threshold. For each map point (M i ,N i ), if (M i ,N i ) exceeds the specified upper and lower bounds, the point is considered an outlier and deleted; S23: When outliers are deleted in step S22, the sub-robot trajectory is fitted to achieve map repair. First, polynomial interpolation is performed to fill in the missing map points and smooth the trajectory. Polynomial interpolation fits a polynomial function through known map points to approximate unknown outlier data map points: Among them, M i Represents the ordinate of a known map point, N i represents the horizontal coordinate of the known map point, f(M) represents the polynomial interpolation function, n represents the number of known map points, and M represents the vertical coordinate of the unknown map point to be interpolated; the first stage of abnormal map point fitting is obtained through polynomial interpolation; S24: According to the coordinates of the abnormal map points in the first stage obtained in step S22 (M i ,N i ), for each adjacent map point Mi and M i+1 , construct a cubic interpolation polynomial function: S i (M)=a i (M-M i ) 3 +b i (M-M i ) 2 +c i (M-M i )+d i ,M∈[M i ,M i+1 ] (10) Among them, S i (M) represents a cubic interpolation polynomial function; a i , b i , c i , d i Represents the coefficients of the cubic spline interpolation polynomial; the coefficients of the cubic spline interpolation polynomial satisfy the requirement that the first and second order derivatives of the interpolation function are continuous and the second order derivative of the interpolation function is zero at the beginning and end: Among them, S′ i (M i ) represents the first-order derivative of the interpolation function, S″ i (M i ) represents the second-order derivative of the interpolation function, and the map points should satisfy the interpolation conditions: S i (M i )=N i ,S i (M i+1 )=N i+1 (12) For each interpolation polynomial segment, solve its coefficient a according to the above formula i , b i , c i , d i ,By satisfying the above conditions, a smooth and continuous interpolation function is obtained, which is used to quadratically fit the unknown map point data, thereby obtaining credible and complete sub-robot map information; S25: Using the graph-based optimization method, all optimized parameters are treated as vertices; for each image keyframe, the pose estimated by the front-end visual odometer is taken as one of the vertices, and all map points corresponding to this image keyframe are also taken as vertices; and between the pose vertex and the map point vertex, the corresponding connection relationship is established by constructing edges; the error term is represented by the edge, and the error term here is represented by the reprojection error; wherein, point P is a point in space, and H1 and H2 are observations of point H obtained by the robot at different positions, and the initial posture information of the robot is obtained by calculation through the front-end module, and the projection position H2 of point H in the right image can be estimated using the pose and the coordinates of H1, but there is still an error between the estimated position H2 and the actual position, and this error is represented by e as the reprojection error, and the error e is minimized by adjusting the robot's pose, and the final adjustment result is taken as the optimal pose of the robot; in the back-end optimization process, that is, after BA optimization, redundant data is deleted, and a maximum amount of data that a host can bear is set.

4. The multi-intelligent robot collaborative navigation and mapping method according to claim 3 is characterized in that: Step S3 specifically includes the following steps: S31: Before the navigation task begins, all sub-robots, i.e. agents, are assigned IDs and connected in a leader-follower manner. The leader robot is responsible for exploring unknown areas, while the follower robot is responsible for further scanning and data refinement of known areas. In the initialization phase of the algorithm, a global leader is declared, and each leader is connected to its direct followers in a hierarchical manner. S32: When determining the leader, the sub-robot with the greatest contribution is selected as the leader based on the map update frequency and map quality. For each robot i, the number of map updates within the time window T is N i , then the map update frequency F i It is expressed as: S33: Evaluate and score the quality of the sub-robot transmitted map, compare the error between the robot's map and the actual environment, and evaluate the consistency of the local map; check the overlapping parts in the map, evaluate whether the maps of different robots are consistent, whether there are conflicts or inaccurate overlaps; assume that the multiple map data of robot i at position j are {m ij1 ,m ij2 ,...,m ijN }; in, is the average map value at position j, δ ij represents the variance value of the map data at position j; the variance values ​​are accumulated and averaged to obtain the noise level of robot i Where M represents the number of locations visited by robot i; Map Noise Rating Q z,i Defined as The map quality is then evaluated by calculating the map smoothness score for each robot and using a high-pass filter H to obtain the high-frequency component h ij ; h ij =H(m ij ) (17) Calculate the energy E of the high frequency component i : Map Quality Rating Q p,i Defined as E i The noise score and quality score are combined to calculate the overall quality score Q of each robot. i : Q i ω1Q z,i +ω2Q p,i (19) Among them, ω1 and ω2 represent weight parameters and their sum is 1; S34: In order to make the frequency and quality scores comparable in the same dimension, they are normalized: Among them, F min and F max are the lowest and highest update frequencies among all robots respectively; Q min and Q max The lowest and highest quality scores of all robots, respectively; combined with the normalized frequency and quality rating Calculate the comprehensive contribution score C of each robot i : Among them, α is a weight parameter with a value between 0 and 1, which is used to adjust the importance of frequency and quality. Finally, the navigation leader robot is determined by the comprehensive contribution score; S35: The leader robot is responsible for exploring unknown areas, while the follower robot is responsible for further scanning and data refinement of known areas; there is a robot group Each robot has a state variable that represents its current role. The dynamic adjustment of roles is carried out through the following strategies. The first is the area coverage strategy. If a robot R i Area coverage explored When the threshold θ is reached, the robot changes from a leader role to a follower role. After becoming a follower, the sub-robot continues to enhance the map of the known area. Finally, the task allocation of the sub-robot is optimized, and an objective function f is defined. Task To minimize the task load imbalance of all robots: in, Indicates robot R i The task load of robot R i The task load is weightedly calculated using the computing resource consumption of the child robots and the size of the scanning area. By dynamically adjusting the roles, each robot adaptively adjusts its role according to the environment and task requirements during the mapping process, thereby achieving load balancing and improving mapping efficiency.

5. The multi-intelligent robot collaborative navigation and mapping method according to claim 4, characterized in that: Step S4 specifically includes the following steps: S41: If the leader sub-robot agent position has not reached the navigation specified position, the navigation task will be started; if the robot has not reached the desired target and the camera detects any obstacle, the properties of the detected obstacle will be stored in the obstacle list , where j:0→n represents the jth obstacle in the list; S42: When the follower agent checks whether a specific object has been detected by the leader agent navigating to the same path by calling the obstacle list; if the detected obstacle exists in the obstacle list, if the distance or gap between the existing obstacle in the list and the currently detected obstacle is equal to or less than 0, then the two obstacles are merged together and considered as a single obstacle; otherwise, the currently detected obstacle is added to the obstacle list; then, the agent broadcasts the updated list to other agents; if the detected obstacle does not exist in the obstacle list, it is determined whether the newly detected obstacle is a completely different obstacle or a part of the previously partially detected obstacle; then the coordinates of the obstacle in the global map are passed to the current child robot agent, and the shortest path obstacle avoidance is used for detour navigation so that the agent can actively perform collision avoidance; S43: When the agent performs the obstacle avoidance task, it first analyzes the properties of the detected obstacles from the obstacle list, i.e. Includes the distance d between the obstacle and the current agent obstacle , the angle of the obstacle and the size of the obstacle r obstacle ; Then the agent updates the Euclidean distance from the current position to the target, that is, and Γ G ; Afterwards, for each obstacle detected, check whether the obstacle is a potential collision risk, that is, whether it is within the agent's planned navigation trajectory: Where Δθ is the obstacle avoidance angle. If the line passes through any detected obstacle, based on the available information, the robot calculates a new collision-free navigation waypoint; finally, the waypoint closest to the calculated shortest path is selected as the robot's temporary goal: f adjusted =φ path ±Δθ (24) Among them, φ adjusted is the adjusted path direction, φ path is the current path direction. Specifically, if the obstacle is on the left side of the path, use a positive value, and if it is on the right side, use a negative value. In this way, the robot will dynamically adjust the direction of advance according to the relative position of the obstacle and its size and distance to avoid collision with the obstacle.

Images