Map building method for body-equipped robot control system

Abstract

The application discloses a map construction method of a body-equipped robot control system and particularly relates to the technical field of intelligent robots; a low-resolution image sequence and sparse point cloud data are acquired, and stable candidate features with time consistency are extracted; a feature stability weight map is constructed in combination with image response stability and spatial projection correlation; features are re-screened based on the weight map, an initial static sparse map is generated; through interframe repositioning effect evaluation and error modeling, abnormal features are removed, newly identified high-confidence features are incrementally fused, and an updated map is optimized; finally, the optimized map is taken as a control input for path planning and attitude adjustment, the application improves the repositionability and long-term stability of the map, and enhances the autonomous navigation and control capability of the body-equipped robot in a dynamic and weakly-perceived environment.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of intelligent robots, in particular to a map construction method of a body-equipped robot control system. BACKGROUND

[0002] A body-equipped robot is a kind of intelligent agent with a physical entity and the ability to interact with the environment, which is widely used in disaster rescue, home service, space exploration and other scenarios. Due to the dynamic changes, complex structure, uneven lighting, sparse texture and other unfavorable factors in its working space, the traditional method of constructing an environment map relying on visual and laser radar sensors has significant limitations in actual deployment. For example, in a narrow or heavily obstructed environment, the echo rate of the laser radar decreases significantly, causing distortion of the topological structure of the map; and the visual SLAM method is prone to tracking loss and loop failure in low-light areas, affecting the positioning robustness.

[0003] In particular, for low-power body-equipped robots with limited size, their sensor configuration is limited, and the computing resources are limited, and they usually cannot carry high-precision devices such as high-performance IMU, depth camera or multi-line laser radar. Therefore, the traditional map construction method is difficult to run stably in such robot systems, resulting in a significant decrease in the spatial perception accuracy relied on by the control system, limiting the efficiency and safety of task execution.

[0004] In addition, due to the frequent obstruction of environmental feature points by dynamic obstacles (such as pedestrians, mobile robotic arms, etc.), a large number of false feature points are introduced during the construction of the map, causing map redundancy or local repetition and further occupying limited computing resources. How to accurately extract the relocalizable features in the static environment under the conditions of low computing budget and limited sensors to construct a high-confidence map that can be used for closed-loop control has become a key technical problem in the field of autonomous control of body-equipped robots. SUMMARY

[0005] The purpose of the present application is to provide a map construction method of a body-equipped robot control system to solve the problems in the background art.

[0006] In order to achieve the above-mentioned purpose, the present application provides the following technical scheme: a map construction method of a body-equipped robot control system, comprising:

[0007] S100, acquiring multi-modal sensor data collected by a body-equipped robot in a target environment with weak perception ability, including a low-resolution image sequence I and sparse point cloud data P;

[0008] S200, performing local structure consistency analysis on the low-resolution image sequence I to extract a stable candidate feature set F0 associated with the current frame, wherein the stable candidate feature satisfies that the response value change rate of the significant area in multiple time windows is lower than a set threshold;

[0009] S300, constructing a feature stability weight map W based on the corresponding projection distribution of the stable candidate feature set F0 in the sparse point cloud data P;

[0010] S400, re-screening the stable candidate feature set F0 based on the feature stability weight map W to obtain a final static environment map feature set F1, and registering it to the initial map coordinate system M0 to obtain an initial local sparse map M1;

[0011] S500, incrementally optimizing the static environment map feature set F1 based on the inter-frame repositioning success rate and error distribution of the initial local sparse map M1, updating the map to M2, and constructing a repositionable high-confidence map relied on the embodied robot control system;

[0012] S600, taking the map M2 as a control reference input for the embodied robot control system for path planning and pose adjustment.

[0013] Preferably, the low-resolution image sequence I is subjected to local structure consistency analysis to extract a stable candidate feature set F0 associated with the current frame, including the following steps:

[0014] Edge enhancement and corner detection processing is performed on each frame of image in the low-resolution image sequence I to obtain an initial feature point set;

