A robot motion control and state monitoring method and device

Abstract

The present invention discloses a robot motion control and state monitoring method and device, which relate to the field of robot control technology, specifically as follows: receiving robot information acquired based on a multimodal sensor array, and synchronously aligning the robot information; performing health monitoring on the robot body state to obtain a health state vector, and analyzing the robot health state in combination with the robot information and the health state vector; the present invention realizes a full-link closed loop from perception, monitoring, modeling, decision-making to execution, and integrates environmental mutations and body health information in real time, and ensures the safety and efficiency of movement through hybrid decision-making and joint compensation. Compared with traditional robot control that relies on pre-planned paths, it can quickly respond to environmental dynamics, evaluate health risks in real time and adaptively adjust strategies, overcome the problems of insufficient real-time and flexibility, realize deep correlation with the robot's own state, and improve the robot's movement accuracy in complex mutation scenarios.

Description

Technical Field

[0001] The present invention relates to the field of robot control technology, and in particular to a robot motion control and state monitoring method and device. Background Art

[0002] With the development of science and technology and artificial intelligence, robots are increasingly used in all walks of life. Especially in high-intensity industrial environments, the application of robotics has significantly improved production efficiency and automation levels. In this context, robot motion control and condition monitoring have become particularly important, especially for robots in high-intensity operations.

[0003] Current robots typically perform motion control based on pre-planned routes or movements. This results in insufficient real-time and flexibility, making it unable to respond to sudden environmental changes in a timely manner, and unable to deeply associate the robot's own state to make the most appropriate action. Summary of the Invention

[0004] The main purpose of the present invention is to provide a robot motion control and state monitoring method and device to overcome the problems mentioned in the above background technology.

[0005] To achieve the above object, according to one aspect of the present invention, a robot motion control and state monitoring method is provided, comprising the following process:

[0006] S1. Receive robot information obtained based on a multimodal sensor array and synchronize the robot information;

[0007] S2. Perform health monitoring on the robot to obtain a health status vector, and analyze the robot's health status in combination with the robot information and the health status vector to implement the corresponding safety protection strategy;

[0008] S3. Build an environmental dynamic model, determine the state of environmental changes based on robot information, and determine whether to reconstruct the environmental dynamic model. If reconstruction is required, output the reconstructed trajectory state vector and the risk weights corresponding to the trajectory obstacles based on the reconstructed environmental dynamic model; the state vector includes the ID, position, velocity, and covariance of the current trajectory;

[0009] S4. Combine the trajectory state vector and the risk weight into a decision state, and generate the final high-level action a through a hybrid algorithm, specifically:

[0010] S41. Hyperparameter Setting: Define how the agent perceives its environment and itself, as well as the range of actions it can perform, to construct the state space and action space in the decision-making process.

[0011] S42. Given a state vector, learn an optimal policy that maps to an action a to minimize the collision risk and track the desired trajectory.

[0012] S43. Convert the high-level action a generated by the strategy into an executable instruction for downstream control;

[0013] S5. Perform joint-level dynamic compensation control according to the converted executable instructions. After execution, drive the encoder and torque sensor to feedback the phase coordinates of the robot in the joint space and the actual measured joint torque. Calculate the difference between the joint compensation torque and the actual measured joint torque to obtain the joint torque difference, which is used for the next state estimation and model update to complete the closed-loop feedback control.

[0014] Furthermore, in step S1, the multimodal sensor array includes a laser radar, a binocular event camera, a flexible tactile skin, and a millimeter-wave radar set on the robot body; the robot information includes original point cloud, event stream, moment matrix, maximum detection distance, speed, angle, and resolution.

[0015] Furthermore, in step S2, the health status of the robot body is monitored to obtain a health status vector, and the robot health status is analyzed in combination with the robot information and the health status vector to execute a corresponding safety protection strategy, as follows:

[0016] (S21) The health status vector includes motor temperature, drive current, voltage, joint torque difference, battery SOC and SOH. When the model prediction residual corresponding to the measured value of any health status vector is greater than 3σ, an alarm is triggered and the robot is directly taken offline. Otherwise, each indicator in the health status vector is mapped to a health sub-score in the interval [0,1], and then the sub-scores are weighted and fused according to the corresponding importance to obtain a health score.

[0017] (S22) Set a health threshold. If the health score is greater than or equal to the health threshold, it means that the current robot is in good health and no processing is required. If the health score is less than the health threshold, it means that there is a certain health risk, and the degradation mode is triggered. The degradation mode is speed limit and joint function limit. If the degradation mode is triggered more than 3 times in a row, the robot will be shut down and offline.

[0018] Furthermore, in step S3, an environmental dynamic model is constructed, and the state of environmental changes is judged based on the robot information to determine whether to reconstruct the environmental dynamic model. If reconstruction is performed, the reconstructed state vector on the trajectory and the risk weights corresponding to the obstacles on the trajectory are output based on the reconstructed environmental dynamic model, as follows:

[0019] (S31) determining whether the currently perceived dynamic obstacle list is too different from the existing environment model to determine whether to reconstruct the environment model; if so, executing step (S32); if not, updating the state vector without remodeling;

[0020] (S32) Clustering and associating the obstacle point cloud, extracting the ID, position, speed and covariance of each obstacle from the continuous frames, specifically:

[0021] Perform density clustering on the point cloud, set the neighborhood radius and the minimum number of points in the cluster; traverse all points, and for each unvisited point p, find all points within its neighborhood radius. If the neighborhood points are ≥ the minimum number of points in the cluster, then expand a cluster starting from this point p, iteratively add all density-reachable points until the cluster no longer grows. In this way, several cluster sets can be generated, each cluster represents the current observation set of an obstacle;

[0022] Based on the trajectory state of the previous moment, the prior state of the current frame is estimated as Where i represents a certain trajectory, with the prior state as the center, the association probability βij of all clusters in the current frame and the trajectory is calculated, j represents the index of all clusters, and the contribution of the soft-assigned measurement point to this trajectory;

