Autonomous navigation and grasping control method and system for body-aware robot

Abstract

The application provides an autonomous navigation and grabbing control method and system of a body intelligent robot, and relates to the technical field of robot control. The method comprises the following steps: collecting a medium frequency signal during robot operation and a speckle image sequence during grabbing of a target bin; then performing two-dimensional fast Fourier transform and average constant false alarm detection on the medium frequency signal to generate millimeter wave point cloud data, and performing enhanced scanning on a running target area with a radial velocity greater than a preset speed threshold to generate laser point cloud data; subsequently, applying a multiple hypothesis tracking algorithm to fuse the two point cloud data to obtain predicted trajectory data of a dynamic obstacle, and mapping the predicted trajectory data into an expanded obstacle area through a speed obstacle method, and calculating a motion control instruction; and simultaneously, dynamically adjusting a grabbing control parameter of an end effector through a digital image correlation method according to the speckle image sequence to complete grabbing of the target bin. The application realizes safe navigation of the robot and adaptive grabbing control of the bin.

Description

Technical Field

[0001] This application relates to the technical field of robot control, and in particular to an autonomous navigation and grasping control method and system for an embodied intelligent robot. Background Technology

[0002] In automated warehousing systems, embodied intelligent robots are key equipment. Their autonomous navigation and stable grasping capabilities directly affect logistics efficiency and operational safety. Moreover, in dynamic warehouse scenarios that include high-speed moving obstacles and the need to handle easily deformable boxes, the reliable operation and adaptive operation of robots have significant application value.

[0003] Currently, embodied intelligent robots typically rely on LiDAR for environmental modeling and navigation, while their grasping actions are based on preset position programs or simple force feedback control.

[0004] However, existing methods face a dual challenge: on the one hand, in dynamic environments where obstacles move at high speeds and randomly, navigation systems that rely solely on instantaneous environmental perception struggle to make continuous and reliable predictions of obstacle movement trends, leading to delayed or overly conservative obstacle avoidance decisions; on the other hand, when faced with bins that experience localized elastic deformation due to material properties or stress, traditional gripping control based on fixed parameters cannot sense and adapt to the actual deformation state of the gripping point, and is prone to gripping failure or damage to goods. Summary of the Invention

[0005] The purpose of this application is to provide an autonomous navigation and grasping control method and system for an embodied intelligent robot, so as to solve the problem in the prior art that it is difficult to achieve real-time accurate obstacle avoidance and robust adaptive grasping of deformable targets in dynamic and complex environments.

[0006] To address the aforementioned technical problems, in a first aspect, this application provides an autonomous navigation and grasping control method for an embodied intelligent robot, comprising:

[0007] The intermediate frequency signal during robot operation and the speckle image sequence when the robot grasps the target hopper were collected;

[0008] The intermediate frequency signal is processed by two-dimensional fast Fourier transform in the distance and velocity dimensions to generate a two-dimensional image, and the two-dimensional image is subjected to average constant false alarm detection to generate millimeter-wave point cloud data.

[0009] Based on the millimeter-wave point cloud data, the target area with a radial velocity greater than a preset velocity threshold is identified, and the target area is subjected to local enhanced scanning to generate laser point cloud data.

[0010] The millimeter-wave point cloud data and the laser point cloud data are fused using a multi-hypothesis tracking algorithm to obtain the predicted trajectory data of the dynamic obstacle, and the velocity obstacle method is used to map the predicted trajectory data into the final inflated obstacle area.

[0011] The robot's motion control commands are calculated based on the inflated barrier region. Simultaneously, the robot's end effector's grasping control parameters are dynamically adjusted using digital image correlation based on the speckle image sequence, and the target material box is grasped according to the grasping control parameters.

[0012] Optionally, the application of the multi-hypothesis tracking algorithm fuses the millimeter-wave point cloud data with the laser point cloud data to obtain predicted trajectory data of the dynamic obstacle, and uses the velocity obstacle method to map the predicted trajectory data into the final inflated obstacle region, including:

[0013] Extract a first sub-point cloud data that matches the target area from the millimeter-wave point cloud data, and combine the laser point cloud data with the first sub-point cloud data in coordinate system 1 to generate fused point cloud data.

[0014] Clustering is performed on the fused point cloud data to obtain multiple target point cloud data. A multi-hypothesis tracking algorithm is applied to each target point cloud data, and the trajectory is initialized using the radial velocity value in the first sub-point cloud data after coordinate system one to obtain an initial trajectory set.

[0015] Based on the fused point cloud data, the initial trajectory set is updated in state and probability is calculated to obtain the predicted trajectory data of the dynamic obstacle;

[0016] The predicted trajectory data, the robot's kinematic parameters, and dynamic parameters are input into the trajectory evaluation model for evaluation processing to obtain the threat level of dynamic obstacles to the robot. Trajectories with threat levels exceeding a preset threat threshold are then selected from the predicted trajectory data to obtain key trajectory data.

[0017] The velocity obstacle method is applied to calculate the initial expanding obstacle zone in the robot's velocity plane based on the expected future position sequence of dynamic obstacles represented by the key trajectory data.

[0018] Calculate the rate of change of the expansion barrier region in this round and the previous round;

[0019] The initial inflated barrier region is used as a spatial constraint and fed back into the calculation process of the multi-hypothesis tracking algorithm. The steps of state update, probability calculation, evaluation processing, screening, barrier region calculation, and rate of change calculation are repeated until the rate of change of the inflated barrier region in two consecutive rounds reaches the preset convergence condition to generate the final inflated barrier region.

[0020] Optionally, the step of inputting the predicted trajectory data, the robot's kinematic parameters, and dynamic parameters into a trajectory evaluation model for evaluation processing to obtain the threat level of the dynamic obstacle to the robot includes:

[0021] The feature extraction module of the trajectory evaluation model obtains the relative motion state of the dynamic obstacle with respect to the robot from the predicted trajectory data.

[0022] The demand calculation module of the trajectory evaluation model calculates the braking deceleration or steering curvature required for the robot to avoid collision with dynamic obstacles based on the relative motion state, and generates motion demand data.

[0023] The comparison module of the trajectory evaluation model compares the motion demand data with the maximum braking deceleration and maximum steering curvature obtained from the robot's kinematic and dynamic parameters one by one, and identifies the exceeding items in the motion demand data that exceed the maximum braking deceleration or the maximum steering curvature.

[0024] The threat level is obtained by calculating the number of overtaking terms and the relative positions corresponding to the overtaking terms using the quantization module of the trajectory evaluation model according to a preset weighting rule.

[0025] Optionally, the step of dynamically adjusting the gripping control parameters of the robot's end effector when gripping the target bin using digital image correlation based on the speckle image sequence includes:

[0026] The speckle image sequence is subjected to subpixel matching calculation by digital image correlation method to obtain the two-dimensional displacement field of the target bin gripping surface. The two-dimensional displacement field is then subjected to spatial differentiation operation to obtain the full-field strain field of the target bin gripping surface.

[0027] Extract strain concentration regions and corresponding principal strain directions from the full-field strain field where the deformation degree is greater than a preset deformation degree threshold. Based on the spatial distribution of the strain concentration regions and the principal strain directions, identify the vulnerable areas and potential slip directions of the target material box.

[0028] The location corresponding to the vulnerable area is converted into the attenuation coefficient in the expected stiffness matrix of the end effector, and the potential slip direction is converted into the force axis direction of the end effector that needs to be strengthened in the grasping force coordinate system.

[0029] The average strain energy density of the full-field strain field is converted into the expected basic value of the gripping force. The compliant control algorithm is then invoked, and the attenuation coefficient, the force axis direction, and the expected basic value are used as input parameters to perform parameter reconstruction and force control calculation to obtain the gripping control parameters.

[0030] Optionally, calculating the robot's motion control commands based on the inflated barrier region includes:

[0031] Based on the change of the inflated barrier region in the velocity space over time, the feasible set of the robot's initial velocity is defined;

