A method, system and storage medium for multi-candidate inverse kinematics solving and null-space pose holding of a redundant robot arm

Abstract

The application discloses a kind of redundancy mechanical arm's multiple candidate inverse kinematics solving and zero space posture keeping method, system and storage medium.The method obtains mechanical arm end target pose, current joint state, last control cycle output joint state and joint limit information, constructs multiple inverse kinematics candidate seeds with different configuration tendency and is solved respectively, obtains multiple candidate joint solutions;According to end error, joint continuity, reference posture deviation, limit risk, coupled joint risk and candidate switching cost, candidate joint solution is comprehensively evaluated, and combined with historical candidate state, state candidate arbitration is executed;Again, according to joint limit margin, target candidate solution is executed near-limit gradual output protection, and joint instruction is generated.The application does not depend on specific robot model or solver framework, and can improve the robustness of redundancy mechanical arm inverse kinematics solving, joint configuration continuity, limit safety and control stability.

Description

Technical Field

[0001] This invention relates to the field of robot motion control technology, and in particular to an inverse kinematics solution method, a zero-space attitude maintenance method, a joint limit protection method, and a candidate joint configuration arbitration method applicable to redundant robotic arms. Background Technology

[0002] A redundant robotic arm is one whose number of joint degrees of freedom is greater than the number of degrees of freedom required to complete the end effector pose task. Due to the existence of redundant degrees of freedom, the robotic arm can further adjust the elbow position, wrist posture, joint distance from limits, or overall arm configuration while satisfying end effector position and posture constraints. Therefore, redundant robotic arms are highly valuable in complex grasping, confined space operations, obstacle avoidance, teleoperation, and humanoid robot control.

[0003] However, redundant degrees of freedom also mean that the inverse kinematics problem no longer has a unique solution. For the same end-effector pose, the robotic arm may have multiple sets of feasible joint solutions. If the control system solves or selects solutions based solely on the end-effector error, problems such as abrupt changes in arm shape, joint jumps, unnatural local configurations, individual joints approaching their limits, and coupled joints squeezing each other's motion space can easily occur.

[0004] In existing technologies, a single initial value is typically used for inverse kinematic iteration, such as using the joint angle of the previous control cycle as the initial value of the current cycle. This method has good real-time performance, but it is prone to getting trapped in local solutions when the target pose changes rapidly, the robotic arm approaches a singular configuration, the joints approach a limit, or redundant configurations need to be reallocated, resulting in the robotic arm failing to obtain an ideal joint configuration.

[0005] To address these issues, some methods impose joint truncation on the solution results. However, this approach often only takes action after the joint reaches its limit, easily leading to sudden velocity changes and mechanical shocks. Other methods incorporate a uniform attitude regression term into the optimization objective to guide the joints towards the reference attitude. However, these methods typically apply the same regression strength to all joints, lacking differentiated treatment for the functional roles of different joints. Furthermore, when the system solves multiple candidate solutions in parallel, existing technologies lack an effective mechanism for stable switching between candidate solutions, easily resulting in frequent jumps in candidate branches and affecting the smoothness of the control output.

[0006] In summary, existing inverse kinematics control methods for redundant robotic arms still have significant shortcomings in terms of solution robustness, coupled joint coordination, attitude maintenance adaptability, and candidate switching stability. A comprehensive solution that can balance end-effector tracking accuracy, configuration continuity, joint limit safety, and real-time control stability is needed. Summary of the Invention

[0007] To address the technical problems in existing redundant robotic arm inverse kinematics control methods, such as the ease with which a single initial value solution can get trapped in local configurations, difficulty in escaping near-limited joints, insufficient adaptability in maintaining a unified zero-space attitude, discontinuous near-limited joint protection, and unstable switching between multiple candidate solutions, this invention provides a method, system, and storage medium for solving multiple candidate inverse kinematics and maintaining zero-space attitude for redundant robotic arms.

[0008] In this process, multiple inverse kinematic candidate seeds with different configuration tendencies are constructed in each control cycle. After solving them separately, each candidate solution is scored by a comprehensive evaluation function. Then, state arbitration is performed in combination with historical states. Finally, near-limit asymptotic output protection is performed on the selected candidate solutions, so that the redundant robotic arm can obtain a more stable, safer and more natural joint configuration while maintaining the end-effector pose accuracy.

[0009] To achieve the above objectives, the present invention provides the following technical solution.

[0010] A method for solving multiple candidate inverse kinematics and maintaining zero-space attitude of a redundant robotic arm includes the following steps: Step 1: Obtain the target pose of the robotic arm end effector, the joint state output in the previous control cycle, the current joint state, and the joint limit information.

[0011] Step 2: Construct multiple inverse kinematics candidate seeds based on the joint state and limit information.

[0012] Step 3: Perform inverse kinematics solution for each candidate seed to obtain multiple candidate joint solutions.

[0013] Step 4: Evaluate the candidate joint solutions comprehensively based on end-effector error, joint continuity, reference posture deviation, limit risk, coupled joint risk, and candidate switching cost.

