A control method and device of a mechanical arm, an electronic device, and a storage medium
By introducing penalty weights and optimization forces into teleoperated robots, the problem of insufficient arm angle optimization is solved, thereby improving safety and flexibility, avoiding collisions, and enhancing motion stability.
Patent Information
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- BEIJING YUANLUO TECHNOLOGY CO LTD
- Filing Date
- 2026-04-22
- Publication Date
- 2026-07-21
AI Technical Summary
In existing teleoperated robots, the optimization of the arm angle lacks effective constraints, resulting in an arm angle that is too large or too small, increasing the risk of collision and reducing posture comfort and movement flexibility.
By introducing penalty weights and optimization forces into the quadratic programming optimization algorithm for joint optimization, the arm angle is limited to a safe range by calculating the target penalty weights and optimization forces to prevent overextension or overbending, and redundancy optimization is performed using the zero space of the robotic arm.
It effectively constrains the arm angle, reduces the risk of collision, improves the posture comfort and motion continuity during remote operation, enhances the safety and flexibility of robotic arm operation, and makes the movement more stable and controllable.
Smart Images
Figure CN122425671A_ABST
Abstract
Description
Technical Field
[0001] This invention relates to the field of teleoperated robots, and more specifically, to a control method, device, electronic device, and storage medium for a robotic arm. Background Technology
[0002] In teleoperated robot systems, operators control the robot remotely via a control device. During operation, only the position and orientation information of the end effector of the robotic arm is provided; the control system of the robotic arm needs to calculate the angles of each joint based on this information. Existing teleoperated robots typically use optimization methods based on quadratic programming (QP) to solve for the robot's joint motion parameters.
[0003] However, existing joint solving strategies based on QP optimization generally lack effective optimization and reasonable range constraints for the robotic arm angles. This leads to the optimized joint angle combinations easily causing the arm angles to exceed the reasonable range, i.e., the arm angles are too large and close to the upper limit, or too small and close to the lower limit. When the arm angles are in the extreme range, the robotic arm elbow will exhibit overextension or overbent postures, which not only significantly increases the risk of collision between the robotic arm and the surrounding environment, but also reduces the posture comfort and subsequent motion flexibility during teleoperation. Summary of the Invention
[0004] In view of this, the purpose of this application is to provide a control method, device, electronic device and storage medium for a robotic arm, which can introduce range constraints on the arm angle of the robotic arm while ensuring the normal operation of the end effector, avoid the arm angle from being too large or too small and entering the limit range, prevent the elbow of the robotic arm from being overextended or overbent, reduce the risk of collision between the robotic arm and the surrounding environment, improve the posture comfort and motion continuity during the remote operation process, and at the same time improve the safety and flexibility of the robotic arm operation process, making the movement of the robotic arm more stable and controllable.
[0005] In a first aspect, embodiments of this application provide a control method for a robotic arm, the method comprising: Obtain the current arm angle and the arm angle at the previous moment of the robotic arm to be controlled; Based on the current arm angle and the arm angle at the previous moment, the target penalty weight corresponding to the current arm angle is determined; the target penalty weight is used to quantify the degree of danger of the robotic arm angle approaching the constraint boundary, as well as the danger of the arm angle movement trend; In the zero space of the robotic arm, the target optimization force for adjusting the current arm angle to a preset desired arm angle is calculated; the zero space refers to the redundant space in which the joints of the robotic arm can move autonomously when the pose of the end effector of the robotic arm remains unchanged. Based on the quadratic programming optimization algorithm, the penalty weights are constructed as penalty terms and jointly optimized with the target optimization force to obtain the joint angle combination of the robotic arm to be controlled at the next moment. Closed-loop motion control of the robotic arm to be controlled is then performed based on the joint angle combination.
[0006] In one possible implementation, determining the target penalty weight corresponding to the current arm angle based on the current arm angle and the arm angle at the previous moment includes: Calculate the current arm angle minus the preset lower bound of the arm angle constraint to obtain a first distance; and calculate the current arm angle minus the preset upper bound of the arm angle constraint to obtain a second distance; and compare the first distance with the preset safety distance threshold to obtain a first numerical comparison result; If the first numerical comparison result is that the first distance is less than the preset safe distance threshold or the second distance is less than the preset safe distance threshold, then the initial penalty weight is calculated based on the preset safe distance threshold and the first distance, or the initial penalty weight is calculated based on the preset safe distance threshold and the second distance. Based on the first numerical comparison result and the second numerical comparison result between the current arm angle and the arm angle at the previous moment, it is determined whether to reduce the initial penalty weight; If the initial penalty weight is reduced, the initial penalty weight is reduced according to the preset reduction coefficient corresponding to the first numerical comparison result to obtain the intermediate penalty weight; If the initial penalty weight is not reduced, then the initial penalty weight is determined as the intermediate penalty weight; The target penalty weight is determined based on the intermediate penalty weight.
[0007] In one possible implementation, calculating the initial penalty weight based on the preset safety distance threshold and the first distance includes: Substitute the preset safety distance threshold and the first distance into the following formula to calculate the initial penalty weight; ; in, As the initial penalty weight, The first distance, This is a preset safe distance threshold.
[0008] In one possible implementation, determining whether to reduce the initial penalty weight based on the first numerical comparison result and the second numerical comparison result between the current arm angle and the arm angle at the previous moment includes: If the first numerical comparison result is that the first distance is less than the preset safe distance threshold, and the current arm angle is greater than the arm angle at the previous moment, then the initial penalty weight is reduced. If the first numerical comparison result is that the second distance is less than the preset safe distance threshold, and the current arm angle is less than the arm angle at the previous moment, then the initial penalty weight is reduced.
[0009] In one possible implementation, determining the target penalty weight based on the intermediate penalty weight includes: The intermediate penalty weights are subjected to a first-order low-pass filter to obtain the target penalty weights.
[0010] In one possible implementation, in the null space of the robotic arm, calculating a target optimization force for adjusting the current arm angle to a preset desired arm angle includes: Subtract the preset desired arm angle from the current arm angle to obtain the arm angle error; and calculate the Jacobian matrix of the change in arm angle with respect to joint velocity under the current arm angle; the Jacobian matrix is used to describe the mapping relationship between joint velocity and the rate of change of arm angle; Remove the motion components in the Jacobian matrix that would cause changes in the pose of the end effector of the robotic arm to be controlled, and obtain the null projection matrix; Based on the arm angle error and the Jacobian matrix, calculate the initial optimization force to adjust the current arm angle to the preset desired arm angle; The initial optimization force is projected onto the null space according to the null space projection matrix to obtain the intermediate optimization force; The target optimization force is determined based on the intermediate optimization force.
[0011] In one possible implementation, determining the target optimization force based on the intermediate optimization force includes: The target optimization force is obtained by adding a damping term to the intermediate optimization force based on the current arm angle and the Jacobian matrix.
[0012] Secondly, embodiments of this application also provide a control device for a robotic arm, the device comprising: The acquisition module is used to acquire the current arm angle and the arm angle at the previous moment of the robotic arm to be controlled; The determination module is used to determine the target penalty weight corresponding to the current arm angle based on the current arm angle and the arm angle at the previous moment; the target penalty weight is used to quantify the degree of danger of the robotic arm angle approaching the constraint boundary, as well as the danger of the arm angle movement trend; The calculation module is used to calculate the target optimization force for adjusting the current arm angle to a preset desired arm angle in the zero space of the robotic arm; the zero space refers to the redundant space in which the joints of the robotic arm can move autonomously when the pose of the end effector of the robotic arm remains unchanged. The solution module is used to construct the penalty weights into penalty terms based on the quadratic programming optimization algorithm, and perform joint optimization with the target optimization force to obtain the joint angle combination of the robotic arm to be controlled at the next moment, and perform closed-loop motion control of the robotic arm to be controlled based on the joint angle combination.
[0013] In one possible implementation, the determining module is specifically configured to calculate the current arm angle minus a preset lower bound of the arm angle constraint to obtain a first distance; and calculate the current arm angle minus a preset upper bound of the arm angle constraint to obtain a second distance; and compare the first distance with the preset safe distance threshold to obtain a first numerical comparison result; if the first numerical comparison result is that the first distance is less than the preset safe distance threshold or the second distance is less than the preset safe distance threshold, then an initial penalty weight is calculated based on the preset safe distance threshold and the first distance, or an initial penalty weight is calculated based on the preset safe distance threshold and the second distance; based on the first numerical comparison result and the second numerical comparison result between the current arm angle and the arm angle at the previous moment, it is determined whether to reduce the initial penalty weight; if the initial penalty weight is reduced, then the initial penalty weight is reduced according to a preset reduction coefficient corresponding to the first numerical comparison result to obtain an intermediate penalty weight; if the initial penalty weight is not reduced, then the initial penalty weight is determined as an intermediate penalty weight; and based on the intermediate penalty weight, a target penalty weight is determined.
[0014] In one possible implementation, the determining module is further configured to: Substitute the preset safety distance threshold and the first distance into the following formula to calculate the initial penalty weight; ; in, As the initial penalty weight, The first distance, This is a preset safe distance threshold.
[0015] In one possible implementation, the determining module is further configured to: If the first numerical comparison result is that the first distance is less than the preset safe distance threshold, and the current arm angle is greater than the arm angle at the previous moment, then the initial penalty weight is reduced. If the first numerical comparison result is that the second distance is less than the preset safe distance threshold, and the current arm angle is less than the arm angle at the previous moment, then the initial penalty weight is reduced.
[0016] In one possible implementation, the determining module is further configured to: The intermediate penalty weights are subjected to a first-order low-pass filter to obtain the target penalty weights.
[0017] In one possible implementation, the calculation module is specifically used to subtract the preset desired arm angle from the current arm angle to obtain the arm angle error; and to calculate the Jacobian matrix of the arm angle change versus joint velocity at the current arm angle; the Jacobian matrix is used to describe the mapping relationship between joint velocity and arm angle change rate; to remove motion components in the Jacobian matrix that would cause changes in the pose of the end effector of the robotic arm to be controlled, to obtain a null space projection matrix; to calculate the initial optimization force to adjust the current arm angle to the preset desired arm angle based on the arm angle error and the Jacobian matrix; to project the initial optimization force onto null space based on the null space projection matrix to obtain an intermediate optimization force; and to determine the target optimization force based on the intermediate optimization force.
[0018] In one possible implementation, the calculation module is further configured to add a damping term to the intermediate optimization force based on the current arm angle and the Jacobian matrix to obtain the target optimization force.
[0019] Thirdly, embodiments of this application also provide an electronic device, including: a processor, a storage medium, and a bus, wherein the storage medium stores machine-readable instructions executable by the processor, and when the electronic device is running, the processor communicates with the storage medium via the bus, and the processor executes the machine-readable instructions to perform the steps of the robotic arm control method as described in any of the first aspects.
[0020] Fourthly, embodiments of this application also provide a computer-readable storage medium storing a computer program, which, when executed by a processor, performs the steps of the robotic arm control method as described in any of the first aspects.
[0021] This application provides a control method, device, electronic device, and storage medium for a robotic arm. The method includes: acquiring the current arm angle and the arm angle at the previous moment of the robotic arm to be controlled; determining the target penalty weight corresponding to the current arm angle based on the current arm angle and the arm angle at the previous moment; the target penalty weight is used to quantify the degree of danger of the robotic arm's arm angle approaching the constraint boundary, as well as the danger of the arm angle's movement trend; calculating the target optimization force in the zero space of the robotic arm to adjust the current arm angle to a preset desired arm angle; the zero space refers to the redundant space in which the joints of the robotic arm can move autonomously when the posture of the robotic arm's end effector remains unchanged; based on a quadratic programming optimization algorithm, the penalty weight is constructed as a penalty term and jointly optimized with the target optimization force to obtain the joint angle combination of the robotic arm to be controlled at the next moment; and the robotic arm to be controlled is subjected to closed-loop motion control based on the joint angle combination. This application can constrain the range of the arm angle, reduce the risk of the robotic arm colliding with the surrounding environment, improve the posture comfort and motion continuity during remote operation, and enhance the safety and flexibility of the robotic arm's operation process, making the robotic arm's movement more stable and controllable. Attached Figure Description
[0022] To more clearly illustrate the technical solutions of the embodiments of this application, the accompanying drawings used in the embodiments will be briefly introduced below. It should be understood that the following drawings only show some embodiments of this application and should not be regarded as a limitation of the scope. For those skilled in the art, other related drawings can be obtained based on these drawings without creative effort.
[0023] Figure 1 A flowchart of a control method for a robotic arm provided in an embodiment of this application is shown; Figure 2 A flowchart illustrating the determination of the target penalty weight provided in an embodiment of this application is shown; Figure 3 A flowchart illustrating the calculation process of the arm angle optimization force provided in an embodiment of this application is shown. Figure 4 This paper shows a structural diagram of a control device for a robotic arm according to an embodiment of the present application; Figure 5 A schematic diagram of the structure of an electronic device provided in an embodiment of this application is shown. Detailed Implementation
[0024] To make the objectives, technical solutions, and advantages of the embodiments of this application clearer, the technical solutions of the embodiments of this application will be clearly and completely described below with reference to the accompanying drawings. It should be understood that the accompanying drawings in this application are for illustrative and descriptive purposes only and are not intended to limit the scope of protection of this application. Furthermore, it should be understood that the schematic drawings are not drawn to scale. The flowcharts used in this application illustrate operations implemented according to some embodiments of this application. It should be understood that the operations in the flowcharts may not be implemented in sequence, and steps without logical contextual relationships may be reversed or implemented simultaneously. In addition, those skilled in the art, guided by the content of this application, may add one or more other operations to the flowcharts, or remove one or more operations from the flowcharts.
[0025] Furthermore, the described embodiments are merely some, not all, of the embodiments of this application. The components of the embodiments of this application described and illustrated herein can typically be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of this application provided in the accompanying drawings is not intended to limit the scope of the claimed application, but merely to illustrate selected embodiments of the application. All other embodiments obtained by those skilled in the art based on the embodiments of this application without inventive effort are within the scope of protection of this application.
[0026] To enable those skilled in the art to utilize the content of this application, and in conjunction with the specific application scenario of "teleoperated robots," the following implementation is provided. For those skilled in the art, the general principles defined herein can be applied to other embodiments and application scenarios without departing from the spirit and scope of this application. Although this application is primarily described within the "teleoperated robot field," it should be understood that this is merely an exemplary embodiment.
[0027] It should be noted that the term "comprising" will be used in the embodiments of this application to indicate the presence of the features declared thereafter, but does not exclude the addition of other features.
[0028] The following is a detailed description of a robotic arm control method provided in the embodiments of this application.
[0029] Reference Figure 1 The diagram shown is a flowchart illustrating a control method for a robotic arm according to an embodiment of this application. The exemplary steps of this embodiment are described below: S101. Obtain the current arm angle and the arm angle at the previous moment of the robotic arm to be controlled.
[0030] In this embodiment, the arm angle represents the "torsion / lateral tilt posture of the entire robotic arm," rather than the rotation of a single joint. In mechanics, the arm angle is usually defined as the angle between a plane (referred to as the arm plane) determined by the shoulder, elbow, and wrist joints and a reference plane. For redundant robotic arms, three key points are defined: shoulder joint S, elbow joint E, and wrist joint W. The current arm angle of the robotic arm to be controlled is obtained through the following steps: Step 1: Obtain the current coordinates of the shoulder joint S, elbow joint E, and wrist joint W of the robotic arm to be controlled.
[0031] For example, the current coordinates of shoulder joint S are (0, 0, 0), the current coordinates of elbow joint E are (1, 2, 0), and the current coordinates of wrist joint W are (3, 0, 0).
[0032] Step 2: Calculate the shoulder-wrist vector (That is, the straight line from the shoulder to the wrist): .
[0033] Continuing from the previous example, .
[0034] Step 3: Calculate the shoulder-elbow vector (The straight line from the shoulder to the elbow): .
[0035] Continuing from the previous example, .
[0036] Step 4: Calculate the unit vector of the shoulder-wrist axis. (Convert the "shoulder → wrist" line to a unit length, retaining only the direction, and use it as the axis of rotation): .
[0037] in, For vectors Get the model.
[0038] Continuing from the previous example, ; .
[0039] Step 5: Project the shoulder-elbow vector onto a plane perpendicular to the shoulder-wrist axis. : ;in, for" The pattern of "".
[0040] Step Six: First, define the reference direction: use the projection of the direction of gravity onto a plane perpendicular to the shoulder-wrist axis as the reference direction. Then, calculate the arm angle. : via vector with reference direction The included angle between them is determined as follows: .
[0041] in, Represents the cross product of vectors. This represents the vector dot product, where axis is the unit vector of the shoulder-wrist axis.
[0042] S102. Based on the current arm angle and the arm angle at the previous moment, determine the target penalty weight corresponding to the current arm angle; the target penalty weight is used to quantify the degree of danger of the robotic arm angle approaching the constraint boundary, as well as the danger of the arm angle movement trend.
[0043] In this embodiment, the closer the robotic arm angle is to the constraint boundary, the greater the probability of collision with surrounding objects, and the greater the target penalty weight. The trend of arm angle movement is towards the constraint boundary, increasing the probability of collision with surrounding objects and thus the target penalty weight. (Refer to...) Figure 2 The following is a flowchart illustrating the determination of the target penalty weight provided in an embodiment of this application: S201. Calculate the current arm angle minus the preset lower bound of the arm angle constraint to obtain the first distance; and calculate the current arm angle minus the preset upper bound of the arm angle constraint to obtain the second distance; and compare the first distance with the preset safety distance threshold to obtain the first numerical comparison result.
[0044] In this embodiment of the application, the allowable range of the arm angle (i.e., the range of arm angles that will not collide with surrounding objects) is predefined: a lower bound for the preset arm angle constraint is defined. and the upper limit of the preset arm angle constraint Calculate the first distance. Second distance .
[0045] In addition, the first distance and the preset safe distance threshold are combined. Numerical comparisons are performed to obtain the first numerical arm angle result, such as: , wait.
[0046] S202. If the first numerical comparison result is that the first distance is less than the preset safe distance threshold or the second distance is less than the preset safe distance threshold, then the initial penalty weight is calculated based on the preset safe distance threshold and the first distance, or the initial penalty weight is calculated based on the preset safe distance threshold and the second distance.
[0047] In this embodiment, the closer the arm angle is to the preset lower or upper bound of the arm angle constraint, the greater the probability of collision with surrounding objects. Therefore, the first numerical comparison result is that the first distance is less than the preset safe distance threshold (i.e., ), or the second distance is less than the preset safe distance threshold, i.e. ( When this occurs, the penalty mechanism is activated.
[0048] Specifically, the initial penalty weight is calculated based on the preset safe distance threshold and the first distance, including: Substitute the preset safe distance threshold and the first distance into the following formula to calculate the initial penalty weight; ; in, As the initial penalty weight, The first distance, This is a preset safe distance threshold.
[0049] Specifically, the initial penalty weight is calculated based on a preset safe distance threshold and a second distance, including: ; in, As the initial penalty weight, The first distance, This is a preset safe distance threshold.
[0050] S203. Based on the first numerical comparison result and the second numerical comparison result between the current arm angle and the arm angle at the previous moment, determine whether to reduce the initial penalty weight.
[0051] In this application embodiment, if the first numerical comparison result is that the first distance is less than the preset safe distance threshold, and the current arm angle is greater than the arm angle at the previous moment, then the initial penalty weight is reduced.
[0052] Here, if the first numerical comparison result is that the first distance is less than the preset safe distance threshold and the current arm angle is greater than the arm angle at the previous moment, it means that the arm angle movement trend is moving away from the constraint boundary and the probability of colliding with surrounding objects is decreasing, so the initial penalty weight can be appropriately reduced.
[0053] In this embodiment of the application, if the first numerical comparison result is that the first distance is less than the preset safe distance threshold, and the current arm angle is less than the arm angle at the previous moment, then the initial penalty weight is not reduced.
[0054] Here, if the first numerical comparison result is that the first distance is less than the preset safe distance threshold and the current arm angle is less than the arm angle at the previous moment, it means that the arm angle movement trend is to move closer to the constraint boundary and the probability of colliding with surrounding objects is increasing, so the initial penalty weight is not reduced.
[0055] In this application embodiment, if the first numerical comparison result is that the second distance is less than the preset safe distance threshold, and the current arm angle is less than the arm angle at the previous moment, then the initial penalty weight is reduced.
[0056] Here, if the first numerical comparison result is that the second distance is less than the preset safe distance threshold and the current arm angle is less than the arm angle at the previous moment, it means that the arm angle movement trend is moving away from the constraint boundary and the probability of colliding with surrounding objects is decreasing, so the initial penalty weight can be appropriately reduced.
[0057] In this application embodiment, if the first numerical comparison result is that the second distance is less than the preset safe distance threshold, and the current arm angle is greater than the arm angle at the previous moment, then the initial penalty weight is reduced.
[0058] Here, if the first numerical comparison result is that the second distance is less than the preset safe distance threshold and the current arm angle is greater than the arm angle at the previous moment, it means that the arm angle movement trend is to move closer to the constraint boundary and the probability of colliding with surrounding objects is increasing, so the initial penalty weight is not reduced.
[0059] S204. If the initial penalty weight is reduced, the initial penalty weight is reduced according to the preset reduction coefficient corresponding to the first numerical comparison result to obtain the intermediate penalty weight.
[0060] In this application embodiment, different first numerical comparison results correspond to different preset reduction coefficients, including the first preset reduction coefficient corresponding to when the first numerical comparison result is that the first distance is less than a preset safety distance threshold. The first numerical comparison result is the second preset reduction coefficient corresponding to the second distance being less than the preset safe distance threshold. Specifically, if the first numerical comparison result indicates that the first distance is less than a preset safe distance threshold, then the first preset reduction coefficient and the initial penalty weight are substituted into the following formula to obtain the intermediate penalty weight. : .
[0061] If the first numerical comparison result indicates that the second distance is less than the preset safe distance threshold, then substitute the second preset reduction coefficient and the initial penalty weight into the following formula to obtain the intermediate penalty weight. : .
[0062] S205. If the initial penalty weight is not reduced, the initial penalty weight shall be determined as the intermediate penalty weight.
[0063] S206. Determine the target penalty weight based on the intermediate penalty weight.
[0064] In this embodiment, the intermediate penalty weight can be directly determined as the target penalty weight. Alternatively, the intermediate penalty weight can be subjected to a first-order low-pass filter to obtain the target penalty weight. Specifically, the formula for applying a first-order low-pass filter to the intermediate penalty weights is as follows: ;in, These are the filter coefficients.
[0065] Here, the low-pass filter formula can avoid penalizing sudden weight changes.
[0066] S103. In the zero space of the robotic arm, calculate the target optimization force used to adjust the current arm angle to the preset desired arm angle; the zero space refers to the redundant space in which the joints of the robotic arm can move autonomously when the position of the end effector of the robotic arm remains unchanged.
[0067] In the embodiments of this application, reference is made to Figure 3 The diagram shown is a flowchart of the calculation process for the arm angle optimization force provided in an embodiment of this application: S301. Subtract the preset desired arm angle from the current arm angle to obtain the arm angle error; and calculate the Jacobian matrix of the change in arm angle with respect to joint velocity under the current arm angle; the Jacobian matrix is used to describe the mapping relationship between joint velocity and the rate of change of arm angle.
[0068] In this embodiment of the application, a preset desired arm angle is set in advance. Calculate arm angle error The Jacobian matrix of arm angle change with respect to joint velocity is a one-dimensional Jacobian matrix obtained by taking the partial derivative of the arm angle with respect to each joint angle, based on the mapping relationship between the arm angle and the joint angle of the robotic arm.
[0069] S302. Remove the motion components in the Jacobian matrix that would cause changes in the pose of the end effector of the robotic arm to be controlled, and obtain the null projection matrix.
[0070] In the embodiments of this application, the pseudo-inverse of the Jacobian matrix is calculated to obtain the null space projection matrix N.
[0071] S303. Based on the arm angle error and the Jacobian matrix, calculate the initial optimization force to adjust the current arm angle to the preset desired arm angle.
[0072] In the embodiments of this application, the arm angle error is... And Jacobi matrix Substituting into the following formula, we obtain the initial optimization force. : ,in Optimize gain for preset.
[0073] S304. Project the initial optimization force onto the zero space according to the zero space projection matrix to obtain the intermediate optimization force.
[0074] In this embodiment, the null projection matrix and the initial optimization force are substituted into the following formula to obtain the intermediate optimization force. : .
[0075] S305. Determine the target optimization force based on the intermediate optimization force.
[0076] In this embodiment, the intermediate optimization force can be directly used as the target optimization force, or a damping term can be added to the intermediate optimization force based on the current arm angle and Jacobian matrix to obtain the target optimization force.
[0077] Specifically, the derivative of the current arm angle (i.e., the rate of change of arm angle), Jacobian matrix and intermediate optimization power Substituting into the following formula, we obtain the target optimization force. : ;in, is the damping coefficient.
[0078] S104. Based on the quadratic programming optimization algorithm, the penalty weight is constructed as a penalty term and jointly optimized with the target optimization force to obtain the joint angle combination of the robot arm to be controlled at the next moment. Closed-loop motion control is performed on the robot arm to be controlled according to the joint angle combination.
[0079] In the embodiments of this application, during full-body teleoperation of a robot, the joint velocities are typically solved using the following standard quadratic programming (QP) optimization problem: ; ; in: This is the joint velocity vector; It is a Hessian matrix (positive definite matrix); The gradient vector; , These are the equality constraint matrix and equality constraint vector; , Let be the inequality constraint matrix and inequality constraint vector; This is the lower bound for joint velocity; This represents the upper limit of joint velocity.
[0080] Specifically, the arm angle constraint and optimization force are integrated into the above QP optimization problem, so that the arm angle constraint can be considered simultaneously when solving the joint angle: (1) Penalty term (Hessian matrix): Add a penalty term to the Hessian matrix of the QP optimization problem: ; in: ; This is the Hessian matrix after the addition. This is the Hessian matrix before addition. It is the identity matrix. and To penalize the lower and upper bounds of the weight, Let be the identity matrix. When the arm angle approaches the limit, the penalty term increases, causing the optimization problem to tend to choose joint angle combinations with arm angles within a safe range.
[0081] ( ) Optimization term (gradient vector): Add optimization force to the gradient vector of the QP optimization problem: ;in, The gradient vector after addition. This is the gradient vector before it was added.
[0082] The optimization problem guides the selection of joint angle combinations that make the arm angle approach the desired value.
[0083] In addition, the control method provided in this application embodiment realizes real-time updates: (1) In each control cycle, the arm angle is calculated in real time according to the current joint angle; the penalty value and optimization force are updated; the QP optimization problem is solved to obtain the optimal joint speed that satisfies the end pose constraint and the arm angle is within the safe range, and then the joint angle combination is calculated based on the control cycle to control the robotic arm.
[0084] In summary, the control method for the robotic arm provided in this application has the following advantages and effects: (1) Solve the problem of excessively large or small arm angle in QP optimization: By adding arm angle constraints and optimization terms to the QP optimization problem, the arm angle constraint can be considered at the same time when solving the joint angle, so as to prevent the arm angle from being too large or too small due to the combination of joint angles, and effectively avoid the excessive extension or excessive bending of the robot arm elbow.
[0085] (2) Effective constraint of workspace: By calculating and constraining the arm angle in real time, the workspace of the robotic arm is limited to a safe range, preventing the robotic arm from entering dangerous configurations and effectively reducing the risk of collision. A distance-based penalty function is adopted to gradually increase / decrease the constraint intensity when approaching / reaching the limit, and smoothing is performed to avoid motion discontinuity caused by hard constraints and improve the stability of the system.
[0086] (3) Preventing dangerous configuration in advance: By setting a safe distance threshold for the arm angle, constraints are applied when there is still a certain distance between the arm angle and the limit, which can prevent the robotic arm from entering the dangerous working area in advance and avoid affecting subsequent operations.
[0087] (4) Directional constraints: Differentiated constraints are applied according to the movement trend of the robotic arm. When moving towards the dangerous direction, stronger constraints are applied, and when moving away from the dangerous direction, the constraint strength is reduced. This ensures safety, improves the efficiency of workspace utilization, and enhances the user experience.
[0088] (5) Zero-space arm angle optimization: Under the premise of ensuring the completion of the end effector task, the arm angle is optimized by using redundant degrees of freedom to make the arm angle as close as possible to the set desired arm angle, so that the arm angle changes the least during teleoperation and avoids collision with surrounding objects; by setting the desired arm angle away from the arm angle limit area, the end effector pose tracking is not limited by the arm angle, thus improving the utilization efficiency of the workspace and the flexibility of operation.
[0089] (6) By modifying the Hessian matrix and gradient vector of the QP optimization problem, it can be easily integrated into the existing optimization framework without significantly modifying the existing system architecture.
[0090] Based on the same inventive concept, this application also provides a control device for a robotic arm corresponding to the control method of the robotic arm. Since the principle of the device in this application is similar to the control method of the robotic arm described above, the implementation of the device can refer to the implementation of the method, and the repeated parts will not be described again.
[0091] Reference Figure 4 The diagram shown is a schematic of a control device for a robotic arm provided in an embodiment of this application. The device includes: The acquisition module 401 is used to acquire the current arm angle and the arm angle at the previous moment of the robotic arm to be controlled; The determining module 402 is used to determine the target penalty weight corresponding to the current arm angle based on the current arm angle and the arm angle at the previous moment; the target penalty weight is used to quantify the degree of danger of the robotic arm angle approaching the constraint boundary, as well as the danger of the arm angle movement trend; The calculation module 403 is used to calculate the target optimization force for adjusting the current arm angle to a preset desired arm angle in the zero space of the robotic arm; the zero space refers to the redundant space in which the joints of the robotic arm can move autonomously when the pose of the end effector of the robotic arm remains unchanged. The solution module 404 is used to construct the penalty weight as a penalty term based on the quadratic programming optimization algorithm, and perform joint optimization with the target optimization force to obtain the joint angle combination of the robotic arm to be controlled at the next moment, and perform closed-loop motion control on the robotic arm to be controlled based on the joint angle combination.
[0092] Here, this device can constrain the range of the arm angle, reducing the risk of collision between the robotic arm and the surrounding environment, improving the posture comfort and motion continuity during remote operation, and enhancing the safety and flexibility of the robotic arm operation process, making the robotic arm movement more stable and controllable.
[0093] like Figure 5 As shown in the embodiment of this application, an electronic device 500 includes a processor 501, a memory 502, and a bus. The memory 502 stores machine-readable instructions that can be executed by the processor 501. When the electronic device is running, the processor 501 communicates with the memory 502 via the bus, and the processor 501 executes the machine-readable instructions to perform the steps of the control method for the robotic arm described above.
[0094] Specifically, the memory 502 and processor 501 mentioned above can be general-purpose memory and processor, without any specific limitations. When the processor 501 runs the computer program stored in the memory 502, it can execute the control method of the robotic arm mentioned above.
[0095] Corresponding to the above-described control method for the robotic arm, this application embodiment also provides a computer-readable storage medium storing a computer program, which, when executed by a processor, performs the steps of the above-described control method for the robotic arm.
[0096] Those skilled in the art will clearly understand that, for the sake of convenience and brevity, the specific working processes of the systems and devices described above can be referred to the corresponding processes in the method embodiments, and will not be repeated here. In the several embodiments provided in this application, it should be understood that the disclosed systems, devices, and methods can be implemented in other ways. The device embodiments described above are merely illustrative. For example, the division of modules is only a logical functional division, and in actual implementation, there may be other division methods. Furthermore, multiple modules or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the displayed or discussed mutual coupling or direct coupling or communication connection can be through some communication interfaces; the indirect coupling or communication connection of devices or modules can be electrical, mechanical, or other forms.
[0097] The modules described as separate components may or may not be physically separate. The components shown as modules may or may not be physical units; that is, they may be located in one place or distributed across multiple network units. Some or all of the units can be selected to achieve the purpose of this embodiment according to actual needs.
[0098] In addition, the functional units in the various embodiments of this application can be integrated into one processing unit, or each unit can exist physically separately, or two or more units can be integrated into one unit.
[0099] If the aforementioned functions are implemented as software functional units and sold or used as independent products, they can be stored in a processor-executable, non-volatile, computer-readable storage medium. Based on this understanding, the technical solution of this application, in essence, or the part that contributes to the prior art, or a portion of the technical solution, can be embodied in the form of a software product. This computer software product is stored in a storage medium and includes several instructions to cause a computer device (which may be a personal computer, server, or network device, etc.) to execute all or part of the steps of the methods described in the various embodiments of this application. The aforementioned storage medium includes various media capable of storing program code, such as USB flash drives, portable hard drives, ROM, RAM, magnetic disks, or optical disks.
[0100] The above are merely specific embodiments of this application, but the scope of protection of this application is not limited thereto. Any variations or substitutions that can be easily conceived by those skilled in the art within the scope of the technology disclosed in this application should be included within the scope of protection of this application. Therefore, the scope of protection of this application should be determined by the scope of the claims.
Claims
1. A control method for a robotic arm, characterized in that, The method includes: Obtain the current arm angle and the arm angle at the previous moment of the robotic arm to be controlled; Based on the current arm angle and the arm angle at the previous moment, the target penalty weight corresponding to the current arm angle is determined; the target penalty weight is used to quantify the degree of danger of the robotic arm angle approaching the constraint boundary, as well as the danger of the arm angle movement trend; In the zero space of the robotic arm, the target optimization force for adjusting the current arm angle to a preset desired arm angle is calculated; the zero space refers to the redundant space in which the joints of the robotic arm can move autonomously when the pose of the end effector of the robotic arm remains unchanged. Based on the quadratic programming optimization algorithm, the penalty weights are constructed as penalty terms and jointly optimized with the target optimization force to obtain the joint angle combination of the robotic arm to be controlled at the next moment. Closed-loop motion control of the robotic arm to be controlled is then performed based on the joint angle combination.
2. The control method for the robotic arm according to claim 1, characterized in that, The step of determining the target penalty weight corresponding to the current arm angle based on the current arm angle and the arm angle at the previous moment includes: Calculate the current arm angle minus the preset lower bound of the arm angle constraint to obtain the first distance; and calculate the current arm angle minus the preset upper bound of the arm angle constraint to obtain the second distance; and compare the first distance with the preset safety distance threshold to obtain the first numerical comparison result. If the first numerical comparison result is that the first distance is less than the preset safe distance threshold or the second distance is less than the preset safe distance threshold, then the initial penalty weight is calculated based on the preset safe distance threshold and the first distance, or the initial penalty weight is calculated based on the preset safe distance threshold and the second distance. Based on the first numerical comparison result and the second numerical comparison result between the current arm angle and the arm angle at the previous moment, it is determined whether to reduce the initial penalty weight; If the initial penalty weight is reduced, the initial penalty weight is reduced according to the preset reduction coefficient corresponding to the first numerical comparison result to obtain the intermediate penalty weight; If the initial penalty weight is not reduced, then the initial penalty weight is determined as the intermediate penalty weight; The target penalty weight is determined based on the intermediate penalty weight.
3. The control method for the robotic arm according to claim 2, characterized in that, The step of calculating the initial penalty weight based on the preset safety distance threshold and the first distance includes: Substitute the preset safety distance threshold and the first distance into the following formula to calculate the initial penalty weight; ； in, As the initial penalty weight, The first distance, This is a preset safe distance threshold.
4. The control method for the robotic arm according to claim 2, characterized in that, The step of determining whether to reduce the initial penalty weight based on the first numerical comparison result and the second numerical comparison result between the current arm angle and the arm angle at the previous moment includes: If the first numerical comparison result is that the first distance is less than the preset safe distance threshold, and the current arm angle is greater than the arm angle at the previous moment, then the initial penalty weight is reduced. If the first numerical comparison result is that the second distance is less than the preset safe distance threshold, and the current arm angle is less than the arm angle at the previous moment, then the initial penalty weight is reduced.
5. The control method for the robotic arm according to claim 2, characterized in that, The step of determining the target penalty weight based on the intermediate penalty weight includes: The intermediate penalty weights are subjected to a first-order low-pass filter to obtain the target penalty weights.
6. The control method for the robotic arm according to claim 1, characterized in that, In the null space of the robotic arm, the target optimization force for adjusting the current arm angle to a preset desired arm angle is calculated, including: Subtract the preset desired arm angle from the current arm angle to obtain the arm angle error; and calculate the Jacobian matrix of the change in arm angle with respect to joint velocity under the current arm angle; the Jacobian matrix is used to describe the mapping relationship between joint velocity and the rate of change of arm angle; Remove the motion components in the Jacobian matrix that would cause changes in the pose of the end effector of the robotic arm to be controlled, and obtain the null projection matrix; Based on the arm angle error and the Jacobian matrix, calculate the initial optimization force to adjust the current arm angle to the preset desired arm angle; The initial optimization force is projected onto the null space according to the null space projection matrix to obtain the intermediate optimization force; The target optimization force is determined based on the intermediate optimization force.
7. The control method for the robotic arm according to claim 6, characterized in that, The step of determining the target optimization force based on the intermediate optimization force includes: The target optimization force is obtained by adding a damping term to the intermediate optimization force based on the current arm angle and the Jacobian matrix.
8. A control device for a robotic arm, characterized in that, The device includes: The acquisition module is used to acquire the current arm angle and the arm angle at the previous moment of the robotic arm to be controlled; The determination module is used to determine the target penalty weight corresponding to the current arm angle based on the current arm angle and the arm angle at the previous moment; the target penalty weight is used to quantify the degree of danger of the robotic arm angle approaching the constraint boundary, as well as the danger of the arm angle movement trend; The calculation module is used to calculate the target optimization force for adjusting the current arm angle to a preset desired arm angle in the zero space of the robotic arm; the zero space refers to the redundant space in which the joints of the robotic arm can move autonomously when the pose of the end effector of the robotic arm remains unchanged. The solution module is used to construct the penalty weights into penalty terms based on the quadratic programming optimization algorithm, and perform joint optimization with the target optimization force to obtain the joint angle combination of the robotic arm to be controlled at the next moment, and perform closed-loop motion control of the robotic arm to be controlled based on the joint angle combination.
9. An electronic device, characterized in that, include: The device includes a processor, a storage medium, and a bus, wherein the storage medium stores machine-readable instructions executable by the processor, and when the electronic device is in operation, the processor communicates with the storage medium via the bus, and the processor executes the machine-readable instructions to perform the steps of the control method for the robotic arm as described in any one of claims 1 to 7.
10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program that, when executed by a processor, performs the steps of the control method for the robotic arm as described in any one of claims 1 to 7.