CN118254161A - Inverse kinematics solving method, electronic device and computer readable storage medium - Google Patents

Abstract

The application discloses an inverse kinematics solving method, electronic equipment and a computer readable storage medium, wherein the inverse kinematics solving method comprises the following steps: acquiring an initial joint state and an expected tail end pose of the mechanical arm; acquiring an initial terminal pose according to the initial joint state, and acquiring an initial pose error based on the initial terminal pose and an expected terminal pose; acquiring an initial iteration joint state according to the initial pose error and the initial joint state; judging whether an initial iteration pose error of the initial iteration end pose corresponding to the initial iteration joint state and the expected end pose is smaller than or equal to a preset threshold value; if not, iteratively solving the iterative joint state so that the iterative pose error of the iterative terminal pose corresponding to the iterative joint state and the expected terminal pose is smaller than or equal to a preset threshold value, or the iterative times reach the preset times, and obtaining the final joint state. According to the application, the final joint state is obtained through iterative solution or when the iteration times reach the preset times, and the success rate of the inverse solution can be improved.

Description

Inverse kinematics solving method, electronic device and computer readable storage medium

Technical Field

The present application relates to the field of motion control technology of mechanical arms, and in particular, to an inverse kinematics solving method, an electronic device, and a computer readable storage medium.

Background

The inverse kinematics problem of the articulated mechanical arm is that under the condition that the position and the gesture of the mechanical arm end effector coordinate system relative to the base coordinate system and the geometrical parameters of all mechanical arm joints are given, the angle values of all mechanical arm joints are obtained, and the inverse process of the forward kinematics is realized. The forward kinematics can obtain a homogeneous coordinate transformation matrix T between the front and rear adjacent joint coordinate systems according to the structural geometric parameters of the mechanical arm, namely a pose matrix between two joints; if the rotation angle theta of each joint is known, the pose matrix of the mechanical arm end effector coordinate system can be obtained by sequentially and continuously right multiplying the joint homogeneous transformation matrix T, and the obtained result is unique. While the solution of inverse kinematics is relatively complex and may have no solution or multiple solutions.

In particular, for a six-joint mechanical arm with a bias at the tail end, which cannot meet pieper criteria, a closed solution is no longer available for the mechanical arm, and the inverse kinematics solution cannot be directly performed through an analytical method or a geometric method. Meanwhile, the traditional KDL (Kinematics and Dynamics) iteration solving mode has the problems of low solving success rate and low efficiency, and is difficult to deal with occasions with large-scale inverse solutions.

Disclosure of Invention

The application provides at least an inverse kinematics solving method, electronic equipment and a computer readable storage medium, which are used for improving the efficiency and success rate of inverse kinematics solving.

The first aspect of the present application provides an inverse kinematics solving method, comprising:

obtaining an initial joint state of the mechanical arm by utilizing approximate closed inverse kinematics solution, and obtaining a desired end pose, wherein the initial joint state comprises initial joint states of all joints of the mechanical arm;

Acquiring an initial terminal pose according to the initial joint state, and acquiring an initial pose error based on the initial terminal pose and an expected terminal pose;

Acquiring an initial iteration joint state according to the initial pose error and the initial joint state;

judging whether an initial iteration pose error of the initial iteration end pose corresponding to the initial iteration joint state and the expected end pose is smaller than or equal to a preset threshold value;

if not, iteratively solving the iterative joint state so that the iterative pose error of the iterative terminal pose corresponding to the iterative joint state and the expected terminal pose is smaller than or equal to a preset threshold value, or the iterative times reach the preset times, obtaining a final joint state, and controlling the mechanical arm to operate according to the final joint state.

Optionally, acquiring the initial iterative joint state according to the initial pose error and the initial joint state includes:

Inputting the initial pose error and the initial joint state into a jacobian matrix iterative equation;

Calculating an initial iteration joint state by utilizing a jacobian matrix iteration equation;

Wherein the jacobian iterative equation comprises a jacobian.

Optionally, before calculating the initial iterative joint state by using the jacobian matrix iterative mode, the inverse kinematics solving method further includes:

an initial jacobian matrix is determined based on the initial joint state.

Optionally, the inverse kinematics solution method further comprises:

Acquiring a pose description of the tail end of the mechanical arm relative to the base;

Deducing a speed control model of the tail end based on the pose;

and discretizing the speed control model to obtain a jacobian matrix iteration equation.

Optionally, acquiring the initial iterative joint state according to the initial pose error and the initial joint state includes:

Constructing a nonlinear least square problem based on the initial pose error and the initial joint state;

