Method and apparatus for optimizing inverse kinematic motion paths for multi-joint mechanisms

Abstract

The application relates to the field of engineering machinery, and discloses a method and device for optimizing inverse solution motion paths of a multi-joint mechanism, the method comprising: optimizing the inverse solution motion paths according to priorities of at least two preset mechanism posture optimization targets, wherein the optimization result of the inverse solution motion path corresponding to a high-priority preset mechanism posture optimization target is based on the optimization result of the inverse solution motion path corresponding to a low-priority preset mechanism posture optimization target. In this way, when optimization is performed for at least two optimization targets, a weighted method is no longer used to integrate the at least two optimization targets, and the problem that the influence of different weighting coefficients on the optimization effect is different is solved.

Description

Technical Field

[0001] This invention relates to the field of engineering machinery, and more specifically, to a method and apparatus for optimizing the inverse kinematic path of a multi-joint mechanism. Background Technology

[0002] Inverse kinematics of a redundant degree-of-freedom boom is a crucial step in control analysis. Because a redundant degree-of-freedom boom does not satisfy the Pieper criterion, its inverse kinematics lacks a closed-form solution and must be solved numerically. Currently, the commonly used method is at the velocity level, based on a system of linear equations relating the end effector velocity and joint velocities. Solving for the general solution yields the following form: in It is the minimum norm solution for velocity. This is the homogeneous general solution part. J + =J T (JJ T ) -1 Let φ be the generalized inverse of matrix J, I be the identity matrix, and φ be an arbitrary vector in the null space of the Jacobian matrix J. By choosing an appropriate φ, and utilizing the "self-motion" characteristic of the redundant robotic arm, the motion path of the boom can be optimized without changing the velocity at the end of the boom.

[0003] Existing technologies disclose motion optimization methods for robots with redundant degrees of freedom, including simplified humanoid robot configuration diagrams as follows: Figure 1 As shown, the gradient projection method is used to optimize the inverse kinematics of a redundant degree-of-freedom mechanism. The optimization objectives are preventing joint overruns and improving operability. A weighted method is used to combine the two optimization objectives. The range of the amplification factor is calculated based on the limits of joint velocity and joint acceleration. Currently, gradient projection method is often used for inverse kinematics optimization of redundant degree-of-freedom cascade mechanisms. Among them, for multi-objective optimization strategies, a weighted method is often used to combine multiple optimization objectives. However, different weighting coefficients have vastly different effects on the optimization effect. Summary of the Invention

[0004] The purpose of this invention is to provide a method and apparatus for optimizing the inverse kinematic path of a multi-joint mechanism, which can solve or at least partially solve the above-mentioned problems.

[0005] To achieve the above objectives, one aspect of the present invention provides a method for optimizing the inverse kinematic path of a multi-joint mechanism, the method comprising: optimizing the inverse kinematic path according to the priority of at least two preset mechanism posture optimization targets, wherein the optimization result of the inverse kinematic path corresponding to the higher priority preset mechanism posture optimization target is based on the optimization result of the inverse kinematic path corresponding to the lower priority preset mechanism posture optimization target.

[0006] Optionally, optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism. The method further includes: for any of the preset mechanism posture optimization targets, when the gradient projection method is used to optimize the inverse motion path, determining the amplification factor according to the preset amplification factor determination rule corresponding to the preset mechanism posture optimization target.

[0007] Optionally, the at least two preset mechanism posture optimization objectives include end-structure posture optimization and joint over-limit prevention optimization, wherein the joint over-limit prevention optimization does not target structures with corresponding anti-joints.

[0008] Optionally, when the multi-joint structure includes anti-joint, the at least two preset mechanism posture optimization objectives also include anti-joint optimization.

[0009] Optionally, when optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism and using the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the joint over-limit optimization includes the following formula: Δ 3,j q j =|q j -q jlimit |,q jlimit =α·sgn(H 3,j )·((q jmid -q jmin )+q jmid ), q jmid =(q jmin +q jmax ) / 2, where k 3,j H represents the amplification factor corresponding to the joint over-limit optimization of structure j in the multi-joint mechanism. 3,j This represents the joint over-limit prevention optimization function corresponding to structure j. This represents the gradient of the joint over-limit prevention optimization function corresponding to structure j. This represents the optimized vector of joint velocities in the multi-joint mechanism. The value represents the optimized value of the joint velocity of structure j, δt represents the optimization time, and I represents the identity matrix. 3,j J represents the Jacobian matrix corresponding to the joint over-limit optimization performed on the structure j. 3,j J + This represents the Jacobian matrix corresponding to the joint over-limit optimization performed on structure j. 3,j The pseudo-inverse matrix of J, q j This represents the joint angle of structure j. The multi-joint mechanism has n structures in total. α is a first preset value, and q... jminq represents the minimum joint angle of structure j. jmax This represents the maximum joint angle of structure j.

[0010] Optionally, the joint over-limit prevention optimization function corresponding to structure j is calculated according to the following formula: β j =(q jmin +q jmax ) / 2.

[0011] Optionally, when optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism and using the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the final structure posture optimization includes: the amplification factor corresponding to the final structure posture optimization is a preset amplification factor.

[0012] Optionally, when optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism and using the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the anti-joint optimization includes the following formula: Δq i =q i -q it , where k 2,i This represents the amplification factor corresponding to the anti-joint optimization performed on structure i in the multi-joint mechanism. This represents the optimized vector of joint velocities in the multi-joint mechanism. Let δt represent the optimization amount of the joint velocity of structure i, δt represent the optimization time, I represent the identity matrix, and J represent the Jacobian matrix. + H represents the pseudo-inverse of the Jacobian matrix J. 2,i This represents the anti-joint optimization function corresponding to structure i. q represents the gradient of the inverse joint optimization function corresponding to structure i. i This represents the joint angle of structure i, and the multi-joint mechanism has n such structures, q it The target joint angle of structure i is indicated.

[0013] Optionally, the anti-joint optimization function corresponding to structure i is calculated according to the following formula: q it =(k i x-β i )·π / 180, b=max(|q it -q imin |,|q it -q imax |), where ki β represents the second preset value corresponding to structure i. i The third preset value corresponding to structure i is represented by q, x represents the x-axis component of the coordinate of the end of the multi-joint mechanism, and q represents the third preset value corresponding to structure i. imin q represents the minimum joint angle of structure i. imax This represents the maximum joint angle of structure i.

