A method and system for controlling movement of a robot

Abstract

This invention discloses a robot motion control method and system, comprising the following steps: acquiring a set of robot body structural parameters and establishing a coordinate mapping relationship between the robot joint space and the base coordinate system; collecting and preprocessing real-time operating data of each joint of the robot; determining the collision trigger time based on joint mutation characteristics and generating a collision event identifier; constructing a collision analysis time window and generating a multi-joint dynamic response coupled representation; constructing a set of kinematic constraints and a set of dynamic constraints; constructing a set of collision force propagation constraint equations and solving them by inversion using the Jacobian matrix pseudo-inverse method and damped least squares method; performing localization analysis on the candidate collision position set to generate a confidence set and determining the target collision position; and generating a set of robot motion control commands based on the collision position determination results. This invention employs a multi-joint coupled inversion method to achieve accurate collision localization and adaptive control, possessing the advantages of high localization accuracy and strong operational safety.

Description

Technical Field

[0001] This invention relates to the field of robot control technology, and in particular to a method and system for controlling the movement of a robot. Background Technology

[0002] With the widespread application of industrial robots in intelligent manufacturing, automated assembly, and human-robot collaboration scenarios, robots inevitably encounter external contact or accidental collisions in high-speed operation or complex working environments. Existing motion control methods typically perform collision detection using a single current threshold, torque threshold, or acceleration threshold, or rely on external force sensors to achieve collision perception and localization. In terms of collision localization, simplified dynamic models are often used for coarse back-calculation, followed by direct triggering of deceleration or emergency stop control strategies.

[0003] The above methods have obvious shortcomings: single signal judgment is easily affected by noise interference, leading to misjudgment or omission; there is a lack of coupling analysis mechanism between dynamic responses of multiple joints, making it difficult to accurately reflect the force propagation characteristics; the simplified model does not fully combine kinematic constraints and dynamic constraints for joint inversion, resulting in insufficient collision location accuracy; the control strategies are mostly passive stopping or fixed deceleration, and do not generate fine-grained movement control commands based on the positioning results, making it difficult to balance safety and motion continuity.

[0004] Therefore, how to provide a method and system for controlling the movement of a robot is a problem that urgently needs to be solved by those skilled in the art. Summary of the Invention

[0005] One objective of this invention is to propose a robot motion control method and system. This invention employs a multi-joint coupling inversion method to achieve precise collision localization and adaptive control, possessing the advantages of high positioning accuracy and strong operational safety.

[0006] A robot movement control method according to an embodiment of the present invention includes the following steps: Obtain the set of robot body structural parameters and establish the coordinate mapping relationship between the robot joint space and the robot base coordinate system; Real-time operating data of each joint of the robot are collected and preprocessed during the robot's motion control cycle to obtain a set of dynamic response sequences of multiple joints; In the set of multi-joint dynamic response sequences, a collision triggering condition set is constructed based on joint mutation features. When the collision triggering condition set is met, the collision triggering time is determined and a collision event identifier is generated. A collision analysis time window is constructed with the collision triggering moment as the center. Within the collision analysis time window, multi-joint response features are extracted from the set of multi-joint dynamic response sequences, and a coupled representation of multi-joint dynamic response is constructed. A set of kinematic constraints is constructed based on a set of robot body structural parameters, and a set of dynamic constraints is constructed based on a set of multi-joint dynamic response sequences. Within the collision analysis time window corresponding to the collision event identifier, a set of collision force propagation constraint equations is constructed based on the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set. The Jacobian matrix pseudo-inverse method and damped least squares method are used to invert and solve the set of collision force propagation constraint equations to obtain the candidate collision position set. Based on the multi-joint dynamic response coupling representation, a localization analysis is performed on the candidate collision location set to generate a confidence set and determine the target collision location, outputting the collision location determination result; The collision location determination result is used as the motion control input to generate a set of robot motion control commands.

[0007] Optionally, the set of robot body structural parameters includes a set of link parameters, a set of joint types, a set of joint numbers, and end effector installation parameters.

[0008] Optionally, the real-time operating data includes joint angle sequence, joint angular velocity sequence, joint angular acceleration sequence, driving current sequence, and joint output torque sequence, and the preprocessing includes timestamp alignment, missing data processing, outlier removal, and dimension normalization.

[0009] Optionally, determining the collision trigger time specifically includes: Within the robot's motion control cycle, set corresponding abrupt change threshold parameters for the drive current sequence, joint output torque sequence, and joint angular acceleration sequence, respectively. Abrupt changes are detected in the drive current sequence of each joint, and an abnormal drive current identification sequence is generated based on a preset drive current abrupt change threshold. For each joint output torque sequence, torque transition discrimination processing is performed, and a torque anomaly identification sequence is formed based on a preset joint output torque mutation threshold; An acceleration mutation identification operation is performed on the joint angle acceleration sequence of each joint, and an acceleration anomaly identification sequence is output based on a preset joint angle acceleration mutation threshold; A collision triggering condition set is constructed based on the abnormal drive current identifier sequence, abnormal torque identifier sequence, and abnormal acceleration identifier sequence. When the collision trigger condition set is met, the collision trigger time is determined to be the earliest sampling time that satisfies the collision trigger condition set, and a collision event identifier corresponding to the collision trigger time is generated.

[0010] Optionally, the construction of the multi-joint dynamic response coupling representation specifically includes: The start and end timestamps of the collision analysis time window are determined with the timestamp corresponding to the collision triggering time as the center, and the multi-joint sub-sequence set within the collision analysis time window is extracted from the multi-joint dynamic response sequence set according to the start and end timestamps. Based on the preset reference sampling segment before the collision triggering moment and the preset response sampling segment after the collision triggering moment, extract the torque change characteristics, current change characteristics and acceleration change characteristics of each joint. For each joint, the characteristics of the peak response time are determined in the torque subsequence, current subsequence, and acceleration subsequence. Based on the characteristics of torque variation, current variation, acceleration variation, and peak response time of each joint, a correlation matrix of joint response is constructed. Based on the peak response time characteristics of each joint, an inter-joint response delay matrix is ​​constructed, and based on the torque change characteristics, current change characteristics, and acceleration change characteristics of each joint, an inter-joint response intensity distribution vector is constructed.

