Mechanical arm pose control method, device and equipment

Abstract

The application provides a mechanical arm pose control method, device and equipment, the method comprises the following steps: determining the expected end linear velocity based on the target pose and the current pose corresponding to the mechanical arm, determining the end Jacobian matrix and the singular activation quantity based on the current pose; determining the gain value of each degree of freedom direction according to the singular activation quantity, and performing scaling processing on the expected end linear velocity according to the gain value to obtain the target linear velocity; constructing a multi-layer quadratic programming task, and solving the target linear velocity through the multi-layer quadratic programming task to obtain the joint angle increment in the current control period; determining the target joint angle of the current control period based on the joint angle increment to control the action of the mechanical arm. The application can actively attenuate the speed demand for weakly reachable directions, reduce joint speed mutation and high-frequency jitter, and improve the accuracy and stability of the mechanical arm during the action process.

Description

Technical Field

[0001] This application relates to the field of robot control technology, and in particular to a method, device and equipment for controlling the posture of a robotic arm. Background Technology

[0002] Redundant robotic arms typically have more joint degrees of freedom than the end-effector task dimension. For example, a 7-DOF robotic arm can perform a 6-dimensional end-effector pose task, enabling the widespread application of redundant robotic arms in robots in scenarios such as teleoperation and dual-arm collaboration. Furthermore, by providing a planner to the upper-level planner, the target pose of the end effector is given, and the control system solves for the corresponding joint space commands in real time to control the position of the robotic arm end.

[0003] Related technical solutions typically employ inverse kinematics methods based on the Jacobian matrix to convert end-effector position or pose errors into joint velocities or joint increments, or quadratic programming methods to transform the inverse kinematics problem into a quadratic programming problem with joint velocity constraints and solve iteratively. However, when the robotic arm's movements approach singular configurations, the minimum singular value of the position Jacobian matrix in the Jacobian matrix-based approach approaches zero. If the end-effector target is still forcibly tracked along unreachable or weakly reachable directions, the solution may require large joint velocities to produce small end-effector movements, leading to abrupt joint command changes, jitter, or inaccurate results. Furthermore, in quadratic programming-based approaches, secondary terms may compete with the end-effector position task, causing problems such as large shoulder loops, elbow jumps, end-effector tracking distortion, or unclear task priorities. Therefore, robotic arms in these related technical solutions struggle to execute movements accurately and stably when their end-effectors enter singular neighborhoods. Summary of the Invention

[0004] This application provides a method, device, and equipment for controlling the pose of a robotic arm, which solves the technical problem in related technologies that it is difficult for a robotic arm to perform actions accurately and stably when its end effector enters a singular neighborhood. This solution solves low-priority tasks within the feasible set of high-priority tasks through hierarchical quadratic programming, which can actively reduce the speed requirements for weakly reachable directions, reduce joint speed abrupt changes and high-frequency jitter, and help improve the accuracy and stability of the robotic arm during the action process.

[0005] In a first aspect, this application provides a method for controlling the pose of a robotic arm, the method comprising: Based on the target pose and current pose of the robotic arm, the desired end-effector linear velocity is determined, and based on the current pose, the end-effector Jacobian matrix is ​​determined, along with the singular activations associated with the end-effector Jacobian matrix. The gain value for each degree of freedom is determined based on the singular activation quantity, and the desired end linear velocity is scaled according to the gain value to obtain the target linear velocity. Construct a multi-level quadratic programming task, and obtain the joint angle increment within the current control cycle by solving the target linear velocity through the multi-level quadratic programming task. In the multi-level quadratic programming task, the low-priority task is solved within the feasible set of the high-priority task. Based on the joint angle increment, the target joint angle of the current control cycle is determined to control the robot arm's movements.

[0006] Secondly, this application also provides a robotic arm pose control device, which includes: The singular value decomposition module is configured to determine the desired end-effector linear velocity based on the target pose and current pose of the robotic arm, and to determine the end-effector Jacobian matrix and the singular activations associated with the end-effector Jacobian matrix based on the current pose. The parameter processing module is configured to determine the gain value of each degree of freedom direction based on the singular activation quantity, and to scale the desired end linear velocity according to the gain value to obtain the target linear velocity. The task planning module is configured to construct a multi-level quadratic planning task and obtain the joint angle increment within the current control cycle by solving the target linear velocity through the multi-level quadratic planning task. In the multi-level quadratic planning task, the low-priority task is solved within the feasible set of the high-priority task. The motion control module is configured to determine the target joint angle of the current control cycle based on the joint angle increment in order to control the movement of the robotic arm.

[0007] Thirdly, this application also provides an electronic device comprising: One or more processors; A storage device is provided for storing one or more programs, which, when executed by one or more processors, enable the one or more processors to implement the robotic arm pose control method of this application.

[0008] This application proposes a solution that solves tasks in layers using hierarchical quadratic programming. Low-priority tasks do not disrupt high-priority tasks. Furthermore, by filtering the expected end-effector linear velocity in the task space, singular direction yielding is achieved. This proactively attenuates the velocity requirements for weakly reachable directions, reduces abrupt changes in joint velocity and high-frequency jitter, and improves the stability of the robotic arm during its movements. Attached Figure Description

[0009] Figure 1 This is a schematic diagram of the steps of a robotic arm pose control method provided in an embodiment of this application.

[0010] Figure 2 This is a schematic diagram illustrating the steps for determining a target linear velocity according to an embodiment of this application.

[0011] Figure 3This is a schematic diagram illustrating the steps of smoothing the target joint angle according to an embodiment of this application.

[0012] Figure 4 This is a schematic diagram of the structure of a robotic arm posture control device provided in an embodiment of this application.

[0013] Figure 5 This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. Detailed Implementation

[0014] The embodiments of this application will be further described in detail below with reference to the accompanying drawings and examples. It should be understood that the specific embodiments described herein are merely illustrative of the embodiments of this application and are not intended to limit the scope of this application. Furthermore, it should be noted that, for ease of description, the accompanying drawings only show the parts related to the embodiments of this application, not all structures. Those skilled in the art, after reading this specification, should be able to conceive that any combination of technical features can constitute an optional implementation method, provided that the technical features do not contradict each other.

[0015] The terms "first," "second," etc., used in the specification and claims of this application are used to distinguish similar objects, not to describe a specific order or sequence. It should be understood that such use of data can be interchanged where appropriate so that embodiments of this application can be implemented in orders other than those illustrated or described herein, and the objects distinguished by "first," "second," etc., are generally of the same class, not limited in number; for example, a first object can be one or more. Furthermore, in the specification and claims, "and / or" indicates at least one of the connected objects, and the character " / " generally indicates that the preceding and following objects are in an "or" relationship. In the description of this application, "multiple" means two or more, and "several" means one or more.

[0016] Redundant robotic arms typically possess more joint degrees of freedom than the dimension of the end effector task; for example, a 7-DOF robotic arm can perform a 6-dimensional end effector pose task. This redundancy in joint degrees of freedom brings significant flexibility and safety to the robotic arm's control. Redundant robotic arms offer higher reliability in areas such as obstacle avoidance, singularity avoidance, and joint torque optimization. These advantages have led to the widespread application of redundant robotic arms in robotics. In robot control scenarios such as teleoperation and dual-arm collaboration, the target pose of the end effector is provided by a higher-level planner, and the control system calculates the corresponding joint space commands in real time based on the target pose to determine the position of the robotic arm's end effector.

[0017] Related technologies provide inverse kinematics methods based on damped least squares to solve for joint space commands corresponding to the target pose. These methods calculate the end-effector Jacobian matrix based on the current joint configuration. However, when the robotic arm moves to a near-singular configuration, the minimum singular value of the end-effector Jacobian matrix calculated based on the current joint configuration in the above scheme approaches zero. If the target pose is still forcibly tracked along the unreachable or weakly reachable direction, the solution may require a large joint velocity to produce a small end-effector motion, which may lead to abrupt changes in joint commands, jitter, or inaccurate solution results.