[0014] Accordingly, another aspect of the present invention provides an apparatus for optimizing the inverse kinematic path of a multi-joint mechanism. The apparatus includes an optimization module for optimizing the inverse kinematic path according to the priority of at least two preset mechanism posture optimization targets, wherein the optimization result of the inverse kinematic path corresponding to the higher priority preset mechanism posture optimization target is based on the optimization result of the inverse kinematic path corresponding to the lower priority preset mechanism posture optimization target.

[0015] Optionally, optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism. The device further includes an amplification factor determination module, which determines the amplification factor according to the preset amplification factor determination rule corresponding to any preset mechanism posture optimization target when the gradient projection method is used to optimize the inverse motion path.

[0016] Optionally, the optimization module optimizes the inverse motion path based on the at least two preset mechanism posture optimization objectives, including end structure posture optimization and joint over-limit prevention optimization, wherein the joint over-limit prevention optimization does not target structures where the corresponding joints are anti-joints.

[0017] Optionally, when the multi-joint structure includes anti-joint optimization, the optimization module further includes anti-joint optimization when optimizing the inverse motion path based on the at least two preset mechanism posture optimization objectives.

[0018] Optionally, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification coefficient corresponding to the joint over-limit optimization includes the following formula: Δ 3,j q j =|q j -q jlimit |,q jlimit =α·sgn(H 3,j )·((q jmid -q jmin )+q jmid ), q jmid =(q jmin +qjmax ) / 2, where k 3,j H represents the amplification factor corresponding to the joint over-limit optimization of structure j in the multi-joint mechanism. 3,j This represents the joint over-limit prevention optimization function corresponding to structure j. This represents the gradient of the joint over-limit prevention optimization function corresponding to structure j. This represents the optimized vector of joint velocities in the multi-joint mechanism. The value represents the optimized value of the joint velocity of structure j, δt represents the optimization time, and I represents the identity matrix. 3,j J represents the Jacobian matrix corresponding to the joint over-limit optimization performed on the structure j. 3,j J + This represents the Jacobian matrix corresponding to the joint over-limit optimization performed on structure j. 3,j The pseudo-inverse matrix of J, q j This represents the joint angle of structure j. The multi-joint mechanism has n structures in total. α is a first preset value, and q... jmin q represents the minimum joint angle of structure j. jmax This represents the maximum joint angle of structure j.

[0019] Optionally, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the joint anti-overlimit optimization function corresponding to structure j is calculated according to the following formula: β j =(q jmin +q jmax ) / 2.

[0020] Optionally, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification coefficient corresponding to the final structure posture optimization includes: the amplification coefficient corresponding to the final structure posture optimization is a preset amplification coefficient.

[0021] Optionally, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the anti-joint optimization includes the following formula: Δq i =q i -q it , where k 2,iThis represents the amplification factor corresponding to the anti-joint optimization performed on structure i in the multi-joint mechanism. This represents the optimized vector of joint velocities in the multi-joint mechanism. Let δt represent the optimization amount of the joint velocity of structure i, δt represent the optimization time, I represent the identity matrix, and J represent the Jacobian matrix. + H represents the pseudo-inverse of the Jacobian matrix J. 2,i This represents the anti-joint optimization function corresponding to structure i. q represents the gradient of the inverse joint optimization function corresponding to structure i. i This represents the joint angle of structure i, and the multi-joint mechanism has n such structures, q it The target joint angle of structure i is indicated.

[0022] Optionally, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the anti-joint optimization function corresponding to structure i is calculated according to the following formula: q it =(k i x-β i )·π / 180, b=max(|q it -q imin |,|q it -q imax |), where k i β represents the second preset value corresponding to structure i. i The third preset value corresponding to structure i is represented by q, x represents the x-axis component of the coordinate of the end of the multi-joint mechanism, and q represents the third preset value corresponding to structure i. imin q represents the minimum joint angle of structure i. imax This represents the maximum joint angle of structure i.

[0023] Furthermore, another aspect of the present invention provides an engineering machine that includes the aforementioned device.

[0024] In addition, another aspect of the present invention provides a machine-readable storage medium storing instructions that cause a machine to perform the method described in any one of claims 1-9.

[0025] Through the above technical solution, when optimizing the inverse kinematic path of a multi-joint mechanism for at least two preset mechanism posture optimization targets, the inverse kinematic path is optimized according to the priority of the preset mechanism posture optimization targets. The optimization result of the inverse kinematic path corresponding to the higher priority preset mechanism posture optimization target is based on the optimization result of the inverse kinematic path corresponding to the lower priority preset mechanism posture optimization target. An ordered serial optimization mechanism is implemented. In this way, when optimizing for at least two optimization targets, the weighted method is no longer used to combine at least two optimization targets, which solves the problem that different weighting coefficients have vastly different effects on the optimization effect.

[0026] Other features and advantages of the present invention will be described in detail in the following detailed description section. Attached Figure Description

[0027] The accompanying drawings are provided to further illustrate the invention and form part of the specification. They are used together with the following detailed description to explain the invention, but do not constitute a limitation thereof. In the drawings:

[0028] Figure 1 This is a simplified diagram of a humanoid robot's configuration;

[0029] Figure 2 This is a simplified structural diagram of a concrete pump truck provided in an embodiment of the present invention;

[0030] Figure 3 This is a logic diagram of inverse kinematic path optimization for multi-joint mechanisms provided in another embodiment of the present invention;

[0031] Figure 4 This is a schematic diagram of the boom joint angle provided in another embodiment of the present invention;

[0032] Figure 5 This is a schematic diagram of the boom tilt angle provided in another embodiment of the present invention;

[0033] Figure 6 This is a schematic diagram illustrating the optimization effect of task direction operability provided by another embodiment of the present invention; and

[0034] Figure 7 This is a schematic diagram of the motion configuration before and after optimization provided in another embodiment of the present invention.

[0035] Explanation of reference numerals in the attached figures

[0036] 1. Body 2. Turntable

[0037] 3 One arm 4 Two arms

[0038] 5 Three-armed 6 Four-armed

[0039] 7 Five arms 8 Six arms Detailed Implementation

[0040] The specific embodiments of the present invention will be described in detail below with reference to the accompanying drawings. It should be understood that the specific embodiments described herein are for illustration and explanation only and are not intended to limit the present invention.