[0014] Step 5: Based on the candidate evaluation results and historical branch states, perform stateful candidate arbitration to determine the target candidate joint solution.

[0015] Step 6: Apply near-limit asymptotic output constraints to the target candidate joint solution based on the limit margin of each joint.

[0016] Step 7: Output the final robotic arm joint commands.

[0017] The specific implementation methods of each of the above steps are explained in detail below.

[0018] Regarding the construction of candidate seeds: To avoid local convergence caused by a single initial value, this invention constructs multiple candidate seeds within each control cycle. The candidate seeds include, but are not limited to, the following types: (1) Continuity seed: The joint state output in the previous control cycle is used as the initial value to maintain motion continuity; (2) Reference pose seed: Based on the previous joint state, an offset is added to return to the reference pose in order to maintain the natural configuration; (3) Limit escape seed: When one or more joints approach the limit, add an offset away from the limit direction; (4) Coordination seed for coupled joints: When there are joint groups that are kinematically or controllably interdependent, apply a coordination bias to the joint group; (5) Historically stable seed: Use candidate configurations that have been historically determined to be stable as initial values; (6) Task preference seed: Construct specific candidate initial values ​​based on the operation task, obstacle avoidance requirements or end attitude preference.

[0019] The number, type, and bias strength of the aforementioned candidate seeds can be dynamically adjusted according to the control cycle, computing power budget, robotic arm structure, and task requirements. This invention does not limit the specific number of candidate seeds, nor does it limit the specific bias formula.

[0020] Assume the redundant robotic arm has There are 1 controllable joints, and the current control cycle is... The joint vector output in the previous cycle was The currently measured joint vector is The reference posture or comfortable posture is The lower and upper limits of the joint are respectively and The candidate seed set is represented as: ; in This represents the number of candidate seeds, which can be a fixed value or dynamically adjusted based on computing power budget, target variation, or risk limits. Each candidate seed is uniformly represented as:

[0021] in As a reference for attitude regression bias, As a limit escape bias, To coordinate the offset of the coupling joint.

[0022] Task preference bias; , , , , for the first The combination coefficients corresponding to each candidate seed. Different candidate seeds can cover different configuration tendencies such as continuity, regressivity, constraint avoidance, coupling coordination, and task preference through differentiated configuration of combination coefficients.

[0023] The reference attitude regression bias is written as:

[0024] in Represents a vector By amplitude threshold Amplitude limiting is applied. The joint limiting margin is defined as:

[0025] The limit risk activation coefficient is defined as:

[0026] in As a soft protective boundary, To strengthen the protection boundary, the limit escape bias is expressed as:

[0027] in This indicates the direction away from the nearest limiting boundary and towards the joint safety zone. For the first The limit escape scale of each joint.

[0028] Regarding the escape mechanism of coupled joints: For redundant robotic arms with mutually coupled joint groups, this invention provides a coupled joint escape mechanism. The coupled joint group can be a forearm rotational joint and a wrist rotational joint, or any two or more joints in the shoulder, elbow, and wrist whose motion spaces influence each other.

[0029] When a joint in a coupled joint group approaches a physical limit, soft limit, velocity limit, or task constraint boundary, the system applies a bias away from the constraint boundary to that joint in the candidate seed, while simultaneously applying biases to other relevant joints in the coupled joint group that regress towards the reference attitude, comfort zone, or a larger available space. This bias is used to guide the inverse kinematics solver into a safer local solution space, rather than being used directly as the final output.

[0030] Suppose there are one or more coupled joint groups .make To select the first joint from the full joint vector The selection matrix for a set of coupled joints is then given by the vector of that set of coupled joints:

[0031] No. The risk function for a set of coupled joints is expressed as:

[0032] in The joint near-limit activation coefficient. Weights for attitude deviation of coupled groups The weighted norm. The coupled joint coordination bias is expressed as:

[0033] in For the first The regression matrix of each coupled joint group The escape term is generated based on the limit direction. The coupling group is biased and limited. The escape term is further expressed as:

[0034] in, This is the escape scale for the coupled joint.

[0035] This expression does not limit the specific joint numbers and quantities of the coupled joint group, and is applicable to any redundant robotic arm with kinematic coupling, limit coupling, or task coupling relationships. Through this mechanism, the present invention can actively adjust the configuration of the coupled joint group before the joint reaches the limit, reducing the risk of hard limit cutoff, wrist twisting, or exhaustion of redundant space in subsequent control cycles.

[0036] Regarding the solution of inverse kinematics: Each candidate seed is input into the inverse kinematics solution module. The inverse kinematics solution module can employ any data solution method suitable for real-time control, including iterative solution based on Jacobi pseudo-inverse, solution based on damped least squares, constraint solution based on quadratic programming, solution based on nonlinear optimization, and joint incremental solution based on model prediction or recursive optimization.

[0037] In each candidate branch, the objective to be solved includes at least the end pose error term, and may further include attitude maintenance term, joint limit term, velocity constraint term, acceleration constraint term, self-collision constraint term, obstacle avoidance constraint term or task preference term.

