CN116277009A

CN116277009A - Three-joint mechanical arm motion control method and device, electronic equipment and storage medium

Info

Publication number: CN116277009A
Application number: CN202310329134.7A
Authority: CN
Inventors: 请求不公布姓名
Original assignee: Hangzhou Weijing Medical Robot Co ltd
Current assignee: Hangzhou Weijing Medical Robot Co ltd
Priority date: 2023-03-28
Filing date: 2023-03-28
Publication date: 2023-06-23

Abstract

The application provides a three-joint mechanical arm motion control method, a three-joint mechanical arm motion control device, electronic equipment and a storage medium, and relates to the technical field of mechanical control. According to the method, mathematical modeling is conducted through a current starting point of the mechanical arm, a command point to be operated, a middle point and motion parameters of all joints, a mathematical model of the three-joint mechanical arm in an RTT configuration is constructed, a reference function relation corresponding to the three-joint mechanical arm in the RTT configuration is constructed, an inverse solution function relation of all joint axes of the mechanical arm under different position relations is obtained based on the reference function relation, and therefore the motion parameters of all joints are obtained according to the inverse solution function relation in an analysis mode. The reference function relationship is uniquely determined according to the joint association parameters under the RTT configuration, so that the inverse solution function relationship of each joint axis under different position relationships is also uniquely determined, and the inverse solution analysis is performed according to the inverse solution function relationship, so that the comprehensive and integral inverse solution of the three-joint mechanical arm of the RTT configuration under various conditions can be realized.

Description

Three-joint mechanical arm motion control method and device, electronic equipment and storage medium

Technical Field

The application relates to the technical field of mechanical control, in particular to a method and a device for controlling movement of a three-joint mechanical arm, electronic equipment and a storage medium.

Background

The RTT three-joint mechanical arm refers to a mechanical arm formed by sequentially mutually linking a rotary joint, a movable joint and a movable joint, and for the motion control of the RTT three-joint mechanical arm, the motion control can be generally realized through the inverse kinematics solution of the mechanical arm, namely the known terminal pose of the mechanical arm, and the motion vectors of all the movable joints in the mechanical arm are solved.

At present, how to realize the inverse solution analysis of the RTT three-joint mechanical arm becomes a technical problem to be solved urgently.

Disclosure of Invention

The invention aims to provide a three-joint mechanical arm motion control method, a three-joint mechanical arm motion control device, electronic equipment and a storage medium for overcoming the defects in the prior art, so that the inverse kinematics analysis of the RTT-configured three-joint mechanical arm can be realized.

In order to achieve the above purpose, the technical solution adopted in the embodiment of the present application is as follows:

in a first aspect, an embodiment of the present application provides a method for controlling motion of a three-joint mechanical arm, which is applied to a processing device in a mechanical arm control system, where the system includes the processing device and the three-joint mechanical arm with a target structure, and the three-joint mechanical arm includes a rotary joint, a first mobile joint and a second mobile joint that are sequentially linked with each other; the method comprises the following steps:

Acquiring an instruction point to which the tail end of the mechanical arm is to be operated;

determining an inverse solution function relation corresponding to the position relation according to the position relation among joint axes of all joints in the mechanical arm;

according to the inverse solution function relation, respectively determining a joint rotation angle of the rotary joint, a first moving distance of the first moving joint and a second moving distance of the second moving joint; the inverse solution function relation is obtained by deformation according to a reference function relation, and the reference function relation is obtained by construction according to algebraic equation relation of the intermediate point and constraint function relation between the intermediate point and the instruction point; the algebraic equation relation of the middle point is constructed according to the starting point where the tail end of the mechanical arm is currently positioned and joint association parameters, wherein the joint association parameters comprise: a first travel distance of the first mobile joint, a first direction vector of a first joint axis of the first mobile joint, a second travel distance of the second mobile joint, and a second direction vector of a second joint axis of the second mobile joint; the middle point comprises a designated point which is passed by the tail end of the mechanical arm in the process of running from the starting point to the instruction point;

The rotating joint of the mechanical arm is controlled to rotate the joint rotation angle, the first moving joint is controlled to move the first moving distance, and the second moving joint is controlled to move the second moving distance, so that the tail end of the mechanical arm moves from the starting point to the instruction point.

Optionally, the determining, according to a positional relationship between joint axes of joints in the mechanical arm, an inverse solution function relationship corresponding to the positional relationship includes:

according to the position relation among the joint axes of all the joints, determining the vector equation relation satisfied among the direction vectors of the joint axes under the position relation;

and determining an inverse solution function relation corresponding to the position relation according to the vector equation relation and the reference function relation.

Optionally, the algebraic equality relation of the intermediate points comprises: a first algebraic equality relation corresponding to the first intermediate point and a second algebraic equality relation corresponding to the second intermediate point;

the first algebraic equation relationship is used to characterize the relationship between the first intermediate point and the starting point, the second distance of movement, and the second direction vector;

The second algebraic equation relationship is used to characterize the relationship between the second intermediate point and the first intermediate point, the first distance of movement, and the first direction vector.

Optionally, the constraint function relationship between the intermediate point and the instruction point includes: a geometric constraint function and an algebraic constraint function;

the geometric constraint function is used for representing the geometric relationship between the second intermediate point and the instruction point and the direction vector of the rotating joint axis of the rotating joint;

the algebraic constraint function is used to characterize algebraic relations between the second intermediate point and the instruction point and a reference point on the rotational joint axis.

Optionally, if the rotational joint axis of the rotational joint is not perpendicular to the second joint axis of the second mobile joint, and the rotational joint axis is perpendicular to the first joint axis of the first mobile joint; the inverse solution function relationship is a function related to the second movement distance in the reference function relationship;

the determining, according to the inverse solution functional relationship, a joint rotation angle of the rotary joint, a first movement distance of the first movement joint, and a second movement distance of the second movement joint, respectively, includes:

Determining the second movement distance according to the inverse solution function relation;

determining the first moving distance according to the second moving distance and a functional relation between the first moving distance and the second moving distance in the reference functional relation;

determining a value of the first intermediate point according to the second travel distance and the first algebraic equation relationship;

determining a value of the second intermediate point according to the value of the first intermediate point, the first travel distance, and the second algebraic equation relationship;

and determining the value of the joint rotation angle according to the value of the second intermediate point, the instruction point and a preset motion function relation between the space point and the joint axis.

Optionally, if the rotational joint axis of the rotational joint is not perpendicular to the first joint axis of the first mobile joint and the rotational joint axis is perpendicular to the second joint axis of the second mobile joint; the inverse solution function relationship is a function related to the first movement distance in the reference function relationship;

