Joint cooperative control method, device and medium based on CAN bus

Abstract

The application discloses a joint cooperative control method and device based on a CAN bus, and a medium, relates to the technical field of industrial robots, and comprises the following steps: collecting the positions, speeds, torques and communication states of all joints, and binding the trajectory progress with the original reference trajectory; identifying single joint communication abnormalities, recording the trajectory progress before the abnormalities and fault handling modes, and controlling the remaining joints to slow down and keep; verifying node configuration and clearing old instructions after communication recovery, determining the motion state of the joint to be accessed according to continuous feedback; according to the communication interruption time, the fault handling mode and the joint motion limit, recursively deriving the position and speed reachable interval, screening the candidate trajectory progress, determining the estimated trajectory progress according to the normalized state distance and the allowed separation ratio; generating a seventh-degree polynomial target re-entry trajectory according to the estimated trajectory progress, checking the motion, torque, end pose and collision constraint, and resuming cooperative control after execution. The application realizes reliable trajectory progress matching and smooth and safe re-entry of multiple joints.

Description

Technical Field

[0001] This invention relates to the field of industrial robot technology, and in particular to a joint collaborative control method, device and medium based on CAN bus. Background Technology

[0002] Industrial robot multi-joint cooperative control typically involves the robot controller periodically communicating with the servo drivers of each joint via CAN or CANopen bus to uniformly issue position, speed, and control words, and to collect joint position, speed, torque, and status information. The control system performs synchronous interpolation, status monitoring, and motion constraint verification for each joint based on a preset reference trajectory. When a node experiences a communication failure, the abnormal node is typically stopped, the remaining joints are decelerated and held in place, and the node is reinitialized after communication is restored. Multi-joint cooperative operation is then restored through trajectory replanning or interpolation.

[0003] Conventional methods have two main problems: First, during communication interruption, abnormal joints may be affected by handling methods such as holding, rapid stopping, or disabling. It is difficult to accurately determine their corresponding progress in the original reference trajectory based solely on the position of a single frame after recovery or the last valid command. Furthermore, confusion may occur between different motion stages at the same position in the reciprocating trajectory. Second, directly connecting to the original trajectory or using simple interpolation after recovery makes it difficult to balance the continuity of motion state with torque stability. Summary of the Invention

[0004] In view of the problems existing in the prior art, the present invention is proposed.

[0005] Therefore, this invention provides a joint cooperative control method based on CAN bus, which solves the problems of inaccurate matching of trajectory progress after communication recovery and insufficient continuity of motion state and torque during reconnection in the prior art.

[0006] To solve the above-mentioned technical problems, the present invention provides the following technical solution: In a first aspect, the present invention provides a joint cooperative control method based on a CAN bus, comprising: acquiring the position, velocity, torque, and communication status of each joint of a robot, and binding them to the trajectory progress of the original reference trajectory; identifying communication anomalies in a single joint, recording the trajectory progress, position, velocity, and fault handling method before the anomaly, and controlling the remaining joints to decelerate and maintain this state; verifying node configuration and clearing old instructions after communication is restored, and determining the motion state of the joint to be connected based on continuous feedback; recursively calculating the position and velocity reachable ranges based on the communication interruption time, fault handling method, and joint motion limitations, and filtering for velocity reachable ranges and consistent motion directions in the original reference trajectory. The candidate trajectory progress is calculated, and the normalized state distance between the position, velocity, and acceleration corresponding to each candidate trajectory progress and the actual motion state of the joint to be connected after communication is restored is calculated. The candidate trajectory progress with the smallest normalized state distance and the ratio of the second smallest distance to the smallest distance reaches the allowable separation ratio is determined as the estimated trajectory progress. The target access trajectory progress and target access time are determined based on the estimated trajectory progress. A seventh-order polynomial target re-access trajectory is generated based on the actual state, and the motion, torque, end pose, and collision constraints are verified. Each joint is controlled to execute the target re-access trajectory, and cooperative control is restored after the changes in position, velocity, and torque meet the access conditions.

[0007] As a preferred embodiment of the CAN bus-based joint cooperative control method of the present invention, the binding of the trajectory progress with the original reference trajectory includes: acquiring the position, velocity, torque and communication status of each joint of the robot; determining the trajectory progress of the original reference trajectory corresponding to the acquisition of the status data of each joint; and associating the position, velocity, torque and communication status with the corresponding trajectory progress according to the correspondence between the joint and the trajectory progress to form the running status record of each joint.

[0008] As a preferred embodiment of the CAN bus-based joint collaborative control method of the present invention, the step of identifying a single joint communication anomaly, recording the trajectory progress, position, speed and fault handling method before the anomaly, and controlling the remaining joints to decelerate and maintain the state includes: determining the single joint with communication anomaly based on the communication status of each joint, and identifying the single joint with communication anomaly as the joint to be connected; extracting the trajectory progress, position and speed corresponding to the joint to be connected before the communication anomaly, and recording the fault handling method adopted for the communication anomaly; and sending deceleration control commands to the remaining joints to decelerate and enter the holding state.

[0009] As a preferred embodiment of the CAN bus-based joint collaborative control method of the present invention, the step of determining the motion state of the joint to be connected based on continuous feedback includes: reading the node configuration of the joint to be connected after communication is restored, and verifying the node configuration; clearing old instructions sent to the joint to be connected before the communication failure but not yet completed; continuously acquiring the position feedback and velocity feedback of the joint to be connected; and determining the actual position, actual velocity, actual acceleration, and motion direction of the joint to be connected based on the continuously acquired position feedback and velocity feedback, thereby forming the actual motion state.

[0010] As a preferred embodiment of the CAN bus-based joint cooperative control method of the present invention, the step of recursively estimating the reachable range of position and speed based on communication interruption time, fault handling method, and joint movement constraints includes: using the position and speed recorded before the communication anomaly as the initial state for recursion of the reachable range, and determining the recursion time domain based on the communication interruption time; determining the state evolution mode of the joint to be connected during the communication interruption based on the fault handling method; under joint movement constraints, sequentially recursively estimating the reachable range of position and speed of the joint to be connected along the recursion time domain; and determining the reachable range of position and speed corresponding to the communication recovery time as the recovery reachable range of the joint to be connected.

