CN117464667A

CN117464667A - Seven-axis mechanical arm movement planning method and device and mechanical arm

Info

Publication number: CN117464667A
Application number: CN202311300415.6A
Authority: CN
Inventors: 孟李艾俐; 谭志强; 刘婷婷
Original assignee: Bone Shengyuanhua Robot Shenzhen Co ltd
Current assignee: Bone Shengyuanhua Robot Shenzhen Co ltd
Priority date: 2023-10-09
Filing date: 2023-10-09
Publication date: 2024-01-30

Abstract

The embodiment of the application is suitable for the technical fields of surgical robots and mechanical arms, and provides a motion planning method and device for a seven-axis mechanical arm and the mechanical arm, wherein the method comprises the following steps: determining a motion path to be planned of the seven-axis mechanical arm, wherein the motion path consists of a plurality of terminal poses; respectively calculating the inverse solution of the seven-axis mechanical arm under each end pose, wherein the inverse solution is the analytic solution of each joint axis of the seven-axis mechanical arm; respectively determining joint configuration coefficients of the seven-axis mechanical arm under each end pose according to the inverse solution; determining the variation condition of joint configuration coefficients under the current terminal pose and the target terminal pose, wherein the target terminal pose is the terminal pose corresponding to the end point of the motion path; and planning the movement of the seven-axis mechanical arm based on the variation condition of the joint configuration coefficient. By the method, the motion planning can be performed on the seven-axis mechanical arm by utilizing the analytic solution, the planning difficulty is reduced, and the universality of the seven-axis mechanical arm is improved.

Description

Seven-axis mechanical arm movement planning method and device and mechanical arm

Technical Field

The embodiment of the application belongs to the technical field of surgical robots and mechanical arms, and particularly relates to a motion planning method and device for a seven-axis mechanical arm and the mechanical arm.

Background

An important measure of the performance of a mechanical arm is the degree of freedom (DoF) of the joint, the number of axes of what is known as a mechanical scale, e.g., six axes, seven axes. Having more degrees of freedom or axes of the joints means that the robot arm has more flexible working performance. For example, seven-axis robotic arms are more flexible than six-axis robotic arms.

The seven-axis mechanical arm has higher flexibility, so that the body pose of the mechanical arm can be changed under the condition that the tail end position is unchanged, the seven-axis mechanical arm has great advantages in an operation auxiliary scene, and compared with the six-axis mechanical arm in the field of medical auxiliary operation robots, the seven-axis mechanical arm is widely applied. However, the seven-axis mechanical arm belongs to a redundant mechanical arm, and the difficulty is higher when the inverse solution of the seven-axis mechanical arm is calculated and the motion planning is performed. In practical applications, customization is usually required to be specific to a specific scene, and the customized mechanical arms are not general.

Disclosure of Invention

In view of this, the embodiment of the application provides a motion planning method and device for a seven-axis mechanical arm and the mechanical arm, which are used for reducing difficulty in motion planning of the seven-axis mechanical arm and improving universality of the seven-axis mechanical arm.

A first aspect of an embodiment of the present application provides an inversion solution method for a seven-axis mechanical arm, including:

generating an equivalent structure diagram of each joint shaft of the seven-shaft mechanical arm; in the structural equivalence diagram, a first joint axis, a second joint axis and a third joint axis in the joint axes are equivalent to shoulder joints, a fourth joint axis is equivalent to elbow joints, and a fifth joint axis, a sixth joint axis and a seventh joint axis are equivalent to wrist joints;

according to the equivalent structure diagram, determining an arm angle of the seven-axis mechanical arm under the current tail end pose; when the arm angle is 0 degree of the third joint axis, the included angle between a reference plane formed by the shoulder joint, the elbow joint and the wrist joint and a plane actually formed by the shoulder joint, the elbow joint and the wrist joint is formed;

respectively determining joint configuration coefficients of the joint shafts, wherein the joint configuration coefficients are used for representing the areas of the joint shafts in a base coordinate system of the seven-shaft mechanical arm;

and respectively calculating the inverse solutions of the joint axes according to the tail end pose, the arm angle and the joint configuration coefficient.

A second aspect of the embodiments of the present application provides a motion planning method for a seven-axis mechanical arm, including:

Determining a motion path to be planned of a seven-axis mechanical arm, wherein the motion path consists of a plurality of tail end poses;

respectively calculating the inverse solution of the seven-axis mechanical arm under each tail end pose, wherein the inverse solution is the analytic solution of each joint axis of the seven-axis mechanical arm;

respectively determining joint configuration coefficients of the seven-axis mechanical arm under each tail end pose according to the inverse solution;

determining the change condition of the joint configuration coefficient under the current tail end pose and the target tail end pose, wherein the target tail end pose is the tail end pose corresponding to the end point of the motion path;

and performing motion planning on the seven-axis mechanical arm based on the variation condition of the joint configuration coefficient.

A third aspect of the embodiments of the present application provides an inversion device of a seven-axis mechanical arm, including:

the generating module is used for generating an equivalent structure diagram of each joint shaft of the seven-shaft mechanical arm; in the structural equivalence diagram, a first joint axis, a second joint axis and a third joint axis in the joint axes are equivalent to shoulder joints, a fourth joint axis is equivalent to elbow joints, and a fifth joint axis, a sixth joint axis and a seventh joint axis are equivalent to wrist joints;

The arm angle determining module is used for determining the arm angle of the seven-axis mechanical arm under the current tail end pose according to the equivalent structure diagram; when the arm angle is 0 degree of the third joint axis, the included angle between a reference plane formed by the shoulder joint, the elbow joint and the wrist joint and a plane actually formed by the shoulder joint, the elbow joint and the wrist joint is formed;

the joint configuration coefficient determining module is used for determining joint configuration coefficients of the joint shafts respectively, wherein the joint configuration coefficients are used for representing areas of the joint shafts in a base coordinate system of the seven-axis mechanical arm;

and the calculation module is used for calculating the inverse solution of each joint axis according to the tail end pose, the arm angle and the joint configuration coefficient.

A fourth aspect of the embodiments of the present application provides a motion planning apparatus for a seven-axis mechanical arm, including:

the motion path determining module is used for determining a motion path to be planned of the seven-axis mechanical arm, and the motion path consists of a plurality of tail end poses;

the calculation module is used for respectively calculating the inverse solutions of the seven-axis mechanical arm under the tail end pose, wherein the inverse solutions are the analytic solutions of the joint axes of the seven-axis mechanical arm;

The joint configuration coefficient determining module is used for respectively determining joint configuration coefficients of the seven-axis mechanical arm under each tail end pose according to the inverse solution;

the coefficient change determining module is used for determining the change condition of the joint configuration coefficient under the current tail end pose and the target tail end pose, wherein the target tail end pose is the tail end pose corresponding to the end point of the motion path;

and the planning module is used for planning the movement of the seven-axis mechanical arm based on the variation condition of the joint configuration coefficient.

A fifth aspect of the embodiments of the present application provides a robot arm, where the robot arm is a seven-axis robot arm, and the inverse solution of the robot arm may be calculated by the method described in the first aspect, and/or the movement path of the robot arm may be planned by the method described in the second aspect.

A sixth aspect of embodiments of the present application provides a computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, the processor implementing the method according to the first and/or second aspects described above when executing the computer program.

A seventh aspect of embodiments of the present application provides a computer readable storage medium storing a computer program which, when executed by a processor, implements a method as described in the first and/or second aspects above.

An eighth aspect of embodiments of the present application provides a computer program product which, when run on a computer, causes the computer to perform the method of the first and/or second aspect described above.

Compared with the prior art, the embodiment of the application has the following advantages:

