Robot motion control and state monitoring method and device

Abstract

The invention discloses a robot motion control and state monitoring method and device, and relates to the technical field of robot control, and the method specifically comprises the following steps: receiving robot information obtained based on a multi-modal sensor array, and carrying out the synchronous alignment processing of the robot information; performing health monitoring on the body state of the robot to obtain a health state vector, and analyzing the health state of the robot in combination with the robot information and the health state vector; according to the method, full-link closed loop from sensing, monitoring, modeling, decision making to execution is achieved, environment sudden change and body health information are fused in real time, safety and efficiency of motion are ensured through mixed decision making and joint compensation, and compared with traditional robot control depending on a pre-planned path, the method has the advantages that the robot control efficiency is improved. The method can quickly respond to environmental dynamics, assess health risks in real time and adaptively adjust the strategy, overcomes the problem of insufficient real-time performance and flexibility, realizes deep association of the state of the robot, and improves the motion precision of the robot in a complex sudden change scene.

Description

Technical Field

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

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

[0003] Currently, robots usually perform motion control based on pre-planned routes or operating actions, which results in insufficient real-time performance and flexibility of the robots, unable to respond promptly to sudden environmental scenarios, and unable to deeply associate the robot's own state to make the most appropriate actions. Summary of the Invention

[0004] The main object of the present invention is to provide a method and device for robot motion control and status monitoring to overcome the problems mentioned in the above background art.

[0005] To achieve the above object, according to one aspect of the present invention, a method for robot motion control and status monitoring is provided, including the following processes:

[0006] S1. Receive robot information obtained based on a multi-modal sensor array and perform synchronous alignment processing on the robot information.

[0007] S2. Conduct health monitoring on the robot body state to obtain a health state vector, and analyze the robot health state by combining the robot information and the health state vector to execute corresponding safety protection strategies.

[0008] S3. Construct an environmental dynamic model, judge the environmental change state according to the robot information, determine whether to reconstruct the environmental dynamic model, and if reconstruction is performed, output a reconstructed trajectory state vector and the risk weight corresponding to the trajectory obstacle based on the reconstructed environmental dynamic model; where the state vector includes the ID, position, speed, and covariance of the current trajectory.

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

[0010] S41. Hyperparameter setting: Define the way for the intelligent agent to perceive the environment and itself, as well as the range of executable actions, to construct the state space and action space in the decision-making process.

[0011] S42. Learn an optimal policy that maps to action a under a given state vector to minimize the collision risk and track the desired trajectory;

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

[0013] S5. Perform joint-level dynamic compensation control according to the converted executable instruction. After the execution is completed, drive the encoder and torque sensing to feedback the phase coordinates of the robot in the joint space and the actually measured joint torque, and calculate the difference between the joint compensation torque and the actually 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] Further, in step S1, the multi-modal sensor array is a lidar, a binocular event camera, a flexible tactile skin, and a millimeter-wave radar arranged on the robot body; the robot information includes the original point cloud, the event stream, the torque matrix, the maximum detection distance, the speed, the angle, and the resolution.

[0015] Further, in step S2, perform health monitoring on the robot body state to obtain a health state vector, and analyze the robot health state in combination with the robot information and the health state vector to execute corresponding safety protection strategies, specifically as follows:

[0016] (S21) The health state vector includes the 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 state vector > 3σ, trigger an alarm and directly take the robot offline; otherwise, map each index in the health state vector to a health sub-score in the [0,1] interval, and then calculate the weighted fusion according to the corresponding importance to obtain the 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, then trigger the downgrade mode, and the downgrade mode is speed limit and joint function limit; if the downgrade mode is triggered continuously more than 3 times, the robot will be shut down and taken offline.

[0018] Further, in step S3, construct an environmental dynamic model, judge the environmental change state according to the robot information, determine whether to reconstruct the environmental dynamic model. If reconstruction is performed, output the state vector on the reconstructed trajectory and the risk weight corresponding to the obstacle on the trajectory based on the reconstructed environmental dynamic model, specifically as follows:

[0019] (S31) Determine whether the currently perceived dynamic obstacle list is significantly different from the existing environment model to decide whether to reconstruct the environment model. If so, execute step (S32); if not, there is no need to re - model and perform state vector update;

[0020] (S32) Cluster and associate the obstacle point cloud, and extract the ID, position, speed, and covariance of each obstacle from consecutive frames. Specifically:

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

[0022] Based on the trajectory state at the previous moment, the prior state predicted to the current frame is denoted as where i represents a certain trajectory. Taking the prior state as the center, calculate the association probability βij between all clusters in the current frame and this trajectory, where j represents the index of all clusters, and softly assign the contribution of the measurement points to this trajectory;

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

[0024] The weighted fusion algorithm is:

[0025]

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

