A singular point processing method and system for a hydraulic mechanical arm based on speed feedback

Abstract

The application provides a singular point processing method and system of a hydraulic mechanical arm based on speed feedback, which comprises the following steps: modeling the joint angle rate change of the hydraulic mechanical arm according to an exponential function; introducing a nonlinear particle swarm parameter optimization algorithm to obtain an upper limit value of a damping factor for the established model; obtaining the required damping factor based on the upper limit value of the damping factor, and then obtaining a Jacobian matrix according to the required damping factor and an instruction end speed, and finally obtaining an improved joint angle speed based on the Jacobian matrix.

Description

Technical Field

[0001] This invention belongs to the field of singularity handling technology for robotic arms, and particularly relates to a method and system for handling singularities in hydraulic robotic arms based on speed feedback. Background Technology

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

[0003] In the inverse kinematics calculation of serial robotic arms, singularity problems are unavoidable. When the robotic arm is in a certain configuration, due to the mathematical characteristics of the inverse kinematics algorithm, infinite joint angular velocities are calculated, causing abrupt changes in the joint angular velocities. For hydraulic robotic arms, singularity problems can cause pressure fluctuations, increase tracking errors, and damage the mechanical equipment.

[0004] like Figure 1 As shown, the PUMA560 hydraulic robotic arm is in a singular configuration at the wrist when the fourth and sixth joints of the serial robot are collinear, i.e., the joint angle of joint 5 is 0 degrees. When the robotic arm is in or near this configuration, due to the mathematical characteristics of the inverse kinematics algorithm, the slight change in the end effector's motion command 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 robotic arm.

[0005]

[0006] In the above formula, J To represent the Jacobian transformation matrix between joint angular velocity and end-effector Cartesian velocity, when joints 4 and 6 are collinear, J It is a singular matrix. As speed approaches infinity, strange phenomena occur. For hydraulic robotic arms, rapid changes in speed can lead to pressure fluctuations, damaging the hydraulic components.

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

[0008]

[0009] Traditional damped least squares methods use constant factors, which can lead to redundant damping outside of singular configurations, thus increasing tracking errors. Improved damped least squares methods, such as those using piecewise functions to obtain the damping factor, introduce velocity discontinuities.

[0010] In summary, for the mechanical arm, such as steel reinforcement rescue in fire fighting, burglary breaking, valve leakage prevention and other special scenes, it is required to be able to operate arbitrarily in front of the mechanical arm, and in this case, the wrist joint singularity point is more likely to appear. In the existing technology, the method of re-planning the trajectory is avoided, or the single-joint shaft control motion is replaced, which has a large error redundancy and reduces the tracking accuracy. SUMMARY

[0011] In order to overcome the shortcomings of the prior art, the present application provides a hydraulic mechanical arm singularity point processing method based on speed feedback, which obtains an improved joint angular velocity based on the joint angular velocity change rate and the damping factor.

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

[0013] In a first aspect, a hydraulic mechanical arm singularity point processing method based on speed feedback is disclosed, comprising:

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

[0015] According to the angular rates of the adjacent two sampling periods of the two joints close to the wrist singularity configuration, the joint angular rate change degree is obtained.

[0016] The hydraulic mechanical arm joint angular rate change degree is modeled based on an exponential function;

[0017] 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;

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

[0019] The singularity point problem exists in both electric or hydraulic driven mechanical arms, and some special problems also exist in hydraulic mechanical arms, so the algorithm corresponding to the technical solution of the present application has a targeted advantage in hydraulic mechanical arms.

[0020] As a further technical solution, the hydraulic mechanical arm joint angular rate change is established as:

[0021] The kth step joint angular velocity of the hydraulic mechanical arm is obtained;

[0022] The k-1th step joint angular velocity of the hydraulic mechanical arm is obtained;

[0023] According to the angular rates of the adjacent two sampling periods of joint four and joint six, the joint angular rate change degree is obtained.

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

[0025]

[0026] wherein, is the angular rate of joint four at time k, is the angular rate of joint four at time k-1, is the angular rate of joint six at time k, is the angular rate of joint six at time k-1.

[0027] As a further technical solution, the degree of change of the hydraulic mechanical arm joint angle rate is modeled based on an exponential function:

[0028]

[0029] wherein, λ max is an upper limit value, and the value of λ max influences the value of λ, if λ max is too large, λ will be too large, resulting in a large tracking error, if λ max is 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 value of the damping factor, specifically including:

