Inverse kinematics solving method for seven-degree-of-freedom multi-bias mechanical arm

Abstract

The invention discloses an inverse kinematics solving method for a seven-degree-of-freedom multi-bias mechanical arm, and relates to the technical field of mechanical arm motion ability analysis. Establishing an inverse kinematics model of the seven-degree-of-freedom multi-bias mechanical arm; defining a shoulder point and a wrist point of the mechanical arm, and establishing an inverse kinematics model based on the dynamic equivalent arm; deriving a joint angle of a real mechanical arm to obtain a nonlinear equation set about theta2 and theta6; solving theta2 and theta6 based on a mixed algorithm of improved LM and improved PSO; and solving theta1, theta3, theta5 and theta7. A concept of a dynamic equivalent arm is put forward, a complex seven joint angle solving problem is converted into a solving problem of two nonlinear equations about two unknown numbers, a complex joint limiting problem is converted into a feasible region of the two unknown numbers, and meanwhile, a mixed algorithm solving strategy based on an improved LM and an improved PSO is put forward. And the solving success rate and the solving efficiency are obviously improved.

Description

Technical Field

[0001] The present invention relates to the technical field of robot arm motion capability analysis, in particular to an inverse kinematics solution method for a seven-degree-of-freedom multi-offset robot arm. Background Art

[0002] With the accelerated advancement of space exploration projects and the construction of space-based platforms, the modular construction of ultra-large orbital facilities has become a core proposition in the development of space technology. In this context, space manipulators, the core supporting equipment of the on-orbit service technology system, are widely used in on-orbit services. Under the "MIRROR" project, the European Space Agency proposed a new multi-arm reconfigurable robotic system. The seven-degree-of-freedom manipulator used in the system is designed with multiple joint offsets. This type of manipulator configuration has a symmetrical joint layout, allowing for forward and reverse installation without structural modification. At the same time, the complex multi-offset configuration has advantages in flexibility and workspace, making it suitable for complex space missions. It has demonstrated significant advantages in tasks such as maintenance of extravehicular equipment on the International Space Station and the capture of failed satellites.

[0003] For on-orbit robotic arm servicing missions, the accuracy, efficiency, and stability of inverse kinematics are crucial. While the geometric significance of arm-angle-based methods is clear and they offer certain unique advantages in configuration control optimization and obstacle avoidance, previous research has primarily focused on SRS configurations. In contrast, the inverse kinematics problem for robotic arms with shoulder, elbow, and wrist offsets is more complex and challenging.

[0004] Although there are currently inverse kinematic solutions for configurations such as SSRMS based on arm angles, these methods cannot be effectively applied to the configuration of a seven-DOF multi-offset manipulator. Furthermore, research on inverse kinematic solutions based on arm angles for configurations similar to seven-DOF multi-offset manipulators is very limited, with most approaches being purely numerical. Therefore, research on the application of arm angle methods to seven-DOF multi-offset manipulator configurations is of great significance. Summary of the Invention

[0005] In order to address the shortcomings of the background technology, the present invention provides an inverse kinematics solution method for a seven-degree-of-freedom multi-offset robotic arm, which proposes the concept of a dynamic equivalent arm, transforms the complex seven-joint angle solution problem into the solution problem of two nonlinear equations about two unknowns, and transforms the complex joint limit problem into the feasible domain of two unknowns. At the same time, a hybrid algorithm solution strategy based on an improved LM and an improved PSO is proposed, which significantly improves the solution success rate and solution efficiency.

[0006] To achieve the above object, the present invention adopts the following technical solution: a method for solving inverse kinematics of a seven-degree-of-freedom multi-offset manipulator, comprising the following steps:

[0007] Step 1: Build an inverse kinematics model for a seven-degree-of-freedom multi-offset manipulator

[0008] The 7-DOF multi-offset manipulator has shoulder offset a1, elbow offset a3, and wrist offset a5. In the initial posture, the axes of joints 2, 4, and 6 are collinear. The MDH system is established and ∑ i (i=0,1,...,7) is the coordinate system, x i (i=0,1,...,7) is the x-axis of the coordinate system, z i (i=0,1,...,7) is the z-axis of the coordinate system, and a is defined i-1 and α i-1 They are respectively around x i-1 Translation and rotation about the axis, d i and θ i They are respectively around z i Translation and rotation of axes;

[0009] Coordinate system Σ i Relative to the coordinate system Σ i-1 The transformation matrix is calculated as follows:

[0010]

[0011] The forward kinematics equation of the robotic arm is:

[0012] 0 T7= 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 6 T7

[0013] The homogeneous transformation matrix of coordinate system Σ7 relative to base coordinate system Σ0 is:

[0014]

[0015] Where, 0 R7 and 0 P7 represents the rotation matrix and position vector of coordinate system Σ7 in base coordinate system Σ0;

[0016] Step 2: Define the shoulder and wrist points of the robotic arm and establish an inverse kinematics model based on the dynamic equivalent arm

[0017] The concept of dynamic equivalent arm is proposed. For joints 1, 2 and 3, when the joint angle of joint 2 is not ±180°, no matter what the seven joint angles are, the straight line extension of z3 can always intersect with z1. The intersection point is recorded as S, which is the shoulder point. The distances between the intersection point and z1 and z3 are recorded as S0 and S′0 respectively. The center of the circle where S0 and S′0 are located is O2. For the rotation of joint 2, S′0S=S0S=S0O2·tan(θ2 / 2) is always true. For joints 5, 6 and 7, when the joint angle 6 is not ±180°, no matter what the seven joint angles are, the straight line extension of z7 It can always intersect with z5, and the intersection point is recorded as W, which is the wrist point. The distances between the intersection point and z5 and z7 are recorded as W0 and W′0 respectively. The center of the circle where W0 and W′0 are located is O6. For the rotation of joint 6, W′0W=W0W=W0O6·tan(θ6 / 2) is always obtained, forming an equivalent SRS manipulator with the same end posture. The equivalent SRS manipulator is built in the same position as the base coordinate system Σ0 and coordinate system Σ7 of the real manipulator. According to the geometric relationship, θ2=θ2′, θ6=θ6′, θ2′ is the angle of joint 2 of the equivalent arm, and θ6′ is the angle of joint 6 of the equivalent arm. Let is the MDH coordinate system of the equivalent arm, x i ′(i=0,1,...,7) is the x-axis of the coordinate system, y i ′(i=0,1,...,7) is the y-axis of the coordinate system, z i ′(i=0,1,...,7) is the z-axis of the coordinate system;

[0018] Step 3: Derive the joint angles of the real robot arm to obtain the nonlinear equations about θ2 and θ6

[0019] S3.1. Derivation of θ4