[0011] As a preferred embodiment of the CAN bus-based joint cooperative control method of the present invention, the determination of the estimated trajectory progress includes: extracting trajectory progress whose corresponding speed is within the speed reachable range from the original reference trajectory; filtering out trajectory progress whose motion direction is inconsistent with the actual motion direction of the joint to be connected from the extracted trajectory progress to obtain candidate trajectory progress; obtaining the position, velocity, and acceleration corresponding to each candidate trajectory progress, and calculating the normalized state distance between it and the actual motion state; arranging each candidate trajectory progress in ascending order of normalized state distance, and calculating the ratio of the second smallest distance to the smallest distance; when the ratio of the second smallest distance to the smallest distance reaches the allowable separation ratio, determining the candidate trajectory progress with the smallest normalized state distance as the estimated trajectory progress.

[0012] As a preferred embodiment of the CAN bus-based joint cooperative control method of the present invention, the step of determining the target access trajectory progress and target access time based on the estimated trajectory progress, generating a seventh-order polynomial target re-access trajectory based on the actual state, and verifying motion, torque, end-effector pose, and collision constraints includes: determining the target access trajectory progress based on the estimated trajectory progress and the trajectory state located after the estimated trajectory progress in the original reference trajectory; determining the target access time based on the actual motion state, the trajectory state corresponding to the target access trajectory progress, and joint motion constraints; solving a seventh-order polynomial using the actual motion state as the starting boundary, the trajectory state corresponding to the target access trajectory progress as the ending boundary, and the target access time as the trajectory time domain to generate the target re-access trajectory; sequentially verifying the target re-access trajectory for motion constraints, torque constraints, end-effector pose constraints, and collision constraints, and determining the target re-access trajectory that passes all constraint verifications as an executable target re-access trajectory.

[0013] As a preferred embodiment of the CAN bus-based joint cooperative control method of the present invention, the step of controlling each joint to execute the target re-entry trajectory and restoring cooperative control after the changes in position, velocity, and torque meet the access conditions includes: controlling the joint to be accessed and the other joints to cooperatively execute the executable target re-entry trajectory; acquiring the position, velocity, and torque of each joint during execution, and determining the changes in position, velocity, and torque of each joint; determining whether the changes in position, velocity, and torque meet the access conditions; and when the access conditions are met, switching the control trajectory of each joint to the original reference trajectory and restoring the cooperative control of each joint of the robot.

[0014] In a second aspect, the present invention provides a computer device including a memory and a processor, wherein the memory stores a computer program, wherein when the computer program is executed by the processor, it implements any step of the CAN bus-based joint coordination control method described in the first aspect of the present invention.

[0015] Thirdly, the present invention provides a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements any step of the CAN bus-based joint cooperative control method as described in the first aspect of the present invention.

[0016] The beneficial effects of this invention are as follows: by recursively estimating the reachable range of joint motion and filtering candidate trajectory progress, the trajectory matching range after communication recovery is accurately narrowed; by determining the estimated trajectory progress through normalized state distance and candidate result separation ratio, the different motion stages in the reciprocating trajectory are accurately distinguished; by generating a seventh-order polynomial re-access trajectory that satisfies the boundary conditions of multi-order motion states, the continuous and smooth transition of joint motion states is achieved; and by carrying out multiple constraint verification and closed-loop monitoring of access states, the safe and stable recovery of multi-joint collaborative control is achieved. Attached Figure Description

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

[0018] Figure 1 This is a flowchart of a joint coordination control method based on a CAN bus.

[0019] Figure 2 A flowchart for identifying communication anomalies in a single joint and maintaining coordinated deceleration among the remaining normal joints.

[0020] Figure 3 Flowchart for determining the motion state and estimated trajectory progress of the joint to be connected.

[0021] Figure 4 The flowchart shows the process of generating, verifying, and coordinating control to restore the target reconnection trajectory.

[0022] Figure 5 This is a schematic diagram of a joint coordination control experimental platform.

[0023] Figure 6 This is a schematic diagram of a single-joint communication anomaly recovery experiment.

[0024] Figure 7 This is a comparison chart of joint synchronization errors. Detailed Implementation

[0025] To make the above-mentioned objects, features and advantages of the present invention more apparent and understandable, the specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings.

[0026] Many specific details are set forth in the following description in order to provide a full understanding of the invention. However, the invention may also be practiced in other ways different from those described herein, and those skilled in the art can make similar extensions without departing from the spirit of the invention. Therefore, the invention is not limited to the specific embodiments disclosed below.

[0027] Secondly, the term "one embodiment" or "embodiment" as used herein refers to a specific feature, structure, or characteristic that may be included in at least one implementation of the present invention. The phrase "in one embodiment" appearing in different places in this specification does not necessarily refer to the same embodiment, nor is it a single or selective embodiment that is mutually exclusive with other embodiments.

[0028] Reference Figures 1-7 This is one embodiment of the present invention, which provides a joint cooperative control method based on a CAN bus, including the following steps: S1. Collect the position, speed, torque and communication status of each joint of the robot, and bind them to the trajectory progress of the original reference trajectory.

[0029] It should be noted that this embodiment can be applied to six-axis articulated robots, or other robots, robotic arms, and mechanical structures with joint structures.

[0030] Taking a six-axis articulated robot as an example, a six-axis articulated robot includes a robot controller, six joint servo drives, encoders respectively set on the six joints, and a CAN bus connecting the robot controller and the joint servo drives.

[0031] The robot controller uses the CANopen protocol to communicate with the servo drivers of each joint. Each joint servo driver operates in a periodic synchronous position mode. The robot controller reads the actual position, actual speed, actual torque and status word of each joint through TPDO, and sends control words and target positions to each joint through RPDO.

[0032] In this embodiment, for example, the CAN bus communication rate is set to 1 Mbit / s, the robot control cycle is set to 5 ms, the SYNC message sending cycle is the same as the robot control cycle, the Heartbeat production cycle is set to 50 ms, and the Heartbeat consumption time is set to 150 ms.

