Joint angle inverse solution method, device and equipment, medium and tree-shaped robot

Abstract

The invention provides a joint angle inverse solution method, device and equipment, a medium and a tree-shaped robot, and relates to the technical field of robotics.The method comprises the steps that according to preset tail end tracking trajectories of multiple branch structures, the Cartesian speeds of the tail ends of the multiple branch structures at the next moment are obtained; according to the joint angle of each joint on the tree-shaped robot at the current moment and the preset motion constraint condition of each joint, adopting a preset target function of the tree-shaped robot to carry out joint angle conversion on the Cartesian speeds of the tail ends of the branch structures at the next moment, and obtaining the joint angle of each joint at the next moment, the preset objective function is an optimization function with the minimum sum of the kinematic errors of all the joints as an objective. And the preset tail end tracking trajectory is converted into the Cartesian speeds of the tail ends of the multiple branch structures at the next moment, then the joint angles of all joints on the tree-shaped robot are solved, and the structural advantages of the tree-shaped robot are brought into play according to the joint angles of all the joints.

Description

Technical Field

[0001] The present invention relates to the field of robotics, and in particular to a joint angle inverse solution method, device, equipment, medium and tree-like robot. Background Art

[0002] A tree-like robot is a type of robot with a branching topology. Its kinematic structure resembles a tree, with multiple branches extending from a single trunk. Both the trunk and branches represent a chain structure with arbitrary degrees of freedom. With the combined motion of the trunk and branches, each branch can complete a task independently or collaboratively.

[0003] Currently, inverse solutions for robots are primarily divided into analytical and numerical solutions. However, traditional closed-form analytical methods mostly focus on individual branches and lack an inverse solution that includes the trunk. This difficulty increases dramatically with increasing degrees of freedom. Conventional numerical iterative methods, starting from the current robot state, iterate to find the nearest inverse solution. However, this method fails to fully exploit the advantages of the tree-like robot structure. Summary of the Invention

[0004] The purpose of the present invention is to address the deficiencies in the above-mentioned prior art and provide a joint angle inverse solution method, device, equipment, medium and tree-like robot, so as to obtain the joint angle of each joint on the tree-like robot at the next moment.

[0005] To achieve the above objectives, the technical solutions adopted in the embodiments of the present application are as follows: In a first aspect, an embodiment of the present application provides a method for inversely solving joint angles of a tree-like robot, wherein the tree-like robot includes a trunk structure and a plurality of branch structures connected to the trunk structure, and the method includes: Obtaining Cartesian velocities of the ends of the plurality of branch structures at a next moment respectively according to preset end tracking trajectories of the plurality of branch structures; According to the joint angles of the joints on the tree-like robot at the current moment and the preset motion constraints of the joints, the preset objective function of the tree-like robot is used to convert the Cartesian velocities of the ends of the multiple branch structures at the next moment into joint angles to obtain the joint angles of the joints at the next moment, wherein the preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of the joints, and the joints on the tree-like robot include the joints on the trunk structure and the joints on the multiple branch structures.

[0006] In an optional embodiment, the method further comprises: performing joint angle conversion on the Cartesian velocities of the ends of the plurality of branch structures at the next moment based on the joint angles of the joints on the tree-like robot at the current moment and the preset motion constraints of the joints, using a preset objective function of the tree-like robot, and obtaining the joint angles of the joints at the next moment, including: Based on the joint angles of the joints at the current moment and the Cartesian velocities of the ends of the multiple branch structures at the next moment, a first objective function of the tree-like robot is solved using preset motion constraints of the joints to obtain optimal velocity error variables of the ends of the multiple branch structures; wherein the first objective function is an optimization function that aims to minimize the sum of the Cartesian velocity errors of the joints; According to the optimal velocity error variables of the ends of the multiple branch structures, the joint angles of the joints at the current moment, and the Cartesian velocities of the ends of the multiple branch structures at the next moment, the second objective function of the tree-like robot is solved using the preset motion constraints of the joints to obtain the joint angular velocities of the joints at the next moment, wherein the second objective function is an optimization function with the goal of minimizing the sum of the joint angle errors of the joints; The joint angles of the joints at the next moment are obtained according to the joint angular velocities of the joints at the next moment and the joint angles at the current moment.

[0007] In an optional embodiment, obtaining the Cartesian velocities of the ends of the multiple branch structures at the next moment respectively according to the preset end tracking trajectories of the multiple branch structures includes: Differentiating a preset terminal tracking trajectory of each branch structure to obtain velocity information and rotation speed information of the terminal of each branch structure at the next moment as the Cartesian velocity of the terminal of each branch structure at the next moment; Among them, the speed information includes: the speed component of the end of each branch structure at the origin of the end coordinate system at the next moment relative to each coordinate axis in the preset base coordinate system, and the rotational speed information includes: the rotational speed component of the end of each branch structure at the end coordinate system at the next moment around each coordinate axis in the preset base coordinate system.

[0008] In an optional embodiment, the preset motion constraints of each joint include: preset joint angular velocity constraints of each joint, preset joint angle constraints, and relationship constraints between the joint angular velocity of each joint and the Cartesian velocity of the end of the multiple branch structures at the next moment.

[0009] In an optional embodiment, if the multiple branch structures include a first branch structure and a second branch structure, the preset joint angular velocity constraint condition is as shown in the following formula:

[0010] in, Represented as the first i The minimum joint angular velocity of the joint, Expressed as the i The maximum joint angular velocity of the joint, Expressed as the i The joint angular velocity of the joint at the next moment; The preset joint angle constraint condition is shown in the following formula:

[0011] in, Expressed as the i The minimum joint angle of the joint, Expressed as the i The maximum joint angle of the joint, Expressed as the i The joint angle of the joint at the current moment, Expressed as the time of one cycle; The relationship constraint condition is shown in the following formula:

[0012] in, It is represented by the first Jacobian matrix corresponding to the chain consisting of the trunk structure and the first branch structure, Represented as 6 lines A zero matrix of columns, Expressed as the joint degrees of freedom of the second branch structure, Represented as 6 lines A zero matrix of columns, Expressed as the joint degrees of freedom of the first branch structure, The first part is represented as the second Jacobian matrix List, The second Jacobian matrix is ​​expressed as Column, the second Jacobian matrix is ​​the Jacobian matrix corresponding to the chain consisting of the trunk structure and the second branch structure, Expressed as the joint degrees of freedom of the backbone structure, It is represented by the Cartesian velocity of the terminal of the multiple branch structures at the next moment, It is represented as the optimal speed error variable at the ends of the plurality of branch structures.

[0013] In an optional embodiment, if the multiple branch structures include a first branch structure and a second branch structure, the first objective function is as shown in the following formula:

[0014] in, The first terminal of the plurality of branch structures is represented by i The optimal speed error variable, Expressed as the number of said optimal speed error variables; The second objective function is shown in the following formula:

[0015] in, Represented as the first i The joint angle of the joint at the current moment, Expressed as the i The joint angular velocity of the joint at the next moment, Expressed as the time of one cycle, Expressed as the i The preset motion midpoint of the joint, Expressed as the i The preset range of motion of the joint, Expressed as the joint degrees of freedom of the backbone structure, Expressed as the joint degrees of freedom of the second branch structure, It is represented as the joint degrees of freedom of the first branch structure.

[0016] In a second aspect, an embodiment of the present application further provides a joint angle inverse solution device for a tree-like robot, wherein the tree-like robot includes a trunk structure and a plurality of branch structures connected to the trunk structure, and the device includes: an acquisition module, configured to respectively acquire the Cartesian velocities of the ends of the plurality of branch structures at a next moment according to preset end tracking trajectories of the plurality of branch structures; A conversion module is used to convert the joint angles of the joints on the tree-like robot at the current moment and the preset motion constraints of the joints, using the preset objective function of the tree-like robot, to the Cartesian velocities of the ends of the multiple branch structures at the next moment, so as to obtain the joint angles of the joints at the next moment, wherein the preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of the joints, and the joints on the tree-like robot include the joints on the trunk structure and the joints on the multiple branch structures.

[0017] In the third aspect, an embodiment of the present application also provides a computer device, comprising: a processor, a storage medium and a bus, wherein the storage medium stores program instructions executable by the processor. When the computer device is running, the processor and the storage medium communicate through the bus, and the processor executes the program instructions to perform the steps of the joint angle inverse solution method of the tree-like robot as described in any one of the first aspects.

[0018] In a fourth aspect, an embodiment of the present application further provides a computer-readable storage medium having a computer program stored thereon, and when the computer program is executed by a processor, the steps of the joint angle inverse solution method of the tree-like robot as described in any one of the first aspects are executed.

[0019] In a fifth aspect, an embodiment of the present application further provides a tree-like robot, the tree-like robot comprising: a controller, a base, a trunk structure fixed to the base, and a plurality of branch structures connected to the trunk structure; The controller is connected to each joint on the tree-like robot and is used to execute the steps of the joint angle inverse solution method of the tree-like robot as described in any one of the first aspects.

[0020] The beneficial effects of this application are: The present invention provides a method, apparatus, device, medium, and tree-like robot for inverse joint angle solution, the method comprising: obtaining the Cartesian velocities of the ends of the multiple branch structures at the next moment according to preset terminal tracking trajectories of the multiple branch structures; converting the Cartesian velocities of the ends of the multiple branch structures at the next moment according to the joint angles of each joint on the tree-like robot at the current moment and the preset motion constraints of each joint using a preset objective function of the tree-like robot to obtain the joint angles of each joint at the next moment, wherein the preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of each joint, and each joint on the tree-like robot includes a joint on a trunk structure and joints on multiple branch structures. The method of the present invention first converts the preset terminal tracking trajectory into the Cartesian velocities of the ends of the multiple branch structures at the next moment, and then, using a preset objective function with preset motion constraints, solves the inverse joint angles of each joint corresponding to the Cartesian velocities of the ends of the multiple branch structures at the next moment under the premise of ensuring the motion safety of each joint, so that the coordinated motion control of the trunk structure and multiple branch structures of the tree-like robot can be achieved according to the joint angles of each joint at the next moment, giving full play to the structural advantages of the tree-like robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0021] In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the following briefly introduces the drawings required for use in the embodiments. It should be understood that the following drawings only illustrate certain embodiments of the present invention and therefore should not be regarded as limiting the scope. For ordinary technicians in this field, other relevant drawings can be obtained based on these drawings without paying any creative work.

[0022] Figure 1 A schematic flow chart of a method for inverse solution of joint angles of a tree-like robot provided in an embodiment of the present application; Figure 2 A schematic flow chart of another method for inverse solution of joint angles of a tree-like robot provided in an embodiment of the present application; Figure 3 A schematic diagram of the functional modules of a joint angle inverse solution device for a tree-like robot provided in an embodiment of the present application; Figure 4 A schematic diagram of a computer device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0023] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments.

[0024] Therefore, the following detailed description of the embodiments of the present application provided in the accompanying drawings is not intended to limit the scope of the present application for protection, but merely represents selected embodiments of the present application. All other embodiments obtained by persons of ordinary skill in the art based on the embodiments in the present application without creative work are within the scope of protection of the present application.

