Trajectory tracking method and device based on task priority and electronic equipment

Abstract

The embodiment of the invention provides a trajectory tracking method and device based on task priority and electronic equipment. The method comprises the following steps: acquiring an expected movement track of a mechanical arm from an initial position to a target position; multiple sets of joint angles corresponding to all joints at all moments in the movement process of the mechanical arm along the expected movement track are determined; if the obstacle exists at the current moment, determining a first group of target joint angles from multiple groups of joint angles corresponding to the current moment; and if the first group of target joint angles enables the shortest distance from the obstacle to one of the rod pieces of the mechanical arm to be smaller than or equal to the preset distance, determining a second group of target joint angles from the multiple groups of joint angles corresponding to the current moment, and repeating the step until the Kth group of target joint angles is determined, so that the mechanical arm avoids the obstacle, and moving along the expected movement track. According to the embodiment of the invention, when the nearest distance between the obstacle and the rod piece is smaller than the preset distance, the obstacle avoidance motion is considered preferentially.

Description

Technical Field

[0001] The embodiments of the present invention relate to the field of robotic arm technology, and specifically to a trajectory tracking method, device and electronic equipment based on task priority. Background Art

[0002] In the field of modern automation and robotics, redundant robots have attracted widespread attention due to their flexibility and robustness in performing complex tasks.

[0003] Redundant robots have more degrees of freedom than are needed to complete basic tasks, which provides them with additional mobility, allowing them to operate in tight spaces, avoid obstacles, and perform complex movements.

[0004] As a kind of redundant robot, the obstacle avoidance problem of the robotic arm usually only considers the situation when the obstacle affects the movement of the robotic arm end and causes it to deviate from the expected trajectory when the robotic arm end moves along the expected trajectory. However, it ignores the situation when the robotic arm body interferes with the obstacle and the robotic arm deviates from the expected trajectory in order to avoid the obstacle. Summary of the invention

[0005] The embodiments of the present invention provide a task priority-based trajectory tracking method, device and electronic device for solving the problem that when a robot arm body interferes with an obstacle, the robot arm deviates from the expected trajectory in order to avoid the obstacle.

[0006] In a first aspect, an embodiment of the present invention provides a trajectory tracking method based on task priority, comprising:

[0007] Obtain the expected motion trajectory of the robot arm from the initial position to the target position;

[0008] Determine multiple sets of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory;

[0009] If there is an obstacle at the current moment, a first set of target joint angles is determined from multiple sets of joint angles corresponding to the current moment;

[0010] If the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then determine the second group of target joint angles from the multiple groups of joint angles corresponding to the current moment, and repeat this step until the Kth group of target joint angles is determined, so that the robotic arm avoids obstacles and moves along the expected motion trajectory, where K is an integer greater than or equal to 1 and less than or equal to the number of multiple groups of joint angles.

[0011] In a possible implementation, determining a plurality of sets of joint angles corresponding to each joint at each time during the movement of the robot arm along the expected motion trajectory includes:

[0012] Get the joint angle and posture of the end of the robot arm;

[0013] According to the joint angles and postures of the end of the robotic arm, multiple groups of joint angles corresponding to the joints at various moments during the movement of the robotic arm along the expected motion trajectory are determined.

[0014] In a possible implementation, the method further includes:

[0015] If the first group of target joint angles makes the closest distances from the obstacle to each rod of the robotic arm greater than a preset distance, it is determined that the robotic arm moves along the expected motion trajectory.

[0016] In a possible implementation, if there is an obstacle at the current moment, determining a first set of target joint angles from multiple sets of joint angles corresponding to the current moment includes:

[0017] If there is an obstacle at the current moment, the sum of the absolute displacements of each joint of the robotic arm from the initial position to the target position is arranged from small to large to obtain an arrangement result; wherein the sum of the absolute displacements is arranged from small to large in sequence of 1, 2..., L, L is an integer greater than or equal to K and less than or equal to the number of groups of joint angles, and the sum of the absolute displacements is represented by the sum of the differences between the joint angles of each joint of the robotic arm at the target position and the joint angles at the initial position;

[0018] According to the arrangement result, a group of joint angles corresponding to the sum of the target absolute displacements is determined as a first group of target joint angles, and the sum of the target absolute displacements is the sum of the absolute displacements with a sequence number of 1.

[0019] In a possible implementation, if the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then a second group of target joint angles is determined from multiple groups of joint angles corresponding to the current moment, and this step is repeated until the Kth group of target joint angles is determined, so that the robotic arm avoids the obstacle and moves along the expected motion trajectory, including:

[0020] If the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then the sum of the absolute displacements numbered 2 is determined from the multiple groups of joint angles corresponding to the current moment, and this is used as the second group of target joint angles. This step is repeated until the sum of the absolute displacements numbered K is determined as the Kth group of target joint angles, so that the robotic arm avoids obstacles and moves along the expected motion trajectory.

[0021] In a second aspect, an embodiment of the present invention provides a trajectory tracking device based on task priority, comprising:

[0022] An acquisition unit, used for acquiring an expected motion trajectory of the robot arm from an initial position to a target position;

[0023] The first determination unit is used to determine a plurality of groups of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory;