And solving the nonlinear least square problem by using an optimization solving library to obtain an initial iteration joint state.

Optionally, before constructing the nonlinear least squares problem based on the initial pose error and the initial joint state, the inverse kinematics solution further includes:

Iterative joint states are solved iteratively by utilizing a Jacobian matrix iterative equation;

When the iteration times reach the preset times, judging whether the iteration pose error corresponding to the final joint state is smaller than or equal to a preset threshold value;

if not, confirming that the inverse solution of the Jacobian matrix iterative equation fails.

Optionally, iteratively solving the iterative joint state includes:

Iteratively solving the iterative joint state by utilizing a jacobian matrix iterative equation and iteratively solving the iterative joint state by utilizing an optimization solving library;

Stopping the process of optimizing and solving the inverse solution of the library when the inverse solution of the jacobian matrix iterative equation is successful, and obtaining the final joint state obtained by the inverse solution of the jacobian matrix iterative equation;

and when the jacobian matrix iterative equation inverse solution fails, continuing to optimize the solution library inverse solution process, and acquiring a final joint state obtained by optimizing the solution library inverse solution.

Optionally, obtaining the initial joint state of the mechanical arm using an approximate closed inverse kinematics solution includes:

Establishing an initial coordinate system of the mechanical arm, wherein the mechanical arm comprises a base, a first mechanical arm joint, a second mechanical arm joint, a third mechanical arm joint, a fourth mechanical arm joint and a fifth mechanical arm joint which are sequentially connected;

And performing approximate adjustment on each mechanical arm joint of the mechanical arm according to the necessary condition criterion so as to obtain an initial joint state of the mechanical arm after the approximate adjustment.

A second aspect of the present application provides an electronic device comprising a memory and a processor coupled to each other, the processor being configured to execute program instructions stored in the memory to implement an inverse kinematics solution as described above.

A third aspect of the application provides a computer readable storage medium storing a computer program which, when executed by a processor, is capable of implementing an inverse kinematics solution as described above.

The beneficial effects of the application are as follows: compared with the prior art, the method and the device have the advantages that the initial joint state and the expected tail end pose of the mechanical arm are obtained, and the iteration joint state corresponding to the iteration tail end pose and the expected tail end pose with the iteration pose error smaller than or equal to the preset threshold value can be obtained by carrying out iteration solution; and when the iteration times reach the preset times, the final joint state can be obtained, and the efficiency and the success rate of the inverse kinematics solution can be improved.

It is to be understood that both the foregoing general description and the following detailed description are exemplary and explanatory only and are not restrictive of the application as claimed.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings required for the description of the embodiments will be briefly described below, and it is apparent that the drawings in the following description are only some embodiments of the present application, and other drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.

FIG. 1 is a flow chart of an embodiment of an inverse kinematics solution according to the present application;

FIG. 2a is a schematic view of a first spatial coordinate system of a robotic arm of the present application;

FIG. 2b is a schematic view of a second spatial coordinate system of the robotic arm of the present application;

FIG. 3 is a schematic diagram showing a specific flow of step S11 in FIG. 1;

FIG. 4 is a schematic diagram showing a first specific flow of step S13 in FIG. 1;

FIG. 5 is a second specific flowchart of step S13 in FIG. 1;

FIG. 6 is a flow chart of another embodiment of the inverse kinematics solution of the present application;

FIG. 7 is a schematic flow chart diagram of a further embodiment of an inverse kinematics solution according to the present application;

FIG. 8 is a schematic diagram of a frame of an embodiment of an electronic device of the present application;

FIG. 9 is a schematic diagram of a frame of an embodiment of a computer readable storage medium of the present application.

Detailed Description

For better understanding of the technical solution of the present application by those skilled in the art, the following provides an inverse kinematics solving method, an electronic device and a computer readable storage medium with reference to the drawings and the detailed description. As described in further detail. It is to be understood that the depicted embodiments are only some, but not all, of the embodiments of the present application. All other embodiments, which can be made by those skilled in the art based on the embodiments of the application without making any inventive effort, are intended to be within the scope of the application.

The terms "first," "second," and the like in this disclosure are used for distinguishing between different objects and not for describing a particular sequential order. Furthermore, the terms "comprise" and "have," as well as any variations thereof, are intended to cover a non-exclusive inclusion. For example, a process, method, system, article, or apparatus that comprises a list of steps or elements is not limited to only those listed steps or elements but may include other steps or elements not listed or inherent to such process, method, article, or apparatus.