[0020] For the shoulder-elbow-wrist three-point SEW, when θ4 is 0, set it to the initial state S0-E0-W0, and ∠S0E0W0=arccos((S0E0 2 +E0W0 2 -S0W0 2 ) / 2·S0E0·E0W0), in the real SEW, ∠SEW=arccos((SE 2 +EW 2 -SW 2 ) / 2·SE·EW), then for each SW, θ4 has two possible solutions, namely θ4=∠SEW-∠S0E0W0 and θ4=2π-∠SEW-∠S0E0W0;

[0021] S3.2. Derivation of initial posture

[0022] Define the arm angle ψ, and set the rotation axis vector of joint 1 and The plane formed is the reference plane. and The plane formed is the arm-shaped surface, and the arm-shaped angle ψ is the reference surface around the vector The angle at which the arm surfaces coincide with each other according to the right-hand screw rule;

[0023] It refers to the posture matrix of joint 3 when the arm angle ψ=0. No matter what the values of the seven joint angles of the robot arm are, there is always Therefore, there is in is the posture matrix of coordinate system 0 with arm angle ψ=0, is the posture matrix of coordinate system 3 in coordinate system 0 when the arm angle ψ=0, It refers to the vector when the arm angle ψ=0 It refers to the arm angle ψ=0

[0024] For the convenience of representation, define c i =cosθ i , s i = sinθ i , from the posture transformation relationship, we can know Therefore, there is

[0025] When the arm angle ψ=0, the vector Treated as a rotating vector Rotated by an angle α, we have where the vector R(l,α)=I3+[u l ×]·sinα+[u l ×] 2 (1-cosα), which is obtained by simultaneous solution and are mutually perpendicular unit vectors, which can be expressed by θ2, θ4 and θ6. Right now

[0026] S3.3 Derivation of shoulder joint angle θ2

[0027] Define the coordinate system 3 in the MDH system construction rule around the vector in the coordinate system 0 The attitude transformation matrix after rotation ψ 0 R ψ =I3+[ 0 u w ×]·sinψ+[0 u w ×] 2 ·(1-cosψ), where [ 0 u w ×] is a vector Therefore, the posture of coordinate system 3 in coordinate system 0 is expressed as where a sij (i,j=1,2,3) represents the matrix 0 The element in row i and column j of R3, a sij (i,j=1,2,3) is a nonlinear expression composed of θ2, θ4 and θ6, where a s33 =f(θ2,θ4,θ6);

[0028] According to the posture transformation matrix, According to the equality of corresponding elements in the same matrix, cos(θ2)=a s33 =f(θ2, θ4, θ6), so θ2 = ±arccos(a s33 )=±arccos(f(θ2, θ4, θ6));

[0029] S3.4 Derivation of wrist joint angle θ6

[0030] according to 4 R7= 7 R4 T ,get where a tij (i,j=1,2,3) represents the matrix 0 The element in row i and column j of R3, a tij (i,j=1,2,3) is a nonlinear expression composed of θ2, θ4 and θ6, where a t23 =g(θ2,θ4,θ6);

[0031] According to the posture transformation matrix, According to the equality of corresponding elements in the same matrix, cos(θ6)=a t23 =g(θ2, θ4, θ6), so θ6 = ±arccos(a t23 )=±arccos(g(θ2, θ4, θ6));

[0032] Derivation of S3.5, θ1, θ3, θ5, and θ7

[0033] According to the formula And the corresponding elements of the matrix are equal: θ1=arctan2(a s23 s2,a s13 s2),θ3=arctan2(a s32s2,-a s31 s2),

[0034] According to the formula So: θ5=arctan2(a t33 s6,-a t13 s6), θ7=arctan2(a t22 s6,-a t21 s6);

[0035] Then solve and eliminate θ4, and get two binary nonlinear equations F(θ2,θ6)=0 and G(θ2,θ6)=0;

[0036] Step 4: Solve θ2 and θ6 based on the hybrid algorithm of improved LM and improved PSO

[0037] Build functions based on the system of equations you want to solve, in

[0038] At the current iteration point At , the function value is:

[0039] Jacobian matrix J k is the function H(x k ) in x k The gradient matrix at :

[0040] According to the improved LM algorithm, the LM parameters are:

[0041] λ k =θ||H k || δ +(1-θ)||J k T H k || δ

[0042] in:

[0043]

[0044] The parameter θ∈[0,1] is a control parameter used to balance ||H k || δ and || J k T H k || δ , the parameter δ∈[1,2] is the exponential parameter;

[0045] Termination condition: When || J k T H k ||δ ≤10 -6 Stop iteration when

[0046] At each iteration, the search direction is calculated: d k =-(J k T J k +λ k I) -1 J k T H k , then update the iteration point: x k+1 =x k +d k ;

[0047] There are seven inequality constraints based on the joint angles and joint limits:

[0048] -170≤L i (θ 2(k+1) ,θ 6(k+1) )≤170(i=1,3,5,7)

[0049] -42.5≤L i (θ 2(k+1) ,θ 6(k+1) )≤170(i=2,4,6)

[0050] in:

[0051] L i (θ 2(k+1) ,θ 6(k+1) )=θ i(k+1) ∝(θ 2(k+1) ,θ 6(k+1) )(i=1,3,4,5,7)

[0052] L i (θ 2(k+1) ,θ 6(k+1) )=θ i(k+1) (i=2,6)

[0053] Construct the feasible region of θ2 and θ6, and check each constraint L after each updated iteration point i (θ 2(k+1) ,θ 6(k+1) ) are satisfied, if any constraint is violated, adjust To meet L i (θ 2(k+1) ,θ 6(k+1))=0 (i is an unsatisfied value), ensuring that the seven joint angles are within the joint limits. Then, the approximate arm method is used to solve the median joint limit of each desired posture using the MDH parameter as the initial value of the improved LM algorithm to solve the two binary nonlinear equations F(θ2,θ6)=0 and G(θ2,θ6)=0;

[0054] The improved PSO algorithm is used to solve the desired pose that is not solved by the improved LM algorithm, considering the binary nonlinear equations Construct objective function: H(θ2,θ6)=F 2 (θ2,θ6)+G 2 (θ2, θ6), the problem is transformed into searching for a solution that makes H(θ2, θ6) = 0 and takes the global minimum value within the joint limit, and an exponential decay strategy is adopted for the inertia weight, which is expressed as follows:

[0055]

[0056] Where: new is the inertia weight of the current iteration, ω min is the minimum value of the inertia weight, ω is the initial inertia weight, iter is the current number of iterations, and maxIterations is the maximum number of iterations;

[0057] An adjustment mechanism based on particle distribution is introduced, and the distribution index calculation formula is as follows:

[0058]

[0059] Where: N is the number of particles, D is the number of dimensions, x k,j is the position of particle k in dimension j, is the average position of all particles in dimension j,

[0060] The inertia weight is dynamically adjusted based on the calculated distribution indicator distribution. The specific adjustment rules are as follows:

[0061]

