Hydraulic mechanical arm singular point processing method and system based on speed feedback

Abstract

The invention provides a hydraulic mechanical arm singular point processing method and system based on speed feedback. The hydraulic mechanical arm singular point processing method comprises the steps that modeling is conducted on joint angular rate changes of a hydraulic mechanical arm according to an exponential function; aiming at the established model, introducing a nonlinear particle swarm parameter optimization algorithm to obtain an upper limit value of a damping factor; and the required damping factor is obtained based on the upper limit value of the damping factor, then a Jacobian matrix is obtained according to the required damping factor and the instruction end speed, and the improved joint angular velocity is obtained based on the Jacobian matrix.

Description

Technical Field

[0001] The present invention belongs to the technical field of singular point processing of mechanical arms, and in particular relates to a method and system for processing singular points of hydraulic mechanical arms based on speed feedback. Background Art

[0002] The statements in this section merely provide background information related to the present invention and do not necessarily constitute prior art.

[0003] In the inverse kinematics calculation of a serial manipulator, it is inevitable to encounter singularity problems. When the manipulator is in a certain configuration, due to the mathematical characteristics of the inverse algorithm of the manipulator, an infinite joint angular velocity is calculated, resulting in a sudden change in the joint angular velocity of the manipulator. For a hydraulic manipulator, the singularity problem will cause pressure fluctuations in the hydraulic manipulator, increase tracking errors, and damage mechanical equipment.

[0004] like Figure 1 The figure shows a PUMA560 hydraulic manipulator. When the four joints and the six joints of the serial robot are collinear, that is, the joint angle of joint 5 is 0 degrees, it is in a singular wrist configuration. When the manipulator is in this configuration or a position near it, due to the mathematical characteristics of the inverse solution algorithm, a small change in the motion command at the end will cause a sudden change in the angular velocity of joints 4 and 6, resulting in tracking errors and damage to the mechanical structure of the manipulator.

[0005]

[0006] In the above formula, J is the Jacobian transformation matrix representing the joint angular velocity and the end Cartesian velocity. When the 4 joints and 6 joints are collinear, J is a singular matrix, It tends to infinity, producing strange phenomena. For hydraulic manipulators, the rapid change in speed will cause pressure fluctuations and damage the hydraulic components of the hydraulic manipulator.

[0007] To address this problem, the traditional damped least squares (DLS) method adds a damping factor λ to the Jacobian kinematic inverse matrix to suppress velocity mutations at the cost of producing a small tracking error.

[0008]

[0009] The traditional damped least squares method uses a constant factor, which will lead to redundant damping outside the singular configuration, thereby increasing the tracking error. The improved damped least squares method, such as the piecewise function to obtain the damping factor, will produce velocity discontinuity.

[0010] In short, for special scenarios such as breaking steel bars, breaking anti-theft windows, and plugging valves in fire rescue, the robotic arm is required to be able to operate arbitrarily in front of it. In this case, wrist joint singularities are more likely to occur. Existing technologies avoid this by replanning the trajectory or replacing it with a single-joint axis-controlled motion, which results in large error redundancy and reduces tracking accuracy. Summary of the invention

[0011] In order to overcome the above-mentioned deficiencies of the prior art, the present invention provides a method for processing singular points of a hydraulic mechanical arm based on velocity feedback, and obtains a damping factor based on the change rate of the joint angular velocity to obtain an improved joint angular velocity.

[0012] To achieve the above objectives, one or more embodiments of the present invention provide the following technical solutions:

[0013] In a first aspect, a method for processing singular points of a hydraulic mechanical arm based on speed feedback is disclosed, comprising:

[0014] When the manipulator is operated to perform six-degree-of-freedom motion at the end in Cartesian space, the current joint angular rate of the hydraulic manipulator is obtained;

[0015] According to the angular velocities of two adjacent sampling periods of two joints close to the wrist singular configuration, the degree of change of the angular velocity of the joints is obtained;

[0016] Modeling based on the degree of change of the angular velocity of the hydraulic manipulator joints is carried out using an exponential function;

[0017] According to the established model, the nonlinear particle swarm parameter optimization algorithm is introduced to obtain the upper limit value of the damping factor in the model;

[0018] The required damping factor is obtained based on the upper limit value of the damping factor, and then the Jacobian matrix is ​​obtained according to the required damping factor and the command end velocity, and the improved joint angular velocity is obtained based on the Jacobian matrix.