according to the embodiment of the application, the solution of the mechanical arm can be solved by using the analytic solution, and the precision of the solved solution of the mechanical arm is improved. And the motion planning of the mechanical arm is performed based on the inverse solution with higher precision, so that the precision of the motion planning is improved. Compared with a seven-axis mechanical arm which is required to be customized for a special scene in the prior art, the inversion solution method and the motion planning method provided by the embodiment of the application are applied to the seven-axis redundant mechanical arm of the human-like arm, so that the seven-axis redundant mechanical arm has higher universality and can be suitable for various scenes.

Drawings

In order to more clearly illustrate the technical solutions in the embodiments of the present application, the following will briefly introduce the drawings that are required to be used in the embodiments or the description of the prior art. It is apparent that the drawings in the following description are only some embodiments of the present application, and that other drawings may be obtained from these drawings without inventive effort for a person of ordinary skill in the art.

Fig. 1 is a schematic structural diagram of a seven-axis mechanical arm according to an embodiment of the present application;

fig. 2 is an equivalent structural diagram of a seven-axis mechanical arm provided in an embodiment of the present application;

FIG. 3 is a schematic illustration of a variation of an articulation axis provided in an embodiment of the present application;

FIG. 4 is a schematic diagram of a correspondence between sub-regions of a process arm corner region and different joint configuration coefficients according to an embodiment of the present disclosure;

FIG. 5 is a schematic diagram of a corner region and corner planning trend according to an embodiment of the present disclosure;

FIG. 6 is a schematic illustration of the effect of an arm angle plan on joint axes provided in an embodiment of the present application;

FIG. 7 is a schematic illustration of the effect of an arm angle plan traversing singular locations on joint axes provided in an embodiment of the present application;

fig. 8 is a schematic diagram of an inversion solution method of a seven-axis mechanical arm according to an embodiment of the present application;

Fig. 9 is a schematic diagram of a motion planning method of a seven-axis mechanical arm according to an embodiment of the present application;

fig. 10 is a schematic diagram of a motion planning procedure of a seven-axis mechanical arm according to an embodiment of the present application;

fig. 11 is a schematic diagram of an inversion device of a seven-axis mechanical arm provided in an embodiment of the present application;

fig. 12 is a schematic diagram of a motion planning device for a seven-axis mechanical arm according to an embodiment of the present application;

fig. 13 is a schematic diagram of a computer device according to an embodiment of the present application.

Detailed Description

In the following description, for purposes of explanation and not limitation, specific details are set forth, such as particular system configurations, techniques, etc. in order to provide a thorough understanding of the embodiments of the present application. It will be apparent, however, to one skilled in the art that the present application may be practiced in other embodiments that depart from these specific details. In other instances, detailed descriptions of well-known systems, devices, circuits, and methods are omitted so as not to obscure the description of the present application with unnecessary detail.

The mechanical arm inversion solution is to reversely solve the angle value of each joint axis of the mechanical arm according to the position and the gesture (the tail end gesture) of the end effector of the mechanical arm, so as to realize the accurate control of the mechanical arm.

The inverse solution of the mechanical arm mainly comprises numerical solution and analytical solution. For the six-axis mechanical arm, because the six-axis mechanical arm can solve the problem of the inverse solution by using the analytic solution conveniently because the rest positions except the singular positions are unique solutions in the process of solving the six-axis mechanical arm from the pose of the tail end of the mechanical arm. In some medical assistance scenarios, in order not to cause excessive disturbance to the doctor's surgical procedure, the elbow joint position of the robotic arm needs to be adjusted while the distal end of the robotic arm remains stationary, which is not possible with a six-axis robotic arm. Therefore, in order to meet the cooperation requirement, the mechanical arm applied to the medical operation robot is mostly designed into a joint redundant mechanical arm, such as a seven-axis mechanical arm, except for a few six-axis mechanical arms designed according to a special scene. However, in the process of resolving, inverting and motion planning by using the analytic solution, the seven-axis mechanical arm can not obtain a unique solution by using only a single parameter, namely the pose of the tail end of the mechanical arm, which requires introducing additional constraint parameters for constraint. These additional constraint parameters need to be determined according to the corresponding scene requirements, which increases the difficulty in the motion planning process. Therefore, in the prior art, a seven-axis mechanical arm applied to a medical operation robot is mostly solved by using a fitting iterative mode such as numerical solution. In practical applications, the numerical solution generally approximates the jacobian matrix at the current time with the jacobian matrix at the previous time, which causes tracking accuracy errors of the mechanical arm. In order to reduce the error, the idea of feedback control can be introduced in a seven-axis mechanical arm, but this also reduces part of the error only to a certain extent, with limited effect.

Aiming at the problems, the embodiment of the application provides a motion planning method of a seven-axis mechanical arm, which can solve the inverse solution of the mechanical arm by using an analytic solution, and improves the precision of the solved inverse solution. And the motion planning of the mechanical arm is performed based on the high-precision inverse solution, so that the precision of the motion planning is improved. The motion planning method of the seven-axis mechanical arm provided by the embodiment of the application is applied to the medical operation robot, and the accuracy of the operation process can be ensured.

The technical scheme of the present application is described below by specific examples.

With reference to figure 1 of the drawings,a schematic structural diagram of a seven-axis mechanical arm provided in an embodiment of the present application is shown. The seven-axis mechanical arm in fig. 1 is a redundant mechanical arm of a human-like arm, and comprises seven joint axes, namely a first joint axis theta in fig. 1 ₁ Second joint axis theta ₂ Third joint axis θ ₃ Fourth joint axis θ ₄ Fifth joint axis θ ₅ A sixth joint axis theta ₆ And a seventh joint axis theta ₇ . Xyz shown at each joint axis position represents the joint coordinates of the joint axis at the corresponding position. In FIG. 1, d _bs Representing the origin of the mechanical arm base coordinate system to the second joint axis theta ₂ D is the distance of the origin of coordinates _se Representing the second joint axis theta ₂ From the origin of coordinates to the fourth joint axis theta ₄ D is the distance of the origin of coordinates _ew Representing the fourth joint axis theta ₄ From the origin of coordinates to the sixth joint axis theta ₆ D is the distance of the origin of coordinates _wf Representing the sixth joint axis theta ₆ From the origin of coordinates to the seventh joint axis theta ₇ Is the distance of the origin of coordinates of (a). In the description of the embodiment of the present application, the first joint axis θ ₁ Second joint axis theta ₂ Third joint axis θ ₃ Fourth joint axis θ ₄ Fifth joint axis θ ₅ A sixth joint axis theta ₆ And a seventh joint axis theta ₇ May also be referred to as shaft 1, shaft 2, shaft 3, shaft 4, shaft 5, shaft 6 and shaft 7.

In order to calculate the inverse solution of the mechanical arm more conveniently and rapidly, the embodiment of the application can solve the inverse solution through structural equivalent conversion by utilizing the equivalent structure diagram of each joint shaft.

Fig. 2 is an equivalent structural diagram of a seven-axis mechanical arm according to an embodiment of the present application. In the structural equivalent diagram shown in fig. 2, the first joint axis, the second joint axis and the third joint axis in the respective joint axes of the seven-axis mechanical arm may be equivalent to one shoulder joint, that is, the first joint axis θ in fig. 1 ₁ Second joint axis theta ₂ Third joint axis θ ₃ Can be equivalently the shoulder joint Ps in fig. 2; the fourth joint axis of the seven-axis arm can be equivalent to an elbow joint, i.e., the fourth joint axis in fig. 1 θ ₄ May be equivalently the elbow joint Pe in fig. 2; the fifth joint axis, the sixth joint axis and the seventh joint axis of the seven-axis mechanical arm can be equivalent to one wrist joint, namely the fifth joint axis theta in fig. 1 ₅ A sixth joint axis theta ₆ And a seventh joint axis theta ₇ May be equivalently the wrist Pw of fig. 2.

