Method and device for controlling motion of a tri-articulated robot arm, electronic device and storage medium

Abstract

The application provides a three-joint mechanical arm motion control method and device, electronic equipment and storage medium, and relates to the technical field of mechanical control. The method performs mathematical modeling through a starting point where the mechanical arm is currently located, an instruction point to be operated to, and intermediate points and motion parameters of each joint, constructs a mathematical model of a three-joint mechanical arm in an RTT configuration, and constructs a corresponding reference function relationship of the three-joint mechanical arm in the RTT configuration. Based on the reference function relationship, inverse solution function relationships of joint axes of the mechanical arm under different position relationships are obtained, so that the motion parameters of each joint are obtained according to the inverse solution function relationships. Since the reference function relationship is uniquely determined according to the joint correlation parameters under the RTT configuration, the inverse solution function relationships of the joint axes under different position relationships are also uniquely determined, and the inverse solution is analyzed according to the inverse solution function relationships, so that the three-joint mechanical arm in the RTT configuration can be comprehensively and completely solved under various conditions.

Description

TECHNICAL FIELD

[0001] The present application relates to the technical field of mechanical control, in particular to a three-joint robot arm motion control method and device, electronic equipment and storage medium. BACKGROUND

[0002] The RTT three-joint robot arm refers to a mechanical arm composed of a rotating joint, a moving joint and a moving joint linked in sequence. For the motion control of the RTT three-joint robot arm, the kinematic inverse solution of the robot arm can be used, which means that the motion vector of each motion joint in the robot arm is solved when the end pose of the robot arm is known.

[0003] At present, how to realize the inverse solution of the RTT three-joint robot arm has become a technical problem to be solved. SUMMARY

[0004] The present application aims at the deficiencies in the prior art, and provides a three-joint robot arm motion control method, device, electronic equipment and storage medium, so as to realize the kinematic inverse solution of the three-joint robot arm with RTT configuration.

[0005] To achieve the above object, the technical scheme adopted by the embodiments of the present application is as follows:

[0006] In a first aspect, the embodiments of the present application provide a three-joint robot arm motion control method applied to a processing device in a robot arm control system, the system comprising a processing device and a three-joint robot arm with a target structure, the three-joint robot arm comprising a rotating joint, a first moving joint and a second moving joint linked in sequence; the method comprising:

[0007] Obtaining an instruction point to which the end of the robot arm is to be operated;

[0008] According to the positional relationship between the joint axes of each joint in the robot arm, determining the inverse solution function relationship corresponding to the positional relationship;

[0009] According to the inverse solution function relationship, joint rotation angles of the rotation joints, a first movement distance of the first movement joint, and a second movement distance of the second movement joint are determined respectively; the inverse solution function relationship is obtained by deforming a reference function relationship, and the reference function relationship is constructed according to an algebraic equation relationship of an intermediate point and a constraint function relationship between the intermediate point and the instruction point; the algebraic equation relationship of the intermediate point is constructed according to a starting point where a terminal end of the mechanical arm currently locates and joint correlation parameters, and the joint correlation parameters include the first movement distance of the first movement joint, a first direction vector of a first joint axis of the first movement joint, the second movement distance of the second movement joint, and a second direction vector of a second joint axis of the second movement joint; the intermediate point includes a specified point passed by the terminal end of the mechanical arm in a process of running from the starting point to the instruction point.

[0010] The rotation joints of the mechanical arm are controlled to rotate the joint rotation angles, the first movement joint is controlled to move the first movement distance, and the second movement joint is controlled to move the second movement distance, so that the terminal end of the mechanical arm runs from the starting point to the instruction point.

[0011] Optionally, the determining of the inverse solution function relationship corresponding to the position relationship between the joint axes of the joints in the mechanical arm includes:

[0012] According to the position relationship between the joint axes of the joints, a vector equation relationship satisfied between direction vectors of the joint axes under the position relationship is determined.

[0013] According to the vector equation relationship and the reference function relationship, the inverse solution function relationship corresponding to the position relationship is determined.

[0014] Optionally, the algebraic equation relationship of the intermediate point includes a first algebraic equation relationship corresponding to a first intermediate point and a second algebraic equation relationship corresponding to a second intermediate point.

[0015] The first algebraic equation relationship is used to represent a relationship between the first intermediate point, the starting point, the second movement distance, and the second direction vector.

[0016] The second algebraic equation relationship is used to represent a relationship between the second intermediate point, the first intermediate point, the first movement distance, and the first direction vector.

[0017] Optionally, the constraint function relationship between the intermediate point and the instruction point includes a geometric constraint function and an algebraic constraint function.

[0018] The geometric constraint function is used to represent a geometric relationship between the second intermediate point, the command point and a direction vector of a rotation joint axis of the rotation joint.

[0019] The algebraic constraint function is used to represent an algebraic relationship between the second intermediate point, the command point and a reference point on the rotation joint axis.

[0020] Optionally, if the rotation joint axis of the rotation joint is not perpendicular to the second joint axis of the second movement joint, and the rotation joint axis is perpendicular to the first joint axis of the first movement joint, the inverse solution function relationship is a function related to the first movement distance in the reference function relationship.

[0021] The determining of the joint rotation angle of the rotation 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 comprises:

[0022] The first movement distance is determined according to the second movement distance and the function relationship between the first movement distance and the second movement distance in the reference function relationship.

[0023] The first movement distance is determined according to the second movement distance and the function relationship between the first movement distance and the second movement distance in the reference function relationship.

[0024] The value of the first intermediate point is determined according to the second movement distance and the first algebraic equation relationship.

[0025] The value of the second intermediate point is determined according to the value of the first intermediate point, the first movement distance and the second algebraic equation relationship.

[0026] The value of the joint rotation angle is determined according to the value of the second intermediate point, the command point and a preset motion function relationship between a space point and a joint axis.

[0027] Optionally, if the rotation joint axis of the rotation joint is not perpendicular to the first joint axis of the first movement joint, and the rotation joint axis is perpendicular to the second joint axis of the second movement joint, the inverse solution function relationship is a function related to the second movement distance in the reference function relationship.

[0028] The determining of the joint rotation angle of the rotation 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 comprises:

[0029] The first movement distance is determined according to the inverse solution function relationship.

[0030] determining the second movement distance according to the first movement distance and the function relationship between the first movement distance and the second movement distance in the reference function relationship;

[0031] determining the value of the first intermediate point according to the second movement distance and the first algebraic equation relationship;

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

[0033] 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 relationship between the spatial point and the joint axis.

[0034] Optionally, if the rotation joint axis of the rotation joint is not perpendicular to the first joint axis of the first movement joint and the rotation joint axis is not perpendicular to the second joint axis of the second movement joint, the inverse solution function relationship is a function related to the second movement distance in the reference function relationship.

[0035] determining the value of the joint rotation angle of the rotation 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, includes:

[0036] determining the second movement distance according to the inverse solution function relationship;

[0037] determining the first movement distance according to the second movement distance and the function relationship between the first movement distance and the second movement distance in the reference function relationship;

[0038] determining the value of the first intermediate point according to the second movement distance and the first algebraic equation relationship;

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

[0040] 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 relationship between the spatial point and the joint axis.

[0041] In a second aspect, the embodiments of the present application further provide a three-joint robot arm motion control device, which is applied to a processing device in a robot arm control system, the system including the processing device and a three-joint robot arm with a target structure, the three-joint robot arm including a rotation joint, a first movement joint and a second movement joint which are sequentially and mutually linked; the device including an acquisition module, a determination module and a control module.

[0042] the acquisition module is configured to acquire an instruction point to which an end of the robot arm is to be moved;

[0043] the determination module is configured to determine, according to a positional relationship between joint axes of the joints in the robot arm, an inverse solution function relationship corresponding to the positional relationship;