[0038] Let the forward kinematics mapping of the robotic arm be... ,in The end position, Let the target pose be the end effector pose. The error of the final task is expressed as:

[0039] in This is a logarithmic mapping from rotation group to rotation vector. For joint increment-based... The iterative solution adopts the following unified form:

[0040] The constraints include:

[0041] in For the terminal Jacobian matrix, This is the terminal error weight matrix. To maintain the weight matrix for pose, For a smooth weight matrix,

[0042] Let be the limiting potential function. For obstacle avoidance, self-collision, task preference, or other additional costs.

[0043] The maximum increment per step.

[0044] This is the upper limit of the second-order joint increment for a single step; if the upper limit of physical acceleration is used, then Δ²qmax,j=amax,jΔt² should be satisfied.

[0045] The limiting potential function is in piecewise continuous form:

[0046] in For the first Risk weight of each joint. This is the penalty coefficient for the strong protection zone. This potential function gradually increases after the joint enters the soft protection zone, ensuring that the risk of deviation is considered in the optimization before the boundary is crossed.

[0047] Regarding differentiated null-space attitude maintenance: This invention employs a differentiated strategy for zero-space attitude maintenance, rather than applying the same regression intensity to all joints. The system sets different attitude maintenance coefficients for different joints based on their mechanical function, range of motion, risk of limitation, task contribution, or coupling relationship with other joints.

[0048] For the Each joint, with its reference pose represented as:

[0049] in For the first The posture of each joint remains at the reference value. These are the joint values ​​among the candidate seeds. For reference or comfortable posture, In order to be with the first Each joint has a corresponding differentiated mapping function. This mapping function can be a fixed scaling function, a limiting function, a nonlinear function, or an adaptive function that changes with the task state.

[0050] In one implementation, the attitude hold strength of each joint is configured according to the following principles: weaker attitude hold is set for proximal joints that determine the wide reachability of the end effector to preserve operational flexibility; moderate attitude hold is set for intermediate joints that determine the overall arm configuration stability to suppress arm shape abrupt changes; stronger attitude hold is set for distal joints that are prone to entanglement, saturation or coupling risks to improve configuration stability; and the attitude hold strength of mission-sensitive joints is dynamically adjusted according to changes in the end effector target, obstacle position or operational phase.

[0051] From the zero-space perspective of the redundant robotic arm, the differential posture maintenance can be written as:

[0052] in For Jacobi's generalized inverse or weighted generalized inverse, The null projection matrix, This refers to secondary joint increments used for attitude maintenance, constraint avoidance, or coupling coordination. The differentiated null space increment is:

[0053] in To maintain the gain matrix for joint-by-joint pose For operability, comfort, or other configuration quality indicators. Let be the limit risk potential function. and These are the configuration quality and constraint avoidance weight matrices, respectively. and These are the gradients of the configuration quality index and the bounding potential function, respectively. Since... , , All of them can be set as diagonal matrices, block matrices, or state-dependent matrices, thus enabling the application of different null space action intensities to different joints.

[0054] Configuration quality indicators can be selected from one of the following forms:

[0055] in Indicates operability. Indicates reference posture comfort. This indicates the joint limit margin quality. To prevent small constants with odd values. The above indicators can be used individually or in a weighted combination.

[0056] Comprehensive evaluation of candidate solutions: After multiple candidate joint solutions are generated, this invention selects the candidate solution that is more suitable for the current control state through a comprehensive evaluation function. The evaluation function includes one or more of the following: end-effector position error, end-effector attitude error, continuity cost relative to the previous output joint state, deviation cost relative to the reference attitude or comfortable attitude, joint limit risk, coupled joint risk, joint velocity or acceleration risk, self-collision or external collision risk, candidate branch switching cost, and cost related to task preferences.

[0057] The comprehensive evaluation function is expressed as follows:

[0058] in For the first Evaluation items For the corresponding weights, The number of evaluation items. The weights are fixed values, or can be adaptively adjusted based on task stage, joint limit margin, target speed, candidate historical states, or robot arm load status.

[0059] Candidate solutions The comprehensive evaluation function is expanded as follows:

[0060] in and Output joint states for historical data. To control the cycle, For the risk function of the coupled joint,

[0061] This is a function to indicate the switching of candidate branches. These are the candidate branches selected in the previous cycle. and These represent the joint velocities and accelerations of the candidate solutions, respectively. This scoring function simultaneously considers task accuracy, continuity, reference attitude, limit safety, velocity smoothness, acceleration smoothness, coupling risk, and switching cost.

[0062] The weights of candidate solutions can also be adjusted adaptively. For example, the limit weight increases as the minimum limit margin decreases:

[0063] in As the baseline value for the limit weight, This is the limit weight adjustment coefficient.

[0064] The continuity weight increases as the target velocity decreases:

[0065] in The target terminal velocity or the target rate of change. As the continuous weight benchmark value, and To adjust parameters, adaptive weights are used to prioritize tracking the target during high-speed motion and to maintain a stable configuration during low-speed or stationary states.