The application provides an inverse kinematics solving method, which is used for realizing inverse kinematics solving on a six-joint mechanical arm with a bias at the tail end and cannot meet pieper criteria, and can improve the efficiency and success rate of the inverse kinematics solving. Referring to fig. 1, fig. 1 is a flow chart illustrating an embodiment of an inverse kinematics solving method according to the present application.

The execution subject of the inverse kinematics solution of the present application may be an inverse kinematics solution, for example, the inverse kinematics solution may be executed by a terminal device or a server or other processing device, where the inverse kinematics solution may include a User Equipment (UE), a mobile device, a User terminal, a cellular phone, a wireless phone, a Personal digital processing (Personal DIGITAL ASSISTANT, PDA), a handheld device, a computing device, a vehicle-mounted device, a wearable device, and the like. In some possible implementations, the inverse kinematics solution may be implemented by a processor invoking computer readable instructions stored in a memory.

Specifically, the inverse kinematics solving method of the embodiment of the present disclosure may include the steps of:

step S11: and obtaining the initial joint state of the mechanical arm by utilizing approximate closed inverse kinematics solution, and obtaining the expected terminal pose.

When the inverse kinematics solution is performed, an initial joint state and an expected end pose of the mechanical arm need to be acquired, so that a new joint state is solved according to the given initial joint state and the expected end pose. The initial joint state comprises initial joint states of all joints of the mechanical arm, and specifically comprises joint angles of all joints relative to zero positions.

Optionally, please continue to refer to fig. 3, in which fig. 3 is a schematic flowchart of step S11 in fig. 1, specifically using the approximate closed inverse kinematics solution to obtain the initial joint state of the mechanical arm.

Specifically, the method comprises the following steps:

Step S111: an initial coordinate system of the mechanical arm is established.

The inverse kinematics solving method is applied to a six-joint mechanical arm with an offset at the tail end, wherein the six-joint mechanical arm cannot meet pieper criteria, and the mechanical arm comprises a base, a first mechanical arm joint, a second mechanical arm joint, a third mechanical arm joint, a fourth mechanical arm joint and a fifth mechanical arm joint which are sequentially connected.

With further reference to fig. 2a, fig. 2a is a schematic diagram of a first spatial coordinate system of the mechanical arm according to the present application, wherein fig. 2a is a schematic diagram of a spatial coordinate system established by DH method (Denavit-Hartenberg) based on a six-joint mechanical arm. As shown in fig. 2a, the base, the first arm joint, the second arm joint, the third arm joint, the fourth arm joint, and the fifth arm joint are respectively points O, a, B, C, D, and E, and the spatial coordinate systems thereof are O₀X₀Y₀Z₀、O₁X₁Y₁Z₁、O₂X₂Y₂Z₂、O₃X₃Y₃Z₃、O₄X₄Y₄Z₄、O₅X₅Y₅Z₅ and O ₆X₆Y₆Z₆, respectively. In fig. 2a, point O is the base of the mechanical arm, point E is the end of the mechanical arm, and each joint rotates around the Z-axis of the corresponding coordinate system.

As can be seen from fig. 2, the six-joint mechanical arm used in the present application cannot meet pieper criteria due to the end joint offset, so that there is no closed solution, i.e. an analytical solution, and therefore, it is necessary to perform approximate adjustment on each mechanical arm joint of the mechanical arm, i.e. execute step S112.

Step S112: and performing approximate adjustment on each mechanical arm joint of the mechanical arm according to the necessary condition criterion so as to obtain an initial joint state of the mechanical arm after the approximate adjustment.

The requirement criterion of the present embodiment is specifically pieper criterion, pieper criterion requires that three adjacent joint axes of the robot intersect at one point or three axes are parallel, and as can be seen from fig. 2a, the fourth joint axis Z ₄, the fifth joint axis Z ₅ and the sixth joint axis Z ₆ cannot intersect at one point, which does not satisfy pieper criterion, i.e. the end has a bias. Because the closed solution does not exist in the six-joint mechanical arm with the offset end, the application adjusts the offset configuration to enable the offset configuration to meet pieper criteria, and then obtains the approximate closed solution by using a geometric method, and takes the approximate closed solution as an initial state. In this embodiment, since the joints of each arm of the six-joint mechanical arm are adjusted, the analytical solution obtained by performing the inverse solution by the geometric method is inaccurate, so that the analytical solution obtained by the solution is defined as an approximate closed solution, and the inverse solution process is defined as an approximate closed inverse kinematics solution process.