[0024] A second determination unit is used to determine a first set of target joint angles from multiple sets of joint angles corresponding to the current moment if there is an obstacle at the current moment;

[0025] The third determination unit is used to determine the second group of target joint angles from the multiple groups of joint angles corresponding to the current moment if the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, and repeat this step until the Kth group of target joint angles is determined, so that the robotic arm avoids the obstacle and moves along the expected motion trajectory, K is an integer greater than or equal to 1 and less than or equal to the number of multiple groups of joint angles.

[0026] In a possible implementation, the first determining unit is specifically configured to:

[0027] Get the joint angle and posture of the end of the robot arm;

[0028] According to the joint angles and postures of the end of the robotic arm, multiple groups of joint angles corresponding to the joints at various moments during the movement of the robotic arm along the expected motion trajectory are determined.

[0029] In a possible implementation, the apparatus further includes: a fourth determining unit;

[0030] The fourth determination unit is used to determine that the robotic arm moves along the expected motion trajectory if the first group of target joint angles makes the closest distances from the obstacle to each rod of the robotic arm greater than a preset distance.

[0031] In a third aspect, an embodiment of the present invention provides an electronic device, including:

[0032] A memory and a processor, wherein the processor and the memory communicate with each other via a bus; the memory stores program instructions executable by the processor, and the processor calls the program instructions to execute the method described in the first aspect and each step in various possible implementations.

[0033] In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium having a computer program stored thereon, which, when executed by a processor, implements the method described in the first aspect and each step in various possible implementations.

[0034] In a fifth aspect, an embodiment of the present invention provides a computer program product comprising instructions, and when the computer program product is run on a computer, each step in the method described in the first aspect and its various possible implementations is executed by the computer.

[0035] The embodiments of the present invention disclose the following technical effects:

[0036] According to the change of the closest distance between the obstacle and the rod, the task priority between obstacle avoidance and trajectory tracking is transformed. When the closest distance between the obstacle and the rod is less than the preset distance, obstacle avoidance is given priority, otherwise, trajectory tracking is given priority.

[0037] Of course, any product implementing the present invention does not necessarily need to achieve all of the advantages described above at the same time. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following briefly introduces the drawings required for use in the embodiments or the description of the prior art. Obviously, the drawings described below are some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0039] Figure 1 A system architecture diagram applicable to the embodiment of the present invention;

[0040] Figure 2 A flowchart of a trajectory tracking method based on task priority provided by an embodiment of the present invention;

[0041] Figure 3 A schematic diagram of a seven-degree-of-freedom robotic arm provided by an embodiment of the present invention;

[0042] Figure 4a-4c A schematic diagram of the distance from a point to a line segment provided by an embodiment of the present invention;

[0043] Figure 5 A schematic diagram of the motion process of a robotic arm under four sets of joint angles provided by an embodiment of the present invention;

[0044] Figure 6 A schematic diagram of the motion process of a robotic arm provided by an embodiment of the present invention under a minimum absolute displacement and a corresponding set of joint angles;

[0045] Figure 7 A schematic diagram of the motion process of a robotic arm at a second smallest absolute displacement and a corresponding set of joint angles provided by an embodiment of the present invention;

[0046] Figure 8 Another schematic flow chart of a trajectory tracking method based on task priority provided by an embodiment of the present invention;

[0047] Fig. 9 A schematic diagram of the structure of a trajectory tracking device based on task priority provided by an embodiment of the present invention;

[0048] Fig.10 A schematic block diagram of an electronic device provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0049] In order to make the purpose, technical solution and advantages of the embodiments of the present invention clearer, the technical solution 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 part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without creative work are within the scope of protection of the present invention.

[0050] The terms used in the embodiments of the present invention are only for the purpose of describing specific embodiments, and are not intended to limit the present invention. The singular forms "a", "said" and "the" used in the embodiments of the present invention and the appended claims are also intended to include plural forms, unless the context clearly indicates other meanings.

[0051] It should be understood that the term "and / or" used in this article is only a description of the association relationship of associated objects, indicating that there can be three relationships. For example, A and / or B can represent: A exists alone, A and B exist at the same time, and B exists alone. In addition, the character " / " in this article generally indicates that the associated objects before and after are in an "or" relationship.

[0052] The word "if" as used herein may be interpreted as "at the time of" or "when" or "in response to determining" or "in response to detecting", depending on the context. Similarly, the phrases "if determining" or "if monitoring (stated condition or event)" may be interpreted as "when determining" or "in response to determining" or "when detecting (stated condition or event)" or "in response to detecting (stated condition or event)", depending on the context.

[0053] As a kind of redundant robot, the obstacle avoidance problem of the robotic arm usually only considers the situation when the obstacle affects the movement of the robotic arm end and causes it to deviate from the expected trajectory when the robotic arm end moves along the expected trajectory. However, it ignores the situation when the robotic arm body interferes with the obstacle and the robotic arm deviates from the expected trajectory in order to avoid the obstacle.

[0054] In view of this, the embodiment of the present invention provides a new idea. In order to facilitate the understanding of the present application, the system architecture based on the embodiment of the present invention is first described. Figure 1 An exemplary system architecture to which the present invention can be applied is shown. Figure 1 As shown in , the system architecture may include a terminal device and a trajectory tracking device based on task priority located on the server side.