[0025] In the description of this application, it should be noted that if the terms "upper", "lower", etc. appear, the orientation or position relationship indicated is based on the orientation or position relationship shown in the accompanying drawings, or is the orientation or position relationship in which the product of the application is usually placed when in use. It is only for the convenience of describing this application and simplifying the description, and does not indicate or imply that the device or element referred to must have a specific orientation, be constructed and operated in a specific orientation. Therefore, it cannot be understood as a limitation on this application.

[0026] In addition, the terms "first," "second," and the like in the description and claims of the present invention and the accompanying drawings are used to distinguish similar objects and are not necessarily used to describe a particular order or precedence. It should be understood that the terms used in this manner are interchangeable where appropriate so that the embodiments of the present invention described herein can be implemented in an order other than those illustrated or described herein. In addition, the terms "including" and "having," as well as any variations thereof, are intended to cover non-exclusive inclusions. For example, a process, method, system, product, or apparatus comprising a series of steps or units is not necessarily limited to those steps or units explicitly listed, but may include other steps or units that are not explicitly listed or that are inherent to these processes, methods, products, or apparatus.

[0027] It should be noted that, in the absence of conflict, the features in the embodiments of this application can be combined with each other.

[0028] A tree-like robot is a type of robot with a branching topology. Its kinematic structure is similar to a tree, with multiple branches extending from a trunk. Both the trunk and the branches represent a chain structure with arbitrary degrees of freedom. Under the joint movement of the trunk and branches, each branch can complete a task independently or collaboratively.

[0029] Specifically, the tree-like robot includes: a controller, a base, a trunk structure fixed on the base, and multiple branch structures connected to the trunk structure. The base can be movable or fixed. The controller can be inside the base or be a separate controller. The controller is connected to the joints on the tree-like robot. By inversely solving the joint angles of the tree-like robot, the joint angles of each joint on the tree-like robot at the next moment are obtained, and each joint is controlled according to the joint angles of each joint at the next moment.

[0030] The following is an explanation of the inverse solution method of the joint angles of the tree-like robot executed by the controller. In order to inversely solve the joint angles of the tree-like robot, not only the joint angles on each branch structure are inversely solved, but also the joint angles on the trunk structure are inversely solved, so as to give full play to the structural advantages of the tree-like robot. An embodiment of the present application provides a method for inverse solution of the joint angles of a tree-like robot. According to the preset terminal tracking trajectories of the multiple branch structures, the Cartesian velocities of the ends of the multiple branch structures at the next moment are respectively obtained. Then, according to the joint angles of each joint on the tree-like robot at the current moment and the preset motion constraints of each joint, the preset objective function of the tree-like robot is used to convert the Cartesian velocities of the ends of the multiple branch structures at the next moment into joint angles to obtain the joint angles of each joint at the next moment. The preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of each joint. The joints on the tree-like robot include the joints on the trunk structure and the joints on the multiple branch structures, thereby obtaining the joint angles of each joint at the next moment corresponding to the Cartesian velocities of the ends of the multiple branch structures at the next moment.

[0031] The following is a detailed explanation of the joint angle inverse solution method of the tree-like robot provided by the embodiment of the present application through specific examples in conjunction with the accompanying drawings. The joint angle inverse solution method of the tree-like robot provided by the embodiment of the present application can also be implemented by a computer device pre-installed with: a preset tree-like robot joint angle inverse solution algorithm or detection software, by running the algorithm or software. The computer device can be, for example, a server or a terminal, and the terminal can be a user's computer. The tree-like robot includes a trunk structure, and a plurality of branch structures connected to the trunk structure, Figure 1 A schematic diagram of a flow chart of a method for inverse solution of joint angles of a tree-like robot provided in an embodiment of the present application; Figure 1 As shown, the method includes: S101 , obtaining Cartesian velocities of the ends of the multiple branch structures at the next moment respectively according to preset end tracking trajectories of the multiple branch structures.

[0032] In this embodiment, a preset terminal tracking trajectory of each branch structure is obtained. The preset terminal tracking trajectory refers to a pre-planned motion path of each branch structure terminal. For example, a branch structure terminal may need to move along a straight line or circular arc trajectory from point A to point B.

[0033] Optionally, the preset terminal tracking trajectory of each branch structure is differentiated to obtain speed information and rotation speed information of the terminal of each branch structure at the next moment as the Cartesian speed of the terminal of each branch structure at the next moment.

[0034] Among them, the speed information includes: the speed component of the origin of the terminal coordinate system of the end of each branch structure at the next moment relative to each coordinate axis in the preset base coordinate system, and the rotational speed information includes: the rotational speed component of the terminal coordinate system of the end of each branch structure at the next moment around each coordinate axis in the preset base coordinate system.

[0035] Specifically, if the multiple branch structures include a first branch structure and a second branch structure, wherein the first branch structure is a branch structure on the left side relative to the main structure, and the second branch structure is a branch structure on the right side relative to the main structure, then the preset terminal tracking trajectory of the first branch structure and the preset terminal tracking trajectory of the second branch structure are differentiated respectively to obtain speed information and rotation speed information of the first branch structure terminal and the second branch structure terminal at the next moment, thereby obtaining the Cartesian speed to be tracked by the first branch structure terminal and the second branch structure terminal at the next moment, wherein the Cartesian speed to be tracked by the first branch structure terminal and the second branch structure terminal at the next moment is expressed as:

[0036] in, Expressed as the velocity of the origin of the first branch structure end coordinate system relative to the preset base coordinate system Quantity; Expressed as the velocity of the origin of the first branch structure end coordinate system relative to the preset base coordinate system Quantity; Expressed as the velocity of the origin of the first branch structure end coordinate system relative to the preset base coordinate system Quantity; It is represented by the coordinate system of the first branch structure end around the preset base coordinate system The speed component of the shaft; It is represented by the coordinate system of the first branch structure end around the preset base coordinate system The speed component of the shaft; It is represented by the coordinate system of the first branch structure end around the preset base coordinate system The speed component of the shaft; Similarly, Expressed as the velocity of the origin of the second branch structure end coordinate system relative to the preset base coordinate system Quantity; Expressed as the velocity of the origin of the second branch structure end coordinate system relative to the preset base coordinate system Quantity; Expressed as the velocity of the origin of the second branch structure end coordinate system relative to the preset base coordinate system Quantity; Expressed as the coordinate system of the second branch structure end around the preset base coordinate system The speed component of the shaft; Expressed as the coordinate system of the second branch structure end around the preset base coordinate system The speed component of the shaft; Expressed as the coordinate system of the second branch structure end around the preset base coordinate system The speed component of the shaft.

[0037] S102. Based on the joint angles of each joint on the tree-like robot at the current moment and the preset motion constraints of each joint, the preset objective function of the tree-like robot is used to convert the Cartesian velocities of the ends of the multiple branch structures at the next moment into joint angles to obtain the joint angles of each joint at the next moment.

[0038] Among them, the preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of each joint. The joints on the tree-like robot include joints on the trunk structure and joints on multiple branch structures.

[0039] Specifically, the joint angles of each joint on the tree-like robot at the current moment, that is, the rotation angles of each joint of the tree-like robot at the current moment, serve as the initial state of the inverse solution. Since joint motion is continuous, the joint angles at the next moment need to be adjusted based on the current angles.

[0040] Since the terminal speed requirements of multiple branch structures of the tree-like robot may conflict and cannot be accurately met at the same time, it is necessary to solve the preset objective function through the joint angles of each joint on the tree-like robot at the current moment, the preset motion constraints of each joint and the Cartesian speeds of the ends of the multiple branch structures at the next moment, so that the sum of the motion errors of each joint is minimized, thereby obtaining the optimal solution, that is, the joint angles of each joint at the next moment.

[0041] In summary, the present invention provides a method for inversely solving the joint angles of a tree-like robot, the method comprising: obtaining the Cartesian velocities of the ends of the multiple branch structures at the next moment according to the preset terminal tracking trajectories of the multiple branch structures; converting the Cartesian velocities of the ends of the multiple branch structures at the next moment according to the joint angles of each joint on the tree-like robot at the current moment and the preset motion constraints of each joint using the preset objective function of the tree-like robot to obtain the joint angles of each joint at the next moment, wherein the preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of each joint, and each joint on the tree-like robot includes the joints on the trunk structure and the joints on the multiple branch structures. The method of the present invention first converts the preset terminal tracking trajectories into the Cartesian velocities of the ends of the multiple branch structures at the next moment, and then, using the preset objective function with preset motion constraints, solves the inverse joint angles of each joint corresponding to the Cartesian velocities of the ends of the multiple branch structures at the next moment under the premise of ensuring the motion safety of each joint, so that the coordinated motion control of the trunk structure and the multiple branch structures of the tree-like robot can be achieved according to the joint angles of each joint at the next moment, giving full play to the structural advantages of the tree-like robot.

[0042] The embodiment of the present application also provides another possible implementation of the joint angle inverse solution method of the tree-like robot. Figure 2 This is a flow chart of another method for inverse solution of joint angles of a tree-like robot provided in an embodiment of the present application. Figure 2 As shown, according to the joint angles of each joint on the tree-like robot at the current moment and the preset motion constraints of each joint, the preset objective function of the tree-like robot is used to convert the Cartesian velocities of the ends of the multiple branch structures at the next moment into joint angles, and the joint angles of each joint at the next moment are obtained, including: S201. Based on the joint angles of each joint at the current moment and the Cartesian velocities of the ends of the multiple branch structures at the next moment, the preset motion constraints of each joint are used to solve the first objective function of the tree-like robot to obtain the optimal velocity error variables of the ends of the multiple branch structures.

[0043] The first objective function is an optimization function that aims to minimize the sum of the Cartesian velocity errors of each joint.

[0044] In this embodiment, a tree-like robot including a first branch structure and a second branch structure is taken as an example, wherein the trunk structure has n degrees of freedom, the first branch structure has m degrees of freedom, the second branch structure has g degrees of freedom, which can be understood as joints. At the current moment t, the joint angle, joint angular velocity, and joint angular acceleration of each joint on the tree-like robot are expressed as:

[0045]

[0046]

[0047] in, It is represented by the joint angle of the trunk structure from bottom to top at the current moment t; It is represented by the joint angles of each joint of the first branch structure from the main structure to the end of the first branch structure at the current time t; It is represented by the joint angles of each joint of the second branch structure from the main structure to the end of the second branch structure at the current time t.

[0048] same, It is represented by the joint angular velocity of the trunk structure from bottom to top at the current moment t; It is represented by the joint angular velocity of each joint of the first branch structure from the main structure to the end of the first branch structure at the current time t; It is expressed as the joint angular velocity of each joint of the second branch structure from the main structure to the end of the second branch structure at the current time t. It is represented by the joint angular acceleration of the trunk structure from bottom to top at the current moment t; It is represented by the joint angular acceleration of each joint of the first branch structure from the main structure to the end of the first branch structure at the current time t; It is represented by the joint angular acceleration of each joint of the second branch structure from the main structure to the end of the second branch structure at the current time t. Represents vector transpose, indicating that the state vectors are all column vectors.

[0049] Optionally, the preset motion constraints of each joint include: preset joint angular velocity constraints of each joint, preset joint angle constraints, and relationship constraints between the joint angular velocity of each joint and the Cartesian velocity of the ends of multiple branch structures at the next moment.