[0044] the determination module is configured to determine, according to the inverse solution function relationship, a joint rotation angle of the rotation joint, a first movement distance of the first movement joint, and a second movement distance of the second movement joint, respectively; the inverse solution function relationship is obtained by deforming a reference function relationship, and the reference function relationship is constructed according to an algebraic equation relationship of an intermediate point and a constraint function relationship between the intermediate point and the instruction point; the algebraic equation relationship of the intermediate point is constructed according to a starting point at which the end of the robot arm is currently located and joint-related parameters, the joint-related parameters including the first movement distance of the first movement joint, a first direction vector of the first joint axis of the first movement joint, the second movement distance of the second movement joint, and a second direction vector of the second joint axis of the second movement joint; the intermediate point includes a specified point passed through by the end of the robot arm in a process of moving from the starting point to the instruction point;

[0045] the control module is configured to control the rotation joint of the robot arm to rotate by the joint rotation angle, control the first movement joint to move by the first movement distance, and control the second movement joint to move by the second movement distance, so that the end of the robot arm moves from the starting point to the instruction point.

[0046] Optionally, the determination module is specifically configured to determine, according to the positional relationship between the joint axes, a vector equation relationship satisfied between direction vectors of the joint axes in the positional relationship;

[0047] the inverse solution function relationship corresponding to the positional relationship is determined according to the vector equation relationship and the reference function relationship.

[0048] Optionally, the algebraic equation relationship of the intermediate point includes a first algebraic equation relationship corresponding to a first intermediate point and a second algebraic equation relationship corresponding to a second intermediate point;

[0049] the first algebraic equation relationship is used to represent a relationship between the first intermediate point, the starting point, the second movement distance, and the second direction vector;

[0050] the second algebraic equation relationship is used to represent a relationship between the second intermediate point, the first intermediate point, the first movement distance, and the first direction vector.

[0051] Optionally, the constraint function relationship between the intermediate point and the instruction point comprises a geometric constraint function and an algebraic constraint function.

[0052] The geometric constraint function is used to represent a geometric relationship between the second intermediate point, the instruction point and a direction vector of the rotational joint axis of the rotational joint.

[0053] The algebraic constraint function is used to represent an algebraic relationship between the second intermediate point, the instruction point and a reference point on the rotational joint axis.

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

[0055] The determining module is specifically configured to determine the second moving distance according to the inverse solution function relationship.

[0056] The first moving distance is determined according to the second moving distance and the function relationship between the first moving distance and the second moving distance in the reference function relationship.

[0057] The value of the first intermediate point is determined according to the second moving distance and the first algebraic equation relationship.

[0058] The value of the second intermediate point is determined according to the value of the first intermediate point, the first moving distance and the second algebraic equation relationship.

[0059] The value of the joint rotation angle is determined according to the value of the second intermediate point, the instruction point and a preset motion function relationship between a spatial point and a joint axis.

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

[0061] The determining module is specifically configured to determine the first moving distance according to the inverse solution function relationship.

[0062] The second moving distance is determined according to the first moving distance and the function relationship between the first moving distance and the second moving distance in the reference function relationship.

[0063] determine the value of the first intermediate point according to the second movement distance and the first algebraic equation relationship;

[0064] determine the value of the second intermediate point according to the value of the first intermediate point, the first movement distance and the second algebraic equation relationship;

[0065] determine the value of the joint rotation angle according to the value of the second intermediate point, the instruction point and a preset motion function relationship between a spatial point and a joint axis.

[0066] Optionally, if the rotation joint axis of the rotation joint is not perpendicular to the first joint axis of the first movement joint and the rotation joint axis is not perpendicular to the second joint axis of the second movement joint, the inverse solution function relationship is a function related to the second movement distance in the reference function relationship.

[0067] The determining module is specifically configured to determine the second movement distance according to the inverse solution function relationship.

[0068] determine the first movement distance according to the second movement distance and a function relationship between the first movement distance and the second movement distance in the reference function relationship.

[0069] determine the value of the first intermediate point according to the second movement distance and the first algebraic equation relationship;

[0070] determine the value of the second intermediate point according to the value of the first intermediate point, the first movement distance and the second algebraic equation relationship;

[0071] determine the value of the joint rotation angle according to the value of the second intermediate point, the instruction point and a preset motion function relationship between a spatial point and a joint axis.

[0072] In a third aspect, an electronic device is provided, including a processor, a storage medium and a bus. 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. The processor executes the machine readable instructions to perform the steps of the three-joint robot motion control method provided in the first aspect.

[0073] In a fourth aspect, a computer readable storage medium is provided. The storage medium stores a computer program. When the computer program is run by a processor, the steps of the three-joint robot motion control method provided in the first aspect are executed.

[0074] The present application has the following beneficial effects:

[0075] The application provides a three-joint mechanical arm motion control method and device, electronic equipment and a storage medium. The method performs mathematical modeling through a starting point where the mechanical arm is currently located, an instruction point to be operated to, an intermediate point and motion parameters of each joint, constructs a mathematical model of the three-joint mechanical arm in the RTT configuration, and constructs a reference function relationship corresponding to the three-joint mechanical arm in the RTT configuration. Based on the reference function relationship, the inverse solution function relationship of the joint axes 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. Since the reference function relationship is uniquely determined according to the joint correlation parameters in the RTT configuration, that is, the joint parameters of the rotary joint, the first moving joint and the second moving joint are determined, so that the inverse solution function relationship of the joint axes under different position relationships is also uniquely determined. The inverse solution under each position relationship is analyzed through the inverse solution function relationship under each position relationship, and the joint rotation angle of the rotary joint, the moving distance of the first moving joint and the moving distance of the second moving joint are respectively analyzed, so that the motion control of the three-joint mechanical arm in the RTT configuration is performed according to the analysis result. The inverse solution of the three-joint mechanical arm in the RTT configuration under various conditions is realized, the inverse solution process of the three-joint mechanical arm is more comprehensive, the inverse solution efficiency is higher, and the accuracy of the inverse solution result is also higher. BRIEF DESCRIPTION OF DRAWINGS

[0076] In order to more clearly illustrate the technical solutions of the embodiments of the application, the following will briefly introduce the drawings needed to be used in the embodiments. It should be understood that the following drawings only show some embodiments of the application, and therefore should not be regarded as a limitation on the scope. For those skilled in the art, other related drawings can also be obtained without creative labor.

[0077] Figure 1 A mathematical modeling schematic diagram of a three-joint mechanical arm provided by the embodiments of the application;

[0078] Figure 2 A flowchart of a first three-joint mechanical arm motion control method provided by the embodiments of the application;

[0079] Figure 3 A flowchart of a second three-joint mechanical arm motion control method provided by the embodiments of the application;

[0080] Figure 4 A flowchart of a third three-joint mechanical arm motion control method provided by the embodiments of the application;

[0081] Figure 5 A flowchart of a fourth three-joint mechanical arm motion control method provided by the embodiments of the application;

[0082] Figure 6 A flowchart of a fifth three-joint mechanical arm motion control method provided for an embodiment of the present application is shown in FIG. 12.

[0083] Figure 7 An infinite solution diagram provided for an embodiment of the present application is shown in FIG. 13.

[0084] Figure 8 A simulation result diagram provided for an embodiment of the present application is shown in FIG. 14.

[0085] Figure 9 A schematic diagram of a three-joint mechanical arm motion control device provided for an embodiment of the present application is shown in FIG. 15.

[0086] Figure 10 A structural diagram of an electronic device provided for an embodiment of the present application is shown in FIG. 16. DETAILED DESCRIPTION