[0027] (S33) Calculate the risk weights 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, as well as the relative velocity vrel between the obstacle and the robot, using the formula Calculate the shortest time to collision TTCi between obstacle i and the robot while maintaining the current relative velocity, and map the shortest time to collision TTCi to a risk weight ωi. The value range of ωi is [0, 1]. The basic mapping rule is that the closer the risk weight ωi is to 1, the smaller the shortest time to collision TTCi is, that is, the time for the robot and the obstacle to collide is extremely short, which belongs to a high-risk scenario and requires immediate and high-priority obstacle avoidance or braking measures; otherwise, it means that the shortest time to collision TTCi is larger, that is, the obstacle and the robot maintain a relatively long distance or the relative motion does not tend to approach, which belongs to a low-risk scenario and the robot can continue to move along the established trajectory without emergency intervention;

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

[0030] Furthermore, in step (S31), it is judged whether the currently perceived dynamic obstacle list is too different from the existing environment model to decide whether to reconstruct the environment model. The judgment basis is as follows:

[0031] (S31.1) Calculate the change rate of the number of targets between the previous obstacle list and the newly detected obstacle list this time, and set a target change threshold. If the change rate of the number of targets > the set target change threshold, it is determined that the environment has changed drastically;

[0032] (S31.2) Calculate the average Euclidean distance of the targets to obtain the trajectory displacement amount, and set a displacement threshold. If the trajectory displacement amount > the set displacement threshold, it is determined that the environment has changed drastically;

[0033] (S31.3) If the change rate of the number of targets ≤ the set target change threshold and the trajectory displacement amount ≤ the set displacement threshold, it is determined that the environment is stable and no model optimization change is required; otherwise, generate a model reconstruction instruction.

[0034] Furthermore, in step S42, under the given state vector, learn an optimal policy that maps to action a. The specific process is as follows:

[0035] (S42.1) Concatenate the ID, position, velocity, state covariance, and risk weight of each obstacle with the pose of the robot to form a state vector, and at the same time execute two path steps (62) and (63);

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

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

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

[0039] Furthermore, in step (S42.2), the MPC algorithm is executed to output the MPC candidate action at this moment. The specific execution process is as follows:

[0040] One of the MPC algorithms: perform a first-order Taylor expansion on the robot dynamics equation at the previous state point to obtain a linearized model. Given the prediction step and sampling time, solve the constrained quadratic programming at each control moment, and then use an efficient solver to obtain the optimal input sequence, and take the first control quantity as the MPC candidate action at this moment for subsequent fusion;

[0041] In step (S42.3), the SAC algorithm is executed to output the SAC candidate action at this moment. The specific execution process is as follows:

[0042] The other is the SAC algorithm: receive the state vector as the input of the SAC algorithm to capture the current dynamic association between the robot itself and multiple obstacles. Use the policy network to receive the state vector and output the action distribution parameters, where the distribution parameters include the mean and log standard deviation; sample a temporary action from the Gaussian distribution, and then compress it to the action range allowed by the policy range. Finally, take the mapped mean as the SAC candidate action.

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

[0044] Identify the type of the high-level action a. If it is joint acceleration, it is directly used as the input of the joint drive compensation control; if it is Cartesian velocity, in each control cycle, with the end velocity as the constraint, combine the inverse kinematics and dynamic model of the robot to solve for a smooth joint position and velocity curve, and at the same time add obstacle avoidance and joint limit constraints to ensure that the joint trajectory meets the physical limits and safety requirements, thereby generating a joint reference trajectory and transmitting it downstream for joint dynamic compensation control.

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

[0046] Dynamic compensation: Using the robot dynamics equation, call the cylinder dynamics library internally to calculate various matrices and vectors, and obtain the pure dynamic compensation torque τu through the robot dynamics equation with the joint acceleration vector, as well as the current joint position and velocity;

[0047] Friction compensation: Based on τu calculated by dynamic compensation, read the joint velocity and the actually measured joint velocity at the same time, and calculate the friction torque τfric according to the Coulomb + viscous friction model with τu;

[0048] Elastic joint compensation: Extract the motor-side angle and load-side angle from the joint reference trajectory, and obtain the elastic compensation τs through the equivalent torsional spring model;

[0049] Inherit the dynamic, friction, and elastic compensation to calculate τu, τfric, and τs, add the three to synthesize the final driving torque τd, and send it to the driver through the inner-loop PD controller for current / torque closed-loop tracking to drive the joint to move synchronously according to the desired acceleration and reference trajectory.

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

[0051] Sensor array module, used to receive the robot information obtained based on the multi-modal sensor array and perform synchronous alignment processing on the robot information;

[0052] Body monitoring module, used to perform health monitoring on the robot body status to obtain a health status vector, and analyze the robot health status in combination with the robot information and the health status vector to execute corresponding safety protection strategies;

[0053] Environmental dynamic modeling module, used to construct an environmental dynamic model, judge the environmental change status according to the robot information, determine whether to reconstruct the environmental dynamic model, and if reconstruction is performed, output the reconstructed trajectory status vector and the risk weight corresponding to the trajectory obstacle based on the reconstructed environmental dynamic model; where the status vector includes the ID, position, velocity, and covariance of the current trajectory;

[0054] Action output module, used to combine the trajectory status 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 the execution is completed, it drives the encoder and torque sensor to feedback the phase coordinates of the robot in the joint space and the actually measured joint torque, calculates the difference between the joint compensation torque and the actually 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.