[0055] The terminal devices may include, but are not limited to, smart mobile terminals, smart home devices, wearable devices, PCs (Personal Computers), etc. Smart mobile terminals may include mobile phones, tablet computers, laptop computers, PDAs (Personal Digital Assistants), Internet cars, etc. Smart home devices may include smart TVs, smart refrigerators, etc. Wearable devices may include smart watches, smart glasses, virtual reality devices, augmented reality devices, mixed reality devices (i.e., devices that can support virtual reality and augmented reality), etc.

[0056] The task priority-based trajectory tracking device may adopt the task priority-based trajectory tracking method provided in the embodiment of the present invention to perform task priority-based trajectory tracking.

[0057] The above-mentioned task priority-based trajectory tracking device can be set on a single server, or on a server group consisting of multiple servers, or on a cloud server. A cloud server, also known as a cloud computing server or cloud host, is a host product in a cloud computing service system to solve the problems of difficult management and weak service scalability in traditional physical hosts and virtual private servers (VPS). Figure 1 In addition to the illustrated architecture, the trajectory tracking device based on task priority can also be set in a computer terminal with strong computing power.

[0058] Figure 2 A flow chart of a trajectory tracking method based on task priority provided by an embodiment of the present invention, the method can be Figure 1 The task priority-based trajectory tracking device in the system shown is executed. Figure 2As shown in , the method may include the following steps:

[0059] Step 201: Obtain the expected motion trajectory of the robot arm from the initial position to the target position.

[0060] Step 202: Determine a plurality of sets of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory.

[0061] Step 203: If there is an obstacle at the current moment, a first set of target joint angles is determined from multiple sets of joint angles corresponding to the current moment.

[0062] Step 204: If the first set of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then determine the second set of target joint angles from the multiple sets of joint angles corresponding to the current moment, and repeat this step until the Kth set of target joint angles is determined, so that the robotic arm avoids the obstacle and moves along the expected motion trajectory, where K is an integer greater than or equal to 1 and less than or equal to the number of multiple sets of joint angles.

[0063] The following is a detailed description of each step in the above process and the effects that can be further produced in conjunction with the embodiments of the present invention. It should be noted that the limitations such as "first" and "second" involved in the embodiments of the present invention do not have limitations in terms of size, order, and quantity, and are only used to distinguish them in name, for example, "first determination unit" and "second determination unit" are used to distinguish two different determination units.

[0064] First, the above step 201, namely, “obtaining the expected motion trajectory of the robot arm from the initial position to the target position” is described in detail in conjunction with an embodiment of the present invention.

[0065] In the embodiment of the present invention, the expected motion trajectory may be an arc, a straight line, an S shape, etc., and the shape of the expected motion trajectory is not specifically limited in the embodiment of the present invention.

[0066] In this embodiment of the present invention, it is assumed that the initial position of the given robotic arm is:

[0067] Pos b =(x b ,y b ,z b ),Pose b =(R b ,P b ,Y b ) (1)

[0068] The pose of the given target position is:

[0069] Pos t =(xt ,y t ,z t ),Pose t =(R t ,P t ,Y t ) (2)

[0070] Use B-spline curves to plan the trajectories of position and attitude respectively.

[0071] Linear interpolation of position can be done using linear B-spline, the formula is as follows:

[0072] Pos(t)=(1-t)Pos b +t·Pos t (3)

[0073] Where: Pos(t) is the position of the end effector of the robot arm at parameter t; Pos b and Pos t are the Cartesian coordinates of the initial position and the target position respectively; t is the interpolation parameter, whose value varies from 0 to 1.

[0074] For attitude interpolation, spherical linear interpolation (SLEEP) is used to interpolate between two rotations. The end pose representation is first converted into quaternion form, and B-spline interpolation is used to complete the rotation representation.

[0075] The interpolation formula of quaternion is:

[0076]

[0077] Among them, Pose(t) is the quaternion result when the interpolation parameter t is used, and the value range of t is 0 to 1; Pose b and Pose t is the quaternion representation of the initial position and the target position; θ is the central angle of interpolation between the two quaternions, and the calculation formula is:

[0078]

[0079] In the formula, the clamp function ensures that the dot product result is in the range of [-1, 1] to avoid out-of-range problems caused by floating-point operations.

[0080] The above step 202, i.e., "determining a plurality of groups of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory", is described in detail below in conjunction with an embodiment of the present invention.

[0081] In an embodiment of the present invention, the joint angles and postures of the end of the robotic arm are obtained, and multiple groups of joint angles corresponding to each joint at each time during the movement of the robotic arm along the expected motion trajectory are determined according to the joint angles and postures of the end of the robotic arm.

[0082] In the embodiment of the present invention, the multiple groups of joint angles corresponding to each moment include the joint angles of each joint of the robotic arm when it is at an initial position and the joint angles of each joint of the robotic arm when it is at a target position.

[0083] In the embodiment of the present invention, Figure 3 The seven-DOF manipulator shown in the figure is taken as an example. The MDH parameters of the seven-DOF manipulator are shown in Table 1.

[0084] Table 1