[0011] The inter-joint response correlation matrix, inter-joint response delay matrix, and inter-joint response intensity distribution vector are fused to obtain a coupled representation of the dynamic response of multiple joints.

[0012] Optionally, the construction of the kinematic constraint set and the dynamic constraint set specifically includes: Read the link parameter set, joint type set, joint number set, and end effector installation parameters from the robot body structure parameter set, and determine the pose expression relationship of each link in the robot base coordinate system based on the link parameter set and end effector installation parameters to generate link pose constraints; The set of limit parameters for each joint is determined based on the set of joint types and the set of joint numbers, and the set of limit parameters is written into the joint limit constraints. Read the set of dynamic response sequences of multiple joints, establish joint inertia constraints based on joint angular acceleration sequences and joint output torque sequences, establish joint friction constraints based on joint angular velocity sequences, and establish drive transmission ratio constraints based on drive current sequences; The linkage pose constraints and joint limit constraints are combined to form a set of kinematic constraints, and the joint inertia constraints, joint friction constraints, and drive transmission ratio constraints are combined to form a set of dynamic constraints.

[0013] Optionally, obtaining the set of candidate collision locations specifically includes: Within the collision analysis time window corresponding to the collision event identifier, the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set are obtained. Based on the multi-joint dynamic response coupling representation, the joint weights participating in the inversion solution are determined, and the joint weights are used as the input for constructing the weighting matrix. The mapping relationship between the external force at the contact point and the external torque at each joint is determined based on the set of robot body structural parameters, and a mapping matrix is ​​generated. Within the collision analysis time window, read the joint output torque sequence and driving current sequence and determine the joint external torque vector; The weighted mapping matrix is ​​obtained by performing weighted processing on the mapping matrix based on the weighted matrix. The collision force propagation constraint equation set is constructed using the weighted mapping matrix, the joint external torque vector, the kinematic constraint set and the dynamic constraint set. The initial solution for the external force inversion is obtained by applying the Jacobian matrix pseudo-inverse method to the weighted mapping matrix; In the collision force propagation constraint equations, the residual minimization optimization iteration is performed on the initial solution of the external force inversion solution under the damped constraint condition based on the damped least squares method to obtain the external force inversion solution; The candidate link number set is obtained by jointly solving the external force inversion solution, the kinematic constraint set and the dynamic constraint set, and the candidate spatial coordinate set is determined by the candidate link number set. The candidate collision position set is obtained by combining the candidate link number set with the candidate spatial coordinate set.

[0014] Optionally, the output of the collision location determination result specifically includes: Obtain the candidate collision location set, multi-joint dynamic response coupling representation, joint response delay matrix, joint response intensity distribution vector, and joint response correlation matrix, and establish an association index between each candidate collision location in the candidate collision location set and the corresponding joint number; Based on the inter-joint response delay matrix, a timing consistency check is performed on the candidate collision position set to generate a timing consistency determination identifier. Spatial weight allocation is performed on the candidate collision location set based on the inter-joint response intensity distribution vector, and the spatial weight values ​​are written into the spatial weight field corresponding to the candidate collision location. Based on the inter-joint response correlation matrix, a coupling consistency determination is performed on the candidate collision location set, and a coupling consistency determination identifier is generated. A confidence set for generating the candidate collision location set is generated based on the temporal consistency determination identifier, spatial weight value, and coupling consistency determination identifier. Based on the confidence set, the candidate collision position with the highest location confidence is selected from the candidate collision position set as the target collision position, and the collision position determination result is output.

[0015] Optionally, the generation of the robot movement control command set specifically includes: The collision location determination result is obtained and the target link number, target spatial coordinates and positioning confidence are parsed. The target link number is then indexed and matched with the link parameter set in the robot body structure parameter set to determine the link pose expression relationship corresponding to the target link number. Based on the relationship between the target spatial coordinates and the link pose, the spatial offset of the target spatial coordinates relative to the robot base coordinate system is determined. Based on the spatial offset, the speed adjustment amount of the speed control command and the acceleration adjustment amount of the acceleration control command are determined. The speed adjustment amount is written into the speed control command, and the acceleration adjustment amount is written into the acceleration control command. The path correction amount of the path correction command is determined based on the target spatial coordinates, the target link number, and the path point sequence of the preset path, and the path correction amount is written into the path correction command. The speed control command, acceleration control command, and path correction command are combined to generate a set of robot movement control commands.

[0016] A robot mobility control system according to an embodiment of the present invention includes: The structural modeling module is used to obtain the set of robot body structural parameters and establish the coordinate mapping relationship between the robot joint space and the robot base coordinate system; The data acquisition module is used to collect real-time operating data of each joint of the robot during the robot's movement control cycle and preprocess it to obtain a set of dynamic response sequences of multiple joints. The trigger determination module is used to construct a collision trigger determination condition set based on joint mutation features in a multi-joint dynamic response sequence set. When the collision trigger determination condition set is met, the collision trigger time is determined and a collision event identifier is generated. The coupling construction module is used to construct a collision analysis time window centered on the collision trigger moment, extract multi-joint response features from the multi-joint dynamic response sequence set within the collision analysis time window, and construct a coupled representation of the multi-joint dynamic response. The constraint construction module is used to construct a set of kinematic constraints based on a set of robot body structural parameters and a set of dynamic constraints based on a set of multi-joint dynamic response sequences. The inversion solution module is used to construct a set of collision force propagation constraint equations based on the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set within the collision analysis time window corresponding to the collision event identifier. The Jacobian matrix pseudo-inverse method and damped least squares method are used to invert and solve the set of collision force propagation constraint equations to obtain a set of candidate collision positions. The localization analysis module is used to perform localization analysis on the candidate collision location set based on the multi-joint dynamic response coupling representation, generate a confidence set, determine the target collision location, and output the collision location determination result. The control generation module is used to generate a set of robot movement control commands by taking the collision position determination results as the motion control input.

[0017] The beneficial effects of this invention are: This invention constructs a set of multi-joint dynamic response sequences and generates a coupled representation of the multi-joint dynamic response within the collision analysis time window. It integrates features such as joint output torque, current variation, acceleration variation, and peak time offset to form a structured coupled expression that reflects the force propagation path and temporal relationship. Based on this, it constructs a set of collision force propagation constraint equations by combining kinematic and dynamic constraint sets, and solves them using the Jacobian matrix pseudo-inverse method and damped least squares method. This ensures that the collision location solution process is simultaneously constrained by both structural parameters and dynamic characteristics, significantly improving the physical consistency and spatial accuracy of the candidate collision location set and reducing the impact of noise disturbances on the positioning results.