[0041] In existing technologies, taking a six-segment robotic arm with redundant degrees of freedom as the research object, the gradient projection method is used to address its inverse motion optimization problem: The optimization function is H = n -1 ·∑[(q l -a l ) / (a l -q min ) 2 ], J represents the joint velocity, and J represents the Jacobian matrix. + Describes the pseudo-inverse of the Jacobian matrix. The value represents the velocity at the end of the robotic arm, k represents the magnification factor, I represents the identity matrix, and a represents the velocity at the end of the robotic arm. l q lmin and q lmax The median, where n represents the total number of arms, q min q represents the minimum joint angle of the joint arm corresponding to the reverse joint. l q represents the joint angle of the articulated arm l. lmin q represents the minimum joint angle of the articulated arm l. lmax This represents the maximum joint angle of the articulated arm l. Furthermore, in existing technologies, humanoid robots are also studied, and the gradient projection method is used for the inverse solution of redundant mechanisms. To address the joint over-limit problem, the optimization function used is... Where, q l q represents the joint angle of the articulated arm l. lmin q represents the minimum joint angle of the articulated arm l. lmax This represents the maximum joint angle of the articulated arm l. The technical solution provided by this invention is an optimization of the inverse kinematics problem for multi-joint mechanisms.

[0042] One aspect of this invention provides a method for optimizing the inverse kinematic path of a multi-joint mechanism. The method includes the following: optimizing the inverse kinematic path based on the priority of at least two preset mechanism posture optimization goals, wherein the optimization result of the inverse kinematic path corresponding to a higher-priority preset mechanism posture optimization goal is based on the optimization result of the inverse kinematic path corresponding to a lower-priority preset mechanism posture optimization goal. The inverse kinematic path is the kinematic path of the multi-joint mechanism when the end-effector velocity remains unchanged, and the end-effector is the end of the multi-joint mechanism that can move freely when mounted on a device. For example, the inverse kinematic path can be represented by the joint angles of the structure in the multi-joint structure. The joint angles can be obtained directly by measuring the joint angles using a joint angle measuring device, or indirectly by measuring measurements related to the joint angles.

[0043] A multi-joint mechanism comprises multiple structures and multiple joints, with adjacent structures connected by joints. For example, a multi-joint mechanism can be the boom of a pump truck, a robotic arm, etc., where the boom includes a segmented boom structure. In this embodiment of the invention, the motion parameters of the structures within the multi-joint mechanism can be used to represent the motion path; for example, the motion parameters can be the joint angles or tilt angles of the structures within the multi-joint mechanism.

[0044] Through the above technical solution, when optimizing the inverse kinematic path of a multi-joint mechanism for at least two preset mechanism posture optimization targets, the inverse kinematic path is optimized according to the priority of the preset mechanism posture optimization targets. The optimization result of the inverse kinematic path corresponding to the higher priority preset mechanism posture optimization target is based on the optimization result of the inverse kinematic path corresponding to the lower priority preset mechanism posture optimization target. An ordered serial optimization mechanism is implemented. In this way, when optimizing for at least two optimization targets, the weighted method is no longer used to combine at least two optimization targets, which solves the problem that different weighting coefficients have vastly different effects on the optimization effect.

[0045] Optionally, in this embodiment of the invention, optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism. The method further includes: for any preset mechanism posture optimization target, when optimizing the inverse motion path using the gradient projection method, determining the amplification coefficient according to the preset amplification coefficient determination rule corresponding to the preset mechanism posture optimization target. In the prior art, the selection of the amplification coefficient is often based on constraints such as joint velocity and joint acceleration, calculating a feasible range of amplification coefficients rather than providing an optimal amplification coefficient, thus failing to fully achieve the optimization target. In this embodiment of the invention, the amplification coefficient is determined according to the preset amplification coefficient determination rule corresponding to the preset mechanism posture optimization target. Thus, during optimization, a definite value of the amplification coefficient is given instead of a range, thereby fully achieving the optimization target. Furthermore, in this embodiment of the invention, the joint angle of the structure connected to the device installing the multi-joint mechanism is the angle between the structure and the horizontal plane; for structures not connected to the device installing the multi-joint mechanism, the joint angle of a structure is the angle between the structure and the extension line of the previous structure. The vertical relationship between structures is determined by their distance from the equipment when the multi-joint mechanism is deployed. Between adjacent structures, the one closer to the equipment is the upper structure, and the one farther away is the lower structure. For example, ... Figure 4 As shown, taking a concrete pump truck as an example, the multi-joint mechanism is the boom, and the structure in the boom is the segmented boom. The boom is installed on the turntable of the pump truck. The joint angle of the three booms 5 is q3, which is the angle between the extension lines of the three booms 5 and the two booms 4. The two booms 4 are the upper segment of the three booms. The one boom 3 is the segmented boom connecting the turntable. The joint angle of the one boom 3 is the angle q1 between the one boom 3 and the horizontal plane.

[0046] Optionally, in this embodiment of the invention, at least two preset mechanism posture optimization objectives include end-structure posture optimization and joint over-limit prevention optimization. The joint over-limit prevention optimization does not target structures where the corresponding joints are anti-joints. The end-structure is the structure furthest from the device when the multi-joint mechanism is deployed. For example, such as... Figure 4 As shown, for the boom of the pump truck, when the boom is extended, the boom segment furthest from the pump truck turntable is the sixth boom segment (8), which is the last boom segment, also known as the final structure. Final structure attitude optimization can minimize the difference between the final structure's attitude and its preset target attitude. Specifically, the tilt angle of the final structure is used to express its attitude, and final structure attitude optimization can minimize the difference between the tilt angle of the final structure and the preset target tilt angle. For a given structure, joint over-limit optimization can make the structure as close as possible to the middle position within its corresponding range of motion. Specifically, the joint angles of the structure are used to represent its movement position, and for a given structure, joint over-limit optimization can make the joint angles of the structure as close as possible to the middle joint angle within its corresponding range of joint angles.

[0047] Optionally, in this embodiment of the invention, when the multi-joint structure includes anti-joints, at least two preset mechanism posture optimization objectives also include anti-joint optimization. For an anti-joint, anti-joint optimization can be to minimize the difference between the motion parameters of the structure corresponding to the anti-joint and the preset target motion parameters. The structure corresponding to an anti-joint is the next structure connected by the anti-joint when the multi-joint mechanism is mounted on the device and the multi-joint mechanism is deployed. The vertical relationship of the structures is distinguished according to their distance from the device when the multi-joint mechanism is mounted on the device and deployed; for two structures connected by one joint, the one closer to the device is the upper structure, and the one farther from the device is the lower structure. For example, such as... Figure 4 As shown, taking a concrete pump truck as an example, the multi-joint mechanism is the boom, and the structure within the boom is the segmented boom. The boom is mounted on the turntable of the pump truck. The joint connecting the three-arm 5 and the four-arm 6 is a reverse joint. The three-arm 5 is the structure above the reverse joint, and the four-arm 6 is the structure below the reverse joint. Specifically, the motion parameter can be the joint angle. For a reverse joint, reverse joint optimization can be to minimize the difference between the joint angle corresponding to the reverse joint and the preset target joint angle.