[0085] Joint <![CDATA[α i ]]> <![CDATA[a i ]]> <![CDATA[θ i ]]> <![CDATA[d i ]]> offset 1 0 0 <![CDATA[θ1]]> <![CDATA[d1]]> 0 2 π / 2 0 <![CDATA[θ2]]> 0 -π / 2 3 0 a3 <![CDATA[θ3]]> 0 π / 2 4 0 a4 <![CDATA[θ4]]> <![CDATA[d4]]> -π / 2 5 -π / 2 0 <![CDATA[θ5]]> <![CDATA[d5]]> 0 6 -π / 2 0 <![CDATA[θ6]]> <![CDATA[d6]]> 0 7 π / 2 0 <![CDATA[θ7]]> <![CDATA[d7]]> 0

[0086] In the formula, a3 = 800mm, a4 = 700mm, d1 = 693.5mm, d4 = 0mm, d5 = 700mm, d6 = 0mm, d7 = 0mm. Where offset is the offset of the initial value of the joint, θ i is the joint angle, d i is the member moment, a i is the length of the rod, α i is the torsion angle of the rod, and i is the joint number.

[0087] (1) Solving the joint angle θ1

[0088] According to forward kinematics, the position of the end of the robot arm Assuming that the joint angle θ7 at the end of the robot arm is known, the MDH coordinate transformation matrix from joint 6 to joint 7 is:

[0089]

[0090] Among them, X6 represents the X-axis of the coordinate system of joint 6 itself, and Z7 represents the Z-axis of the coordinate system of joint 7 itself.

[0091] The homogeneous transformation matrix corresponding to the wrist joint center O6 is:

[0092]

[0093] Among them,

[0094] It is the transformation matrix from the local coordinate system of joint 6 to the local coordinate system of joint 7.

[0095] For θ1, the following relationship exists:

[0096]

[0097] Eliminating equation (8), we can obtain:

[0098] (p x -d6a x )sinθ1+(d6a y -p y )cosθ1=d4 (9)

[0099] Using trigonometric identities to substitute the formula, let:

[0100]

[0101] Then we can get At the same time, from formula (10) we can get:

[0102] φ=arctan2(d6a y -p y ,p x -d6a x ) (11)

[0103] Combining equations (9) and (10), we can obtain θ1. From the “±” sign in equation (12), we can see that θ1 has two sets of solutions.

[0104]

[0105] (2) Solving the joint angle θ5

[0106] Similar to the process of solving the joint angle θ1, when the joint angle θ1 is known, the joint angle θ5 and the joint angle θ1 have the following relationship (i.e., formula (13)). The value of the joint angle θ5 can be obtained by the elimination method.

[0107] a y cosθ1-a x sinθ1=cosθ5 (13)

[0108] Known:

[0109]

[0110] Similarly, formula (13) and formula (14) can be combined to solve θ5. It can be seen from formula (14) that sinθ5 has two positive and negative values ​​after the square root, so θ5 also has two sets of solutions.

[0111] θ5=arctan2(±sinθ5,cosθ5) (15)

[0112] (3) Solution of joint angle θ6

[0113] The following relationship exists between the joint angle θ6, the joint angle θ5, and the joint angle θ1:

[0114]

[0115] As we can see in equation (16), if sinθ5 = 0, the robot is in a singular position. So only when sinθ5 ≠ 0 can we solve for θ6.

[0116] When sinθ5≠0, the values ​​of θ1 and θ5 in formula (16) have been solved through the above steps. Dividing both sides of the equation system by sinθ5, the value of θ6 can be obtained:

[0117]

[0118] (4) Solution of joint angles θ2, θ3, θ4

[0119] The values ​​of θ1 and θ5 have been obtained through the above steps. The values ​​of θ2, θ3, and θ4 have the following equation relationship:

[0120]

[0121] Similar to the process of solving θ6, if sinθ5=0, the robot is in a singular position. Therefore, only when sinθ5≠0 can we solve θ2+θ3+θ4.

[0122] When sinθ5≠0, according to equation group (18), the solution of θ2+θ3+θ4 can be obtained:

[0123]

[0124] make

[0125] M=cosθ1*p x +sinθ1*p y -d5*cosθ 234 -d6sinθ5sinθ 234

[0126] N=p z -d1+d5sinθ 234 -d6sinθ5cosθ 234

[0127] E=-2*M*a3,

[0128] F=2*N*a3

[0129]

[0130] D=E 2 +F 2 -G 2 .

[0131] The values ​​of θ2 and θ2+θ3 have the following equation relationship:

[0132]

[0133] From the above formula we can get:

[0134]

[0135] Correspondingly, we can get θ 23 =The solution of θ2+θ3:

[0136] θ 23 =arctan 2((a3*cosθ2-N) / a4,(M-a3*sinθ2) / a4) (22)

[0137] Then θ3=θ 23 -θ2,θ4=θ 234 -θ 23

[0138] So far, all joint angles have been calculated. Since θ1, θ2 and θ5 each have two solutions, the number of joint angles is 8.

[0139] The above step 203, i.e., "if there is an obstacle at the current moment, determining a first set of target joint angles from multiple sets of joint angles corresponding to the current moment" is described in detail below in conjunction with an embodiment of the present invention.

