Quadruped robot stair detection method based on adaptive granularity tuning

Abstract

The invention relates to a quadruped robot stair detection method based on adaptive granularity tuning, and belongs to the field of intelligent robots. The method at least comprises the following steps: acquiring environment data through a camera and a laser radar of a robot, and carrying out preprocessing and granularity layering on the environment data; extracting features of different granularities by adopting an adaptive granularity tuning strategy, and carrying out weighted fusion on the features of different granularities to obtain fused features; the fusion features are input into a multi-task learning framework to estimate a plurality of key parameters of the stairs, a joint loss function and a self-adaptive weighting mechanism are adopted to allocate loss weights of tasks, correlation between the tasks is measured based on a gradient task correlation analysis method, and finally a stair detection result is output; and continuously feeding back and correcting a stair detection result through a robot posture and stair parameter feedback model. According to the method, the fusion mode of the multi-granularity features is adaptively adjusted according to environment and task requirements, the detection result is corrected according to feedback, and the method can adapt to a complex stair detection environment.

Description

Technical Field

[0001] The invention belongs to the field of intelligent robots and relates to a quadruped robot stair detection method based on adaptive granularity tuning. Background Art

[0002] With the rapid advancement of robotics technology, quadruped robots, as representatives of a new generation of intelligent machinery, are improving at an unprecedented rate in their ability to perform diverse and complex tasks and fine operations. This technological advancement has not only promoted the deepening of industrial automation, but also greatly broadened the application areas of quadruped robots, involving scenes with intensive human activities and complex environments such as home services, emergency rescue operations, and scientific exploration of unknown terrain. In these occasions, quadruped robots have demonstrated incomparable advantages with their excellent stability and excellent maneuverability, especially when facing uneven, irregular, and even dynamically changing ground conditions.

[0003] However, despite the impressive mobility of quadruped robots in complex environments, stairs, as a common environmental obstacle in daily life, still pose a severe challenge to their safe operation and efficient navigation. The detection and recognition of stairs is not only a prerequisite for ensuring that the robot can successfully cross this critical obstacle, but also one of the key technologies for achieving its autonomous navigation and avoiding falling accidents.

[0004] The development of stair detection technology faces challenges in many dimensions. First, the diversity of stair materials, from wood, metal to carpet covering, each material corresponds to different visual characteristics and reflective properties, which brings great challenges to the recognition algorithm. Secondly, changes in ambient light, whether it is the intensity of natural light or the interweaving of light and shadow under artificial lighting conditions, will have a significant impact on the recognition accuracy of stairs. In addition, the partial obstruction of stairs by objects, the reflection of the surface, and the complexity of the background environment, such as similar texture or color interference, further increase the difficulty of stair detection.

[0005] Traditional stair detection methods, such as algorithms based on feature matching or simple geometric shape recognition, can achieve good results in certain specific and controlled environments. However, when faced with complex, changeable and unknown actual environments, they often expose problems such as insufficient flexibility and poor adaptability. Summary of the invention

[0006] In view of this, the purpose of the present invention is to provide a quadruped robot stair detection method based on adaptive granularity tuning, which can adaptively adjust the fusion mode of multi-granularity features according to the environment and task requirements, and introduces a robot posture-stair parameter feedback model to further enhance the intelligence level of the system and adapt to different stair detection environments.

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

[0008] A quadruped robot stair detection method based on adaptive granularity tuning comprises at least the following steps:

[0009] S1, obtain environmental data through the robot's camera and laser radar, pre-process the environmental data, and then perform granular stratification on the pre-processed environmental data;

[0010] S2, using an adaptive granularity tuning strategy to process features of different granularities extracted from images and three-dimensional point clouds of environmental data, and weightedly fusing features of different granularities to obtain fused features;

[0011] S3. Input the fused features into the multi-task learning framework to estimate multiple key parameters of the stairs. A joint loss function and an adaptive weighting mechanism are used to assign loss weights to each task, and a gradient-based task correlation analysis method is used to measure the correlation between tasks. Finally, the stair detection results are output;

[0012] S4. Plan the movement of the quadruped robot according to the stair detection results, and analyze the stair parameters corresponding to the current posture through the robot posture and stair parameter feedback model, and continuously feedback and correct the stair detection results.

[0013] Further, in step S1, at least the following steps are included:

[0014] S11, installing a camera and a laser radar on the head of the quadruped robot to collect image information and point cloud information of the stairs; building an IMU into the head of the quadruped robot to monitor the posture of the robot; wherein the intrinsic parameters of the camera and the extrinsic parameter transformation from the camera to the laser radar are pre-calibrated;

[0015] S12, preprocessing the acquired environmental data, wherein Gaussian blur noise reduction method is used for the image information collected by the camera to reduce regional noise; point cloud information collected by the laser radar is reconstructed to integrate the discrete point cloud data into a continuous three-dimensional model, and outliers are removed by voxel grid filtering technology;