[0056] Advantages of the present invention:

[0057] By combining the Kalman filter prediction residual with the 3σ anomaly detection, the present invention realizes the online health assessment of the motor temperature, drive current / voltage, joint torque difference, and battery SOC / SOH. Under non-significant anomalies, further health management is carried out according to the health state vector of the robot, and the robots with poor states are downgraded (speed limit and joint function limitation). Under the condition of controllable risks and non-emergency situations, the operation time is extended and maintenance is arranged in advance to balance production efficiency and equipment safety; the flexible response ability of the system to health risks is improved, and the problem that the pre-planned control cannot incorporate its own state into the decision-making is solved;

[0058] By closely connecting the environmental perception (step S3) with the high-level decision-making (step S4), the present invention ensures that each control decision takes into account both the threat level of real-time dynamic obstacles and the advantages of model prediction and learning strategies, realizing the safe and efficient movement of the robot in a mutated environment; compared with pure MPC which is vulnerable to model mismatch and pure RL which is prone to losing safety guarantees, the hybrid scheme retains the advantages of both and achieves double innovation in real-time performance and safety. The decision-making framework integrating uncertainty measurement enhances the adaptive ability to sudden scenarios and overcomes the defect that static strategies are prone to failure in dynamic environments;

[0059] The present invention performs real-time compensation on the joints through dynamic compensation, friction and elasticity compensation, improves the control accuracy and smoothness, observes and feedbacks the execution residual, trajectory deviation, etc., realizes the online correction of the internal and external models, and ensures the robustness of long-term operation; traditional joint control often ignores friction and elasticity, resulting in tracking deviation, and the omnidirectional compensation significantly reduces the tracking error and energy waste; the closed-loop feedback enables the system to continuously adapt during operation and avoids the performance degradation under single calibration;

[0060] In summary, the present invention realizes a full-link closed loop from perception, monitoring, modeling, decision-making to execution, fuses environmental mutation and ontology 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 systems that rely on pre-planned paths, this solution can respond to environmental dynamics in milliseconds, evaluate health risks in real time, and adaptively adjust strategies, thus fundamentally overcoming the problems of insufficient real-time performance and flexibility, and realizing a deep association with the state of the robot itself, greatly improving the movement accuracy and safety guarantee ability in complex and changing scenarios. BRIEF DESCRIPTION OF THE DRAWINGS

[0061] The drawings constituting a part of the present invention are used to provide a further understanding of the present invention. The schematic embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute an improper limitation of the present invention. In the drawings:

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

[0063] Figure 2 is a schematic diagram of the connection of system modules of the present invention. DETAILED DESCRIPTION OF THE EMBODIMENTS

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

[0065] In order to enable those skilled in the art to better understand the solution of the present invention, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention. Obviously, the described embodiments are only a part of the embodiments of the present invention, rather than all the embodiments. Based on the embodiments in the present invention, all other embodiments obtained by those of ordinary skill in the art without creative efforts shall fall within the protection scope of the present invention.

[0066] It should be noted that the terms "first", "second", etc. in the specification and claims of the present invention and the above-mentioned drawings are used to distinguish similar objects and do not have to be used to describe a specific order or sequence. It should be understood that such data can be interchanged under appropriate circumstances so that the embodiments of the present invention described herein. In addition, the terms "comprising" and "having" and any variations thereof are intended to cover non-exclusive inclusion. For example, a process, method, system, product or device comprising a series of steps or units does not have to be limited to those steps or units clearly listed, but may include other steps or units not clearly listed or inherent to these processes, methods, products or devices.

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

[0068] Embodiment 1:

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

[0070] S1, a multi-modal sensor array is used to obtain robot information:

[0071] A multi-modal sensor array is configured on the robot to obtain robot information, where the multi-modal sensors include a lidar (capturing dense geometric point clouds), a binocular event camera (rapidly capturing dynamic obstacles), a flexible tactile skin (realizing near-field collision detection), a millimeter-wave radar (supplementing low-visibility environment perception), etc.; specific robot information includes raw point clouds, event streams, torque matrices, maximum detection distances, speeds, angles, and resolutions, and they are synchronously aligned and processed.

[0072] S2, in-depth perception of the robot body state:

[0073] Health monitoring is performed on the robot body to obtain a health status vector, 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 one of the measured values > 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 index in the health status vector is mapped to a health sub-score in the [0,1] interval, and then they are weighted and fused according to the corresponding importance to calculate a health score, where the motor temperature is mapped to obtain the health sub-score h T , and a detailed description is given as an example: Obtain the upper and lower limits of the robot motor temperature setting, the normal temperature Tnom (usually 60 °C) and the limit temperature Tmax (usually 90 °C), and the current motor temperature Tmot. The mapping formula is Set a health threshold. When the health score is greater than or equal to the health threshold, it indicates that the current robot is in good health and no processing is required. When the health score is less than the health threshold, it indicates that there is a certain health risk, and the degradation mode (the degradation mode is speed limit and joint function limit) is triggered to reduce the risk of fault spread, and an operation and maintenance work order is generated to the background; if the degradation mode is triggered more than 3 times continuously, the robot is shut down and taken offline, and an operation and maintenance work order is generated to the background; it should be noted 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 ±3σ of the mean (μ), and only 0.27% of the data may exceed this range. The data exceeding this range is considered significantly abnormal; the 3σ principle is an abnormal detection threshold widely used in the industrial community. If the measured value exceeds μ±3σ, it is considered that the system has a significant abnormality and immediate intervention is required;