[0033] In this embodiment, the robot controller establishes CANopen communication with the six joint servo drives, configuring NMT, Heartbeat, SYNC, TPDO, and RPDO for each joint. Specifically, the status word, actual position, actual velocity, and actual torque of each joint are mapped to the TPDO, and the control word and target position are mapped to the RPDO. When the joint servo drive supports velocity feedforward, the target velocity is also mapped to the RPDO; otherwise, no mapping is performed.

[0034] The robot controller reads the mechanical position limits, maximum permissible speed, maximum permissible acceleration, maximum permissible jerk, rated torque, and maximum permissible torque of each joint, and loads the robot's forward kinematics model, dynamics model, link collision model, and working environment collision model.

[0035] The robot controller uses the pre-planned joint space trajectory as the original reference trajectory. The first, second, and third derivatives of the original reference trajectory with respect to the trajectory progress were calculated, respectively. For trajectory progress The reference position vectors for each joint, in radians. The value range is [0,1].

[0036] Let the trajectory progress velocity, trajectory progress acceleration, and trajectory progress jerk of the original reference trajectory be respectively... , , .in, The trajectory progress speed, in seconds; This represents the trajectory acceleration, expressed in seconds squared. Accelerate the trajectory progress, in cubic seconds.

[0037] Based on the first derivative of the original reference trajectory with respect to the trajectory progress and the trajectory progress velocity, the reference angular velocity of the joint at the trajectory progress is obtained, expressed as: ; in, Number the joints; For the first The reference angular velocity of each joint is expressed in radians per second. For the first The first derivative of the joint reference position with respect to the trajectory progress, in radians.

[0038] Based on the second derivative of the original reference trajectory with respect to the trajectory progress, the trajectory progress velocity, and the trajectory progress acceleration, the reference angular acceleration of the joint is obtained, expressed as: ; in, For the first Reference angular acceleration of each joint, in radians per second squared; For the first The second derivative of the joint reference position with respect to the trajectory progress, in radians.

[0039] Based on the third derivative of the original reference trajectory with respect to the trajectory progress, the trajectory progress velocity, the trajectory progress acceleration, and the trajectory progress jerk, the reference angular jerk of the joint is obtained, expressed as: ; in, For the first Reference angular jerk of each joint, in radians per cubic second; For the first The third derivative of the joint reference position with respect to the trajectory progress, in radians.

[0040] The robot controller collects and caches real-time data in each control cycle. The real-time data includes the local control cycle number, local timestamp, current trajectory progress, current trajectory progress speed, actual position of each joint, actual speed of each joint, actual torque of each joint, status word of each joint, NMT status of each joint, and a marker indicating whether the TPDO of each joint is new data for the current control cycle.

[0041] Furthermore, the robot controller uses a circular buffer to maintain real-time data. In this embodiment, for example, the storage time of the circular buffer is set to 10 seconds, which is greater than the sum of the maximum allowed communication interruption time and the maximum access time.

[0042] S2. Identify communication anomalies in a single joint, record the trajectory progress, position, speed, and fault handling method before the anomaly, and control the remaining joints to decelerate and maintain the speed.

[0043] The robot controller determines the communication status of each joint in each control cycle.

[0044] Specifically, the current joint is identified as an abnormal joint when any joint meets at least one of the abnormal joint identification conditions A1-A6 below: A1: The Heartbeat message has not arrived even after the Heartbeat consumption time has expired.

[0045] A2: No new TPDO was received for this joint for several consecutive control cycles.

[0046] A3: Received an EMCY message related to communication failure, synchronization loss, or node reset.

[0047] A4: The CAN node has entered an error passive state or Bus-off state.

[0048] A5: The joint status word indicates that the servo driver has experienced a communication failure, rapid stop, or reset.

[0049] A6: Feedback data shows initialization value jumps, timestamp interruptions, or continuous repetition of old values.

[0050] In this embodiment, for example, if no new TPDO is received for four consecutive control cycles, the robot controller identifies a communication abnormality in the corresponding joint, and the corresponding detection time is 20ms.

[0051] It should be noted that the number of consecutive invalid feedback cycles is determined based on the robot control cycle and the allowable fault detection time, and is usually taken as 2 to 5 control cycles.

[0052] Furthermore, the robot controller distinguishes between communication faults and non-communication faults based on EMCY error codes and joint status words; for non-communication faults such as overcurrent, overtemperature, encoder damage, and mechanical collisions, the robot is controlled to shut down safely and not enter the subsequent automatic access process.

[0053] When only the first one is detected within the same detection time window When a recoverable communication anomaly occurs at a joint, the communication anomaly data is recorded. The communication anomaly data includes the anomaly joint number, the last valid feedback time, the last valid trajectory progress, the last valid trajectory progress speed, the last valid joint position, the last valid joint angular velocity, and the fault handling method executed by the servo driver during the communication anomaly.

[0054] The fault handling methods include position holding, rapid stop, drive disabling, or holding the last control command.

[0055] Specifically, the robot controller freezes RPDO commands that have not yet been sent to the abnormal joint to prevent the old target positions queued before the communication interruption from being sent after communication is restored; the abnormal joint is handled by the communication watchdog of the servo driver, which executes the configured fault handling mode.

[0056] Meanwhile, the robot controller controls the remaining normal joints to decelerate in coordination along the original reference trajectory.

[0057] Furthermore, instead of modifying the position targets of each normal joint individually, the robot controller reduces the progress speed of the common trajectory, causing the normal joints to gradually decelerate along the original reference trajectory until they maintain the trajectory progress. This helps maintain the normal kinematic relationship between joints.

[0058] After the coordinated deceleration is completed, the first A normal joint should meet the following retention criteria: ; ; ; in, For the first The actual holding position of a normal joint, in radians; For the first The reference position of a normal joint at the point of maintaining trajectory progress, in radians; The trajectory progress maintained after normal joint propulsion stops is dimensionless. For the first The actual holding angular velocity of a normal joint, in radians per second; For the first The actual holding angular acceleration of a normal joint, in radians per second squared; To maintain the allowable positional error, the unit is radians; To maintain permissible speed error, the unit is radians per second; To maintain the allowable error for acceleration, the unit is radians per second squared.