Determining the first movement distance according to the inverse solution function relation;

determining the second moving distance according to the first moving distance and a functional relation between the first moving distance and the second moving distance in the reference functional relation;

Optionally, if the rotational joint axis of the rotational joint is not perpendicular to the first joint axis of the first mobile joint and the rotational joint axis is not perpendicular to the second joint axis of the second mobile joint; the inverse solution function relationship is a function related to the second movement distance in the reference function relationship;

In a second aspect, an embodiment of the present application further provides a motion control device for a three-joint mechanical arm, where the motion control device is applied to a processing device in a mechanical arm control system, where the system includes a processing device and a three-joint mechanical arm with a target structure, and the three-joint mechanical arm includes a rotary joint, a first moving joint and a second moving joint that are sequentially linked with each other; the device comprises: the device comprises an acquisition module, a determination module and a control module;

The acquisition module is used for acquiring an instruction point to which the tail end of the mechanical arm is to be operated;

the determining module is used for determining an inverse solution function relation corresponding to the position relation according to the position relation among the joint axes of all joints in the mechanical arm;

the determining module is used for respectively determining the joint rotation angle of the rotary joint, the first moving distance of the first moving joint and the second moving distance of the second moving joint according to the inverse solution function relation; the inverse solution function relation is obtained by deformation according to a reference function relation, and the reference function relation is obtained by construction according to algebraic equation relation of the intermediate point and constraint function relation between the intermediate point and the instruction point; the algebraic equation relation of the middle point is constructed according to the starting point where the tail end of the mechanical arm is currently positioned and joint association parameters, wherein the joint association parameters comprise: a first travel distance of the first mobile joint, a first direction vector of a first joint axis of the first mobile joint, a second travel distance of the second mobile joint, and a second direction vector of a second joint axis of the second mobile joint; the middle point comprises a designated point which is passed by the tail end of the mechanical arm in the process of running from the starting point to the instruction point;

The control module is used for respectively controlling the rotating joint of the mechanical arm to rotate the joint rotation angle, controlling the first moving joint to move the first moving distance and controlling the second moving joint to move the second moving distance, so that the tail end of the mechanical arm moves from the starting point to the instruction point.

Optionally, the determining module is specifically configured to determine, according to a positional relationship between joint axes of the joints, a vector equation relationship satisfied between direction vectors of the joint axes under the positional relationship;

the determining module is specifically configured to determine the second movement distance according to the inverse solution function relationship;

the determining module is specifically configured to determine the first movement distance according to the inverse solution function relationship;

In a third aspect, an embodiment of the present application provides an electronic device, including: the system comprises a processor, a storage medium and a bus, wherein the storage medium stores machine-readable instructions executable by the processor, when the electronic device is running, the processor communicates with the storage medium through the bus, and the processor executes the machine-readable instructions to execute the steps of the three-joint mechanical arm motion control method as provided in the first aspect.

In a fourth aspect, embodiments of the present application provide a computer readable storage medium having stored thereon a computer program which, when executed by a processor, performs the steps of the method for controlling movement of a three-joint mechanical arm as provided in the first aspect.

The beneficial effects of this application are:

according to the method, mathematical modeling is conducted through a starting point where the mechanical arm is currently located, an instruction point to be operated, a middle point and motion parameters of all joints, a mathematical model of the three-joint mechanical arm in an RTT configuration is constructed, a reference function relation corresponding to the three-joint mechanical arm in the RTT configuration is constructed, an inverse solution function relation of all joint axes of the mechanical arm under different position relations is obtained based on the reference function relation, and therefore the motion parameters of all joints are obtained according to the inverse solution function relation in an analysis mode. The reference function relationship is uniquely determined according to joint association parameters under the RTT configuration, namely, the joint parameters of the rotary joint, the first movable joint and the second movable joint are determined, so that inverse solution function relationships of the axes of the joints under different position relationships are uniquely determined, inverse solution analysis under each position relationship is performed through the inverse solution function relationships under each position relationship, joint rotation angles of the rotary joint, movement distances of the first movable joint and movement distances of the second movable joint can be respectively obtained through analysis, and movement control of the three-joint mechanical arm of the RTT configuration is performed according to analysis results. The three-joint mechanical arm with the RTT configuration can realize the inverse solution solving under various conditions, so that the inverse solution process of the three-joint mechanical arm is more comprehensive, and the accuracy of the inverse solution result is higher due to higher inverse solution efficiency.

Drawings

In order to more clearly illustrate the technical solutions of the embodiments of the present application, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present application and therefore should not be considered limiting the scope, and that other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.

Fig. 1 is a mathematical modeling schematic diagram of a three-joint mechanical arm according to an embodiment of the present application;

fig. 2 is a schematic flow chart of a first method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application;

fig. 3 is a schematic flow chart of a second method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application;

fig. 4 is a schematic flow chart of a third method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application;

fig. 5 is a schematic flow chart of a fourth method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application;

fig. 6 is a schematic flow chart of a fifth method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application;

FIG. 7 is an infinitely schematic illustration of an exemplary embodiment of the present disclosure;

FIG. 8 is a schematic diagram of a simulation result provided in an embodiment of the present application;

fig. 9 is a schematic diagram of a motion control device for a three-joint mechanical arm according to an embodiment of the present application;

fig. 10 is a schematic structural diagram of an electronic device according to an embodiment of the present application.

Detailed Description

For the purpose of making the objects, technical solutions and advantages of the embodiments of the present application more clear, the technical solutions of the embodiments of the present application will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present application, and it should be understood that the accompanying drawings in the present application are only for the purpose of illustration and description, and are not intended to limit the protection scope of the present application. In addition, it should be understood that the schematic drawings are not drawn to scale. A flowchart, as used in this application, illustrates operations implemented according to some embodiments of the present application. It should be understood that the operations of the flow diagrams may be implemented out of order and that steps without logical context may be performed in reverse order or concurrently. Moreover, one or more other operations may be added to the flow diagrams and one or more operations may be removed from the flow diagrams as directed by those skilled in the art.

In addition, the described embodiments are only some, but not all, of the embodiments of the present application. The components of the embodiments of the present application, which are generally described and illustrated in the figures herein, may be arranged and designed in a wide variety of different configurations. Thus, the following detailed description of the embodiments of the present application, as provided in the accompanying drawings, is not intended to limit the scope of the application, as claimed, but is merely representative of selected embodiments of the application. All other embodiments, which can be made by those skilled in the art based on the embodiments of the present application without making any inventive effort, are intended to be within the scope of the present application.