[0018] During the localization phase, this application performs temporal consistency verification, spatial weight allocation, and coupling consistency determination on the candidate collision location set based on the inter-joint response delay matrix, inter-joint response intensity distribution vector, and inter-joint response correlation matrix, generating a confidence set and determining the target collision location accordingly. This multi-dimensional consistency analysis mechanism ensures that the collision localization results not only meet the mechanical inversion conditions but also the statistical consistency requirements between the dynamic responses of multiple joints, improving the reliability and stability of the target collision location and avoiding the mislocalization problem caused by relying solely on a single inversion result in traditional methods.

[0019] In the control phase, this application directly uses the collision location determination result as the motion control input to generate a set of robot motion control commands, including velocity control commands, acceleration control commands, and path correction commands. This tightly links the control strategy with the target link number, target spatial coordinates, and positioning confidence. By coupling the spatial offset and positioning confidence calculations, the control amplitude is matched with the collision severity. Simultaneously, normal direction correction is performed based on the preset path, ensuring the robot maintains motion continuity and path reachability while guaranteeing safety. This achieves a closed-loop motion control mechanism from collision detection and precise positioning to adaptive control generation, improving the overall system safety, positioning accuracy, and operational stability. Attached Figure Description

[0020] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used in conjunction with embodiments of the invention to explain the invention and do not constitute a limitation thereof. In the drawings: Figure 1 This is a flowchart of a robot movement control method proposed in this invention; Figure 2 This is a flowchart illustrating the collision triggering process of a robot movement control method proposed in this invention. Figure 3 This is a flowchart illustrating the inversion solution of a robot movement control method proposed in this invention. Detailed Implementation

[0021] The present invention will now be described in further detail with reference to the accompanying drawings. These drawings are simplified schematic diagrams, illustrating only the basic structure of the invention, and therefore only show the components relevant to the invention.

[0022] refer to Figures 1-3 A method for controlling the movement of a robot includes the following steps: Obtain the set of robot body structural parameters and establish the coordinate mapping relationship between the robot joint space and the robot base coordinate system; Real-time operating data of each joint of the robot are collected and preprocessed during the robot's motion control cycle to obtain a set of dynamic response sequences of multiple joints; In the set of multi-joint dynamic response sequences, a collision triggering condition set is constructed based on joint mutation features. When the collision triggering condition set is met, the collision triggering time is determined and a collision event identifier is generated. A collision analysis time window is constructed with the collision triggering moment as the center. Within the collision analysis time window, multi-joint response features are extracted from the set of multi-joint dynamic response sequences, and a coupled representation of multi-joint dynamic response is constructed. A set of kinematic constraints is constructed based on a set of robot body structural parameters, and a set of dynamic constraints is constructed based on a set of multi-joint dynamic response sequences. Within the collision analysis time window corresponding to the collision event identifier, a set of collision force propagation constraint equations is constructed based on the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set. The Jacobian matrix pseudo-inverse method and damped least squares method are used to invert and solve the set of collision force propagation constraint equations to obtain the candidate collision position set. Based on the multi-joint dynamic response coupling representation, a localization analysis is performed on the candidate collision location set to generate a confidence set and determine the target collision location, outputting the collision location determination result; The collision location determination result is used as the motion control input to generate a set of robot motion control commands.

[0023] In this embodiment, the robot body structural parameter set includes a link parameter set, a joint type set, a joint number set, and end effector installation parameters. The establishment of coordinate mapping relationships includes: establishing a link coordinate system for each link based on the link parameter set according to preset link parameter calibration rules; determining the direction of motion degrees of freedom and motion axis direction for each joint based on the joint type set; determining the topological connection order between links according to the joint number set; constructing a link transformation sequence according to the connection order from the robot base to the end effector; constructing coordinate transformation relationships between adjacent links based on the geometric parameters of each link and the displacement variables of each joint; and recursively combining the coordinate transformation relationships at each level according to the link transformation sequence to obtain a spatial mapping relationship from the robot base coordinate system to any link coordinate system; and performing pose correction on the end effector coordinate system based on the end effector installation parameters to obtain a spatial mapping relationship from the robot base coordinate system to the end effector coordinate system. The spatial mapping relationship is used to transform the spatial position coordinates in any link coordinate system to the robot base coordinate system.

[0024] In this embodiment, the real-time running data includes joint angle sequence, joint angular velocity sequence, joint angular acceleration sequence, driving current sequence, and joint output torque sequence. The preprocessing includes timestamp alignment, missing data processing, outlier removal, and dimension normalization.

[0025] In this embodiment, determining the collision trigger time specifically includes: Within the robot's motion control cycle, set corresponding abrupt change threshold parameters for the drive current sequence, joint output torque sequence, and joint angular acceleration sequence, respectively. Abrupt change detection is performed on the drive current sequence of each joint, and an abnormal drive current identification sequence is generated based on a preset drive current abrupt change threshold. The abrupt change detection is as follows: the difference between the drive current value at the current sampling time and the drive current value at the previous sampling time is used to obtain the change in drive current. The absolute value of the change in drive current is taken and compared with the preset drive current abrupt change threshold. When the absolute value is greater than or equal to the drive current abrupt change threshold, the corresponding sampling time is marked as an abnormal drive current identification. For each joint output torque sequence, torque transition discrimination processing is performed, and a torque anomaly identification sequence is formed based on a preset joint output torque mutation threshold. The torque transition discrimination processing is as follows: the torque increment is obtained by subtracting the joint output torque value of the previous sampling time from the joint output torque value at the current sampling time. The absolute value of the torque increment is taken and compared with the preset joint output torque mutation threshold. When the absolute value is greater than the joint output torque mutation threshold, the corresponding sampling time is marked as a torque anomaly. An acceleration mutation identification operation is performed on the joint angle acceleration sequence of each joint, and an acceleration anomaly identification sequence is output based on a preset joint angle acceleration mutation threshold. The acceleration mutation identification operation is as follows: the difference in joint angle acceleration between two consecutive sampling times is calculated as the acceleration change amplitude, and the absolute value of the acceleration change amplitude is compared with the preset joint angle acceleration mutation threshold. When the absolute value exceeds the joint angle acceleration mutation threshold, the corresponding sampling time is marked as an acceleration anomaly. A collision triggering condition set is constructed based on the abnormal drive current identifier sequence, abnormal torque identifier sequence, and abnormal acceleration identifier sequence. The construction of the collision triggering condition set is as follows: the abnormal drive current identifier sequence, abnormal torque identifier sequence, and abnormal acceleration identifier sequence are aligned by time index. When the abnormal drive current identifier and abnormal torque identifier of the same joint at the same time index position are triggered at the same time, and the abnormal acceleration identifier of the corresponding joint has been triggered within a preset time span before the time index, the collision triggering condition is determined to be met. When the collision trigger condition set is met, the collision trigger time is determined to be the earliest sampling time that satisfies the collision trigger condition set, and a collision event identifier corresponding to the collision trigger time is generated.