[0048] Optionally, in this embodiment of the invention, when optimizing the inverse motion path involves optimizing the joint angles of a multi-joint mechanism and using the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the joint over-limit optimization includes the following formula:

[0049]

[0050]

[0051]

[0052] Δ 3,j q j =|q j -q jlimit |

[0053] q jlimit =α·sgn(H 3,j )·((q jmid -q jmin )+q jmid )

[0054] q jmid =(q jmin +q jmax ) / 2

[0055] Where, k 3,j H represents the amplification factor corresponding to the joint over-limit optimization of structure j in a multi-joint mechanism. 3,jThis represents the joint over-limit prevention optimization function corresponding to structure j. This represents the optimized vector of joint velocities in a multi-joint mechanism. This represents the gradient of the joint over-limit prevention optimization function corresponding to structure j. Let δt represent the optimization amount of the joint velocity of structure j, δt represent the optimization time, and I represent the identity matrix. 3,j J represents the Jacobian matrix corresponding to the joint over-limit optimization of structure j. 3,j J + This represents the Jacobian matrix corresponding to the joint over-limit optimization of structure j. 3,j The pseudo-inverse matrix of J, q j This represents the joint angle of structure j. The multi-joint mechanism has n structures in total. α is a first preset value, and q... jmin q represents the minimum joint angle of structure j. jmax This represents the maximum joint angle of structure j. Here, δt can be a pre-set value. The optimized joint velocity vector of a structure is a vector composed of the optimized joint velocities of the structures included in the multi-joint mechanism. The optimized joint velocity of a structure is the change in joint velocity before and after the joint over-limit optimization calculation. Furthermore, j is the sequence number of structure j. The structures in the multi-joint mechanism are ordered according to their distance from the equipment when the multi-joint mechanism is installed on the equipment and deployed; the greater the distance, the larger the sequence number. For example, as... Figure 4 As shown, taking a concrete pump truck as an example, the multi-joint mechanism is the boom, and the structure within the boom is the segmented boom. The boom is installed on the turntable of the pump truck, and according to the distance from the turntable from near to far, the booms are numbered 1, 2, 3, 4, 5, 6, 7, and 8 respectively. Furthermore, in this embodiment of the invention, the method described here can be used to sort the structures when necessary. `sgn` is a sign function; `sgn(a)` returns the sign of the parameter `a`. 3j Return H 3j The plus or minus sign, return The plus or minus sign.

[0056] Optionally, in this embodiment of the invention, the joint over-limit protection optimization function corresponding to structure j is calculated according to the following formula: β j =(q jmin +q jmax ) / 2. Where H 3,j Let q represent the joint over-limit prevention optimization function corresponding to structure j. j q represents the joint angle of structure j. jmin Represents the minimum joint angle of structure j, q jmaxThis represents the maximum joint angle of structure j.

[0057] Optionally, in this embodiment of the invention, when optimizing the inverse kinematic path involves optimizing the joint angles of a multi-joint mechanism and using the gradient projection method to optimize the inverse kinematic path, the rule for determining the preset amplification factor corresponding to the final structure posture optimization includes: the amplification factor corresponding to the final structure posture optimization is a preset amplification factor. The preset amplification factor is a specific value that can be set according to specific circumstances.

[0058] Optionally, in this embodiment of the invention, the final structure attitude optimization function may be... The multi-joint mechanism has a total of n structures; the final structure is numbered n and its tilt angle is θ. n ; q1 is the joint angle of structure 1, q2 is the joint angle of structure 2, q3 is the joint angle of structure 3, and so on, q n Let n be the joint angle of structure n. The order of the structures can refer to the rules described in the above embodiments; m is a preset angle, which can be set according to specific circumstances. Preferably, it can be π / 2.

[0059] Optionally, in this embodiment of the invention, when optimizing the inverse motion path involves optimizing the joint angles of a multi-joint mechanism and using the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the inverse joint optimization includes the following formula:

[0060]

[0061]

[0062]

[0063] Δq i =q i -q it

[0064] Where, k 2,i This represents the amplification factor corresponding to the anti-joint optimization of structure i in a multi-joint mechanism. This represents the optimized vector of joint velocities in a multi-joint mechanism. Let δt represent the optimization amount of the joint velocity of structure i, δt represent the optimization time, I represent the identity matrix, and J represent the Jacobian matrix. + H represents the pseudo-inverse of the Jacobian matrix J. 2,i This represents the inverse joint optimization function corresponding to structure i. q represents the gradient of the inverse joint optimization function corresponding to structure i. i This represents the joint angle of structure i. The multi-joint mechanism has n structures in total, q itThis represents the target joint angle of structure i. i is the sequence number of structure i, and the sequence number can be set according to the method specified in the above embodiments. Furthermore, δt can be a preset value.

[0065] Optionally, in this embodiment of the invention, the anti-joint optimization function corresponding to structure i is calculated according to the following formula:

[0066]

[0067] q it =(k i x-β i )·π / 180

[0068] b = max(|q it -q imin |,|q it -q imax |)

[0069] Where, k i β represents the second preset value corresponding to structure i. i The third preset value corresponding to structure i is represented, x represents the x-axis component of the coordinate of the end of the multi-joint mechanism, and q is the third preset value corresponding to structure i. imin q represents the minimum joint angle of structure i. imax This represents the maximum joint angle of structure i. Optionally, in this embodiment of the invention, x can be calculated using a forward kinematics model. Optionally, in this embodiment of the invention, k can be preset. i and β i The correspondence between structure i and structure k is established based on the set correspondence. i and β i For example, k can be obtained through simulation. i and β i The correspondence between structure i and structure i.

[0070] The following is combined Figures 2 to 7 Taking a concrete pump truck as an example, this invention provides an exemplary description of a method for optimizing the inverse kinematic path of a multi-joint mechanism. In this embodiment, the multi-joint structure is a boom, and the structure is a segmented boom with a total of six segments. The ordering of the six segments can refer to the method described in the previous embodiment. In this embodiment, optimizing the inverse kinematic path involves optimizing the joint angles of the structure, and the gradient projection method is used to optimize the inverse kinematic path.