The mechanical arm belongs to a special robot, and the motion control of the robot is referred, so that the inverse kinematics solution of the mechanical arm is the most basic processing step in the mechanical arm control. The motion parameters of all joints in the mechanical arm can be obtained through inverse solution, so that the tail end of the mechanical arm can be moved to a designated position from the current position by controlling all joints to move according to the corresponding motion parameters.

Generally, mechanical arms are of various types, including two joints, three joints and multiple joints, and methods for inverse kinematics solution are also of various types, but the existing inverse solution method is low in solution efficiency and cannot guarantee the integrity, convergence and robustness of inverse solution. While the PoE (Product ofExponential, exponential equation) model-based approach overcomes the above problems, it is only applicable to the solution of two-joint robots.

Based on the method, the inverse solution problem of the three-joint mechanical arm is split into the combination solution of the two-joint sub-problem on the basis of the PoE model, so that the inverse solution analysis of the three-joint mechanical arm can be perfectly solved while the solution efficiency is ensured.

The scheme is illustrated by the following specific examples:

fig. 1 is a mathematical modeling schematic diagram of a three-joint mechanical arm according to an embodiment of the present application; the three-joint mechanical arm with the target structure can comprise a rotary joint, a first movable joint and a second movable joint which are sequentially linked with each other. One case can be a three-joint mechanical arm with a rotary joint, a movable joint and a movable joint which are sequentially linked to form an RTT (round trip time) configuration; in another case, the movable joint and the rotary joint are sequentially linked to form the three-joint mechanical arm with the TTR configuration. The RTT configuration and the TTR configuration can be considered to be equivalent to each other, so that the scheme can be applied to the three-joint mechanical arm inverse solution of the RTT configuration and the three-joint mechanical arm inverse solution of the TTR configuration.

As shown in fig. 1Taking mathematical modeling of three-joint mechanical arm in RTT configuration as an example, circle C ₁ The axis 1 represents a first movement joint, the axis 2 represents a second movement joint, θ ₁ Indicating the joint rotation angle theta of the rotary joint ₂ A first movement distance theta representing the first movement joint ₃ Representing a second distance of movement of a second articulation. Zeta type toy ₁ Representing the rotational joint axis, ζ of the rotational joint ₂ First joint axis ζ representing first movement joint ₃ Representing the joint axis of the second mobile joint. P is p ₁ Representing a reference point on the rotational joint axis of the rotational joint. p represents a starting point where the tail end point of the mechanical arm is currently located, q represents an instruction point to which the tail end point of the mechanical arm is to be operated, c represents a first intermediate point, and d represents a second intermediate point; the inverse solution of the three-joint mechanical arm can be disassembled into the inverse solution sub-problem of two joints, and the inverse solution problem from the starting point to the instruction point of the end point of the mechanical arm is disassembled into the sub-problem from the starting point to the first intermediate point, from the first intermediate point to the second intermediate point and from the second intermediate point to the instruction point. Therefore, the combination solution can be carried out according to the inverse solution sub-problem between every two joints, and the inverse solution analysis of three joints is realized.

Through inverse solution analysis, theta can be obtained by solving ₁ 、θ ₂ And theta ₃ Thereby respectively controlling the rotation theta of the rotary joint ₁ Angle, control of first movement joint movement theta ₂ Control the distance of the second movable joint movement theta ₃ To realize the motion control of the three-joint mechanical arm from the starting point to the instruction point.

Fig. 2 is a schematic flow chart of a first method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application; the method can be applied to processing equipment in a mechanical arm control system, wherein the mechanical arm control system comprises the processing equipment and a three-joint mechanical arm with a target structure, and the three-joint mechanical arm can be the mechanical arm with the RTT configuration or the mechanical arm with the TTR configuration; as shown in fig. 2, the method may include:

s201, acquiring an instruction point to which the tail end of the mechanical arm is to be operated.

The instruction point is that point to be moved to the tail end of the mechanical arm, assuming that the tail end of the mechanical arm is at the position 1 at the current moment and the mechanical arm is to be controlled to move to the position 2 at the next moment, the instruction point is that at which the position 2 is located, and the input of the instruction point can be performed through a central control terminal on the mechanical arm, or the input of the instruction point can also be performed through an independent terminal device.

S202, determining an inverse solution function relation corresponding to the position relation according to the position relation among joint axes of all joints in the mechanical arm.

The inverse solution function relationship corresponding to the position relationship can be determined according to the position relationship between the rotary joint and the joint axes of the first movable joint and the second movable joint, respectively.

Because of the different position structures of the joints in the mechanical arm, different position relations among the joint axes can be caused, and the position relations can comprise parallel, vertical, non-parallel or non-vertical.

When the position relations among the joint axes are different, the corresponding inverse solution function relations are also different, namely the inverse solution function relations under the unique determined position relation can be obtained.

S203, according to the inverse solution function relation, respectively determining the joint rotation angle of the rotary joint, the first moving distance of the first moving joint and the second moving distance of the second moving joint.

The inverse solution function relation is obtained by deformation according to the reference function relation, and the reference function relation is obtained by construction according to algebraic equation relation of the intermediate point and constraint function relation between the intermediate point and the instruction point; the algebraic equation relation of the middle point is constructed according to the starting point where the tail end of the mechanical arm is currently positioned and joint association parameters, wherein the joint association parameters comprise: a first travel distance of the first mobile joint, a first direction vector of a first joint axis of the first mobile joint, a second travel distance of the second mobile joint, and a second direction vector of a second joint axis of the second mobile joint; the intermediate point comprises a designated point through which the tail end of the mechanical arm passes in the process of running from the starting point to the instruction point.

By inverse solutionSolving the functional relation, thereby sequentially determining the joint rotation angle theta of the rotary joint ₁ Value of (2), first movement distance θ of first movement joint ₂ Is a value of (a) and a second movement distance θ of the second movement joint ₃ Is a value of (2).

The inverse solution function relationship can be obtained by deforming according to a reference function relationship, and the reference function relationship is obtained by constructing according to algebraic equation relationship of the intermediate point and constraint function relationship between the intermediate point and the instruction point.

As shown in fig. 1, assume that the starting point p is along the joint axis ζ ₃ Move θ ₃ To a first intermediate point c along the joint axis ζ ₂ Move θ ₂ To a second intermediate point d, which is about the joint axis ζ ₁ Rotation θ ₁ To point instruction point q, and reference point p ₁ Is the joint axis xi ₁ Any point above. Then, it can be known that algebraic equation relation of the first intermediate point c and the second intermediate point d is established, respectively.