Referring to fig. 2a in combination with fig. 2b, fig. 2b is a schematic diagram of a second spatial coordinate system of the mechanical arm according to the present application. Because the distance between the connecting rods CD is relatively small, the present application cancels the connecting rods CD, as shown in fig. 2b, where the fourth joint axis Z ₄, the fifth joint axis Z ₅ and the sixth joint axis Z ₆ intersect at a point, and meets the pieper criterion. The mechanical arm coordinate system after adjustment can be solved by using an analytic method or a geometric method to obtain a closed solution, and as only errors introduced by the connecting rod CD exist, the terminal pose corresponding to the closed solution is close to the expected terminal pose, and the terminal pose is used as an initial state, so that the solving success rate can be greatly improved.

Step S12: and acquiring an initial tail end pose from the initial joint state, and acquiring an initial pose error based on the initial tail end pose and the expected tail end pose.

In this embodiment, the initial end pose may be calculated according to the initial joint state acquired in step S11, and the initial pose error may be calculated based on the initial end pose and the desired end pose.

Specifically, the pose description of the end-of-arm tool relative to the base of the arm may be as shown in equation (1):

Wherein [ n _xn_yn_z]^T represents an X-axis attitude vector of the end tool, [ o _xo_yo_z]^T represents a Y-axis attitude vector of the end tool, [ a _xa_ya_z]^T represents a Z-axis attitude vector of the end tool, [ p _xp_yp_z]^T represents a position vector of the end tool; t _t is used for representing the coordinate pose of the end tool, specifically 4*4 matrix; ^i-1T_i The specific calculation formula for representing the pose relation of the joint i relative to the joint i-1 can be shown as formula (2):

Wherein, trans is used for representing coordinate system conversion, rot is used for representing rotation angle conversion. Specifically, transforming the coordinate system O _i-1X_i-1Y_i-1Z_i-1 to the coordinate system O _iX_iY_iZ_i requires two translational transforms and two rotational transforms, and the specific transformation steps are as follows:

(a) Translating a _i-1 along the X _i-1 axis to move the origin of coordinates O _i-1 to O' _i-1;

(b) Alpha _i-1 is rotated by taking the X _i-1 axis as a rotating shaft, so that the Z _i-1 axis and the Z _i axis are in the same direction;

(c) Translation d _i along the Z _i axis moves the origin of coordinates O' _i-1 to O _i;

(d) And rotating theta _i by taking the Z _i axis as a rotating shaft, so that the new X _i-1 axis and the X _i axis are in the same direction.

According to the initial joint state obtained in the step S11, the embodiment combines the formula (1) and the formula (2), and thus the initial end pose can be calculated. Further, the embodiment can calculate the initial pose error based on the initial end pose and the expected end pose, and the pose error calculation formula can be specifically shown as formula (3):

Wherein T ₀ is the initial end pose, θ ₀ is the initial joint state, and T is the desired terminal pose, andE ₀ is the initial pose error, and Namely, delta _x＝f_x·dθ,δ_y＝f_y·dθ,δ_z＝f_z. Dθ, and the rotation angles of f and dθ are delta R.

Step S13: and acquiring an initial iteration joint state according to the initial pose error and the initial joint state.

The embodiment may input a corresponding calculation formula according to the initial pose error e ₀ and the initial joint state θ ₀ to obtain a corresponding initial iterative joint state.

Optionally, in an embodiment, please continue to refer to fig. 4, in which fig. 4 is a first specific flowchart of step S13 in fig. 1, for a specific process of acquiring the initial iterative joint state of the mechanical arm. The present embodiment uses a jacobian iterative equation to solve for. Specifically, the method comprises the following steps:

step 131: and inputting the initial pose error and the initial joint state into a jacobian matrix iterative equation.

In this embodiment, a jacobian iterative equation is pre-established, and the initial pose error e ₀ and the initial joint state θ ₀ are input into the jacobian iterative equation to calculate a corresponding joint state iterative result.

The process of specifically obtaining the jacobian iterative equation can be shown in steps S1311-S1313. Specifically, the method comprises the following steps:

Step 1311: and acquiring a pose description of the tail end of the mechanical arm relative to the base.

The description of the pose of the end of the mechanical arm relative to the base may be specifically shown in the above formula (1), and will not be described herein.

Step 1312: a velocity control model of the tip is derived based on the pose.

The velocity mode of the tail end of the mechanical arm can be obtained by deriving according to the formula (1), and the specific calculation formula of the velocity mode can be shown as the formula (4):

wherein J (theta) is a Jacobian matrix for representing the pose transformation of the tip by the minor changes of the joints of the mechanical arm.

