Method for singularity avoidance in the shoulder and wrist regions of a spherical-wrist 6r industrial robot arm

Abstract

The application relates to the technical field of logistics automation, and discloses a through method for singular point areas of a shoulder and a wrist of a spherical-wrist 6R industrial robot, which comprises the following steps: firstly, off-line loading of geometric parameters of the robot and presetting of three types of singular judgment thresholds; continuously collecting and buffering joint angles in a servo cycle; solving end poses by means of homogeneous transformation and synchronously calculating a Jacobian matrix; and parallelly completing three types of singular state detection of the shoulder, the elbow and the wrist. The scheme distinguishes two types of operation branches, i.e. singular and non-singular, closes damping in the non-singular state, adopts a standard Jacobian pseudo-inverse to output a motion instruction, starts adaptive damping correction only when the robot approaches a singular boundary, and solves joint speeds by adapting three types of mainstream control modes. The scheme can completely identify all singular configurations of the robot, avoids singular missed judgment to cause equipment out of control, continuously and real-timely identifies dangerous areas, and avoids the problem of large rotation of multiple axes caused by small end displacement.

Description

Technical Field

[0001] This invention relates to the field of logistics automation technology, specifically to a method for navigating singularity regions of the shoulder and wrist of a 6R ball-wrist industrial robotic arm. Background Technology

[0002] In logistics automation scenarios, the BallWrist 6R industrial robotic arm undertakes various material gripping and continuous handling operations. The end effector of the robotic arm needs to continuously change its spatial position and posture to complete the task. During the movement, the configuration of the robotic arm links will change continuously with the rotation of the joints. The special geometric configuration formed by the intersection of various links will interfere with the stability of the end motion control.

[0003] Existing singularity detection structures for robotic arms only have a single type of singularity detection, and can only identify singular configurations generated at a single location. This structure cannot simultaneously identify singular states at the shoulder, elbow, and wrist, resulting in some singular areas being missed during operation. Once a singular configuration is missed, even a small end-effector displacement can cause a large-scale rotation across multiple axes. To address this, a method for traversing singularity regions at the shoulder and wrist of a 6R ball-arm industrial robotic arm is proposed. Summary of the Invention

[0004] To address the shortcomings of existing technologies, this invention provides a method for navigating the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm, thereby solving the technical problem that even a small end-effector displacement can cause a large-scale rotation of multiple axes.

[0005] To achieve the above objectives, the present invention provides the following technical solution: a method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm, comprising: S1. System offline initialization: Load the DH geometry parameters of the 6R ball-wrist robotic arm and preset three types of singularity detection thresholds: shoulder distance threshold. Elbow joint angle threshold and wrist Singular intervals, initialize the adaptive damping parameter library.

[0006] S2. Real-time status acquisition: Servo cycle reads the current 6-axis joint angle Cache the joint angle of the previous control cycle .

[0007] S3, Basic Kinematics Solution: The forward kinematics solution for the end-target pose is obtained based on the DH homogeneous transform. Simultaneously calculate the Jacobian matrix to provide geometric data for singularity determination.

[0008] S4. Simultaneous Type III Singularity Detection: Shoulder singularity: Calculate the vertical distance from the center of the wrist to the plane of the shoulder rotation axis. If the distance is less than a threshold, it is considered a shoulder singularity.

[0009] Elbow singularity: Determine whether the 3-axis joint angle falls within the singular range of the extension limit.

[0010] Wrist Anomaly: Direct Verification Is it in If any one of the conditions is met, it is determined that the area has entered a strange danger zone.

[0011] S5, Singular Branch Process: When there is no singular trigger, the damping value is zero. The joint motion command is solved by standard Jacobian pseudo-inverse and the servo is directly and smoothly issued.

[0012] S6. Singular Region Adaptive Damping Control: The method for smooth passage through singular points calculates the sign of motion direction by using the difference in joint angles between preceding and following cycles. , and It distinguishes whether the robotic arm is moving closer to or away from the strange wall.