[0018] To address this, this application provides a robotic arm pose control method, which can be applied to one or more redundant robotic arms, and also to robots with redundant robotic arms. Optionally, in the case of a dual-arm robot, the robot's control system can execute the above-described robotic arm pose control method separately for each robotic arm to achieve independent control.

[0019] This application's solution employs hierarchical quadratic programming (HQP) to solve planning tasks hierarchically according to priority. This allows low-priority tasks to be solved within the feasible set of high-priority tasks, thus constructing a hierarchical quadratic programming (HQP) link from the end-effector pose to joint space commands. This enables the robotic arm to stably generate joint space commands even in singular neighborhoods, near joint limitations, with unreachable targets, or with asynchronous dual-arm input. It suppresses singular jitter, shoulder-elbow loops, and abrupt joint velocity changes, and can actively attenuate velocity requirements in weakly reachable directions, reducing abrupt joint velocity changes and high-frequency jitter, thereby improving the accuracy and stability of the robotic arm during operation. The technical solution of this application will be described in detail below through specific embodiments.

[0020] Figure 1 A schematic diagram of the steps of a robotic arm pose control method provided in an embodiment of this application is shown in the figure. The specific steps of the method include: Step S110: Based on the target pose and current pose of the robotic arm, determine the desired end-effector linear velocity, and based on the current pose, determine the end-effector Jacobian matrix and the singular activations associated with the end-effector Jacobian matrix.

[0021] The target pose is the input parameter received by the robot's control system for the end effector, including the target position and target orientation of the end effector. In some embodiments, the target pose can be parameters sent from a teleoperation device, a data acquisition gripper, a VR (Virtual Reality) device, a motion capture device, a teach pendant, or an upper-level planner. Taking the target pose as a parameter sent by a teleoperation device as an example, for a teleoperation device equipped with an end effector, the end effector can be configured with a pose acquisition sensor. For example, in some embodiments, the pose sensor can be one or more signal acquisition devices such as an encoder, an inertial measurement unit, or a camera. For a teleoperation device without an end effector, the end pose of the teleoperation device's robotic arm can be obtained using the angle data of each joint and the robot's forward kinematics method. The end pose of the teleoperation device is sent as the target pose to the robot's control system, thereby enabling the robot to perform motion planning based on the target pose. The target position is the target position of the robotic arm's end effector in the task space, which can be represented by a three-dimensional vector. The task space is the space for the end effector's movement and is also called Cartesian space or operational space. The target pose is used to describe the target state of the end effector of the robotic arm. The target pose can be represented by quaternions, rotation matrices, Euler angles or axis angles.

[0022] The current pose, as the current pose of the robotic arm's end effector within the control cycle, includes the current position and current orientation of the end effector. The current position represents the actual position of the end effector in the task space, and the current orientation represents the actual orientation of the end effector in the task space; both are represented in the same way as the target position and target orientation mentioned above. Specifically, the robot control system can read the angles of each joint of the current robotic arm and calculate the current pose and end effector Jacobian matrix based on the robot's kinematics model. Alternatively, the robot control system can obtain the current pose, solve for the angles of each joint of the current robotic arm based on the robot's inverse kinematics, and then obtain the end effector Jacobian matrix based on the angles of each joint. The end effector Jacobian matrix establishes a linear mapping relationship between joint space velocity and end effector operation space velocity, and it can be represented by the linear velocity Jacobian matrix and the angular velocity Jacobian matrix.

[0023] The desired end-effector velocity is determined based on the target position in the target pose and the current position in the current pose. In some embodiments, the difference between the target position and the current position is calculated to obtain a position error, and the product of the position error and a preset first proportional gain is calculated to obtain the desired end-effector velocity. The corresponding calculation formula is as follows:

[0024] in, v g For the desired terminal linear velocity, Kp For the first proportional gain, e p This is the position error. p g For the target location, p This is the current location.

[0025] In some embodiments, after determining the desired end-effector linear velocity, it is determined whether the amplitude of the desired end-effector linear velocity is greater than the amplitude of a maximum preset maximum end-effector linear velocity, so as to limit the amplitude of the desired end-effector linear velocity to control its magnitude. Here, the maximum end-effector linear velocity is the maximum velocity of the robot's end-effector in a stable tracking state. Furthermore, if the amplitude of the desired end-effector linear velocity is greater than the amplitude of the maximum end-effector linear velocity, the desired end-effector linear velocity is limited to within the maximum end-effector linear velocity. In some embodiments, if the amplitude of the desired end-effector linear velocity is greater than the amplitude of the maximum end-effector linear velocity, the amplitude of the desired end-effector linear velocity is adjusted to the amplitude of the maximum end-effector linear velocity, while the direction of the desired end-effector linear velocity remains unchanged. The corresponding calculation formula is as follows:

[0026] in, v max For the maximum terminal linear velocity, || v g || is the 2-norm of the desired terminal linear velocity.

[0027] Singular activations characterize the strength of singular effects activation. Smaller singular activations indicate weaker activation, greater distance between the robotic arm and the singular point, and smoother movement. Conversely, larger singular activations indicate stronger activation, closer proximity between the robotic arm and the singular point, and less stable movement. Singular activations are determined based on the singular value decomposition (SVD) of the end-effector Jacobian matrix. Referring to the above embodiment, the control system can determine the end-effector Jacobian matrix based on the current pose of the robotic arm. The resulting end-effector Jacobian matrix includes both linear velocity and angular velocity Jacobian matrices. Singular activations are determined based on the SVD of the linear velocity Jacobian matrix. Specifically, by performing SVD on the linear velocity Jacobian matrix, a diagonal singular value matrix can be obtained.

[0028] in, J v ( θ ) is the linear velocity Jacobian matrix. U This serves as the mission space orientation base. V This serves as the joint space orientation base. This is a diagonal singular value matrix, containing several singular values. Each singular value represents the sensitivity of the end effector's response to joint motion in a given degree of freedom direction. The smaller the singular value, the weaker the end effector's response to joint motion in the corresponding degree of freedom direction. When the singular value approaches 0, joint motion in that degree of freedom direction will not change the velocity of the end effector; the motion in that direction is locked, achieving a singular configuration.

[0029] In some embodiments, the minimum and maximum singular values ​​among all singular values ​​included in the diagonal singular value matrix are determined, and the ratio of the minimum to the maximum singular value is calculated to obtain the singular value ratio. The singular activation amount is then determined based on this singular value ratio. The singular value ratio reflects the balance of the robotic arm's end effector's motion capability in all directions, and the singular activation amount determined based on the singular value ratio provides higher accuracy in singularity assessment. The corresponding calculation formula is as follows:

[0030] Where η is the singular value ratio, s min For the minimum singular value, s max ε is the maximum singular value, and η is a positive parameter. η is a dimensionless positional maneuverability index used to describe the balance of translational motion capabilities at the end effector of a robotic arm; ε>0 prevents s... max = 0 indicates a small positive number that is divided by zero. In some embodiments, the same protection against division by zero can also be achieved in engineering implementation by judging the minimum value of η. The closer η is to 1, the more balanced it is in all directions and the further away it is from the position singularity; the closer it is to 0, the weaker its movement capability in at least one direction and the closer it is to the position singularity. In some embodiments, the singularity yield strength is determined by setting two thresholds, such as the first preset threshold η. star Second preset threshold η full η start η is the threshold at which singular yield begins to intervene. full This is the threshold at which the singular yield reaches its maximum strength. It should be understood that η... star and η full This can be determined based on the current historical operational statistics of the robotic arm. When η is greater than or equal to η star When η is less than η, it is considered that no obvious singular neighborhood has been entered; star When η is less than or equal to η, singular yielding is activated; full When this occurs, it is considered to have entered a singular neighborhood. It is understandable that the singular values ​​actually calculated will fluctuate with joint movement and measurement errors, thus causing discontinuity in joint commands. By setting a first preset threshold as the start threshold and a second preset threshold as the full activation threshold, a continuous transition zone can be formed between the two, so that the singular yield strength gradually increases with the degree of singularity.