[0062] Where: high is the upper limit of inertia weight, ω low is the lower limit of inertia weight, threshold is the threshold of distribution;

[0063] An exponential decay strategy is adopted for the acceleration coefficient, which is expressed as follows:

[0064]

[0065] Where: c 1new and c 2newis the acceleration factor of the current iteration, c 1min and c 2min is the minimum value of the acceleration coefficient, c s1 and c s2 is the initial acceleration factor;

[0066] The particle velocity update formula combined with the dynamically adjusted inertia weight and acceleration coefficient is expressed as follows:

[0067] v k (i+1)=ω new v k (i)+c 1new r1(p kbest -x k (i))+c 2new r2(g best -x k (i))

[0068] Where: v k (i) is the velocity of particle k at the i-th iteration, p kbest is the individual optimal position of particle k, g best is the global optimal position, r1 and r2 are random numbers with a value range of [0,1];

[0069] x k (i+1)=x k (i)+v k (i+1)

[0070] Where: x k (i) is the position of particle k at the i-th iteration;

[0071] For each particle k, in each iteration, a crossover operation is performed with a crossover probability ∈, which ranges from [0,1]. For each particle k, a random number r=rand() in the range of [0,1] is randomly generated. If r<∈, a crossover operation is performed: x k (i+1)=∈x k (i)+(1-∈)g best ;

[0072] Step 5: Solving θ1, θ3, θ5 and θ7

[0073] According to the solved θ2, θ4 and θ6, it is expressed as 0 R3 and 4 R7, and then according to the formula And the corresponding elements of the matrix are equal: θ1=arctan2(a s23 s2,a s13 s2),θ3=arctan2(a s32 s2,-as31 s2);

[0074] According to the formula So: θ5=arctan2(a t33 s6,-a t13 s6), θ7=arctan2(a t22 s6,-a t21 s6);

[0075] Finally, θ2, θ4 and θ6 are solved, and then the corresponding θ1, θ3, θ5 and θ7 are obtained respectively.

[0076] Compared with the prior art, the present invention has the following beneficial effects: the present invention proposes a new inverse kinematics solution for a seven-degree-of-freedom multi-offset manipulator. First, in order to solve the problem that the definition method of the shoulder point and the wrist point in the traditional arm angle method for this type of manipulator fails, the concept of a dynamic equivalent arm is proposed, which solves the problem that the existing methods cannot introduce the arm angle as a redundant parameter. At the same time, based on the solution of the dynamic equivalent arm, the complex seven-joint angle solution problem is converted into the solution problem of two nonlinear equations about two unknowns, and the complex joint limit problem is converted into the feasible domain of two unknowns, which solves the nonlinear For the problem of solving the system of equations, a hybrid algorithm solution strategy based on improved LM and improved PSO is proposed, and an effective initial value solution strategy is designed, which significantly improves the solution success rate and solution efficiency, filling the gap in the application of arm angle-based solutions in the seven-degree-of-freedom multi-bias robotic arm configuration. Compared with the general algorithms KDL, TRACIK, and BIOIK, the inverse kinematics solution of the present invention has a higher solution success rate (98.434%) and higher solution efficiency (0.6435ms), and shows significant advantages in stability, providing safer and more reliable technical support for on-orbit services. BRIEF DESCRIPTION OF THE DRAWINGS

[0077] Figure 1 It is the MDH system diagram for the seven-degree-of-freedom multi-offset manipulator in the method of the present invention;

[0078] Figure 2 It is a schematic diagram of the dynamic equivalent arm in the method of the present invention;

[0079] Figure 3 Schematic diagram of the arm angle of the dynamic equivalent arm in the method of the present invention;

[0080] Figure 4 It is a flow chart of the method of the present invention. DETAILED DESCRIPTION

[0081] The technical solutions of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the invention, rather than all the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0082] like Figures 1 to 4 As shown, a method for solving inverse kinematics of a seven-degree-of-freedom multi-offset manipulator includes the following steps:

[0083] Step 1: Build an inverse kinematics model for a seven-degree-of-freedom multi-offset manipulator

[0084] The seven-degree-of-freedom multi-offset manipulator is a seven-degree-of-freedom manipulator with only rotational joints, having a shoulder offset a1, an elbow offset a3, and a wrist offset a5. In the initial posture, the axes of joints 2, 4, and 6 are collinear.

[0085] Establish MDH system and combine Figure 1 As shown, define ∑ i (i=0,1,...,7) is the coordinate system, x i (i=0,1,...,7) is the x-axis of the coordinate system, z i (i=0,1,...,7) is the z-axis of the coordinate system, and the MDH parameters are shown in Table 1, where a i-1 and α i-1 They are respectively around x i-1 Translation and rotation about the axis, d i and θ i They are respectively around z i Axis translation and rotation.

[0086] Table 1 MDH parameters of the seven-DOF multi-offset manipulator studied in this paper

[0087] joint <![CDATA[a i-1 / m]]> <![CDATA[α i-1 / (°)]]> <![CDATA[d i / m]]> <![CDATA[θ i / (°)]]> Joint limit / (°) 1 0 0 0 0 -170～170 2 0.066 -90 0 -180 -42.5～170 3 0.066 -90 0.53 -180 -170～170 4 0.066 -90 0 -180 -42.5～170 5 0.066 -90 0.53 -180 -170～170 6 0.066 -90 0 -180 -42.5～170 7 0.066 -90 0 -180 -170～170

[0088] Coordinate system Σ i Relative to the coordinate system Σ i-1 The transformation matrix can be calculated by the following formula:

[0089]

[0090] For the convenience of representation, the following definition of c i =cosθ i , s i = sinθ i .

[0091] The forward kinematics equation of the robotic arm is:

[0092] 0 T7= 0 T1 1 T2 2 T3 3 T4 4 T5 5 T6 6 T7

[0093] The homogeneous transformation matrix of coordinate system Σ7 relative to base coordinate system Σ0 is:

[0094]

[0095] Where, 0 R7 and 0 P7 represents the rotation matrix and position vector of coordinate system Σ7 in base coordinate system Σ0 respectively.

[0096] Step 2: Define the shoulder and wrist points of the robotic arm and establish an inverse kinematics model based on the dynamic equivalent arm