The algebraic equation relation of the middle point is constructed according to the starting point where the tail end of the mechanical arm is currently located and the joint association parameters. In a three-joint mechanical arm scene based on RTT configuration, the joint association parameters may include: a first travel distance of the first mobile joint, a first direction vector of a first joint axis of the first mobile joint, a second travel distance of the second mobile joint, and a second direction vector of a second joint axis of the second mobile joint.

The three-joint mechanical arm with different configurations has different joints and different connection relations and position relations among the joints, so that the joint association parameters are different, namely, when the algebraic equation relation of the intermediate points is acquired, the joint association parameters are unique, and therefore, the algebraic equation relation of the determined intermediate points is also unique.

Likewise, the constraint function relationship between the intermediate point and the instruction point is unique. Therefore, the reference function relation of the three-joint mechanical arm with the RTT configuration can be uniquely determined.

Then, through the deformation of the reference function relationship, the inverse solution function relationship of each joint axis under the corresponding position relationship can be uniquely determined, so that the motion parameters of each joint under the position relationship can be accurately obtained by analysis based on the inverse solution function relationship.

S204, controlling the rotation angle of the rotary joint of the mechanical arm, controlling the first moving joint to move by a first moving distance and controlling the second moving joint to move by a second moving distance respectively, so that the tail end of the mechanical arm moves from a starting point to an instruction point.

Optionally, the rotation joint, the first moving joint and the second moving joint are respectively controlled to move according to the determined joint rotation angle, the first moving distance and the second moving distance, so that the end point of the mechanical arm can be moved from the starting point to the instruction point, and one-time motion control is completed.

Then, the instruction point is used as a new starting point, a new instruction point is designated, the steps are repeated, and the motion control is continuously realized again and again.

In summary, according to the three-joint mechanical arm motion control method provided by the embodiment, mathematical modeling is performed through the current starting point of the mechanical arm, the instruction point to be operated to, the middle point and the motion parameters of each joint, so as to construct a mathematical model of the three-joint mechanical arm with the RTT configuration, and a reference function relationship corresponding to the three-joint mechanical arm with the RTT configuration is constructed, and based on the reference function relationship, an inverse solution function relationship of each joint axis of the mechanical arm under different position relationships is obtained, so that the motion parameters of each joint are obtained according to the inverse solution function relationship. The reference function relationship is uniquely determined according to joint association parameters under the RTT configuration, namely, the joint parameters of the rotary joint, the first movable joint and the second movable joint are determined, so that inverse solution function relationships of the axes of the joints under different position relationships are uniquely determined, inverse solution analysis under each position relationship is performed through the inverse solution function relationships under each position relationship, joint rotation angles of the rotary joint, movement distances of the first movable joint and movement distances of the second movable joint can be respectively obtained through analysis, and movement control of the three-joint mechanical arm of the RTT configuration is performed according to analysis results. The three-joint mechanical arm with the RTT configuration can realize the inverse solution solving under various conditions, so that the inverse solution process of the three-joint mechanical arm is more comprehensive, and the accuracy of the inverse solution result is higher due to higher inverse solution efficiency.

Fig. 3 is a schematic flow chart of a second method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application; optionally, in step S202, determining an inverse solution function relationship corresponding to the position relationship according to the position relationship between joint axes of the joints in the mechanical arm may include:

s301, determining a vector equation relation which is satisfied between direction vectors of the joint axes under the position relation according to the position relation between the joint axes of the joints.

The positional relationship between the joint axes may refer to a parallel relationship or a perpendicular relationship between the joint axes, and when the positional relationship between the two joint axes is determined, a vector equation relationship satisfied between the direction vectors of the two joint axes is also determined.

S302, determining an inverse solution function relation corresponding to the position relation according to the vector equation relation and the reference function relation.

And based on the determined vector equation relationship, the reference function relationship can be deformed, so that an inverse solution function relationship corresponding to the position relationship is determined.

The modification of the reference function relationship may be realized by replacing coefficients of parameters in the reference function relationship, etc., as will be understood in particular from the following embodiments.

Alternatively, the algebraic equality relation of the intermediate points may comprise: a first algebraic equality relation corresponding to the first intermediate point and a second algebraic equality relation corresponding to the second intermediate point.

The first algebraic equation relation is used to characterize the relation between the first intermediate point and the starting point, the second movement distance and the second direction vector.

As shown in fig. 1, when the starting point p is along the joint axis ζ ₃ Move θ ₃ To a first intermediate point c along the joint axis ζ ₂ Move θ ₂ To a second intermediate point d, which is about the joint axis ζ ₁ Rotation θ ₁ To point instruction point q, and reference point p ₁ Is the joint axis xi ₁ At any point above, c=p+θ ₃ v ₃ Is established such that the first generation equation relationship of the first intermediate point is c=p+θ ₃ v ₃ Wherein p represents a starting point, θ ₃ For a second distance of movement v ₃ Is the second direction vector (the direction vector of the joint axis of the second mobile joint).

Similarly, when the starting point p is along the joint axis ζ ₃ Move θ ₃ To a first intermediate point c along the joint axis ζ ₂ Move θ ₂ To a second intermediate point d, which is about the joint axis ζ ₁ Rotation θ ₁ To point instruction point q, and reference point p ₁ Is the joint axis xi ₁ At any point above, d=c+θ ₂ v ₂ And the second algebraic equation relation d=c+θ, which is established to obtain the second intermediate point ₂ v ₂ . Wherein θ ₂ For a first distance of movement v ₂ Is a first direction vector (a direction vector of the joint axis of the first mobile joint).

Optionally, the constraint function relationship between the intermediate point and the instruction point may include: geometric constraint functions and algebraic constraint functions.

The geometric constraint function is used for representing the geometric relationship between the second intermediate point and the direction vector of the rotating joint axis of the rotating joint;

as can be seen in FIG. 1, the second intermediate point d and the command point q are both located perpendicular to the joint axis ζ ₁ Circle C of (2) ₁ On the above, a geometric constraint function as shown in formula (1) can be obtained according to the geometric constraint:

ω ₁ ^T (d-q)＝0, (1)

wherein omega ₁ Representing the rotational joint axis of the rotational joint.

The algebraic constraint function is used to characterize the algebraic relationship between the second intermediate point and the reference point on the rotational joint axis and the instruction point.

Similarly, an algebraic constraint function shown in formula (2) can be obtained according to algebraic constraint:

||q-p ₁ ||＝||d-p ₁ ||.(2)