[0066] Regarding stateful candidate arbitration: To avoid frequent switching between multiple candidate solutions, this invention introduces a stateful candidate arbitration mechanism, which combines historical states for judgment rather than selecting the lowest-scoring candidate frame by frame without memory.

[0067] Stateful candidate arbitration includes: (1) minimum hold time - after a candidate branch is selected, it is held first within a preset time or number of frames; (2) equivalent candidate determination - when the scores, end-point errors and limit risks of multiple candidate solutions are all less than the threshold, the current candidate is held first; (3) hysteresis switching threshold - switching is only allowed when the new candidate is significantly better than the current candidate; (4) joint direction flipping penalty - penalty is imposed on repeated changes in joint direction that are prone to causing redundant configuration swing; (5) target stationary guard - when the end target is stationary or changes slowly, the penalty for small jumps in the joint is increased; (6) candidate failure detection - when the current candidate has unacceptable end-point errors, limit risks or constraint violations, immediate switching is allowed.

[0068] The formal representation of a stateful candidate arbitrator is: Let the first... The original scores for each candidate solution are Current historical branch Keep the count as The revised scoring definition is:

[0069] in To switch hysteresis penalty, Penalty for joint rotation. Penalty for stationary guarding of the target. The direction flip penalty is expressed as:

[0070] in The weight for the direction reversal penalty. For the first Sensitivity to directional rotation of each joint

[0071]

[0072] The penalty for stationary guards is represented as follows:

[0073] in The penalty weight for stationary guards, For static defense coefficient, Indicates the state of the final target. For scale parameters, The closer the value is to 1, the closer the target is to being stationary.

[0074] The final candidate branch is determined by the following formula:

[0075] in This is the index of the finally selected candidate branch.

[0076] If the current candidate has not expired within the minimum hold time, then it must be held:

[0077] in To minimize the number of frames to hold, This is the function for determining the validity of candidates.

[0078] The candidate equivalent set is defined as:

[0079] in For an equivalent candidate set, , , These are the equivalent thresholds for scoring, terminal error, and limit risk, respectively.

[0080] If a new candidate belongs to an equivalent set, then historical candidates are retained with priority.

[0081] Regarding near-limit progressive output protection: To avoid hard cutoff when a joint approaches its limit, this invention sets an asymptotic output budget based on the distance from the joint to the limit boundary. The limit boundary can be a physical position limit, a soft limit, a velocity limit, an acceleration limit, a torque limit, or a task-defined safety boundary.

[0082] The system divides the joint state into a safe zone, a progressive protection zone, and a strong protection zone: in the safe zone, the joint is allowed to move at a normal speed or incremental budget; in the progressive protection zone, the allowable increment per cycle gradually decreases as the joint approaches the limit boundary; in the strong protection zone, the joint is restricted from continuing to move in the dangerous direction, while retaining the ability to exit in the safe direction.

[0083] Abstract representation:

[0084] in The joint has a limit margin, direction is the candidate motion direction, and state is the joint's historical state or task state. This is an asymptotic protection function, which can be a linear piecewise function, a nonlinear continuous function, or a lookup table function.

[0085] The final output satisfies:

[0086] in These are candidate output joint states. This is the amplitude limiting function.

[0087] This mechanism allows the joint to decelerate smoothly as it approaches the limit boundary, avoiding sudden speed changes and mechanical shocks caused by hard cut-off.

[0088] In piecewise continuous implementation, the first... The limit margin of each joint is The basic single-cycle increment is Δ The minimum safe exit increment is The allowed increment is defined as:

[0089] To distinguish between movement in dangerous directions and movement in safe directions, a direction-related budget is defined:

[0090] in Candidate output directions, For the safety direction gain. When m_j(t) ≤ m_h and u_j(t) points towards the danger side, set Δq_safe,j(t) = 0 to prevent the joint from continuing to move in the danger direction. The final output is:

[0091] in For the first The final output joint angle of each joint.

[0092] If both velocity and acceleration constraints are considered, the following can also be satisfied:

[0093] in and It decreases as the limit margin decreases.

[0094] Regarding system implementation: This invention also provides a multi-candidate inverse kinematics solution and zero-space attitude maintenance system for a redundant robotic arm, comprising: The target input module is used to receive or generate the target pose at the end of the robotic arm, and to perform coordinate transformation, target amplitude limiting, and target state determination. The robotic arm model module is used to obtain the current joint state, provide the robotic arm's positive kinematics, Jacobian matrix, joint limits, reference pose and joint topology information, and calculate the joint limit margin; The candidate seed generation module is used to generate multiple inverse kinematic initial values ​​based on the previous cycle output, reference posture, limit margin, and coupled joint state. The inverse kinematics solution module is used to generate candidate joint solutions based on numerical optimization, quadratic programming, iterative Jacobian or other solution methods. The zero-space attitude holding module is used to apply differentiated attitude holding constraints or biases to redundant degrees of freedom; The candidate solution evaluation module is used to comprehensively score the accuracy, continuity, security, and naturalness of candidate solutions. A stateful arbitration module is used to select a target candidate solution based on historical candidate states, scoring hysteresis, hold-up frames, and stationary guards; The output protection module is used to apply speed, acceleration, incremental or near-limit asymptotic constraints to the target candidate solution.