[0050] If the multiple branch structures include a first branch structure and a second branch structure, the preset joint angular velocity constraint condition is as shown in the following formula:

[0051] in, Represented as the first i The minimum joint angular velocity of the joint, Expressed as i The maximum joint angular velocity of the joint, Expressed as i The joint angular velocity of the joint at the next moment; The preset joint angle constraint condition is as shown in the following formula:

[0052] in, Expressed as i The minimum joint angle of the joint, Expressed as i The maximum joint angle of the joint, Expressed as i The joint angle of the joint at the current moment, Expressed as the time of one cycle; The relationship constraint is shown in the following formula:

[0053] in, It is represented by the first Jacobian matrix corresponding to the chain consisting of the backbone structure and the first branch structure, Represented as 6 lines A zero matrix of columns, Expressed as the joint degrees of freedom of the second branch structure, Represented as 6 lines A zero matrix of columns, Expressed as the joint degrees of freedom of the first branch structure, The first part is represented as the second Jacobian matrix List, The second Jacobian matrix is ​​expressed as The second Jacobian matrix is ​​the Jacobian matrix corresponding to the chain consisting of the trunk structure and the second branch structure. Represents the joint degrees of freedom of the backbone structure, Expressed as the Cartesian velocity of the end of multiple branch structures at the next moment, Expressed as the optimal speed error variable at the end of multiple branch structures.

[0054] Specifically, the preset joint angular velocity constraint condition is expressed as the actual angular velocity of each joint cannot be less than the minimum value of the joint angular velocity and cannot be greater than the maximum value of the joint angular velocity. The preset joint angle constraint condition is expressed as the actual angle of each joint cannot be less than the minimum value of the joint angle and cannot be greater than the maximum value of the joint angle. The relationship constraint condition is expressed as the Jacobian matrix multiplied by the joint angular velocity of each joint must be equal to the Cartesian velocity of the end of the multiple branch structures at the next moment minus the optimal velocity error variable of the end of the multiple branch structures.

[0055] Among them, the first Jacobian matrix corresponding to the chain composed of the trunk structure and the first branch structure is For 6 lines, n + mThe matrix of columns, the second Jacobian matrix corresponding to the chain consisting of the trunk structure and the second branch structure For 6 lines, n + g A matrix of columns.

[0056] Optionally, the first objective function is as shown in the following formula:

[0057] in, The end of a multiple branch structure i The optimal speed error variable, Expressed as the number of optimal speed error variables.

[0058] Then, according to the joint angle of each joint at the current moment and the Cartesian velocity of the end of the multiple branch structures at the next moment, the preset joint angular velocity constraints and preset joint angle constraints of each joint, as well as the relationship constraints between the joint angular velocity of each joint and the Cartesian velocity of the end of the multiple branch structures at the next moment, are adopted to solve the first objective function of the tree-like robot, complete the first-level quadratic planning, and obtain the optimal velocity error variable of the end of the multiple branch structures.

[0059] Since the Cartesian velocity of the ends of the first branch structure and the second branch structure at the next moment is , including 12 components, the number of optimal speed error variables is also 12, that is, k is 12 at this time, and the optimal speed error variable is expressed as: , where the 12 optimal velocity error components correspond to By introducing the optimal velocity error variable, the problem of poor tracking accuracy caused by the end of some branch structures reaching the boundary of the workspace or near the singular point is solved.

[0060] S202. Based on the optimal velocity error variables at the ends of multiple branch structures, the joint angles of each joint at the current moment, and the Cartesian velocities of the ends of multiple branch structures at the next moment, the preset motion constraints of each joint are used to solve the second objective function of the tree-like robot to obtain the joint angular velocity of each joint at the next moment.

[0061] The second objective function is an optimization function that aims to minimize the sum of the joint angle errors of each joint.

[0062] Specifically, the optimal velocity error variables at the ends of multiple branch structures solved by the first-level quadratic programming are brought into the second-level quadratic programming as known conditions. The solution variable of the second-level quadratic programming is the joint angular velocity at the next moment. In order to minimize the sum of the joint angle errors of each joint, the optimization target is to make the angle of each joint closest to the preset motion midpoint.

[0063] The lower limit of the joint angle of each structure is , the upper limit is , lower right corner mark Represents the joint number The preset motion midpoint for each joint angle Expressed as:

[0064] Preset range of motion for each joint angle Expressed as:

[0065] Optionally, the second objective function is as shown in the following formula:

[0066] in, Represented as the first i The joint angle of the joint at the current moment, Expressed as i The joint angular velocity of the joint at the next moment, Expressed as the time of one cycle, Expressed as i The preset motion midpoint of the joint, Expressed as i The preset range of motion of the joint, Represents the joint degrees of freedom of the backbone structure, Expressed as the joint degrees of freedom of the second branch structure, It is represented as the joint degrees of freedom of the first branch structure.

[0067] Then, according to the optimal speed error variables at the ends of the multiple branch structures, the joint angles of each joint at the current moment, and the Cartesian speeds of the ends of the multiple branch structures at the next moment, the preset joint angular velocity constraints of each joint, the preset joint angle constraints, and the relationship constraints between the joint angular velocity of each joint and the Cartesian speeds of the ends of the multiple branch structures at the next moment are adopted to solve the second objective function of the tree-like robot and obtain the joint angular velocity of each joint at the next moment. Since the optimal speed error variables at the ends of the multiple branch structures are is known, so the optimal joint angular velocity of each joint at the next moment can be solved .