[0026] In this embodiment, the construction of the multi-joint dynamic response coupling representation specifically includes: The start and end timestamps of the collision analysis time window are determined with the timestamp corresponding to the collision trigger moment as the center. The multi-joint sub-sequence set within the collision analysis time window is extracted from the multi-joint dynamic response sequence set according to the start and end timestamps. The multi-joint sub-sequence set includes the joint output torque sub-sequence, driving current sub-sequence, and joint angular acceleration sub-sequence within the collision analysis time window. Based on the preset reference sampling segment before the collision trigger moment and the preset response sampling segment after the collision trigger moment, the torque change characteristics, current change characteristics, and acceleration change characteristics of each joint are extracted. The torque change characteristics include using the average torque value of the preset reference sampling segment before the collision trigger moment as the reference torque value, and using the peak torque value of the preset response sampling segment after the collision trigger moment as the response torque value. The difference between the response torque value and the reference torque value is used to obtain the torque change, and the torque change is associated with the corresponding joint number to form the torque change characteristics of each joint. The current change characteristics include using the average current value of the preset reference sampling segment before the collision trigger moment. The reference current value is used as the reference current value, and the peak current value of the preset response sampling segment after the collision triggering time is used as the response current value. The difference between the response current value and the reference current value is used to obtain the current change, and the current change is associated with the corresponding joint number to form the current change feature of each joint. The acceleration change feature includes using the average acceleration value of the preset reference sampling segment before the collision triggering time as the reference acceleration value, and the peak acceleration value of the preset response sampling segment after the collision triggering time as the response acceleration value. The difference between the response acceleration value and the reference acceleration value is used to obtain the acceleration change, and the acceleration change is associated with the corresponding joint number to form the acceleration change feature of each joint. For each joint, the response peak moment features are determined in the torque subsequence, current subsequence, and acceleration subsequence. The response peak moment features include taking the collision trigger moment as the zero reference moment, locating the sampling point with the largest absolute amplitude in each subsequence and determining the timestamp of the sampling point, determining the time difference between the timestamp and the zero reference moment as the peak moment offset, and associating the peak moment offset with the corresponding joint number to form the response peak moment features of each joint. Based on the characteristics of torque change, current change, acceleration change, and peak response time of each joint, a joint response correlation matrix is ​​constructed. The construction of the joint response correlation matrix includes aligning the corresponding change characteristics and peak response time characteristics of any two joints with the same length, calculating the synchronization consistency of the two joints within the time window, where the synchronization consistency is the normalized result obtained by summing the pointwise products of the feature sequences of the two joints after removing the mean and dividing by the square root of the sum of the squares of the amplitudes of the feature sequences of the two joints, and writing the normalized result into the corresponding joint pair matrix position. Based on the peak response time characteristics of each joint, an inter-joint response delay matrix is ​​constructed. Based on the torque change characteristics, current change characteristics, and acceleration change characteristics of each joint, an inter-joint response intensity distribution vector is constructed. The construction of the inter-joint response delay matrix includes taking the difference of the peak time offset of the corresponding subsequence of any two joints as the delay value and writing the delay value into the corresponding joint pair matrix position. The construction of the inter-joint response intensity distribution vector includes weighting the torque change, current change, and acceleration change of each joint according to preset weights to obtain the joint response intensity value. The weighted summation is obtained by multiplying the torque change by the torque weight, the current change by the current weight, and the acceleration change by the acceleration weight, and then summing them to obtain the intensity composite value. The intensity composite values ​​of each joint are arranged in order of joint number to form the inter-joint response intensity distribution vector.

[0027] The inter-joint response correlation matrix, inter-joint response delay matrix, and inter-joint response intensity distribution vector are fused to obtain a coupled representation of the dynamic response of multiple joints.

[0028] In this embodiment, the construction of the kinematic constraint set and the dynamic constraint set specifically includes: Read the link parameter set, joint type set, joint number set, and end effector installation parameters from the robot body structure parameter set, and determine the pose expression relationship of each link in the robot base coordinate system based on the link parameter set and end effector installation parameters to generate link pose constraints; The limit parameter set of each joint is determined based on the set of joint types and the set of joint numbers, and the limit parameter set is written into the joint limit constraint. The joint limit constraint is used to limit the joint angle sequence within the robot's motion control cycle to meet the upper and lower limits of the corresponding joint angle. The system reads a set of dynamic response sequences from multiple joints, establishes joint inertia constraints based on joint angular acceleration sequences and joint output torque sequences, joint friction constraints based on joint angular velocity sequences, and drive transmission ratio constraints based on drive current sequences. The joint inertia constraint includes using the product of the joint inertia parameter and the joint angular acceleration value as the inertia torque component, and associating the inertia torque component with the corresponding joint number. The joint friction constraint includes using the product of the friction coefficient and the joint angular velocity direction identifier as the Coulomb friction component, the product of the viscous friction coefficient and the joint angular velocity value as the viscous friction component, and the algebraic sum of the Coulomb friction component and the viscous friction component as the friction torque component, and associating the friction torque component with the corresponding joint number. The drive transmission ratio constraint includes obtaining the motor output torque by multiplying the drive current value and the current torque coefficient, obtaining the estimated joint output torque by multiplying the motor output torque and the drive transmission ratio parameter, and associating the estimated joint output torque with the corresponding joint number. The linkage pose constraints and joint limit constraints are combined to form a set of kinematic constraints, and the joint inertia constraints, joint friction constraints, and drive transmission ratio constraints are combined to form a set of dynamic constraints.