[0016] S13. The preprocessed environmental data is stratified into high-granularity layer, medium-granularity layer and low-granularity layer, wherein the high-granularity layer is used to detect fine features of the stairs, including at least high-resolution images and point cloud information of the stair edge, texture and slope of each step; the medium-granularity layer is used to identify the stair structure, including at least the number of stair steps and the height and depth of each step; the low-granularity layer is used to identify the overall size and position of the stairs, including at least the overall inclination, overall height and overall width of the stairs.

[0017] Further, in step S2, at least the following steps are included:

[0018] S21. According to the granularity stratification results, different granularity features are extracted, wherein the Canny edge detection algorithm is used to extract high granularity features, the staircase structure recognition algorithm based on Hough transform is used to extract medium granularity features, and the MMDetection3D target detection platform is used to extract low granularity features;

[0019] S22, using adaptive granularity tuning strategy to fuse hierarchically extracted high-granularity features, medium-granularity features, and low-granularity features, and output the final fusion feature F combined .

[0020] Further, in step S22, the following steps are specifically included:

[0021] S221. Calculate the environment complexity C based on the staircase information in front of the robot obtained by the laser radar and the camera. The formula is:

[0022] C=a·Q+b·R V +c·R L

[0023] Where Q represents the noise level of the sensor input, which includes the data noise of the camera and lidar; R V Represents the complexity of visual information, which includes the feature richness and visual noise in the image; R L Indicates the complexity of point cloud information, which includes the richness of spatial information and the integrity of point cloud data; a, b, c represent adjustment parameters, which are used to balance the value of various factors in the evaluation of environmental complexity;

[0024] S222, setting tuning parameters. The tuning parameters are related to the environment complexity C, which determines the fusion weights of features of different granularities. The calculation method of tuning parameters of different granularity levels is:

[0025]

[0026] In the formula, ω h ,ω m ,ω l They represent the tuning parameters of high, medium and low granularities respectively. The higher the value of the tuning parameter, the greater the weight of the features at the corresponding granularity level in the multi-granularity feature fusion; e is a natural constant; λ>0 is a hyperparameter, which can be adjusted according to actual conditions;

[0027] S223: weighted fusion of features of different granularities according to tuning parameters to obtain the final fusion feature F combined , and its calculation formula is:

[0028] F combined =ω h ·F h +ωm ·F m +ω l ·F l

[0029] In the formula, F h ,F m ,F l are the granularity features extracted from the high granularity layer, the medium granularity layer, and the low granularity layer, respectively.

[0030] Further, in step S3, at least the following steps are included:

[0031] S31, extracting shared features from fused features through the shared feature extraction layer of the multi-task learning framework, establishing independent task layers for all tasks, inputting the shared features into the independent task layers for further processing, and obtaining the detection results of each task;

[0032] S32. Establish a joint loss function Loss for the multi-task learning framework, whose value is the weighted sum of all task losses, expressed as:

[0033]

[0034] In the formula, Loss i represents the loss function of task i, γ i represents the weight of task i, and N is the total number of tasks;

[0035] Introduce a gradient-based adaptive weighting mechanism to adjust the weight γ i , so that the tasks with larger gradients get higher weights, and the update rule is as follows:

[0036]

[0037] In the formula, Represent the gradients of tasks i and j respectively;

[0038] S33, the gradient-based task correlation analysis method is then used to measure the correlation between different tasks. The correlation between the two tasks is obtained by calculating the gradient inner product of task i and task j. The calculation method is:

[0039]

[0040] In the formula, Corr(M i ,M j ) represents the correlation between task i and task j. If Corr(M i ,M j )>0, then task i and task j have a positive correlation; if Corr(M i ,M j)<0, then task i and task j have a negative correlation;

[0041] S34, output the detection results of various parameters of the stairs, which are represented by a parameter set P:

[0042] P={n,θ,W,h,H,d,D}

[0043] In the formula, n represents the number of steps of the stairs, θ represents the inclination angle of the stairs, W represents the width of the stairs, H represents the overall height of the stairs, and D represents the overall depth of the stairs. Respectively represent the height and depth of each step.

[0044] Further, in step S31, the network structure of the multi-task learning framework is divided into a shared feature extraction layer and a subtask network; wherein,

[0045] The final fused features are input into the shared feature extraction layer, and the shared features are extracted using the hybrid neural network model to provide feature inputs for the subtask network. The shared feature extraction layer includes multiple convolution and maximum pooling operations. Each convolution layer generates a new set of feature maps, which are passed to the ReLU activation function to introduce nonlinearity and then downsampled through the maximum pooling layer. After several "convolution + activation + pooling" combinations, a set of shared features for multiple subtasks is obtained.

[0046] In the subtask network, each task has a dedicated independent task layer, which is designed according to the needs of specific tasks. It not only receives shared features from the shared feature extraction layer, but also enhances the ability to generate task-specific features through the conditional generative adversarial network. By introducing conditional information related to the task, it guides the generator to generate feature representations that are more in line with the task requirements.

[0047] Further, in step S4, at least the following steps are included:

[0048] S41, performing motion planning of the quadruped robot based on the stair detection result, and driving the quadruped robot to start stair climbing motion according to the planned path;