[0032] The threat level is input into a constraint regulator. The constraint regulator dynamically calculates the relaxation amount of the initial velocity feasible set based on the threat level, and adjusts the initial velocity feasible set according to the relaxation amount to generate the target velocity feasible set.

[0033] Based on the robot's current speed capability range, a speed sample set is generated, and based on the robot's kinematic model, the trajectory of each speed sample in the speed sample set is deduced to obtain the predicted trajectory corresponding to each speed sample.

[0034] Using the target velocity feasible set as a constraint, the feasibility of the predicted trajectory is checked, and the predicted trajectory that passes the check is taken as a feasible trajectory. A multi-criteria decision model is used to comprehensively evaluate all the feasible trajectories and obtain the evaluation result corresponding to each feasible trajectory.

[0035] The velocity sample corresponding to the optimal feasible trajectory in the evaluation results is taken as the optimal velocity command, and the optimal velocity command is converted into the motion control command of the robot through the inverse kinematics model.

[0036] Optionally, the step of performing a two-dimensional fast Fourier transform on the intermediate frequency signal in both the distance and velocity dimensions to generate a two-dimensional graph includes:

[0037] The intermediate frequency signal is segmented to obtain multiple continuous signal segments, wherein the time length of the signal segment is jointly determined based on the robot's maximum expected movement speed and the wavelength parameters of the millimeter-wave radar.

[0038] Perform a Fast Fourier Transform on each signal segment to generate a distance spectrum corresponding to the signal segment, and arrange the distance spectra corresponding to all the signal segments in chronological order to form a distance spectrum sequence;

[0039] Perform a fast Fourier transform on the velocity dimension of the distance spectrum sequence to obtain two-dimensional spectrum data;

[0040] Based on the robot's real-time motion state data, the clutter region generated by the robot's own motion in the two-dimensional image is determined, and the clutter is suppressed on the two-dimensional spectral data to generate the final two-dimensional image for target detection.

[0041] Optionally, the step of performing average constant false alarm rate (CFAR) detection on the two-dimensional image to generate millimeter-wave point cloud data includes:

[0042] Based on the clutter region, a reference cell is defined in the two-dimensional diagram around each grid cell, and the background noise value of each grid cell is calculated based on the reference cell.

[0043] By combining the background noise value with the preset constant false alarm rate, the detection threshold corresponding to each grid cell is calculated, and the actual amplitude value of the grid cell is compared with the corresponding detection threshold to obtain the potential target point;

[0044] Peak clustering is performed on all the potential target points to obtain multiple target point clusters, and the centroid is extracted from each target point cluster to obtain the distance cell index and velocity cell index corresponding to the potential target point.

[0045] Based on the range unit index and velocity unit index, and combined with the system parameters of the millimeter-wave radar, the three-dimensional spatial coordinates, radial velocity, and azimuth angle of each potential target point are calculated. All the three-dimensional spatial coordinates, radial velocity, and azimuth angle are combined to generate millimeter-wave point cloud data.

[0046] Secondly, this application provides an autonomous navigation and grasping control system for an embodied intelligent robot, comprising:

[0047] The acquisition module is used to acquire the intermediate frequency signal during robot operation and the speckle image sequence when the robot grasps the target hopper;

[0048] The detection module is used to perform two-dimensional fast Fourier transform processing on the intermediate frequency signal in the distance and velocity dimensions to generate a two-dimensional image, and to perform average constant false alarm detection on the two-dimensional image to generate millimeter-wave point cloud data.

[0049] The enhancement module is used to identify the target area with a radial velocity greater than a preset velocity threshold based on the millimeter-wave point cloud data, and to perform local enhancement scanning on the target area to generate laser point cloud data.

[0050] The fusion module is used to fuse the millimeter-wave point cloud data and the laser point cloud data using a multi-hypothesis tracking algorithm to obtain the predicted trajectory data of the dynamic obstacle, and to map the predicted trajectory data into the final inflated obstacle area using the velocity obstacle method.

[0051] The adjustment module is used to calculate the motion control command of the robot based on the expansion barrier area, and dynamically adjust the grasping control parameters of the robot's end effector when grasping the target box according to the speckle image sequence using digital image correlation, and grasp the target box according to the grasping control parameters.

[0052] Thirdly, this application provides an electronic device, comprising:

[0053] Memory, used to store computer programs;

[0054] A processor is configured to execute the computer program to implement the steps of the autonomous navigation and grasping control method for an embodied intelligent robot as described in the first aspect above.

[0055] Fourthly, this application provides a computer-readable storage medium storing a computer program that, when executed by a processor, can implement the steps of the autonomous navigation and grasping control method for an embodied intelligent robot as described in the first aspect above.

[0056] The autonomous navigation and grasping control method for embodied intelligent robots provided in this application has the following beneficial effects: This application provides a data foundation for navigation and grasping by collecting intermediate frequency signals and grasping images during robot operation; then, the intermediate frequency signals are transformed and detected to generate millimeter-wave point clouds to effectively identify distant dynamic obstacles; then, local laser scanning is guided based on velocity characteristics to improve the positioning accuracy of high-speed targets; then, the two types of point clouds are fused and the trajectory is predicted through a multi-hypothesis tracking algorithm, and then mapped into an expanded obstacle region in velocity space through a velocity obstacle method, which can provide the robot with a clear safe collision avoidance boundary; finally, motion commands are generated based on this boundary to achieve real-time obstacle avoidance, and the grasping parameters are dynamically adjusted according to the image sequence, thereby realizing the coordinated operation of real-time accurate obstacle avoidance and robust adaptive grasping of deformable targets.

[0057] Furthermore, this application enhances the accuracy and reliability of obstacle prediction and safety boundary generation in dynamic environments by clustering the fused point cloud, initializing and iteratively updating the trajectory, and combining the robot's own capabilities to assess threats and screen key trajectories, and then calculating the expanded obstacle region and feeding it back to the tracking process for iterative convergence. Attached Figure Description

[0058] To more clearly illustrate the technical solutions of the embodiments of this application or the prior art, the drawings used in the description of the embodiments or the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of this application. For those skilled in the art, other drawings can be obtained based on these drawings without creative effort.

[0059] Figure 1 A flowchart illustrating an autonomous navigation and grasping control method for an embodied intelligent robot provided in an embodiment of this application;

[0060] Figure 2A schematic diagram illustrating a specific implementation of an autonomous navigation and grasping control method for an embodied intelligent robot provided in this application embodiment;

[0061] Figure 3 A schematic diagram of the structure of an autonomous navigation and grasping control system for an embodied intelligent robot provided in this application embodiment;

[0062] Figure 4 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0063] In complex scenarios such as dynamic warehouses, existing robot navigation systems struggle to reliably predict the movement trends of high-speed obstacles during transport, leading to untimely obstacle avoidance. Simultaneously, their grasping control cannot detect local deformations of the material box, easily causing unstable grasping. These two deficiencies together limit the robot's operational capabilities in such environments.

[0064] To address this, this application proposes an autonomous navigation and grasping control method for an embodied intelligent robot. The core idea of ​​this method is to achieve continuous tracking and accurate trajectory prediction of dynamic obstacles through the collaborative scanning of millimeter-wave radar and lidar, and plan a safe obstacle avoidance path accordingly. At the same time, by visually analyzing the deformation state of the grasping surface, the grasping force and pose are adjusted in real time. Thus, the response capability to sudden movements is improved at the navigation level, the adaptability to deformable targets is enhanced at the grasping level, and the reliability and accuracy of operation in dynamic environments are solved in a coordinated manner.

[0065] To enable those skilled in the art to better understand the present application, the present application will be further described in detail below with reference to the accompanying drawings and specific embodiments. Obviously, the described embodiments are merely some embodiments of the present application, and not all embodiments. Based on the embodiments in this application, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of this application.