As can be seen in fig. 1 and 2, the shoulder joint and the wrist joint are gyroscope joints, and the elbow joint is a single joint. A single joint is understood to mean a single joint, which has only one axis, and correspondingly only one degree of freedom. A gyroscope joint, also called an omni-directional joint, has multiple joints, as opposed to a single joint, such a combined joint typically having two to three degrees of freedom. For example, the shoulder joint Ps in fig. 2 is the first joint axis θ ₁ Second joint axis theta ₂ Third joint axis θ ₃ One axis after the equivalent, the equivalent axis has three degrees of freedom, namely the equivalent axis can rotate around x, y and z axes. Similar to the shoulder joint Ps, the wrist joint Pw in fig. 2 is a fifth joint axis θ ₅ A sixth joint axis theta ₆ And a seventh joint axis theta ₇ An equivalent axis, which also has three degrees of freedom, can rotate about the x, y, and z axes.

The reference plane shown in fig. 2 is at the third joint axis θ ₃ At 0 degrees, a plane formed by the shoulder joint Ps, the elbow joint Pe, and the wrist joint Pw, and an angle between the reference plane and a plane (arm plane) actually formed by the shoulder joint Ps, the elbow joint Pe, and the wrist joint Pw in the equivalent structural diagram is referred to as an arm angle of the seven-axis arm, that is, an arm angle ψ in fig. 2.

As can be seen from the equivalent structure diagram in fig. 2, after the end pose position of the seven-axis mechanical arm is determined, an infinite number of solutions can be obtained by setting different values of the arm angle ψ.

In the process of solving the inverse solution by using the equivalent structure diagram, the end pose position of the seven-axis mechanical arm is generally known. As can be seen by combining an equivalent structure diagram, the inverse solution solving process of the seven-axis mechanical arm not only needs to determine the tail end pose position, but also needs to determine the arm angle psi and the joint configuration coefficient GC _i (i＝2,4,6) The inverse solution thus obtained can be made the only solution. The equations used to solve the inverse solutions for the respective joint axes are described below.

For the fourth joint axis theta ₄ The fourth joint axis θ can be calculated using the following formula ₄ Is the inverse of (a):

wherein ψ is the arm angle, P ₂ ⁶ Is the vector from the shoulder joint to the wrist joint in the equivalent structure diagram, d _se The arm length of the big arm from the shoulder joint to the elbow joint, d _ew For forearm arm length from elbow joint to wrist joint, GC ₄ Is the fourth joint axis theta ₄ Joint configuration coefficients of (a). Fourth joint axis θ ₄ Can be used to determine whether the fourth joint axis is in the positive half or the negative half of the X-axis of the base coordinate system. In particular, when GC ₄ When=1, the calculated value of the fourth joint axis will be in the positive half of the X-axis of the base coordinate system, when GC ₄ At = -1, the value of the fourth joint axis calculated will be in the negative half of the X-axis of the base coordinate system. In practical use, GC ₄ The value of (2) may be determined from the working space of the robot arm. Illustratively, in a medical surgical robot-assisted scenario, to maximize the working space of the robotic arm, a GC may be taken ₄ ＝-1。

For the first joint axis theta ₁ Third joint axis θ ₃ Fifth joint axis θ ₅ And a seventh joint axis theta ₇ The inverse solution for each joint axis can be derived using the following formula:

in the above formula, when i=1 or 3, k=2, which means that the first joint axis θ is calculated ₁ Or a third joint axis theta ₃ GC in its calculation formula _k The second joint axis θ may be used ₂ Is of joint arrangementNumber, i.e. GC ₂ Is a value of (2); when i=5 or 7, k=6, which means that the fifth joint axis θ is calculated ₅ Or seventh joint axis theta ₇ GC in its calculation formula _k The sixth joint axis θ may be used ₆ Joint configuration coefficient of (2), i.e. GC ₆ Is a value of (2). A in the above formula _n 、b _n 、c _n 、a _d 、b _d 、c _d Is at a known end pose position and a fourth joint axis theta ₄ Joint configuration coefficient GC of (2) ₄ In the case of (2), let the third joint axis theta ₃ 0 degrees, a parameter derived by a geometric formula. Thus, a in the above formula _n 、b _n 、c _n 、a _d 、b _d 、c _d Are known.

For the second joint axis theta ₂ And a sixth joint axis theta ₆ The inverse solution can be solved using the following formula:

wherein when i=2, k=2, when i=6, k=6, a, b, c are based on the end pose phase and the joint configuration coefficient GC ₄ The equations of a, b, c are also known.

It follows that, among the seven joint axes of the seven-axis robot arm, the fourth joint axis θ ₄ Can be solved by the terminal pose phase and given GC ₄ Directly find out the rest of the first joint axis theta ₁ Second joint axis theta ₂ Third joint axis θ ₃ Fifth joint axis θ ₅ A sixth joint axis theta ₆ And a seventh joint axis theta ₇ There is also a need to determine GC _k The value of (k=2, 6) and the value of the arm angle ψ can be solved.

It should be noted that, since each joint axis of the seven-axis mechanical arm has a respective limit angle, some arm angles ψ may cause some joint axes to exceed the limit positions of the joint axes. Therefore, in the case where the limit angle jointsL of each joint axis is known, in order to solve the inverse solution of each joint axis, a feasible arm angle region at the end pose phase needs to be determined, which can provide a reference for judgment of the subsequent solving result and motion path planning.

In the embodiment of the application, the feasible arm angle area ψ of a seven-axis mechanical arm _range，all Can be determined by the following formula:

ψ _range,all ＝ψ _range,1 ∩ψ _range,2 ∩…∩ψ _range,7

wherein ψ is _range，1 ～Ψ _range，7 In the case where the limiting angles of the respective joint axes are determined, the GC is performed with respect to the distal pose position ₂ ,GC ₆ The feasible arm angle collection area of the joint axis is obtained by analyzing the condition of the corresponding limit arm angle psi at the limit angle position of each joint axis and sorting and combining. After the feasible arm angle collection area of each joint axis is determined, the feasible arm angle area ψ suitable for each joint axis can be obtained by taking the intersection _range，all . In the feasible arm angle area ψ _range，all In the range, any value of the arm angle psi is taken, and the condition that a certain joint axis exceeds the limit of the joint axis does not occur.

From the above description, it is also necessary to determine GC given the end pose position _k The value of (k=2, 6) and the value of the arm angle ψ are calculated to obtain the angle value θ of each joint axis _i (i=1 to 7). In practical applications, the parameter of arm angle ψ is mainly used for collision avoidance. In the surgical robot-assisted scenario, the arm angle ψ may be made to continuously approach a value, e.g. to be _approach =0, while ensuring that the joint axes cannot vary too much. In the approaching process, the change of the arm angle ψ is not more than 3 degrees per 100 mm of the Cartesian distance of the target end pose from the current end pose or 10 degrees of the rotation vector of the joint axis can be set.

Of course, the arm angle target value ψ _target Not necessarily equal to 0, the target value of the arm angle ψ may be dependent on a particular scenario, such as the trend ψ _approach = -pi/4, this angle allows the physician to have a larger working space and will notLeaving the joint axis beyond the limit. The rate of change of the arm angle ψ during motion planning may also be scene dependent. In summary, the approach value ψ _approach And arm angle change rate ψ _v As the opening parameter, experimental determination can be performed according to the corresponding scene. At the arm angle ψ of the current phase with these two parameters _curr The arm angle psi of the target tail end pose, namely the target post, can be determined _target 。

At arm angle ψ of target post determination _target Then, the arm angle ψ needs to be determined _target Whether or not in the aforementioned viable arm angle region ψ _range，all And (3) inner part. If the arm angle ψ _target Not in the aforementioned possible arm angle region ψ _range，all In this case, the closest arm angle value may be taken as the arm angle value of the target phase.