[0140] In the embodiment of the present invention, if there is an obstacle at the current moment, the sum of the absolute displacements of the joints of the robot arm from the initial position to the target position is arranged from small to large to obtain an arrangement result; wherein, the sequence number of the sum of the absolute displacements arranged from small to large is 1, 2..., L, and L is an integer greater than or equal to K and less than or equal to the number of groups of joint angles. The sum of the absolute displacements is represented by the sum of the differences between the joint angles of the joints of the robot arm at the target position and the joint angles at the initial position; wherein, the joint angles of the joints of the robot arm at the initial position are q b , n is the number of groups of joint angles.

[0141]

[0142] in, Represents the joint angle of joint 1 of the robot arm in the initial position, and so on. Indicates the joint angle of joint 7 of the robot arm when it is in the initial position.

[0143] The joint angles of the robot arm at the target position are q t .

[0144]

[0145] in, Indicates the joint angle of the robot arm when joint 1 is at the target position, and so on. Indicates the joint angle when joint 7 of the robot arm is at the target position.

[0146] The sum of the absolute displacements is Δq min .

[0147]

[0148] In the embodiment of the present invention, according to the arrangement result, a group of joint angles corresponding to the sum of the target absolute displacements is determined as the first group of target joint angles. The sum of the target absolute displacements is the sum of the absolute displacements with a sequence number of 1. That is to say, when there is an obstacle at the current moment, a group of joint angles corresponding to the minimum sum of the absolute displacements is determined as the first group of target joint angles. Exemplarily, a group of joint angles corresponding to the minimum sum of the absolute displacements is found from the 8 groups of joint angles and is determined as the first group of target joint angles.

[0149] The following describes in detail step 204 in conjunction with an embodiment of the present invention, i.e., "if the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then determine the second group of target joint angles from the multiple groups of joint angles corresponding to the current moment, and repeat this step until the Kth group of target joint angles is determined, so that the robotic arm avoids the obstacle and moves along the expected motion trajectory, K being an integer greater than or equal to 1 and less than or equal to the number of multiple groups of joint angles."

[0150] In an embodiment of the present invention, if the first set of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then the sum of the absolute displacements numbered 2 is determined from the multiple sets of joint angles corresponding to the current moment as the second set of target joint angles, and this step is repeated until the sum of the absolute displacements numbered K is determined as the Kth set of target joint angles, so that the robotic arm avoids obstacles and moves along the expected motion trajectory.

[0151] That is to say, if the first set of target joint angles makes the closest distance from the obstacle to one of the rods of the robot arm less than or equal to the preset distance, then the sum of the next smallest absolute displacement is determined from the multiple sets of joint angles corresponding to the current moment as the second set of target joint angles, and this step is repeated until the sum of the absolute displacement with a sequence number of K is determined as the Kth set of target joint angles, so that the robot arm avoids the obstacle and moves along the expected motion trajectory. Exemplarily, a set of joint angles corresponding to the sum of the next smallest absolute displacement is determined from the 8 sets of joint angles as the second set of target joint angles.

[0152] It should be noted that the shortest distance from the obstacle to the rod of the robot arm is obtained in the following way:

[0153] In order to avoid obstacles better, it is necessary to solve the shortest distance from the obstacle to the rod. This problem can be simplified to the shortest distance problem from a point to a line segment. There are many algorithms for the shortest distance between a point and a line segment, among which the vector method is the simplest and easy to program. Since vectors have directionality, some directions can be directly determined based on their positive and negative signs. Using this method, you only need to find the projection from the obstacle to the line segment, that is, to the direction of the robot arm rod (see Figure 3 ).

[0154]

[0155] In the formula express The meaning of the unit vector in direction is to determine the direction of the required vector. is the product of two vectors, and Where θ is a vector and The angle between is the length of the vector. Then This is the length of AC, which has no directionality. The whole constitutes a new vector with size and direction That is exist The projection vector in the direction, C is the projection point.

[0156] Here is defined From the directionality of the vector, we know that if the situation is Figure 4a As shown, then r≤0; if it is Figure 4b In the case shown, then 0<r<1; if Figure 4c In the case shown, we get r≥1; special cases such as point P on the line segment, on the extended line of the line segment, etc. are all applicable to this formula, but they only appear as special cases and do not need to be discussed separately. This is also the advantage of the vector algorithm idea. Therefore, according to different r values, the closest distance d can be calculated as:

[0157]

[0158] When the closest distance d is less than the preset distance, the rod is controlled to avoid obstacles; otherwise, the rod is a safe rod and obstacle avoidance does not need to be considered.

[0159] It should be noted that if the first set of target joint angles makes the distances from the obstacle to each rod of the robotic arm greater than the preset distance, it is determined that the robotic arm moves along the expected motion trajectory.

[0160] For example, Figure 3 Take the seven-DOF robot arm shown in the figure as an example. The initial position of the end of the robot arm is: (0.708, -0.546, 1.117) m, and the posture is: (167.8, 24.3, -141.4)°. The eight groups of joint angles corresponding to the initial position are shown in Table 2.

[0161] Table 2

[0162]

[0163]

[0164] The target position of the end of the robot is: (0, 0.952, 1.117) m, and the posture is: (167.0, 24.3, -86.4)°. The eight sets of joint angles corresponding to the target position are shown in Table 3.

[0165] Table 3

[0166]