[0097] The concept of dynamic equivalent arm is proposed, combined with Figure 2 As shown, for joints 1, 2, and 3, when the joint angle of joint 2 is not ±180°, regardless of the values of the seven joint angles of the manipulator, the straight extension of the rotation axis z3 of joint 3 always intersects with the rotation axis z1 of joint 1 (when the joint angle of joint 2 is 0°, z1 and z3 coincide), with the intersection point being denoted as S. For the rotation of joint 2, the movement of S′0 is equivalent to a circular change with S′0O2 = S0O2 as the radius and O2 as the center, where S′0O2 is a fixed value, equal to a1 and a2 in the MDH parameters. Therefore, during the movement of the manipulator, S′0S = S0S, and in the triangle SS0O2, S′0S = S0S = S0O2·tan(θ2 / 2). Because the robot arm is symmetrical about the rotation axis of joint 4, for joints 5, 6, and 7, when joint angle 6 is not ±180°, regardless of the values of the seven joint angles, the straight extension of the rotation axis z7 of joint 7 always intersects with the rotation axis z5 of joint 5 (when the joint angle of joint 6 is 0°, z5 and z7 coincide), with the intersection point being denoted as W. For the rotation of joint 6, the movement of W′0 is equivalent to a circular change with radius W′0O6 = W0O6 and center O6, where W′0O6 is a fixed value equal to a6 and a7 in the MDH parameters. Therefore, during the robot arm's motion, W′0W = W0W, and in the triangle WW0O6, W′0W = W0W = W0O6·tan(θ6 / 2).

[0098] At this point, a dynamic equivalent arm with the same SRS configuration and the same end posture is formed. Joints 1 and 2 as well as joints 6 and 7 of the actual manipulator are always non-planar and non-intersecting, which makes the definition method of the shoulder point and wrist point in the traditional arm angle method no longer applicable. Therefore, the shoulder point S and wrist point W of the equivalent SRS manipulator are selected as the shoulder point and wrist point of the real manipulator. It should be noted that for different desired postures and different arm angles, the MDH parameters of the equivalent SRS manipulator are different, and the SRS manipulator being sought is equivalent rather than approximate. The so-called equivalence means that under the current required target posture, the seven joint angles corresponding to the real manipulator and the equivalent SRS manipulator are the same.

[0099] In order to ensure that the equivalent SRS manipulator and the real manipulator have the same end-to-end pose at the same joint angle, the base coordinate system Σ0 and the coordinate system Σ7 of the equivalent SRS manipulator and the real manipulator need to be built at the same location, and according to the geometric relationship, θ2 = θ2′, θ6 = θ6′. Where θ2′ is the angle of joint 2 of the equivalent arm, and θ6′ is the angle of joint 6 of the equivalent arm. is the MDH coordinate system of the equivalent arm, x i ′(i=0,1,...,7) is the coordinate system x-axis, y-axis i ′(i=0,1,...,7) is the coordinate system y-axis, z i ′(i=0,1,...,7) is the coordinate system The z-axis of the robot arm is not unique. Instead, each planned target pose corresponds to an equivalent SRS robot arm. These equivalent SRS robots have the same configuration but different rod lengths, that is, different MDH parameters.

[0100] Specifically, the MDH parameters of the equivalent SRS manipulator are not irregularly different, as shown in Table 2, and can be represented by the joint angles of joints 2 and 6.

[0101] Table 2 MDH parameters of equivalent SRS manipulator

[0102]

[0103] In practice, this is equivalent to changing the link of joint 1 by S0O2·tan(θ2 / 2) in the positive direction of z1, and the link of joint 3 by S0O2·tan(θ2 / 2) in the negative direction of z3. The link of joint 5 changes by W0O6·tan(θ6 / 2) in the positive direction of z5, and the link of joint 7 changes by W0O6·tan(θ6 / 2) in the negative direction of z7. At this point, the joint angles of joints 2 and 6 are not assigned values, but are instead reflected as unknowns in the MDH parameters of the equivalent SRS manipulator.

[0104] Step 3: Derive the joint angles of the real robot arm to obtain the nonlinear equations about θ2 and θ6

[0105] S3.1. Derivation of θ4

[0106] For the shoulder-elbow-wrist three-point SEW, when θ4 is 0, the initial state is S0-E0-W0, then the length of S0E0 is The length of E0W0 is The length of S0W0 is d3+a1·tan(θ2 / 2)+d5+a4·tan(θ6 / 2). In the initial state S0-E0-W0, ∠S0E0W0=arccos((S0E0 2 +E0W0 2 -S0W0 2 ) / 2·S0E0·E0W0). Next, we calculate θ4. When θ4 is not 0, in the real SEW, compared with the initial state S0-E0-W0, SE=S0E0, EW=E0W0, and

[0107] The coordinates of point S are (0,0,-a1·tan(θ2 / 2)), and the direction vector z7 is ( 0 T7(1,3), 0 T7(2,3), 0 T7(3,3)), where 0 T7 is the desired end position, 0 T7(i,j) represents the matrix 0 The element in row i and column j of T7.

[0108] W point coordinates Therefore, there is SW can be expressed by θ2 and θ6.

[0109] In the real SEW, ∠SEW=arccos((SE 2 +EW 2 -SW 2) / 2·SE·EW), then for each SW, θ4 has two possible solutions, namely θ4=∠SEW-∠S0E0W0 and θ4=2π-∠SEW-∠S0E0W0.

[0110] S3.2. Derivation of initial posture

[0111] First, define the arm angle ψ, combined with Figure 3 As shown, let the rotation axis vector of joint 1 be and The plane formed is the reference plane. and The plane formed is the arm-shaped surface, and the arm-shaped angle ψ is the reference surface around the vector According to the right-hand screw rule, rotate until the arm surfaces coincide with each other.

[0112] It refers to the posture matrix of joint 3 when the arm angle ψ=0. Combining the figure and the MDH system construction rule, we can see that no matter what the values of the seven joint angles of the robot arm are, there is always When the arm angle ψ is 0, the above relationship still holds. in is the posture matrix of coordinate system 0 with arm angle ψ=0, is the posture matrix of coordinate system 3 in coordinate system 0 when the arm angle ψ=0, It refers to the vector when the arm angle ψ=0 It refers to the arm angle ψ=0

[0113] From the posture transformation relationship, we can see Therefore, there is

[0114] The following calculation When the arm angle ψ=0, the vector Can be considered as a vector Rotated by an angle α, we have where the vector R(l,α)=I3+[u l ×]·sinα+[u l ×] 2 ·(1-cosα). In triangle SEW, By combining the equations, we can obtain and Right now and are mutually perpendicular unit vectors, so we have The simultaneous equation can be expressed by θ2, θ4 and θ6 Right now

[0115] S3.3 Derivation of shoulder joint angle θ2

[0116] Define the coordinate system 3 in the MDH system construction rule around the vector in the coordinate system 0 The attitude transformation matrix after rotation ψ 0 R ψ =I3+[ 0 u w ×]·sinψ+[ 0 u w ×] 2 ·(1-cosψ), where [ 0 u w ×] is a vector skew-symmetric array.

[0117] Therefore, the posture of coordinate system 3 in coordinate system 0 can be expressed as where a sij (i,j=1,2,3) represents the matrix 0 The element in row i and column j of R3. sij (i, j = 1, 2, 3) is actually a nonlinear expression composed of θ2, θ4 and θ6, where a s33 =f(θ2,θ4,θ6).