[0059] The position holding tolerance is determined based on the encoder resolution and the robot's normal position tracking error, and is typically taken as 0.001° to 0.05°; the position holding tolerance is determined based on the speed fluctuation in the stopped state, and is typically taken as 0.01° / s to 1° / s; the position holding acceleration tolerance is determined based on the acceleration estimation noise in the stopped state, and is typically taken as 0.1° / s² to 10° / s².

[0060] The robot controller determines that a normal joint is in a stable state only after all normal joints have met the above conditions for 10 consecutive control cycles.

[0061] It should be noted that when two or more joints experience communication abnormalities within the same detection time window, or when the robot controller itself enters the Bus-off state, the entire machine will be shut down under controlled conditions, and automatic access for a single joint will not be performed.

[0062] S3. After communication is restored, verify the node configuration, clear old instructions, and determine the motion state of the joint to be connected based on continuous feedback.

[0063] The robot controller determines whether communication has been restored for abnormal joints based on the Boot-up message, Heartbeat recovery, the reappearance of continuous valid TPDOs, or the clearance of the CAN error state.

[0064] After CANopen communication is restored, the abnormal joint is designated as the joint to be connected, and the joint to be connected is reinitialized. Specifically, the robot controller executes the following initialization processes B1-B6 in sequence: B1: Keep the joint to be connected in the NMT pre-run state.

[0065] B2: Read the device identity object and verify the node number and device model.

[0066] B3: Verify servo operating mode, encoder status, and PDO mapping.

[0067] B4: Clear old RPDO instructions that have not yet been sent in the robot controller.

[0068] B5: Overwrite the old target position that may be retained in the servo driver with a new safety retention target. Here, the safety retention target refers to the target position instruction generated by the robot controller and written to the servo driver based on the actual position after the communication of the joint to be connected is restored. It is used to overwrite the old target position and prevent the joint from moving unexpectedly when the drive is enabled.

[0069] B6: Puts the joint to be connected into NMT running state, but temporarily keeps the driver disabled.

[0070] Furthermore, the robot controller continuously reads the position, actual torque, and status word of the joint to be connected, and checks whether each feedback data belongs to new data in the current control cycle.

[0071] In this embodiment, eight consecutive valid position feedback points are selected, and a cubic polynomial is fitted using the least squares method, which is expressed as: ; in, To fit the obtained first Joint positions, in radians; The time difference relative to the moment of recovery, expressed in seconds; This is a position constant term, in radians; This is the coefficient of the linear term, expressed in radians per second. is the coefficient of the quadratic term, with units of radians per second squared. is the coefficient of the cubic term, with units of radians per cubic second; The joint number to be connected.

[0072] The time corresponding to the last fitted sample is taken as the time to determine the recovery state. The motion state of the joint to be connected is represented as: ; ; ; ; in, The estimated position of the joint to be connected is in radians. The estimated angular velocity of the joint to be connected is expressed in radians per second. The estimated value of the joint angular acceleration to be connected is expressed in radians per second squared. This is the estimated value of the angular acceleration of the joint to be connected, in radians per cubic second.

[0073] It should be noted that using multiple consecutive position feedback points for fitting can reduce the impact of the recovered first frame data, encoder quantization error, and message arrival jitter on speed and acceleration estimation.

[0074] Enter S4 only when the device identity and PDO configuration verification are passed, old control commands have been cleared, continuous feedback is all new data, position change meets the maximum speed limit, actual torque does not exceed the allowable torque and status word remains stable.

[0075] S4. Based on the communication interruption time, fault handling method, and joint movement restrictions, the position and speed reachable range are recursively deduced. Candidate trajectory progresses with reachable speed ranges and consistent movement directions are screened from the original reference trajectory. The normalized state distance between the position, speed, and acceleration corresponding to each candidate trajectory progress and the actual movement state determined by the joint to be accessed after communication is restored is calculated. The candidate trajectory progress with the smallest normalized state distance and the ratio of the second smallest distance to the smallest distance reaches the allowable separation ratio is determined as the estimated trajectory progress.

[0076] First, determine the duration of the communication interruption. Specifically, let the time when the recovery state of the joint to be accessed is determined be... The duration of the communication interruption is .

[0077] in, For the first The duration of communication interruption for each joint, in seconds; The time for restoring the state is determined in seconds; The last valid feedback time is in seconds.

[0078] Secondly, determine the reachable range of the motion. Specifically, establish the initial interval based on the last valid position and velocity before the communication interruption: , .

[0079] in, This represents the position interval at the start of the interval recursion, in radians. This represents the angular velocity interval at the start of the interval recursion, in radians per second; superscript. This indicates the initial recursive state.

[0080] The robot controller determines the permissible angular acceleration range based on the fault handling methods executed by the servo driver during communication anomalies. .

[0081] in, The allowable angular acceleration range of the joint to be connected during the communication interruption, in radians per second squared; The lower limit of permissible angular acceleration, in radians per second squared; The upper limit of permissible angular acceleration is expressed in radians per second squared.

[0082] Specifically, when the servo drive performs position holding, the allowable angular acceleration range is determined based on the position holding control error and the maximum holding torque; when performing rapid stop, it is determined based on the rapid stop deceleration configured in the drive; when performing drive disabling, it is determined based on the robot's gravity, load, friction, and mechanical braking status; when the actual fault handling method cannot be confirmed, a conservative angular acceleration range covering all possible handling methods is adopted.

[0083] The robot controller operates according to the control cycle. The recursive angular velocity range is represented as: ; in, For the first The angular velocity range after each iteration is expressed in radians per second. The interval recursion index; The robot control cycle is measured in seconds. For the first The lower limit of the permissible angular velocity of each joint, in radians per second; For the first The maximum permissible angular velocity for each joint, in radians per second; This represents interval addition. This means that the calculation interval is restricted to the given upper and lower limits.

[0084] Furthermore, the recursive position interval is obtained, represented as: ; in, For the first The position interval after each recursion is in radians; For the first Lower limit of the mechanical position of each joint, in radians; For the first The upper limit of the mechanical position of a joint, in radians.

[0085] It should be noted that in the recursive formula for angular velocity, The dimension of is radians per second; in the position recursion formula, and The dimension of all values ​​is radians.

[0086] The number of iterations is obtained as follows: ; in, Let be the number of iterations in the interval, which is a positive integer; This indicates rounding up to the nearest integer.