[0074] By combining the Kalman filter prediction residual with 3σ anomaly detection, online health assessment of motor temperature, drive current / voltage, joint torque difference, and battery SOC / SOH is realized. In the case of non-significant anomalies, further health management is carried out based on the health state vector of the robot, and degradation processing (speed limit and joint function limit) is performed on the robots with poor states. Under the condition of controllable risk and non-emergency, the operation time is extended and maintenance is arranged in advance to balance production efficiency and equipment safety; the flexible response ability of the system to health risks is improved, and the problem that pre-planned control cannot incorporate its own state into the decision-making is solved;

[0075] S3, Environmental Dynamic Modeling Engine:

[0076] S3-1, Determine whether the currently perceived dynamic obstacle list is too different from the existing environmental model to decide whether to reconstruct the environmental model. If so, execute S3-2; if not, there is no need to perform re-modeling and update the state vector;

[0077] Calculate the target number change rate between the previous obstacle list and the newly detected obstacle list this time, and set a target change threshold (this threshold is derived from the dynamic environment difficulty assessment. Those skilled in the art believe that if the target number change rate exceeds 20%, it will significantly affect the obstacle avoidance success rate). If the target number change rate > the set target change threshold, it is determined that the environment has changed drastically;

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

[0079] If the target change rate ≤ the set target change threshold and the trajectory displacement ≤ the set displacement threshold, it is determined that the environment is stable, and the previous model can be directly reused without model optimization changes; otherwise, an "entire 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 consecutive frames. Specifically:

[0081] Perform density clustering on the point cloud, set the neighborhood radius (this value is selected according to the LiDAR point distribution and the minimum target size, and the inventor takes its value as 0.5m) and the minimum number of points within the cluster; traverse all points, for each unvisited point p, find all points within its neighborhood radius. If the neighborhood points ≥ the minimum number of points within the cluster, then expand a cluster starting from this point p, and iteratively add all density-reachable points until the cluster no longer grows, thereby generating several cluster sets, and each cluster represents the current observation set of an obstacle;

[0082] Based on the trajectory state (position, velocity, and covariance) at the previous moment, the prior state predicted to the current frame is denoted as where i represents a certain trajectory. Taking the prior state as the center, calculate the association probability βi between all clusters in the current frame and this trajectory j , j represents the index of all clusters, and soft-assign the contribution of the measurement point to this trajectory; update the trajectory state with a unique ID to the posterior after JPDA weighted fusion. If a certain trajectory has no match or a very low association probability for a long time, it is regarded as "trajectory termination"; start "trajectory initialization" for the newly detected cluster; the weighted fusion algorithm is:

[0083]

[0084] where represents the prior (predicted) state estimate of the i-th trajectory at the current moment, that is, the position information and velocity information obtained based on the posterior estimate at 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 j-th clustering cluster, H is the observation matrix, Si is the innovation covariance of the i-th trajectory, representing 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, measuring the uncertainty of the predicted state, which is jointly determined by the process noise of the motion model and the previous posterior covariance; Gate refers to the set of measurements that pass the detection in the validation gate, used to screen out the measurements that do not match the current trajectory prediction; T represents the matrix transpose operation, which flips the matrix along the main diagonal to interchange its rows and columns;

[0085] S3-3. Calculate the risk weights based on the time to collision (TTC) between the robot and each dynamic obstacle, and output a set of ωi ∈ [0, 1] for weighting the importance of different obstacles in the decision-making module. Specifically:

[0086] For each obstacle i, calculate the Euclidean distance di between the robot and each obstacle i, and the relative velocity vrel between the obstacle and the robot. If it is positive, it means the robot and the obstacle are approaching; if it is negative or zero, it means they are moving away or maintaining a distance. Through the formula Calculate the time to collision TTCi between obstacle i and the robot at the current relative velocity, and map the time to collision TTCi to the risk weight ωi. The value range of ωi is [0, 1]. The basic mapping rule is that when the risk weight ωi is closer to 1, it means the time to collision TTCi is smaller, that is, the time for the robot and the obstacle to collide is extremely short, belonging to a high-risk scenario, and immediate and high-priority obstacle avoidance or braking measures need to be taken; otherwise, it means the time to collision TTCi is larger, that is, the obstacle and the robot maintain a relatively large distance or the relative movement does not tend to approach, belonging to a low-risk scenario, and the robot can continue to move along the established trajectory without emergency intervention. It should be noted that if the two continue to approach at the current speed, TTCi represents the expected time required for a collision; if they do not approach (vrel < 0), it is considered that there will never be a collision;