[0118] From another perspective, according to the definition of the posture transformation matrix, According to the equality of corresponding elements in the same matrix, cos(θ2)=a s33 =f(θ2, θ4, θ6), so θ2 = ±arccos(a s33 )=±arccos(f(θ2, θ4, θ6)).

[0119] S3.4 Derivation of wrist joint angle θ6

[0120] according to 4 R7= 7 R4 T , we can get where a tij (i,j=1,2,3) represents the matrix 0 The element in row i and column j of R3 can be deduced from the previous text. tij (i, j = 1, 2, 3) is actually a nonlinear expression composed of θ2, θ4 and θ6, where a t23 =g(θ2,θ4,θ6).

[0121] From another perspective, according to the definition of the posture transformation matrix, According to the equality of corresponding elements in the same matrix, cos(θ6)=a t23 =g(θ2, θ4, θ6), so θ6 = ±arccos(a t23 )=±arccos(g(θ2, θ4, θ6)).

[0122] Since θ2, θ4, and θ6 each have two solutions, eight equation combinations can be obtained, each with three nonlinear equations related to θ2, θ4, and θ6. These equations are then solved. The solved θ2, θ4, and θ6 are then substituted into the MDH parameters of the equivalent SRS manipulator to solve for θ1, θ3, θ5, and θ7, as will be explained in detail later. Thus, the complex inverse kinematics problem of the seven-DOF multi-offset manipulator in this paper is transformed into solving three nonlinear equations related to three unknowns (θ2, θ4, and θ6). It is important to note that unlike most other methods based on arm angles, the three nonlinear equations proposed in this paper, based on the concept of the dynamic MDH equivalent SRS manipulator, yield the true θ2, θ4, and θ6 of the seven-DOF multi-offset manipulator. Therefore, the accuracy of the solution to the equations alone is sufficient, and further analysis and correction of the results are unnecessary.

[0123] Derivation of S3.5, θ1, θ3, θ5, and θ7

[0124] According to the above, it can be shown that 0 R3 and 4 R7, according to the formula And the corresponding elements of the matrix are equal: c1s2=a s13 , s1s2=a s23 , that is, θ1=arctan2(a s23 s2,a s13 s2), -s2c3=a s31 , s2s3=a s32 , that is, θ3=arctan2(a s32 s2,-a s31 s2), θ1 and θ3 can both be represented by θ2 and θ6.

[0125] Analogy There is: -c5s6=a t13 , s5s6=a t33 , that is, θ5=arctan2(a t33 s6,-a t13 s6), -s6c7=a t21 , s6s7=a t22, that is, θ7=arctan2(a t22 s6,-a t21 s6), θ5 and θ7 can both be represented by θ2 and θ6.

[0126] Then, these three nonlinear equations are analyzed and solved in detail, θ4 is eliminated, and two binary nonlinear equations F(θ2,θ6)=0 and G(θ2,θ6)=0 are obtained, transforming the three-variable nonlinear equation problem into a two-variable nonlinear problem.

[0127] Step 4: Solve θ2 and θ6 based on the hybrid algorithm of improved LM and improved PSO

[0128] Currently, the main approaches for solving nonlinear systems of equations include traditional optimization algorithms, intelligent algorithms, and hybrid algorithms. Traditional algorithms have the advantages of fast computational speed and mature theory, but are sensitive to initial values and prone to local extrema. Intelligent algorithms have strong global search capabilities and are suitable for multi-extremum problems, but are computationally intensive and converge slowly. Hybrid algorithms, on the other hand, combine the advantages of global search with rapid local convergence, demonstrating excellent stability in solving complex nonlinear problems. To overcome the limitations of a single algorithm, this paper proposes a hybrid algorithm based on a modified Levenberg-Marquardt (LM) algorithm and an improved PSO.

[0129] The improved LM algorithm (2023) is a method between gradient descent and Newton's method. It can adjust the step size by introducing a damping factor. When the current parameters are far from the optimal solution, it uses a gradient descent-like method to ensure stability. When the optimal solution is close, it switches to the Newton method to accelerate convergence. It combines the stability of gradient descent with the fast convergence of the Newton method, and optimizes the parameter selection strategy of LM, making it suitable for nonlinear least squares problems.

[0130] Build functions based on the system of equations you want to solve, in

[0131] At the current iteration point At , the function value is:

[0132] Jacobian matrix J k is the function H(x k ) in x k The gradient matrix at :

[0133] According to the improved LM algorithm, the LM parameters are:

[0134] λ k =θ||H k ||δ +(1-θ)||J k T H k || δ

[0135] in:

[0136]

[0137] The parameter θ∈[0,1] is a control parameter used to balance ||H k || δ and || J k T H k || δ , the parameter δ∈[1,2] is an exponential parameter used to adjust the sensitivity of the parameter.

[0138] Termination condition: When || J k T H k || δ ≤10 -6 Stop iteration when .

[0139] At each iteration, the search direction is calculated: d k =-(J k T J k +λ k I) -1 J k T H k , then update the iteration point: x k+1 =x k +d k .

[0140] To ensure that the solutions are all within the joint limits, constraints need to be added. According to the above derivation, θ1, θ3, θ5, and θ7 can all be represented by θ2 and θ6. Then, according to the joint limits of each joint angle, there are the following seven inequality constraints:

[0141] -170≤L i (θ 2(k+1) ,θ 6(k+1) )≤170(i=1,3,5,7)

[0142] -42.5≤L i (θ 2(k+1) ,θ 6(k+1) )≤170(i=2,4,6)

[0143] in:

[0144] L i (θ2(k+1) ,θ 6(k+1) )=θ i(k+1) ∝(θ 2(k+1) ,θ 6(k+1) )(i=1,3,4,5,7)

[0145] L i (θ 2(k+1) ,θ 6(k+1) )=θ i(k+1) (i=2,6)

[0146] The above inequalities constitute the feasible domain of θ2 and θ6. After each updated iteration point, each constraint L is checked. i (θ 2(k+1) ,θ 6(k+1) ) are satisfied. If any constraint is violated, Projecting back to the feasible region, that is, adjusting To meet L i (θ 2(k+1) ,θ 6(k+1) )=0 (i is an unsatisfied value), ensuring that all seven joint angles are within the joint limits.

[0147] In order to make the working space of the approximate arm include the working space of the real robot arm as much as possible, we stretch d3 and d5 of the approximate arm to The approximate arm is made to reach the farthest end of the real robot arm as much as possible. At the same time, for a very small number of points that are still outside the working space of the approximate arm but within the working space of the real robot arm, there will be no solution using the approximate arm. In this case, the initial value is set to the median value of the joint limit, that is, The MDH parameters are solved according to the above inverse kinematics process to obtain the desired pose. As the initial value of the improved LM algorithm.

[0148] It should be noted that the approximate arm mentioned in the present invention is different from the equivalent SRS manipulator. The approximate arm is a fixed SRS configuration manipulator, and its MDH parameters are shown in Table 3.