And (3) carrying out formula deduction according to a formula (4) to obtain a control model of the tail end of the mechanical arm, wherein a specific calculation formula of the control model can be shown as a formula (5):

wherein J ^T(JJ^T)^-1 is the pseudo-inverse of the Jacobian matrix J (θ).

Step 1313: and discretizing the speed control model to obtain a jacobian matrix iteration equation.

In this embodiment, the speed control model obtained in step S1312 is further discretized, that is, discretizing the formula (5) to obtain a jacobian matrix iterative equation, where a specific calculation formula may be shown in the formula (6):

Wherein θ _k+1 represents the k+1th iterative joint state, e _k represents the pose error obtained by the kth calculation, and θ _k represents the kth iterative joint state.

Step 132: and calculating the initial iteration joint state by using a jacobian matrix iteration equation.

In this embodiment, the initial joint state obtained in step S11 is defined as θ ₀, and the initial pose error obtained in step S12 is defined as e ₀, and the initial iterative joint state θ ₁ can be calculated by inputting the initial pose error into the formula (6).

Wherein, when calculating the initial iteration joint state using the jacobian matrix iteration equation, the data of J ₀ ^T(J₀J₀ ^T)^-1 needs to be known, i.e., the initial jacobian matrix needs to be determined. Alternatively, the specific step of determining the initial jacobian matrix can be as shown in step S133.

Step 133: an initial jacobian matrix is determined based on the initial joint state.

The initial jacobian matrix in this embodiment is specifically obtained by deriving and calculating in equation (1) and equation (4), and will not be described herein.

Optionally, in another embodiment, please continue to refer to fig. 5, in which fig. 5 is a second specific flowchart of step S13 in fig. 1, for specifically acquiring the initial iterative joint state of the mechanical arm. The embodiment uses Ceres to optimize a solution library for solving. Specifically, the method comprises the following steps:

step S231: and constructing a nonlinear least square problem based on the initial pose error and the initial joint state.

The Ceres optimization solving library is used for solving the nonlinear least square problem, and the Ceres optimization solving library can be used for solving the nonlinear least square problem by constructing the nonlinear least square problem based on the initial pose error e ₀ and the initial joint state theta ₀.

Specifically, in this embodiment, an optimization variable and a constraint condition need to be preset, where the optimization variable is a joint state of six joints of the mechanical arm, and the constraint condition may be specifically shown in formula (7):

Where e represents the remaining amount, and the present embodiment is specifically set as the end error; θ _min and θ _max are used to represent joint limitation of the mechanical arm, specifically, θ _min represents a minimum rotation angle of the joint, and θ _max represents a maximum rotation angle of the joint.

Step S232: and solving the nonlinear least square problem by using an optimization solving library to obtain an initial iteration joint state.

In this embodiment, the initial joint state θ ₀ obtained in step S11 is taken as an initial input quantity, the initial pose error e ₀ obtained in step S12 is taken as a residual quantity, and the nonlinear least square problem is solved by using an optimization solving library, so as to obtain an initial iterative joint state θ ₁.

Step S14: and judging whether an initial iteration pose error of the initial iteration end pose corresponding to the initial iteration joint state and the expected end pose is smaller than or equal to a preset threshold value.

The initial iteration joint state θ ₁ is obtained by using a jacobian matrix iterative equation based on step S132, or the initial iteration joint state θ ₁ is obtained by using an optimization solving library to solve a nonlinear least square problem based on step S232, and it is required to determine whether an initial iteration pose error e ₁ between an initial iteration end pose T ₁ corresponding to the initial iteration joint state θ ₁ and a desired end pose T is less than or equal to a preset threshold.

Optionally, when it is determined that the initial iteration pose error e ₁ between the initial iteration end pose T ₁ corresponding to the initial iteration joint state θ ₁ and the desired end pose T is less than or equal to a preset threshold, outputting an initial iteration joint state θ ₁, and controlling the mechanical arm to operate according to the initial iteration joint state θ ₁.

When it is determined that the initial iteration end pose T ₁ corresponding to the initial iteration joint state θ ₁ and the initial iteration pose error e ₁ of the desired end pose T are greater than the preset threshold, step S15 is further performed.

Step S15: if not, iteratively solving the iterative joint state so that the iterative pose error of the iterative terminal pose corresponding to the iterative joint state and the expected terminal pose is smaller than or equal to a preset threshold value, or the iterative times reach the preset times, obtaining a final joint state, and controlling the mechanical arm to operate according to the final joint state.

When it is determined that an initial iteration pose error e ₁ between an initial iteration end pose T ₁ corresponding to the initial iteration joint state θ ₁ and an expected end pose T is greater than a preset threshold, an iteration solution is needed.