[0087] After completion After the recursion, As the reachable range of location during communication recovery, The angular velocity range that can be reached during communication recovery.

[0088] Third, candidate trajectory progress is filtered. Specifically, within the original reference trajectory range covered by the real-time data cache, the robot controller checks the reference position and reference angular velocity corresponding to each trajectory progress.

[0089] Specifically, retain the trajectory progress that simultaneously meets the following C1-C3 screening conditions: C1: The The reference position of each joint is located within the reachable range.

[0090] C2: The The reference angular velocity of each joint is within the achievable angular velocity range.

[0091] C3: When both the actual angular velocity and the reference angular velocity are greater than the zero velocity allowable error, they have the same sign.

[0092] The zero-speed allowable error is determined based on the angular velocity fluctuation under normal stopping conditions, and is usually taken as 0.01° / s to 1° / s.

[0093] Furthermore, based on the trajectory progress and trajectory progress speed before the anomaly occurred, the predicted trajectory progress is calculated, expressed as: ; in, For the first The predicted trajectory progress of each joint, dimensionless; The last valid trajectory progress before the communication anomaly, dimensionless; The final effective trajectory speed, in seconds; The duration of the communication interruption, in seconds; The minimum trajectory progress covered by the real-time data cache is dimensionless; The maximum trajectory progress covered by the real-time data cache is dimensionless.

[0094] Finally, the normalized state distance is calculated. Specifically, the robot controller calculates the normalized state distance for each candidate trajectory progress, expressed as: ; in, progress of candidate trajectories The corresponding normalized state distance is dimensionless. For the first Each joint in trajectory progress The reference position is in radians. For the first Each joint in trajectory progress The reference angular velocity at that point, in radians per second; For the first Each joint in trajectory progress The reference angular acceleration at that point, in radians per second squared; This is the location normalization scale, in radians; The normalized scale for angular velocity is expressed in radians per second. Angular acceleration is a normalized scale, with units of radians per second squared. The trajectory progress normalization scale is dimensionless. , and These are the estimated values ​​of position, angular velocity, and angular acceleration obtained from S3, respectively.

[0095] It should be noted that the normalization scales for position, angular velocity, angular acceleration, and trajectory progress are each taken as three times the standard deviation of the corresponding error during normal robot operation, and must not be less than the minimum resolvable quantity of the encoder or state estimation method.

[0096] It should be noted that each term within the square root is a dimensionless square term obtained by dividing a parameter with the same dimension. Therefore, the normalized state is dimensionless.

[0097] The trajectory progress with the smallest normalized state distance is selected from all candidate trajectory progresses, and is represented as: ; in, The estimated trajectory progress corresponding to the joint to be connected is dimensionless. A set of candidate trajectory progresses that satisfy the conditions of position reachability interval, angular velocity reachability interval, and direction of motion; This represents the trajectory progress corresponding to achieving the minimum normalized state distance.

[0098] Let the minimum normalized state distance and the second minimum normalized state distance be respectively and The separation ratio of candidate results is calculated and expressed as: ; in, The separation ratio of candidate results is dimensionless. The minimum normalized state distance is dimensionless. The second smallest normalized state distance is dimensionless.

[0099] The maximum allowable normalized state distance is determined based on the statistical distribution of normal operation data and simulated communication interruption data, and is usually taken as 2 to 5; the minimum allowable candidate result separation ratio is determined based on the distance distribution between the correct candidate and the adjacent incorrect candidate, and is usually taken as 1.2 to 3.0.

[0100] Only when the minimum normalized state distance does not exceed the maximum allowable value and the candidate result separation ratio is not less than the minimum allowable value, will the result be... As the estimated trajectory progress corresponding to the joint to be connected.

[0101] It should be noted that step S4 first eliminates physically impossible trajectory positions by considering the reachable range of motion during the communication interruption, and then determines the original reference trajectory position of the joint to be connected by using the position, velocity, acceleration and trajectory progress prediction results. This can avoid misjudgment caused by the same joint position corresponding to different motion stages in the reciprocating trajectory.

[0102] S5. Determine the target access trajectory progress and target access time based on the estimated trajectory progress, generate a seventh-order polynomial target re-access trajectory based on the actual state, and verify the motion, torque, end pose and collision constraints.

[0103] The robot controller sets the progress of candidate access trajectories within the range following the original reference trajectory. The progress of the candidate access trajectory meets the requirements. .

[0104] in, The progress of the candidate access trajectory is dimensionless. The estimated trajectory progress of the joint to be connected is dimensionless; The trajectory progression of a normal joint is dimensionless.

[0105] Furthermore, a candidate access time is set for each candidate access trajectory progress. .

[0106] In this embodiment, the candidate access time starts from 0.4s and increases by 0.2s until it reaches 2s; in other embodiments, the candidate access time is usually set to 0.1 to 5s based on the robot's rated speed, work cycle, and allowable pause time.

[0107] Furthermore, the estimated values ​​of position, angular velocity, angular acceleration, and angular jerk obtained in S3 are used as the initial state for the joint to be connected.

[0108] The remaining normal joints are taken as the initial state based on the actual holding position, actual holding angular velocity and actual holding angular acceleration obtained from S2; since the normal joints have been stabilized, their initial angular acceleration is zero.

[0109] Then, a seventh-degree polynomial trajectory is generated. Specifically, a seventh-degree polynomial is generated for each joint, represented as: ; in, For the first The position of each joint during the candidate access process, in radians; The time is measured in seconds, starting from the moment the candidate access begins. For the first The seventh-order polynomial of the joints Order coefficient, in radians per Power of a second; The degree is the polynomial, ranging from 0 to 7; Candidate access time, in seconds; subscript This indicates the candidate access process.

[0110] The initial boundary of the seventh-order polynomial is determined by the actual position, angular velocity, angular acceleration, and angular jerk of the corresponding joint, while the termination boundary is the progress of the original reference trajectory on the candidate access trajectory. The position, angular velocity, angular acceleration, and angular jerk at a given location are expressed as: ; ; ; ; in, When the candidate access ends, the first The position of each joint, in radians; Angular velocity at the end of the candidate access, in radians per second; Angular acceleration at the end of candidate access, in radians per second squared; Angular jerk at the end of the candidate access, in radians per cubic second.