[0087] In the same frame, the robot may face multiple obstacles. Sort all obstacles in descending order according to their corresponding risk weights to ensure that the controller gives priority to the most dangerous obstacles. When the maximum risk weight exceeds the set weight threshold, control the robot to switch to the emergency braking or maximum detour strategy; otherwise, perform local trajectory offset according to the risk weight ratio to avoid multiple low-risk obstacles within a certain tolerance.

[0088] S3-4. Take the trajectory state and its risk weight as the state input and send it to S4 to drive the robot to generate the final motion instruction.

[0089] Clearly determine the sudden change of the scene through the double thresholds of the target number change rate and the trajectory displacement, and achieve a rapid response to sudden crowd flow or obstacle group mutation; use DBSCAN clustering to extract obstacle clusters, and reliably assign IDs, estimate positions / velocities and covariances in multiple frames through EKF and joint probabilistic data association to achieve continuous and consistent obstacle tracking; then map the risk weights based on the time to collision between the obstacle and the robot, provide a priority sorting basis for the decision-making module, and combine the risk threshold to achieve emergency braking or maximum detour for high-risk obstacles and smooth avoidance of low-risk obstacles, taking into account both safety and traffic efficiency.

[0090] S4. Combine the dynamic obstacle status bar with the risk weight to form a decision-making state, and generate the final high-level action through a hybrid algorithm (MPC and SAC); specifically:

[0091] S4-1. Hyperparameter setting:

[0092] Define the way for the agent to perceive the environment and itself, as well as the range of executable actions, to construct the state space and action space in the decision-making process:

[0093] Preliminarily set the hyperparameters of MPC and SAC. The specific hyperparameters of MPC include the update frequency, prediction steps, and sample time; the hyperparameters of SAC include the 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 in this field, no further information will be elaborated here;

[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 splice them with the pose of the robot to form a state vector. At the same time, execute two paths, one of which is the MPC algorithm: at the previous state point Perform a first-order Taylor expansion on the robot's dynamic equation to obtain a linearized model. Given the prediction step and sampling time (usually 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, and take the first control quantity as the MPC candidate action at that moment for subsequent fusion use; where is the current phase coordinate of the robot, and 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 - they jointly constitute the coordinate and velocity components in the PhaseSpace;

[0096] Another one is the SAC algorithm: It receives the state vector as the input of the SAC algorithm to capture the current dynamic association between the robot itself and multiple obstacles. The policy network receives the state vector and outputs the action distribution parameters, which include the mean and logarithmic standard deviation (usually stored in logarithmic form to ensure positive values). This network has two layers, each with 256 fully connected units and uses ReLu activation; it samples a temporary action from the Gaussian distribution and then compresses it to the action range allowed by the policy range through tanh. Finally, the mapped mean is taken as the SAC candidate action, which can not only retain the exploratory nature but also ensure the stability of multiple sampling results; SAC uses the maximum entropy objective and Bellman error to alternately optimize the policy and value network. The experience replay pool usually maintains a size of 5×10 5 and the sampling batch size is 256, updated once per step;

[0097] Based on model integration or MC-Dropout variance, the uncertainty of the two paths is calculated to obtain the uncertainty measure. The candidate actions output by the two paths are combined with the uncertainty measure and weighted by the Logistic function to obtain the final fused action a; it should be noted that the candidate action output by the MPC branch is the joint acceleration, and the candidate action output by the SAC branch is the task space velocity, that is, the Cartesian velocity;

[0098] S4-3, convert the high-level action a generated by the policy into an instruction executable by downstream control, specifically:

[0099] Identify the type of the high-level action a. If it is the joint acceleration, it is directly used as the input of the "joint drive compensation control" in S5; if it is the Cartesian velocity, then within each control cycle, with the end velocity as the constraint, combined with the inverse kinematics and dynamic model of the robot, a smooth joint position and velocity curve is solved, and at the same time, obstacle avoidance and joint limit constraints are added to ensure that the joint trajectory meets the physical limits and safety requirements; thus, a joint reference trajectory can be generated and transmitted to S5;

[0100] By closely connecting environmental perception (step S3) and high-level decision-making (step S4), it is ensured that each control decision takes into account both the threat level of real-time dynamic obstacles and the advantages of model prediction and learning strategies, realizing the safe and efficient movement of the robot in a changing environment; compared with pure MPC that is vulnerable to model mismatch and pure RL that is prone to losing safety guarantees, the hybrid scheme retains the advantages of both and achieves a double innovation in real-time performance and safety. The decision-making framework that integrates the uncertainty measure enhances the adaptive ability to sudden scenarios and overcomes the defect that static policies are prone to failure in dynamic environments.

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

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

[0103] Friction compensation: Inheriting τu obtained from dynamic compensation calculation, simultaneously reading the joint velocity (i.e., the velocity component in the reference trajectory) and the actually measured joint velocity, and calculating the friction torque τfric according to the Coulomb + viscous friction model with τu;

[0104] Elastic joint compensation: Extracting the motor-side angle and load-side angle in the joint reference trajectory, and obtaining the elastic compensation τs through the equivalent torsional spring model;

[0105] Inheriting τu, τfric, and τs obtained from dynamic / friction / elastic compensation calculations, adding the three to synthesize the final driving torque τd, and sending it to the driver through the inner-loop PD controller. The current / torque closed-loop tracks to drive the joint to move synchronously according to the desired acceleration and reference trajectory; after execution, the drive encoder and torque sensor feedback the phase coordinates of the robot in the joint space and the actually measured joint torque (derived from the force / torque sensor or estimated according to the motor current) for the next state estimation and model update to complete the closed-loop feedback control;

[0106] Real-time compensation of the joint is performed through dynamic compensation, friction, and elastic compensation to improve control accuracy and smoothness. The execution residuals, trajectory deviations, etc. are observed and fed back to achieve online calibration of the internal and external models, ensuring the robustness of long-term operation; traditional joint control often ignores friction and elasticity, resulting in tracking deviations. Omnidirectional 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] Embodiment 2:

[0108] As Figure 2 shown, this embodiment also provides a robot motion control and state monitoring device, a robot motion control and state monitoring device for implementing the above-mentioned robot motion control and state monitoring method. The device includes:

[0109] A sensor array module for receiving robot information obtained based on a multi-modal sensor array and performing synchronous alignment processing on the robot information;

[0110] The robot body monitoring module is used to perform health monitoring on the robot body state to obtain a health status vector, and analyze the robot health status by combining the robot information and the health status vector to execute corresponding safety protection strategies;

[0111] The environmental dynamic modeling module is used to build an environmental dynamic model, judge the environmental change state according to the robot information, determine whether to reconstruct the environmental dynamic model, and if reconstruction is carried out, output a reconstructed trajectory status vector and the risk weight corresponding to the trajectory obstacle based on the reconstructed environmental dynamic model; where the status vector includes the ID, position, speed, and covariance of the current trajectory.

[0112] The action output module is used to combine the trajectory status vector and the risk weight into a decision-making state, and generate a 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, and after the execution is completed, drive the encoder and torque sensor to feedback the phase coordinates in the joint space and the actually measured joint torque of the robot, calculate the difference between the joint compensation torque and the actually 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.

[0114] Specifically, the above-mentioned sensor array module, robot body monitoring module, environmental dynamic modeling module, action 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 according to a robot motion control and state monitoring method provided above; the above-mentioned sensor array module, robot body monitoring module, environmental dynamic modeling module, action output module, and joint compensation module can perform operations according to the specific steps given in the robot motion control and state monitoring method.

[0115] It should be noted that it should be understood that the division of each module of the above system is only a division of logical functions. In actual implementation, it can be fully or partially integrated into a physical entity, or physically separated. And these modules can all be implemented in the form of software called by processing elements; they can also all be implemented in the form of hardware; it is also possible that some modules are implemented in the form of software called by processing elements, and some modules are implemented in the form of hardware. For example, the sensor array module can be a separately established processing element, or can be integrated in a certain chip of the above device. In addition, it can also be stored in the memory of the above device in the form of program code, and the function of the above signal processing module is called and executed by a certain processing element of the above device. The implementation of other modules is similar. In addition, all or part of these modules can be integrated together or can be independently implemented. The processing element mentioned here can be an integrated circuit with the ability to process signals. In the implementation process, each step of the above method or each of the above modules can be completed by the integrated logic circuit of the hardware in the processor element or the instruction in the form of software.

[0116] For example, the above modules can be one or more integrated circuits configured to implement the above method, such as: one or more Application Specific Integrated Circuits (ASICs), or, one or more Digital Singnal Processors (DSPs), or, one or more Field Programmable Gate Arrays (FPGAs), etc. Again, when a certain module above is implemented in the form of a processing element scheduling program code, the processing element can be a general-purpose processor, such as a Central Processing Unit (CPU) or other processors that can call program code. Again, these modules can be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0117] The above are only embodiments of the present invention and are not used to limit the present invention. For those skilled in the art, the present invention can have various changes and modifications. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention shall be included within the scope of the claims of the present invention.

Claims

1. A method for robot motion control and status monitoring, characterized in that, It includes the following steps: S1. Receive the robot information obtained based on the implementation of the multi-modal sensor array, and perform synchronous alignment processing on the robot information; S2. Conduct health monitoring on the state of the robot body to obtain a health state vector, and analyze the health state of the robot by combining the robot information and the health state vector to execute corresponding safety protection strategies; S3. Construct an environmental dynamic model, judge the environmental change state according to the robot information, determine whether to reconstruct the environmental dynamic model. If reconstruction is carried out, output the reconstructed trajectory state vector and the risk weight corresponding to the trajectory obstacle based on the reconstructed environmental dynamic model; Among them, the state vector includes the ID, position, speed, 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 the way for the agent to perceive the environment and itself, as well as the range of executable actions, to construct the state space and action space in the decision-making process; S42. Under the given state vector, learn an optimal policy that maps to the action a to minimize the collision risk and track the expected trajectory; S43. Convert the high-level action a generated by the policy into an instruction executable by downstream control; S5. Perform joint-level dynamic compensation control according to the converted executable instruction. After the execution is completed, drive the encoder and torque sensing to feedback the phase coordinates in the joint space and the actually measured joint torque of the robot, and calculate the difference between the joint compensation torque and the actually 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.