[0087] To make the objectives, technical solutions and advantages of the embodiments of the present application clearer, the technical solutions in the embodiments of the present application will be described below in conjunction with the drawings in the embodiments of the present application. It should be understood that the drawings in the present application only serve the purpose of description and illustration, and are not used to limit the scope of protection of the present application. In addition, it should be understood that the schematic drawings are not drawn to scale. The flowcharts show the operations implemented according to some embodiments of the present application. It should be understood that the operations of the flowcharts can be implemented in no particular order, and the steps without logical context relationship can be reversed in order or implemented simultaneously. In addition, one or more other operations can be added to the flowcharts or one or more operations can be removed from the flowcharts under the guidance of the content of the present application.

[0088] In addition, the described embodiments are only some of the embodiments of the present application, not all the embodiments. The components of the embodiments of the present application described and shown in the drawings herein can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present application provided in the drawings is not intended to limit the scope of the claimed present application, but only represents selected embodiments of the present application. Based on the embodiments of the present application, all other embodiments obtained by those skilled in the art without creative labor are within the scope of protection of the present application.

[0089] A mechanical arm belongs to a special robot. Referring to the motion control of a robot, the inverse kinematics of a mechanical arm is the most basic processing step in the control of a mechanical arm. Through the inverse kinematics, the motion parameters of each joint in the mechanical arm can be obtained, so that the end of the mechanical arm can be moved from the current position to the specified position by controlling each joint to move according to the corresponding motion parameters.

[0090] Generally, there are many types of mechanical arms, such as two-joint, three-joint and multi-joint, and there are many methods for kinematic inverse solution, but the existing inverse solution methods are not only low in solving efficiency, but also cannot guarantee the completeness, convergence and robustness of the inverse solution. Although the method based on the PoE (Product of Exponential) model overcomes the above problems, it is only suitable for solving two-joint mechanical arms.

[0091] Based on this, the present scheme is to divide the inverse solution problem of a three-joint mechanical arm into a combination of two-joint sub-problems based on the PoE model, which can not only ensure the solving efficiency, but also perfectly solve the inverse solution analysis of the three-joint mechanical arm.

[0092] The present scheme will be described below through specific embodiments:

[0093] Figure 1 A mathematical modeling schematic diagram of a three-joint mechanical arm is provided for the embodiments of the present application. The present scheme can be applied to a three-joint mechanical arm with a target structure, which can include a rotary joint, a first moving joint and a second moving joint linked to each other in sequence. In one case, it can be a rotary joint-moving joint-moving joint linked in sequence to form a three-joint mechanical arm of RTT configuration; in another case, it can also be a moving joint-moving joint-rotary joint linked in sequence to form a three-joint mechanical arm of TTR configuration. It can be considered that the RTT configuration and the TTR configuration are equivalent to each other, so that the present scheme can be applied not only to the inverse solution of the three-joint mechanical arm of the RTT configuration, but also to the inverse solution of the three-joint mechanical arm of the TTR configuration.

[0094] As shown in Figure 1 , taking the mathematical modeling of a three-joint mechanical arm of RTT configuration as an example, circle C1 represents a rotary joint, axis 1 represents a first moving joint, axis 2 represents a second moving joint, θ1 represents the joint rotation angle of the rotary joint, θ2 represents the first moving distance of the first moving joint, and θ3 represents the second moving distance of the second moving joint. ξ1 represents the rotary joint axis of the rotary joint, ξ2 represents the first joint axis of the first moving joint, and ξ3 represents the joint axis of the second moving joint. p1 represents a reference point on the rotary joint axis of the rotary joint. p represents the starting point where the end point of the mechanical arm is currently located, q represents the command point where the end point of the mechanical arm is to be operated to, c represents a first intermediate point, and d represents a second intermediate point; the inverse solution of the three-joint mechanical arm can be divided into two-joint inverse solution sub-problems, and the inverse solution problem of the end point of the mechanical arm from the starting point to the command point can be divided into sub-problems 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 command point. Thus, the inverse solution of the three-joint can be realized by combining the inverse solution sub-problems between two joints.

[0095] By inverse solution analysis, the values of θ1, θ2 and θ3 can be respectively solved, so that the rotation joint can be controlled to rotate by an angle of θ1, the first moving joint can be controlled to move by a distance of θ2, and the second moving joint can be controlled to move by a distance of θ3, so as to realize the motion control of the three-joint robot arm from the starting point to the command point.

[0096] Figure 2 A flowchart of a first three-joint robot arm motion control method provided by the embodiment of the application is shown. The method can be applied to a processing device in a robot arm control system. The robot arm control system includes the processing device and a three-joint robot arm having a target structure. The three-joint robot arm can be a robot arm of the RTT configuration or a robot arm of the TTR configuration. Figure 2 As shown in the figure, the method can include the following steps.

[0097] S201, obtaining a command point to which an end of the robot arm is to be moved.

[0098] The command point is a target point to which the end of the robot arm is to be moved. Assuming that the end of the robot arm is currently at position 1 and is to be controlled to move to position 2 at the next moment, the command point is the point at which position 2 is located. The input of the command point can be performed through a central control terminal on the robot arm, or can be performed through an independent terminal device.

[0099] S202, determining an inverse solution function relationship corresponding to a position relationship between joint axes of each joint in the robot arm.

[0100] The inverse solution function relationship corresponding to the position relationship can be determined according to the position relationship between the joint axis of the rotation joint and the joint axes of the first moving joint and the second moving joint.

[0101] Due to the different positions and structures of each joint in the robot arm, the joint axes will also have different position relationships. The position relationship herein can include parallel, perpendicular, non-parallel or non-perpendicular.

[0102] When the position relationship between the joint axes is different, the corresponding inverse solution function relationship is also different, that is, the inverse solution function relationship under the position relationship can be uniquely determined.

[0103] S203, respectively determining a joint rotation angle of the rotation joint, a first moving distance of the first moving joint and a second moving distance of the second moving joint according to the inverse solution function relationship.

[0104] The inverse function relationship is obtained by transforming the reference function relationship, and the reference function relationship is constructed according to the algebraic equation relationship of the intermediate point and the constraint function relationship between the intermediate point and the command point.

[0105] The inverse function relationship is obtained by transforming the reference function relationship, and the reference function relationship is constructed according to the algebraic equation relationship of the intermediate point and the constraint function relationship between the intermediate point and the command point.

[0106] The inverse function relationship is obtained by transforming the reference function relationship, and the reference function relationship is constructed according to the algebraic equation relationship of the intermediate point and the constraint function relationship between the intermediate point and the command point.

[0107] As shown in Figure 1 The algebraic equation relationship of the first intermediate point c and the second intermediate point d is established.

[0108] The algebraic equation relationship of the intermediate point is constructed according to the current starting point of the end of the mechanical arm and the joint associated parameters. In the RTT configuration of the three-joint mechanical arm, the joint associated parameters can include the first movement distance of the first movement joint, the first direction vector of the first joint axis of the first movement joint, the second movement distance of the second movement joint, and the second direction vector of the second joint axis of the second movement joint.

[0109] Different configurations of three-joint mechanical arms have different joints, and the connection relationship and position relationship between the joints are different, so that the joint associated parameters are also different. That is, when the algebraic equation relationship of the intermediate point is obtained, the joint associated parameters are unique, so that the determined algebraic equation relationship of the intermediate point is also unique.

[0110] Similarly, the constraint function relationship between the intermediate point and the command point is also unique. Therefore, the reference function relationship of the RTT configuration of the three-joint mechanical arm can be uniquely determined.

[0111] Then, by deforming the reference function relationship, the inverse solution function relationship of each joint axis under the corresponding position relationship can be uniquely determined, and the motion parameters of each joint under the position relationship can be accurately analyzed based on the inverse solution function relationship.