[0149] Table 3 MDH parameters of the approximate arm

[0150]

[0151] The function of the approximate arm is to provide an approximate initial value for solving the two binary nonlinear equations F(θ2,θ6)=0 and G(θ2,θ6)=0, which can greatly reduce the solution time and improve the success rate of the solution.

[0152] An improved PSO algorithm (2020) is used to solve the desired poses not found by the improved LM algorithm. First, the binary nonlinear equations are transformed into an objective function minimization problem. The improved PSO algorithm is then used to find the global optimal solution that minimizes the objective function within a given search space. This method not only avoids the traditional method's reliance on initial values but also effectively escapes local extremes, demonstrating high robustness.

[0153] Consider a system of two nonlinear equations Construct objective function: H(θ2,θ6)=F 2 (θ2,θ6)+G 2 (θ2,θ6).

[0154] Obviously, when H(θ2,θ6)=0, (θ2,θ6) is the exact solution of the original system of equations, so the problem is transformed into searching for a solution within the joint limit that makes H(θ2,θ6)=0 take the global minimum value (ideally 0).

[0155] Compared with the traditional PSO algorithm, this paper makes a new design for the inertia weight and adopts an exponential decay strategy. Compared with the traditional linear decay strategy, exponential decay can adjust the inertia weight more flexibly and avoid premature convergence, as shown below:

[0156]

[0157] Where: new is the inertia weight of the current iteration, ω min is the minimum inertia weight, ω is the initial inertia weight, iter is the current number of iterations, and maxIterations is the maximum number of iterations.

[0158] The distribution of particles can reflect the diversity and search status of the group. By introducing an adjustment mechanism based on particle distribution, the inertia weight can be dynamically adjusted to better balance global exploration and local development capabilities. The particle distribution index (standard deviation of position) is used to measure the distribution of the particle group. The distribution index calculation formula is as follows:

[0159]

[0160] Where: N is the number of particles, D is the number of dimensions, x k,j is the position of particle k in dimension j, is the average position of all particles in dimension j,

[0161] According to the calculated distribution index distribution, the inertia weight can be dynamically adjusted. The specific adjustment rules are as follows:

[0162]

[0163] Where: high is the upper limit of inertia weight, which is used to enhance the global exploration capability, ω low It is the lower limit of inertia weight, which is used to enhance local development capability. Threshold is the threshold of distribution, which is used to judge the concentration of particle distribution.

[0164] The acceleration coefficient is designed and an exponential decay strategy is adopted. The acceleration coefficient increases from the initial value c s1 and c s2 Gradually decays to the minimum value c 1min and c 2min , this adjustment strategy can better balance the impact of individual experience and group experience on particle velocity, as shown below:

[0165]

[0166] Where: c 1new and c 2new is the acceleration factor of the current iteration, c 1min and c 2min is the minimum value of the acceleration coefficient, c s1 and c s2 is the initial acceleration factor.

[0167] The particle's velocity update formula combines the dynamically adjusted inertia weight and acceleration coefficient:

[0168] v k (i+1)=ω new v k (i)+c 1new r1(p kbest -x k (i))+c 2new r2(g best -x k (i))

[0169] Where: v k (i) is the velocity of particle k at the i-th iteration, p kbest is the individual optimal position of particle k, g best is the global optimal position, r1 and r2 are random numbers in the range [0,1].

[0170] x k (i+1)=x k (i)+v k (i+1)

[0171] Where: x k (i) is the position of particle k at iteration i.

[0172] In order to increase particle diversity and avoid premature convergence, a crossover operation strategy is adopted. The goal is to move the particle position to the global optimal position g best By introducing the information of the global optimal position, particles are prevented from gathering around the local optimal solution too early, thereby enhancing the algorithm's global exploration capability. By probabilistically adjusting the particle position, the coverage of the search space is increased, preventing the algorithm from falling into the local optimal solution. The details are as follows:

[0173] For each particle k, in each iteration, a crossover operation is performed with a crossover probability ∈, which takes a value in the range [0,1]. For each particle k, a random number r = rand() in the range [0,1] is randomly generated. If r < ∈, a crossover operation is performed: x k (i+1)=∈x k (i)+(1-e)g best .

[0174] The improved PSO algorithm can improve the solution success rate compared to the improved LM algorithm, but the solution efficiency will decrease. This paper adopts a hybrid algorithm solution strategy, which not only has a high solution success rate, but also high solution efficiency.

[0175] Step 5: Solving θ1, θ3, θ5 and θ7

[0176] According to the above, the solved θ2, θ4 and θ6 can be expressed as 0 R3 and 4 R7, and then according to the formula And the corresponding elements of the matrix are equal: c1s2=a s13 , s1s2=a s23 , that is, θ1=arctan 2(a s23 s2,a s13 s2), -s2c3=a s31 , s2s3=a s32 , that is, θ3=arctan 2(a s32 s2,-a s31 s2).

[0177] Analogy There is: -c5s6=a t13 , s5s6=a t33 , that is, θ5=arctan2(a t33 s6,-a t13 s6), -s6c7=a t21 , s6s7=a t22 , that is, θ7=arctan2(a t22 s6,-a t21s6).

[0178] Finally, eight groups of θ2, θ4 and θ6 can be solved, and then eight corresponding groups of θ1, θ3, θ5 and θ7 can be solved respectively, resulting in eight groups of joint angles.

[0179] In summary, the overall solution method process is combined with Figure 4 As shown, the summary is as follows:

[0180] STEP 1. Input the desired pose 0 T7, desired arm angle ψ and solution error ε;

[0181] STEP2. Use the proposed initial value calculation method based on the equivalent SRS manipulator to calculate the desired pose 0 Initial value x0 under T7 and desired arm angle ψ;

[0182] STEP 3. Use the solved x0 as the initial value of the improved LM algorithm to solve θ2 and θ6, and use the method proposed in this paper to solve the other joint angles θ1, θ3, θ5 and θ7 of the real robot arm within the joint limits;

[0183] STEP4. Substitute the inverse kinematics solutions θ1, θ2, ..., θ7 into the forward motion of the real robot arm to calculate the actual pose 0 T′7 and the actual arm angle ψ′, combined with the desired posture 0 T7 and the desired arm angle ψ to solve the position error || Δ pos ||, attitude error||Δ rot || and arm angle error ||Δ||;

[0184] STEP 5. Determine whether ||Δ is satisfied pos ||≤ε、||Δ rot If ||≤ε and ||Δ||≤ε are satisfied, the solution is considered successful and the inverse kinematics solution is saved, and the solution ends. If not, proceed to STEP 6;

[0185] STEP 6. Use the improved PSO algorithm mentioned in this paper to solve θ2 and θ6, and use the method proposed in this paper to solve the other joint angles θ1, θ3, θ5, and θ7 of the real robot arm within the joint limits;