[0071] The technical solution in this embodiment of the invention relates to the inverse kinematics optimization of redundant degree-of-freedom mechanisms. Specifically, it relates to the inverse kinematics optimization of a concrete pump truck boom, wherein the redundant degree-of-freedom mechanism is a series mechanism comprising multiple degrees of freedom. Figure 2As shown, the concrete pump truck includes a body 1, a turntable 2, a first boom 3, a second boom 4, a third boom 5, a fourth boom 6, a fifth boom 7, and a sixth boom 8. The boom includes a first boom 3, a second boom 4, a third boom 5, a fourth boom 6, a fifth boom 7, and a sixth boom 8. The boom segments are connected by joints, with one degree of freedom corresponding to one joint. The boom is a 5-degree-of-freedom series mechanism, and the boom segments are the structure described in the embodiment of the present invention.

[0072] In this embodiment, optimizing the inverse kinematic path involves optimizing the joint angles of the arm. For example... Figure 4 As shown, the joint angle q1 is the angle between one arm 3 and the horizontal plane; q i (i>1) is the angle between the extensions of segment i and segment i-1. Clockwise rotation of segment i is negative, and counterclockwise rotation of segment i is positive. The direction of rotation is from the extension of the (i-1)th segment to the i-th segment. Figure 5 As shown, the tilt angle θ of the joint arm i The angle between segment arm i and the horizontal plane is defined as follows: clockwise rotation of segment arm i is negative, and counterclockwise rotation is positive. The direction of rotation is from the horizontal plane to the current position of segment arm i. It should be noted that in this embodiment, segment arm i refers to structure i as described in this embodiment, and segment arm j refers to structure j as described in this embodiment. i and j are serial numbers, and the format of these serial numbers can refer to the method described in the above embodiments. Figure 4 As shown, after the segments are sorted according to the method described in the above embodiments, segment 1 is a single arm 3, segment 2 is a double arm 4, segment 3 is a triple arm 5, segment 4 is a quadruple arm 6, segment 5 is a quintuple arm 7, and segment 6 is a hexaple arm 8; in addition, as Figure 4 As shown, the joint corresponding to arm 4 is a reverse joint.

[0073] like Figure 4 and Figure 5 As shown, the tilt angle and joint angle have the following mathematical relationship:

[0074]

[0075]

[0076] In this embodiment, at least two optimization objectives are included: end-structure posture optimization, anti-joint optimization, and joint over-limit prevention optimization. Unlike traditional multi-objective weighted optimization methods, the technical solution provided in this embodiment constructs an ordered serial optimization mechanism, which can be used to achieve hierarchical optimization of at least two optimization objectives, reducing the dependence of the amplification factor on prior testing and providing specific values ​​for the amplification factor.

[0077] Traditional multi-objective weighted optimization methods fix the Jacobian matrix and its pseudo-inverse matrix during a single optimization iteration, relying on a single linear weighting of the gradient projection function to achieve multi-objective optimization. The optimization objectives only affect the gradient function. The ordered serial optimization mechanism provided in this invention first determines the priority of each optimization objective. During a single optimization iteration, for adjacent priority objectives, the optimization result of the lower-priority objective forms the basis for the higher-priority objective. The null projection matrix of the higher-priority objective is generated from the optimization result of the lower-priority objective. This process is repeated until the optimization result of the highest-priority objective is obtained, at which point the single optimization iteration ends, and the single inverse kinematics solution is completed. In this invention, the optimization objectives affect not only the gradient function but also the null projection matrix. Furthermore, in this embodiment, the priority of the optimization objectives can be set according to specific circumstances. Specifically, in this embodiment, joint over-limit prevention optimization has the highest priority and urgency, followed by anti-joint optimization, and end-structure posture optimization has the lowest priority. It should be noted that the end-arm posture optimization described in this embodiment is the same as end-structure posture optimization.

[0078] Specifically, the optimization process or the specific solution process of inverse kinematics can be as follows: Figure 3 As shown. The process involves determining the joint angles q of the current boom and the end effector velocity v of the boom. This can be achieved by directly obtaining the joint angles of each boom segment; alternatively, by obtaining the tilt angle of each segment and combining it with the relationship between the joint angles and tilt angles. For example, if the boom attitude detection sensor is a tilt sensor, the tilt angle is detected using this sensor. The Jacobian matrix is ​​then calculated based on the determined joint angles. 1 J and its pseudo-inverse matrix 1 J + Calculate the gradient of the end-arm pose optimization function H1. in, Update the null space projection matrix, joint angular velocity, and joint angle. Specifically, based on I- 1 J 1 J + Update the null projection matrix, where I is the identity matrix, and... 1 J and 1 J + Let denot and represent the Jacobian matrix and pseudo-inverse matrix of the Jacobian matrix, respectively, calculated during the end-arm pose optimization; according to Update joint angular velocity, This represents the updated joint angular velocities of each joint during end-arm pose optimization; based on Update the joint angles, where Δt is the control period and q0 represents the joint angles of each joint before the end-arm pose optimization. 1q represents the updated joint angles of each joint during distal arm pose optimization. Inverse joint optimization is performed after distal arm pose optimization. The Jacobian matrix is ​​calculated based on the updated joint angles. 2 J and its pseudo-inverse matrix 2 J + ,in, 2 J and 2 J + Let represent the Jacobian matrix and the pseudo-inverse of the Jacobian matrix calculated during inverse joint optimization, respectively. Calculate the gradient of the inverse joint optimization function H2. in, Update the null space projection matrix, joint angular velocity, and joint angle. Specifically, based on I- 2 J 2 J + Update the null projection matrix; according to Update joint angular velocity, This represents the updated joint angular velocity of each joint during anti-joint optimization; based on Update the joint angle, where Δt is the control period. 2 q represents the updated joint angles of each joint during anti-joint optimization. After anti-joint optimization, joint over-limit optimization is performed. During joint over-limit optimization, the following steps are performed sequentially for each joint arm according to its segment number, from smallest to largest. The segment number can be set as described in the above embodiment. Specifically, for any segment j, the joint over-limit optimization operation includes the following: Determine if j is greater than 6. If yes, the optimization process ends, meaning the final inverse solution of the joint angles is obtained, which is the planning result. The planning result is then sent to the lower-level machine. If not, the Jacobian matrix is ​​calculated based on the updated joint angles obtained when performing joint over-limit optimization on segment j-1. 3,j J and its pseudo-inverse matrix 3,j J + ,in, 3,j J and 3,j J + Let represent the Jacobian matrix and the pseudo-inverse of the Jacobian matrix, respectively, calculated when performing joint over-limit optimization for arm j, and calculated when j is 1. 3,j J and 3,j J + The calculation is based on the updated joint angles after inverse joint optimization. The joint over-limit optimization function H for arm j is calculated. 3,j and its gradient in, Determine if |H is satisfied 3,j| Greater than a and j not equal to 4, where a is a constant that can be set according to actual conditions. If so, update the null space projection matrix, joint angular velocity, and joint angle. After updating, assign j = j + 1 and then perform the next joint over-limit optimization operation. Wherein, according to I- 3,j J 3,j J + Update the null projection matrix; according to Update joint angular velocity, This represents the updated joint angular velocities of each joint during the joint over-limit optimization of articulated arm j-1. This represents the updated joint angular velocity of each joint during joint over-limit optimization of arm j; based on Update the joint angle, where Δt is the control period. 3,j q represents the updated joint angles of each joint when performing joint over-limit optimization on articulated arm j. 3,j-1 q represents the updated joint angles of each joint when performing joint over-limit optimization on articulated arm j-1; when j is 1, for 3,j-1 q is 2 q. When |H is not satisfied 3,j When | is greater than a and j is not 4, 3,j q = 3,j-1 q, and assign j = j + 1 before performing the next operation on joint over-limit optimization.