In the examples of the present application, GC _k (k=2, 6) can include four combinations, GC ₂ ＝1,GC ₆ ＝1，GC ₂ ＝1,GC ₆ ＝-1，GC ₂ ＝-1,GC ₆ ＝1，GC ₂ ＝-1,GC ₆ = -1. At arm angle ψ of target post determination _target In the case of (2), θ can be obtained by inverting the above four combinations _i (i=1 to 7), and then performing weighted least square with the current angle value of each joint axis to determine the optimal GC _k (k=2, 6), i.e. a set of GCs corresponding to weighted least squares minima _k (k＝2,6)。

When weighted least squares is performed, the weights of the respective joint axes may be determined according to actual application needs. Illustratively, in some surgical robot-assisted scenarios, the first joint axis θ is of greater concern ₁ And a third joint axis theta ₃ When the weight of each joint axis is set, the first joint axis θ may be set to ₁ And a third joint axis theta ₃ Is set to be relatively larger and for other joint axes, may be set to be relatively smaller. In this way, the first joint axis θ with a larger weight can be ensured ₁ And a third joint axis theta ₃ Does not change much in position.

In the embodiment of the application, in order to realize the motion planning of the seven-axis mechanical arm, the arm angle process planning is also needed in the process. The arm angle process planning refers to planning the arm angle at each end pose position in the process after determining a motion path. For a seven-axis mechanical arm, the process planning of the arm angle can avoid the condition that each joint axis shakes greatly when passing through the singular position. One motion path can be considered to be composed of a plurality of end poses. The singular position of the mechanical arm refers to that under certain postures, the mechanical arm cannot continue to move to the expected position and posture due to the limitation of the length of the connecting rod, the freedom of movement and other factors, namely, the condition that no solution exists or the solution does not exist. In this case, a special position or movement state of some links or joint shafts of the robot arm occurs, so that control of the robot arm becomes extremely difficult or impossible.

As shown in FIG. 3, a schematic diagram of a variation of the joint axis according to the embodiment of the present application is shown, and three curves in FIG. 3 each describe a fifth joint axis θ that varies with time ₅ Is a variation of (2). As can be seen from FIG. 3, when crossing a singular position, if the fifth joint axis θ ₅ Is the change process shown by line 1, the joint axis will jump greatly; in the case of the line 2 change, the fifth joint axis θ ₅ A small amplitude jump will occur; if the fifth joint axis theta ₅ Is a change shown by line 3 in fig. 3, and there is little jump in this process, and the singular position can be traversed smoothly. Therefore, the purpose of arm angle process planning is to ensure that an effective arm angle is planned, so that the changing process of each joint axis can achieve the same effect as line 2 or even line 3 in fig. 3.

The condition that the joint axes of the mechanical arm have large jitter variation occurs when crossing the singular position, namely when the second joint axis theta is moved ₂ And/or sixth joint axis θ ₆ Occurs across zero crossing locations. In the mechanical arm, the angle range of each joint axis is-180 degrees to 180 degrees, and the zero position of the joint axis is the angle of the joint axis The position where the value is 0 degrees. Exemplary if the current axis case is GC ₂ ＝1,GC ₆ =1, the case of the target axis is GC ₂ ＝1,GC ₆ = -1, indicating a sixth joint axis θ ₆ Will change from positive to negative during movement, which will necessarily result in crossing zero-crossing positions.

In the course of the arm angle process planning, the process arm angle region ψ can be determined _region . Process arm corner region ψ _region Refers to all feasible arm angle areas ψ corresponding to a plurality of end poses on a motion path in the planning process _range，all A set of components. Due to GC _k (k=2, 6) can extend over 4 combinations, thus the process arm corner region ψ _region It may also consist of 4 sub-regions, namely:

ψ _region ＝ψ _region1 +ψ _region2 +ψ _region3 +ψ _region4

wherein each subregion and GC _k The corresponding situation of (k=2, 6) can be as shown in fig. 4. I.e. GC ₂ ＝1,GC ₆ The sub-region corresponding to =1 is ψ _region1 ，GC ₂ ＝1,GC ₆ The sub-region corresponding to = -1 is ψ _region2 ，GC ₂ ＝-1,GC ₆ The sub-region corresponding to =1 is ψ _region3 ，GC ₂ ＝-1,GC ₆ The sub-region corresponding to = -1 is ψ _region4 . The above-mentioned viable arm angle subregion ψ _region1 、Ψ _region2 、Ψ _region3 、Ψ _region4 There are intersecting end points between two. Illustratively, as shown in FIG. 5, at point b in FIG. 5, is a viable arm angle region ψ _region1 And psi is _region2 The intersection end points of the two sub-regions. The number of intersecting endpoints of two sub-regions and GC _k The variation of (k=2, 6) is relevant. If only one joint configuration coefficient changes, e.g. only GC ₂ Or GC ₆ If the change occurs, an intersecting endpoint exists between the two sub-areas; if both joint configuration coefficients change, e.g. GC ₂ And GC ₆ Both change, there are two intersecting endpoints for the two sub-regions. The above-mentioned intersecting endThe point may represent the arm angle herein allowing the second joint axis θ ₂ Or a sixth joint axis theta ₆ Can approach 0 in both the respective positive and negative arm angle regions. Illustratively, as shown in FIG. 5 at point b, this point may achieve a sixth joint axis θ ₆ Can approach 0 in both positive and negative arm angle regions, and can realize the sixth joint axis theta ₆ The crossing of the singular positions is performed smoothly.

As can be seen in conjunction with fig. 5, during the traversing process shown in fig. 5, the time is less than t ₁ When the arm angle is in the A subarea, the time is greater than t ₁ When the arm angle is in the B subarea, thereby realizing the sixth joint axis theta ₆ From region ψ _region1 Traversing to region ψ _region2 The object of (i) is to realize the sixth joint axis theta ₆ Joint configuration coefficient GC of (2) ₆ Transition from positive to negative. During the transition, the other joint shafts are not subjected to severe shaking.

In the embodiment of the application, when the arm angle process planning is performed, the arm angle in the whole motion process needs to be planned besides the arm angle when crossing the intersection end point. If there is a bad arm angle planning in the process, the reciprocating motion of some joint shafts can be caused. Illustratively, as shown in fig. 6, a schematic diagram of an effect of arm angle planning on joint axes is provided in an embodiment of the present application. In FIG. 6, a phase ₁ ，……pose _n The indicated line represents the arm angle ψ versus a certain joint axis for each instant. At slave point ₁ Planning to post _n Line 1 in figure 6 will result in a fifth joint axis theta ₅ First overrun the target position and then fall back to the target position. Thus, the ideal effect should be as shown by line 2 or line 3 in fig. 6. Planning the arm angle according to line 2 in fig. 6 can realize the deceleration movement of the mechanical arm when the mechanical arm reaches the target position, thus ensuring the safety of the movement. At the same time, the parabolic angle in line 2 also determines the rate of change of the arm angle from the axis angle, which can be determined from the maximum speed of the corresponding axis of the mechanical arm.

Arm angle gauge when intersection end points appearThe parabola formed by the drawing needs to consider the intersection end point. Fig. 7 is a schematic diagram of an influence of an arm angle plan crossing a singular position on a joint axis, where a point B in fig. 7 is an intersecting endpoint. In practical application, the first joint axis theta of the seven-axis mechanical arm ₁ Third joint axis θ ₃ And a fifth joint axis theta ₅ The normal motion of the mechanical arm is greatly influenced when the reciprocating motion occurs, so that the arm angle planning can be performed only on the first joint axis theta ₁ Third joint axis θ ₃ And a fifth joint axis theta ₅ And respectively planning arm angles. Thus, three corner planning routes can be obtained. An optimal arm angle planning route can be selected from the three routes, and the arm angle planning route can simultaneously ensure the first joint axis theta ₁ Third joint axis θ ₃ And a fifth joint axis theta ₅ No reciprocating motion occurs. Illustratively, the final determined boom angle planned route may be as shown by line 1 in fig. 5.

Based on the algorithm introduced above, the embodiment of the application provides an inversion solution method and a motion planning method for a seven-axis mechanical arm. Fig. 8 is a schematic diagram of an inversion solution method of a seven-axis mechanical arm according to an embodiment of the present application, where the method specifically may include the following steps:

s801, generating an equivalent structure diagram of each joint shaft of the seven-shaft mechanical arm; in the structural equivalence diagram, a first joint axis, a second joint axis and a third joint axis in the joint axes are equivalent to shoulder joints, a fourth joint axis is equivalent to elbow joints, and a fifth joint axis, a sixth joint axis and a seventh joint axis are equivalent to wrist joints.

The method can be applied to the mechanical arm control device, namely the execution main body of the method can be the mechanical arm control device, and the mechanical arm control device can solve the inverse solution of the seven-axis mechanical arm by executing each step in the method, wherein the inverse solution is an analytic solution. The mechanical arm control device may be a device, a unit or a module independent of the mechanical arm, or may be a device integrated with the mechanical arm, which is a unit or a module of the mechanical arm.

The structure of the seven-axis mechanical arm in the embodiment of the application may be shown in fig. 1, and an equivalent structure diagram obtained by performing equivalent processing on each joint axis of the seven-axis mechanical arm in fig. 1 may be shown in fig. 2. The first joint shaft, the second joint shaft and the third joint shaft in each joint shaft of the seven-shaft mechanical arm are equivalent to a shoulder joint, the fourth joint shaft is equivalent to an elbow joint, and the fifth joint shaft, the sixth joint shaft and the seventh joint shaft are equivalent to a wrist joint.

S802, determining an arm angle of the seven-axis mechanical arm under the current tail end pose according to the equivalent structure diagram; and when the arm angle is 0 degree of the third joint axis, the included angle between a reference plane formed by the shoulder joint, the elbow joint and the wrist joint and a plane actually formed by the shoulder joint, the elbow joint and the wrist joint is formed.

In the embodiment of the application, the arm angle of the seven-axis mechanical arm may be an included angle between a reference plane in the equivalent structural diagram and a plane formed by an actual shoulder joint, an elbow joint and a wrist joint. The reference plane is a plane formed by the shoulder joint, the elbow joint and the wrist joint when the third joint axis is 0 degree in the equivalent structural diagram. For example, arm angle ψ in fig. 2.

S803, respectively determining joint configuration coefficients of the joint shafts, wherein the joint configuration coefficients are used for representing the areas of the joint shafts in a base coordinate system of the seven-shaft mechanical arm.

The joint configuration coefficients of the respective joint axes may be used to represent the region in which each joint axis is located in the base coordinate system. In particular, the joint configuration coefficients may represent whether the joint axis is in the positive half or the negative half of the X-axis in the base coordinate system.

GC can be used _i To represent the joint configuration coefficients of the respective axes. Where i=1, …,7, respectively represent the first to seventh joint axes on the seven-axis robot arm.

In the embodiment of the application, when a certain joint axis is in the positive half of the X axis of the base coordinate system, the joint axis canTo determine the joint configuration coefficient of the joint axis to be 1, i.e. GC _i =1; when a certain joint axis is in the negative half region of the X axis of the base coordinate system, the joint configuration coefficient of the joint axis can be determined to be-1, namely GC _i ＝-1。

In one possible implementation of the embodiment of the present application, the joint configuration coefficients of the first joint axis and the third joint axis may use the configuration coefficient of the second joint axis, that is, GC ₁ ＝GC ₂ ＝GC ₃ The method comprises the steps of carrying out a first treatment on the surface of the The joint configuration coefficients of the fifth joint axis and the seventh joint axis may use the configuration coefficient of the sixth joint axis, that is, GC ₅ ＝GC ₆ ＝GC ₇ . Therefore, in determining the joint arrangement coefficients of the respective joint axes, only the joint arrangement coefficients of the second joint axis, the fourth joint axis, and the sixth joint axis, that is, GC may be determined _i (i=2, 4, 6).

S804, respectively calculating inverse solutions of the joint axes according to the tail end pose, the arm angle and the joint configuration coefficient.

In the embodiment of the present application, in order to calculate the inverse solution of each joint axis, various combinations of joint configuration coefficients may be first determined. Wherein, among the joint arrangement coefficients of different combinations, the joint arrangement coefficient GC of the second joint axis ₂ And joint configuration coefficient GC of the sixth joint axis ₆ May be the same or different.

Illustratively, GC _k (k=2, 6) four different combinations can be formed, namely: GC (gas chromatography) ₂ ＝1,GC ₆ ＝1，GC ₂ ＝1,GC ₆ ＝-1，GC ₂ ＝-1,GC ₆ ＝1，GC ₂ ＝-1,GC ₆ ＝-1。

When the inverse solution of each joint axis is calculated, the inverse solution of each joint axis can be calculated according to the terminal pose, the arm angle and the joint configuration coefficients of various combinations, a plurality of groups of inverse solution sets are obtained, and then the unique inverse solution of each joint axis is determined from the plurality of groups of inverse solution sets. The inverse solution for each axis may be calculated using a different formula.

In this embodiment, for the fourth joint axis, a plurality of intermediate variables may be determined according to an equivalent structural diagram, where the intermediate variables may include a vector from a shoulder joint to a wrist joint, a large arm length from the shoulder joint to the elbow joint, and a small arm length from the elbow joint to the wrist joint in the equivalent structural diagram. And then, according to the tail end pose, the arm angle, the joint configuration coefficient of the fourth joint axis and a plurality of intermediate variables, calculating to obtain the inverse solution of the fourth joint axis.

Specifically, the inverse solution of the fourth joint axis may be calculated using the following formula:

wherein ψ is the arm angle, P ₂ ⁶ Is the vector from the shoulder joint to the wrist joint in the equivalent structure diagram, d _se The arm length of the big arm from the shoulder joint to the elbow joint, d _ew For forearm arm length from elbow joint to wrist joint, GC ₄ The coefficients are configured for the joints of the fourth joint axis.

For the second joint axis and the sixth joint axis, similar formulas may be used to calculate the respective joint configuration coefficients.

In the embodiment of the application, a plurality of first association parameters for calculating the inverse solutions of the second joint axis and the sixth joint axis may be determined according to the joint configuration coefficients of the end pose and the fourth joint axis. The first associated parameters may include the parameters a, b, c in the foregoing embodiments.

Then, calculating an inverse solution of the second joint axis according to the end pose, the arm angle, the joint configuration coefficient of the second joint axis and a plurality of first association parameters; and calculating an inverse solution of the sixth joint axis according to the tail end pose, the arm angle, the joint configuration coefficient of the sixth joint axis and a plurality of first association parameters.

Specifically, the inverse solutions of the second joint axis and the sixth joint axis may be calculated using the following formulas:

where when i=2, k=2, when i=6, k=6, a, b, c are a plurality of first correlation parameters, and ψ is an arm angle.

Similar formulas may be used to calculate respective joint configuration coefficients for the remaining first, third, fifth, and seventh joint axes.

In the embodiment of the present application, a plurality of second association parameters when the third joint axis is 0 degrees may be determined according to the terminal pose and the joint configuration coefficient of the fourth joint axis. The second association parameter may include a in the foregoing embodiment _n 、b _n 、c _n 、a _d 、b _d 、c _d And the like.

Then, according to the tail end pose, the arm angle, the joint configuration coefficient of the second joint shaft and a plurality of second association parameters, respectively calculating inverse solutions of the first joint shaft and the third joint shaft; and respectively calculating inverse solutions of the fifth joint axis and the seventh joint axis according to the tail end pose, the arm angle, the joint configuration coefficient of the sixth joint axis and a plurality of second association parameters.

wherein when i=1 or 3, k=2, when i=5 or 7, k=6, a _n 、b _n 、c _n 、a _d 、b _d 、c _d For a plurality of second associated parameters, ψ is the arm angle.