[0013] Damping correction is initiated only when approaching a singular point, and the damping magnitude is calculated inversely proportional to the distance from the singular boundary.

[0014] The robot is matched with three control modes: direct force control, speed control, and point-to-point position control. The corresponding damping correction formula is then applied to correct the end effector output.

[0015] Damped least squares is used to solve for joint velocities to suppress joint vibration impact caused by Jacobi pathology.

[0016] S7, Speed ​​Limiting Smoothing and Servo Deployment: The revised joint commands uniformly apply maximum speed limiting and low-pass filtering to prevent sudden shocks, and then send the driver to execute the grasping and handling tasks.

[0017] S8, Periodic Closed-Loop Feedback: Update the historical joint angle cache and return to the acquisition step for continuous looping to achieve real-time anomaly protection throughout the process.

[0018] Perform offline system initialization, load the DH geometric parameters of the 6R ball-wrist robotic arm, and complete the shoulder distance threshold. Elbow joint angle threshold wrist The singular interval is preset, and the initialization process of the adaptive damping parameter library is completed.

[0019] Entering the servo cycle, real-time status acquisition is performed to read the angles of all six joints of the Ball Wrist 6R industrial robotic arm. For the joint angle corresponding to the previous control cycle Complete cache storage.

[0020] Perform basic kinematic calculations and solve for the target pose at the end of the robotic arm using DH homogeneous transformation operations. Simultaneously, the Jacobian matrix operation is completed, generating geometric data for singularity determination.

[0021] Simultaneous three-category anomaly detection procedures were performed, including calculating the vertical distance from the wrist center to the shoulder rotation axis plane, verifying the triaxial joint angle range, and checking the wrist joint status, directly verifying... Is it in If any one of the judgment conditions is met, the robotic arm is marked as entering a strange danger zone.

[0022] When performing singular branch operations, if no singular determination result is triggered, the damping parameter is set to zero. The joint motion command is obtained by relying on the standard Jacobian pseudo-inverse operation and the motion command is smoothly sent to the servo.

[0023] The system performs adaptive damping control in singular regions, compares the joint angle differences between adjacent cycles to identify the movement trend of the robotic arm relative to the singular wall, activates damping correction only when the robotic arm approaches the singular point, matches three robot control modes to complete the end-effector output correction, and solves the joint velocity using damping least squares operation.

[0024] The system performs speed limiting and smoothing operations and servo-based output. It sequentially performs speed limiting and low-pass filtering on the corrected joint commands, and then transmits the processed commands to the driver, which drives the robotic arm to complete the gripping and handling operations.

[0025] The execution cycle is closed-loop feedback operation, updating the cache area storing historical joint angles, jumping to S2, and continuously looping real-time status acquisition to provide real-time protection against abnormal states of the robotic arm throughout the process.

[0026] Preferably, in S1, during system offline initialization, the DH geometric parameters and the three types of singularity determination thresholds are stored in the same local storage, and the adaptive damping parameter library is deployed according to the storage.

[0027] When performing the system offline initialization operation, the geometric parameters of the 6R robotic arm DH and the three types of singularity determination thresholds are stored in the local storage, and an adaptive damping parameter library is deployed based on the local storage.

[0028] Preferably, in S2, real-time status acquisition, the 6-axis joint angle reading channel and the historical joint angle buffer area are connected by a one-way data docking structure.

[0029] When performing real-time status acquisition, the joint angle data is transmitted unidirectionally to the historical joint angle cache area via the 6-axis joint angle reading channel for storage.

[0030] Preferably, in S3, the kinematic basis solution, the homogeneous transformation operation and the Jacobian matrix operation adopt a parallel data output channel to connect singularity detection.

[0031] When performing the basic kinematics calculations, the homogeneous transformation operation and the Jacobian matrix operation synchronously transmit geometric data to the singularity detection stage through the parallel data output channel.