[0095] The overall calculation process within a control cycle can be summarized as follows: enter:

[0096] Where x_d(t) represents the terminal target state.

[0097] Limit margin and activation coefficient:

[0098] Candidate seeds:

[0099] Inverse kinematics solution: in This represents the function for solving inverse kinematics.

[0100] Overall rating:

[0101] in For time-varying weights, For the first One evaluation item.

[0102] Stateful arbitration:

[0103] in This represents a stateful arbitration function.

[0104] Output protection:

[0105] in This indicates the output protection function.

[0106] The above mathematical model only limits the functional relationships and information flow between modules, and does not limit the specific solver, specific robot model, specific joint number, specific parameter values ​​or specific communication implementation.

[0107] The present invention also provides a redundant robotic arm control system, including a processor, a memory, and a robotic arm control interface, wherein the memory stores a computer program, and the computer program is executed by the processor to implement the above method.

[0108] The present invention also provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the above-described method.

[0109] Compared with existing inverse kinematics methods with a single initial value, the present invention has the following advantages: (1) The present invention constructs multiple inverse kinematic candidate seeds with different configuration tendencies within the same control cycle, which expands the search range of local solutions, reduces the probability of a single initial value falling into an unfavorable configuration, and enables the robotic arm to obtain feasible and ideal joint configurations when the target pose changes rapidly, approaches a singular configuration, or the joint approaches a limit, thereby improving the robustness of the solution.

[0110] (2) By using the limit escape seed and the near limit progressive output protection mechanism, the configuration is actively adjusted before the joint touches the limit, and the deceleration is continuously reduced when approaching the limit, which effectively reduces the risk of the joint touching the limit or hard cut-off and improves the safety of the joint.

[0111] (3) For joint groups with motion coupling or safety correlation, a coordinated bias is generated at the candidate seed level that is far from the limit and returns to the reference posture. This can actively release the motion space of the coupled joints, avoid the coupling joints squeezing each other and exhausting the available space, and improve the coordination of the coupled joints.

[0112] (4) Differentiated zero-space posture maintenance strategy: Different posture maintenance intensity is set according to the functional role, risk level and task contribution of different joints, so that the robotic arm can maintain a more reasonable arm shape and maintain a natural configuration while maintaining operational flexibility.

[0113] (5) The stateful candidate arbitration mechanism effectively reduces the frequent switching between multiple candidate solutions and improves the stability of real-time control output by using minimum hold time, equivalent judgment, hysteresis switching, direction reversal penalty and static guard.

[0114] (6) The number of candidates and the number of solution steps can be dynamically configured according to the computing power budget, which can adapt to the computing resource requirements of different real-time control systems. At the same time, it does not depend on a specific robot model, solver framework or communication interface, and has wide applicability. Attached Figure Description

[0115] Figure 1 This is a diagram showing the overall architecture of the redundant robotic arm multi-candidate inverse kinematics control system of the present invention. Figure 2 This is a flowchart of the multi-candidate seed generation process of the present invention; Figure 3 This is a schematic diagram illustrating the generation of candidate seeds for joint escape in this invention. Figure 4 This is a schematic diagram illustrating the generation of candidate seeds for coupling joint avoidance in this invention. Figure 5 This is a schematic diagram of the differentiated zero-space attitude maintenance module of the present invention; Figure 6 This is a structural diagram of the comprehensive evaluation function for candidate joint solutions in this invention; Figure 7 This is a state machine diagram of a stateful candidate arbitration system according to the present invention; Figure 8 This is a curve diagram showing the near-limit progressive output protection of the present invention. Detailed Implementation

[0116] The present invention will be further described in detail below with reference to the embodiments. The embodiments are only intended to provide a clearer understanding of the technical features, objectives and effects of the present invention.

[0117] refer to Figures 1-8 This invention provides a method for solving multiple candidate inverse kinematics and maintaining zero-space attitude of a redundant robotic arm, including: Acquire the target pose of the robotic arm's end effector, the current joint state, the joint state output from the previous control cycle, and the joint limit information; Multiple inverse kinematics candidate seeds are constructed based on the joint state output from the previous control cycle and the joint limit information. Perform inverse kinematics with differentiated null space attitude preservation constraints based on each candidate seed. Solving this problem yields multiple candidate joint solutions; A comprehensive evaluation is performed on multiple candidate joint solutions, and stateful candidate arbitration is carried out based on the comprehensive evaluation results and historical candidate states to obtain the target candidate joint solution; Perform near-limit progressive output protection on the target candidate joint solution to generate robot arm joint commands.