In the examples of the present application, due to GC _k (k=2, 6) includes a plurality of coefficient combinations, and the inverse solution calculated for each coefficient combination also includes a plurality of sets of inverse solutions. Thus, a unique inverse solution for each joint axis needs to be determined from the multiple sets of inverse solutions.

In embodiments of the present application, a unique inverse solution may be determined from a plurality of sets of inverse solutions based on a weighted least squares approach. Firstly, the weight value of each joint axis can be determined, then, based on the weight value, each inverse solution set is weighted least square with the angle value of each joint axis under the current terminal pose, and the inverse solution set corresponding to the minimum value of the weighted least square is used as the unique inverse solution of each joint axis.

When determining the weight value of each joint axis, the weight value of each joint axis may be assigned according to the joint axis focused on by the actual application scenario. Specifically, at least one target joint axis of the respective joint axes may be determined, and then weight values of the target joint axis and other non-target joint axes are respectively determined; wherein the weight value of the target joint axis is greater than the weight value of the non-target joint axis. The target joint axis is the joint axis which is focused in the practical application scene. For example, in some surgical robotic assisted medical scenarios, where the use of a seven-axis robotic arm may be more focused on the first and third joint axes, the first and third joint axes may be weighted when the weights of the joint axes are assigned.

According to the embodiment, the unique inverse solution of each joint axis of the seven-axis mechanical arm can be calculated, and the inverse solution is an analytic solution with higher precision. On the basis of the inversion solution method, the embodiment of the application also provides a motion planning method of the seven-axis mechanical arm. Fig. 9 is a schematic diagram of a motion planning method for a seven-axis mechanical arm according to an embodiment of the present application, where the method specifically may include the following steps:

and S901, determining a motion path to be planned of the seven-axis mechanical arm, wherein the motion path consists of a plurality of terminal poses.

The present method may be applied to a robot arm control apparatus, which may be the same as the control apparatus for solving the inverse solution of the robot arm in the foregoing embodiment. That is, the mechanical arm control device can solve the inverse solution of each joint axis according to the method, and on the basis, the motion planning of the mechanical arm is realized by executing each step in the method.

In the embodiment of the present application, the movement path to be planned by the mechanical arm may be composed of a plurality of end poses. In practical application, when the path planning is performed on the mechanical arm, the number of the input terminal poses can be multiple, or only one or two terminal poses can be used, and the terminal poses with different numbers need to be processed respectively. The terminal pose of the input does not include the pose of the starting position.

In the embodiment of the application, after determining the input terminal poses, if the number of terminal poses is less than 3, pose interpolation should be performed based on the terminal poses less than 3, so as to obtain a plurality of terminal poses forming a motion path.

For example, if the input terminal pose is less than 2, that is, there may be only one input terminal pose, and in combination with the pose of the starting position, a plurality of terminal poses may be generated by means of spatial linear pose interpolation, so as to form a motion path to be planned. If the input terminal poses are more than or equal to 2 and less than 3, namely the input terminal poses are 2, a plurality of terminal poses can be generated in a space arc pose interpolation mode to form a motion path to be planned.

S902, respectively calculating the inverse solutions of the seven-axis mechanical arm under the tail end pose, wherein the inverse solutions are analysis solutions of all joint axes of the seven-axis mechanical arm.

The inverse solution of the seven-axis mechanical arm under each end pose may be an analytical solution, where the analytical solution may be calculated by using the method of the inversion solution in the previous embodiment, which is not described herein.

S903, respectively determining joint configuration coefficients of the seven-axis mechanical arm under each terminal pose according to the inverse solution.

The joint configuration coefficient of the fourth joint axis of the seven-axis mechanical arm can be determined according to the working space of the mechanical arm, namely, the GC is determined according to the working space of the mechanical arm ₄ The value of (2) is 1 or-1. Further, the first, second, and third joint axes may use the joint arrangement coefficients of the second joint axis, and the fifth, sixth, and seventh joint axes may use the joint arrangement coefficients of the sixth joint axis, so determining the joint arrangement coefficients of the seven-axis robot arm in each end pose from the inverse solution may determine only the joint arrangement coefficients of the second and sixth joint axes, that is, determine the GC _k (k=2, 6)Values.

S904, determining the change condition of the joint configuration coefficient under the current tail end pose and the target tail end pose, wherein the target tail end pose is the tail end pose corresponding to the end point of the motion path.

In this embodiment of the present application, if the number of end poses forming the motion path of the mechanical arm is n, the target end pose may refer to the nth end pose, that is, the target end pose may be an endpoint or a pose corresponding to the endpoint on the motion path.

According to the method described in S903, the joint configuration coefficient corresponding to the nth end pose, i.e. the target GC, can be determined _k (k=2, 6).

The mechanical arm control device may determine a variation of the joint configuration coefficients of the seven-axis mechanical arm in the current end pose and the target end pose, that is, determine a variation of the joint configuration coefficients of the second joint axis and the sixth joint axis of the seven-axis mechanical arm in the current end pose and the target end pose. Specifically, the GC corresponding to the target end pose can be determined _k A value of (k=2, 6) compared to GC corresponding to the current end pose _k Whether the value of (k=2, 6) changes.

In the examples of the present application, GC _k (k=2, 6) can include four combinations, GC ₂ ＝1,GC ₆ ＝1，GC ₂ ＝1,GC ₆ ＝-1，GC ₂ ＝-1,GC ₆ ＝1，GC ₂ ＝-1,GC ₆ = -1. When GC ₂ Or GC ₆ Any value being changed, e.g. GC under the current end pose ₂ ＝1,GC ₆ =1, GC under target end pose ₂ ＝1,GC ₆ = -1, or GC under target end pose ₂ ＝-1,GC ₆ =1, it can be considered that the joint configuration coefficient in the current end pose and the target end pose changes, that is, zero crossing exists.

And S905, performing motion planning on the seven-axis mechanical arm based on the variation condition of the joint configuration coefficient.

In the embodiment of the application, the method can be based on GC _k Whether the value of (k=2, 6) changesAnd performing motion planning in different modes.

In one possible implementation, if the GC _k The values of (k=2, 6) are unchanged, that is, the joint configuration coefficients of the second joint axis and the sixth joint axis are unchanged, a multithreading mode can be used to determine a feasible arm angle area of the seven-axis mechanical arm under each end pose, and then the movement path of the seven-axis mechanical arm is planned according to the feasible arm angle areas.

In a specific implementation, when determining the feasible arm angle area of the seven-axis mechanical arm under each end pose, the limit angle of each joint axis of the seven-axis mechanical arm under each end pose can be determined first, and then the limit angle of each joint axis is intersected, so that the feasible arm angle area of the seven-axis mechanical arm under each end pose is obtained.

After the feasible arm angle area is determined, an arm angle variable curve of the arm angle of the seven-axis mechanical arm, which changes along with time, can be planned in the feasible arm angle area, and the arm angle process planning is completed. And determining the arm angle of the seven-axis mechanical arm at any moment according to the arm angle variation curve, and calculating to obtain the unique inverse solution of the seven-axis mechanical arm based on the arm angle.

In another possible implementation, if the GC _k When the value of (k=2, 6) is changed, for example, the joint arrangement coefficient of the second joint axis or the sixth joint axis is changed, or the joint arrangement coefficients of the two joint axes are changed simultaneously, the intersection end point of the feasible arm angle area of the seven-axis mechanical arm can be calculated by using a multithreading mode according to the change amount, and the movement path of the seven-axis mechanical arm can be planned according to the intersection end point.

In a specific implementation, due to GC _k The value of (k=2, 6) includes a plurality of combinations, and the joint configuration coefficient GC can be determined when calculating the intersection end point of the viable arm angle region _k (k=2, 6) and determining intersection endpoints of a plurality of feasible arm angle subareas by respectively determining the feasible arm angle subareas corresponding to the seven-axis mechanical arm under joint configuration coefficients of the respective combinations.