[0068] It should be noted that in the first-level quadratic programming, the optimal speed error variables at the ends of multiple branch structures are and the joint angular velocity of each joint at the next moment are all unknown, although in the first-level quadratic planning, the joint angular velocity at the next moment can also be obtained , but it is not accurate enough, so through the second-level quadratic programming, the optimal speed error variables at the ends of multiple branch structures are Bring in and get the accurate and optimal joint angular velocity of each joint at the next moment .

[0069] S203 . Obtain the joint angle of each joint at the next moment according to the joint angular velocity of each joint at the next moment and the joint angle at the current moment.

[0070] Specifically, after obtaining the optimal joint angular velocity of each joint at the next moment Afterwards, the joint angle at the current moment The inverse joint angles of each joint at the next moment are obtained by superimposing them. Expressed as:

[0071] Thus, the joint angles of each joint on the tree-like robot at the next moment are obtained.

[0072] In the method provided in the embodiment of the present application, the first objective function of the tree-like robot is solved based on the joint angle of each joint at the current moment and the Cartesian speed of the end of multiple branch structures at the next moment, using the preset motion constraint conditions of each joint, to obtain the optimal speed error variable of the end of multiple branch structures; wherein the first objective function is an optimization function with the minimum sum of the Cartesian speed errors of each joint as the goal; according to the optimal speed error variables at the end of multiple branch structures, the joint angle of each joint at the current moment and the Cartesian speed of the end of multiple branch structures at the next moment, using the preset motion constraint conditions of each joint, the second objective function of the tree-like robot is solved to obtain the joint angular velocity of each joint at the next moment, wherein the second objective function is an optimization function with the minimum sum of the joint angle errors of each joint as the goal; according to the joint angular velocity of each joint at the next moment and the joint angle at the current moment, the joint angle of each joint at the next moment is obtained. Through the first-level quadratic programming, the first objective function is solved to obtain the optimal speed error variables of the ends of the multiple branch structures, and the optimal speed error variables of the ends of the multiple branch structures are input into the second-level quadratic programming as known quantities. Then, the second objective function is solved to obtain the joint angular velocity of each joint on the trunk structure and the multiple branch structures at the next moment, so that each joint can be precisely controlled according to the joint angular velocity of each joint at the next moment, giving full play to the structural advantages of the tree-like robot. By introducing the optimal speed error variables at the ends of the multiple branch structures, the problem of deterioration in tracking accuracy caused by some branch ends reaching the boundary of the workspace or near the singular point is solved.

[0073] The following continues to explain the joint angle inverse solution device and computer equipment of the tree-like robot provided by any of the above embodiments of the present application. Its specific implementation process and the technical effects produced are the same as those of the corresponding method embodiments mentioned above. For the sake of brief description, for the parts not mentioned in this embodiment, please refer to the corresponding content in the method embodiment.

[0074] Figure 3 Schematic diagram of the functional modules of a joint angle inverse solution device of a tree-like robot provided in an embodiment of the present application. The tree-like robot includes a trunk structure and multiple branch structures connected to the trunk structure, such as Figure 3 As shown, the joint angle inverse solution device 100 of the tree-like robot includes: The acquisition module 110 is configured to respectively acquire the Cartesian velocities of the ends of the plurality of branch structures at the next moment according to the preset end tracking trajectories of the plurality of branch structures.

[0075] The conversion module 120 is used to convert the Cartesian velocities of the ends of multiple branch structures at the next moment into joint angles according to the joint angles of each joint on the tree-like robot at the current moment and the preset motion constraints of each joint, using the preset objective function of the tree-like robot to obtain the joint angles of each joint at the next moment, wherein the preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of each joint, and the joints on the tree-like robot include the joints on the trunk structure and the joints on the multiple branch structures.

[0076] Optionally, the conversion module 120 is also used to solve the first objective function of the tree-like robot based on the joint angle of each joint at the current moment and the Cartesian velocity of the end of the multiple branch structures at the next moment, using the preset motion constraints of each joint, to obtain the optimal speed error variable of the end of the multiple branch structures; wherein the first objective function is an optimization function with the goal of minimizing the sum of the Cartesian velocity errors of each joint; according to the optimal speed error variables at the end of the multiple branch structures, the joint angle of each joint at the current moment and the Cartesian velocity of the end of the multiple branch structures at the next moment, using the preset motion constraints of each joint, the second objective function of the tree-like robot is solved, to obtain the joint angular velocity of each joint at the next moment, wherein the second objective function is an optimization function with the goal of minimizing the sum of the joint angle errors of each joint; according to the joint angular velocity of each joint at the next moment and the joint angle at the current moment, the joint angle of each joint at the next moment is obtained.

[0077] Optionally, the acquisition module 110 is also used to differentiate the preset terminal tracking trajectory of each branch structure to obtain the speed information and rotation speed information of the terminal of each branch structure at the next moment as the Cartesian speed of the terminal of each branch structure at the next moment; wherein the speed information includes: the speed component of the origin of the terminal coordinate system of the terminal of each branch structure at the next moment relative to each coordinate axis in the preset base coordinate system, and the rotation speed information includes: the rotation speed component of the terminal coordinate system of the terminal of each branch structure at the next moment around each coordinate axis in the preset base coordinate system.

[0078] Optionally, the preset motion constraints of each joint include: preset joint angular velocity constraints of each joint, preset joint angle constraints, and relationship constraints between the joint angular velocity of each joint and the Cartesian velocity of the ends of multiple branch structures at the next moment.

[0079] Optionally, if the multiple branch structures include a first branch structure and a second branch structure, the preset joint angular velocity constraint condition is as shown in the following formula:

[0080] in, Represented as the first iThe minimum joint angular velocity of the joint, Expressed as i The maximum joint angular velocity of the joint, Expressed as i The joint angular velocity of the joint at the next moment; The preset joint angle constraint condition is as shown in the following formula:

[0081] in, Expressed as i The minimum joint angle of the joint, Expressed as i The maximum joint angle of the joint, Expressed as i The joint angle of the joint at the current moment, Expressed as the time of one cycle; The relationship constraint condition is shown in the following formula:

[0082] in, It is represented by the first Jacobian matrix corresponding to the chain consisting of the trunk structure and the first branch structure, Represented as 6 lines A zero matrix of columns, Expressed as the joint degrees of freedom of the second branch structure, Represented as 6 lines A zero matrix of columns, Expressed as the joint degrees of freedom of the first branch structure, The first part is represented as the second Jacobian matrix List, The second Jacobian matrix is ​​expressed as The second Jacobian matrix is ​​the Jacobian matrix corresponding to the chain consisting of the trunk structure and the second branch structure. Represents the joint degrees of freedom of the backbone structure, Expressed as the Cartesian velocity of the end of multiple branch structures at the next moment, Expressed as the optimal speed error variable at the end of multiple branch structures.

[0083] Optionally, if the multiple branch structures include a first branch structure and a second branch structure, the first objective function is as shown in the following formula:

[0084] in, The end of a multiple branch structure i The optimal speed error variable, Expressed as the number of optimal speed error variables; The second objective function is shown in the following formula:

[0085] in, Represented as the first i The joint angle of the joint at the current moment, Expressed as i The joint angular velocity of the joint at the next moment, Expressed as the time of one cycle, Expressed as i The preset motion midpoint of the joint, Expressed as i The preset range of motion of the joint, Represents the joint degrees of freedom of the backbone structure, Expressed as the joint degrees of freedom of the second branch structure, It is represented as the joint degrees of freedom of the first branch structure.

[0086] The above-mentioned device is used to execute the method provided in the above-mentioned embodiment. Its implementation principle and technical effect are similar and will not be repeated here.

[0087] The above modules can be one or more integrated circuits configured to implement the above methods, such as one or more application-specific integrated circuits (ASICs), one or more microprocessors, or one or more field programmable gate arrays (FPGAs). For example, when a module is implemented by scheduling program code through a processing element, the processing element can be a general-purpose processor, such as a central processing unit (CPU) or other processor that can call program code. For another example, these modules can be integrated together and implemented in the form of a system-on-a-chip (SOC).

[0088] Figure 4 This is a schematic diagram of a computer device provided in an embodiment of the present application, which can be used for inverse solution of joint angles of a tree-like robot. Figure 4 As shown, the computer device includes: a processor 210 , a storage medium 220 , and a bus 230 .

[0089] Storage medium 220 stores machine-readable instructions executable by processor 210. When the computer device is running, processor 210 communicates with storage medium 220 via bus 230, and processor 210 executes the machine-readable instructions to perform the steps of the above-described method embodiment. The specific implementation methods and technical effects are similar and will not be repeated here.

[0090] Optionally, the present application further provides a storage medium 220 on which a computer program is stored. When the computer program is executed by a processor, the steps of the above method embodiment are executed. The specific implementation and technical effects are similar and will not be repeated here.

[0091] In the several embodiments provided by the present invention, it should be understood that the disclosed devices and methods can be implemented in other ways. For example, the device embodiments described above are merely illustrative. For example, the division of the units is merely a logical function division. In actual implementation, there may be other division methods, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. In addition, the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0092] The units described as separate components may or may not be physically separate, and the components shown as units may or may not be physical units, that is, they may be located in one place or distributed across multiple network units. Some or all of these units may be selected to achieve the purpose of this embodiment according to actual needs.

[0093] In addition, the functional units in various embodiments of the present invention may be integrated into a single processing unit, each unit may exist physically separately, or two or more units may be integrated into a single unit. The aforementioned integrated units may be implemented in the form of hardware or hardware plus software functional units.

[0094] The aforementioned integrated unit implemented as a software functional unit can be stored in a computer-readable storage medium. The software functional unit, stored in a storage medium, includes instructions for causing a computer device (which may be a personal computer, server, or network device, etc.) or a processor to execute portions of the method steps described in various embodiments of the present invention. The aforementioned storage medium includes various media capable of storing program code, such as a USB flash drive, a removable hard drive, a read-only memory (ROM), a random access memory (RAM), a magnetic disk, or an optical disk.

[0095] The above are only specific embodiments of the present invention, but the scope of protection of the present invention is not limited thereto. Any changes or substitutions that can be easily conceived by a person skilled in the art within the technical scope disclosed in this invention should be included in the scope of protection of the present invention. Therefore, the scope of protection of the present invention should be based on the scope of protection of the claims.

Claims

1. A method for inverse solution of joint angles of a tree-like robot, characterized in that: The tree-like robot includes a trunk structure and a plurality of branch structures connected to the trunk structure, and the method includes: Obtaining Cartesian velocities of the ends of the plurality of branch structures at a next moment respectively according to preset end tracking trajectories of the plurality of branch structures; According to the joint angles of the joints on the tree-like robot at the current moment and the preset motion constraints of the joints, the preset objective function of the tree-like robot is used to convert the Cartesian velocities of the ends of the multiple branch structures at the next moment into joint angles to obtain the joint angles of the joints at the next moment, wherein the preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of the joints, and the joints on the tree-like robot include the joints on the trunk structure and the joints on the multiple branch structures.