[0112] S204, respectively control the rotation joint of the mechanical arm to rotate the joint rotation angle, control the first movement joint to move the first movement distance, and control the second movement joint to move the second movement distance, so that the end of the mechanical arm runs from the starting point to the command point.

[0113] Optionally, the rotation joint, the first movement joint and the second movement joint are controlled to move according to the determined joint rotation angle, the first movement distance and the second movement distance, so that the end point of the mechanical arm can be moved from the starting point to the command point, and one motion control is completed.

[0114] Next, the command point is taken as a new starting point again, a new command point is specified, and the above steps are repeated to realize one motion control after another.

[0115] In summary, the three-joint mechanical arm motion control method provided in the embodiment is to perform mathematical modeling by the starting point currently located by the mechanical arm, the command point to be run to, the intermediate point and the motion parameters of each joint, to construct the mathematical model of the three-joint mechanical arm in RTT configuration, and to construct the reference function relationship corresponding to the three-joint mechanical arm in RTT configuration. Based on the reference function relationship, the inverse solution function relationship of the joint axis of the mechanical arm under different position relationships is obtained, and the motion parameters of each joint are analyzed according to the inverse solution function relationship. Since the reference function relationship is uniquely determined according to the joint correlation parameters under the RTT configuration, that is, it is determined according to the joint parameters of the rotation joint, the first movement joint and the second movement joint, the inverse solution function relationship of each joint axis under different position relationships is also uniquely determined. The inverse solution analysis under each position relationship is performed through the inverse solution function relationship under each position relationship, and the joint rotation angle of the rotation joint, the movement distance of the first movement joint and the movement distance of the second movement joint are respectively analyzed. Therefore, the motion control of the three-joint mechanical arm in RTT configuration is performed according to the analysis result. The inverse solution of the three-joint mechanical arm in RTT configuration under various conditions is realized, the inverse solution process of the three-joint mechanical arm is more comprehensive, the inverse solution efficiency is higher, and the accuracy of the inverse solution result is also higher.

[0116] Figure 3 The flowchart of the second three-joint mechanical arm motion control method provided in the embodiment of the application; optionally, in step S202, the inverse solution function relationship corresponding to the position relationship is determined according to the position relationship between the joint axes of each joint in the mechanical arm, which can include:

[0117] S301, determine a vector equation relationship between direction vectors of the joint axes under the position relationship according to the position relationship between the joint axes of the joints.

[0118] The position relationship between the joint axes can refer to a parallel relationship or a perpendicular relationship between the joint axes, and when the position relationship between two joint axes is determined, the vector equation relationship between the direction vectors of the two joint axes is also determined.

[0119] S302, determine an inverse solution function relationship corresponding to the position relationship according to the vector equation relationship and a reference function relationship.

[0120] And based on the determined vector equation relationship, the reference function relationship can be deformed to determine the inverse solution function relationship corresponding to the position relationship.

[0121] Here, the deformation of the reference function relationship can be achieved by replacing the coefficients of the parameters in the reference function relationship, and the specific implementation can be understood according to the following embodiments.

[0122] Optionally, the algebraic equation relationship of the intermediate point can include a first algebraic equation relationship corresponding to a first intermediate point and a second algebraic equation relationship corresponding to a second intermediate point.

[0123] The first algebraic equation relationship is used to represent the relationship between the first intermediate point, the starting point, the second movement distance, and the second direction vector.

[0124] As shown in Figure 1 When the starting point p moves θ3 to the first intermediate point c along the joint axis ξ3, the first intermediate point c moves θ2 to the second intermediate point d along the joint axis ξ2, the second intermediate point d rotates θ1 to the command point q about the joint axis ξ1, and the reference point p1 is an arbitrary point on the joint axis ξ1, it is known that c=p+θ3v3 holds, and thus the first algebraic equation relationship of the first intermediate point is c=p+θ3v3, wherein p represents the starting point, θ3 is the second movement distance, and v3 is the second direction vector (the direction vector of the joint axis of the second movement joint).

[0125] The second algebraic equation relationship is used to represent the relationship between the second intermediate point, the first intermediate point, the first movement distance, and the first direction vector.

[0126] Similarly, when the starting point p moves along the joint axis ξ3 by θ3 to a first intermediate point c, the first intermediate point c moves along the joint axis ξ2 by θ2 to a second intermediate point d, the second intermediate point d rotates by θ1 around the joint axis ξ1 to the command point q, and the reference point p1 is an arbitrary point on the joint axis ξ1, it is known that d = c + θ2v2 holds, that is, the second algebraic equation relationship of the second intermediate point d = c + θ2v2 is obtained. Wherein, θ2 is the first movement distance, and v2 is the first direction vector (the direction vector of the joint axis of the first movement joint).

[0127] Optionally, the constraint function relationship between the intermediate point and the command point can include: a geometric constraint function and an algebraic constraint function.

[0128] The geometric constraint function is used to represent the geometric relationship between the second intermediate point and the command point and the direction vector of the rotation joint axis of the rotation joint.

[0129] As Figure 1 It is known that the second intermediate point d and the command point q are both located on the circle C1 perpendicular to the joint axis ξ1, and the geometric constraint function as shown in formula (1) can be obtained according to the geometric constraint:

[0130] ω1 T (d-q) = 0, (1)

[0131] Wherein, ω1 represents the rotation joint axis of the rotation joint.

[0132] The algebraic constraint function is used to represent the algebraic relationship between the second intermediate point and the command point and the reference point on the rotation joint axis.

[0133] Similarly, the algebraic constraint function as shown in formula (2) can be obtained according to the algebraic constraint:

[0134] ||q-p1|| = ||d-p1||. (2)

[0135] Then, the first algebraic equation relationship c = p + θ3v3 and the second algebraic equation relationship d = c + θ2v2 are substituted into formula (1) and formula (2) respectively, and the square of both sides of formula (2) is taken, and then is replaced with the corresponding Rodrigues formula, formula (1) and formula (2) are respectively converted into formula (3) and formula (4)

[0136] x1θ2+y1θ3+z1 = 0, (3)

[0137] x2θ2 2 +y2θ3 2 +a1θ2θ3+a2θ2+a3θ3+z2 = 0, (4)

[0138] where the parameters are x1=ω1 T v2,y1=ω1 T v3,z1=ω1 T (p-q),x2=y2=1,a1=2v3 T v2,a2=2(p-p1) T v2,a3=2(p-p1) T v3,z2=||p-p1|| 2 ||q-p1|| 2 .

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

[0140] Next, the cases are discussed to explain the inverse solution of the joint motion parameters of the joint axis under each position relationship.

[0141] First of all, it needs to be clear that for two rotary joints, if ξ1∥ξ2, ω1 is established; and for any two joints, if ξ1⊥ξ2, ω1 T ω2=0 is established.

[0142] Figure 4 A flowchart of a third three-joint robot motion control method provided by the embodiment of the application; case 1: ξ1 is not perpendicular to ξ3 but ξ1⊥ξ2, that is, the rotary joint axis ξ1 of the rotary joint is not perpendicular to the second joint axis ξ3 of the second moving joint, and the rotary joint axis ξ1 is perpendicular to the first joint axis ξ2 of the first moving joint; and the inverse solution function relationship is a function related to the second moving distance in the reference function relationship.

[0143] When the joint axis ξ1 is not perpendicular to ξ3 but ξ1⊥ξ2, it indicates that the direction vectors between the joint axes satisfy the vector equation relationship: ω1 T v3≠0 and ω1 T v2=0, at this time, the formula (3) in the reference function relationship is converted into θ3=-z1 / y1.

[0144] In step S203, according to the inverse solution function relationship, 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 are determined respectively, which can include:

[0145] S401, determining the second moving distance according to the inverse solution function relationship.