[0079] The calculation methods for the optimization functions and amplification coefficients corresponding to each optimization objective are as follows.

[0080] The optimization objective of the last arm is to make its tilt angle approach -π / 2. The mathematical relationship of the last arm attitude optimization function is expressed as follows:

[0081] like Figure 4 As shown, the joint corresponding to arm 6 is a reverse joint, the index of arm 6 is 4, arm 6 is segment 4, and the reverse joint optimization function is: Where, q 4t = (k4x-β4)·π / 180, is an empirical formula; k4 is the second preset value corresponding to segment 4, β4 is the third preset value corresponding to segment 4, and both k4 and β4 are empirical values, closely related to the boom structure; b = max(|q 4t -q 4min |,|q 4t -q 4max |), x is the x-axis component of the terminal coordinate, q 4min and q 4maxThese are the minimum and maximum values ​​of the joint angles of the four arms (6).

[0082] The joint over-limit protection optimization function corresponding to arm j is: In the formula, β j =(q jmin +q jmax ) / 2, q j q represents the joint angle of arm j. jmin q represents the minimum joint angle of arm j. jmax This represents the maximum joint angle of arm j.

[0083] k1 can be a preset amplification factor, which is an empirical value related to the boom configuration and can be determined through simulation.

[0084] k2 can be calculated using the following methods. Δq4=q4-q 4t Where Δq4 represents the current joint angles q4 and q of the four arms 6. 4t The difference, q 4t δt represents the target joint angle of the four booms; I is the identity matrix; δt is the optimization time, the value of which is related to the hydraulic flow saturation coefficient of the boom system and the boom configuration, for example, δt = 0~100; The optimization amount for the joint velocity of the four arms 6, for The fourth component, Let k2 be a 6-dimensional vector. The size of k2 can be varied by adjusting the optimization time, and its value directly affects the dynamic optimization effect of the anti-joint.

[0085] k3 can be calculated using the following: the amplification factor corresponding to the joint over-limit optimization of arm j. Δ 3,j q j =|q j -q jlimit |,q jlimit =α·sgn(H 3,j )·((q jmid -q jmin )+q jmid ), q jmid =(q jmin +q jmax ) / 2. Where I is the identity matrix; Let J be the optimal value for the joint velocity of arm j. The j-th component; It is a 6-dimensional vector; δt is the optimization time, the value of which is related to the hydraulic flow saturation coefficient of the boom system and the boom configuration, for example, δt = 0~100; qj Indicates the joint angle of arm j; q jmin q jmax The minimum and maximum joint angles of articulated arm j are given respectively; sgn(a) returns the sign of parameter a; α is an empirical value.

[0086] Furthermore, it should be noted that in this embodiment, there is only one anti-joint, so anti-joint optimization is performed for this one anti-joint; however, when there are multiple anti-joints, anti-joint optimization needs to be performed for all anti-joints before proceeding to the next level of optimization.

[0087] For a tandem boom mechanism of a 6-section pump truck, within a range of 12m end height and 40m length, the inverse kinematics optimization effect is as follows: Figure 6 and 7 As shown, firstly, the operability of the task direction is significantly improved; secondly, at the same end-effector height, the working space during automatic boom operation is expanded. Specifically, Figure 7 Figure a) in the diagram represents the motion configuration before optimization. Figure 7 Figure b) in the diagram represents the optimized motion configuration.

[0088] The technical solutions in this invention embodiment have the following key technical points: 1) A serial optimization strategy of multi-objective optimization is adopted. For at least two optimization objectives, such as distal arm posture, anti-joint size, and joint over-limit prevention, serial optimization is performed within one cycle. This avoids the dependence of multiple objectives on weighting coefficients and eliminates the need to pre-test and set a range for an amplification coefficient. Instead, it provides a specific value for the amplification coefficient rather than a range. 2) Objective function forms for anti-joint size optimization, distal arm posture optimization, and joint over-limit prevention are proposed. Each optimization function form has a clear physical meaning and is concise. Among the three major optimization objectives mentioned in this paper, the first optimization function for distal arm angle is the difference between the sum of all joint angles and the target angle, and the optimization objective is to minimize this difference; the second optimization function is the difference between the anti-joint angle and the target angle, and the optimization objective is to minimize this difference; the third optimization function represents the joint angle being as close as possible to the median of the movable range. 3) A method for calculating the amplification coefficient corresponding to each optimization function is proposed. The calculation process also has a clear physical meaning, and its magnitude is determined, avoiding the phenomenon of selecting within a certain range. 4) The optimization order is optimal, but it can also be randomized. When determining the optimization order, the importance of each optimization objective should be ranked, and the most important one should be optimized last.

[0089] The technical solution provided in this invention is based on the gradient projection method and optimizes targets such as the end-arm posture, anti-joint size, and prevention of joint over-limit. It has the following advantages: 1) It adopts a serial optimization strategy of multi-objective optimization, which directly changes the initial posture and gradient projection matrix of the inverse kinematics of high-priority targets. In a way that does not rely on weighting coefficients, it highlights high-priority targets while not losing the initiative of low-priority targets, and the optimization effect is more obvious; 2) The optimization function form and the calculation form of the amplification coefficient of each optimization target have clear physical meaning and are concise; 3) The calculation of the amplification coefficient can give a definite value rather than a range, thus achieving the ultimate goal of optimization.