[0118] In some embodiments, the plurality of inverse kinematics candidate seeds include at least two of the following: continuous seed, reference posture seed, limit escape seed, coupled joint coordination seed, historical stable seed, and task preference seed.

[0119] In some embodiments, when constructing candidate seeds, for a group of coupled joints with kinematic or control coupling relationships, if the limiting margin of the first joint is less than a preset limiting threshold, an offset is applied to the first joint away from the limiting boundary, and an offset is applied to the second joint to return to the reference posture.

[0120] In some embodiments, the inverse kinematics solution includes an end-effector error term and a null-space attitude maintenance term, wherein the null-space attitude maintenance term sets different attitude maintenance strengths based on the mechanical function, range of motion, or risk of limitation of different joints.

[0121] In some embodiments, the comprehensive evaluation includes at least three of the following: end-effector position error, end-effector attitude error, joint continuity cost, reference attitude deviation cost, joint limitation risk, coupled joint risk, and candidate switching cost.

[0122] In some embodiments, stateful candidate arbitration includes: applying a minimum hold time to the currently selected candidate branch; when the scores, terminal errors, and limit risk differences of multiple candidate solutions are all less than the corresponding thresholds, determining the multiple candidate solutions as equivalent candidates and prioritizing the hold of the current branch; and when the terminal target is in a stationary state or the target change rate is less than a preset low-speed threshold, applying an additional penalty to the change of the candidate joint solution relative to the previous output.

[0123] In some embodiments, the near-limit progressive output protection dynamically adjusts the allowable output increment per control cycle based on the distance of each joint from the limit boundary; the closer the joint is to the limit boundary, the smaller the allowable output increment.

[0124] In some embodiments, the near-limit progressive output protection further distinguishes between directions toward and away from the limit boundary based on the candidate motion direction, and assigns a larger permissible output increment to the direction away from the limit boundary.

[0125] This invention also discloses a multi-candidate inverse kinematics solution and zero-space attitude maintenance system for a redundant robotic arm, comprising: The target input module is used to receive or generate the target pose of the robotic arm's end effector. The robotic arm model module is used to obtain the current joint state, provide the robotic arm's positive kinematics, Jacobian matrix, joint limits, reference pose and joint topology information, and calculate the joint limit margin; The candidate seed generation module is used to generate multiple inverse kinematic candidate seeds based on the joint state, limit margin and coupled joint state output in the previous control cycle. The inverse kinematics solution module is used to generate candidate joint solutions based on each candidate seed. The zero-space attitude holding module is used to apply differentiated attitude holding constraints to redundant degrees of freedom; The candidate solution evaluation module is used to give a comprehensive score to each candidate joint solution; The stateful arbitration module is used to determine the target candidate joint solution based on historical candidate states and scoring results; The output protection module is used to apply near-limit asymptotic output constraints to the target candidate joint solution.

[0126] The present invention also discloses a computer-readable storage medium having a computer program stored thereon, wherein the computer program, when executed by a processor, implements the method described above.

[0127] Example 1: Real-time Control of a Universal Redundant Robotic Arm Reference Figure 1 As shown, the redundant robotic arm control system receives the end-effector target pose through the target input module in each control cycle and reads the joint angles, current joint angles, and joint limit margins output in the previous cycle through the robotic arm model module. The controller first constructs multiple candidate seeds through a candidate seed generation module: using the joint state output in the previous control cycle as a continuity seed; constructing a limit escape seed based on the joint limit margin; constructing a posture regression seed based on a preset reference posture; and constructing a coupled joint coordination seed based on the coupled joint state. Each candidate seed is then fed into the inverse kinematics solution module. In this embodiment, the inverse kinematics solution module employs an iterative solution method based on damped least squares. Each candidate seed undergoes several iterations to obtain the corresponding candidate joint solution. During the solution process, the objective function includes an end-effector pose error term and a null space posture preservation term, the latter used to guide the redundant degrees of freedom to approach the reference configuration. Subsequently, the candidate solution evaluation module calculates the comprehensive score of each candidate solution. The evaluation indicators include: end-effector position error, end-effector posture error, joint change relative to the previous output, deviation relative to the reference posture, joint limit risk, and coupled joint risk. If a candidate solution is slightly better in terms of end-point error but has a significant risk of limit or joint jump, its overall score will be higher than that of a safer and more continuous candidate solution. A stateful arbitration module determines the final candidate solution. In this embodiment, the arbitration module sets a minimum hold time (e.g., 5 control cycles) and a hysteresis switching threshold. The current candidate is held first if it is within the minimum hold time and has not failed; switching is only allowed if the overall score of a new candidate is lower than that of the current candidate and the difference exceeds the hysteresis threshold. The output protection module performs near-limit progressive output protection on the target candidate solution, calculates the allowable output increment per control cycle based on the limit margin of each joint, limits the candidate output within the allowable increment range, generates the final joint command, and outputs it to the robotic arm control interface.