[0031] Initialize parameters: inertia weight ω, learning factor c1, c2;

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

[0033] Obtain the upper limit value of the damping factor.

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

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

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

[0037] According to the angular rates of the adjacent two sampling periods of the two joints close to the wrist singularity configuration, the degree of change of the joint angle rate is calculated;

[0038] The modeling module is configured to model the change degree of the joint angle rate of the hydraulic mechanical arm based on an exponential function;

[0039] The joint angle velocity improvement module is configured to introduce a nonlinear particle swarm optimization algorithm to obtain the upper limit value of the damping factor in the model for the established 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 angle velocity is obtained based on the Jacobian matrix.

[0041] The above one or more technical solutions have the following beneficial effects:

[0042] The technical solution of the embodiment obtains the change degree of the joint angle rate according to the adjacent two sampling periods of the joint angle rate close to the wrist singular configuration, establishes a model based on the change degree of the joint angle rate, facilitates subsequent real-time and accurate control of the mechanical arm, and avoids the problems of complex control and large error caused by the way of judging whether the mechanical arm enters the singular region in the prior art.

[0043] After the model is established, the upper limit value of the damping factor is solved by using the nonlinear particle swarm optimization algorithm, compared with the traditional least square method of damping, the damping redundancy is reduced, the tracking error is reduced while the smoothness of the motion near the singular configuration is ensured.

[0044] The advantages of the additional aspects of the present application will be partially given in the following description, partially become obvious from the following description, or be known by the practice of the present application. BRIEF DESCRIPTION OF DRAWINGS

[0045] The drawings accompanying the specification of the present application form part of the present application and are used to provide a further understanding of the present application, the illustrative embodiments of the present application and their description serve to explain the present application and do not constitute an improper limitation of the present application.

[0046] Figure 1 The schematic diagram of the wrist singular configuration structure of the embodiment of the present application;

[0047] Figure 2 The flowchart of the NPSO nonlinear particle swarm optimization algorithm;

[0048] Figure 3 The flowchart of the method of the embodiment of the present application;

[0049] Figure 4 The physical diagram of the hydraulic mechanical arm;

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

[0051] Figure 6 (a)-(b) in FIG. 1 are respectively different damping factors. Figure 8 (a)-(b) in FIG. 1 are respectively different damping factors. DETAILED DESCRIPTION

[0052] It should be noted that the following detailed description is exemplary in nature and is intended to provide further description of the application. Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this application belongs.

[0053] It is to be understood that the terminology used herein is for the purpose of describing particular embodiments only and is not intended to be limiting of example embodiments in accordance with the present application.

[0054] In the case of no conflict, the embodiments in the application and the features in the embodiments can be combined with each other.

[0055] Embodiment one

[0056] The embodiment discloses a hydraulic mechanical arm singularity processing method based on speed feedback, referring to the accompanying drawings. Figure 3 The embodiment is described by taking the PUMA560 type hydraulic mechanical arm as an example. When the mechanical arm performs the six-degree-of-freedom motion of the end in the Cartesian space, the encoder installed on the joint of the mechanical arm can obtain the current joint angular velocity in real time.

[0057] The hydraulic mechanical arm singularity processing method based on speed feedback specifically comprises the following steps.

[0058] Step one: obtaining the joint angular velocities of two joints, i.e., joint 4 and joint 6, close to the wrist singularity configuration, specifically obtaining the angular velocities of the adjacent two sampling periods of the two joints, i.e., the angular velocity of joint 4 at time k, the angular velocity of joint 4 at time k-1, the angular velocity of joint 6 at time k, and the angular velocity of joint 6 at time k-1.

[0059] In the embodiment, according to the angular velocities of joint four and joint six in the adjacent two sampling periods, the Δ representing the change degree of the joint angular velocity is obtained. The closer the mechanical arm is to the singularity configuration, the greater the velocity mutation rate is, and therefore a larger λ is required. The change of the joint angular velocity is modeled according to the exponential function.

[0060]

[0061]

[0062] In the formula, λ max is the upper limit value, and λ maxThe value of λ is affected by the value of the joint angle. If λ is too large, the tracking error will be too large. If λ is too small, the joint angle velocity will change too much and the joint angle velocity change cannot be effectively suppressed. max max

[0063] wherein, is the joint four angular velocity at time k, is the joint four angular velocity at time k-1, is the joint six angular velocity at time k, is the joint six angular velocity at time k-1.