[0029] In this embodiment, obtaining the candidate collision location set specifically includes: Within the collision analysis time window corresponding to the collision event identifier, the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set are obtained. Based on the multi-joint dynamic response coupling representation, the joint weights participating in the inversion solution are determined, and the joint weights are used as the input for constructing the weighting matrix. The weighting matrix is ​​obtained by: reading the inter-joint response intensity distribution vector, inter-joint response correlation matrix, and inter-joint response delay matrix from the multi-joint dynamic response coupling representation; extracting the corresponding response intensity value for each joint; dividing the response intensity value of each joint by the sum of the response intensity values ​​of all joints to obtain the joint weight; constructing a diagonal matrix based on the joint weights, writing the joint weights of each joint in the order of joint number on the main diagonal of the matrix, and setting the values ​​of the off-diagonal positions to zero to obtain the initial weighting matrix; and performing consistency correction processing on the initial weighting matrix, including correlation scaling of the main diagonal weight values ​​based on the inter-joint response correlation matrix and temporal decay correction of the weight values ​​based on the inter-joint response delay matrix to obtain the weighting matrix. The mapping relationship between the external force at the contact point and the external torque at each joint is determined based on the set of robot body structural parameters, and a mapping matrix is ​​generated. Within the collision analysis time window, read the joint output torque sequence and driving current sequence and determine the joint external torque vector; The determination of the joint external torque vector includes: aligning the joint output torque sequence and the drive current sequence with time indices within the collision analysis time window; reading the joint output torque value and drive current value for each joint at the same sampling time; obtaining the motor output torque value based on the product of the drive current value and the current torque coefficient, and obtaining the estimated joint output torque value calculated from the drive current by multiplying the motor output torque value by the drive transmission ratio parameter; calculating the difference between the actual joint output torque value in the joint output torque sequence and the estimated joint output torque value calculated from the drive current to obtain the external torque component; and arranging the external torque components of each joint at the corresponding sampling time within the collision analysis time window according to the joint number to form the joint external torque vector. The weighted mapping matrix is ​​obtained by performing weighted processing on the mapping matrix based on the weighted matrix. The collision force propagation constraint equation set is constructed using the weighted mapping matrix, the joint external torque vector, the kinematic constraint set and the dynamic constraint set. The initial solution of the external force inversion solution is obtained by performing the Jacobian matrix pseudo-inverse method on the weighted mapping matrix. The Jacobian matrix pseudo-inverse method includes transposing the weighted mapping matrix to obtain the transpose matrix, multiplying the weighted mapping matrix with the transpose matrix to obtain a square matrix, performing the inversion operation on the square matrix to obtain the inverse matrix, multiplying the inverse matrix with the transpose matrix to obtain the pseudo-inverse matrix, and multiplying the pseudo-inverse matrix with the joint external moment vector to obtain the initial solution of the external force inversion solution. In the collision force propagation constraint equations, the damped least squares method is used to perform residual minimization optimization iteration on the initial solution of the external force inversion solution under damping constraints to obtain the external force inversion solution. The damped least squares method includes introducing a damping term into the square matrix to obtain a damping square matrix, performing an inversion operation on the damping square matrix to obtain the damping inverse matrix, multiplying the damping inverse matrix with the transpose matrix to obtain the damping pseudo-inverse matrix, multiplying the damping pseudo-inverse matrix with the joint external moment vector to obtain the external force inversion solution, and substituting the external force inversion solution into the collision force propagation constraint equations to calculate the residual value, and updating the external force inversion solution based on the residual value. The candidate link number set is obtained by jointly solving the external force inversion solution, the kinematic constraint set and the dynamic constraint set, and the candidate spatial coordinate set is determined by the candidate link number set. The joint solution includes: using the external force inversion solution as the contact point external force input, substituting the external force inversion solution into the link pose expression relationship determined by the robot body structural parameter set, calculating the equivalent force distribution of each link in the robot base coordinate system, and judging the pose reachability of each link based on the kinematic constraint set, and selecting link numbers that satisfy the link pose constraints and joint limit constraints to form a candidate link number set; verifying the consistency between the external force inversion solution and the joint inertia constraints, joint friction constraints, and drive transmission ratio constraints in the dynamic constraint set, calculating the equivalent joint torque generated at each link by the external force inversion solution, and comparing the residuals of the equivalent joint torque with the joint output torque sequence within the collision analysis time window, retaining link numbers whose residual values ​​meet the preset threshold conditions; for each link in the candidate link number set, solving the contact point spatial coordinates in the local coordinate system of the corresponding link, and transforming them to the robot base coordinate system through the coordinate mapping relationship in the robot body structural parameter set to obtain a candidate spatial coordinate set; The candidate collision position set is obtained by combining the candidate link number set with the candidate spatial coordinate set.

[0030] In this embodiment, the output of the collision location determination result specifically includes: Obtain the candidate collision location set, multi-joint dynamic response coupling representation, joint response delay matrix, joint response intensity distribution vector, and joint response correlation matrix, and establish an association index between each candidate collision location in the candidate collision location set and the corresponding joint number; Based on the inter-joint response delay matrix, a timing consistency check is performed on the candidate collision position set to generate a timing consistency judgment identifier. The timing consistency check includes determining the joint corresponding to the candidate collision position based on the association index, calculating the difference of the peak time offset between the joint corresponding to the candidate collision position and the other joints, and comparing the difference of the peak time offset with a preset timing consistency threshold. The preset timing consistency threshold is determined based on the length of each link and the range of joint response propagation time in the robot body structural parameter set. Based on the inter-joint response intensity distribution vector, spatial weight allocation processing is performed on the candidate collision position set, and the spatial weight value is written into the spatial weight field corresponding to the candidate collision position. The spatial weight allocation processing includes determining the joint corresponding to the candidate collision position based on the association index, reading the joint response intensity value of the joint corresponding to the candidate collision position, and dividing the joint response intensity value by the sum of all joint response intensity values ​​to obtain the spatial weight value. Based on the inter-joint response correlation matrix, a coupling consistency determination is performed on the candidate collision position set, and a coupling consistency determination identifier is generated. The coupling consistency determination includes determining the joint corresponding to the candidate collision position based on the association index, reading the correlation value between the joint corresponding to the candidate collision position and the other joints, and comparing the correlation value with a preset coupling consistency threshold. The preset coupling consistency threshold is determined based on the statistical distribution of the inter-joint response correlation matrix in the multi-joint dynamic response coupling representation. A confidence set for the candidate collision location set is generated based on the temporal consistency determination identifier, spatial weight value, and coupling consistency determination identifier. The generation of the confidence set includes calculating the location confidence for each candidate collision location. The location confidence is the weighted sum of the temporal consistency determination identifier value, spatial weight value, and coupling consistency determination identifier value. Based on the confidence set, the candidate collision position with the highest positioning confidence is selected from the candidate collision position set as the target collision position, and the collision position determination result is output. The collision position determination result includes the target link number, the target spatial coordinates, and the positioning confidence corresponding to the target link number and the target spatial coordinates.