[0049] S42. Establish a robot posture and stair parameter feedback model, calculate the stair parameters corresponding to the current posture according to the posture information of the robot during stair climbing, and feed the parameters back to the multi-task learning framework to correct the stair parameter detection results.

[0050] Further, in step S41, the process of performing motion planning of the quadruped robot according to the stair parameter detection result is as follows:

[0051] According to the height h and depth d of each step, plan the robot's stride A and step frequency B when climbing the stairs; adjust the robot's stride size by adjusting the rotation angle of the hip and knee joints;

[0052] After determining the stride length, the stride frequency is further optimized, that is, the time required to complete each step is shortened while ensuring the stability of the robot's body.

[0053] Further, in step S42, the following steps are specifically included:

[0054] S421. During the movement, the robot's posture is monitored in real time through the inertial data of the IMU, and the real-time inclination angle θ of the robot is calculated. real , calculated as follows:

[0055]

[0056] In the formula, a x ,a y ,a z They are the accelerations measured by the IMU along the X, Y, and Z axes of the robot coordinate system; the robot coordinate system is set as a right-handed coordinate system, with the origin defined at the center of the laser radar light source, where the X axis points in the forward direction, the Y axis points to the left, and the Z axis points upward;

[0057] S422. Calculate the leg posture of the robot according to the current rotation angle of the leg motor group of the quadruped robot. For each mechanical leg, which includes a hip joint, a knee joint, a calf and a thigh, the movement of each joint is controlled by a motor. Then, the posture calculation formula for one of the legs is:

[0058]

[0059] In the formula, the hip joint position (x hip ,y hip ,z hip ) is determined according to the relative position and orientation of the robot body; x knee ,z knee Indicates the position of the knee joint, l thigh represents the length of the robot thigh, α represents the rotation angle of the robot hip joint motor, x foot ,z foot Indicates the foothold position, l calf represents the length of the robot's calf, and β represents the rotation angle of the robot's knee joint motor;

[0060] S423, converting the foothold position from the robot coordinate system to the current stair space coordinate system; setting the center of the lower edge of the first step riser as the coordinate origin of the current stair space coordinate system, with the X-axis pointing horizontally forward along the stair forward direction, the Y-axis pointing horizontally to the left, and the Z-axis pointing vertically upward to the horizontal plane. The robot moves in a straight line, i.e., ignoring the displacement in the Y-axis direction, and then obtaining the position of the hip joint in the stair space coordinate system by reverse calculation, thereby obtaining the real-time position of the robot in the current stair space coordinate system;

[0061] S424, according to the posture data of the robot in the current stair space obtained by real-time monitoring, the stair detection result is dynamically adjusted, and the adjusted parameter calculation formula is:

[0062]

[0063] In the formula, θ final ,h final ,d final represents the inclination angle, overall height and depth of the stairs finally obtained after feedback update, θ represents the inclination angle of the stairs in the S3 detection result, h, d represent the height and depth of each step respectively, n real Indicates the number of steps the robot has currently walked, that is, the number of steps the robot has walked obtained through real-time monitoring, L indicates the distance the robot has currently walked, and K indicates the dynamic gain;

[0064] The calculation method of dynamic gain K is:

[0065]

[0066] In the above formula, S is the attitude stability index, which reflects the fluctuation degree of the robot attitude data, which is integrated with the IMU noise σ imu , target posture deviation δ t and external noise σ e Get; target posture deviation δ t represents the deviation between the posture expected by the system and the actual measured posture; E represents the historical fusion error; |P f,i -P| means when you reach the i-th step, the stair parameter P is calculated in real time f,i The difference between the test result P, n history is the number of historical data points and μ is the tuning parameter.

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

[0068] 1) The present invention uses laser radar and camera fusion to perceive the environment, giving full play to the flexible characteristics of the quadruped robot, and can be flexibly deployed in various complex environments;

[0069] 2) The present invention can select appropriate multi-granularity processing methods according to actual needs, avoiding the waste of computing resources due to over-fine processing, while ensuring rapid response at critical moments. This efficient resource management not only improves the robot's operating efficiency, but also reduces energy consumption, providing support for the long-term execution of complex tasks.

[0070] 3) The present invention proposes an adaptive granular tuning strategy and a robot posture-stair parameter feedback model, which effectively improves the accuracy and environmental adaptability of stair detection. The system can dynamically adjust the detection results and parameters according to the robot's posture during stair climbing, thereby achieving a real-time optimized detection strategy and greatly reducing errors caused by detection errors.

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

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

[0073] Figure 1 This is a main flow chart of the staircase detection method according to an embodiment of the present invention;

[0074] Figure 2 This is a diagram of the installation location of the equipment described in an embodiment of the present invention.

[0075] Figure 3 It is a schematic diagram of the stair detection result according to the embodiment of the present invention.

[0076] Figure 4 A schematic diagram of the coordinate system and real-time monitoring parameter calculation according to an embodiment of the present invention. DETAILED DESCRIPTION

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