[0146] At this time, the inverse solution function relationship is θ3=-z1 / y1, and then the value of the second moving distance θ3 can be determined through the inverse solution function relationship.

[0147] S402, determining the first movement distance according to the second movement distance and a function relationship between the first movement distance and the second movement distance in the reference function relationship.

[0148] Optionally, the value of the second movement distance θ3 is substituted into formula (4) in the reference function relationship, formula (4) is a quadratic equation about the first movement distance θ2, and then the value of the first movement distance θ2 can be solved.

[0149] S403, determining the value of the first intermediate point according to the second movement distance and the first algebraic equation relationship.

[0150] Then, the value of the second movement distance θ3 is substituted into the first algebraic equation relationship c=p+θ3v3, and the value of the first intermediate point c can be determined.

[0151] S404, determining the value of the second intermediate point according to the value of the first intermediate point, the first movement distance and the second algebraic equation relationship.

[0152] And the value of the first intermediate point c and the value of the first movement distance θ2 are substituted into the second algebraic equation relationship d=c+θ2v2, and the value of the second intermediate point d can be determined.

[0153] S405, determining the value of the joint rotation angle according to the value of the second intermediate point, the command point and the preset motion function relationship between the spatial point and the joint axis.

[0154] It should be noted that the three-joint kinematics model based on the screw theory has clear physical meaning, that is, when the spatial point p rotates by an angle θ around the joint axis ξ to the spatial point q, that is, The homogeneous coordinate conversion is converted into a spatial coordinate representation, and the preset motion function relationship between the spatial point and the joint axis can be represented by the following formula (5):

[0155]

[0156] Wherein, the vector r represents any point r on the joint axis ξ.

[0157] In the 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 rotating joint, θ represents the joint rotation angle θ1, and the values of the second spatial point d and the command point q are substituted into formula (5) to calculate the value of the joint rotation angle θ1.

[0158] Figure 5A flowchart of a fourth three-joint robot arm motion control method provided by the embodiments of the present application is shown in FIG. 4. Case 2: ξ1 is not perpendicular to ξ2 but ξ1 is perpendicular to ξ3, that is, the rotation joint axis ξ1 of the rotation joint is not perpendicular to the first joint axis ξ2 of the first movement joint, and the rotation joint axis ξ1 is perpendicular to the second joint axis ξ3 of the second movement joint. Then, the inverse solution function relationship is a function related to the first movement distance in the reference function relationship.

[0159] When the joint axis ξ1 is not perpendicular to ξ2 but ξ1 is perpendicular to ξ3, it indicates that the direction vectors of the joint axes satisfy the vector equation relationship: ω1 T v2≠0 and ω1 T v3=0. At this time, formula (3) in the reference function relationship is converted to θ2=-z1 / x1.

[0160] In step S203, the joint rotation angle of the rotation joint, the first movement distance of the first movement joint, and the second movement distance of the second movement joint are determined according to the inverse solution function relationship, which can include:

[0161] S501, determining the first movement distance according to the inverse solution function relationship.

[0162] At this time, the inverse solution function relationship is θ2=-z1 / x1. Then, the value of the first movement distance θ2 can be determined by the inverse solution function relationship.

[0163] S502, determining the second movement distance according to the first movement distance and the function relationship between the first movement distance and the second movement distance in the reference function relationship.

[0164] Alternatively, the value of the first movement distance θ2 is substituted into formula (4) in the reference function relationship. Then, formula (4) is a quadratic equation about the second movement distance θ3. Thus, the value of the second movement distance θ3 can be solved.

[0165] S503, determining the value of the first intermediate point according to the second movement distance and the first algebraic equation relationship.

[0166] Then, the value of the second movement distance θ3 is substituted into the first algebraic equation relationship c=p+θ3v3. Thus, the value of the first intermediate point c can be determined.

[0167] S504, determining the value of the second intermediate point according to the value of the first intermediate point, the first movement distance, and the second algebraic equation relationship.

[0168] And the value of the first intermediate point c and the value of the first movement distance θ2 are substituted into the second algebraic equation relationship d=c+θ2v2. Thus, the value of the second intermediate point d can be determined.

[0169] S505, determining the value of the joint rotation angle according to the value of the second intermediate point, the command point, and the preset motion function relationship between the spatial point and the joint axis.

[0170] Thus, the value of the joint rotation angle θ1 can be calculated by substituting the values of the second spatial point d and the command point q into formula (5).

[0171] Figure 6 A flowchart of a fifth three-joint robot arm motion control method provided by the embodiment of the application is shown in the figure. Case 3: ξ1 is not perpendicular to ξ2 and ξ1 is not perpendicular to ξ3, that is, the rotation joint axis ξ1 of the rotation joint is not perpendicular to the first joint axis ξ2 of the first moving joint, and the rotation joint axis ξ1 is not perpendicular to the second joint axis ξ3 of the second moving joint; then the inverse solution function relationship is the function related to the second moving distance in the reference function relationship.

[0172] When the joint axis ξ1 is not perpendicular to ξ2 and ξ1 is not perpendicular to ξ3, it indicates that the direction vectors between the joint axes satisfy the vector equation relationship: ω1 T v3≠0 and ω1 T v2≠0, at this time, the first moving distance θ2 can be represented by the second moving distance θ3 by combining formula (3) and formula (4) in the reference function relationship: θ2=-(y1θ3+z1) / x1.

[0173] Alternatively, substituting θ2=-(y1θ3+z1) / x1 into formula (4) can convert formula (4) into the following formula (6):

[0174] m1θ3 2 +m2θ3+m3=0, (6)

[0175] In the formula, the coefficients of each term are m1=x2y1 2 +x1 2 y2-a1x1y1,m2=2x2y1z1-a1x1z1-a2x1y1+a3x1 2 , and m3=x2z1 2 -a2x1z1+x1 2 z2.

[0176] In step S203, determining the joint rotation angle of the rotation 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 relationship can include:

[0177] S601, determining the second moving distance according to the inverse solution function relationship.

[0178] At this time, the inverse solution function relationship is represented as m1θ3 2+ m2θ3 + m3 = 0, if m1≠0, the second movement distance θ3 is solved as If m1 = 0, the formula (5) is converted to m2θ3 + m3 = 0, so that the second movement distance θ3 can be solved.

[0179] S602, according to the second movement distance, and the function relationship between the first movement distance and the second movement distance in the reference function relationship, the first movement distance is determined.

[0180] Optionally, the value of the second movement distance θ3 is substituted into the formula (3) in the reference function relationship, the formula (3) is a first-order equation about the first movement distance θ2, and then the value of the first movement distance θ2 can be solved.

[0181] S603, according to the second movement distance and the first algebraic equation relationship, the value of the first intermediate point is determined.

[0182] Then, the value of the second movement distance θ3 is substituted into the first algebraic equation relationship c = p + θ3v3, and the value of the first intermediate point c can be determined.

[0183] S604, according to the value of the first intermediate point, the first movement distance and the second algebraic equation relationship, the value of the second intermediate point is determined.

[0184] And the value of the first intermediate point c and the value of the first movement distance θ2 are substituted into the second algebraic equation relationship d = c + θ2v2, and the value of the second intermediate point d can be determined.

[0185] S605, according to the value of the second intermediate point, the command point and the preset motion function relationship between the spatial point and the joint axis, the value of the joint rotation angle is determined.

[0186] Therefore, the values of the second spatial point d and the command point q are substituted into the formula (5), and the value of the joint rotation angle θ1 can be calculated.

[0187] Case 4: ξ1⊥ξ3 and ξ1⊥ξ2, that is, the rotation joint axis ξ1 of the rotation joint is perpendicular to the first joint axis ξ2 of the first movement joint, and the rotation joint axis ξ1 is perpendicular to the second joint axis ξ3 of the second movement joint.