[0019] The singularity problem exists on both electric and hydraulically driven robotic arms, and some special problems will arise on hydraulic robotic arms. Therefore, the algorithm corresponding to the technical solution of the present application has targeted advantages on hydraulic robotic arms.

[0020] As a further technical solution, the hydraulic mechanical arm joint angular rate changes, when established:

[0021] Get the k-th step joint angular velocity of the hydraulic manipulator;

[0022] Get the k-1th step joint angular velocity of the hydraulic manipulator;

[0023] According to the angular velocity of joint 4 and joint 6 in two adjacent sampling periods, the degree of change of the angular velocity of the joint is obtained.

[0024] As a further technical solution, the specific formula for characterizing the degree of change of the joint angular velocity is:

[0025]

[0026] in, is the angular velocity of joint 4 at time k, is the angular velocity of joint 4 at time k-1, is the angular velocity of joint six at time k, is the angular velocity of joint six at time k-1.

[0027] As a further technical solution, the hydraulic manipulator joint angular velocity change degree is modeled according to an exponential function:

[0028]

[0029] In the formula, λ max is the upper limit, λ max The value of affects the value of λ. If λ max If λ is too large, λ will be too large, resulting in excessive tracking error. max Too small, λ will be too small.

[0030] As a further technical solution, a nonlinear particle swarm parameter optimization algorithm is introduced to obtain the upper limit of the damping factor, specifically including:

[0031] Initialization parameters: inertia weight ω, learning factors c1, c2;

[0032] Establish a fitness function, which is an indicator of whether the current solution is the optimal solution, and use the tracking error and the joint angular velocity mutation rate as independent variables of the fitness function;

[0033] Find the upper limit of the damping factor.

[0034] As a further technical solution, the inertia weight reflects the impact of historical particle speed on the current particle speed, and the learning factor affects the global search ability of the algorithm.

[0035] In a second aspect, a hydraulic manipulator singularity processing system based on speed feedback is disclosed, comprising:

[0036] The joint angle rate change acquisition module is configured to: acquire the current joint angle rate of the hydraulic manipulator arm when the manipulator arm is operated to perform six-degree-of-freedom motion of the end in Cartesian space;

[0037] According to the angular velocities of two adjacent sampling periods of two joints close to the wrist singular configuration, the degree of change of the angular velocity of the joints is obtained;

[0038] The modeling module is configured to: model according to an exponential function based on the degree of change of the joint angular velocity of the hydraulic manipulator;

[0039] The joint angular velocity improvement module is configured as follows: for the established model, a nonlinear particle swarm parameter optimization algorithm is introduced to obtain the upper limit value of the damping factor in the model;

[0040] The required damping factor is obtained based on the upper limit value of the damping factor, and then the Jacobian matrix is ​​obtained according to the required damping factor and the command end velocity, and the improved joint angular velocity is obtained based on the Jacobian matrix.

[0041] One or more of the above technical solutions have the following beneficial effects:

[0042] The sub-technical scheme of this embodiment obtains the degree of change of the angular velocity of the two joints of the singular configuration close to the wrist according to the angular velocity of two adjacent sampling periods, and establishes a model based on the degree of change of the angular velocity of the joints, so as to facilitate the subsequent real-time and accurate control of the robotic arm, and avoids the problem of complex control and large errors caused by controlling the movement of the robotic arm by judging whether the robotic arm enters the singular area in the prior art.

[0043] After the model is established in the sub-technical solution of this embodiment, a nonlinear particle swarm parameter optimization algorithm is used to solve the upper limit value of the damping factor. Compared with the traditional damped least squares method, the damping redundancy is reduced, and the tracking error is reduced while ensuring the smoothness of motion near the singular configuration.

[0044] Advantages of additional aspects of the present invention will be given in part in the following description, and in part will become obvious from the following description, or will be learned through practice of the present invention. BRIEF DESCRIPTION OF THE DRAWINGS

[0045] The accompanying drawings in the specification, which constitute a part of the present invention, are used to provide a further understanding of the present invention. The exemplary embodiments of the present invention and their descriptions are used to explain the present invention and do not constitute improper limitations on the present invention.

[0046] Figure 1 This is a schematic diagram of a unique wrist configuration structure according to an embodiment of the present invention;

[0047] Figure 2 This is the flow chart of the NPSO nonlinear particle swarm optimization algorithm;