[0066] The core of this application is to provide an autonomous navigation and grasping control method for an embodied intelligent robot, and a flowchart of one specific implementation is shown below. Figure 1 As shown, the method includes:

[0067] S101. Collect the intermediate frequency signal of the robot during operation and the speckle image sequence when the robot grabs the target hopper.

[0068] Among them, the intermediate frequency signal refers to the processed signal generated by the robot's vehicle-mounted millimeter-wave radar when detecting targets. It is between transmitting high-frequency signals and receiving low-frequency baseband signals and directly carries key information such as the distance and speed of obstacles in front.

[0069] A speckle image sequence refers to a set of images formed by a series of images of a target bin surface captured by an industrial camera mounted at the end of a robot under specific structured light illumination. The speckles in these images will shift as the target bin is subjected to force and deformation.

[0070] In step S101, the millimeter-wave radar first continuously transmits and receives electromagnetic waves, and after internal mixing processing, outputs an intermediate frequency signal containing obstacle information; at the same time, when the robot end effector approaches the target bin, the industrial camera turns on and triggers the structured light projector, and continuously captures the surface of the illuminated area of ​​the target bin at a fixed frame rate, thereby obtaining a set of speckle image sequences that change over time.

[0071] S102. Perform two-dimensional fast Fourier transform processing on the intermediate frequency signal in the distance and velocity dimensions to generate a two-dimensional graph, and perform average constant false alarm detection on the two-dimensional graph to generate millimeter-wave point cloud data.

[0072] In one specific implementation, step S102 includes:

[0073] Step 1021: The intermediate frequency signal is segmented to obtain multiple continuous signal segments. The time length of the signal segment is determined jointly based on the robot's maximum expected movement speed and the wavelength parameters of the millimeter-wave radar.

[0074] In step 1021, the continuously acquired intermediate frequency signals are sorted according to their time length. Cutting is performed, among which... For the length of time, For millimeter-wave radar wavelength, The maximum expected speed of the robot is given, thus a series of signal segments that are connected end to end in time are obtained.

[0075] Step 1022: Perform a Fast Fourier Transform on the distance dimension for each signal segment to generate the distance spectrum corresponding to the signal segment, and arrange the distance spectra corresponding to all the signal segments in chronological order to form a distance spectrum sequence.

[0076] In step 1022, for each signal segment, a Fast Fourier Transform (FFT) is performed independently, and the FFT is performed along the time dimension of the signal, which can convert the signal from the time domain to the frequency domain. Since the delay time of the target reflected wave is proportional to the distance, the frequency position of the peak in the transformed spectrum corresponds to the distance of the target, thereby generating the range spectrum of the signal segment. Then, the range spectra generated by all signal segments are arranged in chronological order to form a two-dimensional data matrix, or range spectrum sequence, in which the row direction represents different distances and the column direction represents different times, corresponding to different signal segments.

[0077] Step 1023: Perform a fast Fourier transform on the velocity dimension of the distance spectrum sequence to obtain two-dimensional spectrum data.

[0078] In step 1023, a fast Fourier transform is performed again on the range spectrum sequence along its column direction. This transform analyzes the frequency of signal strength change with time in the same range cell, and the frequency of change is proportional to the radial velocity of the target relative to the radar. After this transformation, a two-dimensional spectrum data is finally obtained, with the two dimensions corresponding to range and velocity, respectively. Each value in the spectrum represents the energy intensity of the target at a specific range and velocity.

[0079] Step 1024: Determine the clutter region generated by the robot's own motion in the two-dimensional image based on the robot's real-time motion state data, and suppress the clutter in the two-dimensional spectrum data to generate the final two-dimensional image for target detection.

[0080] The robot's real-time motion state data refers to the robot's own motion information acquired in real time by sensors such as encoders and inertial measurement units, and the real-time motion state data may include at least one of linear velocity and angular velocity.

[0081] The clutter region generated by the robot's own motion refers to the strong energy concentration area formed in the two-dimensional spectrum data due to the reflection of radar waves by the robot's own structure. Because the robot itself is moving, these originally stationary reflectors appear to the radar with a specific speed, thus forming strip-shaped interference in the speed dimension.

[0082] In step 1024, setting real-time motion state data may include linear velocity, linear velocity It is the radial velocity component of the robot's own motion along the direction of the millimeter-wave radar beam; subsequently, setting For empirical tolerance, in two-dimensional spectral data, the positioning linear velocity is at... The region formed by all grid cells within the range is the clutter region generated by its own motion. Clutter suppression is then performed by setting the spectral amplitude within this region to zero. Finally, the clean spectral matrix after processing and removing the main self-interference is used as the two-dimensional map for target detection.

[0083] Step 1025: Based on the clutter region, define a reference cell within a preset area centered on each grid cell in the two-dimensional diagram, and calculate the background noise value of each grid cell based on the reference cell.

[0084] In this context, a grid cell refers to a basic element in a two-dimensional graph in the form of a data matrix. It is uniquely determined by its distance index and velocity index. In the distance dimension, one grid cell corresponds to a distance resolution, such as 0.5 meters, and in the velocity dimension, one grid cell corresponds to a velocity resolution, such as 0.1 meters per second.

[0085] A reference cell is a set of grid cells used to statistically calculate background noise within a predetermined neighborhood, excluding clutter regions and potential target points.

[0086] In step 1025, for each grid cell in the two-dimensional image, a preset surrounding window is set around it, such as a 5×5 rectangular area; then, reference cells are selected within this window, for example, two cells in front and two cells in the distance dimension and two cells in the velocity dimension, for a total of 24 surrounding cells to form reference cells, but cells belonging to clutter regions are explicitly excluded, and the average value μ of the amplitude values ​​of all valid reference cells is calculated, and the average value is used as the background noise value of the current grid cell to be detected. It should be noted that the specific dimensions of the reference unit need to be dynamically set according to the actual project, and this application does not impose any restrictions.

[0087] Step 1026: Combine the background noise value with the preset constant false alarm rate to calculate the detection threshold corresponding to each grid cell, and compare the actual amplitude value of the grid cell with the corresponding detection threshold to obtain the potential target point.

[0088] The preset constant false alarm rate refers to the highest probability that pure noise is mistaken for a target, which is a pre-set constant.

[0089] In step 1026, for each grid cell, its corresponding background noise value is used. Based on the selected constant false alarm rate (CFAR) detection algorithm, such as cell average CFAR (CA-CFAR), the threshold is determined using the formula Threshold. Calculate its detection threshold, where the scaling factor is... The value is determined based on a preset constant false alarm rate and a formula based on the noise distribution assumption; then, the actual amplitude value of each grid cell is compared with its detection threshold. If the actual amplitude value is greater than the detection threshold, the grid cell is marked as a potential target point.

[0090] Step 1027: Perform peak clustering on all the potential target points to obtain multiple target point clusters, and extract the centroid from each target point cluster to obtain the distance cell index and velocity cell index corresponding to the potential target point.

[0091] The centroid refers to the weighted average position of all points within the target cluster in terms of distance and velocity dimensions. This position represents the optimal estimated point of the target in the spectral map.

[0092] The distance cell index and velocity cell index refer to the row and column numbers of the centroid in the two-dimensional graph, which are the discrete coordinates of the target in the digital spectrum space.