Then, an arm angle variation curve of the arm angle of the seven-axis mechanical arm, which varies with time, is planned in each feasible arm angle subarea, and the arm angle variation curve passes through the intersection end points. Thus, the arm angle of the seven-axis mechanical arm at any moment can be determined according to the arm angle variation curve, and based on the arm angle, the unique inverse solution of the seven-axis mechanical arm is calculated.

In the embodiment of the application, based on the inversion solution method and the motion planning method described above, the collision avoidance function can be further added in the process of using the seven-axis mechanical arm. For example, a collision avoidance angle may be determined, from which a viable arm angle region of the robotic arm is limited.

Next, an introduction is made to a motion planning method for a seven-axis mechanical arm according to the embodiments of the present application in combination with a complete example.

Fig. 10 is a schematic diagram of a motion planning procedure of a seven-axis mechanical arm according to an embodiment of the present application. According to the flow shown in fig. 10, at the beginning of the planning, the current planning mode may first be determined, e.g. belonging to a single-step solution mode or a process movement planning mode.

In the single-step resolving mode, the number of target end poses ose input to the robot arm control device may be only one, that is, in the single-step resolving mode, the unique inverse solution of each joint axis may be calculated based on one of the input end poses ose. In the process, the calculated unique inverse solution is also judged by combining the feasible arm angle area of each joint shaft, so that the inverse solution result is ensured to be within the range of the feasible arm angle area.

In the process motion planning mode, for each input end pose, a plurality of end poses can be combined into a motion path to be planned. The terminal pose of the input does not include the pose of the starting position of the mechanical arm. In the process motion planning mode, if the number of input end poses is less than 3, n end poses should be generated by pose interpolation. Specifically, in the case where the number of input end poses is less than 3, if the number of input end poses is less than 2, i.e., the input end poses is only 1, then in combination with the initial position poses, a null can be employed And generating n tail end poses in an inter-linear pose interpolation mode. If the number of the input terminal poses is less than 3 and more than or equal to 2, namely the input terminal poses are only 2, n terminal poses can be generated by adopting a space arc pose interpolation mode. If the number of input end poses is greater than or equal to 3, then the end poses greater than or equal to 3 may be directly taken as n end poses. For n terminal poses, a multithread solving mode can be adopted to calculate a plurality of intermediate variables required by inversion solution. These intermediate variables may be those in the respective formulas in the embodiments corresponding to the inversion solution method described above, such as P ₂ ⁶ ，d _se ，d _ew ，a _n ，b _n ，c _n ，a _d ，b _d ，c _d Variables such as a, b, c, etc. On the other hand, for the n terminal poses of the input, the inverse solution of the n terminal pose can be calculated by adopting the same calculation mode as the single-step calculation mode, and the target GC is determined according to the inverse solution _k (k=2, 6), i.e. GC at the last end pose _k (k=2, 6).

Then, by judging the target GC _k (k=2, 6) and current GC _k The variation of (k=2, 6) can be used in different ways for the subsequent motion planning procedure.

The inversion solution method and the motion planning method for the seven-axis mechanical arm provided by the embodiment of the application are used for carrying out inversion solution and motion planning on the seven-axis redundant mechanical arm of the human-like arm. Compared with a seven-axis mechanical arm which is required to be customized for a special scene in the prior art, the inversion solution method and the motion planning method provided by the embodiment of the application are applied to the seven-axis redundant mechanical arm of the human-like arm, so that the seven-axis redundant mechanical arm has higher universality and can be suitable for various scenes. For example, the inversion solution method and the motion planning method provided by the embodiment of the application are applied to the seven-axis redundant mechanical arm, so that the seven-axis redundant mechanical arm can be applied to different medical equipment to realize multiple functions such as auxiliary operation.

Secondly, according to the embodiment of the application, the solution is used for solving the inverse solution of the seven-axis redundant mechanical arm, so that the accuracy of a solving result can be ensured. In the case of surgical robot assisted medical treatment, the accuracy of surgery is improved. In addition, as the solution and the motion planning are carried out by using the analytic solution, the solution is a result of ingenious space geometric calculation according to the mechanical configuration, and the solution method is more real-time than a numerical solution in a mode of iterative fitting, so that the response speed of the motion planning process based on the inverse solution is also higher, and the method can be applied to the case of teleoperation, which is the capability which cannot be achieved by carrying out the solution in a cyclic iteration mode.

Thirdly, by applying the method to carry out motion planning, the problem that the joint shafts shake relatively much near the zero position due to the fact that the second, fourth and sixth joint shafts of the seven-shaft redundant mechanical arm of the human arm form singular configurations when the joint shafts are at the zero position can be well processed, and continuous smooth planning in the whole motion process can be achieved. In addition, the method further ensures the safety and stability of the motion planning process by limiting the change trend of the arm angle.

Fourth, the method has higher expansibility, and collision avoidance function can be added in the motion planning process, so that the mechanical arm can carry out auxiliary motion planning in a better pose, the universality of the method is further improved, and the application range of the seven-axis redundant mechanical arm is improved.

It should be noted that, the sequence number of each step in the above embodiment does not mean the sequence of execution sequence, and the execution sequence of each process should be determined by its function and internal logic, and should not constitute any limitation on the implementation process of the embodiment of the present application.

Referring to fig. 11, a schematic diagram of an inversion device of a seven-axis mechanical arm provided in an embodiment of the present application may specifically include a generating module 1101, an arm angle determining module 1102, a joint configuration coefficient determining module 1103, and a calculating module 1104, where:

the generating module 1101 is configured to generate an equivalent structure diagram of each joint axis of the seven-axis mechanical arm; in the structural equivalence diagram, a first joint axis, a second joint axis and a third joint axis in the joint axes are equivalent to shoulder joints, a fourth joint axis is equivalent to elbow joints, and a fifth joint axis, a sixth joint axis and a seventh joint axis are equivalent to wrist joints;

The arm angle determining module 1102 is configured to determine an arm angle of the seven-axis mechanical arm under the current end pose according to the equivalent structure diagram; when the arm angle is 0 degree of the third joint axis, the included angle between a reference plane formed by the shoulder joint, the elbow joint and the wrist joint and a plane actually formed by the shoulder joint, the elbow joint and the wrist joint is formed;

a joint configuration coefficient determining module 1103, configured to determine joint configuration coefficients of the joint axes, where the joint configuration coefficients are used to represent an area where each joint axis is located in a base coordinate system of the seven-axis mechanical arm;

a calculation module 1104 is configured to calculate an inverse solution of each joint axis according to the end pose, the arm angle, and the joint configuration coefficient.

According to the device for solving the inversion of the seven-axis mechanical arm, which is provided by the embodiment of the application, each step in each related method embodiment can be realized by using the device, so that the inversion of the seven-axis mechanical arm is realized.

Referring to fig. 12, a schematic diagram of a motion planning apparatus for a seven-axis mechanical arm provided in an embodiment of the present application may specifically include a motion path determining module 1201, a calculating module 1202, a joint configuration coefficient determining module 1203, a coefficient variation determining module 1204, and a planning module 1205, where:

The motion path determining module 1201 is configured to determine a motion path to be planned for the seven-axis mechanical arm, where the motion path is composed of a plurality of end poses;

a calculation module 1202, configured to calculate an inverse solution of the seven-axis mechanical arm under each of the end poses, where the inverse solution is an analytic solution of each joint axis of the seven-axis mechanical arm;

the joint configuration coefficient determining module 1203 is configured to determine joint configuration coefficients of the seven-axis mechanical arm under each of the end poses according to the inverse solution;

a coefficient variation determining module 1204, configured to determine a variation condition of the joint configuration coefficient under a current end pose and a target end pose, where the target end pose is an end pose corresponding to an endpoint of the motion path;

and a planning module 1205, configured to plan the motion of the seven-axis mechanical arm based on the variation of the joint configuration coefficient.