[0188] When the joint axis ξ1⊥ξ3 and ξ1⊥ξ2, ω1 T v2 = 0 and ω1 T v3 = 0, that is, the points c, d, p and q are in the same plane, the formula (3) in the reference function relationship is always true, and there are two unknown variables of the joint angle θ1 and θ3 in the formula (4), and there are infinite solutions.

[0189] Figure 7This is a schematic diagram illustrating an infinite solution provided in an embodiment of this application. For example... Figure 7 As shown, all points on circle C1 are inverse solutions.

[0190] Thus, the inverse kinematics of joint motion parameters under various possible joint axis positional relationships have been explained in detail. Based on the joint rotation angle θ1 of the rotary joint, the first movement distance θ2 of the first locating joint, and the second movement distance θ3 of the second locating joint obtained from the inverse kinematics, the motion control of the robotic arm can be realized.

[0191] The following simulation verification of the solution is conducted through specific embodiments.

[0192] First, let's explain the basic mathematical knowledge involved:

[0193] The physical meaning of the three-joint kinematic model based on spinor theory is clear: it represents the rotation or movement of a spatial point about the joint axis, as shown below:

[0194]

[0195] Under the same joint axis, by reversing the sequence of movements and changing the positions of the starting and ending points, formula (4-1) can be transformed into:

[0196]

[0197] Spatial point p rotates by an angle θ around the joint axis ξ to spatial point q, i.e. Converting homogeneous coordinates to spatial coordinates, we obtain formula (4-3):

[0198]

[0199] It is worth noting that formula (4-3) is the same as formula (5) mentioned above.

[0200] Where vector r represents any point r on the joint axis ξ. Rodriguez formula Substituting into formula (4-3), we get:

[0201] x sinθ + y cosθ + z = 0 (4-4)

[0202] in Because x T y=0 and x T x = y T The properties of y, the rotation angle θ can be obtained through and Please provide a solution.

[0203] The distance invariance principle is usually applied to solve the angle of the revolute joint, and is accompanied by squaring both sides of the equation. For example, the distance between vector s and vector t is δ, i.e. ||s-t|| = δ, then squaring both sides of the equation gives ||s||2+||t||2-2st= δ2. 2 +||t|| 2 +2t T s=δ 2 .

[0204] Simulation verification of the inverse solution of the RTT sub-problem

[0205] Figure 8 A simulation result diagram provided by an embodiment of the present application. The possible forms of the RTT sub-problem and all the analytical inverse solution methods are analyzed in detail in the foregoing, and case 3: the general case that the joint axis ξ1 is not perpendicular to ξ3 nor perpendicular to ξ2 is selected for simulation to verify the correctness of the analytical solution. Given a set of joint wrenches and reference point positions satisfying the joint axis relationship, the direction vectors of the three joint axes are ω1 = [1 0 1] T , v2 = [0 1 1] T , and v3 = [2 1 0] T , the reference point position vector of the revolute joint is p1 = [0 0 0] T , and the position vector of the starting point p is p = [5 -10 -5] T . The motion ranges of the three joints are and According to the given motion ranges, 51 data points are uniformly sampled therefrom, the position of point q after the motion of point p around each joint is calculated according to formula (4-1), and the inverse solution of the three joints is performed according to the position of point q. The angle values of the inverse solution are compared with the given sampling data points. It should be noted that the direction vectors of each motion joint need to be normalized before calculation, i.e. ω = ω / ||ω||. After the inverse solution values of the joints are calculated, the inverse solution needs to be determined, such as discarded if it does not meet the range limit. As shown in Figure 8 , where the symbol "o" represents the sampling points of each joint, the symbol "+" represents the inverse solution values of each joint, curve 1 represents the angle θ1 of the revolute joint, curve 2 represents the first moving distance θ2, and curve 3 represents the second moving distance θ3. It is easy to know from Figure 8 that the sampling points and the inverse solution values are consistent, and there is no error in the theoretical calculation, verifying the correctness of the analytical solution of the RTT sub-problem.

[0206] In summary, the three-joint robot motion control method provided in the embodiment is to perform mathematical modeling through the starting point where the robot currently locates, the instruction point to be operated to, the intermediate point, and the motion parameters of each joint, to construct the mathematical model of the three-joint robot in the RTT configuration, and to construct the reference function relationship corresponding to the three-joint robot in the RTT configuration. Based on the reference function relationship, the inverse solution function relationship of the joint axes of the robot in different position relationships is obtained, so that the motion parameters of each joint are obtained according to the inverse solution function relationship. Since the reference function relationship is uniquely determined according to the joint correlation parameters in the RTT configuration, that is, the joint parameters of the rotary joint, the first moving joint and the second moving joint are determined, so that the inverse solution function relationship of the joint axes in different position relationships is also uniquely determined. The inverse solution in each position relationship is analyzed through the inverse solution function relationship in each position relationship, and the joint rotation angle of the rotary joint, the moving distance of the first moving joint and the moving distance of the second moving joint are respectively analyzed, so that the motion control of the three-joint robot in the RTT configuration is performed according to the analysis result. The inverse solution of the three-joint robot in the RTT configuration in multiple cases is realized, the inverse solution process of the three-joint robot is more comprehensive, the inverse solution efficiency is higher, and the accuracy of the inverse solution result is also higher.

[0207] The following describes the device, equipment, storage medium and the like for performing the three-joint robot motion control method provided in the present application. The specific implementation process and technical effects are described above, and will not be described here.

[0208] Figure 9 A schematic diagram of a three-joint robot motion control device provided in an embodiment of the present application is shown in the figure. The functions implemented by the three-joint robot motion control device correspond to the steps of the method described above. The device can be understood as the processing device described above. As shown in the figure, the device can include an acquisition module 910, a determination module 920, and a control module 930. Figure 9

[0209] The acquisition module 910 is configured to acquire an instruction point to be operated to by a terminal of a robot.

[0210] The determination module 920 is configured to determine an inverse solution function relationship corresponding to a position relationship according to the position relationship between the joint axes of each joint in the robot.

[0211] ​The determining module 920 is configured to determine the joint rotation angle of the rotation joint, the first movement distance of the first movement joint, and the second movement distance of the second movement joint respectively according to an inverse solution function relationship, wherein the inverse solution function relationship is obtained by transforming a reference function relationship, and the reference function relationship is constructed according to an algebraic equation relationship of an intermediate point and a constraint function relationship between the intermediate point and the command point; the algebraic equation relationship of the intermediate point is constructed according to a starting point currently located by the end of the robot arm and joint correlation parameters, and the joint correlation parameters include the first movement distance of the first movement joint, a first direction vector of the first joint axis of the first movement joint, the second movement distance of the second movement joint, and a second direction vector of the second joint axis of the second movement joint; the intermediate point includes a specified point passed through by the end of the robot arm in a process of running from the starting point to the command point.

[0212] The control module 930 is configured to control the rotation joint of the robot arm to rotate by the joint rotation angle, control the first movement joint to move by the first movement distance, and control the second movement joint to move by the second movement distance, so that the end of the robot arm runs from the starting point to the command point.

[0213] Optionally, the determining module 920 is specifically configured to determine a vector equation relationship satisfied between direction vectors of joint axes of each joint according to a position relationship between the joint axes.

[0214] The inverse solution function relationship corresponding to the position relationship is determined according to the vector equation relationship and the reference function relationship.

[0215] Optionally, the algebraic equation relationship of the intermediate point includes a first algebraic equation relationship corresponding to a first intermediate point and a second algebraic equation relationship corresponding to a second intermediate point.