[0023] Update the trajectory state with a unique ID to the weighted fusion posterior;

[0024] The weighted fusion algorithm is:

[0025]

[0026] in represents the prior state estimate of the i-th trajectory at the current moment, that is, the position and velocity information obtained based on the posterior estimate of the previous moment and the motion model prediction; Ki represents the Kalman gain matrix of the i-th trajectory, zj represents the measurement vector of the j-th cluster, H is the observation matrix, Si is the innovation covariance of the i-th trajectory, which represents the variance of the difference between the predicted measurement and the actual measurement, Pi - represents the prior covariance matrix corresponding to the i-th trajectory, which measures the uncertainty of the predicted state and is determined by the noise of the motion model process and the previous posterior covariance; Gate refers to the set of measurements that pass the test in the validation gate, which is used to filter out measurements that do not match the current trajectory prediction; T represents the matrix transpose operation, which flips the matrix along the main diagonal so that its rows and columns are interchanged;

[0027] (S33) Calculate the risk weight based on the shortest collision time of each dynamic obstacle relative to the robot, and output a set of risk weights ω i∈[0,1], specifically:

[0028] For each obstacle i, calculate the Euclidean distance di between the robot and each obstacle i and the relative speed vrel between the obstacle and the robot, using the formula Calculate the shortest collision time TTCi between obstacle i and the robot while maintaining the current relative speed, and map the shortest collision time TTCi to the risk weight ωi, with the value range of ωi being [0,1]. The basic mapping rule is that when the risk weight ωi is closer to 1, the shortest collision time TTCi is smaller, that is, the time before the robot and the obstacle will collide is extremely short, which is a high-risk scenario and requires immediate and high-priority obstacle avoidance or braking measures; conversely, when the shortest collision time TTCi is larger, that is, the obstacle and the robot maintain a long distance or the relative motion is not close, which is a low-risk scenario and the robot can continue to move along the established trajectory without emergency intervention;

[0029] In the same frame, all obstacles faced by the robot are sorted from high to low according to their corresponding risk weights, ensuring that the controller prioritizes the most dangerous obstacles. When the maximum risk weight exceeds the set weight threshold, the robot is controlled to switch to emergency braking or maximum detour strategy; otherwise, the local trajectory is offset according to the risk weight ratio to avoid multiple low-risk obstacles within a certain tolerance.

[0030] Furthermore, in the step (S31), it is determined whether the currently perceived dynamic obstacle list is too different from the existing environment model to decide whether to reconstruct the environment model. The determination is based on the following:

[0031] (S31.1) Calculate the target number change rate between the previous obstacle list and the newly detected obstacle list, and set a target change threshold. If the target number change rate is greater than the set target change threshold, determine that the environment has changed dramatically.

[0032] (S31.2) Calculating the average Euclidean distance of the target to obtain a trajectory displacement, and setting a displacement threshold. If the trajectory displacement is greater than the set displacement threshold, it is determined that the environment has changed dramatically;

[0033] (S31.3) If the target number change rate is less than or equal to the set target change threshold, and the trajectory displacement is less than or equal to the set displacement threshold, the environment is determined to be stable and no model optimization changes are required; otherwise, a model reconstruction instruction is generated.

[0034] Furthermore, in step S42, an optimal strategy mapped to action a is learned under a given state vector. The specific process is as follows:

[0035] (S42.1) Concatenate the ID, position, velocity, state covariance, and risk weight of each obstacle with the robot's posture to form a state vector, and simultaneously execute steps (62) and (63) of the two paths;

[0036] (S42.2) Execute the MPC algorithm and output the MPC candidate action at that moment;

[0037] (S42.3) Execute the SAC algorithm and output the SAC candidate action at that moment;

[0038] (S42.4) Calculate the uncertainty of the two paths to obtain an uncertainty measure, combine the candidate actions output by the two paths with the uncertainty measure and weight them with a logistic function to obtain the final fusion action a.

[0039] Furthermore, step (S42.2) executes the MPC algorithm and outputs the MPC candidate action at that moment. The specific execution process is as follows:

[0040] One MPC algorithm performs a first-order Taylor expansion on the robot's dynamics equations at the previous state point to obtain a linearized model. Given a prediction step size and sampling time, it solves a constrained quadratic programming problem at each control moment. An efficient solver is then used to obtain the optimal input sequence. The first control variable is then selected as the candidate MPC action at that moment for subsequent fusion.

[0041] Step (S42.3) executes the SAC algorithm and outputs the SAC candidate action at that moment. The specific execution process is as follows:

[0042] The other is the SAC algorithm: it receives a state vector as input to the SAC algorithm to capture the current dynamic relationship between the robot itself and multiple obstacles, uses the policy network to receive the state vector, and outputs the action distribution parameters, which include the mean and logarithmic standard deviation; adopts a temporary action from the Gaussian distribution, and then compresses it to the action range allowed by the policy range, and finally takes the mapped mean as the SAC candidate action.

[0043] Furthermore, in step S43, the high-level action a generated by the strategy is converted into an instruction executable by the downstream control, specifically as follows:

[0044] Identify the type of high-level action a. If it is a joint acceleration, it is directly used as the input for joint drive compensation control. If it is a Cartesian velocity, then in each control cycle, the terminal velocity is used as a constraint. Combined with the robot kinematics inverse solution and dynamic model, a smooth joint position and velocity curve is obtained. At the same time, obstacle avoidance and joint limit constraints are added to ensure that the joint trajectory meets physical limits and safety requirements. The joint reference trajectory is thus generated and passed to the downstream for joint dynamic compensation control.

[0045] Furthermore, the joint-level dynamic compensation control process in S5 is as follows:

[0046] Dynamic compensation: The robot dynamics equation is used to call the cylinder dynamics library internally to calculate various matrices and vectors, and then the pure dynamic compensation torque τu is obtained by combining them with the joint acceleration vector and the current joint position and velocity through the robot dynamics equation;