[0015] Based on a set time window, the gray response value change rate of the corresponding feature points in the continuous multiple frames of images is calculated;

[0016] The feature points with a response value change rate lower than a set threshold in all time windows are screened to construct a candidate feature set F0.

[0017] Preferably, the feature stability weight map W is constructed, including:

[0018] The feature point set F0 in each image frame is subjected to pixel coordinate to radar coordinate system extrinsic conversion to establish a spatial mapping relationship between the image features and the point cloud data P;

[0019] After the coordinate mapping is completed, the projection hit number of each feature point in the multiple frames of point clouds is counted, and the spatial correlation strength of each feature point in the three-dimensional space is calculated;

[0020] A two-dimensional weighted function is constructed according to the spatial correlation strength of each feature point and its response stability in the image sequence to generate a feature stability weight map W.

[0021] Preferably, the two-dimensional weighted function is constructed according to the spatial correlation strength of each feature point and its response stability in the image sequence, including:

[0022] The response stability of the feature points in the image sequence is set as a time stability factor S1, and the hit frequency normalization value in the point cloud projection is a spatial correlation factor S2;

[0023] A two-dimensional weighted function W(f) = α × S1(f) + β × S2(f) is constructed, where f is a feature point, α and β are empirical weight coefficients, and α + β = 1;

[0024] The corresponding weight value W(f) is calculated for each point f in all feature points F0, and is mapped to a two-dimensional weight map consistent with the image size.

[0025] Preferably, the stable candidate feature set F0 is re-screened based on the feature stability weight map W to obtain the final static environment map feature set F1, including:

[0026] According to the weight value corresponding to each feature point in the feature stability weight map W, the stable candidate feature set F0 is sorted, and a weight threshold W0 is set to screen feature points with a weight value higher than the weight threshold W0 to form a high-confidence feature subset;

[0027] The high-confidence feature subset is subjected to spatial consistency constraint processing, and feature points that violate the preset geometric stability condition are removed by calculating the relative geometric relationship of the feature points in adjacent frames to obtain the final static environment map feature set F1;

[0028] Based on the pose information corresponding to the feature points at the acquisition time, the final static environment map feature set F1 is uniformly converted into the initial map coordinate system M0 to complete the coordinate registration of the feature points;

[0029] The feature point set that has completed coordinate registration is written into the map data structure as a node to construct an initial local sparse map M1 containing only static high-confidence features.

[0030] Preferably, the static environment map feature set F1 is incrementally optimized, and the map is updated to M2, including:

[0031] During the execution of the task by the robot, the number of relocalizable features in F1 is detected in each frame of image, and the inter-frame relocalization success rate is calculated;

[0032] The feature matching error distribution is estimated by the least squares method, and the abnormal feature points with a relocalization error greater than a relocalization standard threshold are marked as objects to be removed;

[0033] New stable feature points are identified in the current frame of image, and based on the image response stability and spatial consistency principle, new high-confidence features are screened and incorporated into F1 to construct an updated feature set F2;

[0034] The feature points remaining after the abnormal points are removed are registered to the map coordinate system, and are updated to the map M2.

[0035] Preferably, the principle of image response stability and spatial consistency is used to screen the newly added high-confidence features and incorporate them into F1, which includes:

[0036] Candidate feature points that are not yet included in the map feature set F1 are detected in consecutive frame images, and their multi-frame response intensity changes are recorded.

[0037] The image response change rate of each candidate feature point is calculated, and if the change rate is lower than a first threshold value within a set time window, the feature is marked as a response stable feature.

[0038] The response stable features are verified for geometric consistency between adjacent frames, and features with a relative position change exceeding a set error threshold are screened out.

[0039] The feature points that pass the double screening of image response and spatial consistency are calculated for a weighted confidence score, and if the score is higher than a set threshold value, they are incorporated into the static environment map feature set F1.

[0040] In the above technical solutions, the present application provides technical effects and advantages:

[0041] 1. The present application introduces a multi-screening mechanism based on image response stability and spatial geometric consistency, effectively extracts static environment features with high temporal consistency and spatial repositionability, and can still construct a high-confidence sparse map under weak perception conditions, significantly improving the map construction capability of embodied robots in low-resolution, sparse point cloud scenes. Compared with traditional methods that rely on single-frame vision or dense laser sensing for map construction, the present application can obtain stronger environmental structure expression effect under the premise of limited computing resources, and enhance the robustness of the map to dynamic interference and local occlusion.

[0042] 2. The present application can dynamically remove invalid features and fuse newly identified high-confidence features through continuous relocation evaluation and incremental optimization update of the map feature set, construct a map structure M2 with long-term stability and high repositionability, and provide stable and accurate spatial reference input for path planning and attitude adjustment, thereby improving the autonomous control accuracy and task execution continuity of embodied robots in complex and uncertain environments, and having good practical value. BRIEF DESCRIPTION OF DRAWINGS

[0043] In order to more clearly illustrate the technical solutions in the embodiments or the prior art, the drawings needed in the embodiments will be briefly introduced below. Obviously, the drawings described below are only some embodiments described in the present application, and other drawings can also be obtained by those skilled in the art based on these drawings.

[0044] Figure 1 A method flowchart of the present application. DETAILED DESCRIPTION

[0045] For the purposes of the embodiments of the present application, the technical solutions and advantages, the technical solutions in the embodiments of the present application will be described clearly and completely below with reference to the drawings in the embodiments of the present application. Obviously, the described embodiments are part of the embodiments of the present application, rather than all the embodiments. Based on the embodiments in the present application, all other embodiments obtained by those skilled in the art without creative labor fall within the scope of protection of the present application.

[0046] Embodiment, please refer to Figure 1 As shown in the figure, the map construction method of the embodied robot control system described in the embodiment includes:

[0047] S100, acquiring multi-modal sensor data collected by an embodied robot in a target environment with weak perception capability, including a low-resolution image sequence I and sparse point cloud data P.

[0048] In the embodiment, the embodied robot is deployed in a type of underground pipe gallery environment with narrow space and dramatic light changes. Considering that the robot body size is only 15 cm x 20 cm, limited by power consumption and load, it is only equipped with the following low-power sensor modules:

[0049] A 320x240 resolution gray camera with a frame rate of 15 FPS;

[0050] A single-line laser radar with a ranging angle of 180° and an angular resolution of 1°;

[0051] A miniature inertial measurement unit (IMU) with a sampling frequency of 50 Hz;

[0052] A low-power computing unit (such as Raspberry Pi 4B, equipped with a lightweight SLAM framework).

[0053] In the S100 step, the embodied robot slowly travels at a constant speed of about 0.3 m / s in the starting state, continuously collects the environment image sequence I={I1, I2,..., In} through the camera, where the resolution of each frame of image In is fixed at 320x240 pixels and is stored as a gray image form to reduce the subsequent processing calculation amount.

[0054] At the same time, the single-line laser radar synchronously collects the corresponding sparse laser point cloud data P={P1, P2,..., Pn} at a frequency of 1 frame per second, where each group of point cloud data Pn contains about 180 ranging points. Since this radar only provides two-dimensional scanning data, it brings challenges to subsequent spatial structure inference.

[0055] To enhance the time alignment of different sensor data, the timestamps provided by the IMU are used in the embodiment to synchronize the multi-modal data, ensuring that the images In and the point clouds Pn are collected within the same time window.

[0056] In addition, to improve the robustness of subsequent feature selection, the original image sequence I is also subjected to a lightweight preprocessing operation, including Gaussian filter denoising, contrast adaptive enhancement, and edge detection, to improve the feature response rate in low-texture areas. The point cloud data P is excluded from long-distance interference echoes by setting a distance threshold (e.g., 4 meters).

[0057] Finally, the data set output by S100 is the time-aligned image sequence I and the sparse point cloud sequence P, which provides input basis for stable feature extraction and structure weight modeling in subsequent map construction.