[0090] Accordingly, another aspect of the present invention provides an apparatus for optimizing the inverse kinematic path of a multi-joint mechanism. The apparatus includes an optimization module for optimizing the inverse kinematic path according to the priority of at least two preset mechanism posture optimization targets, wherein the optimization result of the inverse kinematic path corresponding to the higher priority preset mechanism posture optimization target is based on the optimization result of the inverse kinematic path corresponding to the lower priority preset mechanism posture optimization target.

[0091] Optionally, in this embodiment of the invention, optimizing the inverse motion path is to optimize the joint angles of the structure in the multi-joint mechanism. The device further includes: an amplification factor determination module, used to determine the amplification factor according to the preset amplification factor determination rule corresponding to the preset mechanism posture optimization target when the gradient projection method is used to optimize the inverse motion path for any preset mechanism posture optimization target.

[0092] Optionally, in this embodiment of the invention, the optimization module optimizes the inverse motion path based on at least two preset mechanism posture optimization objectives, including end structure posture optimization and joint over-limit optimization, wherein the joint over-limit optimization does not target structures where the corresponding joint is a reverse joint.

[0093] Optionally, in an embodiment of the present invention, when the multi-joint structure includes anti-joint optimization, the optimization module optimizes the inverse motion path based on at least two preset mechanism posture optimization objectives, which also includes anti-joint optimization.

[0094] Optionally, in this embodiment of the invention, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the joint over-limit optimization includes the following formula: Δ 3,j q j =|q j -q jlimit |,q jlimit =α·sgn(H 3,j )·((qjmid -q jmin )+q jmid ), q jmid =(q jmin +q jmax ) / 2, where k 3,j H represents the amplification factor corresponding to the joint over-limit optimization of structure j in a multi-joint mechanism. 3,j This represents the joint over-limit prevention optimization function corresponding to structure j. This represents the optimized vector of joint velocities in a multi-joint mechanism. Let δt represent the optimization amount of the joint velocity of structure j, δt represent the optimization time, and I represent the identity matrix. 3,j J represents the Jacobian matrix corresponding to the joint over-limit optimization of structure j. 3,j J + This represents the Jacobian matrix corresponding to the joint over-limit optimization of structure j. 3,j The pseudo-inverse matrix of J, q j This represents the joint angle of structure j. The multi-joint mechanism has n structures in total. α is a first preset value, and q... jmin q represents the minimum joint angle of structure j. jmax This represents the maximum joint angle of structure j.

[0095] Optionally, in this embodiment of the invention, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the joint anti-overlimit optimization function corresponding to structure j is calculated according to the following formula: β j =(q jmin +q jmax ) / 2.

[0096] Optionally, in this embodiment of the invention, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the final structure posture optimization includes: the amplification factor corresponding to the final structure posture optimization is a preset amplification factor.

[0097] Optionally, in this embodiment of the invention, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the anti-joint optimization includes the following formula: Δq i =q i -q it , where k 2,i This represents the amplification factor corresponding to the anti-joint optimization of structure i in a multi-joint mechanism. This represents the optimized vector of joint velocities in a multi-joint mechanism. Let δt represent the optimization amount of the joint velocity of structure i, δt represent the optimization time, I represent the identity matrix, and J represent the Jacobian matrix. + H represents the pseudo-inverse of the Jacobian matrix J. 2,i H represents the inverse joint optimization function corresponding to structure i. 2,i Let q represent the gradient of the inverse joint optimization function corresponding to structure i. i This represents the joint angle of structure i. The multi-joint mechanism has n structures in total, q it The target joint angle of structure i is represented.

[0098] Optionally, in this embodiment of the invention, when the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the anti-joint optimization function corresponding to structure i is calculated according to the following formula: q it =(kx-β)·π / 180, b=max(|q it -q imin |,|q it -q imax |), where k i β represents the second preset value corresponding to structure i. i The third preset value corresponding to structure i is represented, x represents the x-axis component of the coordinate of the end of the multi-joint mechanism, and q is the third preset value corresponding to structure i. imin q represents the minimum joint angle of structure i. imax This represents the maximum joint angle of structure i.

[0099] The specific working principle and benefits of the device for optimizing the inverse motion path of a multi-joint mechanism provided in this embodiment of the invention are similar to those of the method for optimizing the inverse motion path of a multi-joint mechanism provided in this embodiment of the invention, and will not be repeated here.

[0100] Furthermore, another aspect of the present invention provides an engineering machine that includes the device described in the above embodiments.

[0101] In addition, another aspect of the present invention provides a machine-readable storage medium storing instructions for causing a machine to perform the above-described method.

[0102] The preferred embodiments of the present invention have been described in detail above with reference to the accompanying drawings. However, the present invention is not limited to the specific details of the above embodiments. Within the scope of the technical concept of the present invention, various simple modifications can be made to the technical solution of the present invention, and these simple modifications all fall within the protection scope of the present invention.

[0103] It should also be noted that the various specific technical features described in the above embodiments can be combined in any suitable manner without contradiction. To avoid unnecessary repetition, the present invention will not describe the various possible combinations separately.

[0104] Furthermore, various different embodiments of the present invention can be combined in any way, as long as they do not violate the spirit of the present invention, they should also be regarded as the content disclosed by the present invention.

Claims

1. A method for optimizing the inverse kinematic path of a multi-joint mechanism, characterized in that, The method includes: The inverse kinematic path is optimized according to the priority of at least two preset mechanism attitude optimization objectives. The optimization result of the inverse kinematic path corresponding to the preset mechanism attitude optimization objective with higher priority is based on the optimization result of the inverse kinematic path corresponding to the preset mechanism attitude optimization objective with lower priority, and an ordered serial optimization mechanism is implemented. For a single optimization in the ordered serial optimization mechanism, the null projection matrix of the optimization objective with higher priority among adjacent priorities is generated by the optimization result of the optimization objective with lower priority.

2. The method according to claim 1, characterized in that, Optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism. The method further includes: For any of the preset mechanism posture optimization targets, when the gradient projection method is used to optimize the inverse motion path, the amplification factor is determined according to the preset amplification factor determination rule corresponding to the preset mechanism posture optimization target.

3. The method according to claim 2, characterized in that, The at least two preset mechanism posture optimization objectives include end-structure posture optimization and joint over-limit prevention optimization, wherein the joint over-limit prevention optimization does not target structures with corresponding anti-joints.

4. The method according to claim 3, characterized in that, In the case where the multi-joint mechanism includes anti-joint optimization, the at least two preset mechanism posture optimization objectives also include anti-joint optimization.