then, the first-generation equation relation c=p+θ described above will be described ₃ v ₃ And a second algebraic equation relation d=c+θ ₂ v ₂ Respectively substituting into the formula (1) and the formula (2), squaring the two sides of the equal sign of the formula (2), and then

Replaced by the corresponding Rodrijies formula, formulas (1) and (2) are then converted to formulas (3) and (4), respectively

x ₁ θ ₂ +y ₁ θ ₃ +z ₁ ＝0, (3)

x ₂ θ ₂ ² +y ₂ θ ₃ ² +a ₁ θ ₂ θ ₃ +a ₂ θ ₂ +a ₃ θ ₃ +z ₂ ＝0, (4)

Wherein the parameter is x ₁ ＝ω ₁ ^T v ₂ ,y ₁ ＝ω ₁ ^T v ₃ ,z ₁ ＝ω ₁ ^T (p-q),x ₂ ＝y ₂ ＝1,a ₁ ＝2v ₃ ^T v ₂ ,a ₂ ＝2(p-p ₁ ) ^T v ₂ ,a ₃ ＝2(p-p ₁ ) ^T v ₃ ,z ₂ ＝||p-p ₁ || ² -||q-p ₁ || ² 。

Then, the formula (3) and the formula (4) are collectively referred to as a reference function relationship.

The following discussion is directed to the inverse solution of the joint motion parameters of the joint axis at each positional relationship.

It is first clear that for both rotational joints, if ζ ₁ ∥ξ ₂ There is

Establishment; and for any two joints, if ζ ₁ ⊥ξ ₂ Omega of ₁ ^T ω ₂ =0 holds.

Fig. 4 is a schematic flow chart of a third method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application; case 1: zeta type toy ₁ Not perpendicular to xi ₃ But xi ₁ ⊥ξ ₂ I.e. the rotational joint axis ζ of the rotational joint ₁ A second joint axis ζ not perpendicular to the second movable joint ₃ And the axis ζ of the rotary joint ₁ First joint axis ζ perpendicular to the first movable joint ₂ The method comprises the steps of carrying out a first treatment on the surface of the The inverse solution function relationship is a function of the second travel distance correlation in the reference function relationship.

As the joint axis xi ₁ Not perpendicular to xi ₃ But xi ₁ ⊥ξ ₂ When the direction vectors of the joint axes are shown to satisfy the vector equation relation: omega ₁ ^T v ₃ Not equal to 0 and ω ₁ ^T v ₂ =0, at this time, equation (3) in the reference function relationship is converted into θ ₃ ＝-z ₁ /y ₁ 。

In step S203, determining the joint rotation angle of the rotary joint, the first movement distance of the first movement joint, and the second movement distance of the second movement joint according to the inverse solution function relationship, respectively, may include:

s401, determining a second moving distance according to the inverse solution function relation.

At this time, the inverse solution function relationship is referred to as θ ₃ ＝-z ₁ /y ₁ Then, the second movement distance theta can be determined by inverse solution function relation ₃ Is a value of (2).

S402, determining the first moving distance according to the second moving distance and a functional relation between the first moving distance and the second moving distance in the reference functional relation.

Optionally, the second movement distance θ ₃ Is substituted into equation (4) in the reference function relationship, then equation (4) is with respect to the first movement distance θ ₂ Is a one-dimensional quadratic equation of (a),then the first moving distance theta can be obtained by solving ₂ Is a value of (2).

S403, determining the value of the first intermediate point according to the second moving distance and the relation of the first generation equation.

Then, the second movement distance θ ₃ The value substituted into the first-generation equality relation c=p+θ ₃ v ₃ The value of the first intermediate point c can be determined.

S404, determining the value of the second intermediate point according to the value of the first intermediate point, the first moving distance and the second algebraic equation relation.

And the value of the first intermediate point c and the first moving distance theta ₂ The value substituted into the second algebraic equality relation d=c+θ ₂ v ₂ The value of the second intermediate point d can be determined.

S405, determining the value of the joint rotation angle according to the value of the second intermediate point, the instruction point and the preset motion function relation between the space point and the joint axis.

It should be noted that, the three-joint kinematic model based on the rotation theory has definite physical meaning, namely when the space point p rotates around the joint axis xi by an angle theta to the space point q, namely

If true, the homogeneous coordinates are converted into space coordinates for representation, and the motion function relationship between the preset space point and the joint axis can be obtained and represented by the following formula (5):

where vector r represents any point r on the joint axis ζ.

In the present embodiment, when the spatial point p is taken as the second intermediate point d and the spatial point q is taken as the command point q, ω represents the joint axis of the rotary joint, θ represents the joint rotation angle θ ₁ Thereby substituting the values of the second space point d and the instruction point q into the formula (5) to calculate the joint rotation angle theta ₁ Is a value of (2).

Fig. 5 is a schematic flow chart of a fourth method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application; case 2: zeta type toy ₁ Not perpendicular to xi ₂ But xi ₁ ⊥ξ ₃ I.e. the rotational joint axis ζ of the rotational joint ₁ First joint axis ζ not perpendicular to the first movable joint ₂ And the axis ζ of the rotary joint ₁ A second joint axis ζ perpendicular to the second movable joint ₃ The method comprises the steps of carrying out a first treatment on the surface of the The inverse solution function relationship is a function of the first travel distance correlation in the reference function relationship.

As the joint axis xi ₁ Not perpendicular to xi ₂ But xi ₁ ⊥ξ ₃ When the direction vectors of the joint axes are shown to satisfy the vector equation relation: omega ₁ ^T v ₂ Not equal to 0 and ω ₁ ^T v ₃ =0, at this time, equation (3) in the reference function relationship is converted into θ ₂ ＝-z ₁ /x ₁ 。

s501, determining a first moving distance according to the inverse solution function relation.

At this time, the inverse solution function relationship is referred to as θ ₂ ＝-z ₁ /x ₁ Then, the first movement distance theta can be determined by the inverse solution function relation ₂ Is a value of (2).

S502, determining a second moving distance according to the first moving distance and a functional relation between the first moving distance and the second moving distance in the reference functional relation.

Alternatively, the first movement distance θ ₂ Is substituted into equation (4) in the reference function relationship, then equation (4) is with respect to the second movement distance θ ₃ Then, the second movement distance θ can be solved ₃ Is a value of (2).

S503, determining the value of the first intermediate point according to the second moving distance and the relation of the first generation equation.

S504, determining a value of a second intermediate point according to the value of the first intermediate point, the first moving distance and the second algebraic equation relation.

S505, determining the value of the joint rotation angle according to the value of the second intermediate point, the instruction point and the preset motion function relation between the space point and the joint axis.