Specifically, when iterative computation is performed by using a jacobian matrix iterative equation, and when an iterative pose error between an iterative end pose corresponding to the iterative joint state and an expected end pose T obtained by computation is less than or equal to a preset threshold, stopping the iterative computation, and outputting the iterative joint state. Or when the number of iterative calculations reaches a preset number, proving that the inverse solution fails. Alternatively, the preset number of times may be specifically 30 times.

Or when the optimal solution library is used for solving, the optimal solution library outputs a final joint state, and when the pose error of the tail end pose corresponding to the joint state and the expected tail end pose T is smaller than a preset threshold, the inverse solution is successful, otherwise, the inverse solution is failed.

Alternatively, the preset thresholds of the different calculation methods may be the same or different.

According to the method, two calculation modes are used, including iterative calculation by utilizing a jacobian matrix iterative equation or solving of a nonlinear least square problem by utilizing an optimization solving library, so that the final joint state of the mechanical arm based on the expected terminal pose T is calculated, and the success rate of inverse solution can be improved.

Optionally, in an embodiment, the jacobian iterative equation and the optimization solution library may be run in a serial implementation manner to solve the final joint state of the mechanical arm, and with reference to fig. 6, fig. 6 is a schematic flow chart of another embodiment of the inverse kinematics solution method of the present application.

Specifically, the method comprises the following steps:

step S31: and iteratively solving the iterative joint state by using the jacobian matrix iterative equation.

In this embodiment, the iterative joint state is first solved iteratively by using the jacobian iterative equation, and the specific iterative calculation process is described in the above embodiment and is not described herein again.

Step S32: and when the iteration times reach the preset times, judging whether the iteration pose error corresponding to the final joint state is smaller than or equal to a preset threshold value.

When the iteration times reach the preset times, namely the iteration times reach 30 times, the calculated joint state is the final joint state, and whether the iteration pose error corresponding to the final joint state is smaller than or equal to a preset threshold value is further judged.

Step S33: if not, confirming that the inverse solution of the Jacobian matrix iterative equation fails.

And when the iteration pose error corresponding to the final joint state is judged to be larger than a preset threshold value, confirming that the inverse solution of the Jacobian matrix iteration equation fails. Meanwhile, when judging that the inverse solution of the jacobian matrix iterative equation fails, the step S231-step S232 can be adopted for carrying out iterative solution, and the final joint state is obtained through calculation of an optimization solution library, so that the success rate of the inverse solution can be improved.

Because the efficiency of the jacobian matrix iteration is high, and the optimization success rate of the Ceres optimization solution library is high, the embodiment firstly uses the jacobian matrix iteration mode to quickly solve most of inverse solution problems; if the inverse solution fails, the Ceres optimization solution library is further used for optimization, so that the success rate of the inverse solution can be greatly improved.

Alternatively, in another embodiment, the jacobian iterative equation and the optimization solution library may be run in parallel to solve the final joint state of the mechanical arm, and the specific process please continue to refer to fig. 7, where fig. 7 is a schematic flow chart of another embodiment of the inverse kinematics solution method of the present application. Specifically, the method comprises the following steps:

Step S41: and iteratively solving the iterative joint state by utilizing the jacobian matrix iterative equation and iteratively solving the iterative joint state by utilizing an optimization solving library.

The embodiment simultaneously utilizes the jacobian matrix iterative equation and the optimization solving library to iteratively solve the iterative joint state, namely, double-threaded operation is realized.

Step S42: and stopping the process of optimizing and solving the inverse solution of the library when the inverse solution of the jacobian matrix iterative equation is successful, and obtaining the final joint state obtained by the inverse solution of the jacobian matrix iterative equation.

And when the inverse solution of the jacobian matrix iteration equation is judged to be successful, stopping the process of optimizing the inverse solution of the jacobian matrix iteration equation, and obtaining a final joint state obtained by the inverse solution of the jacobian matrix iteration equation, wherein the final joint state is used as the final joint state of the mechanical arm for motion control.

Step S43: and when the jacobian matrix iterative equation inverse solution fails, continuing to optimize the solution library inverse solution process, and acquiring a final joint state obtained by optimizing the solution library inverse solution.

And when judging that the inverse solution of the Jacobian matrix iterative equation fails, continuing to optimize the inverse solution process of the solution library, and acquiring a final joint state obtained by optimizing the inverse solution of the solution library, wherein the final joint state is used as the final joint state of the mechanical arm for motion control.