[0032] Preferably, in the S4 and synchronous three-type anomaly detection, the three types of anomaly determinations of shoulder, elbow and wrist are connected in parallel to the same anomaly danger area marker.

[0033] When performing simultaneous three-category anomaly detection operations, the shoulder anomaly determination, elbow anomaly determination, and wrist anomaly determination simultaneously transmit determination signals to the anomaly danger zone marker.

[0034] Preferably, in the S5 singular branch process, the Jacobian pseudo-inverse solution output is directly connected to the servo command smoothing transmission channel, and the damped zero-return series solution input is connected to the servo command smoothing transmission channel.

[0035] When performing singular branch process operations, the damping is reduced to zero and first connected to the Jacobian pseudo-inverse solution. After the solution generates instructions, they are directly sent to the servo instruction smooth transmission channel.

[0036] Preferably, in the S6, singular region adaptive damping control, the motion direction identification output terminal initiates unidirectional connected damping correction, and the damping calculation is matched with the three types of control modes.

[0037] When performing singular region adaptive damping control, the signal generated by motion direction identification is unidirectionally transmitted to the damping correction initiation stage, and the damping calculation stage connects with the three types of control mode matching stage to perform calculations.

[0038] Preferably, in the S6, singular region adaptive damping control, the damping correction formula mapping and damping least squares solution adopt a first-to-last serial data transmission structure.

[0039] When performing singular region adaptive damping control, the damping correction formula mapping output data is sequentially transmitted to the damping least squares solution to complete the joint velocity calculation.

[0040] Preferably, in S7, speed limiting smoothing and servo sending, speed limiting and low-pass filtering are connected in series before and after, and the filter output is directly connected to the driver signal interface port.

[0041] When performing speed limiting smoothing and servo-based operations, joint commands are first processed by speed limiting, then sent to a low-pass filter, and finally directly transmitted to the driver signal interface port after filtering.

[0042] Preferably, in S8, the historical joint angle update cache output is reconnected to the joint angle reading acquisition channel of S2, which is used for real-time status acquisition.

[0043] During the execution cycle closed-loop feedback operation, the historical joint angle update cache outputs stored data, and the data is sent back to the joint angle reading acquisition channel for real-time status acquisition.

[0044] Compared with the prior art, the present invention provides a method for navigating singularity regions of the shoulder and wrist of a 6R ball-wrist industrial robotic arm, which has the following beneficial effects: The 6R Ball-Wrist Industrial Robotic Arm utilizes a method to simultaneously detect three types of singular states—shoulder, elbow, and wrist—in parallel. This method can fully cover all singular configurations of the 6R Ball-Wrist Industrial Robotic Arm, preventing the robotic arm from going out of control due to the omission of a single singularity. Relying on offline preset exclusive singularity judgment thresholds and continuous detection during servo cycles, it can identify dangerous areas in real time throughout the entire process, avoiding large-scale multi-axis rotation caused by small end displacements during grasping and handling operations from the source.

[0045] The independent non-singular branch processing link directly shuts down the damping correction logic when the robotic arm does not enter the singular danger zone. The entire process adopts the standard Jacobian pseudo-inverse output smooth motion command, and will not apply additional damping loss to the end tracking performance in the normal working range. It takes into account the smoothness of the regular trajectory operation and the smoothness of the movement in the singular area, and realizes seamless switching between the two working conditions.

[0046] By relying on the difference in joint angles between the previous and next cycles to identify the movement trend of the robotic arm relative to the singular boundary, the damping correction operation is only initiated when the robotic arm approaches the singular point. When the robotic arm moves away from the singular region, the damping constraint is not continuously applied, thus avoiding the trajectory following lag problem caused by full-process damping. This ensures smooth passage through the singular region while maintaining the accuracy of normal motion control.