[0047] Friction compensation: Based on the τu calculated by dynamic compensation, the joint velocity and the actual measured joint velocity are read at the same time, and the friction torque τfric is calculated based on the Coulomb + viscous friction model.

[0048] Elastic joint compensation: Extract the motor side angle and load side angle in the joint reference trajectory and use them through the equivalent torsion spring model to obtain the elastic compensation τs;

[0049] The dynamics, friction and elastic compensation are inherited to calculate τu, τfric and τs, and the three are added together to synthesize the final driving torque τd, which is then sent to the driver through the inner-loop PD controller. The current / torque closed-loop tracking is used to drive the joint to move synchronously according to the desired acceleration and reference trajectory.

[0050] According to a second aspect of the present invention, the present invention provides a robot motion control and state monitoring device for implementing the above-mentioned robot motion control and state monitoring method, the device comprising:

[0051] The sensor array module is used to receive robot information acquired based on the multimodal sensor array and synchronize and align the robot information;

[0052] The body monitoring module is used to monitor the health of the robot body to obtain a health state vector, and analyze the robot's health state by combining the robot information and the health state vector to implement the corresponding safety protection strategy;

[0053] The environmental dynamic modeling module is used to build an environmental dynamic model. It determines the environmental change state based on the robot information and decides whether to reconstruct the environmental dynamic model. If reconstruction is required, it outputs the reconstructed trajectory state vector and the risk weights corresponding to the trajectory obstacles based on the reconstructed environmental dynamic model. The state vector includes the ID, position, velocity, and covariance of the current trajectory.

[0054] The action output module is used to combine the trajectory state vector and the risk weight into a decision state and generate the final high-level action a through a hybrid algorithm;

[0055] The joint compensation module is used to perform joint-level dynamic compensation control according to the converted executable instructions. After execution, the drive encoder and torque sensor feedback the phase coordinates of the robot in the joint space and the actual measured joint torque, and calculate the difference between the joint compensation torque and the actual measured joint torque to obtain the joint torque difference, which is used for the next state estimation and model update to complete closed-loop feedback control.

[0056] Beneficial effects of the present invention:

[0057] This invention combines Kalman filter prediction residuals with 3σ anomaly detection to achieve online health assessment of motor temperature, drive current / voltage, joint torque difference, and battery SOC / SOH. In the case of non-significant anomalies, further health management is performed based on the robot's health state vector. Robots in poor condition are downgraded (speed limit and joint function restriction). In risk-controlled, non-emergency situations, operating time is extended and maintenance is scheduled in advance, balancing production efficiency and equipment safety. This improves the system's flexible response to health risks and solves the problem of pre-planned control being unable to incorporate its own status into decision-making.

[0058] By closely linking environmental perception (step S3) with high-level decision-making (step S4), the present invention ensures that each control decision not only considers the threat level of real-time dynamic obstacles but also takes into account the advantages of model prediction and learning strategies, thereby achieving safe and efficient movement of the robot in sudden change environments. Compared with pure MPC, which is susceptible to model mismatch, and pure RL, which is prone to loss of safety guarantees, the hybrid solution retains the advantages of both, achieving dual innovations in real-time performance and safety. The decision-making framework that integrates uncertainty measurement enhances the ability to adapt to sudden scenarios and overcomes the defect that static strategies are prone to failure in dynamic environments.

[0059] The present invention compensates joints in real time through dynamic compensation, friction compensation, and elasticity compensation, improving control accuracy and smoothness. It observes and feeds back execution residuals and trajectory deviations, enabling online correction of internal and external models and ensuring robustness in long-term operation. Traditional joint control often ignores friction and elasticity, leading to tracking deviations. The use of all-round compensation significantly reduces tracking errors and energy waste. Closed-loop feedback enables the system to continuously adapt during operation, avoiding performance degradation under single calibration.

[0060] In summary, the present invention achieves a closed-loop, full-link approach from perception, monitoring, modeling, decision-making, to execution, integrating environmental mutations with the robot's health information in real time, and ensuring safe and efficient movement through hybrid decision-making and joint compensation. Compared to traditional robot control systems that rely on pre-planned paths, this solution can respond to environmental dynamics at the millisecond level, assess health risks in real time, and adaptively adjust strategies, thereby fundamentally overcoming the problems of insufficient real-time performance and flexibility, and achieving a deep correlation with the robot's own state, significantly improving movement accuracy and safety assurance capabilities in complex and sudden scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] The accompanying drawings, which constitute part of the present invention, are provided to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are provided to explain the present invention and do not constitute an undue limitation of the present invention. In the accompanying drawings:

[0062] Figure 1 It is a schematic flow chart of the method of the present invention;

[0063] Figure 2 It is a schematic diagram of system module connection of the present invention. DETAILED DESCRIPTION

[0064] It should be noted that, in the absence of conflict, the embodiments and features of the embodiments of the present invention can be combined with each other. The present invention will be described in detail below with reference to the accompanying drawings and in combination with the embodiments.

[0065] In order to enable those skilled in the art to better understand the solutions of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the embodiments described are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts should fall within the scope of protection of the present invention.

[0066] It should be noted that the terms "first," "second," and the like in the specification and claims of the present invention and the accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate for the embodiments of the present invention described herein. In addition, the terms "including," "having," and any variations thereof are intended to cover non-exclusive inclusions. For example, a process, method, system, product, or apparatus comprising a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to these processes, methods, products, or apparatuses.

[0067] In order to make the objects and advantages of the present invention more clearly understood, the present invention is further described below in conjunction with embodiments; it should be understood that the specific embodiments described herein are merely used to explain the present invention and are not intended to limit the present invention.

[0068] Example 1:

[0069] According to an embodiment of the present invention, Figure 1 As shown, a robot motion control and state monitoring method is provided, which includes the following steps:

[0070] S1, multimodal sensor array to obtain robot information:

[0071] The robot is equipped with a multimodal sensor array to obtain robot information. The multimodal sensors include lidar (to capture dense geometric point clouds), binocular event cameras (to quickly capture dynamic obstacles), flexible tactile skin (to achieve near-field collision detection), millimeter-wave radar (to supplement low-visibility environment perception), etc.; specific robot information includes original point clouds, event streams, torque matrices, maximum detection distance, speed, angle and resolution, which are synchronized and aligned.

[0072] S2, robot body state depth perception:

[0073] The health status vector of the robot body is obtained by health monitoring, where the health status vector includes motor temperature, drive current, voltage, joint torque difference, battery SOC and SOH. When the model prediction residual corresponding to any measurement value is greater than 3σ, an alarm is triggered, the robot is directly taken offline, all movements and actuator drives are stopped, and an operation and maintenance work order is generated to the background; otherwise, each indicator in the health status vector is mapped to a health sub-score in the [0,1] interval, and then the health score is calculated by weighted fusion according to the corresponding importance, where the motor temperature is mapped to obtain the health sub-score h T , as an example to explain in detail: Get the upper and lower limit settings of the robot motor temperature, the normal temperature Tnom (usually 60℃) and the limit temperature Tmax (usually 90℃), as well as the current motor temperature Tmot, the mapping formula is Set a health threshold. If the health score is greater than or equal to the health threshold, it means that the current robot is in good health and no processing is required. If the health score is less than the health threshold, it means that there is a certain health risk, and the degradation mode is triggered (the degradation mode is speed limit and joint function limit) to reduce the risk of fault propagation, and an operation and maintenance work order is generated to the background. If the degradation mode is triggered more than three times in a row, the robot will be shut down and offline, and an operation and maintenance work order will be generated to the background. It should be explained that 3σ represents the 3σ principle (σ represents the standard deviation in statistics). In the normal distribution (Gaussian distribution), about 99.73% of the data points will fall within the range of ±3σ of the mean (μ), and only 0.27% of the data may exceed this range. Data outside this range is considered to be significantly abnormal. The 3σ principle is an anomaly detection threshold widely used in the industry. If the measured value exceeds μ±3σ, it is considered that the system has a significant abnormality and requires immediate intervention.

[0074] By combining Kalman filter prediction residuals with 3σ anomaly detection, online health assessments of motor temperature, drive current / voltage, joint torque difference, and battery SOC / SOH are achieved. In the event of non-significant anomalies, further health management is performed based on the robot's health status vector. Robots in poor condition are downgraded (speed and joint function restrictions are implemented). In risk-controlled, non-emergency situations, operating time is extended and maintenance is scheduled in advance, balancing production efficiency and equipment safety. This improves the system's flexible response to health risks and addresses the issue of pre-planned control failing to incorporate its own status into decision-making.

[0075] S3, environmental dynamics modeling engine:

[0076] S3-1, determine whether the currently perceived dynamic obstacle list is too different from the existing environment model to decide whether to reconstruct the environment model. If so, execute S3-2; if not, update the state vector without remodeling;

[0077] Calculate the target number change rate between the previous obstacle list and the newly detected obstacle list, and set a target change threshold (this threshold is based on the dynamic environment difficulty assessment. Those skilled in the art believe that a target change rate exceeding 20% ​​will significantly affect the obstacle avoidance success rate). If the target number change rate is greater than the set target change threshold, it is determined that the environment has changed dramatically.

[0078] Calculate the average Euclidean distance to the target to obtain the trajectory displacement and set a displacement threshold (this value is selected from the dynamic detection method based on voxel grid difference to capture significant movement beyond the sensor resolution. Those skilled in the art believe that a trajectory displacement exceeding 0.5m will significantly affect obstacle avoidance). If the trajectory displacement is greater than the set displacement threshold, it is determined that the environment has changed dramatically.

[0079] If the target number change rate is less than or equal to the set target change threshold, and the trajectory displacement is less than or equal to the set displacement threshold, the environment is considered stable and the previous model can be reused directly without any model optimization changes. Otherwise, a "whole table reconstruction" instruction is generated.

[0080] S3-2, cluster and associate the obstacle point cloud / radar echo detected by the sensor, and use EKF+JPDA to extract the ID, position, velocity and covariance of each obstacle from the continuous frames. Specifically:

[0081] Perform density clustering on the point cloud, set the neighborhood radius (this value is selected based on the LiDAR point distribution and the minimum target size, and the technicians set it to 0.5m) and the minimum number of points in the cluster; traverse all points, and for each unvisited point p, find all points within its neighborhood radius. If the neighborhood points are ≥ the minimum number of points in the cluster, then expand a cluster starting from the point p, iteratively add all density-reachable points until the cluster stops growing. In this way, several cluster sets can be generated, each cluster represents the current observation set of an obstacle;

[0082] Based on the trajectory state (position, velocity and covariance) of the previous moment, the prior state of the current frame is estimated as Where i represents a certain trajectory, and with the prior state as the center, the association probability βi of all clusters in the current frame with the trajectory is calculated j , j represents the index of all clusters, and the contribution of the soft-assigned measurement point to the trajectory is updated; the trajectory state with a unique ID is updated to the posterior after JPDA weighted fusion. If a trajectory has no match for a long time or the association probability is extremely low, it is considered "trajectory termination"; "trajectory initialization" is started for the new detection cluster; the weighted fusion algorithm is:

[0083]

[0084] in represents the prior (predicted) state estimate of the i-th trajectory at the current moment, that is, the position and velocity information obtained based on the posterior estimate of the previous moment and the motion model prediction; K i represents the Kalman gain matrix of the i-th trajectory, z j represents the measurement vector of the jth cluster, H is the observation matrix, Si is the innovation covariance of the i-th trajectory, represents the variance of the difference between the predicted measurement and the actual measurement, P i - represents the prior covariance matrix corresponding to the i-th trajectory, which measures the uncertainty of the predicted state and is determined by the noise of the motion model process and the previous posterior covariance; Gate refers to the set of measurements that pass the test in the validation gate, which is used to filter out measurements that do not match the current trajectory prediction; T represents the matrix transpose operation, which flips the matrix along the main diagonal so that its rows and columns are interchanged;