[0078] Among them, the drawings are only used for illustrative explanations, and they only represent schematic diagrams rather than actual pictures, and should not be understood as limitations on the present invention. In order to better illustrate the embodiments of the present invention, some parts of the drawings may be omitted, enlarged or reduced, and do not represent the size of actual products. For those skilled in the art, it is understandable that some well-known structures and their descriptions in the drawings may be omitted.

[0079] The same or similar numbers in the drawings of the embodiments of the present invention correspond to the same or similar parts; in the description of the present invention, it should be understood that if the terms "upper", "lower", "left", "right", "front", "rear", etc. indicate the orientation or position relationship, they are based on the orientation or position relationship shown in the drawings, which is only for the convenience of describing the present invention and simplifying the description, rather than indicating or implying that the device or element referred to must have a specific orientation, be constructed and operate in a specific orientation. Therefore, the terms describing the position relationship in the drawings are only used for illustrative purposes and cannot be understood as limiting the present invention. For ordinary technicians in this field, the specific meanings of the above terms can be understood according to specific circumstances.

[0080] See also Figure 1 to Figure 4 , a staircase detection method for quadruped robots based on adaptive granularity tuning.

[0081] Example

[0082] like Figure 1 As shown, the present embodiment provides a quadruped robot stair detection method based on adaptive granularity tuning, comprising the following steps:

[0083] S1: using a high-resolution camera and laser radar carried by a quadruped robot to collect environmental data, and preprocessing the collected data, including image denoising, enhancement and point cloud reconstruction of the laser radar data, and then granularly layering the preprocessed sensor data; the step S1 specifically includes the following steps:

[0084] S11: Figure 2 As shown in the figure, the camera and laser radar are installed on the head of the quadruped robot to collect point cloud information and image information of the stairs, and the IMU is built into the head of the quadruped robot to monitor the robot's posture; the OpenCalib calibration toolbox is used to calibrate the camera's intrinsic parameters and the camera-to-lidar extrinsic parameter transformation, and the timestamps of the camera and the laser radar are aligned to ensure the time and space consistency of the collected image and point cloud data;

[0085] S12: Preprocessing the collected sensor data includes two aspects: on the one hand, denoising the image data collected by the camera, using the Gaussian blur denoising method to remove the noise in the image; on the other hand, point cloud reconstruction of the lidar data, integrating the discrete point cloud data into a continuous three-dimensional model, and using voxel grid filtering technology to remove abnormal points;

[0086] S13: The preprocessed sensor data is divided into high-granularity layer, medium-granularity layer and low-granularity layer. The high-granularity layer includes high-resolution images and rich 3D point cloud data, such as the edge, texture and slight slope of each step of the stairs, which are used to detect the fine features of the stairs. The medium-granularity layer includes the number of steps of the stairs and the height and depth of each step, which are used to identify the main structure of the stairs. Figure 3 As shown, the low-granularity layer focuses on larger-scale features, including the overall inclination, overall height and width of the stairs, which are used to confirm the overall size and location of the stairs.

[0087] S2: first extract features of different granularities from the image and the three-dimensional point cloud, then use an adaptive granularity tuning strategy to process multi-granularity features, set tuning parameters, and adaptively adjust the tuning parameters according to different detection environments to adjust the processing methods of features of different granularities in the fusion process, and finally weightedly fuse features of different granularities to obtain the final fusion features; step S2 specifically includes the following steps:

[0088] S21: First, perform feature stratification according to the rules described in S13, extract features of different granularities from the image and 3D point cloud, use MMDetection3D to determine the overall size of the stairs and their position in the scene and extract low-granularity features; use the staircase structure recognition algorithm based on Hough transform to identify and segment the steps at each level and extract medium-granularity features; use the Canny edge detection algorithm to detect the precise edges and detailed textures of the stairs and extract high-granularity features;

[0089] S22: Adopting adaptive granularity tuning strategy to fuse the multi-granularity features extracted from S21:

[0090] S221: First, the complexity of the environment must be calculated. When a robot enters an unfamiliar environment, it needs to evaluate the complexity of the environment. Usually, a quadruped robot climbs up the stairs by climbing forward, so the complexity of the environment is calculated based on the complexity of the staircase information in front of the robot obtained by the laser radar and camera, as shown in formula (1):

[0091] C=a·Q+b·R V +c·R L (1)

[0092] Where C represents the complexity of the environment, Q represents the noise level of the sensor input, including the data noise from the camera and lidar, and R V Represents the complexity of visual information, including the feature richness and visual interference in the image, R L It represents the complexity of point cloud information, including the richness of spatial information and the integrity of point cloud data. a, b, and c represent adjustment parameters, which are used to balance the value of various factors in the assessment of environmental complexity.

[0093] S222: Then set the tuning parameter, which is related to the environmental complexity and determines the weight of different granularity features in multi-granularity fusion; according to the evaluation result of the environmental complexity C, the system will adaptively adjust the tuning parameters of different granularity features in the fusion. When the evaluation result of the environmental complexity is high, the fusion result is more inclined to high-granularity features, and the medium-granularity tuning parameter is reduced; otherwise, the high-granularity tuning parameter is reduced and the medium-granularity tuning parameter is increased; the low-granularity tuning parameter remains stable to ensure the accuracy of the overall stair detection. The calculation formula of the tuning parameters of different granularity levels is shown in formula (2):