[0186] STEP7. Substitute the inverse kinematics solution θ1, θ2, ..., θ7 into the forward motion of the real robot arm to calculate the actual position 0 T′7 and the actual arm angle ψ′, combined with the desired posture 0 T7 and the desired arm angle ψ are used to solve the pose error ||Δ pos ||, attitude error||Δ rot || and arm angle error ||Δ||;

[0187] STEP8. Determine whether ||Δ is satisfied pos ||≤ε、||Δ rot If ||≤ε and ||Δ||≤ε are satisfied, the solution is considered successful, and the inverse kinematics solution is saved, and the solution ends. If they are not satisfied, the solution is considered failed.

[0188] It will be apparent to those skilled in the art that the present invention is not limited to the details of the exemplary embodiments described above and that the invention can be implemented in other configurations without departing from the spirit or essential characteristics of the invention. Therefore, the embodiments should be considered in all respects as illustrative and non-restrictive, and the scope of the invention is defined by the appended claims, not the foregoing description, and all variations coming within the meaning and range of equivalents of the claims are intended to be embraced therein. Any reference sign in a claim should not be construed as limiting the claim to which it relates.

[0189] In addition, it should be understood that although this specification is described in terms of implementation methods, not every implementation method contains only one independent technical solution. This narrative method of the specification is only for the sake of clarity. Those skilled in the art should regard the specification as a whole. The technical solutions in each embodiment can also be appropriately combined to form other implementation methods that can be understood by those skilled in the art.

Claims

1. A method for solving inverse kinematics of a seven-degree-of-freedom multi-offset manipulator, characterized by: The following steps are involved: Step 1: Build an inverse kinematics model for a seven-degree-of-freedom multi-offset manipulator The 7-DOF multi-offset manipulator has shoulder offset a1, elbow offset a3, and wrist offset a5. In the initial posture, the axes of joints 2, 4, and 6 are collinear. The MDH system is established and ∑ i (i=0,1,...,7) is the coordinate system, x i (i=0,1,...,7) is the x-axis of the coordinate system, z i (i=0,1,...,7) is the z-axis of the coordinate system, and a is defined i-1 and α i-1 They are respectively around x i-1 Translation and rotation about the axis, d i and θ i They are respectively around z i Translation and rotation of axes; Coordinate system Σ i Relative to the coordinate system Σ i-1 The transformation matrix is calculated as follows: The forward kinematics equation of the robotic arm is: <h2 style=";text-align:left;direction:ltr"> 0 <h2 style=";text-align:left;direction:ltr"> T7=<h2 style=";text-align:left;direction:ltr"> 0 <h2 style=";text-align:left;direction:ltr"> T1<h2 style=";text-align:left;direction:ltr"> 1 <h2 style=";text-align:left;direction:ltr"> T2<h2 style=";text-align:left;direction:ltr"> 2 <h2 style=";text-align:left;direction:ltr"> T3<h2 style=";text-align:left;direction:ltr"> 3 <h2 style=";text-align:left;direction:ltr"> T4<h2 style=";text-align:left;direction:ltr"> 4 <h2 style=";text-align:left;direction:ltr"> T5<h2 style=";text-align:left;direction:ltr"> 5 <h2 style=";text-align:left;direction:ltr"> T6<h2 style=";text-align:left;direction:ltr"> 6 <h2 style=";text-align:left;direction:ltr"> T7 The homogeneous transformation matrix of coordinate system Σ7 relative to base coordinate system Σ0 is: Where, 0 R7 and 0 P7 represents the rotation matrix and position vector of coordinate system Σ7 in base coordinate system Σ0; Step 2: Define the shoulder and wrist points of the robotic arm and establish an inverse kinematics model based on the dynamic equivalent arm The concept of dynamic equivalent arm is proposed. For joints 1, 2 and 3, when the joint angle of joint 2 is not ±180°, no matter what the seven joint angles are, the straight line extension of z3 can always intersect with z1. The intersection point is recorded as S, which is the shoulder point. The distances between the intersection point and z1 and z3 are recorded as S0 and S0′ respectively. The center of the circle where S0 and S0′ are located is O2. For the rotation of joint 2, S0′S=S0S=S0O2·tan(θ2 / 2) is always true. For joints 5, 6 and 7, when the joint angle 6 is not ±180°, no matter what the seven joint angles are, the straight line extension of z7 It can always intersect with z5, and the intersection point is recorded as W, which is the wrist point. The distances between the intersection point and z5 and z7 are recorded as W0 and W0′ respectively. The center of the circle where W0 and W0′ are located is O6. For the rotation of joint 6, W0′W=W0W=W0O6·tan(θ6 / 2) is always obtained, forming an equivalent SRS manipulator with the same end posture. The equivalent SRS manipulator is built in the same position as the base coordinate system Σ0 and coordinate system Σ7 of the real manipulator. According to the geometric relationship, θ2=θ2′, θ6=θ6′, θ2′ is the angle of joint 2 of the equivalent arm, and θ6′ is the angle of joint 6 of the equivalent arm. Let is the MDH coordinate system of the equivalent arm, x i ′(i=0,1,...,7) is the x-axis of the coordinate system, y i ′(i=0,1,...,7) is the y-axis of the coordinate system, z i ′(i=0,1,...,7) is the z-axis of the coordinate system; Step 3: Derive the joint angles of the real robot arm to obtain the nonlinear equations about θ2 and θ6 S3.

1. Derivation of θ4 For the shoulder-elbow-wrist three-point SEW, when θ4 is 0, set it to the initial state S0-E0-W0, and ∠S0E0W0=arccos((S0E0 2 +E0W0 2 -S0W0 2 ) / 2·S0E0·E0W0), in the real SEW, ∠SEW=arccos((SE 2 +EW 2 -SW 2 ) / 2·SE·EW), then for each SW, θ4 has two possible solutions, namely θ4=∠SEW-∠S0E0W0 and θ4=2π-∠SEW-∠S0E0W0; S3.