[0047] The adaptive damping correction logic can synchronously match three mainstream industrial control modes: direct force control, speed control, and point-to-point position control. There is no need to change the damping calculation logic when switching between different gripping and handling operation modes. The joint speed is solved uniformly through damping least squares, continuously suppressing joint vibration and impact caused by ill-conditioned Jacobian matrix, and adapting to the multi-condition switching needs of logistics automation.

[0048] After damping correction, all joint commands are uniformly subjected to speed limiting and low-pass filtering before being sent to the driver. This eliminates the impact and vibration of the robotic arm caused by sudden changes in commands before and after damping intervention. In conjunction with periodic closed-loop feedback, the historical joint angle cache is continuously updated, and the entire process of acquisition, calculation, detection and control is executed in a loop to form an uninterrupted full-cycle singularity protection closed loop. Attached Figure Description

[0049] Figure 1This is a schematic diagram of the method flow of the present invention.

[0050] Figure 2 This is a schematic diagram of the linkage coordinate system of the 6R industrial robotic arm of the present invention.

[0051] Figure 3 This is a schematic diagram of the parameter variables of the present invention. Detailed Implementation

[0052] This invention provides a technical solution: a method for navigating the singularity regions of the shoulder and wrist of a 6R ball-wrist industrial robotic arm, comprising: [See details]. Figure 1 , Figure 2 and Figure 3 S1, System offline initialization: Load the DH geometry parameters of the 6R ball-wrist robotic arm and preset three types of singularity detection thresholds: shoulder distance threshold. Elbow joint angle threshold and wrist Singular intervals, initialize the adaptive damping parameter library.

[0053] S2. Real-time status acquisition: Servo cycle reads the current 6-axis joint angle Cache the joint angle of the previous control cycle .

[0054] S3, Basic Kinematics Solution: The forward kinematics solution for the end-target pose is obtained based on the DH homogeneous transform. Simultaneously calculate the Jacobian matrix to provide geometric data for singularity determination.

[0055] S4. Simultaneous Type III Singularity Detection: Shoulder singularity: Calculate the vertical distance from the center of the wrist to the plane of the shoulder rotation axis. If the distance is less than a threshold, it is considered a shoulder singularity.

[0056] Elbow singularity: Determine whether the 3-axis joint angle falls within the singular range of the extension limit.

[0057] Wrist Anomaly: Direct Verification Is it in If any one of the conditions is met, it is determined that the area has entered a strange danger zone.

[0058] S5, Singular Branch Process: When there is no singular trigger, the damping value is zero. The joint motion command is solved by standard Jacobian pseudo-inverse and the servo is directly and smoothly issued.

[0059] S6. Singular Region Adaptive Damping Control: The method for smooth passage through singular points calculates the sign of motion direction by using the difference in joint angles between preceding and following cycles. , and It distinguishes whether the robotic arm is moving closer to or away from the strange wall.

[0060] Damping correction is initiated only when approaching a singular point, and the damping magnitude is calculated inversely proportional to the distance from the singular boundary.

[0061] The robot is matched with three control modes: direct force control, speed control, and point-to-point position control. The corresponding damping correction formula is then applied to correct the end effector output.

[0062] Damped least squares is used to solve for joint velocities to suppress joint vibration impact caused by Jacobi pathology.

[0063] S7, Speed ​​Limiting Smoothing and Servo Deployment: The revised joint commands uniformly apply maximum speed limiting and low-pass filtering to prevent sudden shocks, and then send the driver to execute the grasping and handling tasks.

[0064] S8, Periodic Closed-Loop Feedback: Update the historical joint angle cache and return to the acquisition step for continuous looping to achieve real-time anomaly protection throughout the process.

[0065] By relying on the system to initialize and uniformly store the robotic arm's geometric parameters, singularity criteria, and damping configuration offline, subsequent cycle operations do not require repeated loading of basic parameters, simplifying the preparation operations before the robotic arm's operation.

[0066] Joint angles are continuously recorded over consecutive periods by real-time status monitoring. The data provides complete raw data support for identifying the motion trends of the robotic arm relative to singular boundaries.