According to the embodiment, the efficiency and the speed of the inverse solution can be improved through the iterative solution of the double-thread operation jacobian matrix iterative equation and the optimization solution library.

According to the method, the initial joint state T ₀ and the expected end pose T of the mechanical arm are obtained, and iterative solution is carried out, so that the iterative joint state corresponding to the iterative end pose and the expected end pose T with the iterative pose error smaller than or equal to a preset threshold value can be obtained; and when the iteration times reach the preset times, the final joint state can be obtained, and the efficiency and the success rate of the inverse kinematics solution can be improved. The method comprises the steps of realizing kinematic inverse solution calculation by combining a jacobian matrix iterative equation and an optimization solution library, carrying out iterative calculation by particularly preferentially utilizing the jacobian matrix iterative equation with high iteration efficiency, and further adopting the optimization solution library to optimize and solve when the inverse solution fails, wherein the iterative calculation is carried out through serial; or by bilinear parallel iterative solution, the efficiency and success rate of the inverse solution can be effectively improved.

Meanwhile, the application adjusts all the mechanical arm joints of the mechanical arm, so that the adjusted mechanical arm meets pieper criteria, an approximate closed solution can be obtained, and the end pose corresponding to the closed solution is close to the expected end pose T and is used as the initial state of the mechanical arm joints, so that the solving success rate can be greatly improved.

The application also provides an electronic device, please refer to fig. 8, fig. 8 is a schematic diagram of a frame of an embodiment of the electronic device of the application. The electronic device 80 comprises a memory 81 and a processor 82 coupled to each other, the processor 82 being adapted to execute program instructions stored in the memory 81 for implementing the steps of any of the inverse kinematics solution embodiments described above. In one particular implementation scenario, electronic device 80 may include, but is not limited to: the microcomputer and the server, and the electronic device 80 may also include a mobile device such as a notebook computer and a tablet computer, which is not limited herein.

In particular, the processor 82 is configured to control itself and the memory 81 to implement the steps of any of the inverse kinematics solution embodiments described above. The processor 82 may also be referred to as a CPU (Central Processing Unit ). The processor 82 may be an integrated circuit chip having signal processing capabilities. The Processor 82 may also be a general purpose Processor, a digital signal Processor (DIGITAL SIGNAL Processor, DSP), an Application SPECIFIC INTEGRATED Circuit (ASIC), a Field-Programmable gate array (Field-Programmable GATE ARRAY, FPGA) or other Programmable logic device, a discrete gate or transistor logic device, a discrete hardware component. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like. In addition, the processor 82 may be commonly implemented by an integrated circuit chip.

The present application also provides a computer readable storage medium, please refer to fig. 9, fig. 9 is a schematic diagram of a frame of an embodiment of the computer readable storage medium of the present application. The computer readable storage medium 90 stores a computer program 91 that can be run by a processor, the computer program 91 being used to implement the steps in any of the inverse kinematics solution embodiments described above.

In some embodiments, functions or modules included in an apparatus provided by the embodiments of the present disclosure may be used to perform a method described in the foregoing method embodiments, and specific implementations thereof may refer to descriptions of the foregoing method embodiments, which are not repeated herein for brevity.

The foregoing description of various embodiments is intended to highlight differences between the various embodiments, which may be the same or similar to each other by reference, and is not repeated herein for the sake of brevity.

In the several embodiments provided in the present application, it should be understood that the disclosed method and apparatus may be implemented in other manners. For example, the apparatus embodiments described above are merely illustrative, e.g., the division of modules or units is merely a logical functional division, and there may be additional divisions of actual implementation, e.g., units or components may be combined or integrated into another system, or some features may be omitted, or not performed. Alternatively, the coupling or direct coupling or communication connection shown or discussed with each other may be an indirect coupling or communication connection via some interfaces, devices or units, which may be in electrical, mechanical, or other forms.

In addition, each functional unit in the embodiments of the present application may be integrated in one processing unit, or each unit may exist alone physically, or two or more units may be integrated in one unit. The integrated units may be implemented in hardware or in software functional units.

The integrated units, if implemented in the form of software functional units and sold or used as stand-alone products, may be stored in a computer readable storage medium. Based on such understanding, the technical solution of the present application may be embodied in essence or a part contributing to the prior art or all or part of the technical solution in the form of a software product stored in a storage medium, including several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) or a processor (processor) to execute all or part of the steps of the methods of the embodiments of the present application. And the aforementioned storage medium includes: a usb disk, a removable hard disk, a Read-Only Memory (ROM), a random access Memory (RAM, random Access Memory), a magnetic disk, or an optical disk, or other various media capable of storing program codes.