[0058] S200, performing local structure consistency analysis on the low-resolution image sequence I to extract a stable candidate feature set F0 associated with the current frame, wherein the stable candidate features satisfy a change rate of response values in a plurality of time windows that is lower than a set threshold.

[0059] In the image feature extraction process, to ensure that the features obtained from the low-resolution image sequence I have good structure description ability and time stability, the extraction of the stable candidate feature set F0 is completed as follows:

[0060] For each frame of image Ik in the image sequence I, first perform edge enhancement operation to improve the structure information expression ability of low-contrast areas. The edge enhancement uses the gradient convolution method based on Sobel operator, which combines the horizontal and vertical gray scale gradients of the image to generate an enhanced image Gk. The specific calculation steps are as follows:

[0061] Let the input image Ik be a two-dimensional gray scale matrix, and calculate its gradient images Gx and Gy in the horizontal and vertical directions respectively;

[0062] Convolve Ik using Sobel operator to obtain Gx and Gy; calculate the gradient amplitude Gk(x, y) of each pixel = ; normalize Gk to the [0, 255] gray scale range.

[0063] On the enhanced image Gk, apply the Shi-Tomasi corner detection algorithm to extract feature points with significant gray scale changes. Set the response threshold to λ0, and take the feature points with response values greater than λ0 as the initial feature point set Fk, Fk = {fk1, fk2,..., fkn}, where each feature point fkn contains its two-dimensional image coordinates and local response intensity value.

[0064] To measure the stability of feature points in the image sequence, set the time window length as T frames (for example, T = 5), and calculate the change rate of the response value of each initial feature point in the time window.

[0065] Specifically, for a feature point, its gray response value sequence corresponding to consecutive image frames in the time window is {r1, r2,..., rt}, and its change rate R is defined as: R = maximum response value minus minimum response value, divided by average response value, multiplied by 100%. The change rate reflects the response fluctuation degree of the feature point in the time sequence, and the smaller the value, the more stable the response.

[0066] To obtain feature points with time consistency, set a change rate threshold R0 (such as R0 = 15%), and select the feature point set with a change rate R less than the threshold as the stable candidate feature set F0, that is: F0 = {fki | R(fki) < R0, and fki exists in consecutive T frames of images}.

[0067] To further improve the spatial distribution uniformity of features, a grid uniform sampling strategy is used to process the feature points in F0 for spatial deduplication. Specifically, the image is divided into several grid regions, and only the feature point with the maximum response value is retained in each grid to form the final F0 set.

[0068] The set F0 will be used as input to participate in the subsequent process of constructing a feature stability weight map, to ensure that the input feature points have good stability and distribution balance in time and space.

[0069] S300, based on the projection distribution of the stable candidate feature set F0 corresponding to the sparse point cloud data P, a feature stability weight map W is constructed.

[0070] To establish the spatial correspondence between image feature points and sparse point cloud data, first complete the mapping and conversion of feature points from image pixel coordinates to radar coordinate system. The specific process is as follows:

[0071] Obtain the camera intrinsic matrix K (including focal length fx, fy and principal point coordinates cx, cy) through camera calibration, which is used for conversion of pixel coordinates to normalized imaging plane coordinates.

[0072] Obtain the extrinsic matrix T of the camera and the radar, which contains the rotation matrix R and the displacement vector t. The extrinsic parameter is obtained through joint calibration, which is used to complete the spatial transformation from the camera coordinate system to the radar coordinate system.

[0073] For any feature point f in the image frame, its pixel coordinates are (u, v), and its corresponding ray direction is obtained by the following calculation: normalize the pixel coordinates (u, v) to (x, y, 1), the calculation formula is: x = (u-cx) / fx, y = (v-cy) / fy. After mapping (x, y, 1) to the ray direction of the camera coordinate system, the coordinate transformation is completed through the external parameter matrix T = [R|t], and the projection direction vector of the feature point f in the radar coordinate system is obtained.

[0074] After completing the coordinate conversion, the feature point ray is matched with the point cloud data of the corresponding frame, and it is counted whether there is a point cloud falling in the vicinity of the projection direction of the feature point within a certain depth tolerance range, which is defined as follows:

[0075] For the feature point f, a spatial tolerance angle threshold θ (for example, set to 5 degrees) is set in its ray direction, and it is judged whether there is a point in the point cloud data Pk falling within the projection cone range.

[0076] If it is hit in a frame of point cloud data, the hit count of the feature point f is recorded as 1.

[0077] Suppose T frames of data are analyzed in total, then the spatial correlation strength S2 of the feature point f is defined as: S2(f) = hit count / T, the value range is 0 to 1. The higher the spatial correlation strength S2, the greater the probability of repeated appearance of the feature point in three-dimensional space, and the stronger the spatial repositioning ability.

[0078] Combining the response stability of the feature point in the image sequence and the spatial correlation strength in the point cloud projection, a weighted function W(f) is defined to quantify the overall stability of the feature.

[0079] Suppose f is a feature point, its image response stability is a time stability factor S1(f), that is, the inverse normalized value of the response value change rate of the point in T frames of images, which is specifically defined as: S1(f) = 1-[(maximum response value-minimum response value) / average response value], the result is normalized to [0, 1].

[0080] Set the weight parameters α and β to represent the importance of the time dimension and the space dimension, respectively, which satisfy α+β=1. In the embodiment, α=0.5 and β=0.5 are set, which means that both are equally important. Then the calculation formula of the weighted score of the feature point f is: W(f) = α×S1(f)+β×S2(f).

[0081] Map the weighted score W(f) of all feature points f∈F0 to the corresponding pixel position in the image frame to generate a two-dimensional feature stability weight graph W in the form of a gray value matrix, whose size is the same as the original image frame. Figure 1 Therefore.

[0082] To suppress isolated outliers and improve the spatial continuity of feature regions, a 3x3 Gaussian filter is applied to the generated weight map W for smoothing, enhancing the structural stability of the feature distribution.

[0083] The finally generated feature stability weight map W serves as the basis for subsequent static map feature screening, ensuring that the input features have high confidence in both time and space consistency, and providing a reliable environment description basis for the control system.

[0084] S400, based on the feature stability weight map W, the stable candidate feature set F0 is re-screened to obtain the final static environment map feature set F1, and it is registered to the initial map coordinate system M0 to obtain the initial local sparse map M1.

[0085] Based on the aforementioned constructed feature stability weight map W, for each feature point f in the stable candidate feature set F0, its corresponding weight value W(f) is obtained, and it is sorted in descending order of weight value.

[0086] Set the weight threshold W0, and select all feature points that satisfy W(f)≥W0 as the high-confidence feature subset F'. In this embodiment, the weight threshold W0 is set to 0.6, and the weight value is normalized to the range of 0 to 1. This threshold is determined through experiments, which can effectively eliminate noise feature points while maintaining the coverage rate of the map. The screening process is as follows:

[0087] Iterate through each feature point f in F0; if W(f)≥W0, add f to the high-confidence feature subset F'; finally form the set F'={f∈F0|W(f)≥W0}.

[0088] Since some high-weight feature points may not have spatial consistency due to short-term pseudo-stability, the feature points in F' need to be geometrically verified.

[0089] For each pair of matching feature points (fki, fk+1,j) appearing in adjacent image frames, calculate the relative Euclidean distance Δd before and after image coordinate transformation, and calculate the change direction angle θ. Set the following geometric stability conditions:

[0090] Δd cannot change by more than a preset threshold d0 (in this embodiment, d0 is set to 5 pixels) between consecutive frames;

[0091] θ cannot change by more than an angle threshold θ0 (in this embodiment, θ0 is set to 15 degrees).

[0092] If any feature point f violates the above stability conditions in multiple pairs, it is removed from the set F'. After this step, the final static environment map feature set F1 is obtained.

[0093] To ensure the uniformity of the map coordinates, each feature point in the feature set F1 needs to be converted from the image coordinate system or the camera coordinate system to the initial map coordinate system M0. The specific steps are as follows:

[0094] The body robot pose information of the feature point f at the acquisition time is obtained, including the three-dimensional position (x, y, z) and the attitude angle (roll, pitch, yaw);

[0095] The transformation matrix Tpose is constructed to convert the coordinates of the feature point f from the camera coordinate system to the initial map coordinate system M0. For each f∈F1, the coordinate transformation operation is performed, and the three-dimensional coordinate value fM0 of the feature point in the M0 coordinate system is recorded.

[0096] This step ensures that all feature points in the subsequent map construction have a unified spatial reference frame, which facilitates feature fusion and pose matching.

[0097] The feature point set F1 that has completed coordinate registration is written into the map data structure. The map structure uses a graph-like sparse graph representation, and the feature points are used as graph nodes. The nodes can be connected by edges according to the temporal relationship and spatial proximity relationship. The specific construction method is as follows:

[0098] For each feature point f∈F1, a node is created in the map structure, and its three-dimensional coordinates and initial frame index are recorded;

[0099] An edge is added to the node pair with a spatial distance less than a preset distance threshold (such as 0.3 meters) to record the local structure constraint;

[0100] All nodes and edges form an initial local sparse map M1, which serves as the basic structure for map construction and subsequent optimization.

[0101] This initial map only retains static features with high weight and high spatial stability, has good relocalizability and anti-interference performance, and significantly improves the perception stability of the subsequent body robot control system.

[0102] S500, based on the inter-frame relocalization success rate and error distribution of the initial local sparse map M1, incrementally optimizes the static environment map feature set F1, updates the map to M2, and constructs a relocalizable high-confidence map that the body robot control system relies on.

[0103] In each image, a fast feature matching algorithm (such as a local feature matching algorithm based on optical flow or descriptors) is used to detect the number of feature points in the current frame that can successfully match the registered feature points in the map feature set F1.

[0104] Let Nm be the number of valid matching features detected in the current frame image, and Nt be the number of theoretically visible features in the map feature set F1. Then the relocation success rate R of this frame is defined as: R = Nm ÷ Ny × 100%. If the relocation success rate of several consecutive frames is lower than the preset minimum relocation ratio threshold (e.g., set to 60%), then the map update mechanism is triggered.

[0105] After feature matching is completed, the Euclidean error between the image coordinates of each pair of matching points is calculated, and an error vector set is constructed. The least squares method is used to model the error distribution, and the mean μ and standard deviation σ of the distribution are estimated.

[0106] The relocalization threshold is set to μ plus twice σ, i.e., the error tolerance = μ + 2σ. Any matching point with an error greater than this tolerance is considered an anomaly that does not conform to the current pose model, and the corresponding map feature points are marked as objects to be removed.

[0107] This method avoids the isolated impact of single-frame errors by using error statistical modeling, thereby improving the robustness of the elimination mechanism.

[0108] In the current image frame, a corner detection algorithm (such as the Shi-Tomasi method) is used to extract new candidate feature points that are not yet included in the map feature set F1. Subsequently, within a set time window (such as 5 frames), the response intensity value sequence of each candidate feature point is recorded.

[0109] The rate of change of image response is defined as the maximum response value minus the minimum response value of the point within the time window, divided by the average response value. If this rate of change is lower than a first response stability threshold (e.g., set to 0.15), the feature point is marked as a response-stable feature.

[0110] For each stable feature point, extract its coordinate position in consecutive frame images, and calculate the relative displacement Δd and direction change angle θ between adjacent frames:

[0111] If Δd is less than a preset distance threshold (e.g., 2 pixels) and θ is less than a direction consistency threshold (e.g., 10 degrees), then the feature point is considered to have spatial consistency; otherwise, it is considered a short-term noise point and is removed.

[0112] For feature points that pass the dual screening, a weighted confidence score W(f) is calculated by combining their image response stability factor S1 and spatial consistency factor S2, i.e., W(f) = q1 × S1 + q2 × S2, where q1 and q2 are the weight coefficients of the response and spatial factors, respectively, satisfying q1 + q2 = 1. In this embodiment, q1 = 0.4 and q2 = 0.6 are taken. If W(f) is higher than the set confidence threshold (e.g., 0.7), the feature point is included in the newly added high-confidence feature set.