[0094]

[0095] : Among them, ω h ,ω m ,ω l They represent the tuning parameters of high, medium and low granularities respectively. The higher the value of the tuning parameter, the greater the weight of the features at the corresponding granularity level in the multi-granularity feature fusion. e is a natural constant, and λ is a hyperparameter that can be adjusted according to actual conditions. λ>0;

[0096] S223: Perform weighted fusion on features of different granularities according to the tuning parameters to obtain the final fusion feature F combined , the specific calculation formula is shown in formula (3):

[0097] F combined =ω h ·F h +ω m ·F m +ω l ·F l (3)

[0098] Among them, F h ,F m ,F l They represent the features of three different granularities, high, medium and low, extracted in step S21 respectively.

[0099] S3: The final fusion feature obtained in S2 is input into the multi-task learning framework, and the shared feature extraction layer and the independent task layer are used to realize the simultaneous estimation of multiple key parameters of the stairs, and the joint loss function and the adaptive weighting mechanism are used to reasonably allocate the loss weights of each task, focusing on the tasks with large errors, and using the gradient-based task correlation analysis method to measure the correlation between tasks to ensure that the tasks can promote each other during the training process, and finally output the stair detection results; the step S3 specifically includes:

[0100] S31: The final fusion feature F obtained in S23 combined The shared feature extraction layer is input into the multi-task learning framework, and the shared features are extracted from the final fused features using a hybrid neural network model. The extracted shared features will be used for subsequent processing of all tasks. An independent task layer is designed for each task (such as the detection of the inclination, width, height and depth of each step of the stairs). These independent task layers will further process the shared features and output their own detection results.

[0101] S311: The network structure of the multi-task learning framework is divided into two parts, the shared feature extraction layer and the sub-task network; the shared feature extraction layer is the core part of the entire multi-task learning framework. It inputs the final fusion feature obtained in S23, uses the hybrid neural network model to extract shared features, and provides feature inputs for the sub-task network. The shared feature extraction layer contains multiple convolution and maximum pooling operations. Each convolution layer generates a new set of feature maps, which are then passed to the ReLU activation function to introduce nonlinearity, and then downsampled through the maximum pooling layer. After multiple such "convolution + activation + pooling" combinations, a set of shared features that are useful for multiple sub-tasks is obtained; during the training process, this layer is trained to be independent of the specific task, that is, it only focuses on how to extract useful features from the input data, and does not care about how these features will be used by subsequent independent task layers;

[0102] S312: In the subtask network, each task has a dedicated independent task layer. These independent task layers are designed according to the needs of specific tasks. They not only receive shared features from the shared feature extraction layer, but also integrate the idea of ​​conditional generative adversarial networks. Conditional generative adversarial networks are used here to enhance the ability to generate task-specific features. By introducing conditional information related to the task, the generator is guided to generate feature representations that better meet the task requirements.

[0103] S32: A joint loss function Loss is designed in the multi-task learning framework, whose value is the weighted sum of all task losses, as shown in formula (4); in order to adjust the weights between tasks according to the situation, a gradient-based adaptive weighting mechanism is introduced. This mechanism gives higher weights to tasks with larger gradients, thereby paying more attention to tasks with larger errors during training. The weight γ i The update rule of is shown in formula (5):

[0104]

[0105] Among them, Loss i represents the loss function of task i, γ i represents the weight of task i, N is the total number of tasks, Represent the gradients of tasks i and j respectively;

[0106] S33: At the same time, the present invention introduces a gradient-based task correlation analysis method for measuring the correlation between different tasks. By calculating the gradient inner product of task i and task j, the correlation between the two tasks is obtained, as shown in formula (6):

[0107]

[0108] Among them, Corr(M i ,M j ) represents the correlation between task i and task j. If Corr(M i ,M j )>0, then task i and task j have a positive correlation, and the training process will promote each other; if Corr(M i ,M j )<0, then task i and task j have a negative correlation, and the weights need to be adjusted in time during training to avoid mutual interference;

[0109] S34: Output the detection results of various parameters of the stairs, which can be expressed by a parameter set P, as shown in formula (7):

[0110] P={n,θ,W,h,H,d,D} (7)

[0111] Where n represents the number of steps in this section of stairs, θ represents the inclination angle of this section of stairs, W represents the width of this section of stairs, H represents the overall height of this section of stairs, and D represents the overall depth of this section of stairs. Respectively represent the height and depth of each step.

[0112] S4: planning the robot's stair climbing motion according to the stair detection result; proposing a robot posture-stair parameter feedback model, which first calculates the real-time robot posture information according to the IMU data and the kinematic equation of the quadruped robot, analyzes the stair parameters corresponding to the current posture of the robot, and then feeds back the corrected stair detection result to ensure the accuracy of the stair detection; the step S4 specifically includes the following steps:

[0113] S41: Based on the stair parameter detection results obtained in S3, the quadruped robot is motion-planned, and the quadruped robot starts stair climbing according to the planned path; based on the height h and depth d of each step, the robot's stride A and step frequency B are planned when climbing the stairs. The main purpose is to adjust the robot's stride size by adjusting the rotation angles of the hip joint and the knee joint to ensure that each step of the robot can take a step up. After determining the stride, in order to further adjust the robot's movement rhythm and avoid imbalance problems caused by too fast or too large movements, it is necessary to optimize the step frequency, that is, on the premise of ensuring the stability of the robot's body, try to shorten the time required to complete each step, thereby improving the overall climbing speed. Therefore, the step frequency is determined by the overall height, depth and stride of the stairs. After determining the stride and step frequency, the robot starts to climb the stairs according to the specified stride and step frequency.

[0114] S42: A robot posture-stair parameter feedback model is proposed. This model uses the posture information of the robot during stair climbing to calculate the stair parameters corresponding to the current posture, and feeds these parameters back to step S3 to correct the stair parameter detection results:

[0115] S421: First, during the movement, the robot's posture is monitored in real time through the inertial data of the IMU, and the real-time inclination angle θ of the robot is calculated. real , as shown in formula (8):

[0116]

[0117] where a x ,a y ,a z They are the accelerations measured by the IMU along the X, Y, and Z axes of the robot coordinate system; Figure 4 As shown, the robot coordinate system is set as a right-hand coordinate system, and the origin of the coordinate is defined at the center of the laser radar light source, where the X-axis points to the forward direction, the Y-axis points to the left, and the Z-axis points directly upward;

[0118] S422: Then, the postures of the four legs of the robot are calculated according to the current rotation angle of the motor group of the quadruped robot leg. Usually, each leg of the quadruped robot is composed of a hip joint, a knee joint, a calf and a thigh, wherein the movement of each joint is controlled by a motor. Then, the posture calculation formula of one leg is shown in formula (9):

[0119]

[0120] The hip joint position (x hip ,y hip ,z hip ) can be determined based on the relative position and orientation of the robot body. Since the y coordinates of the entire leg are the same, no calculation is done here; x knee ,z knee Indicates the position of the knee joint, l thigh represents the length of the robot thigh, α represents the rotation angle of the robot hip joint motor, x foot ,z foot Indicates the foothold position, l calf represents the length of the robot's calf, and β represents the rotation angle of the robot's knee joint motor. Similarly, the postures of the other three legs can be obtained;

[0121] S423: Then, the foothold position is converted from the robot coordinate system to the current stair space coordinate system, as shown in formula (10), the coordinate origin of the current stair space coordinate system is set at the center of the lower edge of the first step riser, wherein the X-axis points horizontally forward along the direction of the stair, the Y-axis points horizontally to the left, and the Z-axis points vertically to the horizontal plane upward, which conforms to the right-hand rule; for the convenience of calculation, the present invention considers that the robot's motion path is a straight line, and ignores the displacement in the Y-axis direction; then, the positions of the four hip joints in the stair space coordinate system are reversely calculated by formula (9), and then the real-time position of the robot in the current stair space coordinate system is obtained;

[0122]

[0123] Where (x cur ,y cur ,z cur ) represents the landing point of one leg of the robot in the current stair space coordinate system. Similarly, the landing points of the other three legs can be obtained. (x0, y0, z0) represents the origin of the current stair space coordinate system.

[0124] S424: According to the posture data of the robot in the current stair space obtained through real-time monitoring, the stair detection result is dynamically adjusted to improve the accuracy and adaptability of the detection. The specific calculation formula is shown in formula (11):

[0125]

[0126] where θ final ,h final ,d finalrepresents the inclination angle, overall height and depth of the stairs finally obtained after feedback update, θ represents the inclination angle of the stairs in the S3 detection result, h, d represent the height and depth of each step respectively, n real represents the number of steps currently walked by the robot, that is, the number of steps walked by the robot obtained by real-time monitoring, L represents the distance currently walked by the robot, K represents the dynamic gain, and the calculation formula is shown in formula (12):

[0127]

[0128] Where S is the attitude stability index, which reflects the fluctuation degree of the robot attitude data. imu , target posture deviation δ t and external noise σ e The target posture deviation δ is obtained t represents the deviation between the posture expected by the system and the actual measured posture, E represents the historical fusion error, |P f,i -P| means when you reach the i-th step, the stair parameter P is calculated in real time f,i The difference between the detection result P obtained by S3, n history is the number of historical data points and μ is the tuning parameter.

[0129] 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 quadruped robot stair detection method based on adaptive granularity tuning, characterized by: At least the following steps are included: S1, obtain environmental data through the robot's camera and laser radar, pre-process the environmental data, and then perform granular stratification on the pre-processed environmental data; S2, using an adaptive granularity tuning strategy to process features of different granularities extracted from images and three-dimensional point clouds of environmental data, and weightedly fusing features of different granularities to obtain fused features; S3. Input the fused features into the multi-task learning framework to estimate multiple key parameters of the stairs. A joint loss function and an adaptive weighting mechanism are used to assign loss weights to each task, and a gradient-based task correlation analysis method is used to measure the correlation between tasks. Finally, the stair detection results are output; S4. Plan the movement of the quadruped robot according to the stair detection results, and analyze the stair parameters corresponding to the current posture through the robot posture and stair parameter feedback model, and continuously feedback and correct the stair detection results.