[0067] Basic kinematics calculation synchronously outputs end-effector pose Along with the Jacobian matrix, it provides a unified and complete geometric calculation basis for the synchronous detection of three types of singularities, ensuring that the singularity determination results are consistent and reliable.

[0068] Simultaneous detection of three types of anomalies—shoulder, elbow, and wrist—can identify all types of anomalies and dangerous areas of the robotic arm, avoiding motion loss problems caused by missing a single anomaly.

[0069] The process of setting up an independent singular branch distinguishes between normal and singular working conditions. In the absence of singular conditions, the standard motion solution logic is used, which will not increase the computational overhead of the robotic arm's normal operation.

[0070] Singular region adaptive damping control can be adapted to all mainstream operation control modes of robots. Damping adjustment is only activated when approaching singular points, which will not interfere with the normal motion performance of the robotic arm when it is away from the singular region. At the same time, it weakens the joint shaking phenomenon caused by Jacobi matrix ill-conditioning.

[0071] Speed ​​limiting smoothing and servo-based command delivery buffer joint commands to eliminate mechanical shocks caused by sudden command changes, thereby improving the smoothness of the robotic arm's grasping and handling operations.

[0072] The closed-loop feedback system enables continuous monitoring throughout the entire process, allowing for dynamic identification of unusual areas that may appear at any time during the operation of the robotic arm, thus achieving uninterrupted real-time protection against such unusual features.

[0073] S1. During system offline initialization, the DH geometric parameters and the three types of singularity determination thresholds are stored in the same local storage, and the adaptive damping parameter library is deployed according to the storage.

[0074] All basic parameters and damping configurations are centrally stored in local storage, and parameter retrieval operations are centralized and unified, reducing the operational fragmentation caused by retrieving parameters from multiple regions.

[0075] S2. In real-time status acquisition, the 6-axis joint angle reading channel and the historical joint angle buffer area are connected by a one-way data docking structure.

[0076] The one-way data connection structure limits the direction of data transmission, preventing data in the cache area from flowing back into the read channel and ensuring the stability of the data collection and storage order.

[0077] S3. In the fundamental kinematics calculation, homogeneous transformation operations and Jacobian matrix operations use parallel data output channels to connect to singularity detection.

[0078] The two types of operations output data in parallel, eliminating the need to wait for a single operation to complete before transmission, thus shortening the time required to supply geometric data to the anomaly detection stage.

[0079] S4. In the simultaneous detection of three types of anomalies, the anomaly determinations for the shoulder, elbow, and wrist are connected in parallel to the same anomaly danger area marker.

[0080] The three types of judgments are applied in parallel to the marker, and any singular judgment signal can directly trigger the marker without any delay or obstruction of the judgment signal.

[0081] In the S5 singular branch process, the Jacobian pseudo-inverse solution output is directly connected to the servo command smoothing transmission channel, and the damped zero-resolved series solution input is used.

[0082] The damping is zeroed out before series connection and the command channel is directly connected for solution. The command transmission path is simple under no singular operating conditions, reducing intermediate data transfer links.

[0083] In S6, in the singular region adaptive damping control, the motion direction identification output terminal initiates unidirectional connected damping correction, and the damping calculation is connected to the three types of control mode matching.

[0084] The unidirectional transmission of motion direction signals will not interfere with the identification process in the reverse direction. Damping calculation and mode matching are directly connected, and the damping calculation for different control modes is smoothly connected.

[0085] S6. In the adaptive damping control of singular regions, the mapping of the damping correction formula and the damping least squares solution adopt a serial data transmission structure.

[0086] The serial data transmission structure fixes the order of operations, ensuring that the damping correction data is completely transmitted to the solution and that the operation logic is not confused.

[0087] In S7, speed limiting and smoothing, and servo output, speed limiting and low-pass filtering are connected in series before and after, and the filter output is directly connected to the driver signal interface port.