[0031] The ratio of the first parameter to the second parameter is used as the original activation value. The first parameter is the difference between the first preset threshold and the singular value ratio, and the second parameter is the difference between the first preset threshold and the second preset threshold. The specific calculation formula is as follows:

[0032] Here, a0 is the original activation value obtained by linearly normalizing η, used to represent the yield strength. The original activation value is smoothed according to a preset smoothing function to obtain the singular activation value. This smoothing function restricts the original activation value to [0,1]. It can be understood that the smoothing function avoids on / off transitions near the threshold. The closer the singular activation value is to 1, the closer it is to the singular neighborhood.

[0033] Optionally, in the smoothing process, the smoothing function used is a cubic interpolation polynomial, for example, using the function... This achieves a smooth transition with continuous first derivatives in the [0,1] interval. It should be noted that in some embodiments, the smoothing function used to obtain singular activation values ​​by smoothing the original activation values ​​can also be a linear function, a sigmoid function, a piecewise polynomial function, an exponential function, or a lookup table function, to achieve smoothing of the original activation values.

[0034] In some embodiments, the minimum singular value in the diagonal singular value matrix is ​​used as the singular activation. In other embodiments, metrics related to the degree of singularity, such as operability or condition number, may also be used as the singular activation.

[0035] Step S120: Determine the gain value of each degree of freedom direction based on the singular activation quantity, and scale the desired end linear velocity according to the gain value to obtain the target linear velocity.

[0036] In some embodiments, the desired end-effector linear velocity is filtered to adjust its gain using singular activations. Optionally, the desired end-effector linear velocity is filtered using DLS (Damped Least-Squares) filtering, which introduces a damping factor into the calculation to adjust the velocity magnitude and reduce jitter caused by singularities. In this embodiment, the damping factor is adjusted using singular activations. For example, the singular activations and damping factors are positively correlated; that is, the larger the singular activation, the larger the damping factor, thus adjusting the gain and attenuating the velocity in any direction when approaching a singularity. For instance, in some embodiments, a correspondence table between singular activations and damping factors is pre-established, where each singular activation corresponds to a damping factor. Based on this table, the corresponding damping factor can be determined by looking up the table after determining the singular activation. In some embodiments, other damping gain forms that increase with singularity can be used to determine the target linear velocity, such as adaptive damping, selective damped least squares, task scaling, velocity cone limiting, etc., to achieve filtering of the desired end linear velocity and thus obtain the target linear velocity. For example, when filtering the desired end linear velocity using adaptive damping, the desired end linear velocity is decoupled according to each direction in Cartesian space, and different damping coefficients are independently adjusted for the components in different directions to redetermine the components in different directions and obtain the target linear velocity.

[0037] Step S130: Construct a multi-level quadratic programming task, and use the multi-level quadratic programming task to solve for the target linear velocity to obtain the joint angle increment within the current control cycle.

[0038] The quadratic programming task is constructed based on the quadratic programming mathematical optimization method, which is used to solve the optimization problem of a quadratic objective function with linear constraints. It can be understood that a multi-level quadratic programming task is constructed according to task priority; that is, each level of the quadratic programming task has a corresponding priority. In the multi-level quadratic programming task, low-priority tasks are solved within the feasible set of high-priority tasks. In other words, this scheme uses a hierarchical quadratic programming method to ensure that high-priority tasks are fully satisfied, while low-priority tasks are optimized in the remaining space. That is, the first level of tasks is optimized first, and then the resulting feasible set is used as the constraint condition for the second level of tasks. Subsequent tasks are solved in this manner to ensure that each task is solved according to priority during the solution process. The target linear velocity serves as the input to the multi-level quadratic programming task. The joint angle increment of each joint within the current control cycle is obtained by solving for the target linear velocity through the multi-level quadratic programming task.

[0039] Step S140: Based on the joint angle increment, determine the target joint angle of the current control cycle to control the robot arm's movement.

[0040] The target joint angle is the sum of the current joint angle and the joint angle increment. After determining the target joint angle, joint space control commands corresponding to the target joint angle are generated to control the robot arm's movements. For example, after determining the target joint angle, the system first plans it as a smooth trajectory. This is done by determining the position, velocity, and acceleration boundary conditions of the start and end points, calculating a continuous motion trajectory curve related to time and position, and generating a series of setpoint sequences containing position, velocity, and acceleration commands by sampling the motion trajectory curve. Each point sequence represents the joint angle of each target joint. Then, the required torque is calculated according to the target value in the setpoint sequence and the real-time joint angle deviation fed back at each joint, and converted into joint space control commands to drive the motor. The generated joint space control commands are used to control parameters such as the angle change rate and amount of the robot arm's joints, and are sent to the robot's underlying controller to drive the robot arm to perform actions. This joint space consists of the variables of all robot joints (such as the angle of rotary joints or the displacement of translational joints). Optionally, during the generation of control commands, the target joint angle is also smoothed to further reduce the abnormal effects caused by singular neighborhoods, unreachable targets, or remote operation input jitter, so as to output more continuous control commands in the real-time control cycle.

[0041] As can be seen from the above scheme, this scheme solves the task in layers through hierarchical quadratic programming. Low-priority tasks will not disrupt high-priority tasks. By filtering the expected end linear velocity in the task space, singular direction yielding is achieved, thereby actively attenuating the velocity requirements for weakly reachable directions, reducing joint velocity abrupt changes and high-frequency jitter, and improving the stability of the robotic arm during operation.

[0042] Figure 2 A schematic diagram illustrating the steps for determining a target linear velocity according to an embodiment of this application is shown in the figure. In one embodiment, the desired end linear velocity is filtered to obtain the target linear velocity. The specific steps include: Step S210: Determine the damping coefficient based on the singular activation quantity.

[0043] The damping coefficient is used to adjust the magnitude of the end linear velocity, reducing jitter caused by singularities. A larger damping coefficient results in higher yield strength. In some embodiments, the damping coefficient is the product of the singular activation amount, the maximum singular value, and a preset damping ratio coefficient, calculated using the following formula:

[0044] Where μ is the damping coefficient, a is the singular activation quantity, and K μ s is the preset damping proportional coefficient. maxThe maximum singularity is s. Understandably, when the robotic arm moves away from the singularity, the singular activation value 'a' approaches 0, and the damping coefficient 'μ' also approaches 0; when the robotic arm enters the singular neighborhood, the singular activation value 'a' approaches 1, and the damping coefficient 'μ' increases with the maximum singularity value 's'. max Adaptively increased. Optionally, the singular activation amount can be the singular activation amount determined based on the singularity ratio provided in the above embodiments, or it can be an index related to the degree of singularity, such as minimum singularity, operability, or condition number, as the singular activation amount to determine the damping coefficient.

[0045] Step S220: Determine the gain value in each degree of freedom direction based on the damping coefficient.

[0046] The damping coefficient is used as the damping factor in each degree of freedom direction to generate the corresponding gain value. The corresponding calculation formula is as follows:

[0047] Among them, h i Let s be the gain value in direction i of degree of freedom. i This corresponds to the singular values ​​in direction i of the corresponding degree of freedom. For example, for the linear velocity Jacobian matrix J... v (θ) Performing singular value decomposition yields a task space directional basis U where each column vector corresponds to a degree of freedom direction in the task space. The resulting diagonal singular value matrix is... The singular values ​​corresponding to the column vectors are the singular values ​​in the direction of that degree of freedom.