[0048] Figure 3 This is a flow chart of a method according to an embodiment of the present invention;

[0049] Figure 4 This is a physical picture of the hydraulic mechanical arm;

[0050] Figure 5 The construction of the corresponding electrical system;

[0051] Figure 6 (a)-(b)- Figure 8 (a)-(b) are schematic diagrams comparing the technical solution of the present application and the traditional method under different damping factors. DETAILED DESCRIPTION

[0052] It should be noted that the following detailed descriptions are exemplary and are intended to provide further explanation of the present invention. Unless otherwise specified, all technical and scientific terms used herein have the same meanings as those commonly understood by those skilled in the art to which the present invention belongs.

[0053] It should be noted that the terms used herein are for describing specific embodiments only and are not intended to be limiting of exemplary embodiments according to the present invention.

[0054] In the absence of conflict, the embodiments of the present invention and the features of the embodiments may be combined with each other.

[0055] Embodiment 1

[0056] This embodiment discloses a method for processing singular points of a hydraulic mechanical arm based on speed feedback, see the attached Figure 3 As shown, this embodiment is described by taking the processing of the PUMA560 hydraulic robot arm as an example. When the robot arm is operated to perform six-degree-of-freedom motion at the end in Cartesian space, the encoder installed on the joint of the robot arm will obtain the current joint angular rate in real time.

[0057] The above-mentioned method for handling singular points of hydraulic manipulators based on speed feedback specifically includes:

[0058] Step 1: Obtain the joint angular rates of the two joints of the singular configuration close to the wrist, namely, joint 4 and joint 6. Specifically, obtain the angular rates of the two adjacent sampling periods of the above two joints, namely, the angular rate of joint 4 at moment k, the angular rate of joint 4 at moment k-1, the angular rate of joint 6 at moment k, and the angular rate of joint 6 at moment k-1;

[0059] In this implementation example, in the above step one, Δ that characterizes the degree of change of the joint angular velocity is calculated based on the angular velocity of joints four and six in two adjacent sampling periods; the closer the robot arm is to the singular configuration, the greater the speed mutation rate generated, so a larger λ is required to model the change of the joint angular velocity according to an exponential function.

[0060]

[0061]

[0062] In the formula, λ max is the upper limit, λ maxThe value of affects the value of λ. If λ max If λ is too large, λ will be too large, resulting in excessive tracking error. max If λ is too small, λ will be too small, and the velocity mutation of the joint angle cannot be effectively suppressed.

[0063] in, is the angular velocity of joint 4 at time k, is the angular velocity of joint 4 at time k-1, is the angular velocity of joint six at time k, is the angular velocity of joint six at time k-1.

[0064] Step 2: square the difference between the angular velocity of joint 4 at time k and the angular velocity of joint 4 at time k-1, square the difference between the angular velocity of joint 6 at time k and the angular velocity of joint 6 at time k-1, sum the two squared data, and then take the square root to obtain the degree of change of the joint angular velocity; then model the degree of change of the joint angular velocity of the hydraulic manipulator according to the exponential function;

[0065] According to the established model, the nonlinear particle swarm parameter optimization algorithm is introduced to obtain the upper limit of the damping factor.

[0066] In this implementation example, in the above step 2, in order to obtain a suitable λ max , introduce the nonlinear particle swarm optimization (NPSO) algorithm. For details of the algorithm, see the attached Figure 2 shown.

[0067] The parameter design in this embodiment is as follows:

[0068] About the inertia weight ω:

[0069] The inertia weight reflects the influence of the historical particle velocity on the current particle velocity.

[0070]

[0071] Among them, through experience, ω max The general value is 0.1, min is 0.9, k is the current iteration number, and G is the maximum iteration number. max With ω min are the maximum and minimum values ​​of ω respectively. In the initial stage, ω has a larger value, which is convenient for improving the global search ability and avoiding falling into the local optimal solution. In the later stage of iteration, ω decreases rapidly under the influence of the exponential function to shorten the convergence time.

[0072] Regarding the learning factors c1, c2, the learning factors affect the global search ability of the algorithm.

[0073]

[0074] Among them, c iu c i The upper limit value, c id c i The lower limit of . In the initial stage, c1>c2, which means that the group search ability is stronger than the individual search ability, to promote a wider range of searches. In the later stage, c2>c1, the individual search ability is stronger than the group search ability, to improve the convergence speed. Through experience, c 1d =c 2d =0.5,c 1u =c 2u =2.