[0111] The coefficients of eight seventh-degree polynomials are solved using eight boundary conditions, including the initial and final conditions.

[0112] Furthermore, the robot controller performs discrete sampling of the candidate trajectory according to the control cycle and checks whether each sampling point meets the position, angular velocity, angular acceleration and angular jerk limits of each joint.

[0113] Specifically, by examining the dynamic constraints, the robot controller calculates the joint torques corresponding to the candidate trajectories based on the dynamic model, expressed as: ; in, This is the joint torque vector corresponding to the candidate trajectory, in Newton-meters; represents the candidate joint position vector, in radians; represents the candidate joint angular velocity vector, in radians per second; represents the candidate joint angular acceleration vector, in radians per second squared. The robot's inertia matrix; The matrix represents the Coriolis force and the centrifugal force. This is the robot's gravitational torque vector, expressed in Newton-meters. This is the frictional torque vector, with units of Newton-meters.

[0114] The calculated torque of each joint in the candidate trajectory must not exceed the allowable torque of the corresponding joint; the allowable torque is determined by the continuous torque and short-time peak torque of the servo drive, usually taken as 60% to 100% of the rated torque, and must not exceed the drive's specified value.

[0115] After checking the end-effector pose and collision conditions, the robot controller uses a forward kinematics model to calculate the robot end-effector position and pose corresponding to the candidate trajectory, and compares them with the end-effector position and pose corresponding to the original reference trajectory. The end-effector position deviation is expressed as: ; in, This represents the end position deviation, in meters. is the end position vector corresponding to the candidate trajectory, in meters; This is the original reference end position vector at the corresponding time, in meters; This represents the Euclidean distance.

[0116] The robot controller also calculates the minimum distances between links and between the robot and the environment using link collision models and working environment collision models.

[0117] The minimum allowable safe distance is determined based on the robot model error, tooling positioning error, and operational safety requirements, and is usually taken as 5 to 200 mm; the allowable deviation of the end position is determined based on the robot's operational process accuracy, and is usually taken as 0.1 to 10 mm.

[0118] Finally, the robot controller determines the target and then the trajectory for the joint to be connected. Specifically, the robot controller eliminates candidate trajectories that do not meet the joint motion restrictions, joint torque restrictions, end-effector pose requirements, or collision distance requirements.

[0119] From the remaining candidate trajectories, the candidate trajectory with the shortest access time is selected first; when multiple candidate trajectories have the same access time, the candidate trajectory with the smaller maximum deviation of the end position is selected; when the maximum deviation of the end position is the same, the candidate trajectory with the smaller maximum deviation of the end attitude is selected; when the end attitude deviation is still the same, the candidate trajectory with the smaller proportion of peak joint torque to allowable torque is selected.

[0120] This embodiment uses a step-by-step comparison method to determine the target and then access the trajectory, without setting manual weights or using simple weighted summation.

[0121] The final target access trajectory progress is obtained. Target access time And the coefficients of the seventh-order polynomials corresponding to each joint.

[0122] If no feasible trajectory exists within the current candidate access time, the candidate access time is extended and a new candidate trajectory is generated. If no feasible trajectory exists after the maximum allowed access time is reached, the robot is kept stopped and the entire trajectory is replanned or manually confirmed.

[0123] S6. Control each joint to execute the target reconnect trajectory, and resume coordinated control after the changes in position, speed and torque meet the reconnection conditions.

[0124] Control each joint to execute the target reconnect trajectory, and resume coordinated control after the changes in position, velocity and torque meet the reconnection conditions.

[0125] The robot controller first calculates the target position of each joint corresponding to the first control cycle of the target and then the trajectory, and loads it to the servo driver of each joint via RPDO.

[0126] After the target position is loaded in the first control cycle, the joint to be connected is put into the running enable state to avoid the old target position before the communication interruption is executed momentarily during drive enable.

[0127] Within each SYNC control cycle, the robot controller calculates the corresponding target position and target angular velocity based on the seventh-order polynomial coefficients of each joint, and sends the target position via RPDO; when the servo driver supports velocity feedforward, it also sends the target angular velocity.

[0128] The robot controller receives the actual position, actual angular velocity, actual torque and status word of each joint through TPDO, and checks whether the feedback data is new data for the current control cycle.

[0129] During the target reconnection process, if any of the following conditions D1-D6 occur, execution will be immediately aborted and the process will return to S2. Specifically: D1: The joint to be connected has experienced another communication failure.

[0130] D2: The actual position or actual angular velocity of any joint deviates from the target and then re-enters the trajectory.

[0131] D3: The actual torque of any joint exceeds the allowable torque.

[0132] D4: The robot's minimum collision distance is less than the minimum allowable safe distance.

[0133] D5: The robot's end-effector pose deviation continues to increase.

[0134] D6: Any joint status word enters a fault state.

[0135] When the target access time ends, the robot controller compares the actual state of each joint with the progress of the target access trajectory. The corresponding original reference state.

[0136] Each joint should meet the following requirements , .

[0137] in, For the first Within the first control cycle, the first The actual position of each joint, in radians; The reference position corresponding to the target access trajectory progress, in radians; For the first Within the first control cycle, the first The actual angular velocity of each joint, in radians per second; The reference angular velocity corresponding to the progress of the target access trajectory, in radians per second; Allowable error for access location, in radians; Allowable error for access speed, in radians per second; This is the sequence number of the robot control cycle.

[0138] The allowable error for the access position is determined based on the encoder resolution and the robot's normal position tracking accuracy, and is usually taken as 0.001° to 0.05°; the allowable error for the access speed is determined based on the normal trajectory tracking speed error, and is usually taken as 0.01° / s to 1° / s.

[0139] The ratio of actual torque change between adjacent control cycles is expressed as: ; in, For the first The actual torque change ratio of adjacent control cycles of each joint is dimensionless. For the first Within the first control cycle, the first The actual torque of each joint, measured in Newton-meters; The first in the previous control cycle The actual torque of each joint, measured in Newton-meters; For the first The rated torque of each joint, measured in Newton-meters.

[0140] The allowable value of the actual torque change ratio is determined based on the allowable torque change rate of the servo drive and the normal trajectory switching data, and is usually taken as 0.5% to 5% of the rated torque for each control cycle.