[0085] S3-3 calculates the risk weight based on the shortest collision time (TTC) of each dynamic obstacle relative to the robot, and outputs a set of ωi∈[0,1], which is used to weight the importance of different obstacles in the decision module; specifically:

[0086] For each obstacle i, calculate the Euclidean distance di between the robot and each obstacle i and the relative speed vrel between the obstacle and the robot. If it is a positive value, it means that the robot and the obstacle are approaching; if it is negative or zero, it means that they are moving away or keeping a distance; through the formula Calculate the shortest collision time TTCi between obstacle i and the robot while maintaining the current relative speed, and map the shortest collision time TTCi to the risk weight ωi, with the value range of ωi being [0,1]. The basic mapping rule is that when the risk weight ωi is closer to 1, the shortest collision time TTCi is smaller, that is, the time when the robot and the obstacle are about to collide is extremely short, which is a high-risk scenario and requires immediate and high-priority obstacle avoidance or braking measures; conversely, the shortest collision time TTCi is larger, that is, the obstacle and the robot maintain a long distance or the relative motion is not close, which is a low-risk scenario and can continue to move along the established trajectory without emergency intervention; it should be explained that if the two continue to approach at the current speed, TTCi represents the time required for the expected collision; if they are not close (vrel < 0), it is considered that there will never be a collision;

[0087] In the same frame, the robot may face multiple obstacles. All obstacles are sorted in descending order according to their corresponding risk weights, ensuring that the controller prioritizes the most dangerous obstacles. When the maximum risk weight exceeds the set weight threshold, the robot is controlled to switch to emergency braking or maximum detour strategy; otherwise, the local trajectory is deflected according to the risk weight ratio to avoid multiple low-risk obstacles within a certain tolerance.

[0088] S3-4, takes the trajectory state and its risk weight as state input and sends it to S4 to drive the robot to generate the final motion command;

[0089] The dual thresholds of target number change rate and trajectory displacement are used to clearly determine sudden scene changes, enabling rapid response to sudden crowd flows or sudden changes in obstacle groups; DBSCAN clustering is used to extract obstacle clusters, and EKF and joint probability data are associated to reliably assign IDs, estimate positions / velocities, and covariances in multiple frames to achieve continuous and consistent obstacle tracking; risk weights are then mapped based on the shortest collision time between obstacles and robots, providing a priority ranking basis for the decision-making module. Combined with risk thresholds, emergency braking or maximum detours are implemented for high-risk obstacles, and smooth avoidance is performed for low-risk obstacles, taking into account both safety and traffic efficiency.

[0090] S4 combines the dynamic obstacle status bar and the risk weight into a decision state and generates the final high-level action through a hybrid algorithm (MPC and SAC); specifically:

[0091] S4-1, hyperparameter setting:

[0092] Define how the agent perceives the environment and itself, as well as the range of actions it can perform, to construct the state space and action space in the decision-making process:

[0093] Initially set the hyperparameters of MPC and SAC. Specific MPC hyperparameters include update frequency, number of prediction steps, and sample time; SAC hyperparameters include learning rate, replay pool size, batch size, discount factor, target network update rate, etc. Since MPC and SAC are well-known and commonly used technical operations for those skilled in the art, we will not elaborate on them in detail.

[0094] S4-2: Given a state vector, learn an optimal policy that maps to action a to minimize the collision risk and track the desired trajectory. Specifically:

[0095] Extract the ID, position, speed, state covariance and risk weight of each obstacle from S3, and concatenate it with the robot's posture to form a state vector. Execute two paths at the same time, one of which is an MPC algorithm: at the previous state point Perform a first-order Taylor expansion on the robot's dynamics equation to obtain a linearized model. Given a prediction step size and sampling time (generally equal to the control period), solve the constrained quadratic programming at each control moment, and then use an efficient solver (such as OSQP, qpOASES) to obtain the optimal input sequence. The first control variable is taken as the MPC candidate action at that moment for subsequent fusion. is the current phase coordinate of the robot, q represents the configuration of the robot at a certain moment, that is, the joint angle (or joint position) vector; Represents the first-order derivative of q with respect to time, that is, the angular velocity (for rotational joints) or linear velocity (for translational joints) of each joint; q determines the position, Determines the change over time - together they constitute the coordinate and velocity components in the phase space;

[0096] The other is the SAC algorithm: receiving the state vector as the input of the SAC algorithm to capture the current dynamic relationship between the robot itself and multiple obstacles, using the policy network to receive the state vector and output the action distribution parameters, including the mean and logarithmic standard deviation (usually stored in logarithmic form to ensure positive values). The network has two layers, each with 256 units fully connected and activated by ReLu; a temporary action is taken from the Gaussian distribution, and then it is compressed to the action range allowed by the policy range through tanh, and finally the mapped mean is taken as the SAC candidate action, which can retain the exploratory nature and ensure the stability of multiple sampling results; SAC uses the maximum entropy target and Bellman error to alternately optimize the policy and value network, and the experience replay pool is usually kept at 5×10 5 Size, the sampling batch is 256, and it is updated once per step;

[0097] The uncertainty of the two paths is calculated based on model integration or MC-Dropout variance to obtain an uncertainty measure. The candidate actions output by the two paths are combined with the uncertainty measure and weighted with a logistic function to obtain the final fused action a. It should be explained that the candidate actions output by the MPC branch are joint accelerations, while the candidate actions output by the SAC branch are task space velocities, i.e., Cartesian velocities.

[0098] S4-3, convert the high-level action a generated by the strategy into instructions that can be executed by downstream control, specifically:

[0099] Identify the type of high-level action a. If it is a joint acceleration, it is directly used as the input for S5's "joint drive compensation control." If it is a Cartesian velocity, then within each control cycle, using the terminal velocity as a constraint, the robot's inverse kinematics solution and dynamic model are combined to solve for a smooth joint position and velocity curve. At the same time, obstacle avoidance and joint limit constraints are added to ensure that the joint trajectory meets physical limits and safety requirements. This generates a joint reference trajectory and transmits it to S5.