2. The method for robot motion control and status monitoring according to claim 1, wherein In step S1, the multi-modal sensor array is a lidar, a binocular event camera, a flexible tactile skin, and a millimeter-wave radar arranged on the robot body; the robot information includes the original point cloud, the event stream, the torque matrix, the maximum detection distance, the speed, the angle, and the resolution.

3. A method for robot motion control and status monitoring according to claim 2, characterized in that, In step S2, conduct health monitoring on the state of the robot body to obtain a health state vector, and analyze the health state of the robot by combining the robot information and the health state vector to execute corresponding safety protection strategies. Specifically as follows: (S21) The health state vector includes the motor temperature, drive current, voltage, joint torque difference, battery SOC and SOH. When the model prediction residual corresponding to the measured value of any item of the health state vector > 3σ, trigger an alarm and directly take the robot offline; otherwise, map each index in the health state vector to a health sub-score in the [0,1] interval, and then perform weighted fusion calculation according to the corresponding importance to obtain the health score; (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 then trigger the degradation mode. The degradation mode is speed limit and joint function limit; if the degradation mode is triggered continuously more than 3 times, the robot will be shut down and taken offline.

4. A robot motion control and state monitoring method according to claim 3, characterized in that, In step S3, an environmental dynamic model is constructed. Based on the robot information, the environmental change state is judged to determine whether to reconstruct the environmental dynamic model. If reconstruction is performed, the state vectors on the reconstructed trajectory and the risk weights corresponding to the obstacles on the trajectory are output based on the reconstructed environmental dynamic model, as follows: (S31) Judge whether the currently perceived dynamic obstacle list is too different from the existing environmental model to decide whether to reconstruct the environmental model. If so, execute step (S32); if not, there is no need to perform re - modeling and the state vector is updated; (S32) Cluster and associate the obstacle point cloud, and extract the ID, position, speed, and covariance of each obstacle from consecutive frames, specifically: Perform density clustering on the point cloud, setting the neighborhood radius and the minimum number of points in a cluster; traverse all points. For each unvisited point p, find all points within its neighborhood radius. If the number of neighborhood points ≥ the minimum number of points in a cluster, start expanding a cluster from this point p and iteratively add all density - reachable points until the cluster no longer grows, thus generating several cluster sets, and each cluster represents the current observation set of an obstacle; Based on the trajectory state at the previous moment, the prior state predicted for the current frame is denoted as where i represents a certain trajectory. With the prior state as the center, the association probability βij between all clusters in the current frame and this trajectory is calculated. j represents the index of all clusters, which is the contribution of the soft-assigned measurement points to this trajectory; Update the trajectory state with a unique ID to the posterior after weighted fusion; The weighted fusion algorithm is as follows: Among them represents the prior state estimate of the i-th trajectory at the current moment, that is, the position information 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, representing the variance of the difference between the predicted measurement and the actual measurement, and Pi - represents the prior covariance matrix corresponding to the i-th trajectory, which measures the uncertainty of the predicted state and is jointly determined by the process noise of the motion model and the previous posterior covariance; Gate refers to the set of measurements that pass the detection in the validation gate, used to screen out the measurements that do not match the current trajectory prediction; T represents the matrix transpose operation, which flips the matrix along the main diagonal to interchange its rows and columns; (S33) Calculate the risk weights 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: For each obstacle i, calculate the Euclidean distance di between the robot and each obstacle i, as well as the relative velocity vrel between the obstacle and the robot. Through the formula Calculate the shortest time to collision TTCi between obstacle i and the robot while maintaining the current relative velocity, and map the shortest time to collision TTCi to a risk weight ωi. The value range of ωi is [0, 1]. The basic mapping rule is that the closer the risk weight ωi is to 1, the smaller the shortest time to collision TTCi is, that is, the time when the robot and the obstacle are about to collide is extremely short, which belongs to a high-risk scenario and requires immediate and high-priority obstacle avoidance or braking measures; on the contrary, it means that the shortest time to collision TTCi is larger, that is, the obstacle and the robot maintain a relatively long distance or the relative motion does not tend to approach, which belongs to a low-risk scenario and can continue to move along the established trajectory without emergency intervention; In the same frame, all the obstacles faced by the robot are sorted in descending order according to their corresponding risk weights, ensuring that the controller gives priority to the most dangerous obstacles. When the maximum risk weight exceeds the set weight threshold, the robot is controlled to switch to the emergency braking or maximum detour strategy; otherwise, local trajectory offset is performed according to the risk weight ratio to avoid multiple low - risk obstacles within a certain tolerance.

5. A method for robot motion control and status monitoring according to claim 4, characterized in that, In step (S31), it is judged whether the currently perceived dynamic obstacle list is too different from the existing environmental model to decide whether to reconstruct the environmental model. The judgment basis is as follows: (S31.1) Calculate the change rate of the number of targets between the previous obstacle list and the newly detected obstacle list this time, and set a target change threshold. If the change rate of the number of targets > the set target change threshold, it is determined that the environment has changed drastically; (S31.2) Calculate the average Euclidean distance of the targets to obtain the trajectory displacement, and set a displacement threshold. If the trajectory displacement > the set displacement threshold, it is determined that the environment has changed drastically; (S31.3) If the change rate of the number of targets ≤ the set target change threshold and the trajectory displacement ≤ the set displacement threshold, it is determined that the environment is stable and there is no need to optimize and change the model; otherwise, a model reconstruction instruction is generated.

6. A method for robot motion control and state monitoring according to claim 5, characterized in that, In step S42, under the given state vector, an optimal strategy mapping to action a is learned. The specific process is as follows: (S42.1) Concatenate the ID, position, speed, state covariance, and risk weight of each obstacle with the pose of the robot to form a state vector, and simultaneously execute two path steps (62) and step (63); (S42.2) Execute the MPC algorithm to output the MPC candidate action at this moment; [[ID= (S42.4) Calculate the uncertainty of the two paths to obtain an uncertainty metric, and combine the candidate actions output by the two paths with the uncertainty metric and weight them using a Logistic function to obtain the final fused action a.

7. A robot motion control and state monitoring method according to claim 6, characterized in that, In step (S42.2), execute the MPC algorithm and output the MPC candidate action at this moment. The specific execution process is as follows: One of the MPC algorithms: Perform a first-order Taylor expansion on the robot dynamics equation at the previous state point to obtain a linearized model. Given the prediction step and sampling time, solve the constrained quadratic programming at each control moment, and then use an efficient solver to obtain the optimal input sequence, and take the first control quantity as the MPC candidate action at this moment for subsequent fusion use; In step (S42.3), execute the SAC algorithm and output the SAC candidate action at this moment. The specific execution process is as follows: The other is the SAC algorithm: Receive the state vector as the input of the SAC algorithm to capture the current dynamic association between the robot itself and multiple obstacles. Use the policy network to receive the state vector and output the action distribution parameters, where the distribution parameters include the mean and log standard deviation; Sample a temporary action from the Gaussian distribution, and then compress it to the action range allowed by the policy range. Finally, take the mapped mean as the SAC candidate action.

8. A method for robot motion control and status monitoring according to claim 7, characterized in that In the step S43, convert the high-level action a generated by the policy into an instruction executable by downstream control, specifically as follows: Identify the type of the high-level action a. If it is joint acceleration, it is directly used as the input of the joint drive compensation control; if it is Cartesian velocity, in each control cycle, with the end velocity as the constraint, combine the inverse kinematics and dynamic model of the robot, solve to obtain a smooth joint position and velocity curve, and at the same time add obstacle avoidance and joint limit constraints to ensure that the joint trajectory meets the physical limits and safety requirements, thereby generating a joint reference trajectory and transmitting it downstream for joint dynamic compensation control.

9. A method for robot motion control and status monitoring according to claim 8, characterized in that The joint-level dynamic compensation control process in S5 is as follows: Dynamic compensation: Use the robot dynamics equation to internally call the cylinder dynamics library to calculate various matrices and vectors, and obtain the pure dynamic compensation torque τu through the robot dynamics equation with the joint acceleration vector and the current joint position and velocity; Friction compensation: According to the τu calculated by the dynamic compensation, read the joint velocity and the actually measured joint velocity at the same time, and calculate the friction torque τfric according to the Coulomb + viscous friction model with τu; Elastic joint compensation: Extract the motor-side angle and load-side angle from the joint reference trajectory, and obtain the elastic compensation τs through the equivalent torsional spring model; Inherit the τu, τfric, and τs calculated by the dynamic, friction, and elastic compensations, add the three to synthesize the final driving torque τd, and send it to the driver through the inner-loop PD controller for current / torque closed-loop tracking to drive the joint to move synchronously according to the desired acceleration and reference trajectory.

10. A robot motion control and status monitoring device for implementing the robot motion control and status monitoring method according to any one of claims 1-9, characterized in that, The device includes: A sensor array module for receiving the robot information obtained based on the multi-modal sensor array and performing synchronous alignment processing on the robot information; The body monitoring module is used to perform health monitoring on the state of the robot body to obtain a health status vector, and analyze the health status of the robot by combining the robot information and the health status vector to execute corresponding safety protection strategies; The environmental dynamic modeling module is used to construct an environmental dynamic model, judge the environmental change state according to the robot information, determine whether to reconstruct the environmental dynamic model, and if reconstruction is performed, output a reconstructed trajectory status vector and the risk weight corresponding to the trajectory obstacle based on the reconstructed environmental dynamic model; where the status vector includes the ID, position, speed, and covariance of the current trajectory; The action output module is used to combine the trajectory status vector and the risk weight into a decision-making state, and generate a 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, and after the execution is completed, drive the encoder and torque sensing to feedback the phase coordinates of the robot in the joint space and the actually measured joint torque, calculate the difference between the joint compensation torque and the actually 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.

Images