[0048] It should be understood that the use of s max As a basis for calculating the damping coefficient, this step can be performed so that the degree of velocity decay in each degree of freedom direction mainly depends on the relative singular value. For example, the degree of velocity decay in degree of freedom direction i depends on s. i / s max (Divide both the numerator and denominator of the formula in this step by s) max (can be obtained), not s i The magnitude of the value is adjusted, thereby reducing the dependence on the numerical scale of the singular values ​​themselves and achieving scale normalization of the damping coefficient. The closer the robotic arm is to a singularity in the degree of freedom direction i, the greater the corresponding singular value s. i The smaller the value, the corresponding gain value h i The smaller the value, the stronger the end-effector velocity decay in that direction.

[0049] Step S230: Project the desired end linear velocity onto the mission space direction base to obtain the projection value.

[0050] The mission space orientation basis can be obtained by performing singular value decomposition on the linear velocity Jacobian matrix in the terminal Jacobian matrix. Specifically, by performing singular value decomposition on the linear velocity Jacobian matrix, we can obtain:

[0051] Among them, J v (θ) is the linear velocity Jacobian matrix, U is the task space direction basis, and V is the joint space direction basis. This is a diagonal singular value matrix, containing several singular values. The desired end-effector linear velocity is projected onto the mission space orientation basis. For example, the corresponding projection value can be obtained by calculating the transpose of the mission space orientation basis and the desired end-effector linear velocity, as shown in the following formula:

[0052] Among them, v t Let v be the projection value, U be the task space direction basis, and v be the projection value. g The desired terminal linear velocity.

[0053] Step S240: Scale the projected value according to the gain value in each degree of freedom direction, and then project the scaled projected value back onto the task space to obtain the target linear velocity.

[0054] In some embodiments, a diagonal matrix is ​​constructed using the `diag` function as a linear transformation matrix. This matrix is ​​then used in conjunction with matrix multiplication to achieve anisotropic scaling of the vector or coordinates, which is subsequently projected onto the task space to generate the target linear velocity. The corresponding calculation formula is as follows:

[0055] Among them, v s Let U be the target linear velocity, U be the mission space direction basis, and v be the target linear velocity. g Let be the desired end-effector linear velocity. It can be understood that a singular direction is one in which the usability of the robotic arm's movement in the task space deteriorates. When a direction approaches singularity, the corresponding singularity value s... i As it decreases, the gain value h in that direction decreases. i It also decreases, causing the target linear velocity in that direction to also decrease, meaning the end-velocity tracking requirement in that direction is actively attenuated; and when moving away from the singularity, h i Approaching 1, v s The desired end-effector linear velocity is close to the original value. To address this, this solution filters the desired end-effector linear velocity, actively attenuating the end-effector linear velocity requirements in weakly reachable or inaccessible directions. This effectively reduces the occurrence of sudden changes in joint velocity near singularities at the source, thus contributing to improved stability of the control scheme.

[0056] In some embodiments, a multi-layer quadratic programming task is constructed based on actual task requirements to solve for the joint angle increment within the current control cycle. For example, in one embodiment, the primary requirement for robotic arm control is to ensure that the end effector reaches the target position, followed by ensuring that the robotic arm moves to the target posture, and finally, minimizing joint changes. Based on the above task requirements, a three-layer quadratic programming task is constructed, with the highest priority first-layer task being the end effector position tracking task, the second-layer task being the end effector posture tracking task, and the third-layer task being the joint posture maintenance and minimum joint change task. Specifically, the first-layer task is used to solve for the first feasible set associated with the joint angle increment from the input target linear velocity; the second-layer task is used to solve for the second feasible set associated with the joint angle increment within the current control cycle within the first feasible set; and the third-layer task is used to solve for the joint angle increment within the current control cycle within the second feasible set. The first-layer task, the second-layer task, and the third-layer task are then solved sequentially to determine the joint angle increment within the current control cycle.

[0057] Understandably, in the hierarchical quadratic programming solution process, both the second-level and third-level tasks are optimization tasks lower than the positional tasks. Lower-priority tasks are solved within the feasible set determined by higher-priority tasks, without compromising the feasibility of higher-priority tasks by adding hard constraints. When a second-level or third-level task fails to obtain a valid solution due to numerical errors, constraint conflicts, solution timeouts, or solver malfunctions, the control system abandons the solution result for that level and reverts to a valid solution from the previous priority level. Specifically, if a second-level task fails, the solution result of the first-level task is used; if a third-level task fails, the valid solution result of the second-level task is used; or if both the second-level and third-level tasks fail, the solution result of the first-level task is used.

[0058] It should be noted that HQP can be solved using solvers such as HPIPM, OSQP, qpOASES, and ProxQP, and then deployed into the robot's control system.

[0059] In one embodiment, the first layer of the multi-layer quadratic programming task is an end-effector position tracking task, and joint angle constraints and joint velocity constraints are set in the first layer task. The first layer task is the highest priority task, and joint angle constraints and joint velocity constraints are added to the highest priority task. The joint angle constraints are hard constraints, and the joint velocity constraints are soft constraints. The joint angle constraints correspond to physical boundaries that the robot hardware cannot overcome, preventing safety issues for the robotic arm. The joint velocity constraints correspond to the maximum rotational speed of the robotic arm joint motors, preventing motion spatiotemporal disturbances when the joint velocity exceeds a threshold. The joint angle constraints are used to determine the first target range of joint angle changes, and the joint velocity constraints are used to determine the second target range of joint velocity changes. Specifically, the joint angle constraints can be expressed by the following formula:

[0060] Where θ is the current joint angle, θ min For the minimum angle, θ max For the maximum angle, θ represents the joint angle increment. It can be understood that the current joint angle θ includes the angles of multiple joints, such as θ = [θ1, θ2, ..., θ]. i This constraint can limit the joint angle; for the current joint θ i If the current joint has already exceeded the boundary, the constraint only allows the joint to move in the direction of returning to the first target interval, for example, when θ i Less than θ min At that time, it was required θ is greater than or equal to 0; when θ i Greater than θ max At that time, it was required θ is less than or equal to 0. In extreme cases, if the upper and lower bounds conflict, it degenerates into a zero-step constraint.

[0061] Accordingly, the joint velocity constraint can be expressed by the following formula:

[0062] Among them, v θ,min and v θ,max s represents the lower and upper limits of joint velocity. v,minus and s v,plus The relaxation amount is non-negative. Furthermore, the joint position constraints are set as hard constraints, and the joint velocity constraints as soft constraints. Then, in the quadratic planning task performed by the first-level task, the target linear velocity is converted into the desired position increment. For example, by calculating the product of the target linear velocity and the control period, the filtered target linear velocity is converted into the desired position increment within one control period. The corresponding calculation formula is as follows:

[0063] Among them, y p v is the desired position increment. s For the target linear velocity, t represents the duration of the control cycle. Furthermore, based on the desired position increment, velocity soft constraint relaxation variables, relaxation variable penalty weights, and the linear velocity Jacobian matrix in the terminal Jacobian matrix, a quadratic programming function associated with the joint angle increment is determined and used as the first objective function. The velocity soft constraint relaxation variables are defined by the aforementioned non-negative relaxation amount s. v,minus and s v,plus The slack variable penalty weights are used as weight values ​​and combined with the velocity soft constraint slack variables to form a penalty term to constrain the joint angle increment. Furthermore, another parameter term in the first objective function is constructed using the expected position increment and the linear velocity Jacobian matrix, thereby obtaining the first feasible set by minimizing the constructed first objective function. Accordingly, the quadratic programming function for the first-level task can be written as:

[0064] Among them, s v J is a slack variable with a soft constraint on velocity. v (θ) is the Jacobian matrix of linear velocity. θ is the joint angle increment, w v The slack variable is penalized with weights. Then, given the constraints of joint angle and joint velocity, the first objective function is solved to obtain the first feasible set corresponding to the joint angle increment. For example, the first feasible set can be obtained by a solver based on the effective set method. Specifically, the solver for the quadratic programming problem based on the effective set method identifies and utilizes the constraints at the current iteration point to transform the original quadratic programming problem with inequality constraints into a quadratic programming problem with equality constraints. Specifically, during the iteration process, starting from an initial feasible solution, the optimal point is gradually approached by continuously updating the effective set. In each iteration, the condition at the current iteration point is checked; if not, another improvement direction is determined and a new feasible solution is searched. At the new feasible solution, the effective constraints are re-identified and the effective set is updated. This process is repeated to obtain all feasible solutions, ultimately yielding an effective set containing all feasible solutions, which serves as the first feasible set. It should be noted that in some embodiments, the solution can also be based on the interior point method to obtain the first feasible set.

[0065] In summary, the control system prioritizes end-effector position tracking as the highest-priority first-level task. Within this first-level task, it introduces joint angle constraints as hard constraints and joint velocity constraints as soft constraints, and constructs corresponding quadratic programming functions. This ensures that the joints do not cross physical position boundaries during the solution of the feasible set, while reducing the risk of no solution due to velocity constraints. This helps improve the accuracy and stability of the robotic arm control.

[0066] In one embodiment, the second-layer task constructed in the multi-level quadratic programming task is the end-effector attitude tracking task. This second-layer task is the second-highest priority task, solved within the feasible set of the first-layer task. This is to avoid the end-effector attitude tracking task disrupting the priority of the end-effector position task; that is, when attitude tracking and position tracking conflict, the control system prioritizes ensuring the optimality of the first-layer task. In the second-layer task, based on the target attitude in the target pose and the current attitude in the current pose, the three-dimensional attitude error is determined, and based on the three-dimensional attitude error and a preset second proportional gain, the desired angular velocity is determined. For example, if the attitude is represented by a rotation matrix, the three-dimensional attitude error can be obtained through Lie algebra-logarithmic mapping. Lie algebra-logarithmic mapping is the inverse operation of exponential mapping, used to map elements in a Lie group (such as rotation matrices or transformation matrices) back to the corresponding Lie algebra space (vectors or antisymmetric matrices). That is, when the attitude is represented in rotation matrix form, the antisymmetric matrix obtained by logarithmic mapping is converted into a three-dimensional vector. Understandably, the relative rotation of the target attitude with respect to the current attitude in the current end-effector coordinate system can be transformed into an angular velocity vector conforming to the kinematic Jacobian definition through mapping, thus enabling the controller to calculate the effective joint redundancy decomposition torque. Specifically, the product of the transpose matrix of the current attitude and the target attitude is used as the argument in the logarithmic function, and the corresponding calculation formula is as follows:

[0067] Among them, e r R is the 3D attitude error, where R is the current attitude. g The target attitude is defined as follows. Optionally, if the attitude is represented by quaternions, the relative quaternions between the target attitude and the current attitude can be calculated first, and then converted into an equivalent axis-angle error vector to obtain the three-dimensional attitude error. Furthermore, the product of the three-dimensional attitude error and the second proportional gain is used as the desired angular velocity, and the corresponding calculation formula is as follows:

[0068] Among them, w g For the desired angular velocity, K rThis is the second proportional gain. In some embodiments, after determining the desired angular velocity, it can also be determined whether the desired angular velocity is greater than the maximum angular velocity, so that if the desired angular velocity is greater than the maximum angular velocity, the desired angular velocity is norm-limited, wherein the limiting method for the desired angular velocity is the same as the limiting method for the desired end-velocity.

[0069] Furthermore, based on the desired angular velocity and the angular velocity Jacobian matrix in the end-effector Jacobian matrix, a quadratic programming function related to the joint angle increment is determined and used as the second objective function. For example, in the constructed quadratic programming function, the desired angular velocity increment obtained by multiplying the desired angular velocity by the control period is used as one parameter, and the product of the angular velocity Jacobian matrix and the joint angle increment is used as another parameter. Specifically, the quadratic programming function for the second-level task can be written as:

[0070]

[0071] Among them, J w (θ) is the Jacobian matrix of angular velocity. θ is the joint angle increment, y r The desired angular velocity increment is defined as follows: Furthermore, given the constraint condition of the first feasible set, the second objective function is solved to obtain the second feasible set corresponding to the joint angle increment. For example, the second feasible set can be obtained by solving the problem using the effective set method, or it can be obtained using the interior point method. It should be noted that the method for solving the second objective function is the same as that for solving the first objective function; the specific process can be referred to in the above embodiments. However, the solution process differs from that of the first-level task in that the first feasible set obtained from the first-level task is used as the constraint condition in the solution of the second-level task. Therefore, the control system of this scheme treats the attitude tracking task as the second-highest priority task, and while ensuring the priority of position tracking, it can maximize the use of redundant joints to achieve rotation without interfering with the accuracy of position tracking, thus helping to ensure the accuracy and stability of the robotic arm control.

[0072] In one embodiment, the third-layer task constructed in the multi-layer quadratic programming task is a joint attitude preservation and minimum joint change task. This third-layer task is a low-priority task, solved within the feasible set of the second-layer task. It utilizes redundant degrees of freedom to propel redundant joints to more favorable configurations in singular neighborhoods without disrupting the upper-layer task, and without interfering with the end-effector tracking task. The joint attitude error is calculated in the third-layer task, specifically based on the current joint angle and the desired attitude in the preset reference configuration. The preset reference configuration can be the median configuration of each joint's range of motion, the initial safe configuration, or a user-specified configuration. For example, the median configuration can be used as the preset reference configuration, and the difference between the current joint angle and the desired attitude can be used as the joint attitude error. Furthermore, the desired joint angle increment is determined based on the joint attitude error and a preset third proportional gain; for example, the product of the joint attitude error and the third proportional gain can be used as the desired joint angle increment. The corresponding calculation formula is as follows:

[0073]

[0074] Among them, e j θ represents the joint posture error. ref Let θ be the desired posture, θ be the current joint angle, and K be the desired posture. j This is the third proportional gain. t is the duration of the control cycle. θ ref The desired joint angle increment is defined as follows. Then, based on the desired joint angle increment and the joint change regularization term, a quadratic programming function associated with the joint angle increment is determined and used as the third objective function. The joint change regularization term is associated with the joint angle increment obtained in the previous control cycle. It can be understood that the joint change regularization term is added to the third-level task to reduce jumps in the solution results between adjacent cycles. For example, the joint change regularization term is the product of the squared value of the joint angle increment minus the norm of the joint angle increment obtained in the previous control cycle, and the regularization coefficient. The corresponding expression is as follows:

[0075] Where ρ is the regularity coefficient. θ is the joint angle increment. θ prev The joint angle increment is obtained from the previous control cycle. Furthermore, the joint change regularization term is used as a parameter in the third objective function, and the difference between the joint angle increment and the desired joint angle increment is used as another parameter. Based on this, the quadratic programming function for the third-layer task can be written as:

[0076] Furthermore, given the constraint condition of the second feasible set, the third objective function is solved to obtain the desired joint angle increment. For example, the joint angle increment within the current control cycle can be obtained by solving the effective set method using a solver, or it can be solved using the interior point method. It should be noted that the method for solving the third objective function is the same as that for solving the second objective function, and the specific process can be referred to the above embodiment. However, the solution process differs from that of the second-level task in that the second feasible set obtained from the second-level task is used as the constraint condition in the solution of the third-level task. Therefore, this scheme treats the joint posture maintenance and minimum joint change tasks as low-priority tasks, thereby controlling the joint movement within the remaining degrees of freedom without affecting the first two levels of tasks, which helps to improve the stability of the robotic arm's movements.