2. The quadruped robot staircase detection method based on adaptive granularity tuning according to claim 1 is characterized in that: In step S1, at least the following steps are included: S11, installing a camera and a laser radar on the head of the quadruped robot to collect image information and point cloud information of the stairs; building an IMU into the head of the quadruped robot to monitor the posture of the robot; wherein the intrinsic parameters of the camera and the extrinsic parameter transformation from the camera to the laser radar are pre-calibrated; S12, preprocessing the acquired environmental data, wherein Gaussian blur noise reduction method is used for the image information collected by the camera to reduce regional noise; point cloud information collected by the laser radar is reconstructed to integrate the discrete point cloud data into a continuous three-dimensional model, and outliers are removed by voxel grid filtering technology; S13. The preprocessed environmental data is stratified into high-granularity layer, medium-granularity layer and low-granularity layer, wherein the high-granularity layer is used to detect fine features of the stairs, including at least high-resolution images and point cloud information of the stair edge, texture and slope of each step; the medium-granularity layer is used to identify the stair structure, including at least the number of stair steps and the height and depth of each step; the low-granularity layer is used to identify the overall size and position of the stairs, including at least the overall inclination, overall height and overall width of the stairs.

3. The quadruped robot staircase detection method based on adaptive granularity tuning according to claim 1 is characterized in that: In step S2, at least the following steps are included: S21. According to the granularity stratification results, different granularity features are extracted, wherein the Canny edge detection algorithm is used to extract high granularity features, the staircase structure recognition algorithm based on Hough transform is used to extract medium granularity features, and the MMDetection3D target detection platform is used to extract low granularity features; S22, using adaptive granularity tuning strategy to fuse hierarchically extracted high-granularity features, medium-granularity features, and low-granularity features, and output the final fusion feature F combined .

4. The quadruped robot staircase detection method based on adaptive granularity tuning according to claim 3 is characterized in that: In step S22, the following steps are specifically included: S221. Calculate the environment complexity C based on the staircase information in front of the robot obtained by the laser radar and the camera. The formula is: C=a·Q+b·R V +c·R L Where Q represents the noise level of the sensor input, which includes the data noise of the camera and lidar; R V Represents the complexity of visual information, which includes the feature richness and visual noise in the image; R L Indicates the complexity of point cloud information, which includes the richness of spatial information and the integrity of point cloud data; a, b, c represent adjustment parameters, which are used to balance the value of various factors in the evaluation of environmental complexity; S222, setting tuning parameters. The tuning parameters are related to the environment complexity C, which determines the fusion weights of features of different granularities. The calculation method of tuning parameters of different granularity levels is: In the formula, ω h ,ω m ,ω l They represent the tuning parameters of high, medium and low granularities respectively. The higher the value of the tuning parameter, the greater the weight of the features at the corresponding granularity level in the multi-granularity feature fusion; e is a natural constant; λ>0 is a hyperparameter, which can be adjusted according to actual conditions; S223: weighted fusion of features of different granularities according to tuning parameters to obtain the final fusion feature F combined , and its calculation formula is: F combined =ω h ·F h +oh m ·F m +oh l ·F l In the formula, F h ,F m ,F l are the granularity features extracted from the high granularity layer, the medium granularity layer, and the low granularity layer, respectively.

5. The quadruped robot staircase detection method based on adaptive granularity tuning according to claim 1, characterized in that: In step S3, at least the following steps are included: S31, extracting shared features from fused features through the shared feature extraction layer of the multi-task learning framework, establishing independent task layers for all tasks, inputting the shared features into the independent task layers for further processing, and obtaining the detection results of each task; S32. Establish a joint loss function Loss for the multi-task learning framework, whose value is the weighted sum of all task losses, expressed as: In the formula, Loss i Table S represents the loss function of task i, γ i represents the weight of task i, and N is the total number of tasks; Introduce a gradient-based adaptive weighting mechanism to adjust the weight γ i , so that the tasks with larger gradients get higher weights, and the update rule is as follows: In the formula, Represent the gradients of tasks i and j respectively; S33, the gradient-based task correlation analysis method is then used to measure the correlation between different tasks. The correlation between the two tasks is obtained by calculating the gradient inner product of task i and task j. The calculation method is: In the formula, Corr(M i ,M j ) represents the correlation between task i and task j. If Corr(M i ,M j )>0, then task i and task j have a positive correlation; if Corr(M i ,M j )<0, then task i and task j have a negative correlation; S34, output the detection results of various parameters of the stairs, which are represented by a parameter set P: P={n,θ,W,h,H,d,D} In the formula, n represents the number of steps of the stairs, θ represents the inclination angle of the stairs, W represents the width of the stairs, H represents the overall height of the stairs, and D represents the overall depth of the stairs. Respectively represent the height and depth of each step.