[0113] The newly added high-confidence feature points screened in the previous step are merged with the remaining feature points after the abnormal feature points are removed in F1 to construct an updated feature set F2.

[0114] For each feature point in F2, its coordinates are converted to the initial map coordinate system M0 using the homogeneous transformation matrix according to its corresponding acquisition time pose, and spatial registration is completed.

[0115] After registration, F2 is written into the map structure to update and generate a new map version M2. This map retains long-term stable feature points and incorporates newly identified high-confidence features in the current environment, significantly enhancing the repositioning ability and spatial perception robustness of the embodied robot control system in dynamic or changing environments.

[0116] S600, the map M2 is used as a control reference input for the embodied robot control system for path planning and pose adjustment.

[0117] The map M2 is composed of a set of high-confidence three-dimensional feature points registered to the initial map coordinate system, which not only has image response stability and spatial consistency, but also contains frame index, timestamp and local structure information associated with it.

[0118] Before path planning and pose control, a set of feature points adjacent to the current position of the robot in the current local area is extracted from M2 to construct a local navigation reference set. The extraction of the local reference set uses a spatial range limitation method, i.e. setting a radius R (e.g. 1.5 meters) centered at the current position of the robot, and selecting feature points within the radius.

[0119] A topological graph structure is constructed using the local feature point set, where nodes represent referenceable spatial positions and edges represent passable paths between nodes.

[0120] During the process of advancing along the planned path, the robot adjusts its pose in real time according to the feature point distribution in the map M2, including body turning angle, pitch angle and movement speed, to ensure the consistency of the advancing direction and the path. The specific implementation is as follows:

[0121] Get the reference feature points on the front path segment; calculate the direction vector of the center line of the feature point set; adjust the current orientation of the robot to the direction vector through a pose control algorithm (such as a PID controller or model predictive control), and adjust the speed according to the path curvature.

[0122] At the same time, by comparing the map M2 with the current frame of sensor data, it is detected whether there is a dynamic obstacle (such as a pedestrian or a mobile device) that is not marked in the map. If detected, local path re-planning is triggered, and a temporary path segment is quickly generated based on the most recently updated feature point set to ensure obstacle avoidance safety.

[0123] In the process of executing the control task, if the path execution error is greater than the preset tolerance (e.g., deviates from the target path by more than 10 cm), the position relocation module is triggered, and position correction information is fed back according to the real-time matching feature points.

[0124] The correction information can be fed back to the map M2, and a potential drift area is marked, so as to provide a reference for subsequent map re-optimization and control error compensation, and realize the closed-loop perception-decision-control integrated process of the embodied robot control system.

[0125] The above is only a specific embodiment of the present application, but the protection scope of the present application is not limited thereto, and any person skilled in the art can easily think of changes or replacements within the technical range disclosed in the present application, which should be covered within the protection scope of the present application.

Claims