[0077] Optionally, during the construction of the third objective function, it is determined whether the singular activation value is greater than a third preset threshold. If the singular activation value is greater than the third preset threshold, a bias is added to the third objective function, whereby the bias is used to adjust the value of the desired joint angle increment. For example, this bias is used as the amount to be subtracted from the desired joint angle increment, thereby updating the constructed third objective function. Optionally, the determined bias is associated with the target direction of the redundant joint moving away from the unfavorable configuration. This target direction can be determined based on the joint midpoint, elbow height, operability gradient, or a preset safe posture for amplitude limiting. When constructing the bias, it can be determined based on the singular activation value, a preset fourth proportional gain, and the target direction. The calculation formula for the constructed bias is as follows:

[0078] in, θ escape Here, 'a' is the bias, 'a' is the singular activation, and 'K' is the 't' e For the fourth proportional gain, d escape For the target direction, t represents the duration of the control period. Therefore, the constructed third objective function is updated as follows:

[0079] In response, this solution adds a bias when the singular activation amount is greater than the third preset threshold, using the bias as the corresponding escape bias. Furthermore, it is added only in the third-level task and not in the first-level task, which enables the control system to push redundant joints to move to a more favorable configuration in the singular neighborhood without directly disrupting the end-position tracking task.

[0080] Figure 3The figure illustrates the steps for smoothing target joint angles according to an embodiment of this application. As shown, this solution maintains a short-time look-ahead target queue to estimate the target velocity for each joint, and calculates the smoothed joint position based on velocity, acceleration, and jerk constraints (i.e., jerk constraints). Specific steps include: Step S310: Add the target joint angle to the preset queue.

[0081] The parameters in the preset queue include target joint angles within several control cycles. This preset queue serves as a short-term look-ahead target queue, and optionally, the window range of the preset queue can be set based on actual application needs. The added target joint angles are determined based on the solved joint angle increments; for example, the sum of the joint angle increment and the current joint angle can be used as the target joint angle. The target joint angles within each control cycle can be arranged in chronological order in the preset queue, with the target joint angle of the current control cycle being the last value in the queue. It should be noted that in some embodiments, the target joint angles can be preprocessed before being added to the preset queue. For example, the target joint angle of the current control cycle can be compared with the target joint angle of the previous control cycle to obtain the corresponding difference. If the difference is less than a fourth preset threshold, the target joint angle of the previous control cycle is used as the target joint angle of the current control cycle.

[0082] Step S320: Determine motion trend parameters based on the target joint angles within several control cycles.

[0083] The motion trend parameters include target velocity and desired acceleration used to adjust joint angles. Several parameters exist in a preset queue. The motion trend parameters represent the trend of joint angle changes, and the target velocity is the rate of change of the joint angle. When calculating the motion trend parameters, some parameters in the preset queue can be selected for calculation. For example, the target velocity can be estimated based on the difference between the angles of the first and last target joints in the queue; that is, the ratio of this difference to the time interval between the two parameters is used as the target velocity to achieve speed limiting. Furthermore, the desired acceleration is calculated through PD control. For example, a preset first proportional value is calculated as the product of the target position error (the difference between the selected first and last target joint angles), and a preset second proportional value is calculated as the product of the target velocity error (the difference between the target velocity and the initial velocity of the joint movement, such as selecting the joint velocity determined in the previous control cycle as the initial velocity). The sum of these two products is used as the desired acceleration.

[0084] Step S330: Perform acceleration limiting processing and jerk limiting processing on the desired acceleration in sequence to obtain a limited acceleration.

[0085] The acceleration limiting process is used to restrict the desired acceleration within a preset range to achieve amplitude limiting. Specifically, if the desired acceleration is greater than the upper limit of the preset range, the desired acceleration is set to the upper limit; if the desired acceleration is less than the lower limit of the preset range, the desired acceleration is set to the lower limit. Jerk limiting is used to restrict changes in the desired acceleration to prevent abrupt changes. For example, by comparing the desired acceleration with the desired acceleration determined in the previous control cycle, this difference is limited to another preset range. For instance, if the difference is greater than the upper limit of this other preset range, the desired acceleration is set to the sum of the desired acceleration determined in the previous control cycle and the upper limit; if the difference is less than the lower limit of this other preset range, the desired acceleration is set to the sum of the desired acceleration determined in the previous control cycle and the lower limit. The limited acceleration is obtained after the above amplitude limiting process. It is conceivable that if the desired acceleration is within the above two preset ranges, then this desired acceleration is the limited acceleration.

[0086] Step S340: Integrate the restricted acceleration to obtain the joint velocity and the smoothed joint angle, so as to control the movement of the robotic arm.

[0087] The restricted acceleration is integrated to obtain its integral value over time. This integral value is then added to the initial velocity to obtain the joint velocity. Furthermore, the joint velocity is integrated again to obtain a new joint angle, based on the joint angle value adjusted in the previous control cycle. This new joint angle is then used as the target angle to control the robotic arm's movement. Therefore, this scheme, by smoothing the target joint angle, can further reduce joint command spikes caused by singular neighborhoods, unreachable targets, or teleoperation input jitter.

[0088] Optionally, after determining the joint angle, it is further determined whether the joint angle meets the requirements by using joint angle constraints. These constraints are introduced in the constructed quadratic programming task and include a first target interval corresponding to the joint angle change. If the smoothed joint angle is outside the first target interval corresponding to the joint angle constraints, the joint velocity and its corresponding acceleration in the motion parameters are set to zero to prevent further joint movement. Furthermore, if the target velocity is less than a first preset value and the difference between the joint angle and the already moved angle is less than a second preset value, the joint velocity and its corresponding acceleration in the motion parameters are also set to zero. It can be understood that the first and second preset values ​​serve as smaller thresholds to determine whether the joint is approaching the target position. When the target velocity is less than the first preset value and the difference between the joint angle and the already moved angle is less than the second preset value, it can be determined that the joint is approaching the target position, and no further movement is performed to avoid frequent shaking of the joint when approaching the target position.

[0089] Figure 4 This is a schematic diagram of the structure of a robotic arm pose control device provided in an embodiment of this application. It is used to execute the redundant robotic arm pose control method provided in the above embodiment, and has the corresponding functional modules and beneficial effects of the execution method. As shown in the figure, the device includes a singular value decomposition module 401, a parameter processing module 402, a task planning module 403, and a motion control module 404.

[0090] The singular value decomposition module 401 is configured to determine the desired end-effector linear velocity based on the target pose and current pose of the robotic arm, and to determine the end-effector Jacobian matrix and the singular activations associated with the end-effector Jacobian matrix based on the current pose. The parameter processing module 402 is configured to determine the gain value of each degree of freedom direction based on the singular activation quantity, and to scale the desired end linear velocity based on the gain value to obtain the target linear velocity. The task planning module 403 is configured to construct a multi-level quadratic planning task and obtain the joint angle increment within the current control cycle by solving the target linear velocity through the multi-level quadratic planning task. In the multi-level quadratic planning task, the low-priority task is solved within the feasible set of the high-priority task. The motion control module 404 is configured to determine the target joint angle of the current control cycle based on the joint angle increment in order to control the movement of the robotic arm.

[0091] To address this, this solution uses a singular value decomposition module to process the target pose and the current pose to obtain the desired end-effector linear velocity and singular activations. The parameter processing module filters the desired end-effector linear velocity, and the task planning module implements hierarchical quadratic programming to solve the task in layers. This proactively reduces the velocity requirements for weakly reachable directions, minimizes joint velocity abrupt changes and high-frequency jitter, and improves the stability of the robotic arm during movement.