Thereby substituting the values of the second space point d and the instruction point q into the formula (5) to calculate the joint rotation angle theta ₁ Is a value of (2).

Fig. 6 is a schematic flow chart of a fifth method for controlling motion of a three-joint mechanical arm according to an embodiment of the present application; case 3: zeta type toy ₁ Not perpendicular to xi ₂ And xi ₁ Not perpendicular to xi ₃ I.e. the rotational joint axis ζ of the rotational joint ₁ First joint axis ζ not perpendicular to the first movable joint ₂ And the axis ζ of the rotary joint ₁ A second joint axis ζ not perpendicular to the second movable joint ₃ The method comprises the steps of carrying out a first treatment on the surface of the The inverse solution function relationship is a function of the second travel distance correlation in the reference function relationship.

As the joint axis xi ₁ Not perpendicular to xi ₂ And xi ₁ Not perpendicular to xi ₃ When the direction vectors of the joint axes are shown to satisfy the vector equation relation: omega ₁ ^T v ₃ Not equal to 0 and ω ₁ ^T v ₂ Not equal to 0, in this case, the first movement distance θ can be calculated by combining the equation (3) and the equation (4) in the reference function relationship ₂ By a second distance of movement theta ₃ The representation is: θ ₂ ＝-(y ₁ θ ₃ +z ₁ )/x ₁ 。

Alternatively, θ ₂ ＝-(y ₁ θ ₃ +z ₁ )/x ₁ Substituting into equation (4), equation (4) may be converted into equation (6) as follows:

m ₁ θ ₃ ² +m ₂ θ ₃ +m ₃ ＝0, (6)

wherein each term coefficient is m ₁ ＝x ₂ y ₁ ² +x ₁ ² y ₂ -a ₁ x ₁ y ₁ ,m ₂ ＝2x ₂ y ₁ z ₁ -a ₁ x ₁ z ₁ -a ₂ x ₁ y ₁ +a ₃ x ₁ ² And m ₃ ＝x ₂ z ₁ ² -a ₂ x ₁ z ₁ +x ₁ ² z ₂ 。

s601, determining a second moving distance according to the inverse solution function relation.

At this time, the inverse solution function relationship is expressed as m ₁ θ ₃ ² +m ₂ θ ₃ +m ₃ =0, if m ₁ Not equal to 0, the second moving distance θ ₃ Solving as

If m is ₁ =0, equation (5) is then converted to m ₂ θ ₃ +m ₃ =0, thereby obtaining the second moving distance θ ₃ 。

S602, determining the first moving distance according to the second moving distance and a functional relation between the first moving distance and the second moving distance in the reference functional relation.

Optionally, the second movement distance θ ₃ Is substituted into formula (3) in the reference function relationship, then formula (3) is as for the first movement distance θ ₂ Then, the first movement distance theta can be solved ₂ Is a value of (2).

S603, determining a value of the first intermediate point according to the second moving distance and the first generation equation relation.

S604, determining the value of the second intermediate point according to the value of the first intermediate point, the first moving distance and the second algebraic equation relation.

S605, determining the value of the joint rotation angle according to the value of the second intermediate point, the instruction point and the preset motion function relation between the space point and the joint axis.

Case 4: zeta type toy ₁ ⊥ξ ₃ And xi ₁ ⊥ξ ₂ I.e. the rotational joint axis ζ of the rotational joint ₁ First joint axis ζ perpendicular to the first movable joint ₂ And the axis ζ of the rotary joint ₁ A second joint axis ζ perpendicular to the second movable joint ₃ 。

As the joint axis xi ₁ ⊥ξ ₃ And xi ₁ ⊥ξ ₂ When it indicates ω ₁ ^T v ₂ =0 and ω ₁ ^T v ₃ =0, i.e. the points c, d, p, q are all in the same plane, equation (3) in the reference function relationship is constant, and there are two unknown variable joint angles θ in equation (4) ₁ And theta ₃ There are infinite solutions.

Fig. 7 is an infinitely-solved schematic diagram provided in an embodiment of the present application. As shown in FIG. 7, circle C ₁ All points on the table are inverse solutions.

To this end, for joint transport in various possible joint axis positionsThe inverse solution of the dynamic parameters is described in detail, so that the joint rotation angle theta of the rotary joint is obtained based on the inverse solution ₁ First movement distance theta of first movement joint ₂ Second movement distance θ of the second movement joint ₃ The motion control of the mechanical arm can be realized.

The scheme is verified through simulation through a specific embodiment.

First, description is made of the basic mathematical knowledge involved:

The three-joint kinematics model based on the rotation theory has definite physical meaning, namely the rotation or the movement of a space point around a joint axis, and the three-joint kinematics model is as follows:

under the same joint axis, equation (4-1) can be converted into by reversing the order of the movements, and reversing the positions of the starting and ending points of the movements:

the spatial point p rotates by an angle theta about the joint axis xi to a spatial point q, i.e

Converting the homogeneous coordinates into a spatial coordinate representation, equation (4-3) can be obtained:

it is noted that equation (4-3) is also equation (5)

Where vector r represents any point r on the joint axis ζ. Will be

Rodrijies formula (I)

Substituting formula (4-3) to obtain:

x sinθ+y cosθ+z＝0 (4-4)

wherein the method comprises the steps of

Due to x ^T y=0 and x ^T x＝y ^T y, the rotation angle θ can be determined by +.>

And->

And (5) solving.

The distance invariant principle is typically applied in solving the angle of the revolute joint, accompanied by squaring the two ends of the opposite equation. For example, the distance between vector s and vector t is δ, i.e., ||s-t|=δ, then the square is easily taken across the pair-wise equation ² +||t|| ² +2t ^T s＝δ ² 。

Simulation verification of RTT sub-problem analysis inverse solution

Fig. 8 is a schematic diagram of a simulation result provided in an embodiment of the present application. The form in which RTT sub-problems may exist and all analytical inverse solutions are analyzed in detail above, where case 3 is selected: joint axis xi ₁ Not perpendicular to xi ₃ Nor is perpendicular to ζ ₂ Which is simulated to verify the correctness of the analytical solution. Giving a group of joint rotation and reference point positions meeting the relation of joint axes, wherein the direction vectors of the three joint axes are omega ₁ ＝[1 0 1] ^T 、v ₂ ＝[0 1 1] ^T And v ₃ ＝[2 1 0] ^T The reference point position vector of the rotary joint is p ₁ ＝[0 0 0] ^T The position vector of the starting point p is p= [ 5-10-5] ^T . Given three joints of range of motion