2. Derivation of initial posture Define the arm angle ψ, and set the rotation axis vector of joint 1 and The plane formed is the reference plane. and The plane formed is the arm-shaped surface, and the arm-shaped angle ψ is the reference surface around the vector The angle at which the arm surfaces coincide with each other according to the right-hand screw rule; It refers to the posture matrix of joint 3 when the arm angle ψ=0. No matter what the values of the seven joint angles of the robot arm are, there is always Therefore, there is in is the posture matrix of coordinate system 0 with arm angle ψ=0, is the posture matrix of coordinate system 3 in coordinate system 0 when the arm angle ψ=0, It refers to the vector when the arm angle ψ=0 It refers to the arm angle ψ=0 For the convenience of representation, define c i =cosθ i , s i = sinθ i , from the posture transformation relationship, we can know Therefore, there is When the arm angle ψ=0, the vector Treated as a rotating vector Rotated by an angle α, we have where the vector R(l,α)=I3+[u l ×]·sinα+[u l ×] 2 (1-cosα), which is obtained by simultaneous solution and are mutually perpendicular unit vectors, which can be expressed by θ2, θ4 and θ6. Right now S3.3 Derivation of shoulder joint angle θ2 Define the coordinate system 3 in the MDH system construction rule around the vector in the coordinate system 0 The attitude transformation matrix after rotation ψ 0 R ψ =I3+] 0 u w ×]·sinψ+[ 0 u w ×] 2 ·(1-cosψ), where [ 0 u w ×] is a vector Therefore, the posture of coordinate system 3 in coordinate system 0 is expressed as where a sij (i,j=1,2,3) represents the matrix 0 The element in row i and column j of R3, a sij (i,j=1,2,3) is a nonlinear expression composed of θ2, θ4 and θ6, where a s33 =f(θ2,θ4,θ6); According to the posture transformation matrix, According to the equality of corresponding elements in the same matrix, cos(θ2)=a s33 =f(θ2, θ4, θ6), so θ2 = ±arccos(a s33 )=±arccos(f(θ2, θ4, θ6)); S3.4 Derivation of wrist joint angle θ6 according to 4 R7= 7 R4 T ,get where a tij (i,j=1,2,3) represents the matrix 0 The element in row i and column j of R3, a tij (i,j=1,2,3) is a nonlinear expression composed of θ2, θ4 and θ6, where a t23 =g(θ2,θ4,θ6); According to the posture transformation matrix, According to the equality of corresponding elements in the same matrix, cos(θ6)=a t23 =g(θ2, θ4, θ6), so θ6 = ±arccos(a t23 )=±arccos(g(θ2, θ4, θ6)); Derivation of S3.5, θ1, θ3, θ5, and θ7 According to the formula And the corresponding elements of the matrix are equal: θ1=arctan2(a s23 s2,a s13 s2),θ3=arctan2(a s32 s2,-a s31 s2), According to the formula So: θ5=arctan2(a t33 s6,-a t13 s6), θ7=arctan2(a t22 s6,-a t21 s6); Then solve and eliminate θ4, and get two binary nonlinear equations F(θ2,θ6)=0 and G(θ2,θ6)=0; Step 4: Solve θ2 and θ6 based on the hybrid algorithm of improved LM and improved PSO Build functions based on the system of equations you want to solve, in At the current iteration point At , the function value is: Jacobian matrix J k is the function H(x k ) in x k The gradient matrix at : According to the improved LM algorithm, the LM parameters are: l k =θ||H k || δ +(1-θ)||J k T H k || δ in: The parameter θ∈[0,1] is a control parameter used to balance ||H k || δ and || J k T H k || δ , the parameter δ∈[1,2] is the exponential parameter; Termination condition: When || J k T H k || δ ≤10 -6 Stop iteration when At each iteration, the search direction is calculated: d k =-(J k T J k +λ k I) -1 J k T H k , then update the iteration point: x k+1 =x k +d k ; There are seven inequality constraints based on the joint angles and joint limits: -170≤L i (i 2(k+1) ,i 6(k+1) )≤170(i=1,3,5,7) -42.5≤L i (i 2(k+1) ,i 6(k+1) )≤170(i=2,4,6) in: L i (i 2(k+1) ,i 6(k+1) )=θ i(k+1) ∝(θ 2(k+1) ,i 6(k+1) (i=1,3,4,5,7) L i (i 2(k+1) ,i 6(k+1) )=θ i(k+1) (i=2.6) Construct the feasible region of θ2 and θ6, and check each constraint L after each updated iteration point i (θ 2(k+1) ,θ 6(k+1) ) are satisfied, if any constraint is violated, adjust To meet L i (θ 2(k+1) ,θ 6(k+1) )=0 (i is an unsatisfied value), ensuring that the seven joint angles are within the joint limits. Then, the approximate arm method is used to solve the median joint limit of each desired posture using the MDH parameter as the initial value of the improved LM algorithm to solve the two binary nonlinear equations F(θ2,θ6)=0 and G(θ2,θ6)=0; The improved PSO algorithm is used to solve the desired pose that is not solved by the improved LM algorithm, considering the binary nonlinear equations Construct objective function: H(θ2,θ6)=F 2 (θ2,θ6)+G 2 (θ2, θ6), the problem is transformed into searching for a solution that makes H(θ2, θ6) = 0 and takes the global minimum value within the joint limit, and an exponential decay strategy is adopted for the inertia weight, which is expressed as follows: Where: new is the inertia weight of the current iteration, ω min is the minimum value of the inertia weight, ω is the initial inertia weight, iter is the current number of iterations, and maxIterations is the maximum number of iterations; An adjustment mechanism based on particle distribution is introduced, and the distribution index calculation formula is as follows: Where: N is the number of particles, D is the number of dimensions, x k,j is the position of particle k in dimension j, is the average position of all particles in dimension j, The inertia weight is dynamically adjusted based on the calculated distribution indicator distribution. The specific adjustment rules are as follows: Where: high is the upper limit of inertia weight, ω low is the lower limit of inertia weight, threshold is the threshold of distribution; An exponential decay strategy is adopted for the acceleration coefficient, which is expressed as follows: Where: c 1new and c 2new is the acceleration factor of the current iteration, c 1min and c 2min is the minimum value of the acceleration coefficient, c s1 and c s2 is the initial acceleration factor; The particle velocity update formula combined with the dynamically adjusted inertia weight and acceleration coefficient is expressed as follows: v k (i+1)=ω new v k (i)+c 1new r1(p kbest -x k (i))+c 2new r2(g best -x k (i)) Where: v k (i) is the velocity of particle k at the i-th iteration, p kbest is the individual optimal position of particle k, g best is the global optimal position, r1 and r2 are random numbers with a value range of [0,1]; x k (i+1)=x k (i)+v k (i+1) Where: x k (i) is the position of particle k at the i-th iteration; For each particle k, in each iteration, a crossover operation is performed with a crossover probability ∈, which ranges from [0,1]. For each particle k, a random number r=rand() in the range of [0,1] is randomly generated. If r<∈, a crossover operation is performed: x k (i+1)=∈x k (i)+(1-∈)g best ; Step 5: Solving θ1, θ3, θ5 and θ7 According to the solved θ2, θ4 and θ6, it is expressed as 0 R3 and 4 R7, and then according to the formula And the corresponding elements of the matrix are equal: θ1=arctan2(a s23 s2,a s13 s2),θ3=arctan2(a s32 s2,-a s31 s2); According to the formula So: θ5=arctan2(a t33 s6,-a t13 s6), θ7=arctan2(a t22 s6,-a t21 s6); Finally, θ2, θ4 and θ6 are solved, and then the corresponding θ1, θ3, θ5 and θ7 are obtained respectively.

Images