In one possible implementation manner of the embodiment of the present application, the inverse solution of the seven-axis mechanical arm under each end pose may be calculated by using the method for solving the inverse solution of the seven-axis mechanical arm described in the foregoing method embodiments.

According to the motion planning device for the seven-axis mechanical arm, which is provided by the embodiment of the application, each step in each related method embodiment can be realized, and motion planning for the seven-axis mechanical arm can be realized.

For the device embodiments, since they are substantially similar to the method embodiments, the description is relatively simple, and reference should be made to the description of the method embodiments.

Referring to fig. 13, a schematic diagram of a computer device according to an embodiment of the present application is shown. As shown in fig. 13, a computer device 1300 in an embodiment of the present application includes: a processor 1310, a memory 1320, and a computer program 1321 stored in the memory 1320 and executable on the processor 1310. The processor 1310, when executing the computer program 1321, implements the steps in the above-described method embodiments, such as steps S901 to S905 shown in fig. 9. Alternatively, the processor 1310, when executing the computer program 1321, performs the functions of the modules/units in the above-described apparatus embodiments, for example, the functions of the modules 1201 to 1205 shown in fig. 12.

Illustratively, the computer program 1321 may be partitioned into one or more modules/units that are stored in the memory 1320 and executed by the processor 1310 to complete the present application. The one or more modules/units may be a series of computer program instruction segments capable of performing particular functions in order to describe the execution of the computer program 1321 in the computer device 1300.

The computer device 1300 may be a mechanical arm control device for implementing the steps in the foregoing method embodiments, where the computer device 1300 may be a computing device such as a desktop computer, a cloud server, or the like. The computer device 1300 may include, but is not limited to, a processor 1310, a memory 1320. It will be appreciated by those skilled in the art that fig. 13 is only one example of a computer device 1300 and is not intended to be limiting of the computer device 1300, and may include more or less components than those shown, or may combine certain components, or different components, e.g., the computer device 1300 may also include input and output devices, network access devices, buses, etc.

The processor 1310 may be a central processing unit (Central Processing Unit, CPU), but may also be other general purpose processors, digital signal processors (Digital Signal Processor, DSP), application specific integrated circuits (Application Specific Integrated Circuit, ASIC), off-the-shelf programmable gate arrays (Field-Programmable Gate Array, FPGA) or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, or the like. A general purpose processor may be a microprocessor or the processor may be any conventional processor or the like.

The memory 1320 may be an internal storage unit of the computer device 1300, for example, a hard disk or a memory of the computer device 1300. The memory 1320 may also be an external storage device of the computer device 1300, such as a plug-in hard disk, a Smart Media Card (SMC), a Secure Digital (SD) Card, a Flash memory Card (Flash Card) or the like, which are provided on the computer device 1300. Further, the memory 1320 may also include both internal and external storage units of the computer device 1300. The memory 1320 is used to store the computer program 1321 and other programs and data required by the computer device 1300. The memory 1320 may also be used to temporarily store data that has been output or is to be output.

The embodiment of the application also discloses a mechanical arm which is a seven-axis mechanical arm, wherein the inverse solution of the mechanical arm can be calculated through the inversion solution method of the seven-axis mechanical arm, and/or the motion path of the mechanical arm can be planned through the motion planning method of the seven-axis mechanical arm.

The embodiment of the application also discloses a computer device, which comprises a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the processor realizes the inversion solution method of the seven-axis mechanical arm and/or the motion planning method of the seven-axis mechanical arm when executing the computer program.

The embodiment of the application also discloses a computer readable storage medium, wherein the computer readable storage medium stores a computer program, and the computer program realizes the inversion solution method of the seven-axis mechanical arm and/or the motion planning method of the seven-axis mechanical arm when being executed by a processor.

The embodiment of the application also discloses a computer program product, which when run on a computer, causes the computer to execute the method for solving the inversion of the seven-axis mechanical arm and/or the method for planning the movement of the seven-axis mechanical arm.

The above embodiments are only for illustrating the technical solution of the present application, and are not limiting. Although the present application has been described in detail with reference to the foregoing embodiments, it should be understood by those of ordinary skill in the art that: the technical scheme described in the foregoing embodiments can be modified or some technical features thereof can be replaced by equivalents; such modifications and substitutions do not depart from the spirit and scope of the technical solutions of the embodiments of the present application, and are intended to be included in the scope of the present application.

Claims

1. The motion planning method of the seven-axis mechanical arm is characterized by comprising the following steps of:

2. The method of claim 1, wherein said determining the variation of the joint configuration coefficients at the current end pose and the target end pose comprises:

and determining the variation condition of joint configuration coefficients of a second joint axis and a sixth joint axis of the seven-axis mechanical arm under the current tail end pose and the target tail end pose.

3. The method of claim 2, wherein the motion planning of the seven-axis robotic arm based on the variation of the joint configuration coefficients comprises:

If the joint configuration coefficients of the second joint shaft and the sixth joint shaft are unchanged, determining a feasible arm angle area of the seven-axis mechanical arm under each tail end pose, and planning a motion path of the seven-axis mechanical arm according to the feasible arm angle area;

if the joint configuration coefficients of the second joint axis and/or the sixth joint axis are changed, calculating an intersection endpoint of a feasible arm angle area of the seven-axis mechanical arm, and planning a motion path of the seven-axis mechanical arm according to the intersection endpoint of the feasible arm angle area.

4. A method according to claim 3, wherein said determining viable arm angle regions for the seven-axis robotic arm at each of the end poses comprises:

determining the limit angles of all joint shafts of the seven-shaft mechanical arm under all the tail end pose respectively;

and acquiring intersection of the limit angles of the joint shafts to obtain feasible arm angle areas of the seven-axis mechanical arm under the tail end pose.

5. The method of claim 4, wherein planning the path of motion of the seven-axis robotic arm according to the viable arm angle region comprises:

Planning an arm angle variation curve of the arm angle of the seven-axis mechanical arm, which varies with time, in the feasible arm angle area;

determining the arm angle of the seven-axis mechanical arm at any moment according to the arm angle variation curve;

and calculating the unique inverse solution of the seven-axis mechanical arm based on the arm angle of the seven-axis mechanical arm at any moment.

6. A method according to claim 3, wherein said calculating the intersection end points of the feasible arm angle areas of the seven-axis mechanical arm comprises:

determining a plurality of combinations of the joint configuration coefficients;

respectively determining a feasible arm angle subarea corresponding to the seven-axis mechanical arm under the joint configuration coefficients of each combination;

determining the intersection end points of a plurality of the feasible arm angle subareas.

7. The method of claim 6, wherein planning the path of movement of the seven-axis robotic arm from the intersecting end points of the viable arm angle regions comprises:

planning an arm angle variation curve of the arm angle of the seven-axis mechanical arm, which varies with time, in each feasible arm angle subarea; wherein the arm angle change curve passes through the intersection endpoint;

8. The method of any of claims 3-7, further comprising, prior to planning movement of the seven-axis robotic arm based on the variation in the joint configuration coefficients:

determining a collision avoidance angle;

and limiting the feasible arm angle area according to the collision avoidance angle.

9. The method of any one of claims 1-7, wherein the determining a path of motion to be planned for a seven-axis robotic arm comprises:

determining the terminal pose of the input;

if the number of the terminal poses is smaller than 3, performing pose interpolation based on the terminal poses smaller than 3 to obtain a plurality of terminal poses forming the motion path.

10. A robot arm, which is a seven-axis robot arm, characterized in that the movement path of the robot arm is planned by the seven-axis robot arm movement planning method according to any one of claims 1-9.

11. A computer device comprising a memory, a processor and a computer program stored in the memory and executable on the processor, characterized in that the processor implements the method of motion planning for a seven-axis robot according to any of claims 1-9 when executing the computer program.

12. A computer readable storage medium storing a computer program, wherein the computer program when executed by a processor implements the method of motion planning for a seven-axis robotic arm according to any one of claims 1-9.