And->

According to the given motion range, 51 groups of data points are uniformly sampled, the position of a point p around a point q after each joint moves is calculated according to a formula (4-1), and three joints are subjected to inverse solution according to the position of the point q, and the angle value of the inverse solution is compared with the given sampled data points. It should be noted that the direction vectors of the various motion joints need to be normalized prior to computation, i.e. ω=ω -i omega i. After the inverse solution value of the joint is calculated, the inverse solution needs to be determined, and if the range limit is not satisfied, the inverse solution is discarded correspondingly. As shown in FIG. 8, where the symbol "o" represents the sampling point for each joint, the symbol "+" represents the inverse solution value for each joint, and curve 1 represents the rotational joint angle θ ₁ Curve 2 represents the first movement distance θ ₂ Curve 3 represents the second movement distance θ ₃ . As is apparent from fig. 8, the sampling points are consistent with the inverse solution values, and there is no error in the theoretical calculation, so that the correctness of the proposed RTT sub-problem resolution solution is verified.

In summary, in the three-joint mechanical arm motion control method provided in this embodiment, mathematical modeling is performed through the starting point where the mechanical arm is currently located, the instruction point to be operated to, the intermediate point and the motion parameters of each joint, so as to construct a mathematical model of the three-joint mechanical arm with the RTT configuration, construct a reference function relationship corresponding to the three-joint mechanical arm with the RTT configuration, and obtain an inverse solution function relationship of each joint axis of the mechanical arm under different positional relationships based on the reference function relationship, thereby analyzing and obtaining the motion parameters of each joint according to the inverse solution function relationship. The reference function relationship is uniquely determined according to joint association parameters under the RTT configuration, namely, the joint parameters of the rotary joint, the first movable joint and the second movable joint are determined, so that inverse solution function relationships of the axes of the joints under different position relationships are uniquely determined, inverse solution analysis under each position relationship is performed through the inverse solution function relationships under each position relationship, joint rotation angles of the rotary joint, movement distances of the first movable joint and movement distances of the second movable joint can be respectively obtained through analysis, and movement control of the three-joint mechanical arm of the RTT configuration is performed according to analysis results. The three-joint mechanical arm with the RTT configuration can realize the inverse solution solving under various conditions, so that the inverse solution process of the three-joint mechanical arm is more comprehensive, and the accuracy of the inverse solution result is higher due to higher inverse solution efficiency.

The following describes a device, equipment, a storage medium, etc. for executing the method for controlling motion of a three-joint mechanical arm provided in the present application, and specific implementation processes and technical effects of the method are referred to above, which are not described in detail below.

Fig. 9 is a schematic diagram of a three-joint mechanical arm motion control device according to an embodiment of the present application, where functions implemented by the three-joint mechanical arm motion control device correspond to steps executed by the above method. The device is understood to be a processing apparatus as described above. As shown in fig. 9, the apparatus may include: an acquisition module 910, a determination module 920, a control module 930;

an obtaining module 910, configured to obtain an instruction point to which the end of the mechanical arm is to run;

the determining module 920 is configured to determine an inverse solution function relationship corresponding to the position relationship according to the position relationship between joint axes of the joints in the mechanical arm;

the determining module 920 is configured to determine a joint rotation angle of the rotary joint, a first movement distance of the first moving joint, and a second movement distance of the second moving joint according to the inverse solution function relationship, respectively; the inverse solution function relation is obtained by deformation according to a reference function relation, and the reference function relation is obtained by construction according to algebraic equation relation of the intermediate point and constraint function relation between the intermediate point and the instruction point; the algebraic equation relation of the middle point is constructed according to the starting point where the tail end of the mechanical arm is currently positioned and joint association parameters, wherein the joint association parameters comprise: a first travel distance of the first mobile joint, a first direction vector of a first joint axis of the first mobile joint, a second travel distance of the second mobile joint, and a second direction vector of a second joint axis of the second mobile joint; the middle point comprises a designated point which is passed by the tail end of the mechanical arm in the process of running from the starting point to the instruction point;

The control module 930 is configured to control a rotation angle of the rotary joint of the mechanical arm, control the first moving joint to move by a first moving distance, and control the second moving joint to move by a second moving distance, so that the end of the mechanical arm moves from a start point to a command point.

Optionally, the determining module 920 is specifically configured to determine, according to a positional relationship between joint axes of the joints, a vector equation relationship satisfied between direction vectors of the joint axes under the positional relationship;

the first generation equation relation is used for representing the relation between the first middle point and the starting point, the second moving distance and the second direction vector;

Optionally, if the rotational joint axis of the rotational joint is not perpendicular to the second joint axis of the second mobile joint, and the rotational joint axis is perpendicular to the first joint axis of the first mobile joint; the inverse solution function relationship is a function related to the second moving distance in the reference function relationship;

a determining module 920, specifically configured to determine a second movement distance according to the inverse solution function relationship;

determining a first moving distance according to the second moving distance and a functional relation between the first moving distance and the second moving distance in the reference functional relation;

determining a value of the first intermediate point according to the second movement distance and the first generation equation relationship;

determining a value of a second intermediate point according to the value of the first intermediate point, the first movement distance and the second algebraic equation relationship;

and determining the value of the joint rotation angle according to the value of the second intermediate point, the instruction point and the preset motion function relation between the space point and the joint axis.

Optionally, if the rotational joint axis of the rotational joint is not perpendicular to the first joint axis of the first mobile joint and the rotational joint axis is perpendicular to the second joint axis of the second mobile joint; the inverse solution function relationship is a function related to the first moving distance in the reference function relationship;

a determining module 920, specifically configured to determine a first movement distance according to the inverse solution function relationship;

determining a second moving distance according to the first moving distance and a functional relation between the first moving distance and the second moving distance in the reference functional relation;

Optionally, if the rotational joint axis of the rotational joint is not perpendicular to the first joint axis of the first mobile joint and the rotational joint axis is not perpendicular to the second joint axis of the second mobile joint; the inverse solution function relationship is a function related to the second moving distance in the reference function relationship;

The foregoing apparatus is used for executing the method provided in the foregoing embodiment, and its implementation principle and technical effects are similar, and are not described herein again.

The above modules may be one or more integrated circuits configured to implement the above methods, for example: one or more application specific integrated circuits (Application Specific Integrated Circuit, abbreviated as ASIC), or one or more microprocessors (digital singnal processor, abbreviated as DSP), or one or more field programmable gate arrays (Field Programmable Gate Array, abbreviated as FPGA), or the like. For another example, when a module above is implemented in the form of a processing element scheduler code, the processing element may be a general-purpose processor, such as a central processing unit (Central Processing Unit, CPU) or other processor that may invoke the program code. For another example, the modules may be integrated together and implemented in the form of a system-on-a-chip (SOC).