[0088] Speed ​​limiting and low-pass filtering are connected in series, the instruction buffer processing flow is fixed, and the processed instructions can be directly sent to the driver without any additional transfer in the transmission link.

[0089] In S8, the historical joint angle update cache output is reconnected to the joint angle reading acquisition channel of S2, which is used for real-time status acquisition.

[0090] The cache directly connects to the acquisition channel, and the updated historical data can be directly supplied to the next cycle of acquisition, forming a complete closed-loop data flow link.

[0091] This solution involves performing offline system initialization to determine the DH geometric parameters of the 6R wrist robotic arm and the shoulder distance threshold. Elbow joint angle threshold and wrist The singular interval is stored in local storage, and an adaptive damping parameter library is deployed based on the local storage to complete all parameter configurations before the robotic arm operation.

[0092] After completing offline initialization, the robotic arm enters its servo cycle, operating via 6-axis joint angles. Read the real-time 6-axis joint angle data transmitted unidirectionally from the channel to the historical joint angle buffer area for storage, retaining the joint angle from the previous control cycle. data.

[0093] The kinematics fundamentals calculation operation is initiated. The homogeneous transformation operation module and the Jacobian matrix operation module perform calculations synchronously through the parallel data output channel, and the pose of the robot arm's end effector is solved based on the DH homogeneous transformation. It synchronously outputs the Jacobian matrix, providing complete geometric data to the singularity detection stage.

[0094] Simultaneous anomaly detection procedures were performed, with shoulder, elbow, and wrist anomaly assessments connected in parallel to anomaly hazard area markers. The vertical distance from the wrist center to the shoulder rotation axis plane was calculated, the three-axis joint angle intervals were checked, and the wrist joint status was verified, directly confirming the anomaly detection. Is it in Once any judgment condition is met, a judgment signal is transmitted to the singular danger zone marker, marking the robotic arm as having entered the singular danger zone.

[0095] When performing singular branch process operations, the damping is first connected to the Jacobian pseudo-inverse solver input terminal. When no singular marker signal is received, the damping is returned to zero and the zero damping parameter is output to the Jacobian pseudo-inverse solver. After the Jacobian pseudo-inverse solver generates joint motion commands, they are directly sent to the servo command smooth transmission channel to send the servo.

[0096] Upon receiving a singular danger zone marker signal, the singular zone adaptive damping control operation is executed. The motion trend signal generated by motion direction recognition is unidirectionally transmitted to the damping correction start. The damping calculation is performed by matching the three types of control modes. The damping correction formula maps the output data and sequentially transmits it to the damping least squares solution to complete the joint velocity calculation. Based on the motion trend of the robotic arm, the end output correction is only completed when approaching the singular point.

[0097] After performing speed limiting smoothing and servo-based operations, the joint commands, after damping correction, are first sent to the speed limiting processing, and then transmitted to the low-pass filter for buffering. The output of the low-pass filter is directly connected to the driver signal interface port, and the processed motion commands are transmitted to the driver to drive the 6R ball-wrist industrial robotic arm to complete the gripping and handling operations.

[0098] After a single servo control cycle is completed, a closed-loop feedback operation is performed. The historical joint angle update cache outputs the updated historical joint angle data, and the data is sent back to the joint angle reading acquisition channel for real-time status acquisition. The process then jumps to the real-time status acquisition step and continues to run in a loop. The entire process dynamically identifies the strange areas that appear during the operation of the robotic arm, achieving uninterrupted real-time anomaly protection.

[0099] After completing a single material grabbing and handling task, the robotic arm receives the next end-point target pose instruction from the host computer, re-enters the kinematics fundamental calculation operation, and repeats the entire process of subsequent singularity detection, motion control and closed-loop feedback to continuously complete batch logistics automation tasks.

[0100] Before the robotic arm changes the work material or the gripping posture, it re-executes the offline initialization operation to update the DH geometric parameters, singularity judgment threshold and adaptive damping parameter library to adapt to the new working conditions and the singularity protection requirements under different material sizes and gripping postures.