The foregoing is only illustrative of the present application and is not to be construed as limiting the scope of the application, and all equivalent structures or equivalent flow modifications which may be made by the teachings of the present application and the accompanying drawings or which may be directly or indirectly employed in other related art are within the scope of the application.

Claims (10)

1. The inverse kinematics solving method for the offset configuration mechanical arm is characterized by comprising the following steps of:

Obtaining an initial joint state of the mechanical arm by utilizing approximate closed inverse kinematics solution, and obtaining a desired end pose, wherein the initial joint state comprises initial joint states of all joints of the mechanical arm;

acquiring an initial end pose from the initial joint state, and acquiring an initial pose error based on the initial end pose and the expected end pose;

acquiring an initial iteration joint state according to the initial pose error and the initial joint state;

judging whether an initial iteration pose error of the initial iteration end pose corresponding to the initial iteration joint state and the expected end pose is smaller than or equal to a preset threshold value;

If not, iteratively solving the iterative joint state so that the iterative pose error of the iterative terminal pose corresponding to the iterative joint state and the expected terminal pose is smaller than or equal to the preset threshold value, or the iterative times reach the preset times, obtaining a final joint state, and controlling the mechanical arm to operate according to the final joint state.

2. The method of claim 1, wherein the inverse kinematics solution is,

The obtaining the initial iterative joint state according to the initial pose error and the initial joint state comprises the following steps:

inputting the initial pose error and the initial joint state into a jacobian matrix iterative equation;

Calculating the initial iterative joint state by using the jacobian matrix iterative equation;

Wherein the jacobian iterative equation comprises a jacobian.

3. The inverse kinematics solution according to claim 2, wherein,

Before the initial iterative joint state is calculated by using the jacobian matrix iterative mode, the inverse kinematics solving method further includes:

And determining an initial jacobian matrix according to the initial joint state.

4. The inverse kinematics solution according to claim 2, wherein,

The inverse kinematics solving method further comprises:

acquiring a pose description of the tail end of the mechanical arm relative to the base;

deriving a velocity control model of the tip based on the pose;

And discretizing the speed control model to obtain the jacobian matrix iteration equation.

5. The method of claim 1, wherein the inverse kinematics solution is,

The obtaining the initial iterative joint state according to the initial pose error and the initial joint state comprises the following steps:

Constructing a nonlinear least squares problem based on the initial pose error and the initial joint state;

And solving the nonlinear least square problem by using an optimization solving library to obtain the initial iteration joint state.

6. The method of claim 5, wherein the inverse kinematics solution is,

Before the nonlinear least squares problem is constructed based on the initial pose error and the initial joint state, the inverse kinematics solution method further includes:

Iteratively solving the iterative joint state by utilizing a jacobian matrix iterative equation;

When the iteration times reach the preset times, judging whether the iteration pose error corresponding to the final joint state is smaller than or equal to the preset threshold value;

And if not, confirming that the inverse solution of the Jacobian matrix iterative equation fails.

7. The method of claim 1, wherein the inverse kinematics solution is,

The iterative solution of the iterative joint state comprises:

iteratively solving the iterative joint state by utilizing a jacobian matrix iterative equation, and iteratively solving the iterative joint state by utilizing an optimization solving library;

Stopping the optimization solving library inverse solution process when the jacobian matrix iterative equation inverse solution is successful, and obtaining a final joint state obtained by the jacobian matrix iterative equation inverse solution;

And when the jacobian matrix iterative equation inverse solution fails, continuing the optimization solution library inverse solution process, and obtaining a final joint state obtained by the optimization solution library inverse solution.

8. The method of claim 1, wherein the inverse kinematics solution is,

The obtaining the initial joint state of the mechanical arm by using approximate closed inverse kinematics solution comprises:

Establishing an initial coordinate system of the mechanical arm, wherein the mechanical arm comprises a base, a first mechanical arm joint, a second mechanical arm joint, a third mechanical arm joint, a fourth mechanical arm joint and a fifth mechanical arm joint which are connected in sequence;

and performing approximate adjustment on each mechanical arm joint of the mechanical arm according to a necessary condition criterion so as to obtain an initial joint state of the mechanical arm after the approximate adjustment.

9. An electronic device comprising a memory and a processor coupled to each other, the processor configured to execute program instructions stored in the memory to implement the inverse kinematics solution as claimed in any of claims 1-8.

10. A computer readable storage medium, characterized in that the computer readable storage medium stores a computer program which, when executed by a processor, implements the inverse kinematics solution according to any of claims 1-8.