[0141] When each joint meets the requirements for position error, angular velocity error and actual torque change ratio for 10 consecutive control cycles, and the status words of each joint are stable, the target reconnection is considered complete.

[0142] The robot controller sets the current trajectory progress to The time parameterization results of the original reference trajectory are re-adopted, and the recovered joint positions, joint velocities, joint torques, status words, and data validity markers are written into the real-time data cache to continue normal multi-joint collaborative control.

[0143] It should be noted that the target re-entry trajectory adopts the actual motion state of each joint at the start time and remains consistent with the position, angular velocity, angular acceleration and angular jerk of the original reference trajectory at the end time. Therefore, it can reduce the sudden velocity change and torque impact caused by trajectory switching.

[0144] This embodiment provides an experimental verification of a joint cooperative control method based on a CAN bus. The experimental platform is as follows: Figure 5 As shown, the system includes a six-axis articulated robot, a robot controller, six sets of joint servo drives, a CAN bus module, encoder feedback lines, and a host computer monitoring terminal. The robot controller communicates with each joint servo drive using the CANopen protocol. It reads the actual position, speed, torque, and status word of the joints periodically via TPDO, and sends control words and target positions via RPDO. The control cycle is set to 5ms, and the CAN bus communication rate is set to 1Mbit / s.

[0145] During the experiment, the six-axis articulated robot was made to run according to a preset joint space trajectory. The CAN communication of the corresponding node of the third joint was artificially interrupted after 1.5 seconds of trajectory execution to simulate conditions such as instantaneous loosening of connectors, accumulation of error frames, or nodes entering a bus-off state. Figure 6 As shown, after the robot controller detects a communication anomaly in a single joint, it identifies the third joint as the abnormal joint, freezes its old RPDO commands to be sent, and controls the remaining normal joints to decelerate collaboratively along the original reference trajectory to maintain the state.

[0146] After communication with the third joint is restored, the robot controller verifies the node number, servo operating mode, encoder status, and PDO mapping, clears old control commands, and continuously reads the position feedback of the third joint while the drive is disabled. Based on the continuous feedback data, the position, velocity, and acceleration of the third joint at the time of restoration are determined and used as the actual motion state of the joint to be connected.

[0147] Furthermore, based on the communication interruption time, the rapid stopping method of the third joint, and the speed and acceleration limits of the joint, the position reachable range and speed reachable range of the third joint are recursively obtained; candidate trajectory progresses that are located in the reachable range and have the same direction of movement are selected from the original reference trajectory; and the normalized state distance between the state corresponding to each candidate trajectory progress and the actual movement state of the joint to be connected is calculated; the trajectory progress with the smallest distance and the candidate result separation ratio that meets the requirements is determined as the estimated trajectory progress.

[0148] Subsequently, after estimating the trajectory progress and maintaining the trajectory progress of normal joints, candidate access positions and access times are set. A seventh-order polynomial trajectory is generated with the actual state of each joint as the starting point and the corresponding state of the original reference trajectory as the ending point. The joint position, velocity, acceleration, jerk, torque, end pose and collision distance are checked in sequence to determine the target re-access trajectory.

[0149] Figure 7 Exemplary comparison results are shown. When using the conventional strategy of directly restoring the current target position, the peak synchronization error of the third joint is approximately 5.3°, and the error decay time is relatively long. When using the method of this invention, the peak synchronization error is approximately 0.45°, and it recovers to near zero in a shorter time. This demonstrates that the proposed method can reduce the joint chasing amplitude and synchronization error after communication recovery, and reduce end-effector pose abrupt changes and joint torque impacts.

[0150] This embodiment also provides a computer device applicable to the joint coordination control method based on CAN bus, including: a memory and a processor; the memory is used to store computer-executable instructions, and the processor is used to execute the computer-executable instructions to implement the joint coordination control method based on CAN bus as proposed in the above embodiment.

[0151] The computer device can be a terminal, comprising a processor, memory, communication interface, display screen, and input devices connected via a system bus. The processor provides computing and control capabilities. The memory includes non-volatile storage media and internal memory. The non-volatile storage media stores the operating system and computer programs. The internal memory provides an environment for the operation of the operating system and computer programs stored in the non-volatile storage media. The communication interface is used for wired or wireless communication with external terminals; wireless communication can be achieved through Wi-Fi, carrier networks, NFC (Near Field Communication), or other technologies. The display screen can be an LCD screen or an e-ink screen. The input devices can be a touch layer covering the display screen, buttons, a trackball, or a touchpad on the computer device's casing, or an external keyboard, touchpad, or mouse.

[0152] This embodiment also provides a storage medium storing a computer program, which, when executed by a processor, implements the CAN bus-based joint cooperative control method proposed in the above embodiments. The storage medium can be implemented by any type of volatile or non-volatile storage device or a combination thereof, such as Static Random Access Memory (SRAM), Electrically Erasable Programmable Read-Only Memory (EEPROM), Erasable Programmable Read Only Memory (EPROM), Programmable Read-Only Memory (PROM), Read-Only Memory (ROM), magnetic storage, flash memory, magnetic disk, or optical disk.

[0153] In summary, this invention achieves accurate narrowing of the trajectory matching range after communication recovery by recursively estimating the reachable range of joint motion and filtering candidate trajectory progress; it achieves accurate differentiation of different motion stages in reciprocating trajectories by determining the estimated trajectory progress through normalized state distance and candidate result separation ratio; it achieves continuous and smooth transition of joint motion states by generating a seventh-order polynomial re-access trajectory that satisfies the boundary conditions of multiple motion states; and it achieves safe and stable recovery of multi-joint collaborative control by conducting multiple constraint verifications and closed-loop monitoring of access states.

[0154] It should be noted that the above embodiments are only used to illustrate the technical solutions of the present invention and are not intended to limit it. Although the present invention has been described in detail with reference to preferred embodiments, those skilled in the art should understand that modifications or equivalent substitutions can be made to the technical solutions of the present invention without departing from the spirit and scope of the technical solutions of the present invention, and all such modifications or substitutions should be covered within the scope of the claims of the present invention.

Claims