[0031] In this embodiment, the generation of the robot movement control command set specifically includes: The collision location determination result is obtained and the target link number, target spatial coordinates and positioning confidence are parsed. The target link number is then indexed and matched with the link parameter set in the robot body structure parameter set to determine the link pose expression relationship corresponding to the target link number. Based on the relationship between the target spatial coordinates and the link pose, the spatial offset of the target spatial coordinates relative to the robot base coordinate system is determined. Based on the spatial offset, the speed adjustment amount of the speed control command and the acceleration adjustment amount of the acceleration control command are determined. The speed adjustment amount is written into the speed control command, and the acceleration adjustment amount is written into the acceleration control command. The speed adjustment amount is the product of the basic speed adjustment amount calculated based on the spatial offset and the positional confidence level; the acceleration adjustment amount is the product of the basic acceleration adjustment amount calculated based on the spatial offset and the positional confidence level. The path correction amount of the path correction command is determined based on the target spatial coordinates, the target link number, and the path point sequence of the preset path. The path correction amount is written into the path correction command. The path correction amount is the path correction vector obtained by locating the path point closest to the target spatial coordinates in the path point sequence, using the direction of the line connecting the target spatial coordinates to the path point as the correction direction, and using the normal offset as the correction distance. The normal offset is obtained by projecting the spatial offset along the normal direction of the preset path. The normal direction of the preset path is determined by the direction of the line connecting adjacent path points in the path point sequence, and the perpendicular direction of the path tangential direction in the robot base coordinate system is used as the normal direction. The speed control command, acceleration control command, and path correction command are combined to generate a set of robot movement control commands.

[0032] A mobile control system for a robot includes: The structural modeling module is used to obtain the set of robot body structural parameters and establish the coordinate mapping relationship between the robot joint space and the robot base coordinate system; The data acquisition module is used to collect real-time operating data of each joint of the robot during the robot's movement control cycle and preprocess it to obtain a set of dynamic response sequences of multiple joints. The trigger determination module is used to construct a collision trigger determination condition set based on joint mutation features in a multi-joint dynamic response sequence set. When the collision trigger determination condition set is met, the collision trigger time is determined and a collision event identifier is generated. The coupling construction module is used to construct a collision analysis time window centered on the collision trigger moment, extract multi-joint response features from the multi-joint dynamic response sequence set within the collision analysis time window, and construct a coupled representation of the multi-joint dynamic response. The constraint construction module is used to construct a set of kinematic constraints based on a set of robot body structural parameters and a set of dynamic constraints based on a set of multi-joint dynamic response sequences. The inversion solution module is used to construct a set of collision force propagation constraint equations based on the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set within the collision analysis time window corresponding to the collision event identifier. The Jacobian matrix pseudo-inverse method and damped least squares method are used to invert and solve the set of collision force propagation constraint equations to obtain a set of candidate collision positions. The localization analysis module is used to perform localization analysis on the candidate collision location set based on the multi-joint dynamic response coupling representation, generate a confidence set, determine the target collision location, and output the collision location determination result. The control generation module is used to generate a set of robot movement control commands by taking the collision position determination results as the motion control input.

[0033] Example 1: To verify the feasibility of this invention in practice, it was applied to a six-degree-of-freedom industrial robot production line in an automotive parts manufacturing workshop. Located in East China, this production line performs welding and assembly operations. During continuous operation, the robot needs to complete high-speed reciprocating motion within a confined space, frequently interacting with conveying devices, tooling fixtures, and manual operation areas. Previously, during operation, when the robotic arm experienced minor collisions or contact interference, the system relied on a single current threshold for detection. This was easily affected by load fluctuations and environmental interference, leading to false alarms or failure to promptly identify actual collisions. Furthermore, after triggering such collisions, the system typically initiated an emergency stop, causing fluctuations in the production cycle.

[0034] In this scenario, the present invention continuously collects real-time operational data of each joint of the robot and constructs a set of multi-joint dynamic response sequences. After identifying joint mutation characteristics, it automatically generates collision event identifiers and constructs a collision analysis time window centered on the collision trigger moment. Within the time window, multi-joint response features are extracted to generate a coupled representation of multi-joint dynamic responses. Simultaneously, a set of kinematic constraints and a set of dynamic constraints are constructed by combining the robot's body structural parameters. By constructing a set of collision force propagation constraint equations, external force inversion is performed to obtain a set of candidate collision positions. Subsequently, a confidence set is generated based on the inter-joint response delay matrix, response intensity distribution vector, and response correlation matrix to determine the target collision position. The collision position determination result is used as the motion control input to generate a set of robot motion control commands, enabling the robot to coordinate and adjust speed control commands, acceleration control commands, and path correction commands while maintaining a safe distance.

[0035] During continuous operation of the production line, multiple contact events caused by tooling misalignment occur at the actual assembly station. By deploying the motion control method of this invention, the system can promptly identify collision trigger moments within the robot's motion control cycle and quickly solve for the candidate collision position set and determine the target collision position. Compared with the original single-threshold detection method, this invention maintains stable recognition performance under complex load conditions, reduces the number of false triggers, and minimizes cycle time loss due to sudden stops while maintaining production cycle continuity. No additional external force sensors are added during application; positioning and control generation can be completed based on existing operational data, verifying the feasibility and applicability of this invention in real-world industrial scenarios.