[0100] By closely linking environmental perception (step S3) with high-level decision-making (step S4), each control decision is ensured to take into account both the threat level of real-time dynamic obstacles and the advantages of model prediction and learning strategies, thereby achieving safe and efficient movement of the robot in a sudden change environment. Compared with pure MPC, which is susceptible to model mismatch, and pure RL, which is prone to losing safety guarantees, the hybrid solution retains the advantages of both, achieving dual innovations in real-time performance and safety. The decision-making framework that integrates uncertainty measurement enhances the adaptability to sudden scenarios and overcomes the defect that static strategies are prone to failure in dynamic environments.

[0101] S5, joint-level dynamic compensation control, specifically:

[0102] Dynamic compensation: The robot dynamics equation is used to call the cylinder dynamics library internally to calculate various matrices (inertia matrix, centrifugal matrix, gravity term) and vectors, and then the pure dynamic compensation torque τu is obtained by combining them with the joint acceleration vector and the current joint position and velocity through the robot dynamics equation;

[0103] Friction compensation: inherit the τu calculated by dynamic compensation, read the joint velocity (i.e., the velocity component in the reference trajectory) and the actual measured joint velocity, and calculate the friction torque τfric based on the Coulomb + viscous friction model2.

[0104] Elastic joint compensation: Extract the motor side angle and load side angle in the joint reference trajectory and use them through the equivalent torsion spring model to obtain the elastic compensation τs;

[0105] The dynamics / friction / elasticity compensation is inherited to calculate τu, τfric and τs, and the three are added to form the final driving torque τd, which is then sent to the driver through the inner loop PD controller. The current / torque closed loop is tracked to drive the joint to move synchronously with the desired acceleration and reference trajectory. After execution, the drive encoder and torque sensor feedback the phase coordinate of the robot in the joint space. The actual measured joint torque (derived from the force / torque sensor or estimated based on the motor current) is used for the next state estimation and model update to complete the closed-loop feedback control.

[0106] Real-time compensation of joints is performed through dynamic compensation, friction and elasticity compensation to improve control accuracy and smoothness. Execution residuals, trajectory deviations, etc. are observed and fed back to achieve online correction of internal and external models, ensuring long-term robustness. Traditional joint control often ignores friction and elasticity, resulting in tracking deviations. The use of all-round compensation significantly reduces tracking errors and energy waste. Closed-loop feedback enables the system to continuously adapt during operation, avoiding performance degradation under single calibration.

[0107] Example 2:

[0108] like Figure 2 As shown, this embodiment further provides a robot motion control and state monitoring device, a robot motion control and state monitoring device, used to implement the above-mentioned robot motion control and state monitoring method, the device comprising:

[0109] The sensor array module is used to receive robot information acquired based on the multimodal sensor array and synchronize and align the robot information;

[0110] The body monitoring module is used to monitor the health of the robot body to obtain a health state vector, and analyze the robot's health state by combining the robot information and the health state vector to implement the corresponding safety protection strategy;

[0111] The environmental dynamic modeling module is used to build an environmental dynamic model. It determines the environmental change state based on the robot information and decides whether to reconstruct the environmental dynamic model. If reconstruction is required, it outputs the reconstructed trajectory state vector and the risk weights corresponding to the trajectory obstacles based on the reconstructed environmental dynamic model. The state vector includes the ID, position, velocity, and covariance of the current trajectory.

[0112] The action output module is used to combine the trajectory state vector and the risk weight into a decision state and generate the final high-level action a through a hybrid algorithm;

[0113] The joint compensation module is used to perform joint-level dynamic compensation control according to the converted executable instructions. After execution, the drive encoder and torque sensor feedback the phase coordinates of the robot in the joint space and the actual measured joint torque, and calculate the difference between the joint compensation torque and the actual measured joint torque to obtain the joint torque difference, which is used for the next state estimation and model update to complete closed-loop feedback control.

[0114] Specifically, the above-mentioned sensor array module, body monitoring module, environment dynamic modeling module, motion output module and joint compensation module can be embedded in a computer processing system. The computer calls the above-mentioned modules to complete the tasks of robot motion control and state monitoring based on the above-mentioned robot motion control and state monitoring method; the above-mentioned sensor array module, body monitoring module, environment dynamic modeling module, motion output module and joint compensation module can perform operations according to the specific steps given in the above-mentioned robot motion control and state monitoring method.

[0115] It should be noted that the division of the various modules of the above system is merely a division of logical functions. In actual implementation, they can be fully or partially integrated into a physical entity, or they can be physically separated. These modules can be implemented entirely in the form of software called by a processing element; or entirely in the form of hardware; or some modules can be implemented in the form of software called by a processing element, and some modules can be implemented in the form of hardware. For example, the sensor array module can be a separate processing element, or it can be integrated into a chip of the above device. In addition, it can be stored in the memory of the above device in the form of program code, and called by a processing element of the above device to perform the functions of the above signal processing module. The implementation of other modules is similar. In addition, these modules can be fully or partially integrated together, or they can be implemented independently. The processing element described here can be an integrated circuit with signal processing capabilities. In the implementation process, each step of the above method or each of the above modules can be completed by the hardware integrated logic circuit in the processor element or by software instructions.

[0116] For example, the above modules may be one or more integrated circuits configured to implement the above methods, such as one or more application-specific integrated circuits (ASICs), one or more digital signal processors (DSPs), or one or more field programmable gate arrays (FPGAs). For another example, when a module is implemented by scheduling program code through a processing element, the processing element may be a general-purpose processor, such as a central processing unit (CPU) or other processor that can call program code. For another example, these modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0117] The above are merely embodiments of the present invention and are not intended to limit the present invention. It will be apparent to those skilled in the art that various modifications and variations of the present invention are possible. Any modifications, equivalent substitutions, improvements, etc. made within the spirit and principles of the present invention are intended to be included within the scope of the claims of the present invention.