[0092] Based on the above embodiments, the singular value decomposition module 401 is specifically configured as follows: Based on the target end-effector position in the target pose and the current end-effector position in the current pose, determine the position error amount, and based on the position error amount and the preset first proportional gain, determine the desired end-effector linear velocity. When the norm of the desired end velocity is greater than the maximum end velocity, the desired end velocity is subject to norm limiting to keep it within the maximum end velocity.

[0093] Based on the above embodiments, the singular value decomposition module 401 is further configured as follows: Singular value decomposition is performed on the linear velocity Jacobian matrix in the terminal Jacobian matrix to obtain the corresponding singular value ratio, which is determined based on the singular values ​​in the linear velocity Jacobian matrix. The ratio of the first parameter to the second parameter is used as the original activation value, where the first parameter is the difference between the first preset threshold and the singular value ratio, and the second parameter is the difference between the first preset threshold and the second preset threshold. The original activation values ​​are smoothed using a preset smoothing function to obtain singular activation values.

[0094] Based on the above embodiments, the singular value decomposition module 401 is further configured as follows: Determining the damping coefficient based on singular activations; Based on the damping coefficient, the gain value in each degree of freedom direction is determined; Project the desired end-effector linear velocity onto the mission space orientation base to obtain the projection value; The projected value is scaled according to the gain value in each degree of freedom direction, and then the scaled projected value is projected back onto the task space to obtain the target linear velocity.

[0095] Based on the above embodiments, the task planning module 403 is specifically configured as follows: The first, second, and third level tasks are constructed in descending order of priority. The first level task is used to solve for the target linear velocity input to obtain the first feasible set associated with the joint angle increment. The second level task is used to solve for the second feasible set associated with the joint angle increment in the current control cycle within the first feasible set. The third level task is used to solve for the joint angle increment in the current control cycle within the second feasible set. Solve the first, second, and third level tasks sequentially to determine the joint angle increments within the current control cycle.

[0096] Based on the above embodiments, joint angle constraints and joint velocity constraints are set in the first-level task. The joint angle constraints are used to determine the first target range of joint angle changes, and the joint velocity constraints are used to determine the second target range of joint velocity changes. Task planning module 403 is configured as follows: Convert the target linear velocity into the desired position increment; Based on the expected position increment, the preset velocity soft constraint relaxation variable, the preset relaxation variable penalty weight, and the linear velocity Jacobian matrix in the end Jacobian matrix, a quadratic programming function associated with the joint angle increment is determined and used as the first objective function. Solving the first objective function under joint angle and joint velocity constraints yields the first feasible set corresponding to the joint angle increment.

[0097] Based on the above embodiments, the task planning module 403 is configured as follows: Based on the target pose in the target pose and the current pose in the current pose, the three-dimensional pose error is determined, and based on the three-dimensional pose error and the preset second proportional gain, the desired angular velocity is determined. Based on the desired angular velocity and the angular velocity Jacobian matrix in the end-effector Jacobian matrix, a quadratic programming function associated with the joint angle increment is determined and used as the second objective function. Given the constraints of the first feasible set, the second objective function is solved to obtain the second feasible set corresponding to the joint angle increment.

[0098] Based on the above embodiments, the task planning module 403 is configured as follows: Based on the current joint angle and the desired posture in the preset reference configuration, determine the joint posture error, and based on the joint posture error and the preset third proportional gain, determine the desired joint angle increment. Based on the expected joint angle increment and the joint change regularization term, a quadratic programming function associated with the joint angle increment is determined and used as the third objective function. The joint change regularization term is associated with the joint angle increment obtained from the previous control cycle. Given the second feasible set of constraints, the third objective function is solved to obtain the desired joint angle increment.

[0099] Based on the above embodiments, the task planning module 403 is further configured as follows: If the singular activation amount is greater than the third preset threshold, an offset is added to the third objective function. The offset is used to adjust the value of the desired joint angle increment.

[0100] Based on the above embodiments, the motion control module 404 is specifically configured as follows: Add the target joint angle to the preset queue. The parameters in the preset queue include the target joint angle within several control cycles. Based on the target joint angle within several control cycles, motion trend parameters are determined, including the target velocity and desired acceleration used to adjust the joint angle. The desired acceleration is sequentially subjected to acceleration limiting and jerk limiting processes to obtain a constrained acceleration. The limited acceleration is integrated to obtain the joint velocity and the smoothed joint angle, which are then used to control the robotic arm's movements.

[0101] Based on the above embodiments, the motion control module 404 is further configured as follows: If the smoothed joint angle is outside the first target interval corresponding to the joint angle constraint, the joint velocity and the acceleration corresponding to the joint velocity in the motion parameters are set to zero. The joint angle constraint is the constraint introduced in the constructed quadratic programming task. If the target speed is less than the first preset value and the difference between the joint angle and the angle that has been moved is less than the second preset value, the joint speed and the acceleration corresponding to the joint speed in the motion parameters are set to zero.

[0102] It is worth noting that in the above embodiments, the functional modules are divided according to functional logic, but are not limited to the above division, as long as the corresponding functions can be achieved; in addition, the specific names of each functional module are only for easy differentiation and are not used to limit the scope of protection of this application.

[0103] Figure 5This is a schematic diagram of the structure of an electronic device provided in an embodiment of this application. It is used to execute the robotic arm pose control method provided in the above embodiment, and has corresponding functional modules and beneficial effects for executing the method. As shown in the figure, the device includes a processor 501, a memory 502, an input device 503, and an output device 504. The number of processors 501 can be one or more; the figure shows one processor 501 as an example. The processor 501, memory 502, input device 503, and output device 504 can be connected via a bus or other means; the figure shows a connection via a bus as an example. The memory 502, as a computer-readable storage medium, can be used to store software programs, computer-executable programs, and modules, such as the program instructions / modules corresponding to the robotic arm pose control method in the embodiments of this application. The processor 501 executes various corresponding functional applications and data processing by running the software programs, instructions, and modules stored in the memory 502, thereby realizing the above-mentioned robotic arm pose control method.

[0104] The memory 502 may primarily include a program storage area and a data storage area. The program storage area may store the operating system and at least one application program required for a given function; the data storage area may store data recorded or created during use. Furthermore, the memory 502 may include high-speed random access memory and non-volatile memory, such as at least one disk storage device, flash memory device, or other non-volatile solid-state storage device. In some embodiments, the memory 502 may further include a memory remotely located relative to the processor 501, which can be connected to a server via a network. Examples of such networks include, but are not limited to, the Internet, intranets, local area networks, mobile communication networks, and combinations thereof.

[0105] The input device 503 can be used to input corresponding digital or character information to the processor 501, and to generate key signal inputs related to the user settings and function control of the device; the output device 504 can be used to send or display key signal outputs related to the user settings and function control of the device.

[0106] This application also provides a storage medium storing computer-executable instructions, which, when executed by a processor, are used to perform related operations in the robotic arm pose control method provided in any embodiment of this application.

[0107] Computer-readable storage media include both permanent and non-permanent, removable and non-removable media, and information storage can be achieved by any method or technology. Information can be computer-readable instructions, data structures, program modules, or other data. Examples of computer storage media include, but are not limited to, phase-change memory (PRAM), static random access memory (SRAM), dynamic random access memory (DRAM), other types of random access memory (RAM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), flash memory or other memory technologies, CD-ROM, digital versatile optical disc (DVD) or other optical storage, magnetic tape, disk storage or other magnetic storage devices, or any other non-transfer medium that can be used to store information accessible by a computing device.