6. The quadruped robot staircase detection method based on adaptive granularity tuning according to claim 5 is characterized in that: In step S31, the network structure of the multi-task learning framework is divided into a shared feature extraction layer and a subtask network; wherein, The final fused features are input into the shared feature extraction layer, and the shared features are extracted using the hybrid neural network model to provide feature inputs for the subtask network. The shared feature extraction layer contains multiple convolution and maximum pooling operations. Each convolution layer generates a new set of feature maps. The generated feature maps are passed to the ReLU activation function to introduce nonlinearity, and then downsampled through the maximum pooling layer. After several "convolution + activation + pooling" combinations, a set of shared features for multiple subtasks is obtained. In the subtask network, each task has a dedicated independent task layer, which is designed according to the needs of specific tasks. It not only receives shared features from the shared feature extraction layer, but also enhances the ability to generate task-specific features through the conditional generative adversarial network. By introducing conditional information related to the task, it guides the generator to generate feature representations that are more in line with the task requirements.

7. The quadruped robot staircase detection method based on adaptive granularity tuning according to claim 1, characterized in that: In step S4, at least the following steps are included: S41, performing motion planning of the quadruped robot based on the stair detection result, and driving the quadruped robot to start stair climbing motion according to the planned path; S42. Establish a robot posture and stair parameter feedback model, calculate the stair parameters corresponding to the current posture according to the posture information of the robot during stair climbing, and feed the parameters back to the multi-task learning framework to correct the stair parameter detection results.

8. The quadruped robot staircase detection method based on adaptive granularity tuning according to claim 7, characterized in that: In step S41, the process of performing motion planning of the quadruped robot according to the stair parameter detection result is as follows: According to the height h and depth d of each step, plan the robot's stride A and step frequency B when climbing the stairs; adjust the robot's stride size by adjusting the rotation angle of the hip and knee joints; After determining the stride length, the stride frequency is further optimized, that is, the time required to complete each step is shortened while ensuring the stability of the robot's body.

9. The quadruped robot staircase detection method based on adaptive granularity tuning according to claim 7, characterized in that: In step S42, the following steps are specifically included: S421. During the movement, the robot's posture is monitored in real time through the inertial data of the IMU, and the real-time inclination angle θ of the robot is calculated. real , calculated as follows: In the formula, a x ,a y ,a z They are the accelerations measured by the IMU along the X, Y, and Z axes of the robot coordinate system; the robot coordinate system is set as a right-handed coordinate system, with the origin defined at the center of the laser radar light source, where the X axis points in the forward direction, the Y axis points to the left, and the Z axis points upward; S422. Calculate the leg posture of the robot according to the current rotation angle of the leg motor group of the quadruped robot. For each mechanical leg, which includes a hip joint, a knee joint, a calf and a thigh, the movement of each joint is controlled by a motor. Then, the posture calculation formula for one of the legs is: In the formula, the hip joint position (x hip ,y hip ,z hip ) is determined according to the relative position and orientation of the robot body; x knee ,z knee Indicates the position of the knee joint, l thigh represents the length of the robot thigh, α represents the rotation angle of the robot hip joint motor, x foot ,z foot Indicates the foothold position, l calf represents the length of the robot's calf, and β represents the rotation angle of the robot's knee joint motor; S423, converting the foothold position from the robot coordinate system to the current stair space coordinate system; setting the center of the lower edge of the first step riser as the coordinate origin of the current stair space coordinate system, with the X-axis pointing horizontally forward along the stair forward direction, the Y-axis pointing horizontally to the left, and the Z-axis pointing vertically upward to the horizontal plane. The robot moves in a straight line, i.e., ignoring the displacement in the Y-axis direction, and then obtaining the position of the hip joint in the stair space coordinate system by reverse calculation, thereby obtaining the real-time position of the robot in the current stair space coordinate system; S424, according to the posture data of the robot in the current stair space obtained by real-time monitoring, the stair detection result is dynamically adjusted, and the adjusted parameter calculation formula is: In the formula, θ final ,h final ,d final represents the inclination angle, overall height and depth of the stairs finally obtained after feedback update, θ represents the inclination angle of the stairs in the S3 detection result, h, d represent the height and depth of each step respectively, n real Indicates the number of steps the robot has currently walked, that is, the number of steps the robot has walked obtained through real-time monitoring, L indicates the distance the robot has currently walked, and K indicates the dynamic gain; The calculation method of dynamic gain K is: In the above formula, S is the attitude stability index, which reflects the fluctuation degree of the robot attitude data, which is integrated with the IMU noise σ imu , target posture deviation δ t and external noise σ e Get; target posture deviation δ t represents the deviation between the posture expected by the system and the actual measured posture; E represents the historical fusion error; |P f,i -P| means when you reach the i-th step, the stair parameter P is calculated in real time f,i The difference between the test result P, n history is the number of historical data points and μ is the tuning parameter.

Images