Claims

1. A robot motion control and state monitoring method, characterized in that: The following steps are involved: S1. Receive robot information obtained based on a multimodal sensor array and synchronize the robot information; S2. Perform health monitoring on the robot to obtain a health status vector, and analyze the robot's health status in combination with the robot information and the health status vector to implement the corresponding safety protection strategy; S3. Build an environmental dynamic model, determine the state of environmental changes based on the robot information, and determine whether to reconstruct the environmental dynamic model. If reconstruction is performed, the reconstructed trajectory state vector and the risk weight corresponding to the trajectory obstacle are output based on the reconstructed environmental dynamic model. The state vector includes the ID, position, velocity, and covariance of the current trajectory; S4. Combine the trajectory state vector and the risk weight into a decision state, and generate the final high-level action a through a hybrid algorithm, specifically: S41. Hyperparameter Setting: Define how the agent perceives its environment and itself, as well as the range of actions it can perform, to construct the state space and action space in the decision-making process. S42. Given a state vector, learn an optimal policy that maps to an action a to minimize the collision risk and track the desired trajectory. S43. Convert the high-level action a generated by the strategy into an executable instruction for downstream control; S5. Perform joint-level dynamic compensation control based on the converted executable instructions. After execution, the drive encoder and torque sensor provide feedback on the robot's phase coordinates in the joint space and the actual measured joint torque. The difference between the joint compensation torque and the actual measured joint torque is calculated to obtain the joint torque difference, which is used for the next state estimation and model update, completing closed-loop feedback control. In step S2, the health status of the robot body is monitored to obtain a health status vector. The robot health status is analyzed in combination with the robot information and the health status vector to implement the corresponding safety protection strategy, as follows: (S21) The health status vector includes motor temperature, drive current, voltage, joint torque difference, battery SOC and SOH. When the model prediction residual corresponding to the measured value of any health status vector is greater than 3σ, an alarm is triggered and the robot is directly taken offline. Otherwise, each indicator in the health status vector is mapped to a health sub-score in the range [0,1], and then the sub-scores are weighted and fused according to the corresponding importance to obtain the health score. (S22) Setting a health threshold. If the health score is greater than or equal to the health threshold, it indicates that the robot is currently in good health and no further processing is required. If the health score is less than the health threshold, it indicates that there is a certain health risk and a degradation mode is triggered. The degradation mode includes speed and joint function restrictions. If the degradation mode is triggered more than three times in a row, the robot is shut down and taken offline. In step S3, an environmental dynamic model is constructed. The state of the environment change is judged based on the robot information, and it is determined whether to reconstruct the environmental dynamic model. If reconstruction is performed, the reconstructed state vector on the trajectory and the risk weights corresponding to the obstacles on the trajectory are output based on the reconstructed environmental dynamic model. The details are as follows: (S31) determining whether the currently perceived dynamic obstacle list differs too much from the existing environment model to determine whether to reconstruct the environment model; if so, executing step (S32); if not, updating the state vector without remodeling; (S32) Clustering and associating the obstacle point cloud, extracting the ID, position, speed and covariance of each obstacle from the continuous frames, specifically: Perform density clustering on the point cloud, set the neighborhood radius and the minimum number of points in the cluster; traverse all points, and for each unvisited point p, find all points within its neighborhood radius. If the neighborhood points are ≥ the minimum number of points in the cluster, then expand a cluster starting from this point p, iteratively add all density-reachable points until the cluster no longer grows. In this way, several cluster sets can be generated, each cluster represents the current observation set of an obstacle; Based on the trajectory state of the previous moment, the prior state of the current frame is estimated as , where i represents a certain trajectory, and with the prior state as the center, the association probability of all clusters in the current frame with the trajectory is calculated , j represents the index of all clusters, and the contribution of the soft-assigned measurement points to this trajectory; Update the trajectory state with a unique ID to the weighted fusion posterior; The weighted fusion algorithm is: in represents the prior state estimate of the i-th trajectory at the current moment, that is, the position and velocity information obtained based on the posterior estimate at the previous moment and the motion model prediction; represents the Kalman gain matrix of the i-th trajectory, represents the measurement vector of the jth cluster, H is the observation matrix, is the innovation covariance of the ith trajectory, which represents the variance of the difference between the predicted measurement and the actual measurement, represents the prior covariance matrix corresponding to the i-th trajectory, which measures the uncertainty of the predicted state and is determined by the noise of the motion model process and the previous posterior covariance. Gate refers to the set of measurements that pass the validation gate and is used to filter out measurements that do not match the current trajectory prediction. T represents the matrix transpose operation, which flips the matrix along the main diagonal so that its rows and columns are interchanged. (S33) Calculate the risk weight based on the shortest collision time of each dynamic obstacle relative to the robot, and output a set of risk weights ∈[0,1], specifically: For each obstacle i, calculate the Euclidean distance between the robot and each obstacle i and the relative speed between the obstacle and the robot , through the formula Calculate the shortest collision time between obstacle i and the robot while maintaining the current relative speed , and the shortest collision time Mapping to risk weights ,and The value range is [0,1], and the basic mapping rule is that when the risk weight The closer it is to 1, the shorter the collision time. The smaller the value, the shorter the time until the robot collides with the obstacle. This is a high-risk scenario, requiring immediate, high-priority obstacle avoidance or braking measures. Conversely, it indicates the shortest collision time. The larger the value, the farther the obstacle is from the robot or the relative motion is not approaching. This is a low-risk scenario and the robot can continue to move along the established trajectory without emergency intervention. In the same frame, all obstacles faced by the robot are sorted from high to low according to their corresponding risk weights, ensuring that the controller prioritizes the most dangerous obstacles. When the maximum risk weight exceeds the set weight threshold, the robot is controlled to switch to emergency braking or maximum detour strategy; otherwise, the local trajectory is offset according to the risk weight ratio to avoid multiple low-risk obstacles within a certain tolerance.