[0128] Example 2: Coordinated Escape of Coupled Joint Assembly Reference Figure 3 , 4As shown, the redundant robotic arm contains a coupled joint group consisting of a forearm rotational joint and a wrist rotational joint. The controller monitors the limit margin and mutual compensation space of this coupled joint group through the robotic arm model module. When the forearm rotational joint enters the progressive protection zone, the candidate seed generation module applies a bias away from the limit direction to this joint, while simultaneously applying a bias towards the reference pose to the wrist rotational joint. This bias is incorporated into the construction of the candidate seed, rather than being directly used as the final output. Specifically, the coupled joint coordination bias is generated as follows: First, the limit margin of each joint is calculated. and limit risk activation coefficient Then, regression and escape terms are calculated based on the current state and reference attitude of the coupled joint group; finally, they are combined into a coupled joint coordinated bias. After inverse kinematics solving, the candidate seed yields a candidate solution that still meets the end-effector error requirements but offers a safer coupling joint. In the subsequent comprehensive evaluation, this candidate solution receives a higher score due to its lower coupling joint risk and is thus selected by the stateful arbitration module. Through this mechanism, the present invention proactively adjusts the coupling joint configuration before the joint reaches its limit, avoiding problems such as wrist torsion, forearm rotation saturation, or exhaustion of redundant space in subsequent control cycles.

[0129] Example 3: Differentiated Null Space Attitude Maintenance Reference Figure 5 As shown, the controller categorizes joints into three types based on their mechanical function and kinematic characteristics. Proximal joints (such as the shoulder joint) primarily affect end-effector accessibility over a wide range, and are given a weaker attitude hold strength to preserve operational flexibility. Intermediate joints (such as the elbow joint) primarily affect the overall arm configuration, and are given a moderate attitude hold strength to suppress abrupt changes in arm shape. Distal joints (such as the wrist joint) are prone to entanglement, saturation, or coupling risks, and are given a stronger attitude hold strength to improve configurational stability. In the zero-space attitude hold module, differentiated attitude hold is achieved through a joint-by-joint gain matrix. This is achieved by setting the attitude maintenance gain for each joint corresponding to each diagonal element. The gain values ​​are smaller for proximal joints, moderate for intermediate joints, and larger for distal joints. This division is dynamically adjusted based on the robot arm topology, task stage, or online risk assessment results. For example, when the risk of a joint's limitation increases, the attitude maintenance gain of that joint is temporarily increased to help it return to a safe region more quickly.

[0130] Example 4: Candidate Stable Arbitration in Remote Operation Scenarios Reference Figure 6 , 7 As shown, in teleoperation scenarios, the end-target may be affected by the operator's hand tremors, causing frequent changes in the target's pose within a small range. The stateful arbitration module activates a target stationary guard mechanism: monitoring the end-target's rate of change. When the target rate of change is lower than a preset threshold Approaching 1, Static Guard Penalty The algorithm takes effect by imposing additional penalties on the changes in candidate joint solutions relative to the previous output. Simultaneously, an equivalent candidate determination mechanism is enabled: when the scores, end-effector errors, and limit risk differences of multiple candidate solutions are all less than the corresponding equivalent thresholds, they are determined to be equivalent candidates, and the current historical branch is prioritized. A joint direction reversal penalty is also enabled: for joints prone to causing redundant configuration oscillations, additional penalties are imposed when their motion direction reverses relative to the previous cycle. Through these multiple mechanisms, minor joint jumps are effectively suppressed when the target is at low speed or stationary. Switching only occurs when a new candidate significantly outperforms the current candidate in the overall score, reducing minor jitter of the robotic arm during teleoperation.

[0131] Example 5: Near-limit progressive output protection Reference Figure 8 As shown, the output protection module sets an asymptotic output budget based on the distance from the joint to the limit boundary. The system divides the joint state into three regions: a safe zone and a safe zone. Progressive protected areas Heqiang Nature Reserve ,in For soft protection boundary threshold, To ensure strong protection of boundary thresholds, joints are allowed to increment within the safety zone according to the basic single-cycle increment. Movement; incremental increases are permitted within the progressive protection zone from As the limit margin decreases, it linearly decreases to Only the minimum safe exit increment is allowed within the strong protection zone. Furthermore, movement is only permitted in the safe direction. The output protection module further determines the candidate movement direction: if it points towards the safe side (away from the limit boundary), the allowable increment is multiplied by the gain coefficient. If the direction points to the danger side, the increment remains unchanged. The final output is the joint state output in the previous cycle plus the increment after limiting. Through this progressive protection mechanism, the joint decelerates smoothly when approaching the limit boundary, avoiding sudden speed changes and mechanical shocks caused by hard cutoff.