The modules may be connected or communicate with each other via wired or wireless connections. The wired connection may include a metal cable, optical cable, hybrid cable, or the like, or any combination thereof. The wireless connection may include a connection through a LAN, WAN, bluetooth, zigBee, or NFC, or any combination thereof. Two or more modules may be combined into a single module, and any one module may be divided into two or more units. It will be clearly understood by those skilled in the art that, for convenience and brevity of description, specific working procedures of the above-described system and apparatus may refer to corresponding procedures in the method embodiments, which are not described in detail in this application.

Fig. 10 is a schematic structural diagram of an electronic device according to an embodiment of the present application, as shown in fig. 10, where the device may include: a processor 801, and a storage medium 802.

The storage medium 802 is used to store a program, and the processor 801 calls the program stored in the storage medium 802 to execute the above-described method embodiment. The specific implementation manner and the technical effect are similar, and are not repeated here.

Therein, the storage medium 802 stores program code that, when executed by the processor 801, causes the processor 801 to perform various steps in the three-joint mechanical arm motion control method according to various exemplary embodiments of the present application described in the section of the description of the "exemplary method" above.

The processor 801 may be a general purpose processor such as a Central Processing Unit (CPU), digital signal processor (Digital Signal Processor, DSP), application specific integrated circuit (Application Specific Integrated Circuit, ASIC), field programmable gate array (Field Programmable Gate Array, FPGA) or other programmable logic device, discrete gate or transistor logic, discrete hardware components, and may implement or perform the methods, steps, and logic blocks disclosed in embodiments of the present application. The general purpose processor may be a microprocessor or any conventional processor or the like. The steps of a method disclosed in connection with the embodiments of the present application may be embodied directly in a hardware processor for execution, or in a combination of hardware and software modules in the processor for execution.

The storage medium 802 is a non-volatile computer-readable storage medium that can be used to store non-volatile software programs, non-volatile computer-executable programs, and modules. The storage medium may include at least one type of storage medium, and may include, for example, flash Memory, a hard disk, a multimedia card, a card-type storage medium, a random access storage medium (Random Access Memory, RAM), a static random access storage medium (Static Random Access Memory, SRAM), a programmable Read-Only storage medium (Programmable Read Only Memory, PROM), a Read-Only storage medium (ROM), a charged erasable programmable Read-Only storage medium (Electrically Erasable Programmable Read-Only storage), a magnetic storage medium, a magnetic disk, an optical disk, and the like. A storage medium is any other medium that can be used to carry or store desired program code in the form of instructions or data structures and that can be accessed by a computer, but is not limited to such. The storage medium 802 in the embodiments of the present application may also be a circuit or any other device capable of implementing a storage function, for storing program instructions and/or data.

Optionally, the present application also provides a program product, such as a computer readable storage medium, comprising a program for performing the above-described method embodiments when being executed by a processor.

In the several embodiments provided in this application, it should be understood that the disclosed apparatus and method may be implemented in other ways. For example, the apparatus embodiments described above are merely illustrative, e.g., the division of the units is merely a logical function division, and there may be additional divisions when actually implemented, e.g., multiple 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 form.

The units described as separate units may or may not be physically separate, and units shown as units may or may not be physical units, may be located in one place, or may be distributed on a plurality of network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

In addition, each functional unit in each embodiment 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 hardware plus software functional units.

The integrated units implemented in the form of software functional units described above may be stored in a computer readable storage medium. The software functional unit is stored in a storage medium, and includes several instructions for causing a computer device (which may be a personal computer, a server, or a network device, etc.) or a processor (english: processor) to perform part of the steps of the methods described in the embodiments of the present application. And the aforementioned storage medium includes: u disk, mobile hard disk, read-Only Memory (ROM), random access Memory (Random Access Memory, RAM), magnetic disk or optical disk, etc.

Claims

1. The motion control method of the three-joint mechanical arm is characterized by being applied to processing equipment in a mechanical arm control system, wherein the system comprises the processing equipment and the three-joint mechanical arm with a target structure, and the three-joint mechanical arm comprises a rotary joint, a first movable joint and a second movable joint which are sequentially linked with each other; the method comprises the following steps:

2. The method according to claim 1, wherein determining an inverse solution function relationship corresponding to the positional relationship according to the positional relationship between joint axes of joints in the mechanical arm includes:

3. The method of claim 1, wherein the algebraic equality relation of the intermediate points comprises: a first algebraic equality relation corresponding to the first intermediate point and a second algebraic equality relation corresponding to the second intermediate point;

4. A method according to claim 3, wherein the constraint function relationship between the intermediate point and the instruction point comprises: a geometric constraint function and an algebraic constraint function;

5. A method according to claim 3, wherein if the rotational joint axis of the rotational joint is not perpendicular to the second joint axis of the second mobile joint and the rotational joint axis is perpendicular to the first joint axis of the first mobile joint; the inverse solution function relationship is a function related to the second movement distance in the reference function relationship;

6. A method according to claim 3, wherein if the rotational joint axis of the rotational joint is not perpendicular to the first joint axis of the first mobile joint and the rotational joint axis is perpendicular to the second joint axis of the second mobile joint; the inverse solution function relationship is a function related to the first movement distance in the reference function relationship;

7. A method according to claim 3, wherein if the rotational joint axis of the rotational joint is not perpendicular to the first joint axis of the first mobile joint and the rotational joint axis is not perpendicular to the second joint axis of the second mobile joint; the inverse solution function relationship is a function related to the second movement distance in the reference function relationship;

8. The three-joint mechanical arm motion control device is characterized by being applied to processing equipment in a mechanical arm control system, wherein the system comprises the processing equipment and a three-joint mechanical arm with a target structure, and the three-joint mechanical arm comprises a rotary joint, a first movable joint and a second movable joint which are sequentially mutually linked; the device comprises: the device comprises an acquisition module, a determination module and a control module;

9. An electronic device, comprising: the three-joint mechanical arm motion control method according to any one of claims 1 to 7, wherein the storage medium stores program instructions executable by the processor, and the processor communicates with the storage medium through the bus when the electronic device is running.

10. A computer-readable storage medium, characterized in that the storage medium has stored thereon a computer program which, when executed by a processor, performs the steps of the three-joint robot arm motion control method according to any one of claims 1 to 7.