[0216] The first algebraic equation relationship is used to represent a relationship between the first intermediate point, the starting point, the second movement distance, and the second direction vector.

[0217] The second algebraic equation relationship is used to represent a relationship between the second intermediate point, the first intermediate point, the first movement distance, and the first direction vector.

[0218] Optionally, the constraint function relationship between the intermediate point and the command point includes a geometric constraint function and an algebraic constraint function.

[0219] The geometric constraint function is used to represent a geometric relationship between the second intermediate point, the command point, and a direction vector of a rotation joint axis of the rotation joint.

[0220] The algebraic constraint function is used to represent an algebraic relationship between the second intermediate point, the command point, and a reference point on the rotation joint axis.

[0221] Optionally, if the rotation joint axis of the rotation joint is not perpendicular to the second joint axis of the second movement joint, and the rotation joint axis is perpendicular to the first joint axis of the first movement joint, the inverse function relationship is a function related to the second movement distance in the reference function relationship.

[0222] The determining module 920 is specifically configured to determine the second movement distance according to the inverse function relationship.

[0223] The first movement distance is determined according to the second movement distance and the function relationship between the first movement distance and the second movement distance in the reference function relationship.

[0224] The value of the first intermediate point is determined according to the second movement distance and the first algebraic equation relationship.

[0225] The value of the second intermediate point is determined according to the value of the first intermediate point, the first movement distance and the second algebraic equation relationship.

[0226] The value of the joint rotation angle is determined according to the value of the second intermediate point, the command point and the preset motion function relationship between the spatial point and the joint axis.

[0227] Optionally, if the rotation joint axis of the rotation joint is not perpendicular to the first joint axis of the first movement joint, and the rotation joint axis is perpendicular to the second joint axis of the second movement joint, the inverse function relationship is a function related to the first movement distance in the reference function relationship.

[0228] The determining module 920 is specifically configured to determine the first movement distance according to the inverse function relationship.

[0229] The second movement distance is determined according to the first movement distance and the function relationship between the first movement distance and the second movement distance in the reference function relationship.

[0230] The value of the first intermediate point is determined according to the second movement distance and the first algebraic equation relationship.

[0231] The value of the second intermediate point is determined according to the value of the first intermediate point, the first movement distance and the second algebraic equation relationship.

[0232] The value of the joint rotation angle is determined according to the value of the second intermediate point, the command point and the preset motion function relationship between the spatial point and the joint axis.

[0233] Optionally, if the rotation joint axis of the rotation joint is not perpendicular to the first joint axis of the first movement joint, and the rotation joint axis is not perpendicular to the second joint axis of the second movement joint, the inverse function relationship is a function related to the second movement distance in the reference function relationship.

[0234] The determining module 920 is specifically configured to determine the second movement distance according to the inverse solution function relationship.

[0235] The first movement distance is determined according to the second movement distance and the function relationship between the first movement distance and the second movement distance in the reference function relationship.

[0236] The value of the first intermediate point is determined according to the second movement distance and the first algebraic equation relationship.

[0237] The value of the second intermediate point is determined according to the value of the first intermediate point, the first movement distance, and the second algebraic equation relationship.

[0238] The value of the joint rotation angle is determined according to the value of the second intermediate point, the instruction point, and the preset motion function relationship between the spatial point and the joint axis.

[0239] The apparatus is used for executing the method provided by the foregoing embodiments, and has similar implementation principles and technical effects, which will not be described herein again.

[0240] The modules can be one or more integrated circuits configured to implement the above method, for example, one or more application specific integrated circuits (ASICs), or one or more digital singnal processors (DSPs), or one or more field programmable gate arrays (FPGAs), etc. For another example, when a certain module above is implemented in the form of a processing element scheduling code, the processing element can be a general-purpose processor, such as a central processing unit (CPU) or other processor that can invoke program code. For another example, the modules can be integrated together in the form of a system on a chip (SOC).

[0241] The modules above can be connected or communicate with each other via wired connection or wireless connection. The wired connection can include metal cables, optical cables, hybrid cables, or any combination thereof. The wireless connection can include connection in the form of LAN, WAN, Bluetooth, ZigBee, or NFC, or any combination thereof. Two or more modules can be combined into a single module, and any module can be divided into two or more units. It can be clearly understood by those skilled in the art that, for the convenience and brevity of description, the specific working process of the system and apparatus described above can refer to the corresponding process in the method embodiments, which will not be described herein again.

[0242] Figure 10A structural schematic diagram of an electronic device provided by an embodiment of the present application is shown in FIG. 7. As shown in FIG. 7, the device can include a processor 801 and a storage medium 802. Figure 10

[0243] The storage medium 802 is configured to store a program, and the processor 801 invokes the program stored in the storage medium 802 to execute the method embodiments described above. The specific implementation manners and technical effects are similar, and thus will not be described herein again.

[0244] The storage medium 802 stores program codes, and when the program codes are executed by the processor 801, the processor 801 performs various steps of the three-joint robot motion control method according to various exemplary embodiments of the present application described in the “Exemplary Method” section of the present specification.

[0245] The processor 801 can be a general-purpose processor, such as a central processing unit (CPU), a digital signal processor (DSP), an application specific integrated circuit (ASIC), a field programmable gate array (FPGA) or other programmable logic device, a discrete gate or transistor logic device, a discrete hardware component, and can implement or execute each method, step and logic block disclosed in the embodiments of the present application. The general-purpose processor can be a microprocessor or any conventional processor, etc. The steps of the method disclosed in conjunction with the embodiments of the present application can be directly embodied as a hardware processor for execution, or be executed by a combination of hardware and software modules in the processor.

[0246] ​The storage medium 802, as a non-volatile computer readable storage medium, can be used to store non-volatile software programs, non-volatile computer executable programs and modules. The storage medium can include at least one type of storage medium, for example, can include flash memory, hard disk, multimedia card, card type storage medium, random access memory (RAM), static random access memory (SRAM), programmable read-only memory (PROM), read-only memory (ROM), electrically erasable programmable read-only memory (EEPROM), magnetic storage medium, magnetic disk, optical disk, etc. The storage medium is any other medium capable of carrying or storing desired program codes in the form of instructions or data structures and capable of being accessed by a computer, but is not limited thereto. The storage medium 802 in the embodiments of the present application can also be a circuit or any other device capable of realizing a storage function, used to store program instructions and / or data.

[0247] Optionally, the present application also provides a program product, for example, a computer readable storage medium, comprising a program, which when executed by a processor, is used to execute the above-mentioned method embodiments.

[0248] In several embodiments provided by the present application, it should be understood that the disclosed apparatus and method can be implemented in other manners. For example, the described apparatus embodiments are merely schematic, and the division of units is merely a logical function division, and there can be another division manner in actual implementation, for example, multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the displayed or discussed mutual couplings or direct couplings or communication connections can be indirect couplings or communication connections through some interfaces, devices or units, and can be electrical, mechanical or other forms.

[0249] The units described as separated components can or can not be physically separated, and the components displayed as units can or can not be physical units, i.e., can be located in one place, or can be distributed on a plurality of network units. Some or all of the units can be selected according to actual needs to achieve the purposes of the embodiments of the present application.

[0250] In addition, each functional unit in each embodiment of the present application can be integrated in one processing unit, or each unit can exist physically, or two or more units can be integrated in one unit. The integrated unit can be realized in the form of hardware or in the form of hardware plus software functional unit.

[0251] The integrated unit realized in the form of software functional unit can be stored in a computer readable storage medium. The software functional unit stored in a storage medium includes a plurality of instructions for enabling a computer device (which can be a personal computer, a server, or a network device, etc.) or a processor to execute part of the steps of the method described in each embodiment of the present application. The aforementioned storage medium includes a U disk, a mobile hard disk, a read-only memory (ROM), a random access memory (RAM), a magnetic disk or an optical disk, and various media capable of storing program codes.