1. A method of map building for a body- mounted robot control system, characterized by: Comprise: S100, acquire multi-modal sensor data collected by embodied robot in target environment with weak perception ability, including low-resolution image sequence I and sparse point cloud data P; S200, local structure consistency analysis is carried out to the low-resolution image sequence I, and the stable candidate feature set F0 associated with the current frame is extracted, wherein the stable candidate feature satisfies that the significant area response value change rate in multiple time windows is lower than the set threshold; S300, based on the projection distribution of the stable candidate feature set F0 in the sparse point cloud data P, the feature stability weight map W is constructed, comprising: The feature point set F0 in each image frame is converted from pixel coordinates to radar coordinate system, and the spatial mapping relationship between image features and point cloud data P is established; After completing the coordinate mapping, the projection hit number of each feature point in multiple frames of point cloud is counted, and the spatial correlation strength of each feature point in three-dimensional space is calculated; According to the spatial correlation strength of each feature point and its response stability in the image sequence, a two-dimensional weighted function is constructed, and a feature stability weight map W is generated; S400, based on the feature stability weight map W, the stable candidate feature set F0 is re-screened, the final static environment map feature set F1 is obtained, and it is registered to the initial map coordinate system M0, to obtain the initial local sparse map M1; S500, based on the inter-frame repositioning success rate and error distribution of the initial local sparse map M1, the static environment map feature set F1 is incrementally optimized, and the map is updated to M2, and a repositionable high-confidence map relied on the embodied robot control system is constructed; Wherein, the incrementally optimizing the static environment map feature set F1 and updating the map to M2 comprises: During the task execution of the robot, the repositionable number of features registered in F1 is detected in each frame of image, and the inter-frame repositioning success rate is calculated; The feature matching error distribution is estimated by least squares method, and the abnormal feature points with repositioning error greater than the repositioning standard threshold are marked as objects to be removed; New stable feature points are identified in the current frame of image, and based on the image response stability and spatial consistency principle, the newly added high-confidence features are screened and incorporated into F1, and an updated feature set F2 is constructed; The remaining feature points after removing the abnormal points and the newly added high-confidence features are uniformly registered to the map coordinate system, and are updated to the map M2; Wherein, the screening of the newly added high-confidence features into F1 based on the image response stability and spatial consistency principle comprises: Detect the candidate feature points not yet contained in the map feature set F1 in the continuous frame of image, and record the multi-frame response intensity change thereof; Calculate the image response change rate of each candidate feature point, if the change rate is lower than the first threshold in the set time window, mark it as a response stable feature; The response stable features are verified for geometric consistency between adjacent frames, and the feature points with relative position change exceeding the set error threshold are screened out; The feature points screened by the image response and spatial consistency are calculated for weighted confidence score, and if the score is higher than the set threshold, they are incorporated into the static environment map feature set F1. S600, taking the map M2 as a control reference input of the embodied robot control system for path planning and pose adjustment.

2. The map building method of a morphic robot control system according to claim 1, characterized by: The local structure consistency analysis is performed on the low-resolution image sequence I to extract a stable candidate feature set F0 associated with the current frame, including the following steps: Edge enhancement and corner detection processing is performed on each frame of the low-resolution image sequence I to obtain an initial feature point set; Based on a set time window, the change rate of the gray response value of the corresponding feature points in the continuous multiple frames of images is calculated; The feature points with a response value change rate lower than a set threshold in all time windows are screened to construct a candidate feature set F0.

3. The map building method of a morphic robot control system according to claim 1, characterized by: The two-dimensional weighted function is constructed according to the spatial correlation strength of each feature point and the response stability of the feature point in the image sequence, including: The response stability of the feature point in the image sequence is set as a time stability factor S1, and the hit frequency normalized value in the point cloud projection is set as a spatial correlation factor S2; A two-dimensional weighted function W(f) = α × S1(f) + β × S2(f) is constructed, where f is a feature point, α and β are empirical weight coefficients, and α + β = 1; The corresponding weight value W(f) of each point f in all feature points F0 is calculated and mapped to a two-dimensional weight graph consistent with the image size.

4. The map building method of a morphic robot control system according to claim 1, characterized by: The stable candidate feature set F0 is re-screened based on the feature stability weight graph W to obtain a final static environment map feature set F1, including: According to the weight value corresponding to each feature point in the feature stability weight graph W, the stable candidate feature set F0 is sorted, and a weight threshold W0 is set to screen the feature points with a weight value higher than the weight threshold W0 to form a high-confidence feature subset; The high-confidence feature subset is subjected to spatial consistency constraint processing to remove feature points that violate the preset geometric stability condition by calculating the relative geometric relationship of the feature points in adjacent frames to obtain the final static environment map feature set F1; Based on the pose information corresponding to the feature points at the acquisition time, the final static environment map feature set F1 is uniformly converted into the initial map coordinate system M0 to complete the coordinate registration of the feature points; The feature point set that has completed the coordinate registration is written into the map data structure as a node to construct an initial local sparse map M1 containing only static high-confidence features.

Images