[0064] Step two: square the difference between the joint four angular velocity at time k and the joint four angular velocity at time k-1, square the difference between the joint six angular velocity at time k and the joint six angular velocity at time k-1, and sum the two squared data to obtain the joint angular velocity change degree. Then, the hydraulic manipulator joint angular velocity change degree is modeled based on an exponential function.

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

[0066] In the above step two, in order to obtain a suitable λ max , a nonlinear particle swarm (NPSO) parameter optimization algorithm is introduced. The algorithm is shown in the accompanying drawing. Figure 2

[0067] Regarding the parameter design in the embodiment, the following is specific:

[0068] Regarding the inertia weight ω:

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

[0070]

[0071] wherein, through experience, ω max is generally 0.1, ω min is 0.9, k is the current iteration step number, and G is the maximum iteration step number. ω max max and ω min min are the maximum and minimum values of ω, respectively. In the initial stage, ω has a large value, which facilitates improving the global search ability and avoiding falling into a local optimal solution. In the later iteration stage, ω rapidly decreases under the influence of the exponential function, so as to shorten the convergence time.

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

[0073] ​​​

[0074] wherein c iu is an upper limit value of c i , c id is a lower limit value of c i . In the initial stage, c1>c2, which means the group search ability is stronger than the individual search ability, to promote a wider search. In the later stage, c2>c1, which means the individual search ability is stronger than the group search ability, to improve the convergence speed. Through experience value, 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 taken as independent variables of the fitness function.

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

[0078] wherein ∫δ 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 actual value of the joint angular velocity of the robot arm from the joint angular velocity command of the output robot arm, and the actual value of the joint angular velocity of the robot arm can be obtained by the value of the encoder.

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

[0080] Formula 2:

[0081] ​Based on the above scheme, the joint angular velocity is obtained through the encoder, the encoder is a general component in the joint motor, and additional detection equipment does not need to be separately added; the algorithm can adjust the value of the damping factor lambda according to the mutation of the angular velocity, greatly reducing the redundancy of the error, when outside the singular point, since the manipulator moves at a constant speed, lambda is 0, and the tracking accuracy is not affected, when close to the singular point, the speed mutation gradually increases, lambda gradually increases, when gradually away from the singular point, the speed mutation decreases, lambda also gradually decreases, and after leaving the singular region, lambda is reduced to 0 again.

[0082] In order to verify the effect of the technical scheme of the application, specific tests are made, and the following settings are made:

[0083] The hydraulic manipulator is shown in the accompanying drawings, and the structure of the corresponding electrical system is shown in the accompanying drawings. Figure 4 Figure 5

[0084] Test results:

[0085] The suppression performance of the algorithm of the embodiment on the speed mutation under different end velocities is compared with that of the traditional damping least square method, referring to (a)-(b) in the accompanying drawings, Figure 6 (a)-(b) in the accompanying drawings, Figure 7 (a)-(b) in the accompanying drawings and Figure 8 (a)-(b) in the accompanying drawings. The test results show that the effect of the proposed algorithm is obviously better than that of the traditional damping least square method on the speed mutation problem at the singular position.

[0086] Embodiment two

[0087] The purpose of the embodiment is to provide a computer device, including a memory, a processor and a computer program stored on the memory and executable on the processor, and the processor executes the program to realize the steps of the above method.

[0088] Embodiment three

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

[0090] A computer readable storage medium, having a computer program stored thereon, the program being executed by a processor to execute the steps of the above method.

[0091] Embodiment four

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

[0093] The modeling module is configured to model the change of the joint angular velocity of the hydraulic manipulator as 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 for the established model.

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

[0096] Embodiment five

[0097] The purpose of the present embodiment is to provide a computer program product containing instructions which, when run on a computer, cause the computer to perform the method and functions involved in any of the above embodiments.

[0098] The steps and methods involved in the device of the above embodiments correspond to Embodiment One, and the specific implementation can refer to the relevant description of Embodiment One. The term "computer readable storage medium" should be understood to include a single medium or multiple media of one or more instruction sets; it should also be understood to include any medium capable of storing, encoding, or carrying instruction sets for execution by a processor and causing the processor to perform any of the methods in the present application.