[0075] Fitness function:

[0076] The fitness function is an indicator of whether the current solution is the optimal solution, and the tracking error and the joint angular velocity mutation rate are used as independent variables of the fitness function.

[0077] F = log 10 (∫δ / 1e -3 +1)+log 10 (∫|e| / 1e -3 +1)

[0078] Among them, ∫δ is the absolute integral of the joint angular velocity change rate, δ is the absolute value of Δ, ∫|e| is the integral of the absolute value of the tracking error, and |e| is the absolute value of the tracking error. The tracking error is obtained by subtracting the output robot joint angular velocity command from the actual value of the robot joint angular velocity. The actual value of the robot joint angular velocity can be obtained through the value of the encoder.

[0079] Step 3: Obtain the required damping factor based on the upper limit of the damping factor, and then obtain the Jacobian matrix based on the required damping factor and the command end velocity, and obtain the improved joint angular velocity based on the Jacobian matrix, where: Figure 3 The end command velocity in is the x point in formula 1, the Jacobian matrix is ​​J, and the improved joint angular velocity is the q point in formula 2.

[0080] Formula 2:

[0081] Based on the above scheme, the joint angular velocity is obtained through the encoder, which is a common component in the joint motor and does not require additional detection equipment. The algorithm can automatically adjust the value of the damping factor λ according to the mutation of the angular velocity, greatly reducing the redundancy of the error. Outside the singularity, since the robot arm moves at a constant speed, λ is 0, which has no effect on the tracking accuracy. When approaching the singularity, the speed mutation gradually increases, and λ gradually increases. When gradually moving away from the singularity, the speed mutation decreases, and λ also gradually decreases. After leaving the singularity area, λ drops back to 0.

[0082] In order to verify the effect of the technical solution of this application, a specific experiment was conducted, setting:

[0083] For the hydraulic mechanical arm, please refer to the attached Figure 4 As shown, the corresponding electrical system structure is shown in the attached Figure 5 shown.

[0084] Test results:

[0085] The performance of the example algorithm in this embodiment in suppressing velocity mutations is compared with the traditional damped least squares method at different terminal speeds. Figure 6 (a)-(b) in Figure 7 (a)-(b) and Figure 8 The experimental results show that the proposed algorithm is significantly better than the traditional damped least squares method for the problem of velocity mutation at singular positions.

[0086] Embodiment 2

[0087] The purpose of this embodiment is to provide a computer device, including a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above method when executing the program.

[0088] Embodiment 3

[0089] The purpose of this embodiment is to provide a computer-readable storage medium.

[0090] A computer-readable storage medium stores a computer program, which executes the steps of the above method when executed by a processor.

[0091] Embodiment 4

[0092] The purpose of this embodiment is to provide a hydraulic manipulator singular point processing system based on speed feedback, comprising:

[0093] The modeling module is configured to: model the change of the joint angular rate of the hydraulic manipulator according to an exponential function;

[0094] The upper limit value calculation module of the damping factor is configured to: introduce a nonlinear particle swarm parameter optimization algorithm to obtain the upper limit value of the damping factor according to the established model;

[0095] The joint angular velocity improvement module is configured to obtain the required damping factor based on the upper limit value of the damping factor, then obtain the Jacobian matrix according to the required damping factor and the command end velocity, and obtain the improved joint angular velocity based on the Jacobian matrix.

[0096] Embodiment 5

[0097] The purpose of this embodiment is to provide a computer program product containing instructions, which, when running on a computer, enables the computer to execute the methods and functions involved in any of the above embodiments.

[0098] The steps involved in the apparatus of the above embodiment correspond to the method embodiment 1, and the specific implementation method can refer to the relevant description part of embodiment 1. The term "computer-readable storage medium" should be understood as a single medium or multiple media including one or more instruction sets; it should also be understood to include any medium that can store, encode or carry an instruction set for execution by a processor and enable the processor to execute any method in the present invention.

[0099] Those skilled in the art should understand that the modules or steps of the present invention described above can be implemented by a general-purpose computer device, or alternatively, they can be implemented by a program code executable by a computing device, so that they can be stored in a storage device and executed by the computing device, or they can be made into individual integrated circuit modules, or multiple modules or steps therein can be made into a single integrated circuit module for implementation. The present invention is not limited to any specific combination of hardware and software.