[0167] The robot moves from the initial position to the target position in a straight line, with a movement time of 10 seconds and a time interval of 0.1 seconds. In the absence of obstacles, four groups of joint angles, 1, 2, 3, and 4, are selected as references. The movement process of the robot arm under the four groups of joint angles is as follows: Figure 5 shown.

[0168] According to formula (25), the minimum absolute displacement and the corresponding set of joint angles are the first set of joint angles. If a spherical obstacle with a center coordinate of (0.4, 0.4, 0.9) m and a radius of 30 mm is set in the motion space of the robot. According to the diameter of the robot rod and the radius of the obstacle, the preset distance for the robot to avoid the obstacle is set to 40 mm. According to formula (27), the first set of joint angles will encounter obstacles during operation and cannot pass smoothly. The movement process of the robot is as follows: Figure 6 shown.

[0169] According to the task priority, select the group of joint angles corresponding to the sum of the second smallest absolute displacement. According to formula (25), this group of joint angles is the second group of joint angles. At this time, the closest distance from the rod to the obstacle is greater than the preset distance. At this time, the robot can pass smoothly. The movement process of the robot is as follows: Figure 7 shown.

[0170] From the above content, it can be seen that the embodiment of the present invention realizes the conversion of the task priority between obstacle avoidance movement and trajectory tracking according to the change of the closest distance between the obstacle and the rod. When the closest distance between the obstacle and the rod is less than the preset distance, the obstacle avoidance movement is given priority, otherwise, the trajectory tracking task is given priority.

[0171] Based on the same concept as the above method embodiment, the embodiment of the present invention also provides a trajectory tracking method based on task priority, and its flow chart is as follows: Figure 8 As shown, including S801-S806.

[0172] Step 801, obtaining the expected motion trajectory of the robot arm from the initial position to the target position.

[0173] In the embodiment of the present invention, the implementation process of step 801 is the same as step 201 and will not be repeated here.

[0174] Step 802 , determining a plurality of groups of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory.

[0175] In the embodiment of the present invention, the implementation process of step 802 is the same as step 202 and will not be repeated here.

[0176] Step 803: If there is an obstacle at the current moment, a first set of target joint angles is determined from multiple sets of joint angles corresponding to the current moment.

[0177] In the embodiment of the present invention, the implementation process of step 803 is the same as step 203, which will not be repeated here.

[0178] Step 804 , determining whether the first set of target joint angles makes the shortest distance from the obstacle to each rod of the robotic arm less than or equal to a preset distance.

[0179] In the embodiment of the present invention, if the first set of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, obstacle avoidance is prioritized, and step 805 is executed. If the first set of target joint angles makes the closest distance from the obstacle to each rod of the robotic arm greater than the preset distance, it is determined that the robotic arm has priority along the expected motion trajectory, and step 806 is executed.

[0180] Step 805, if the first set of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then determine the second set of target joint angles from the multiple sets of joint angles corresponding to the current moment, and repeat this step until the Kth set of target joint angles is determined, so that the robotic arm avoids obstacles and moves along the expected motion trajectory.

[0181] In the embodiment of the present invention, the implementation process of step 805 is the same as step 204, which will not be repeated here.

[0182] Step 806: If the first set of target joint angles makes the closest distances from the obstacle to each rod of the robotic arm greater than a preset distance, it is determined that the robotic arm moves along the expected motion trajectory.

[0183] According to an embodiment of another aspect, a trajectory tracking device based on task priority is provided. Fig. 9 A schematic block diagram of a task priority-based trajectory tracking device according to an embodiment is shown. The device can be arranged at Figure 1 The server side of the architecture shown in Figure 1. Fig. 9 As shown, the device 900 may include: an acquisition unit 901, a first determination unit 902, a second determination unit 903, and a third determination unit 904. The main functions of each component unit are as follows:

[0184] The acquisition unit 901 is used to acquire the expected motion trajectory of the robot arm from the initial position to the target position;

[0185] The first determination unit 902 is used to determine multiple groups of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory;

[0186] The second determining unit 903 is used to determine a first set of target joint angles from multiple sets of joint angles corresponding to the current moment if there is an obstacle at the current moment;

[0187] The third determination unit 904 is used to determine the second group of target joint angles from the multiple groups of joint angles corresponding to the current moment if the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, and repeat this step until the Kth group of target joint angles is determined, so that the robotic arm avoids the obstacle and moves along the expected motion trajectory, K being an integer greater than or equal to 1 and less than or equal to the number of multiple groups of joint angles.

[0188] In a possible implementation, the first determining unit 902 is specifically configured to:

[0189] Get the joint angle and posture of the end of the robot arm;

[0190] According to the joint angles and postures of the end of the robotic arm, multiple groups of joint angles corresponding to the joints at various moments during the movement of the robotic arm along the expected motion trajectory are determined.

[0191] In a possible implementation, the apparatus further includes: a fourth determining unit;

[0192] The fourth determination unit is used to determine that the robotic arm moves along the expected motion trajectory if the first group of target joint angles makes the closest distances from the obstacle to each rod of the robotic arm greater than a preset distance.

[0193] In a possible implementation, the second determining unit 903 is specifically configured to:

[0194] If there is an obstacle at the current moment, the sum of the absolute displacements of each joint of the robotic arm from the initial position to the target position is arranged from small to large to obtain an arrangement result; wherein the sum of the absolute displacements is arranged from small to large in sequence of 1, 2..., L, L is an integer greater than or equal to K and less than or equal to the number of groups of joint angles, and the sum of the absolute displacements is represented by the sum of the differences between the joint angles of each joint of the robotic arm at the target position and the joint angles at the initial position;

[0195] According to the arrangement result, a group of joint angles corresponding to the sum of the target absolute displacements is determined as a first group of target joint angles, and the sum of the target absolute displacements is the sum of the absolute displacements with a sequence number of 1.

[0196] In a possible implementation, the third determining unit 904 is specifically configured to:

[0197] If the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then the sum of the absolute displacements numbered 2 is determined from the multiple groups of joint angles corresponding to the current moment, and this is used as the second group of target joint angles. This step is repeated until the sum of the absolute displacements numbered K is determined as the Kth group of target joint angles, so that the robotic arm avoids obstacles and moves along the expected motion trajectory.

[0198] Each embodiment in this specification is described in a progressive manner, and the same or similar parts between the embodiments can be referred to each other, and each embodiment focuses on the differences from other embodiments. In particular, for the device embodiment, since it is basically similar to the method embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method embodiment. The device embodiment described above is only schematic, wherein the unit described as a separate component may or may not be physically separated, and the component displayed as a unit may or may not be a physical unit, that is, it may be located in one place, or it may be distributed on multiple network units. Some or all of the modules can be selected according to actual needs to achieve the purpose of the scheme of this embodiment. Ordinary technicians in this field can understand and implement it without paying creative work.

[0199] In addition, an embodiment of the present invention further provides a computer-readable storage medium on which a computer program is stored. When the program is executed by a processor, the steps of any one of the methods in the aforementioned method embodiments are implemented.

[0200] And an electronic device, comprising:

[0201] one or more processors; and

[0202] A memory associated with the one or more processors, the memory being used to store program instructions, wherein the program instructions, when read and executed by the one or more processors, execute the steps of the method described in any one of the aforementioned method embodiments.

[0203] An embodiment of the present invention further provides a computer program product, including a computer program, which implements the steps of any one of the methods in the aforementioned method embodiments when executed by a processor.

[0204] in, Fig.10 The architecture of the electronic device is exemplarily shown, which may include a processor 1010, a video display adapter 1011, a disk drive 1012, an input / output interface 1013, a network interface 1014, and a memory 1020. The processor 1010, the video display adapter 1011, the disk drive 1012, the input / output interface 1013, the network interface 1014, and the memory 1020 may be communicatively connected via a communication bus 1030.

[0205] The processor 1010 may be implemented by a general-purpose CPU, a microprocessor, an application-specific integrated circuit (ASIC), or one or more integrated circuits, etc., and is used to execute relevant programs to implement the technical solutions provided by the embodiments of the present invention.

[0206] The memory 1020 can be implemented in the form of ROM (Read Only Memory), RAM (Random Access Memory), static storage device, dynamic storage device, etc. The memory 1020 can store an operating system 1021 for controlling the operation of the electronic device 1000, and a basic input and output system (BIOS) 1022 for controlling the low-level operation of the electronic device 1000. In addition, a web browser 1023, a data storage management system 1024, and a trajectory tracking device 1025 based on task priority, etc. can also be stored. The above-mentioned trajectory tracking device 1025 based on task priority can be an application program that specifically implements the operations of the aforementioned steps in the embodiment of the present invention. In short, when the technical solution provided by the embodiment of the present invention is implemented by software or firmware, the relevant program code is stored in the memory 1020 and is called and executed by the processor 1010.

[0207] The input / output interface 1013 is used to connect the input / output module to realize information input and output. The input / output module can be configured in the device as a component (not shown in the figure), or it can be externally connected to the device to provide corresponding functions. Among them, the input device may include a keyboard, a mouse, a touch screen, a microphone, various sensors, etc., and the output device may include a display, a speaker, a vibrator, an indicator light, etc.

[0208] The network interface 1014 is used to connect to a communication module (not shown) to realize communication interaction between the device and other devices. The communication module can realize communication through a wired mode (such as USB, network cable, etc.) or a wireless mode (such as mobile network, WIFI, Bluetooth, etc.).

[0209] The bus 1030 comprises a pathway for transmitting information between the various components of the device (eg, the processor 1010, the video display adapter 1011, the disk drive 1012, the input / output interface 1013, the network interface 1014, and the memory 1020).

[0210] It should be noted that, although the above device only shows a processor 1010, a video display adapter 1011, a disk drive 1012, an input / output interface 1013, a network interface 1014, a memory 1020, a bus 1030, etc., in the specific implementation process, the device may also include other components necessary for normal operation. In addition, it can be understood by those skilled in the art that the above device may also only include components necessary for implementing the solution of the present application, and does not necessarily include all the components shown in the figure.

[0211] Through the description of the above implementation methods, those skilled in the art can clearly understand that each implementation method can be implemented by means of software plus a necessary general hardware platform, and of course, it can also be implemented by hardware. Based on this understanding, the above technical solution is essentially or the part that contributes to the prior art can be embodied in the form of a software product, and the computer software product can be stored in a computer-readable storage medium, such as ROM / RAM, a disk, an optical disk, etc., including a number of instructions for a computer device (which can be a personal computer, a server, or a network device, etc.) to execute the methods described in each embodiment or some parts of the embodiments.