2. A robot motion control and state monitoring method according to claim 1, characterized in that: In step S1, a multimodal sensor array is provided on the robot body, including a laser radar, a binocular event camera, a flexible tactile skin, and a millimeter-wave radar; and the robot information includes original point cloud, event stream, moment matrix, maximum detection distance, speed, angle, and resolution.

3. A robot motion control and state monitoring method according to claim 2, characterized in that: In the step (S31), it is determined whether the currently perceived dynamic obstacle list is too different from the existing environment model to determine whether to reconstruct the environment model. The determination is based on the following: (S31.1) Calculate the target number change rate between the previous obstacle list and the newly detected obstacle list, and set a target change threshold. If the target number change rate is greater than the set target change threshold, determine that the environment has changed dramatically. (S31.2) Calculating the average Euclidean distance of the target to obtain a trajectory displacement, and setting a displacement threshold. If the trajectory displacement is greater than the set displacement threshold, it is determined that the environment has changed dramatically. (S31.3) If the target number change rate is less than or equal to the set target change threshold, and the trajectory displacement is less than or equal to the set displacement threshold, the environment is determined to be stable and no model optimization changes are required; otherwise, a model reconstruction instruction is generated.

4. A robot motion control and state monitoring method according to claim 3, characterized in that: In step S42, under a given state vector, an optimal strategy mapped to action a is learned. The specific process is: (S42.1) Concatenate the ID, position, velocity, state covariance, and risk weight of each obstacle with the robot's posture to form a state vector, and simultaneously execute steps (62) and (63) of the two paths; (S42.2) Execute the MPC algorithm and output the MPC candidate action at that moment; (S42.3) Execute the SAC algorithm and output the SAC candidate action at that moment; (S42.4) Calculate the uncertainty of the two paths to obtain an uncertainty measure, combine the candidate actions output by the two paths with the uncertainty measure and weight them with a logistic function to obtain the final fusion action a.

5. A robot motion control and state monitoring method according to claim 4, characterized in that: Step (S42.2) executes the MPC algorithm and outputs the MPC candidate action at that moment. The specific execution process is as follows: One MPC algorithm performs a first-order Taylor expansion on the robot's dynamics equations at the previous state point to obtain a linearized model. Given a prediction step size and sampling time, it solves a constrained quadratic programming problem at each control moment. An efficient solver is then used to obtain the optimal input sequence. The first control variable is then selected as the candidate MPC action at that moment for subsequent fusion. Step (S42.3) executes the SAC algorithm and outputs the SAC candidate action at that moment. The specific execution process is as follows: The other is the SAC algorithm: it receives a state vector as input to the SAC algorithm to capture the current dynamic relationship between the robot itself and multiple obstacles, uses the policy network to receive the state vector, and outputs the action distribution parameters, which include the mean and logarithmic standard deviation; adopts a temporary action from the Gaussian distribution, and then compresses it to the action range allowed by the policy range, and finally takes the mapped mean as the SAC candidate action.

6. A robot motion control and state monitoring method according to claim 5, characterized in that: In step S43, the high-level action a generated by the strategy is converted into instructions executable by the downstream control, specifically as follows: Identify the type of high-level action a. If it is a joint acceleration, it is directly used as the input for joint drive compensation control. If it is a Cartesian velocity, then in each control cycle, the terminal velocity is used as a constraint. Combined with the robot kinematics inverse solution and dynamic model, a smooth joint position and velocity curve is obtained. At the same time, obstacle avoidance and joint limit constraints are added to ensure that the joint trajectory meets physical limits and safety requirements. The joint reference trajectory is thus generated and passed to the downstream for joint dynamic compensation control.

7. A robot motion control and state monitoring method according to claim 6, characterized in that: The joint-level dynamic compensation control process in S5 is as follows: Dynamic compensation: The robot dynamics equation is used to call the cylinder dynamics library internally to calculate various matrices and vectors, and then the pure dynamic compensation torque is obtained by combining them with the joint acceleration vector and the current joint position and speed through the robot dynamics equation. ; Friction compensation: calculated based on dynamic compensation , read the joint velocity and the actual measured joint velocity at the same time, and compare it with The friction torque is calculated based on the Coulomb + viscous friction model ; Elastic joint compensation: Extract the motor side angle and load side angle in the joint reference trajectory and use them to obtain elastic compensation through the equivalent torsion spring model ; Inherited dynamics, friction and elasticity compensation calculations 、 and And add the three to synthesize the final driving torque The current / torque closed-loop tracking is used to drive the joint to move synchronously with the desired acceleration and reference trajectory.

8. A robot motion control and state monitoring device, used to implement a robot motion control and state monitoring method according to any one of claims 1 to 7, characterized in that: The device includes: The sensor array module is used to receive robot information acquired based on the multimodal sensor array and synchronize and align the robot information; The body monitoring module is used to monitor the health of the robot body to obtain a health state vector, and analyze the robot's health state by combining the robot information and the health state vector to implement the corresponding safety protection strategy; The environmental dynamic modeling module is used to build an environmental dynamic model. It determines the environmental change state based on the robot information and decides whether to reconstruct the environmental dynamic model. If reconstruction is required, it outputs the reconstructed trajectory state vector and the risk weights corresponding to the trajectory obstacles based on the reconstructed environmental dynamic model. The state vector includes the ID, position, velocity, and covariance of the current trajectory. The action output module is used to combine the trajectory state vector and the risk weight into a decision state and generate the final high-level action a through a hybrid algorithm; The joint compensation module is used to perform joint-level dynamic compensation control according to the converted executable instructions. After execution, the drive encoder and torque sensor feedback the phase coordinates of the robot in the joint space and the actual measured joint torque, and calculate the difference between the joint compensation torque and the actual measured joint torque to obtain the joint torque difference, which is used for the next state estimation and model update to complete closed-loop feedback control.

Images