[0100] Although the above describes the specific implementation mode of the present invention in conjunction with the accompanying drawings, it is not intended to limit the scope of protection of the present invention. Those skilled in the art should understand that various modifications or variations that can be made by those skilled in the art on the basis of the technical solution of the present invention without creative work are still within the scope of protection of the present invention.

Claims

1. A method for processing singular points of a hydraulic manipulator based on velocity feedback, characterized in that: include: When the manipulator is operated to perform six-degree-of-freedom motion at the end in Cartesian space, the current joint angular rate of the hydraulic manipulator is obtained; According to the angular velocities of two adjacent sampling periods of two joints close to the wrist singular configuration, the degree of change of the angular velocity of the joints is obtained; Modeling based on the degree of change of the angular velocity of the hydraulic manipulator joints is carried out using an exponential function; According to the established model, the nonlinear particle swarm parameter optimization algorithm is introduced to obtain the upper limit value of the damping factor in the model; The required damping factor is obtained based on the upper limit value of the damping factor, and then the Jacobian matrix is ​​obtained according to the required damping factor and the command end velocity, and the improved joint angular velocity is obtained based on the Jacobian matrix.

2. A method for processing singular points of a hydraulic mechanical arm based on velocity feedback as claimed in claim 1, characterized in that: The hydraulic manipulator joint angular velocity changes when establishing: Get the k-th step joint angular velocity of the hydraulic manipulator; Get the k-1th step joint angular velocity of the hydraulic manipulator; According to the angular velocity of joint 4 and joint 6 in two adjacent sampling periods, the degree of change of the angular velocity of the joint is obtained.

3. A method for processing singular points of a hydraulic mechanical arm based on velocity feedback as claimed in claim 1 or 2, characterized in that: The specific formula to characterize the degree of change of joint angular velocity is: in, is the angular velocity of joint 4 at time k, is the angular velocity of joint 4 at time k-1, is the angular velocity of joint six at time k, is the angular velocity of joint six at time k-1.

4. A method for processing singular points of a hydraulic mechanical arm based on velocity feedback as claimed in claim 1, characterized in that: Modeling based on the exponential function of the change in the angular velocity of the hydraulic manipulator joints: In the formula, λ max is the upper limit, λ max The value of affects the value of λ. If λ max If λ is too large, λ will be too large, resulting in excessive tracking error. max Too small, λ will be too small.

5. A method for processing singular points of hydraulic mechanical arms based on velocity feedback as claimed in claim 1, characterized in that: The nonlinear particle swarm parameter optimization algorithm obtains the upper limit of the damping factor, which includes: Initialization parameters: inertia weight ω, learning factors c1, c2; Establish a fitness function, which is an indicator of whether the current solution is the optimal solution, and use the tracking error and the joint angular velocity mutation rate as independent variables of the fitness function; Find the upper limit of the damping factor.

6. A method for processing singular points of a hydraulic mechanical arm based on velocity feedback as claimed in claim 1, characterized in that: The inertia weight reflects the impact of historical particle velocity on the current particle velocity, and the learning factor affects the global search capability of the algorithm.

7. A hydraulic manipulator singular point processing system based on speed feedback, characterized in that include: The joint angle rate change acquisition module is configured to: acquire the current joint angle rate of the hydraulic manipulator arm when the manipulator arm is operated to perform six-degree-of-freedom motion of the end in Cartesian space; According to the angular velocities of two adjacent sampling periods of two joints close to the wrist singular configuration, the degree of change of the angular velocity of the joints is obtained; The modeling module is configured to: model according to an exponential function based on the degree of change of the joint angular velocity of the hydraulic manipulator; The joint angular velocity improvement module is configured as follows: for the established model, a nonlinear particle swarm parameter optimization algorithm is introduced to obtain the upper limit value of the damping factor in the model; The required damping factor is obtained based on the upper limit value of the damping factor, and then the Jacobian matrix is ​​obtained according to the required damping factor and the command end velocity, and the improved joint angular velocity is obtained based on the Jacobian matrix.

8. A computer program product, comprising a computer program, characterized in that When the computer program is executed by a processor, the method according to any one of claims 1 to 6 is implemented.

9. A computer device comprising a memory, a processor, and a computer program stored in the memory and executable on the processor, characterized in that: When the processor executes the program, the steps of the method described in any one of claims 1 to 6 are implemented.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the program is executed by a processor, the steps of the method described in any one of claims 1 to 6 are performed.

Images