[0212] Finally, it should be noted that the above embodiments are only used to illustrate the technical solutions of the embodiments of the present invention, rather than to limit them. Although the embodiments of the present invention have been described in detail with reference to the aforementioned embodiments, those skilled in the art should understand that they can still modify the technical solutions described in the aforementioned embodiments, or replace some or all of the technical features therein with equivalents. However, these modifications or replacements do not cause the essence of the corresponding technical solutions to deviate from the scope of the technical solutions of the embodiments of the present invention.

Claims

1. A trajectory tracking method based on task priority, characterized in that: include: Obtain the expected motion trajectory of the robot arm from the initial position to the target position; Determine multiple sets of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory; If there is an obstacle at the current moment, a first set of target joint angles is determined from multiple sets of joint angles corresponding to the current moment; If the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then determine the second group of target joint angles from the multiple groups of joint angles corresponding to the current moment, and repeat this step until the Kth group of target joint angles is determined, so that the robotic arm avoids obstacles and moves along the expected motion trajectory, where K is an integer greater than or equal to 1 and less than or equal to the number of multiple groups of joint angles.

2. The method according to claim 1, characterized in that The method of determining a plurality of sets of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory includes: Get the joint angle and posture of the end of the robot arm; According to the joint angles and postures of the end of the robotic arm, multiple groups of joint angles corresponding to the joints at various moments during the movement of the robotic arm along the expected motion trajectory are determined.

3. The method according to claim 1, characterized in that The method further comprises: If the first group of target joint angles makes the closest distances from the obstacle to each rod of the robotic arm greater than a preset distance, it is determined that the robotic arm moves along the expected motion trajectory.

4. The method according to claim 1, characterized in that: If there is an obstacle at the current moment, determining a first set of target joint angles from multiple sets of joint angles corresponding to the current moment includes: If there is an obstacle at the current moment, the sum of the absolute displacements of each joint of the robotic arm from the initial position to the target position is arranged from small to large to obtain an arrangement result; wherein the sum of the absolute displacements is arranged from small to large in sequence of 1, 2..., L, L is an integer greater than or equal to K and less than or equal to the number of groups of joint angles, and the sum of the absolute displacements is represented by the sum of the differences between the joint angles of each joint of the robotic arm at the target position and the joint angles at the initial position; According to the arrangement result, a group of joint angles corresponding to the sum of the target absolute displacements is determined as a first group of target joint angles, and the sum of the target absolute displacements is the sum of the absolute displacements with a sequence number of 1.

5. The method according to claim 4, characterized in that If the first set of target joint angles makes the closest distance from the obstacle to one of the rods of the robot arm less than or equal to the preset distance, then determine the second set of target joint angles from the multiple sets of joint angles corresponding to the current moment, and repeat this step until the Kth set of target joint angles is determined, so that the robot arm avoids the obstacle and moves along the expected motion trajectory, including: If the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, then the sum of the absolute displacements numbered 2 is determined from the multiple groups of joint angles corresponding to the current moment, and this is used as the second group of target joint angles. This step is repeated until the sum of the absolute displacements numbered K is determined as the Kth group of target joint angles, so that the robotic arm avoids obstacles and moves along the expected motion trajectory.

6. A trajectory tracking device based on task priority, characterized in that: include: An acquisition unit, used for acquiring an expected motion trajectory of the robot arm from an initial position to a target position; The first determination unit is used to determine a plurality of groups of joint angles corresponding to each joint at each moment during the movement of the robot arm along the expected motion trajectory; A second determination unit is used to determine a first set of target joint angles from multiple sets of joint angles corresponding to the current moment if there is an obstacle at the current moment; The third determination unit is used to determine the second group of target joint angles from the multiple groups of joint angles corresponding to the current moment if the first group of target joint angles makes the closest distance from the obstacle to one of the rods of the robotic arm less than or equal to the preset distance, and repeat this step until the Kth group of target joint angles is determined, so that the robotic arm avoids the obstacle and moves along the expected motion trajectory, K is an integer greater than or equal to 1 and less than or equal to the number of multiple groups of joint angles.

7. The device according to claim 6, characterized in that The first determining unit is specifically configured to: Get the joint angle and posture of the end of the robot arm; According to the joint angles and postures of the end of the robotic arm, multiple groups of joint angles corresponding to the joints at various moments during the movement of the robotic arm along the expected motion trajectory are determined.

8. The device according to claim 5, characterized in that The device further includes: a fourth determining unit; The fourth determination unit is used to determine that the robotic arm moves along the expected motion trajectory if the first group of target joint angles makes the closest distances from the obstacle to each rod of the robotic arm greater than a preset distance.

9. An electronic device, characterized in that: include: A memory and a processor, wherein the processor and the memory communicate with each other via a bus; The memory stores program instructions executable by the processor, and the processor can execute the method according to any one of claims 1 to 5 by calling the program instructions.

10. A computer-readable storage medium having a computer program stored thereon, characterized in that: When the computer program is executed by a processor, the method according to any one of claims 1 to 5 is implemented.

Images