Claims

1. A motion control method for a three-joint robotic arm, characterized in that, A processing device is used in a robotic arm control system. The system includes the processing device and a three-joint robotic arm with a target structure. The three-joint robotic arm includes a rotary joint, a first translational joint, and a second translational joint that are sequentially linked to each other. The method includes: Obtain the command point to which the end effector of the robotic arm is to be moved; Based on the positional relationship between the joint axes of each joint in the robotic arm, determine the inverse function relationship corresponding to the positional relationship; Based on the inverse function relationship, the joint rotation angle of the rotary joint, the first movement distance of the first movable joint, and the second movement distance of the second movable joint are determined respectively. The inverse function relationship is obtained by transforming the reference function relationship, which is constructed based on the algebraic equation relationship of the intermediate point and the constraint function relationship between the intermediate point and the command point. The algebraic equation relationship of the intermediate point is constructed based on the current starting point of the end effector of the robotic arm and the joint association parameters. The joint association parameters include: the first movement distance of the first movable joint, the first direction vector of the first joint axis of the first movable joint, the second movement distance of the second movable joint, and the second direction vector of the second joint axis of the second movable joint. The intermediate point includes the designated point traversed by the end effector of the robotic arm during its movement from the starting point to the command point. The robotic arm's rotary joints are controlled to rotate by the joint rotation angle, the first movable joint is controlled to move by the first moving distance, and the second movable joint is controlled to move by the second moving distance, so that the end effector of the robotic arm moves from the starting point to the command point.

2. The method according to claim 1, characterized in that, The step of determining the inverse function relationship corresponding to the positional relationship between the joint axes of each joint in the robotic arm includes: Based on the positional relationship between the joint axes of each joint, determine the vector equation relationship satisfied by the direction vectors of each joint axis under the positional relationship; Based on the vector equation relationship and the reference function relationship, determine the inverse solution function relationship corresponding to the positional relationship.

3. The method according to claim 1, characterized in that, The algebraic equation relationship of the intermediate point includes: the first algebraic equation relationship corresponding to the first intermediate point and the second algebraic equation relationship corresponding to the second intermediate point; The first algebraic equation is used to characterize the relationship between the first intermediate point, the starting point, the second moving distance, and the second direction vector. The second algebraic equation is used to characterize the relationship between the second intermediate point, the first intermediate point, the first moving distance, and the first direction vector.

4. The method according to claim 3, characterized in that, The constraint function relationship between the intermediate point and the instruction point includes: geometric constraint function and algebraic constraint function; The geometric constraint function is used to characterize the geometric relationship between the second intermediate point, the command point, and the direction vector of the rotational joint axis of the rotational joint; The algebraic constraint function is used to characterize the algebraic relationship between the second intermediate point, the command point, and the reference point on the axis of the rotary joint.

5. The method according to claim 3, characterized in that, If the axis of rotation of the rotary joint is not perpendicular to the axis of the second joint of the second movable joint, and the axis of rotation of the rotary joint is perpendicular to the axis of the first joint of the first movable joint, then the inverse function relationship is the function related to the second movement distance in the reference function relationship. The step of determining the joint rotation angle of the rotary joint, the first movement distance of the first movable joint, and the second movement distance of the second movable joint according to the inverse solution function relationship includes: The second moving distance is determined based on the inverse solution function relationship; The first moving distance is determined based on the second moving distance and the functional relationship between the first moving distance and the second moving distance in the reference functional relationship; The value of the first intermediate point is determined based on the second moving distance and the first algebraic equation. The value of the second intermediate point is determined based on the value of the first intermediate point, the first moving distance, and the second algebraic equation. The value of the joint rotation angle is determined based on the value of the second intermediate point, the command point, and the motion function relationship between the preset spatial point and the joint axis.

6. The method according to claim 3, characterized in that, If the axis of rotation of the rotary joint is not perpendicular to the first axis of the first movable joint, and the axis of rotation of the rotary joint is perpendicular to the second axis of the second movable joint, then the inverse function relationship is the function related to the first moving distance in the reference function relationship. The step of determining the joint rotation angle of the rotary joint, the first movement distance of the first movable joint, and the second movement distance of the second movable joint according to the inverse solution function relationship includes: The first moving distance is determined based on the inverse solution function relationship; The second movement distance is determined based on the first movement distance and the functional relationship between the first movement distance and the second movement distance in the reference functional relationship; The value of the first intermediate point is determined based on the second moving distance and the first algebraic equation. The value of the second intermediate point is determined based on the value of the first intermediate point, the first moving distance, and the second algebraic equation. The value of the joint rotation angle is determined based on the value of the second intermediate point, the command point, and the motion function relationship between the preset spatial point and the joint axis.

7. The method according to claim 3, characterized in that, If the axis of rotation of the rotary joint is not perpendicular to the first axis of the first movable joint, and the axis of rotation is not perpendicular to the second axis of the second movable joint, then the inverse function relationship is the function related to the second movement distance in the reference function relationship. The step of determining the joint rotation angle of the rotary joint, the first movement distance of the first movable joint, and the second movement distance of the second movable joint according to the inverse solution function relationship includes: The second moving distance is determined based on the inverse solution function relationship; The first moving distance is determined based on the second moving distance and the functional relationship between the first moving distance and the second moving distance in the reference functional relationship; The value of the first intermediate point is determined based on the second moving distance and the first algebraic equation. The value of the second intermediate point is determined based on the value of the first intermediate point, the first moving distance, and the second algebraic equation. The value of the joint rotation angle is determined based on the value of the second intermediate point, the command point, and the motion function relationship between the preset spatial point and the joint axis.

8. A motion control device for a three-joint robotic arm, characterized in that, A processing device is used in a robotic arm control system. The system includes the processing device and a three-joint robotic arm with a target structure. The three-joint robotic arm includes a rotary joint, a first traverse joint, and a second traverse joint that are linked together in sequence. The device includes: an acquisition module, a determination module, and a control module. The acquisition module is used to acquire the command point to be reached by the end effector of the robotic arm; The determining module is used to determine the inverse function relationship corresponding to the positional relationship based on the positional relationship between the joint axes of each joint in the robotic arm; The determining module is used to determine the joint rotation angle of the rotary joint, the first movement distance of the first movable joint, and the second movement distance of the second movable joint according to the inverse function relationship. The inverse function relationship is obtained by transforming the reference function relationship, which is constructed based on the algebraic equation relationship of the intermediate point and the constraint function relationship between the intermediate point and the command point. The algebraic equation relationship of the intermediate point is constructed based on the current starting point of the end effector of the robotic arm and the joint association parameters. The joint association parameters include: the first movement distance of the first movable joint, the first direction vector of the first joint axis of the first movable joint, the second movement distance of the second movable joint, and the second direction vector of the second joint axis of the second movable joint. The intermediate point includes the designated point traversed by the end effector of the robotic arm during its movement from the starting point to the command point. The control module is used to control the rotary joints of the robotic arm to rotate by the joint rotation angle, control the first movable joint to move by the first moving distance, and control the second movable joint to move by the second moving distance, so that the end of the robotic arm moves from the starting point to the command point.

9. An electronic device, characterized in that, include: The device includes a processor, a storage medium, and a bus. The storage medium stores program instructions executable by the processor. When the electronic device is running, the processor communicates with the storage medium via the bus, and the processor executes the program instructions to implement the steps of the three-joint robotic arm motion control method as described in any one of claims 1 to 7.

10. A computer-readable storage medium, characterized in that, The storage medium stores a computer program, which, when executed by a processor, performs the steps of the three-joint robotic arm motion control method as described in any one of claims 1 to 7.

Images