5. The method according to claim 3, characterized in that, When optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism and using the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the joint over-limit optimization includes the following formula: in, This represents the amplification factor corresponding to the joint over-limit optimization performed on structure j in the multi-joint mechanism. This represents the joint over-limit prevention optimization function corresponding to structure j. This represents the gradient of the joint over-limit prevention optimization function corresponding to structure j. This represents the optimized vector of joint velocities in the multi-joint mechanism. This represents the optimized amount of the joint velocity of structure j. Indicates optimization time. I Represents the identity matrix. This represents the Jacobian matrix corresponding to the joint over-limit optimization performed on the structure j. This represents the Jacobian matrix corresponding to the joint over-limit optimization performed on structure j. The pseudo-inverse matrix, q j This represents the joint angle of structure j, and the multi-joint mechanism has n such structures. The first preset value, This represents the minimum joint angle of structure j. This represents the maximum joint angle of structure j.

6. The method according to claim 5, characterized in that, The joint over-limit prevention optimization function corresponding to structure j is calculated according to the following formula: 。 7. The method according to claim 3, characterized in that, When optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism and using the gradient projection method to optimize the inverse motion path, the rules for determining the preset amplification coefficient corresponding to the final structure posture optimization include: The amplification factor corresponding to the final structure attitude optimization is a preset amplification factor.

8. The method according to claim 4, characterized in that, When optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism and using the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the anti-joint optimization includes the following formula: in, The structure in the multi-joint mechanism is indicated. The corresponding amplification factor is used for the anti-joint optimization. This represents the optimized vector of joint velocities in the multi-joint mechanism. Represents the structure The optimization amount of the joint speed. Indicates optimization time. I Represents the identity matrix. Represents the Jacobian matrix. Representing the Jacobian matrix The pseudo-inverse matrix, Represents the structure The corresponding inverse joint optimization function, Represents the structure The gradient of the corresponding inverse joint optimization function, Represents the structure The joint angles, and the multi-joint mechanism has a total of n structures. Represents the structure The target joint angle.

9. The method according to claim 8, characterized in that, The structure The corresponding anti-joint optimization function is calculated according to the following formula: in, Represents the structure The corresponding second preset value, Represents the structure The corresponding third preset value, This indicates the coordinates of the end of the multi-joint mechanism. Axial components, Represents the structure The minimum joint angle, Represents the structure The maximum joint angle.

10. A device for optimizing the inverse kinematic path of a multi-joint mechanism, characterized in that, The device includes: An optimization module is used to optimize the inverse kinematic path according to the priority of at least two preset mechanism attitude optimization targets. The optimization result of the inverse kinematic path corresponding to the preset mechanism attitude optimization target with higher priority is based on the optimization result of the inverse kinematic path corresponding to the preset mechanism attitude optimization target with lower priority, and an ordered serial optimization mechanism is implemented. In a single optimization, for optimization targets with adjacent priorities, the null projection matrix of the optimization target with higher priority is generated by the optimization result of the optimization target with lower priority.

11. The apparatus according to claim 10, characterized in that, Optimizing the inverse motion path involves optimizing the joint angles of the structure in the multi-joint mechanism. The device further includes: The amplification factor determination module is used to determine the amplification factor according to the preset amplification factor determination rule corresponding to any preset mechanism posture optimization target when the gradient projection method is used to optimize the inverse motion path.

12. The apparatus according to claim 11, characterized in that, The optimization module optimizes the inverse motion path based on at least two preset mechanism posture optimization objectives, including end structure posture optimization and joint over-limit optimization. The joint over-limit optimization does not optimize structures where the corresponding joints are anti-joints.

13. The apparatus according to claim 12, characterized in that, In the case where the multi-joint mechanism includes anti-joint optimization, the optimization module optimizes the inverse motion path based on the at least two preset mechanism posture optimization objectives, which also includes anti-joint optimization.

14. The apparatus according to claim 12, characterized in that, When the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification coefficient corresponding to the joint anti-overlimit optimization includes the following formula: in, This represents the amplification factor corresponding to the joint over-limit optimization performed on structure j in the multi-joint mechanism. This represents the joint over-limit prevention optimization function corresponding to structure j. This represents the gradient of the joint over-limit prevention optimization function corresponding to structure j. This represents the optimized vector of joint velocities in the multi-joint mechanism. This represents the optimized amount of the joint velocity of structure j. Indicates optimization time. I Represents the identity matrix. This represents the Jacobian matrix corresponding to the joint over-limit optimization performed on the structure j. This represents the Jacobian matrix corresponding to the joint over-limit optimization performed on structure j. The pseudo-inverse matrix, q j This represents the joint angle of structure j, and the multi-joint mechanism has n such structures. The first preset value, This represents the minimum joint angle of structure j. This represents the maximum joint angle of structure j.

15. The apparatus according to claim 14, characterized in that, When the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the joint anti-overlimit optimization function corresponding to structure j is calculated according to the following formula: 。 16. The apparatus according to claim 12, characterized in that, When the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rules for determining the preset amplification coefficient corresponding to the final structure posture optimization include: The amplification factor corresponding to the final structure attitude optimization is a preset amplification factor.

17. The apparatus according to claim 13, characterized in that, When the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the rule for determining the preset amplification factor corresponding to the anti-joint optimization includes the following formula: in, The structure in the multi-joint mechanism is indicated. The corresponding amplification factor is used for the anti-joint optimization. This represents the optimized vector of joint velocities in the multi-joint mechanism. Represents the structure The optimization amount of the joint speed. Indicates optimization time. I Represents the identity matrix. Represents the Jacobian matrix. Representing the Jacobian matrix The pseudo-inverse matrix, Represents the structure The corresponding inverse joint optimization function, Represents the structure The gradient of the corresponding inverse joint optimization function, Represents the structure The joint angles, and the multi-joint mechanism has a total of n structures. Represents the structure The target joint angle.

18. The apparatus according to claim 17, characterized in that, When the optimization module optimizes the inverse motion path to optimize the joint angles of the structure in the multi-joint mechanism and uses the gradient projection method to optimize the inverse motion path, the structure The corresponding anti-joint optimization function is calculated according to the following formula: in, Represents the structure The corresponding second preset value, Represents the structure The corresponding third preset value, This indicates the coordinates of the end of the multi-joint mechanism. Axial components, Represents the structure The minimum joint angle, Represents the structure The maximum joint angle.

19. An engineering machinery, characterized in that, The engineering machinery includes the device according to any one of claims 10-18.

20. A machine-readable storage medium, characterized in that, The machine-readable storage medium stores instructions for causing the machine to perform the method described in any one of claims 1-9.

Images