2. The method according to claim 1, characterized in that The method comprises: performing joint angle conversion on the Cartesian velocities of the ends of the plurality of branch structures at the next moment based on the joint angles of the joints on the tree-like robot at the current moment and the preset motion constraints of the joints, and using the preset objective function of the tree-like robot to obtain the joint angles of the joints at the next moment. Based on the joint angles of the joints at the current moment and the Cartesian velocities of the ends of the multiple branch structures at the next moment, a first objective function of the tree-like robot is solved using preset motion constraints of the joints to obtain optimal velocity error variables of the ends of the multiple branch structures; wherein the first objective function is an optimization function that aims to minimize the sum of the Cartesian velocity errors of the joints; According to the optimal velocity error variables of the ends of the multiple branch structures, the joint angles of the joints at the current moment, and the Cartesian velocities of the ends of the multiple branch structures at the next moment, the second objective function of the tree-like robot is solved using the preset motion constraints of the joints to obtain the joint angular velocities of the joints at the next moment, wherein the second objective function is an optimization function with the goal of minimizing the sum of the joint angle errors of the joints; The joint angles of the joints at the next moment are obtained according to the joint angular velocities of the joints at the next moment and the joint angles at the current moment.

3. The method according to claim 1, characterized in that The obtaining, according to the preset terminal tracking trajectories of the plurality of branch structures, respectively the Cartesian velocities of the terminals of the plurality of branch structures at the next moment, comprises: Differentiating a preset terminal tracking trajectory of each branch structure to obtain velocity information and rotation speed information of the terminal of each branch structure at the next moment as the Cartesian velocity of the terminal of each branch structure at the next moment; Among them, the speed information includes: the speed component of the end of each branch structure at the origin of the end coordinate system at the next moment relative to each coordinate axis in the preset base coordinate system, and the rotational speed information includes: the rotational speed component of the end of each branch structure at the end coordinate system at the next moment around each coordinate axis in the preset base coordinate system.

4. The method according to claim 1, wherein The preset motion constraints of each joint include: preset joint angular velocity constraints of each joint, preset joint angle constraints, and relationship constraints between the joint angular velocity of each joint and the Cartesian velocity of the ends of the multiple branch structures at the next moment.

5. The method according to claim 4, characterized in that If the multiple branch structures include a first branch structure and a second branch structure, the preset joint angular velocity constraint condition is as shown in the following formula: in, Represented as the first i The minimum joint angular velocity of the joint, Expressed as the i The maximum joint angular velocity of the joint, Expressed as the i The joint angular velocity of the joint at the next moment; The preset joint angle constraint condition is shown in the following formula: in, Expressed as the i The minimum joint angle of the joint, Expressed as the i The maximum joint angle of the joint, Expressed as the i The joint angle of the joint at the current moment, Expressed as the time of one cycle; The relationship constraint condition is shown in the following formula: in, It is represented by the first Jacobian matrix corresponding to the chain consisting of the trunk structure and the first branch structure, Represented as 6 lines A zero matrix of columns, Expressed as the joint degrees of freedom of the second branch structure, Represented as 6 lines A zero matrix of columns, Expressed as the joint degrees of freedom of the first branch structure, The first part is represented as the second Jacobian matrix List, The second Jacobian matrix is ​​expressed as Column, the second Jacobian matrix is ​​the Jacobian matrix corresponding to the chain consisting of the trunk structure and the second branch structure, Expressed as the joint degrees of freedom of the backbone structure, It is represented by the Cartesian velocity of the terminal of the multiple branch structures at the next moment, It is represented as the optimal speed error variable at the ends of the plurality of branch structures.

6. The method according to claim 2, characterized in that If the multiple branch structures include a first branch structure and a second branch structure, the first objective function is as shown in the following formula: in, The first terminal of the plurality of branch structures is represented by i The optimal speed error variable, Expressed as the number of said optimal speed error variables; The second objective function is shown in the following formula: in, Represented as the first i The joint angle of the joint at the current moment, Expressed as the i The joint angular velocity of the joint at the next moment, Expressed as the time of one cycle, Expressed as the i The preset motion midpoint of the joint, Expressed as the i The preset range of motion of the joint, Expressed as the joint degrees of freedom of the backbone structure, Expressed as the joint degrees of freedom of the second branch structure, It is represented as the joint degrees of freedom of the first branch structure.

7. A joint angle inverse solution device for a tree-like robot, characterized in that: The tree-like robot includes a trunk structure and a plurality of branch structures connected to the trunk structure, and the device includes: an acquisition module, configured to respectively acquire the Cartesian velocities of the ends of the plurality of branch structures at a next moment according to preset end tracking trajectories of the plurality of branch structures; A conversion module is used to convert the joint angles of the joints on the tree-like robot at the current moment and the preset motion constraints of the joints, using the preset objective function of the tree-like robot, to the Cartesian velocities of the ends of the multiple branch structures at the next moment, so as to obtain the joint angles of the joints at the next moment, wherein the preset objective function is an optimization function with the goal of minimizing the sum of the motion errors of the joints, and the joints on the tree-like robot include the joints on the trunk structure and the joints on the multiple branch structures.

8. A computer device, characterized in that: include: A processor, a storage medium and a bus, wherein the storage medium stores program instructions executable by the processor. When the computer device is running, the processor and the storage medium communicate via the bus, and the processor executes the program instructions to perform the steps of the joint angle inverse solution method of the tree-like robot as described in any one of claims 1 to 6.

9. A computer-readable storage medium, characterized in that The storage medium stores a computer program, and when the computer program is executed by a processor, the steps of the method for inverse solution of joint angles of a tree-like robot according to any one of claims 1 to 6 are executed.

10. A tree-like robot, characterized in that: The tree-like robot comprises: a controller, a base, a trunk structure fixed on the base, and a plurality of branch structures connected to the trunk structure; The controller is connected to each joint on the tree-like robot and is used to execute the steps of the joint angle inverse solution method of the tree-like robot as described in any one of claims 1 to 6.

Images