[0101] When the robotic arm is stopped for maintenance or restarted after a long period of inactivity, a complete offline initialization operation is performed to reset all parameters in the local storage, clear the historical joint angle cache area, eliminate data deviations accumulated over long-term operation, and ensure the stable operation of subsequent anomaly detection and damping control calculations.

[0102] The improved DH method establishes a coordinate system for each link of the robotic arm. Adjacent coordinate systems arrive Coordinate transformation is accomplished using four parameters, with the following general definition: Joint angle, unit: rad: Axis, will Rotate to The rotation angle is collinear with the joint variable of the rotating joint, while this value is fixed for the moving joint.

[0103] Linkage offset, unit mm: along Axis, from Move to The translation distance is the joint variable of the moving joint, while this value is fixed for the rotating joint.

[0104] Linkage length, unit mm: along Axis, from Move to The translation distance is the link Two-end shafts and The length of the common perpendicular between them.

[0105] Linkage twist angle, unit: rad: Axis, will Rotate to The rotation angle parallel to the parallel.

[0106] At this point, its improved DH parameters can be obtained as follows: , , and .

[0107] Forward kinematics Linkage coordinate system To coordinate system homogeneous transformation matrix

[0108] in This represents the offset distance between the axes of two adjacent joints. for shaft and Translation distance between axes. The angle of twist between adjacent axes. For joints about the axis of rotation The angle of rotation.

[0109] Forward kinematic equations 6. Pose equations of the end effector of the free robotic arm relative to the base It can be obtained by multiplying the homogeneous transformation matrices of each link: .

[0110] Inverse kinematics Desired pose of the robotic arm end effector When grasping an object, the grasping position and posture of the object are represented by the right side of this equation.

[0111]

[0112] Transformation of forward kinematic equations Because the axes of the fourth and fifth joints intersect at a single point, deformation occurs.

[0113]

[0114] After deformation, there exists an equation , , ,and

[0115] in, and .

[0116] Calculate the inverse solution Expanding the equations in the forward kinematics equations, we can obtain: (Equation 1) (Equation 2) (Equation 3) (Equation 4) (Equation 5) (Equation 6) We can solve equation 3 to get... The solution can be obtained by combining equations 1 and 2. and The solution can be found from equation 4. The solution can be found from equation 5. The solution can be found from equation 6. .

[0117] Choose the inverse solution The minimum joint motion criterion is used to select the inverse kinematics solution that best fits the current grasping situation from all inverse kinematics solutions. The minimum joint motion criterion is then used to filter the entire inverse kinematics solution set, selecting the set of joint angles with the minimum total joint displacement. This serves as the optimal inverse solution for matching the current capture conditions.

[0118] Core discrimination formula, minimum joint motion criterion:

[0119] Variable definition : The angle of the first axis joint in a certain set of inverse solutions.

[0120] : The current actual joint angle of the robotic arm's first axis.

[0121] The solution with the smallest total rotation amplitude of all joints corresponding to this set of inverse solutions is the optimal inverse solution.

[0122] Wrist singularity assessment When the inverse solution has 5 axes angles During this period, the BallWrist 6R industrial robotic arm is in a wrist singularity state, and it is necessary to avoid the robotic arm from grasping objects in this state. The fifth joint of the 6R wrist robotic arm, i.e., the joint angle of the wrist's central axis.

[0123] Cause and effect relationship Forward kinematics is a forward mapping benchmark: given joint angles, the end-effector pose is derived, used to verify the robot arm's motion accuracy and provide kinematic model parameters. Inverse kinematics is a reverse task solution: given the end-effector pose required for grasping, the joint angles are derived backward, relying entirely on the DH transformation model and geometric constraints established by forward kinematics. The forward solution yields... The transformation rules and linkage structure constraints are the core theoretical premises for inverse equation deformation, step-by-step solution, and singularity detection. The joint angles calculated by inverse solution can be used as input to the forward solution model, and the pose can be reverse-checked to verify whether it meets the grasping requirements, forming a closed-loop verification.