[0036] Table 1. Performance Comparison of the Invention and Traditional Robot Motion Control Methods

[0037] As can be clearly seen from Table 1, the method of the present invention is superior to the traditional method in many indicators.

[0038] In terms of collision recognition response time, the traditional single-threshold method takes 23.4ms, while the method of this invention takes 19.6ms, a reduction of 3.8ms and an improvement of approximately 16.2%. The improvement in response time comes from the simultaneous construction of a multi-joint dynamic response coupling representation within the collision analysis time window, and the parallel computation combining the kinematic constraint set and the dynamic constraint set, reducing the delay caused by serial judgment of single signals.

[0039] Regarding the average collision positioning error, the traditional method has an error of 8.7 mm, while this invention has an error of 6.2 mm, a reduction of 2.5 mm, representing an improvement of approximately 28.7%. The error reduction is mainly due to solving the external force inversion solution through the collision force propagation constraint equations and optimizing the residuals under the damped least squares method constraint, making the candidate collision position set more consistent with structural parameters and dynamic characteristics, thereby improving the stability of spatial positioning.

[0040] The false alarm rate decreased from 6.1% to 4.3%; the false negative rate decreased from 4.5% to 3.1%. These two indicators decreased by 1.8 and 1.4 percentage points, respectively. The improvement is attributed to the introduction of consistency verification using inter-joint response delay and correlation matrices during the localization phase. This generates a confidence set before determining the target collision location, reducing false alarms caused by single abrupt changes in signals.

[0041] The number of emergency stop triggers was reduced from 28 times / 1000h to 22 times / 1000h; the average recovery time was shortened from 5.2s to 4.1s. Because this invention generates speed control commands and path correction commands based on positional confidence, achieving flexible control to replace some emergency stop operations, both the number of stops and the recovery time are moderately improved.

[0042] The fluctuation range of production cycle time decreased from 4.8% to 3.6%; the percentage of stable system operation time increased from 93.2% to 95.0%. This improvement stems from the closed-loop collaborative mechanism generated by collision detection, localization, and control, which enables the robot to maintain a high continuous operation rate while ensuring safety.

[0043] The above are merely preferred embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any equivalent substitutions or modifications made by those skilled in the art within the scope of the technology disclosed in the present invention, based on the technical solution and inventive concept of the present invention, should be covered within the scope of protection of the present invention.

Claims

1. A method for controlling the movement of a robot, characterized in that, Includes the following steps: Obtain the set of robot body structural parameters and establish the coordinate mapping relationship between the robot joint space and the robot base coordinate system; Real-time operating data of each joint of the robot are collected and preprocessed during the robot's motion control cycle to obtain a set of dynamic response sequences of multiple joints; In the set of multi-joint dynamic response sequences, a collision triggering condition set is constructed based on joint mutation features. When the collision triggering condition set is met, the collision triggering time is determined and a collision event identifier is generated. A collision analysis time window is constructed with the collision triggering moment as the center. Within the collision analysis time window, multi-joint response features are extracted from the set of multi-joint dynamic response sequences, and a coupled representation of multi-joint dynamic response is constructed. A set of kinematic constraints is constructed based on a set of robot body structural parameters, and a set of dynamic constraints is constructed based on a set of multi-joint dynamic response sequences. Within the collision analysis time window corresponding to the collision event identifier, a set of collision force propagation constraint equations is constructed based on the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set. The Jacobian matrix pseudo-inverse method and damped least squares method are used to invert and solve the set of collision force propagation constraint equations to obtain the candidate collision position set. Based on the multi-joint dynamic response coupling representation, a localization analysis is performed on the candidate collision location set to generate a confidence set and determine the target collision location, outputting the collision location determination result; The collision location determination result is used as the motion control input to generate a set of robot motion control commands.

2. The robot movement control method according to claim 1, characterized in that, The set of robot body structural parameters includes a set of link parameters, a set of joint types, a set of joint numbers, and end effector installation parameters.

3. The robot movement control method according to claim 1, characterized in that, The real-time operating data includes joint angle sequence, joint angular velocity sequence, joint angular acceleration sequence, driving current sequence, and joint output torque sequence. The preprocessing includes timestamp alignment, missing data processing, outlier removal, and dimension normalization.

4. The robot movement control method according to claim 1, characterized in that, The determination of the collision trigger time specifically includes: Within the robot's motion control cycle, set corresponding abrupt change threshold parameters for the drive current sequence, joint output torque sequence, and joint angular acceleration sequence, respectively. Abrupt changes are detected in the drive current sequence of each joint, and an abnormal drive current identification sequence is generated based on a preset drive current abrupt change threshold. For each joint output torque sequence, torque transition discrimination processing is performed, and a torque anomaly identification sequence is formed based on a preset joint output torque mutation threshold; An acceleration mutation identification operation is performed on the joint angle acceleration sequence of each joint, and an acceleration anomaly identification sequence is output based on a preset joint angle acceleration mutation threshold; A collision triggering condition set is constructed based on the abnormal drive current identifier sequence, abnormal torque identifier sequence, and abnormal acceleration identifier sequence. When the collision trigger condition set is met, the collision trigger time is determined to be the earliest sampling time that satisfies the collision trigger condition set, and a collision event identifier corresponding to the collision trigger time is generated.

5. The robot movement control method according to claim 1, characterized in that, The construction of the multi-joint dynamic response coupling representation specifically includes: The start and end timestamps of the collision analysis time window are determined with the timestamp corresponding to the collision triggering time as the center, and the multi-joint sub-sequence set within the collision analysis time window is extracted from the multi-joint dynamic response sequence set according to the start and end timestamps. Based on the preset reference sampling segment before the collision triggering moment and the preset response sampling segment after the collision triggering moment, extract the torque change characteristics, current change characteristics and acceleration change characteristics of each joint. For each joint, the characteristics of the peak response time are determined in the torque subsequence, current subsequence, and acceleration subsequence. Based on the characteristics of torque variation, current variation, acceleration variation, and peak response time of each joint, a correlation matrix of joint response is constructed. Based on the peak response time characteristics of each joint, an inter-joint response delay matrix is ​​constructed, and an inter-joint response intensity distribution vector is constructed based on the torque change characteristics, current change characteristics, and acceleration change characteristics of each joint. The inter-joint response correlation matrix, inter-joint response delay matrix, and inter-joint response intensity distribution vector are fused to obtain a coupled representation of the dynamic response of multiple joints.