[0132] Example 6: Modular System Implementation Reference Figure 1As shown, the system modules communicate through standardized data interfaces, and the internal implementation of each module can be adjusted according to specific platforms and requirements. The target input module adapts to different data formats based on different communication interfaces (such as EtherCAT, CANopen, ROS, etc.). The robotic arm model module loads model parameters through a Unified Robot Description Format (URDF) file or directly specifies them through a parameter configuration file. The candidate seed generation module allows configuration of the number, type, and bias coefficients of candidate seeds. The inverse kinematics solution module adopts a plug-in architecture, supporting the switching of solvers according to different application scenarios. The zero-space attitude maintenance module maintains the attitude maintenance gain matrix of each joint and dynamically adjusts it based on limit risk. The candidate solution evaluation module supports online configuration of evaluation metrics and weights. The stateful arbitration module maintains the historical state and maintenance count of candidate branches. The output protection module dynamically calculates the allowable output increment based on the limit margin of each joint.

[0133] The above embodiments fully illustrate the feasibility and technical advantages of the present invention. Those skilled in the art can make various modifications and variations based on the principles of this invention, and all such modifications should be included within the scope of protection of this invention.

Claims

1. A method for solving multiple candidate inverse kinematics and maintaining zero-space attitude of a redundant robotic arm, characterized in that, include: Acquire the target pose of the robotic arm's end effector, the current joint state, the joint state output from the previous control cycle, and the joint limit information; Multiple inverse kinematics candidate seeds are constructed based on the joint state output from the previous control cycle and the joint limit information. Perform inverse kinematics with differentiated null space attitude preservation constraints based on each candidate seed. Solving this problem yields multiple candidate joint solutions; A comprehensive evaluation is performed on multiple candidate joint solutions, and stateful candidate arbitration is carried out based on the comprehensive evaluation results and historical candidate states to obtain the target candidate joint solution; The target candidate joint solution is subjected to near-limit progressive output protection to generate robotic arm joint commands.

2. The method for solving multiple candidate inverse kinematics and maintaining attitude in null space according to claim 1, characterized in that, The plurality of inverse kinematics candidate seeds include at least two of the following: continuous seed, reference posture seed, limit escape seed, coupled joint coordination seed, historical stable seed, and task preference seed.

3. The method for solving multiple candidate inverse kinematics and maintaining attitude in null space according to claim 1, characterized in that, When constructing the candidate seed, for a group of coupled joints with kinematic or control coupling relationship, if the limit margin of the first joint is less than the preset limit threshold, an offset away from the limit boundary is applied to the first joint, and an offset towards the reference posture is applied to the second joint.

4. The method for solving multiple candidate inverse kinematics and maintaining attitude in null space according to claim 1, characterized in that, The inverse kinematics solution includes an end-effector error term and a zero-space attitude maintenance term. The zero-space attitude maintenance term sets different attitude maintenance intensities based on the mechanical function, range of motion, or risk of limitation of different joints.

5. The method for solving multiple candidate inverse kinematics and maintaining attitude in null space according to claim 1, characterized in that, The comprehensive evaluation includes at least three of the following: end-effector position error, end-effector attitude error, joint continuity cost, reference attitude deviation cost, joint limitation risk, coupled joint risk, and candidate switching cost.

6. The method for solving multiple candidate inverse kinematics and maintaining attitude in null space according to claim 1, characterized in that, The stateful candidate arbitration includes: applying a minimum hold time to the currently selected candidate branch; when the scores, terminal errors, and limit risk differences of multiple candidate solutions are all less than the corresponding thresholds, determining the multiple candidate solutions as equivalent candidates and prioritizing the hold time of the current branch; and when the terminal target is in a stationary state or the target change rate is less than a preset low-speed threshold, applying an additional penalty to the change of the candidate joint solution relative to the previous output.

7. The method for solving multiple candidate inverse kinematics and maintaining attitude in null space according to claim 1, characterized in that, The near-limit progressive output protection dynamically adjusts the allowable output increment per control cycle based on the distance of each joint from the limit boundary. The closer the joint is to the limit boundary, the smaller the allowable output increment.

8. The method for solving multiple candidate inverse kinematics and maintaining attitude in null space according to claim 7, characterized in that, The near-limit progressive output protection also distinguishes between directions toward and away from the limit boundary based on the candidate motion direction, and assigns a larger allowable output increment to directions away from the limit boundary.

9. A multi-candidate inverse kinematics solution and zero-space attitude maintenance system for a redundant robotic arm, characterized in that, include: The target input module is used to receive or generate the target pose of the robotic arm's end effector. The robotic arm model module is used to obtain the current joint state, provide the robotic arm's positive kinematics, Jacobian matrix, joint limits, reference pose and joint topology information, and calculate the joint limit margin; The candidate seed generation module is used to generate multiple inverse kinematic candidate seeds based on the joint state, limit margin and coupled joint state output in the previous control cycle. The inverse kinematics solution module is used to generate candidate joint solutions based on each candidate seed. The zero-space attitude holding module is used to apply differentiated attitude holding constraints to redundant degrees of freedom; The candidate solution evaluation module is used to give a comprehensive score to each candidate joint solution; The stateful arbitration module is used to determine the target candidate joint solution based on historical candidate states and scoring results; The output protection module is used to apply near-limit asymptotic output constraints to the target candidate joint solution.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that, When the computer program is executed by a processor, it implements the method as described in any one of claims 1 to 8.

Images