1. A joint cooperative control method based on CAN bus, characterized in that, include: Collect the position, speed, torque and communication status of each joint of the robot, and bind them to the trajectory progress of the original reference trajectory; Identify communication anomalies in a single joint, record the trajectory progress, position, speed, and fault handling method before the anomaly, and control the remaining joints to decelerate and maintain the speed. After communication is restored, the node configuration is verified, old instructions are cleared, and the motion state of the joint to be connected is determined based on continuous feedback. Based on the communication interruption time, fault handling method and joint movement limit, the position and velocity reachable range are recursively deduced. Candidate trajectory progress with velocity reachable range and consistent movement direction is screened in the original reference trajectory. The normalized state distance between the position, velocity and acceleration corresponding to each candidate trajectory progress and the actual movement state determined by the joint to be connected after communication is restored is calculated. The candidate trajectory progress with the smallest normalized state distance and the ratio of the second smallest distance to the smallest distance reaches the allowable separation ratio is determined as the estimated trajectory progress. The target access trajectory progress and target access time are determined based on the estimated trajectory progress. A seventh-order polynomial target re-access trajectory is generated based on the actual state, and the motion, torque, end pose and collision constraints are verified. Control each joint to execute the target reconnect trajectory, and resume coordinated control after the changes in position, velocity and torque meet the reconnection conditions.

2. The joint cooperative control method based on CAN bus as described in claim 1, characterized in that, The trajectory progress binding with the original reference trajectory includes: Obtain the position, velocity, torque, and communication status of each joint of the robot; Determine the trajectory progress of the original reference trajectory corresponding to the acquisition of joint status data; Based on the correspondence between joints and trajectory progress, position, velocity, torque, and communication status are associated with the corresponding trajectory progress to form a record of the operating status of each joint.

3. The joint cooperative control method based on CAN bus as described in claim 1, characterized in that, The process of identifying communication anomalies in a single joint, recording the trajectory progress, position, speed, and fault handling method before the anomaly, and controlling the deceleration and maintenance of the remaining joints includes: Based on the communication status of each joint, identify the single joint that has a communication failure and designate it as the joint to be connected. Extract the trajectory progress, position, and speed of the joint to be connected before the communication anomaly, and record the fault handling methods adopted for the communication anomaly; Send deceleration control commands to the remaining joints to slow them down and put them into a holding state.

4. The joint coordination control method based on CAN bus as described in claim 1, characterized in that, The step of determining the motion state of the joint to be connected based on continuous feedback includes: Read the node configuration of the joint to be connected after communication is restored, and verify the node configuration; Clear old instructions sent to the joint to be connected before the communication failure that have not yet been completed; Continuously acquire position and velocity feedback from the joint to be connected; Based on continuously acquired position and velocity feedback, the actual position, actual velocity, actual acceleration, and direction of motion of the joint to be connected are determined, thus forming the actual motion state.

5. The joint cooperative control method based on CAN bus as described in claim 1, characterized in that, The recursive determination of the reachable position and velocity range based on communication interruption time, fault handling method, and joint movement limitations includes: The position and speed recorded before the communication failure are used as the initial state for recursion of the reachable range, and the recursion time domain is determined according to the communication interruption time. Determine the state evolution of the joint to be accessed during the communication interruption based on the fault handling method; Under joint movement restrictions, the position reachable range and velocity reachable range of the joint to be connected are recursively calculated along the recursive time domain; The location reachable range and speed reachable range corresponding to the communication recovery time are determined as the recovery reachable range of the joint to be connected.

6. The joint cooperative control method based on CAN bus as described in claim 1 or 5, characterized in that, The determination of the estimated trajectory progress includes: Extract the trajectory progress corresponding to the speed within the achievable speed range from the original reference trajectory; From the extracted trajectory progress, trajectory progress whose movement direction is inconsistent with the actual movement direction of the joint to be connected is filtered out to obtain candidate trajectory progress; The position, velocity, and acceleration corresponding to the progress of each candidate trajectory are obtained respectively, and the normalized state distance between them and the actual motion state is calculated. Arrange the progress of each candidate trajectory in ascending order of normalized state distance, and calculate the ratio of the second smallest distance to the smallest distance; When the ratio of the second smallest distance to the smallest distance reaches the allowable separation ratio, the candidate trajectory progress with the smallest normalized state distance is determined as the estimated trajectory progress.

7. The joint cooperative control method based on CAN bus as described in claim 1, characterized in that, The process of determining the target access trajectory progress and target access time based on the estimated trajectory progress, generating a seventh-order polynomial target re-access trajectory based on the actual state, and verifying motion, torque, end-effector pose, and collision constraints includes: The target access trajectory progress is determined based on the estimated trajectory progress and the trajectory status in the original reference trajectory that is after the estimated trajectory progress. The target access time is determined based on the actual motion state, the trajectory state corresponding to the target access trajectory progress, and joint motion constraints. Using the actual motion state as the starting boundary, the trajectory state corresponding to the target access trajectory progress as the ending boundary, and the target access time as the trajectory time domain, a seventh-degree polynomial is solved to generate the target re-access trajectory. The target reentry trajectory is sequentially checked for motion constraints, torque constraints, end-effector pose constraints, and collision constraints. The target reentry trajectory that passes all constraint checks is determined as the executable target reentry trajectory.

8. The joint cooperative control method based on CAN bus as described in claim 1, characterized in that, The control of each joint to execute the target re-entry trajectory, and the restoration of coordinated control after the changes in position, velocity, and torque meet the re-entry conditions, includes: Control the joint to be connected and the other joints to coordinate the execution of the executable target reconnection trajectory; During execution, the position, velocity, and torque of each joint are acquired to determine the changes in position, velocity, and torque of each joint. Determine whether changes in position, velocity, and torque meet the connection conditions; When the access conditions are met, the control trajectory of each joint is switched to the original reference trajectory, and the coordinated control of each joint of the robot is restored.

9. A computer device comprising a memory and a processor, wherein the memory stores a computer program, characterized in that, When the processor executes the computer program, it implements the steps of the CAN bus-based joint coordination control method according to any one of claims 1 to 8.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by the processor, it implements the steps of the CAN bus-based joint cooperative control method according to any one of claims 1 to 8.

Images