Claims

1. A method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm, characterized in that, Includes the following steps: S1. System offline initialization: Load the DH geometry parameters of the 6R ball-wrist robotic arm and preset three types of singularity detection thresholds: shoulder distance threshold. Elbow joint angle threshold and wrist Singular intervals, initialize the adaptive damping parameter library; S2. Real-time status acquisition: Servo cycle reads the current 6-axis joint angle Cache the joint angle of the previous control cycle ; S3, Basic Kinematics Solution: The forward kinematics solution for the end-target pose is obtained based on the DH homogeneous transform. Simultaneously calculate the Jacobian matrix to provide geometric data for singularity determination; S4. Simultaneous Type III Singularity Detection: Shoulder singularity: Calculate the vertical distance from the center of the wrist to the plane of the shoulder rotation axis. If the distance is less than a threshold, a shoulder singularity is determined. Elbow Singularity: Determine whether the 3-axis joint angle falls within the singularity range of the extension limit; Wrist Anomaly: Direct Verification Is it in If any one of the conditions is met, it is determined that the area has entered a strange danger zone; S5, Singular Branch Process: When there is no singular trigger, the damping value is zero. The joint motion command is solved by standard Jacobian pseudo-inverse and the servo is directly and smoothly issued. S6. Singular Region Adaptive Damping Control: The method for smooth passage through singular points calculates the sign of motion direction by using the difference in joint angles between preceding and following cycles. , and Distinguish whether the robotic arm is moving closer to or away from the strange wall; Damping correction is initiated only when approaching a singular point, and the damping magnitude is calculated inversely proportional to the distance from the singular boundary. Match the robot's three control modes and substitute them into the corresponding damping correction formula to correct the end output; Damped least squares is used to solve for joint velocities to suppress jitter. S7, Speed ​​Limiting Smoothing and Servo Deployment: The revised joint commands are uniformly subjected to maximum speed limiting and low-pass filtering, and then sent to the driver to execute the grasping and handling tasks. S8, Periodic Closed-Loop Feedback: Update the historical joint angle cache, return to the collection step and continuously loop for real-time protection.

2. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the S1 system offline initialization, the DH geometric parameters and the three types of singularity determination thresholds are stored in the same local storage, and the adaptive damping parameter library is deployed according to the storage.

3. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the S2 real-time status acquisition, the 6-axis joint angle reading channel and the historical joint angle buffer area are connected by a one-way data docking structure.

4. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the aforementioned S3 and kinematic fundamental solution, the homogeneous transformation operation and the Jacobian matrix operation adopt a parallel data output channel to connect singularity detection.

5. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the S4 synchronous three-type anomaly detection, the three types of anomaly determinations—shoulder, elbow, and wrist—are connected in parallel to the same anomaly danger area marker.

6. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the S5 singular branch process, the Jacobian pseudo-inverse solution output is directly connected to the servo command smoothing transmission channel, and the damped zero-return series solution input is connected to the servo command smoothing transmission channel.

7. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the S6 singular region adaptive damping control, the motion direction identification output terminal initiates unidirectional connected damping correction, and the damping calculation is matched with the three types of control modes.

8. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the S6 singular region adaptive damping control, the damping correction formula mapping and damping least squares solution adopt a first-to-last serial data transmission structure.

9. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the S7, speed limiting smoothing and servo sending, speed limiting and low-pass filtering are connected in series before and after, and the filter output is directly connected to the driver signal interface port.

10. The method for traversing the singularity region between the shoulder and wrist of a 6R ball-wrist industrial robotic arm according to claim 1, characterized in that: In the S8 periodic closed-loop feedback, the historical joint angle update cache output is reconnected to the joint angle reading and acquisition channel of S2 real-time status acquisition.

Images