6. The robot movement control method according to claim 1, characterized in that, The construction of the kinematic constraint set and the dynamic constraint set specifically includes: Read the link parameter set, joint type set, joint number set, and end effector installation parameters from the robot body structure parameter set, and determine the pose expression relationship of each link in the robot base coordinate system based on the link parameter set and end effector installation parameters to generate link pose constraints; The set of limit parameters for each joint is determined based on the set of joint types and the set of joint numbers, and the set of limit parameters is written into the joint limit constraints. Read the set of dynamic response sequences of multiple joints, establish joint inertia constraints based on joint angular acceleration sequences and joint output torque sequences, establish joint friction constraints based on joint angular velocity sequences, and establish drive transmission ratio constraints based on drive current sequences; The linkage pose constraints and joint limit constraints are combined to form a set of kinematic constraints, and the joint inertia constraints, joint friction constraints, and drive transmission ratio constraints are combined to form a set of dynamic constraints.

7. The robot movement control method according to claim 1, characterized in that, The specific steps to obtain the candidate collision location set include: Within the collision analysis time window corresponding to the collision event identifier, the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set are obtained. Based on the multi-joint dynamic response coupling representation, the joint weights participating in the inversion solution are determined, and the joint weights are used as the input for constructing the weighting matrix. The mapping relationship between the external force at the contact point and the external torque at each joint is determined based on the set of robot body structural parameters, and a mapping matrix is ​​generated. Within the collision analysis time window, read the joint output torque sequence and driving current sequence, and determine the joint external torque vector; The weighted mapping matrix is ​​obtained by performing weighted processing on the mapping matrix based on the weighted matrix. The collision force propagation constraint equation set is constructed using the weighted mapping matrix, the joint external torque vector, the kinematic constraint set and the dynamic constraint set. The initial solution for the external force inversion is obtained by applying the Jacobian matrix pseudo-inverse method to the weighted mapping matrix; In the collision force propagation constraint equations, the residual minimization optimization iteration is performed on the initial solution of the external force inversion solution under the damped constraint condition based on the damped least squares method to obtain the external force inversion solution; The candidate link number set is obtained by jointly solving the external force inversion solution, the kinematic constraint set and the dynamic constraint set, and the candidate spatial coordinate set is determined by the candidate link number set. The candidate collision position set is obtained by combining the candidate link number set with the candidate spatial coordinate set.

8. The robot movement control method according to claim 1, characterized in that, The output of the collision location determination result specifically includes: Obtain the candidate collision location set, multi-joint dynamic response coupling representation, joint response delay matrix, joint response intensity distribution vector, and joint response correlation matrix, and establish an association index between each candidate collision location in the candidate collision location set and the corresponding joint number; Based on the inter-joint response delay matrix, a timing consistency check is performed on the candidate collision position set to generate a timing consistency determination identifier. Spatial weight allocation is performed on the candidate collision location set based on the inter-joint response intensity distribution vector, and the spatial weight values ​​are written into the spatial weight field corresponding to the candidate collision location. Based on the inter-joint response correlation matrix, a coupling consistency determination is performed on the candidate collision location set, and a coupling consistency determination identifier is generated. A confidence set for generating the candidate collision location set is generated based on the temporal consistency determination identifier, spatial weight value, and coupling consistency determination identifier. Based on the confidence set, the candidate collision position with the highest location confidence is selected from the candidate collision position set as the target collision position, and the collision position determination result is output.

9. The robot movement control method according to claim 1, characterized in that, The generation of the robot movement control command set specifically includes: The collision location determination result is obtained and the target link number, target spatial coordinates and positioning confidence are parsed. The target link number is then indexed and matched with the link parameter set in the robot body structure parameter set to determine the link pose expression relationship corresponding to the target link number. Based on the relationship between the target spatial coordinates and the link pose, the spatial offset of the target spatial coordinates relative to the robot base coordinate system is determined. Based on the spatial offset, the speed adjustment amount of the speed control command and the acceleration adjustment amount of the acceleration control command are determined. The speed adjustment amount is written into the speed control command, and the acceleration adjustment amount is written into the acceleration control command. The path correction amount of the path correction command is determined based on the target spatial coordinates, the target link number, and the path point sequence of the preset path, and the path correction amount is written into the path correction command. The speed control command, acceleration control command, and path correction command are combined to generate a set of robot movement control commands.

10. A robot motion control system, executing the robot motion control method according to any one of claims 1 to 9, characterized in that, include: The structural modeling module is used to obtain the set of robot body structural parameters and establish the coordinate mapping relationship between the robot joint space and the robot base coordinate system; The data acquisition module is used to collect real-time operating data of each joint of the robot during the robot's movement control cycle and preprocess it to obtain a set of dynamic response sequences of multiple joints. The trigger determination module is used to construct a collision trigger determination condition set based on joint mutation features in a multi-joint dynamic response sequence set. When the collision trigger determination condition set is met, the collision trigger time is determined and a collision event identifier is generated. The coupling construction module is used to construct a collision analysis time window centered on the collision trigger moment, extract multi-joint response features from the multi-joint dynamic response sequence set within the collision analysis time window, and construct a coupled representation of the multi-joint dynamic response. The constraint construction module is used to construct a set of kinematic constraints based on a set of robot body structural parameters and a set of dynamic constraints based on a set of multi-joint dynamic response sequences. The inversion solution module is used to construct a set of collision force propagation constraint equations based on the multi-joint dynamic response coupling representation, kinematic constraint set, and dynamic constraint set within the collision analysis time window corresponding to the collision event identifier. The Jacobian matrix pseudo-inverse method and damped least squares method are used to invert and solve the set of collision force propagation constraint equations to obtain a set of candidate collision positions. The localization analysis module is used to perform localization analysis on the candidate collision location set based on the multi-joint dynamic response coupling representation, generate a confidence set, determine the target collision location, and output the collision location determination result. The control generation module is used to generate a set of robot movement control commands by taking the collision position determination results as the motion control input.

Images