[0093] In step 1027, these potential target points are traversed, and potential target points that are adjacent to each other in the two-dimensional space of distance and velocity are grouped into the same group. For example, potential target points whose distance and velocity index differences are both within a certain neighborhood radius are grouped into the same group to form several target point clusters. Then, for each target point cluster, according to the coordinates of each point in the cluster ( , ) and amplitude value A i And through the formula: Calculate its centroid coordinates ( , Finally, the continuous centroid coordinates are mapped to the nearest discrete grid to obtain the distance cell index and velocity cell index corresponding to the potential target point.

[0094] Step 1028: Based on the range unit index and velocity unit index, and combined with the system parameters of the millimeter-wave radar, calculate the three-dimensional spatial coordinates, radial velocity, and azimuth of each potential target point, and combine all the three-dimensional spatial coordinates, radial velocity, and azimuth to generate millimeter-wave point cloud data.

[0095] Among them, the system parameters of millimeter-wave radar refer to the inherent hardware and configuration parameters of the radar, which mainly include carrier frequency / wavelength, frequency modulation slope, antenna spacing, etc.

[0096] In step 1028, the distance cell index is used. and velocity unit index And combined with radar system parameters, a basic solution is performed, that is, through the formula Calculate the distance from the potential target point to the phase center of the millimeter-wave radar antenna. ,in, The range resolution of the radar; then, using the formula... Calculate radial velocity ,in, For radar velocity resolution;

[0097] Secondly, for radars capable of angle measurement, the azimuth angle can be estimated using the phase difference information between multiple receiving antenna channels. The formula for calculating the azimuth angle is as follows: ,in, For millimeter-wave radar wavelength, It is the azimuth angle. This represents the phase difference information, where d is the antenna spacing.

[0098] Then, for each potential target point, its validity is judged based on at least one of the following criteria: signal-to-noise ratio, distance continuity, or velocity rationality. False alarm points that do not meet the preset judgment conditions are eliminated, and the remaining points are determined as valid targets.

[0099] Finally, combining the target's range, azimuth, and external parameters such as the radar's installation height and orientation in the robot's coordinate system, the target's three-dimensional spatial coordinates (x, y, z) are calculated using the conversion formula from spherical to rectangular coordinates. , z is determined by the installation height and pitch angle information. Then, the three-dimensional spatial coordinates, radial velocity and azimuth angle corresponding to each target point are combined to obtain millimeter wave point cloud data.

[0100] This application not only extracts the precise spatial location of obstacles, but also simultaneously obtains their radial velocity, which lays the foundation for subsequent identification of high-speed dynamic targets. At the same time, it effectively improves the robustness of radar perception and the reliability of target detection in complex and dynamically changing warehouse environments through adaptive threshold detection and self-clutter suppression.

[0101] S103. Identify the target area with a radial velocity greater than a preset velocity threshold based on the millimeter-wave point cloud data, and perform local enhanced scanning on the target area to generate laser point cloud data.

[0102] In step S103, the millimeter-wave point cloud data is filtered according to a preset speed threshold, such as 1.5 m / s, and all points with radial velocity absolute values ​​greater than this preset speed threshold are extracted to form a preliminary set of high-speed target points. Then, spatial clustering analysis is performed on these high-speed target points to group points with similar locations into the same group. The spatial distribution of each group is then calculated, and a three-dimensional spatial boundary range is generated for each independent group as a running target area.

[0103] Subsequently, the spatial coordinate parameters of the target area are converted into specific control commands for the lidar. Then, based on these commands, the lidar drives its internal optical components to project the scanning beam onto the designated target area in multiple ways while maintaining panoramic scanning mode, in order to implement local enhanced scanning. The multiple ways include: a first way of increasing the number of scans per unit time, and a second way of increasing the density of scan lines or the number of sampling points in the same area in space.

[0104] The original measurement data is processed after scanning. Specifically, by calculating the time difference between the emission and reception of each laser pulse and combining it with the radar's internal calibration parameters, the precise three-dimensional spatial coordinates of each reflection point in the radar's own coordinate system are calculated. Then, all scan points are integrated to form laser point cloud data.

[0105] This application improves the accuracy of geometric shape perception and state update speed of key dynamic obstacles without increasing the global scanning burden of the system, thereby providing a better data foundation for subsequent accurate tracking and trajectory prediction.

[0106] S104. The millimeter-wave point cloud data and the laser point cloud data are fused using a multi-hypothesis tracking algorithm to obtain the predicted trajectory data of the dynamic obstacle, and the predicted trajectory data is mapped to the final inflated obstacle area using the velocity obstacle method.

[0107] The explanations of the multiple hypothesis tracking algorithm and the velocity barrier method can be found in the relevant technologies, and will not be elaborated here.

[0108] In one specific implementation, such as Figure 2 As shown, step S104 includes:

[0109] Step 1041: Extract the first sub-point cloud data that matches the target area from the millimeter-wave point cloud data, and perform coordinate system unification between the laser point cloud data and the first sub-point cloud data to generate fused point cloud data.

[0110] In step 1041, the millimeter-wave point cloud data is traversed according to the spatial range of the target area, and all points whose coordinates are within this range are selected to form the first sub-point cloud data. At the same time, the laser point cloud data collected at the same time is read, and the coordinates of each point in the laser point cloud data are transformed and calculated using a pre-calibrated sensor extrinsic parameter matrix, which describes the spatial relationship between the laser radar and the millimeter-wave radar coordinate system. After the transformation is completed, the laser point cloud data and the first sub-point cloud data are in the same coordinate reference system. Finally, the two sets of point data with aligned coordinates are merged into the same data structure to generate fused point cloud data.

[0111] Step 1042: Perform clustering processing on the fused point cloud data to obtain multiple target point cloud data. Apply a multi-hypothesis tracking algorithm to each target point cloud data and initialize the trajectory using the radial velocity value in the first sub-point cloud data after coordinate system one to obtain an initial trajectory set.

[0112] In step 1042, a distance-based clustering algorithm is applied to the fused point cloud data to group points that are close to each other in space into the same group, thereby obtaining several target point cloud data. Then, for each target point cloud data, a new tracker is created, and a point in the first sub-point cloud data that best matches the current target point cloud in terms of spatial location is found through a multi-hypothesis tracking algorithm based on global nearest neighbors. The radial velocity value of this point is read, and this position and radial velocity are used as the initial observation. Combined with a preset motion model, such as a uniform velocity model and its uncertainty covariance, multiple slightly different initial trajectories are generated. For example, an initial observation may generate 3 to 5 different initial trajectories such as "uniform speed straight", "slight left turn", and "slight right turn". All initial trajectories are combined to form the initial trajectory set of the target.

[0113] For example, suppose that after clustering the above fused point cloud data, a target point cloud data consisting of 15 points is obtained, corresponding to a B forklift; then the tracker reads the first sub-point cloud data that coincides with the position of the target point cloud and obtains the initial speed of 2 m / s to the right; then, the multi-hypothesis tracking algorithm generates 4 initial trajectory sets based on this: trajectory 1, trajectory 2, trajectory 3, and trajectory 4, where trajectory 1 is to maintain a rightward movement of 2 m / s, trajectory 2 is to accelerate to a rightward movement of 2.5 m / s, trajectory 3 is to decelerate to a rightward movement of 1.5 m / s, and trajectory 4 is to maintain the speed but make minor adjustments to the direction.

[0114] Step 1043: Based on the fused point cloud data, perform state updates and probability calculations on the initial trajectory set to obtain the predicted trajectory data of the dynamic obstacle.

[0115] In step 1043, when the fused point cloud data arrives at the next moment, the multi-hypothesis tracking algorithm enters the iterative update cycle. The multi-hypothesis tracking algorithm first matches the new observation point with each of the existing initial trajectories. For each initial trajectory, a Kalman filter is used to receive the new observation point associated with the trajectory, and the state of the trajectory is updated according to the preset motion model to correct its current position and velocity estimation.

[0116] Meanwhile, the Kalman filter extrapolates forward to predict the target's position sequence over a future period, and calculates the probability of each initial trajectory while updating the state. The probability calculation can be based on the degree of agreement between the new observation and the trajectory prediction, the correlation quality, and historical probabilities. As time goes on, initial trajectories with high probabilities are retained and enhanced, while those with low probabilities gradually disappear. Finally, the trajectory with the highest probability for each target is selected, and its current state and future prediction sequence are used as the predicted trajectory data for dynamic obstacles.

[0117] Step 1044: Input the predicted trajectory data, the robot's kinematic parameters and dynamic parameters into the trajectory evaluation model for evaluation processing to obtain the threat level of dynamic obstacles to the robot, and filter out the trajectories whose threat level exceeds a preset threat threshold from the predicted trajectory data to obtain key trajectory data.

[0118] It should be noted that the trajectory evaluation model may include multiple modules, and the model type, structural design, module structural design and parameter design, model training process, etc. can all be set according to the actual situation. This embodiment does not limit this.

[0119] Step 1044 may include the following steps:

[0120] Step a1: Obtain the relative motion state of the dynamic obstacle with respect to the robot from the predicted trajectory data through the feature extraction module of the trajectory evaluation model.

[0121] The feature extraction module can be a lightweight fully connected neural network.

[0122] In step a1, the predicted trajectory data is received by the input layer of the feature extraction module of the trajectory evaluation model. Then, the basic motion association pattern is extracted by linear transformation and combination through the first fully connected layer, and a 64-dimensional intermediate feature vector is obtained. Then, the intermediate feature vector is mapped element-wise by the ReLU function through the first nonlinear activation layer to enhance the model's ability to represent complex and sudden motion patterns, and outputs a nonlinear 64-dimensional activated feature vector. Then, the regularization layer uses the Dropout technique to randomly set some dimensions of the activated vector to zero during the training phase to simulate the uncertainty of sensor noise and temporary target occlusion in the dynamic warehouse, and outputs a regularized 64-dimensional feature vector with some dimensions suppressed.

[0123] Subsequently, the regularized features are further linearly compressed and aggregated through a second fully connected layer to focus on the motion patterns that have the most significant impact on collision risk, and a refined 32-dimensional feature vector is output. The ReLU function is then used again to maintain the non-linear expressive power of the features, and a non-linear 32-dimensional feature vector is output. Finally, the 32-dimensional feature vector is linearly projected onto the final representation space through an output projection layer to output a fixed 16-dimensional, highly abstract feature vector of "relative motion state".

[0124] Step a2: Using the requirement calculation module of the trajectory evaluation model, based on the relative motion state, calculate the braking deceleration or steering curvature required for the robot to avoid collision with dynamic obstacles, and generate motion requirement data.

[0125] The demand calculation module is mainly an analytical calculation layer based on kinematic formulas, which includes a braking demand calculation unit and a steering demand calculation unit.

[0126] In step a2, the demand calculation module of the trajectory evaluation model receives the vector corresponding to the relative motion state, and then the braking demand calculation layer extracts the relative distance and approach velocity components from the feature vector of the relative motion state, and performs analytical calculation using kinematic formulas to output a required braking deceleration; at the same time, the steering demand calculation layer extracts the relative position and heading information from the feature vector of the relative motion state, and performs analytical calculation using a geometric model to output a required steering curvature, and the two calculation results are collectively referred to as motion demand data.

[0127] It should be noted that this embodiment does not limit the expression of the kinematic formula, and can be set accordingly according to the actual situation.

[0128] Step a3: Using the comparison module of the trajectory evaluation model, the motion demand data is compared one by one with the maximum braking deceleration and maximum steering curvature obtained from the robot's kinematic and dynamic parameters, and the exceeding items in the motion demand data that exceed the maximum braking deceleration or the maximum steering curvature are identified.

[0129] In step a3, the comparison module of the trajectory evaluation model receives motion demand data and the maximum braking deceleration and maximum steering curvature obtained from the robot parameters. Then, the module compares the motion demand data with the maximum braking deceleration and maximum steering curvature one by one through the threshold comparison logic layer. For example, it compares the calculated braking deceleration with the robot's maximum braking deceleration and the calculated steering curvature with the robot's maximum steering curvature. If any demand value in the motion demand data exceeds the corresponding capability limit, it is marked as an excess item.

[0130] Step a4: Using the quantization module of the trajectory evaluation model, the number of the transcendental terms and the relative positions corresponding to the transcendental terms are calculated according to a preset weighting rule to obtain the threat level.

[0131] In step a4, the quantization module of the trajectory evaluation model comprehensively quantifies the overtaking terms. Specifically, first, the total number of overtaking terms is counted; second, for each overtaking term, a weight is assigned based on the relative position of the dynamic obstacle and the robot at the time of its occurrence. The quantification of the relative position is achieved by defining a risk area in front of the robot. For example, a cone-shaped area directly in front of the robot is designated as the "forward collision zone" and assigned the highest weight; the area to the side is designated as the "lateral zone" and assigned a medium weight; then, the module determines the risk area to which the obstacle belongs based on the position of the closest point between the predicted trajectory of the obstacle and the predicted trajectory of the robot, and obtains the corresponding weight.

[0132] Finally, the threat level T was passed. Calculate, where, It is the number of transcendental terms. It is the corresponding quantitative value obtained for each exceeding item based on its relative position. After the calculation is completed, the threat level of all trajectories is compared with the preset threat threshold to filter out the trajectories whose threat level exceeds the preset threat threshold and mark them as key trajectory data.

[0133] Step 1045: Apply the velocity obstacle method and calculate the initial expansion obstacle zone in the robot's velocity plane based on the expected future position sequence of dynamic obstacles represented by the key trajectory data.

[0134] In step 1045, for a critical trajectory data point, the velocity barrier method extracts its predicted position sequence over a future period. Then, for each future position in the sequence, the position point is geometrically expanded according to the robot's contour radius to form a circular danger zone. Then, the core principle of the velocity barrier method is applied: for the robot's current position, it is calculated which constant velocity vectors will cause it to enter the danger zone at some future moment, and all these "bad" velocity vectors constitute a region in the robot's velocity plane. Finally, the union of the danger velocity regions generated by all critical trajectory data at all future moments is taken to obtain the initial expanded barrier zone.

[0135] Step 1046: Calculate the rate of change of the expansion barrier zone in this round and the previous round.

[0136] In step 1046, the initial expansion barrier region stored after the previous iteration is read, and the rate of change between the initial expansion barrier region calculated in this round and the initial expansion barrier region in the previous round is calculated. The formula for calculating is: ,in, This is the initial expansion barrier region calculated in this round. It is the initial expansion barrier zone of the previous round.

[0137] Step 1047: The initial inflated barrier region is used as a spatial constraint and fed back into the calculation process of the multi-hypothesis tracking algorithm. The steps of state update, probability calculation, evaluation processing, screening, barrier region calculation, and rate of change calculation are repeated until the rate of change of the inflated barrier region in two consecutive rounds reaches the preset convergence condition to generate the final inflated barrier region.

[0138] In step 1047, the initial inflated obstacle zone is converted into constraint information that can be understood by the tracking algorithm. When performing the state update and probability calculation in step 1043 in the next execution, the multi-hypothesis tracking algorithm will add a new evaluation dimension when performing data association and trajectory probability calculation: it will check the response speed that the robot may take in the future implied by each initial trajectory. If an initial trajectory implies that the robot must choose the speed to fall into the current inflated obstacle zone in order to avoid collision, then this initial trajectory is considered to conflict with the upper-level safety planning goal. Therefore, the multi-hypothesis tracking algorithm will reduce the probability of this initial trajectory, and the reduction in probability makes it less likely that such trajectories will be selected as high-probability outputs in subsequent screening.

[0139] Then, based on the updated probability distribution, the process from steps 1043 to 1046 is re-executed. Due to the change in the tracking results, the newly calculated inflationary barrier region will also change. This cycle repeats, forming a closed-loop feedback from perception and tracking to planning constraints. Finally, when the rate of change of the inflationary barrier region calculated in two consecutive iterations is reached... When the convergence value is less than a preset convergence condition, such as If the result is less than 1% for two consecutive rounds, the result is considered stable and the iteration is stopped. The final expanded obstacle region obtained at this time is the final expanded obstacle region that integrates dynamic obstacle prediction and the robot's active obstacle avoidance intention.

[0140] This application not only improves the robustness and prediction accuracy of trajectory tracking, but also ensures that the safety boundary obtained by the navigation decision layer is verified in a closed loop and closely matched with the robot's own obstacle avoidance capabilities, thereby improving the robot's obstacle avoidance safety and action reliability in complex dynamic environments.

[0141] S105. Calculate the motion control command of the robot based on the expansion barrier area, and dynamically adjust the grasping control parameters of the robot's end effector when grasping the target box according to the speckle image sequence using digital image correlation method, and grasp the target box according to the grasping control parameters.

[0142] In one specific implementation, step S105 includes:

[0143] Step 1051: Based on the change of the inflated barrier region in the velocity space over time, define the feasible set of the robot's initial velocity.

[0144] In step 1051, the geometric boundary of the final inflated obstacle zone in the velocity space is analyzed and extracted. This boundary is usually composed of several line segments or curves. In the entire velocity plane, the complement of the boundary of the inflated obstacle zone is calculated. At the same time, combined with the boundary formed by the robot's own physical velocity limit, a closed and connected safe region is finally obtained. This safe region is the initial velocity feasible set, which contains all feasible velocity combinations that can avoid conflict with the predicted trajectory of the dynamic obstacle.

[0145] Step 1052: Input the threat level into a constraint regulator. The constraint regulator dynamically calculates the relaxation amount of the initial velocity feasible set based on the threat level, and adjusts the initial velocity feasible set according to the relaxation amount to generate the target velocity feasible set.

[0146] In step 1052, the constraint regulator receives the threat level from S104. Internally, it pre-defines a mapping relationship between the threat level and the relaxable amount. This mapping relationship follows a pre-defined rule: a high threat level indicates a high dynamic risk in the environment, and the relaxable amount should be set to a small value to more accurately execute safety constraints; a low threat level indicates a relatively safe environment, and to improve the robot's operating efficiency, such as making its speed closer to the desired ideal speed, a positive relaxable amount that increases as the threat level decreases can be allowed. Subsequently, the relaxable amount is used to translate each boundary segment of the initial velocity feasible set, moving it a distance outside the safe zone as the relaxable amount, thereby generating a moderately relaxed target velocity feasible set.

[0147] Step 1053: Based on the robot's current speed capability range, generate a speed sample set, and according to the robot's kinematic model, perform trajectory deduction for each speed sample in the speed sample set to obtain the predicted trajectory corresponding to each speed sample.

[0148] It should be noted that this embodiment does not limit the expression of the robot's kinematic model, and can be set accordingly based on the actual situation.

[0149] In step 1053, based on the robot's current speed capability range, system sampling is performed within a rectangular or circular area formed by the physical limits of the robot's linear and angular velocities. The sampling method can be uniform grid division or dense sampling near the boundary, thereby generating a velocity sample set covering all possible velocity samples. Then, for each velocity sample in the velocity sample set, the robot's differential kinematics model uses numerical integration methods, such as the Euler method, to gradually deduce the robot's movement direction and distance in each future control cycle, starting from the current position, and finally connects them into a complete spatial path. This path is the predicted trajectory corresponding to the velocity sample.

[0150] Step 1054: Using the target velocity feasible set as a constraint, perform a feasibility test on the predicted trajectory, take the predicted trajectory that passes the test as the feasible trajectory, and use a multi-criteria decision model to comprehensively evaluate all the feasible trajectories to obtain the evaluation result corresponding to each feasible trajectory.

[0151] The explanation and structural design of the multi-criteria decision-making model can be referred to relevant technologies, and will not be elaborated in this embodiment.

[0152] In step 1054, all predicted trajectories are traversed, and it is checked whether the coordinates of the initial velocity sample corresponding to each trajectory fall within the region of the feasible set of the target velocity. Only trajectories whose coordinates are within the safe region are retained and marked as feasible trajectories. Subsequently, a multi-criteria decision model is launched to comprehensively evaluate all the selected feasible trajectories. This model integrates multiple evaluation criteria, such as the distance between the trajectory endpoint and the global target point, the overall smoothness of the trajectory, the time cost required for trajectory execution, and the closest distance between the trajectory and a known static obstacle.

[0153] Then, the multi-criteria decision model assigns corresponding weights to each criterion and scores each feasible trajectory independently on each criterion. Finally, the comprehensive evaluation result of each trajectory is calculated by weighted summation. The weights can be preset by the analytic hierarchy process, for example, they can be assigned values ​​of 0.4, 0.3, 0.2, and 0.1 respectively. The weights can be dynamically adjusted according to the actual task mode. This application does not impose specific limitations.

[0154] Step 1055: Take the velocity sample corresponding to the optimal feasible trajectory in the evaluation results as the optimal velocity command, and convert the optimal velocity command into the motion control command of the robot through the inverse kinematics model.

[0155] In step 1055, the evaluation results of all feasible trajectories are compared, and the feasible trajectory with the highest score is selected. The velocity samples used to generate this optimal trajectory, namely the target linear velocity and target angular velocity of the robot chassis, are extracted and used as the optimal velocity command. Next, the optimal velocity command is input into the inverse kinematics model corresponding to the robot platform type. This model, based on the robot's wheel system layout and size parameters, uses the differential drive kinematics inverse solution formula to calculate the unified chassis velocity into independent target rotational speeds or target steering angles for each drive wheel. This series of calculated wheel control commands constitutes the final motion control command. This embodiment does not limit the expression of the inverse kinematics formula.

[0156] Step 1056: Perform sub-pixel matching calculation on the speckle image sequence using digital image correlation to obtain the two-dimensional displacement field of the target bin gripping surface. Perform spatial differentiation operation on the two-dimensional displacement field to obtain the full-field strain field of the target bin gripping surface.

[0157] In step 1056, a frame before the robot end effector contacts the hopper is selected from the speckle image sequence as a reference image, and a frame after contact and force application is selected as a deformed image. Then, a series of computational sub-regions are defined on the reference image, and each sub-region contains a unique speckle pattern.

[0158] Then, digital image correlation is used to perform subpixel precision search and matching on each sub-region of the deformed image to find the most similar new position, thereby calculating the displacement of the center point of each sub-region in two directions. After traversing the entire region of interest, a complete two-dimensional displacement field is formed. Subsequently, spatial differentiation is performed on the two-dimensional displacement field to calculate the gradient of the displacement in the plane point by point using the central difference numerical method, thereby obtaining the full-field strain field describing the distribution of normal strain and shear strain.

[0159] Step 1057: Extract the strain concentration region and the corresponding principal strain direction from the full-field strain field where the deformation degree is greater than the preset deformation degree threshold. Based on the spatial distribution of the strain concentration region and the principal strain direction, identify the vulnerable area and potential slip direction of the target material box.

[0160] It should be noted that this embodiment does not limit the value of the preset deformation degree threshold, and can be set accordingly according to the actual situation.

[0161] In step 1057, the entire strain field is scanned to filter out all grid points whose strain amplitude exceeds a preset deformation threshold. These discrete points are then clustered based on spatial proximity to form several independent strain concentration regions. The average principal strain direction is calculated for each strain concentration region, and these regions are analyzed in conjunction with prior knowledge: if the strain concentration region appears at the corner, seam, or known structural weakness of the hopper, it is marked as a vulnerable region; at the same time, the principal strain direction is analyzed, and the direction of the maximum tensile strain indicates the tendency of the material to be stretched, while the opposite direction may indicate the potential shrinkage or sliding tendency of the material under the gripping force, and this direction is identified as the potential slip direction.

[0162] Step 1058: Convert the position corresponding to the vulnerable area into the attenuation coefficient in the expected stiffness matrix of the end effector, and convert the potential slip direction into the force axis direction of the end effector that needs to be strengthened in the grasping force coordinate system.

[0163] In step 1058, the physical location of the identified vulnerable area is mapped to the local coordinate system of the end effector's gripping surface. For example, a vulnerable area located on the right side of the gripping surface corresponds to a specific direction in the end effector coordinate system. Then, for this direction, the corresponding stiffness element is found in the original desired stiffness matrix and multiplied by a damping coefficient less than 1, thereby actively reducing the stiffness of the robot end effector in this direction to make it more compliant during contact. At the same time, the potential slip direction vector is transformed into the gripping force coordinate system. For example, if the potential slip direction is mainly projected in the negative X-axis direction in the gripping force coordinate system, the system marks the X-axis as the force axis direction that needs to be strengthened. This means that in subsequent force control, sufficient clamping force or friction force needs to be ensured in this axis to prevent slippage.

[0164] Step 1059: Convert the average strain energy density of the full-field strain field into the expected basic value of the gripping force, and call the compliant control algorithm, taking the attenuation coefficient, the force axis direction and the expected basic value as input parameters, to perform parameter reconstruction and force control calculation processing to obtain the gripping control parameters.

[0165] This embodiment does not limit the type of compliance control algorithm used, and can be set accordingly based on the actual situation.

[0166] In step 1059, the average strain energy density of the entire strain field is calculated, and this average strain energy density is converted into a desired basic value for the gripping force according to a preset mapping relationship. This value is used as the basis for the initial gripping force magnitude. The preset mapping relationship can be the mapping relationship between the average strain energy density and the desired basic value, which can be obtained through... To achieve, among which, As the expected base value, The average strain energy density is given by k and b, which are calibration coefficients. These coefficients can have different values ​​for different materials of the hopper, and this application does not impose any restrictions on them.

[0167] Subsequently, the compliant control algorithm based on admittance control is invoked. First, the parameters are reconstructed: the original desired stiffness matrix is ​​modified using the attenuation coefficient to generate a new, anisotropic end stiffness; then, the gain matrix of the force control loop is configured according to the force axis direction information, and a higher force control bandwidth or a smaller force error tolerance is set on the critical axis; then, the desired base value is used as part of the force reference input of the admittance control outer loop.

[0168] Next, force control calculation is performed: the algorithm continuously reads the actual contact force fed back by the six-dimensional force sensor and compares it with the expected force calculated based on the new impedance model and position error. Then, it dynamically calculates the speed or position adjustment amount that needs to be compensated at the end through the admittance model. Finally, after this series of calculations, real-time gripping control parameters that can directly drive the gripper motor are generated.

[0169] This application achieves dual-thread collaborative control of navigation and obstacle avoidance and precise grasping. The navigation side generates safe and efficient motion commands through dynamically adjusted safety speed boundaries and multi-target optimization. The grasping side adaptively adjusts the grasping force and compliance by visually perceiving the microscopic deformation of the bin, and effectively protects the fragile bin while ensuring stable grasping. This enables the embodied intelligent robot to safely reach the target location in a dynamic warehouse environment and reliably complete the handling of bins in complex states, thereby improving the overall intelligence, adaptability and reliability of the system.

[0170] Figure 3 This is a schematic diagram illustrating a specific implementation of an autonomous navigation and grasping control system for an embodied intelligent robot provided in this application embodiment. (Refer to...) Figure 3 The system may include:

[0171] The acquisition module 31 is used to acquire the intermediate frequency signal during robot operation and the speckle image sequence when the robot grasps the target hopper.

[0172] The detection module 32 is used to perform two-dimensional fast Fourier transform processing on the intermediate frequency signal in the distance and velocity dimensions to generate a two-dimensional image, and to perform average constant false alarm detection on the two-dimensional image to generate millimeter-wave point cloud data.

[0173] The enhancement module 33 is used to identify the target area with a radial velocity greater than a preset velocity threshold based on the millimeter-wave point cloud data, and to perform local enhancement scanning on the target area to generate laser point cloud data.

[0174] The fusion module 34 is used to fuse the millimeter-wave point cloud data and the laser point cloud data using a multi-hypothesis tracking algorithm to obtain the predicted trajectory data of the dynamic obstacle, and to map the predicted trajectory data into the final inflated obstacle area using the velocity obstacle method.

[0175] The adjustment module 35 is used to calculate the motion control command of the robot based on the expansion barrier area, and dynamically adjust the grasping control parameters of the robot's end effector when grasping the target box according to the speckle image sequence using digital image correlation, and grasp the target box according to the grasping control parameters.

[0176] The autonomous navigation and grasping control system of the embodied intelligent robot in this application embodiment is used to implement the aforementioned autonomous navigation and grasping control method of the embodied intelligent robot. Therefore, the specific implementation of the autonomous navigation and grasping control system of the embodied intelligent robot can be found in the embodiment section of the autonomous navigation and grasping control method of the embodied intelligent robot above. The specific implementation can be referred to the description of the corresponding embodiments, and will not be repeated here.

[0177] like Figure 4 As shown, this application also provides an electronic device, including: a memory 41 for storing a computer program; and a processor 42 for executing the computer program to implement the steps of the autonomous navigation and grasping control method of any of the above-described embodied intelligent robots.

[0178] This application also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, implements the steps of the autonomous navigation and grasping control method for any of the above-described embodied intelligent robots.

[0179] In one exemplary embodiment, the aforementioned computer-readable storage medium may include, but is not limited to, various media capable of storing computer programs, such as USB flash drives, read-only memory, random access memory, portable hard drives, magnetic disks, or optical disks.

[0180] Embodiments of the present invention also provide a computer program product, which includes a computer program that, when executed by a processor, implements the steps in any of the embodiments of the autonomous navigation and grasping control method for an embodied intelligent robot.

[0181] Those skilled in the art will further recognize that the units and algorithm steps of the various examples described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, computer software, or a combination of both. To clearly illustrate the interchangeability of hardware and software, the components and steps of the various examples have been generally described in terms of functionality in the foregoing description. Whether these functions are implemented in hardware or software depends on the specific application and design constraints of the technical solution. Those skilled in the art can use different methods to implement the described functions for each specific application, but such implementations should not be considered beyond the scope of this invention.

[0182] The above provides a detailed description of the autonomous navigation and grasping control method and system for an embodied intelligent robot provided in this application. Specific examples have been used to illustrate the principles and implementation methods of this application. The descriptions of the embodiments above are merely for the purpose of helping to understand the method and its core ideas. It should be noted that those skilled in the art can make various improvements and modifications to this application without departing from its principles, and these improvements and modifications also fall within the protection scope of this application.

Claims

1. A method for autonomous navigation and grasping control of an embodied intelligent robot, characterized in that, include: The intermediate frequency signal during robot operation and the speckle image sequence when the robot grasps the target hopper were collected; The intermediate frequency signal is processed by two-dimensional fast Fourier transform in the distance and velocity dimensions to generate a two-dimensional image, and the two-dimensional image is subjected to average constant false alarm detection to generate millimeter-wave point cloud data. Based on the millimeter-wave point cloud data, the target area with a radial velocity greater than a preset velocity threshold is identified, and the target area is subjected to local enhanced scanning to generate laser point cloud data. The millimeter-wave point cloud data and the laser point cloud data are fused using a multi-hypothesis tracking algorithm to obtain the predicted trajectory data of the dynamic obstacle, and the velocity obstacle method is used to map the predicted trajectory data into the final inflated obstacle area. The robot's motion control commands are calculated based on the expansion barrier region. Simultaneously, the robot's end effector's grasping control parameters are dynamically adjusted using digital image correlation based on the speckle image sequence, and the target material box is grasped according to the grasping control parameters. The application of the multi-hypothesis tracking algorithm fuses the millimeter-wave point cloud data with the laser point cloud data to obtain predicted trajectory data of dynamic obstacles, and uses the velocity obstacle method to map the predicted trajectory data into the final inflated obstacle region, including: Extract a first sub-point cloud data that matches the target area from the millimeter-wave point cloud data, and combine the laser point cloud data with the first sub-point cloud data in coordinate system 1 to generate fused point cloud data. Clustering is performed on the fused point cloud data to obtain multiple target point cloud data. A multi-hypothesis tracking algorithm is applied to each target point cloud data, and the trajectory is initialized using the radial velocity value in the first sub-point cloud data after coordinate system one to obtain an initial trajectory set. Based on the fused point cloud data, the initial trajectory set is updated in state and probability is calculated to obtain the predicted trajectory data of the dynamic obstacle; The predicted trajectory data, the robot's kinematic parameters, and dynamic parameters are input into the trajectory evaluation model for evaluation processing to obtain the threat level of dynamic obstacles to the robot. Trajectories with threat levels exceeding a preset threat threshold are then selected from the predicted trajectory data to obtain key trajectory data. The velocity obstacle method is applied to calculate the initial expanding obstacle zone in the robot's velocity plane based on the expected future position sequence of dynamic obstacles represented by the key trajectory data. Calculate the rate of change of the expansion barrier region in this round and the previous round; The initial inflated barrier region is used as a spatial constraint and fed back into the calculation process of the multi-hypothesis tracking algorithm. The steps of state update, probability calculation, evaluation processing, screening, barrier region calculation, and rate of change calculation are repeated until the rate of change of the inflated barrier region in two consecutive rounds reaches the preset convergence condition to generate the final inflated barrier region.

2. The method according to claim 1, characterized in that, The process of inputting the predicted trajectory data, the robot's kinematic parameters, and dynamic parameters into a trajectory evaluation model for evaluation processing, to obtain the threat level of dynamic obstacles to the robot, includes: The feature extraction module of the trajectory evaluation model obtains the relative motion state of the dynamic obstacle with respect to the robot from the predicted trajectory data. The demand calculation module of the trajectory evaluation model calculates the braking deceleration or steering curvature required for the robot to avoid collision with dynamic obstacles based on the relative motion state, and generates motion demand data. The comparison module of the trajectory evaluation model compares the motion demand data with the maximum braking deceleration and maximum steering curvature obtained from the robot's kinematic and dynamic parameters one by one, and identifies the exceeding items in the motion demand data that exceed the maximum braking deceleration or the maximum steering curvature. The threat level is obtained by calculating the number of overtaking terms and the relative positions corresponding to the overtaking terms using the quantization module of the trajectory evaluation model according to a preset weighting rule.

3. The method according to claim 1, characterized in that, The step of dynamically adjusting the gripping control parameters of the robot's end effector when gripping the target bin using digital image correlation based on the speckle image sequence includes: The speckle image sequence is subjected to subpixel matching calculation by digital image correlation method to obtain the two-dimensional displacement field of the target bin gripping surface. The two-dimensional displacement field is then subjected to spatial differentiation operation to obtain the full-field strain field of the target bin gripping surface. Extract strain concentration regions and corresponding principal strain directions from the full-field strain field where the deformation degree is greater than a preset deformation degree threshold. Based on the spatial distribution of the strain concentration regions and the principal strain directions, identify the vulnerable areas and potential slip directions of the target material box. The location corresponding to the vulnerable area is converted into the attenuation coefficient in the expected stiffness matrix of the end effector, and the potential slip direction is converted into the force axis direction of the end effector that needs to be strengthened in the grasping force coordinate system. The average strain energy density of the full-field strain field is converted into the expected basic value of the gripping force. The compliant control algorithm is then invoked, and the attenuation coefficient, the force axis direction, and the expected basic value are used as input parameters to perform parameter reconstruction and force control calculation to obtain the gripping control parameters.

4. The method according to claim 1, characterized in that, The calculation of the robot's motion control commands based on the inflated barrier region includes: Based on the change of the inflated barrier region in the velocity space over time, the feasible set of the robot's initial velocity is defined; The threat level is input into a constraint regulator, which dynamically calculates the relaxation amount of the initial velocity feasible set based on the threat level, and adjusts the initial velocity feasible set according to the relaxation amount to generate the target velocity feasible set. Based on the robot's current speed capability range, a speed sample set is generated, and based on the robot's kinematic model, the trajectory of each speed sample in the speed sample set is deduced to obtain the predicted trajectory corresponding to each speed sample. Using the target velocity feasible set as a constraint, the feasibility of the predicted trajectory is checked, and the predicted trajectory that passes the check is taken as a feasible trajectory. A multi-criteria decision model is used to comprehensively evaluate all the feasible trajectories and obtain the evaluation result corresponding to each feasible trajectory. The velocity sample corresponding to the optimal feasible trajectory in the evaluation results is taken as the optimal velocity command, and the optimal velocity command is converted into the motion control command of the robot through the inverse kinematics model.

5. The method according to claim 1, characterized in that, The step of performing a two-dimensional fast Fourier transform on the intermediate frequency signal in both distance and velocity dimensions to generate a two-dimensional graph includes: The intermediate frequency signal is segmented to obtain multiple continuous signal segments, wherein the time length of the signal segment is jointly determined based on the robot's maximum expected movement speed and the wavelength parameters of the millimeter-wave radar. Perform a Fast Fourier Transform on each signal segment to generate a distance spectrum corresponding to the signal segment, and arrange the distance spectra corresponding to all the signal segments in chronological order to form a distance spectrum sequence; Perform a fast Fourier transform on the velocity dimension of the distance spectrum sequence to obtain two-dimensional spectrum data; Based on the robot's real-time motion state data, the clutter region generated by the robot's own motion in the two-dimensional image is determined, and the clutter is suppressed on the two-dimensional spectral data to generate the final two-dimensional image for target detection.

6. The method according to claim 1, characterized in that, The step of performing average constant false alarm rate (CFAR) detection on the two-dimensional image to generate millimeter-wave point cloud data includes: Based on the clutter region, a reference cell is defined in the two-dimensional diagram around each grid cell, and the background noise value of each grid cell is calculated based on the reference cell. By combining the background noise value with the preset constant false alarm rate, the detection threshold corresponding to each grid cell is calculated, and the actual amplitude value of the grid cell is compared with the corresponding detection threshold to obtain the potential target point; Peak clustering is performed on all the potential target points to obtain multiple target point clusters, and the centroid is extracted from each target point cluster to obtain the distance cell index and velocity cell index corresponding to the potential target point. Based on the range unit index and velocity unit index, and combined with the system parameters of the millimeter-wave radar, the three-dimensional spatial coordinates, radial velocity, and azimuth angle of each potential target point are calculated. All the three-dimensional spatial coordinates, radial velocity, and azimuth angle are combined to generate millimeter-wave point cloud data.

7. An autonomous navigation and grasping control system for an embodied intelligent robot, characterized in that, A method for implementing autonomous navigation and grasping control of an embodied intelligent robot as described in claim 1 includes: The acquisition module is used to acquire the intermediate frequency signal during robot operation and the speckle image sequence when the robot grasps the target hopper; The detection module is used to perform two-dimensional fast Fourier transform processing on the intermediate frequency signal in the distance and velocity dimensions to generate a two-dimensional image, and to perform average constant false alarm detection on the two-dimensional image to generate millimeter-wave point cloud data. The enhancement module is used to identify the target area with a radial velocity greater than a preset velocity threshold based on the millimeter-wave point cloud data, and to perform local enhancement scanning on the target area to generate laser point cloud data. The fusion module is used to fuse the millimeter-wave point cloud data and the laser point cloud data using a multi-hypothesis tracking algorithm to obtain the predicted trajectory data of the dynamic obstacle, and to map the predicted trajectory data into the final inflated obstacle area using the velocity obstacle method. The adjustment module is used to calculate the motion control command of the robot based on the expansion barrier area, and dynamically adjust the grasping control parameters of the robot's end effector when grasping the target box according to the speckle image sequence using digital image correlation, and grasp the target box according to the grasping control parameters.

8. An electronic device, characterized in that, include: Memory, used to store computer programs; A processor, configured to implement the autonomous navigation and grasping control method for an embodied intelligent robot as described in any one of claims 1 to 6 when executing the computer program.

9. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, enables the autonomous navigation and grasping control method of the embodied intelligent robot as described in any one of claims 1 to 6.

Images