[0099] Those skilled in the art should understand that the above-mentioned modules or steps of the present application can be realized by a general computer device, and alternatively, they can be realized by program codes executable by a computing device, so that they can be stored in a storage device for execution by a computing device, or they can be made into individual integrated circuit modules, or a plurality of modules or steps among them can be made into a single integrated circuit module. The present application is not limited to any specific combination of hardware and software.

[0100] The above describes the specific embodiments of the present application in conjunction with the accompanying drawings, but is not a limitation on the scope of protection of the present application. Those skilled in the art should understand that various modifications or variations made by those skilled in the art on the basis of the technical solutions of the present application without creative labor are still within the scope of protection of the present application.

Claims

1. A velocity feedback based singularity handling method for a hydraulic manipulator, characterized in that, The method comprises the following steps: When operating the manipulator to perform six-degree-of-freedom motion of the end in the Cartesian space, the current joint angular velocity of the hydraulic manipulator is obtained; According to the angular velocities of the two joints close to the wrist singularity configuration in two adjacent sampling periods, the degree of change of the joint angular velocity is calculated, the two joints close to the wrist singularity configuration being joint four and joint six; The degree of change of the joint angular velocity of the hydraulic manipulator is modeled based on an exponential function; In the formula, is an upper limit value, The value of The value of If it is too large, The tracking error will be too large, and if If it is too small, It will be too small. The specific formula for representing the degree of change of the joint angular velocity is: ； wherein, is the angular rate of joint four at time k, is the angular rate of joint four at time k-1, is the angular rate of joint six at time k, is the angular rate of joint six at time k-1; 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; Based on the upper limit value of the damping factor, the required damping factor is obtained, and then the Jacobian matrix is obtained according to the required damping factor and the command end speed, and the improved joint angular velocity is obtained based on the Jacobian matrix.

2. The method of claim 1, wherein the singular point processing method is based on velocity feedback. The degree of change of the joint angular velocity of the hydraulic manipulator is established as follows: The kth step joint angular velocity of the hydraulic manipulator is obtained; The k-1th step joint angular velocity of the hydraulic manipulator is obtained; According to the angular velocities of joint four and joint six in two adjacent sampling periods, the degree of change of the joint angular velocity is calculated.

3. The method of claim 1, wherein the velocity feedback is introduced as The nonlinear particle swarm parameter optimization algorithm is used to obtain the upper limit value of the damping factor, which specifically comprises: Initialization parameters: inertia weight , learning factor , ; An fitness function is established, which 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; The upper limit value of the damping factor is obtained.

4. The method of claim 3, wherein the velocity feedback-based singularity handling method for a hydraulic robot arm is characterized by, The inertia weight reflects the influence of the historical particle velocity on the current particle velocity, and the learning factor affects the global search ability of the algorithm.

5. A velocity feedback based singularity handling system for a hydraulic manipulator, characterized by The method comprises the following steps: The joint angular velocity change acquisition module is configured to: when operating the manipulator to perform six-degree-of-freedom motion of the end in the Cartesian space, obtain the current joint angular velocity of the hydraulic manipulator; According to the angular velocities of the two joints close to the wrist singularity configuration in two adjacent sampling periods, the degree of change of the joint angular velocity is calculated, the two joints close to the wrist singularity configuration being joint four and joint six; The modeling module is configured to: model the degree of change of the joint angular velocity of the hydraulic manipulator based on an exponential function; In the formula, is an upper limit value, The value of The value of If it is too large, The tracking error will be too large, and if If it is too small, It will be too small. The specific formula for representing the degree of change of the joint angular velocity is: ； wherein, is the angular rate of joint four at time k, is the angular rate of joint four at time k-1, is the angular rate of joint six at time k, is the angular rate of joint six at time k-1; The joint angular velocity improvement module is configured to: for the established model, introduce a nonlinear particle swarm parameter optimization algorithm to obtain the upper limit value of the damping factor in the model; Based on the upper limit value of the damping factor, the required damping factor is obtained, and then the Jacobian matrix is obtained according to the required damping factor and the command end speed, and the improved joint angular velocity is obtained based on the Jacobian matrix.

6. A computer program product comprising a computer program, characterized in that, The computer program is executed by the processor to implement the method of any one of claims 1 to 4.

7. A computer device comprising a memory, a processor, and a computer program stored on the memory and executable on the processor, characterized in that, The processor executes the program to implement the steps of the method of any one of claims 1 to 4.

8. A computer readable storage medium having stored thereon a computer program, characterized in that, The program is executed by the processor to perform the steps of the method of any one of claims 1 to 4.

Images