[0108] It should also be noted that the terms "comprising," "including," or any other variations thereof are intended to cover non-exclusive inclusion, such that a process, method, article, or apparatus that comprises a list of elements includes not only those elements but also other elements not expressly listed, or elements inherent to such process, method, article, or apparatus. Unless otherwise specified, an element defined by the phrase "comprising one..." does not exclude the presence of other identical elements in the process, method, article, or apparatus that includes that element.

[0109] Note that the above are merely preferred embodiments and the technical principles employed in this application. Those skilled in the art will understand that this application is not limited to the specific embodiments described herein, and various obvious changes, readjustments, and substitutions can be made without departing from the scope of protection of this application. Therefore, although this application has been described in detail through the above embodiments, this application is not limited to the above embodiments, and may include many other equivalent embodiments without departing from the concept of this application, the scope of which is determined by the scope of the appended claims.

Claims

1. A method for controlling the pose of a robotic arm, characterized in that, The method includes: Based on the target pose and current pose of the robotic arm, the desired end-effector linear velocity is determined, and based on the current pose, the end-effector Jacobian matrix is ​​determined and the singular activations associated with the end-effector Jacobian matrix are determined. The gain value for each degree of freedom direction is determined based on the singular activation amount, and the desired end linear velocity is scaled based on the gain value to obtain the target linear velocity. A multi-level quadratic programming task is constructed, and the joint angle increment within the current control cycle is obtained by solving the target linear velocity through the multi-level quadratic programming task. In the multi-level quadratic programming task, the low-priority task is solved within the feasible set of the high-priority task. Based on the joint angle increment, the target joint angle of the current control cycle is determined to control the movement of the robotic arm; The step of constructing a multi-level quadratic programming task and obtaining the joint angle increment within the current control cycle by solving the target linear velocity through the multi-level quadratic programming task includes: The first-level task, the second-level task, and the third-level task are constructed in descending order of priority. The first-level task is used to solve for the target linear velocity input to obtain a first feasible set associated with the joint angle increment. The second-level task is used to solve for the second feasible set associated with the joint angle increment in the current control cycle within the first feasible set. The third-level task is used to solve for the joint angle increment in the current control cycle within the second feasible set. The first layer task, the second layer task, and the third layer task are solved sequentially to determine the joint angle increment within the current control cycle; Furthermore, joint angle constraints and joint velocity constraints are set in the first layer of tasks. The joint angle constraints are used to determine the first target range of joint angle changes, and the joint velocity constraints are used to determine the second target range of joint velocity changes. The solution process for the first-level task includes: Convert the target linear velocity into the desired position increment; Based on the desired position increment, the preset velocity soft constraint relaxation variable, the preset relaxation variable penalty weight, and the linear velocity Jacobian matrix in the end Jacobian matrix, a quadratic programming function associated with the joint angle increment is determined and used as the first objective function. Solving the first objective function under the joint angle constraint and the joint velocity constraint yields the first feasible set corresponding to the joint angle increment.

2. The robotic arm pose control method according to claim 1, characterized in that, The determination of the singular activations associated with the terminal Jacobian matrix includes: Singular value decomposition is performed on the linear velocity Jacobian matrix in the terminal Jacobian matrix to obtain the corresponding singular value ratio, which is determined based on the singular values ​​in the linear velocity Jacobian matrix. The ratio of the first parameter to the second parameter is used as the original activation value, wherein the first parameter is the difference between the first preset threshold and the singular value ratio, and the second parameter is the difference between the first preset threshold and the second preset threshold. The singular activation amount is obtained by smoothing the original activation amount according to a preset smoothing function.

3. The robotic arm pose control method according to claim 1 or 2, characterized in that, The step of determining the gain value for each degree of freedom direction based on the singular activation amount, and scaling the desired end-effector linear velocity according to the gain value to obtain the target linear velocity, includes: The damping coefficient is determined based on the singular activation amount; Based on the damping coefficient, the gain value in each degree of freedom direction is determined; The desired end-effector linear velocity is projected onto the mission space orientation base to obtain the projection value; The projected value is scaled according to the gain value in each degree of freedom direction, and the scaled projected value is then projected back onto the task space to obtain the target linear velocity.

4. The robotic arm pose control method according to claim 1, characterized in that, The solution process for the second-level task includes: Based on the target pose in the target pose and the current pose in the current pose, the three-dimensional pose error is determined, and based on the three-dimensional pose error and the preset second proportional gain, the desired angular velocity is determined. Based on the desired angular velocity and the angular velocity Jacobian matrix in the end Jacobian matrix, a quadratic programming function associated with the joint angle increment is determined and used as the second objective function; Given the constraints of the first feasible set, the second objective function is solved to obtain the second feasible set corresponding to the joint angle increment.

5. The robotic arm pose control method according to claim 1, characterized in that, The solution process for the third-layer task includes: Based on the current joint angle and the desired posture in the preset reference configuration, the joint posture error is determined, and based on the joint posture error and the preset third proportional gain, the desired joint angle increment is determined. Based on the expected joint angle increment and the joint change regularization term, a quadratic programming function associated with the joint angle increment is determined and used as the third objective function. The joint change regularization term is associated with the joint angle increment obtained in the previous control cycle. Given the second feasible set of constraints, the third objective function is solved to obtain the desired joint angle increment.

6. The robotic arm pose control method according to claim 5, characterized in that, Before determining the quadratic programming function associated with the joint angle increment based on the desired joint angle increment and the joint change regularization term, and using it as the third objective function, the method further includes: If the singular activation amount is greater than a third preset threshold, an offset is added to the third objective function, and the offset is used to adjust the value of the desired joint angle increment.

7. A robotic arm posture control device, characterized in that, The device includes: The singular value decomposition module is configured to determine the desired end-effector linear velocity based on the target pose and current pose of the robotic arm, and to determine the end-effector Jacobian matrix and the singular activations associated with the end-effector Jacobian matrix based on the current pose. The parameter processing module is configured to determine the gain value of each degree of freedom direction based on the singular activation quantity, and to scale the desired end linear velocity based on the gain value to obtain the target linear velocity. The task planning module is configured to construct a multi-level quadratic planning task and obtain the joint angle increment within the current control cycle by solving the target linear velocity through the multi-level quadratic planning task. In the multi-level quadratic planning task, the low-priority task is solved within the feasible set of the high-priority task. The motion control module is configured to determine the target joint angle of the current control cycle based on the joint angle increment in order to control the movement of the robotic arm. The task planning module is specifically configured as follows: The first-level task, the second-level task, and the third-level task are constructed in descending order of priority. The first-level task is used to solve for the target linear velocity input to obtain a first feasible set associated with the joint angle increment. The second-level task is used to solve for the second feasible set associated with the joint angle increment in the current control cycle within the first feasible set. The third-level task is used to solve for the joint angle increment in the current control cycle within the second feasible set. The first layer task, the second layer task, and the third layer task are solved sequentially to determine the joint angle increment within the current control cycle; Furthermore, joint angle constraints and joint velocity constraints are set in the first layer of tasks. The joint angle constraints are used to determine the first target range of joint angle changes, and the joint velocity constraints are used to determine the second target range of joint velocity changes. The solution process for the first-level task includes: Convert the target linear velocity into the desired position increment; Based on the desired position increment, the preset velocity soft constraint relaxation variable, the preset relaxation variable penalty weight, and the linear velocity Jacobian matrix in the end Jacobian matrix, a quadratic programming function associated with the joint angle increment is determined and used as the first objective function. Solving the first objective function under the joint angle constraint and the joint velocity constraint yields the first feasible set corresponding to the joint angle increment.

8. An electronic device, characterized in that, include: One or more processors; A storage device for storing one or more programs, which, when executed by one or more processors, cause the one or more processors to implement the robotic arm pose control method as described in any one of claims 1-6.

Images