WO2017219639A1

WO2017219639A1 - Motion trail planning method and device for robotic arm, and robot

Info

Publication number: WO2017219639A1
Application number: PCT/CN2016/113187
Authority: WO
Inventors: 罗汉杰
Original assignee: 广州视源电子科技股份有限公司
Priority date: 2016-06-20
Filing date: 2016-12-29
Publication date: 2017-12-28
Also published as: CN105922265B; CN105922265A

Abstract

A motion trail planning method for a robotic arm, comprising: controlling a robotic arm to move with a predetermined motion trail according to preset starting point coordinates and end point coordinates and a speed curve algorithm; after receiving a stop instruction sent by a user, obtaining the current motion time of the robotic arm, and determining the current motion stage of the robotic arm according to the current motion time and the time duration configured for each motion stage; and according to the current motion stage of the robotic arm, changing predetermined time durations of the motion stages and predetermined initial parameters in an initial parameter set, generating a changed motion trail, and controlling the robotic arm to move with the changed motion trail. A motion trail planning device (100) for a robotic arm and a robot. After a stop instruction of a user is received, a motion trail is replanned, so that a motion arm is stopped as soon as possible on the premise of keeping stable motion and speed continuity.

Description

Method, device and robot for trajectory planning of mechanical arm

Technical field

The invention relates to the field of robot control, in particular to a method, a device and a robot for planning a motion path of a mechanical arm.

Background technique

In the working process of the manipulator, the trajectory of the manipulator needs to be planned. Generally, the trajectory planning is implemented by a predetermined speed planning algorithm. For example, an S-type (Double S) velocity curve algorithm can be used. To complete the trajectory planning of the robot arm.

Some speed planning algorithms need to receive the start and end points provided by the user in advance when planning the trajectory. Then the program will generate a series of interpolation points between the two points to describe the trajectory of the arm. For example, in the teaching function, the user sends an instruction through the handheld device, so that the robot arm starts from the starting point and moves linearly in a certain direction until reaching the end point.

However, many times, the end position of the end point is determined by the user in real time according to the actual situation, so the user can not give a specific end point coordinate at the beginning of the movement, which leads to the robot arm based on the current speed planning algorithm. The motion trajectory cannot be planned in advance during the work process, which affects the user experience.

Summary of the invention

In view of the above problems, an object of the present invention is to provide a method, a device and a robot for trajectory planning of a robot arm, which can re-plan the motion trajectory according to the user's operation, and meet the user's use requirements.

The invention provides a method for planning a motion trajectory of a mechanical arm, comprising the following steps:

Controlling the robot arm to move according to a predetermined motion trajectory according to preset start point coordinates, end point coordinates, and speed curve algorithm; wherein the speed curve algorithm plans the motion trajectory of the arm according to a preset initial parameter set to at least Two phases of motion and a duration for each phase of motion;

Obtaining a current motion time of the robot arm after receiving a stop command issued by the user, and determining a current motion phase of the robot arm according to the current motion time and a duration configured for each motion phase;

Generating a modified motion trajectory according to a duration of the motion stage in which the moving arm is currently in motion and a predetermined initial parameter in the initial parameter set, and controlling the mechanical arm to change motion The trajectory moves.

Preferably, the speed curve algorithm is an S-type speed curve algorithm, and the initial parameter set includes at least a path, a maximum acceleration of the acceleration phase, a maximum speed, a minimum deceleration of the deceleration phase, a maximum jerk, and a minimum jerk. These initial parameters include: an accelerated motion phase, a uniform motion phase, and a deceleration phase, the accelerated motion phase including a first acceleration motion phase, a second acceleration motion phase, and a third acceleration motion phase; The first deceleration phase, the second deceleration phase, and the third deceleration phase are included.

Preferably, the duration of the predetermined motion phase and the initial reference are changed according to a motion phase in which the moving arm is currently located a predetermined initial parameter in the set, generating a changed motion trajectory, and controlling the robot arm to move with the changed motion trajectory, specifically:

When it is determined that the mechanical arm is currently in the first acceleration motion phase, changing the durations of the first acceleration motion phase, the third acceleration motion phase, the first deceleration motion phase, and the third deceleration motion phase to a current exercise time, and changing the duration of the second accelerated motion phase, the uniform velocity motion phase, and the second deceleration motion phase to zero;

And determining, according to the current motion time, the initial parameters of the distance, the maximum speed, the maximum acceleration of the acceleration motion phase, and the minimum deceleration of the deceleration motion phase, generating a changed motion trajectory according to the changed initial parameters, and controlling the mechanism The arm moves with the changed motion trajectory.

Preferably, the changing the duration of the predetermined motion phase and the predetermined initial parameter in the initial parameter set according to the motion phase in which the moving arm is currently located, generating a modified motion trajectory, and controlling the robot arm Exercise with the changed motion trajectory, specifically:

When it is determined that the mechanical arm is currently in the second acceleration motion phase, change the duration of the second acceleration motion phase and the second deceleration motion phase to tt ₁ and change the duration of the uniform motion phase to zero Where t is the current exercise time, and t ₁ -t ₀ is a pre-configured duration from the start time t ₀ to the end of the first acceleration motion phase;

And modifying the two initial parameters of the distance and the maximum speed according to the current motion time t, generating a changed motion trajectory according to the changed initial parameter, and controlling the robot arm to perform the changed motion trajectory. motion.

When it is determined that the mechanical arm is currently in the third acceleration motion phase, the duration of the uniform motion phase is changed to zero;

The initial parameter of the route is changed according to the current motion time, the changed motion trajectory is generated according to the changed initial parameter, and the robot arm is controlled to move with the changed motion trajectory.

When it is determined that the mechanical arm is currently in a uniform motion phase, the duration of the uniform motion phase is changed to tt ₃ ; wherein t is the current motion time, and t ₃ -t ₀ is a pre-configured time t from the start timing ₀ to the duration of the end of the third acceleration motion phase;

The initial parameter of the path is changed according to the current motion time t, the changed motion track is generated according to the changed initial parameter, and the robot arm is controlled to move with the changed motion track.

Preferably, before the controlling the robot arm to move according to a predetermined motion trajectory according to the preset starting point coordinate, the end point coordinate, and the speed curve algorithm, the method further includes:

Generating a working area of the robot arm based on a joint parameter of the robot arm;

Generating a ray according to a coordinate and a moving direction of the starting point input in advance, and calculating an intersection of the ray and a boundary of the working area to generate a set of intersection points;

Calculating a distance between each intersection point in the intersection set and the starting point, acquiring coordinates of an intersection point corresponding to the minimum distance, and marking the intersection point as an end point.

The invention also provides a motion trajectory planning device for a mechanical arm, comprising:

a motion control unit, configured to control the robot arm to move according to a predetermined motion trajectory according to a preset starting point coordinate, an end point coordinate, and a velocity curve algorithm; wherein the speed curve algorithm is to move the mechanical arm according to a preset initial parameter set The motion trajectory is planned for at least two motion phases, and each motion phase is configured for a duration;

a motion time acquisition unit, configured to acquire a current motion time of the robot arm after receiving a stop command issued by a user, and determine the robot arm according to the current motion time t and a duration configured for each motion phase The current stage of exercise;

a motion trajectory changing unit, configured to change a duration of a predetermined motion phase and a predetermined initial parameter in the initial parameter set according to a motion phase in which the motion arm is currently in motion, generate a modified motion trajectory, and control the The arm moves with the changed motion trajectory.

Preferably, the method further comprises:

a work area generating unit, configured to generate a working area of the mechanical arm based on a joint parameter of the mechanical arm to be operated;

An intersection point generating unit configured to generate a ray according to a coordinate and a moving direction of the starting point input in advance, and calculate an intersection of the ray and a boundary of the working area to generate a set of intersection points;

An end point marking unit is configured to calculate a distance of each intersection point in the intersection point set from the starting point, acquire coordinates of an intersection point corresponding to the minimum distance, and mark the intersection point as an end point.

The present invention also provides a robot comprising the above described motion trajectory planning device.

The method, device and robot for trajectory planning of the robot arm provided by the embodiment of the present invention, after receiving the stop command of the user before detecting the entering the deceleration motion phase, performing current and subsequent motion trajectories according to the current motion phase. Re-planning, so that the moving arm stops as soon as possible while maintaining smooth motion and continuous speed, so as to meet the user's use requirements.

DRAWINGS

In order to more clearly illustrate the technical solutions of the present invention, the drawings used in the embodiments will be briefly described below. It is obvious that the drawings in the following description are only some embodiments of the present invention, which are common in the art. For the skilled person, other drawings can be obtained from these drawings without any creative work.

FIG. 1 is a schematic flow chart of a method for planning a motion trajectory of a mechanical arm according to an embodiment of the present invention.

Figure 2 is a schematic diagram of the motion process of the S-shaped velocity curve algorithm.

Figure 3 is a flow chart of the calculation principle of the S-shaped velocity curve algorithm.

4 is a schematic structural view of a SCARA type robot arm.

Fig. 5 is a schematic view showing the SCARA type robot arm shown in Fig. 4 in a DH coordinate system.

Fig. 6 is a schematic view showing the working area of the SCARA type robot arm shown in Fig. 4.

Figure 7 is a plan view of the work area shown in Figure 6.

Figure 8 is a schematic illustration of the connection of the ray to the work area.

FIG. 9 is a schematic flow chart of calculating an intersection of a ray and a circle by an optimization algorithm of a ray-circle intersection test provided by an embodiment of the present invention.

10(a) to 10(c) are schematic diagrams of the optimization algorithm shown in Fig. 9.

FIG. 11 is a schematic structural diagram of a motion trajectory planning apparatus for a mechanical arm according to an embodiment of the present invention.

FIG. 12 is another schematic structural diagram of a motion trajectory planning device for a mechanical arm according to an embodiment of the present invention.

detailed description

The technical solutions in the embodiments of the present invention are clearly and completely described in the following with reference to the accompanying drawings in the embodiments of the present invention. It is obvious that the described embodiments are only a part of the embodiments of the present invention, but not all embodiments. All other embodiments obtained by those skilled in the art based on the embodiments of the present invention without creative efforts are within the scope of the present invention.

Embodiments of the present invention provide a method for planning a motion trajectory of a mechanical arm, which can determine an end point coordinate in real time according to a stop command issued by a user, and plan a new motion trajectory in advance.

Referring to FIG. 1 , an embodiment of the present invention provides a schematic flowchart of a method for planning a motion trajectory of a mechanical arm, which may be performed by a motion trajectory planning device of a mechanical arm (hereinafter referred to as a motion trajectory planning device), and includes at least the following steps:

S101. Control a robot arm to move according to a predetermined motion trajectory according to a preset start point coordinate, an end point coordinate, and a speed curve algorithm. wherein the speed curve algorithm plans the motion trajectory of the mechanical arm according to a preset initial parameter set. For at least two phases of motion, and configure a duration for each phase of motion.

In the embodiment of the present invention, the robot arm is the most widely used automatic mechanical device in the field of robot technology, and is widely used in the fields of industrial manufacturing, medical treatment, entertainment service, military, semiconductor manufacturing, and space exploration. Although they differ in their form, they all share a common feature of being able to accept instructions and accurately position them at a certain point in the three-dimensional (or two-dimensional) space according to the instructions.

In the embodiment of the present invention, the movement of the mechanical arm is generally controlled according to a speed curve algorithm, for example, a common trapezoidal velocity curve algorithm, a sinusoidal velocity curve algorithm, and an S-shaped velocity curve algorithm, wherein the S-shaped velocity curve The algorithm is widely used because it can control the rate of change of acceleration and smoothly transition at the speed connection, so it is very stable during operation and does not vibrate.

Among them, the S-type velocity curve algorithm needs to input the initial parameter set (including the path q ₁ , the starting velocity v ₀ , the starting acceleration a _{0 the} ending velocity v ₁ , the ending acceleration a ₁ , and the maximum acceleration a _{max in the} acceleration motion phase during operation). , the maximum speed v _max , the minimum deceleration a _min , the maximum jerk j _max and the minimum jerk j _min ) in the deceleration phase, and then the trajectory of the motion trajectory according to these initial parameters. As shown in Figure 2, in general, the typical S-type velocity curve algorithm will plan the motion trajectory into three motion phases: the acceleration motion phase (duration T _a ), the uniform motion phase (duration T _v ), and deceleration. Movement phase (duration T _d ), more specifically: the acceleration motion phase includes a first acceleration motion phase (corresponding to a time interval [t ₀ , t ₁ ), the duration is T _j1 , and the acceleration is gradually increased from 0 To a _max ), the second acceleration motion phase (corresponding to the time interval [t ₁ , t ₂ ), at which time the acceleration remains unchanged) and the third acceleration motion phase (corresponding to the time interval [t ₂ , t ₃ ), continuing The time is T _j1 , the acceleration gradually decreases from a _max to 0); the deceleration motion phase includes a first deceleration motion phase (corresponding to the time interval [t ₄ , t ₅ ), the duration is T _j2 , and the acceleration is from 0 gradually decreases to a _min ), the second deceleration phase (corresponding to the time interval [t ₅ , t ₆ ), at which time the acceleration remains unchanged) and the third deceleration phase (corresponding to the time interval [t ₆ , t ₇ ), the duration is T _j2 , and the acceleration is gradually increased from a _min to 0), Wherein, in the acceleration acceleration phase and the acceleration deceleration phase, the corresponding jerk is j _max and j _min , respectively, and generally, j _max =−j _min , T _j1 =T _j2 , a _max =a _min .

The process of trajectory planning by the S-type velocity curve algorithm will be described in detail below. In order to facilitate the calculation, v ₀ = v ₁ =0, a ₀ = a ₁ =0 is taken here. As shown in FIG. 3, first, the S-type speed curve algorithm first needs to accept the initial parameters given by the user, including: path q ₁ , starting point velocity v ₀ , starting point acceleration a ₀ end point speed v ₁ , end point acceleration a _1. The maximum acceleration a _max , the maximum speed v _max , the minimum deceleration a _min , the maximum jerk j _max and the minimum jerk j _{min of the} acceleration motion phase. Then, the S-type velocity curve algorithm judges based on these initial parameters.

Whether it is established, if it is established, the specific time parameters T _j1 , T _j2 , T _a and T _d are calculated using equation (1). If not, the formula (2) is used to calculate the specific time parameters T _j1 , T _j2 , T _a and T _d . Thereafter, after T _j1 , T _j2 , T _{a ,} and T _d are obtained, T _v is calculated according to the T _a calculated by the formula (3). Next, it is judged whether T _v >0 is established, and if it is established, the final t ₀ , t ₁ , t ₂ , t ₃ , t ₄ , t ₅ , t ₆ are calculated using equations (6) and (7). t ₇ . If not, further judge whether it is satisfied

If it is satisfied, T _v and new T _a and T _d are obtained according to formula (4), and the final t ₀ , t ₁ , t ₂ , t ₃ , t are calculated according to formula (6) and formula (7). ₄ , t ₅ , t ₆ , t ₇ . If not, calculate T _v and new T _j1 , T _j2 , T _a and T _d using equation (5), and calculate the final t ₀ , t ₁ according to formula (6) and formula (7). , t ₂ , t ₃ , t ₄ , t ₅ , t ₆ , t ₇ . The specific forms of the formulas (1) to (7) are as follows:

In the embodiment of the present invention, after obtaining the formula or the equation as described in the formulas (6), (7), the position of the robot arm at any time can be obtained, and the specific formula (8) can be referred to as follows:

S102. Acquire a current motion time of the robot arm after receiving a stop command issued by the user, and determine a current motion phase of the robot arm according to the current motion time and a duration configured for each motion phase. .

In the embodiment of the present invention, when the user uses the mechanical arm, the user may issue a stop command as needed (for example, during the teaching, the user can issue a stop command to the robot arm through the teaching box), then After receiving the stop command issued by the user, the motion trajectory planning device acquires the current motion time t of the robot arm, and determines, according to the current motion time t and the duration (or time interval), that the robot arm is currently located. The stage of exercise.

For example, if the motion trajectory planning device detects that the current running time is 5S, and t ₃ = 4s, t ₄ = 6S, it can be determined that the current motion phase is a uniform motion phase. For another example, if the motion trajectory planning device detects that the current running time is 2S, and t ₁ = 1.5 s, t ₄ = 3 S, it can be determined that the current motion phase is the second acceleration motion phase.

S103. Change a duration of a predetermined motion phase and a predetermined initial parameter in the initial parameter set according to a motion phase in which the motion arm is currently in motion, generate a modified motion track, and control the robot arm to be modified. The motion track is moving.

In the embodiment of the present invention, after the user issues a stop command, the motion trajectory planning device needs to stop the robot arm as soon as possible and remains stable. For this reason, the motion trajectory planning device needs to be according to the motion. The motion phase in which the arm is currently is changing the duration of the predetermined motion phase and the predetermined initial parameters in the initial set of parameters.

specifically:

S1031. When it is determined that the mechanical arm is currently in the first acceleration motion phase, the durations of the first acceleration motion phase, the third acceleration motion phase, the first deceleration motion phase, and the third deceleration motion phase Change to t and change the duration of the second acceleration motion phase, the uniform velocity motion phase, and the second deceleration motion phase to zero.

In the embodiment of the present invention, when the motion trajectory planning device determines that the user issues a stop command during the first acceleration motion phase, that is, the current running time t<t ₁ , the motion trajectory planning device immediately terminates the first acceleration motion phase. At the same time, in order to stop the robot arm as soon as possible, the durations of the second acceleration motion phase, the uniform velocity motion phase, and the second deceleration motion phase are also changed to zero.

Then, the time interval of each motion phase becomes: the time interval corresponding to the first acceleration motion phase is [t ₀ , t _1n1 ), and the time interval corresponding to the second acceleration motion phase is [t _1n1 , t _2n1 ), the time interval corresponding to the third acceleration motion phase is [t _2n1 , t _3n1 ), and the time interval corresponding to the uniform motion phase is [t _3n1 , t _4n1 ), and the first deceleration phase corresponds to The time interval is [t _4n1 , t _5n1 ), the time interval corresponding to the second deceleration motion phase is [t _5n1 , t _6n1 ), and the time interval corresponding to the third deceleration motion phase is changed to [t _6n1 , t _7n1 ), where t ₀ =0, t _1n1 = t _2n1 = t, t _3n1 = t _4n1 = 2t, t _5n1 = t _6n1 = 3t, t _7n1 = 4t.

S1032, according to the current motion time, the initial parameters of the path, the maximum speed, the maximum acceleration of the acceleration motion phase, and the minimum deceleration phase of the deceleration motion phase are changed, and the changed motion trajectory is generated according to the changed initial parameters, and the control center is controlled. The robot arm moves with the changed motion trajectory.

Wherein, the maximum acceleration a _{max of the} acceleration motion phase is changed to j _max *t, the minimum deceleration a _{min of the} deceleration motion phase is changed to -j _min *t, and the maximum velocity v _{max is} changed to t*a _max q _{1 is} changed to t*a _max *2t, after which the changed motion trajectory can be obtained according to formula (8), and the robot arm is controlled to move with the changed motion trajectory.

specifically:

S1033, when it is determined that the mechanical arm is currently in the second acceleration motion phase, change the duration of the second acceleration motion phase and the second deceleration motion phase to tt ₁ , and change the duration of the uniform motion phase Zero.

In the embodiment of the present invention, when the motion trajectory planning device determines that the user issues a stop command during the second acceleration motion phase, when the current running time t satisfies: t ₁ <t<t ₂ , the motion trajectory planning device Immediately terminating the second acceleration motion phase, that is, the second acceleration motion phase, and in order to stop the robot arm as soon as possible, the duration of the second deceleration motion phase is also changed to tt ₁ , and the uniform motion phase is changed. Zero.

Then, the time interval of each motion phase becomes: the time interval corresponding to the first acceleration motion phase is [t ₀ , t ₁ ), and the time interval corresponding to the second acceleration motion phase is [t ₁ , t The time interval corresponding to the third acceleration motion phase is [t, t _3n2 ), and the time interval corresponding to the uniform motion phase is [t _3n2 , t _4n2 ), and the time interval corresponding to the first deceleration motion phase [t _4n2 , t _5n2 ), the time interval corresponding to the second deceleration motion phase is [t _5n2 , t _6n2 ), and the time interval corresponding to the third deceleration motion phase is changed to [t _6n2 , t _7n2 ), Where t ₀ =0, t _3n2 = t _4n2 = t + T _j1 , t _5n2 = t _4n2 + T _j2 , t _6n2 = t _5n2 + tt ₁ , t _7n2 = t _6n2 + T _j2 .

S1034, according to the current motion time t, the two initial parameters of the distance q ₁ and the maximum speed v _max are changed, the changed motion track is generated according to the changed initial parameter, and the robot arm is controlled. The changed motion track is moved.

Wherein, the changed v _max =(TT _j1 )j _max T _j1 , and the changed q ₁ =(TT _j1 )a _max T, T=T _a -(t ₂ -t).

Thereafter, the changed motion trajectory can be obtained according to formula (8), and the robot arm is controlled to move with the changed motion trajectory.

specifically:

S1035, when it is determined that the mechanical arm is currently in a third acceleration motion phase, changing the duration of the uniform motion phase Zero.

In the embodiment of the present invention, when the motion trajectory planning device determines that the user issues a stop command during the third acceleration motion phase, the current running time t satisfies: t ₂ <t<t ₃ , in order to stop the mechanism as soon as possible The arm changes the uniform motion phase to zero.

Then, the time interval of each motion phase becomes: the time interval corresponding to the first acceleration motion phase is [t ₀ , t ₁ ), and the time interval corresponding to the second acceleration motion phase is [t ₁ , t ₂ ), the time interval corresponding to the third acceleration motion phase is [t ₂ , t ₃ ), and the time interval corresponding to the uniform motion phase is [t ₃ , t _4n3 ), and the first deceleration phase corresponds to The time interval is [t _4n3 , t _5n3 ), the time interval corresponding to the second deceleration motion phase is [t _5n3 , t _6n3 ), and the time interval corresponding to the third deceleration motion phase is changed to [t _6n3 , t _7n3 ), where t ₀ =0, t _4n3 = t ₃ , t _5n3 = t _4n3 + T _j2 , t _6n3 = t _5n3 + T _{d -} _2T _j2 , t _7n2 = t _6n3 + T _j2 .

S1036: The initial parameter q ₁ is changed according to the current motion time t, the changed motion track is generated according to the changed initial parameter, and the robot arm is controlled to move with the changed motion track.

Where q ₁ =q ₁ -v _max T _v . Thereafter, the changed motion trajectory can be obtained according to formula (8), and the robot arm is controlled to move with the changed motion trajectory.

specifically:

S1037, when it is determined that the mechanical arm is currently in a uniform motion phase, change the duration of the uniform motion phase to tt ₃ .

At this time, the time interval of each motion phase becomes: the time interval corresponding to the first acceleration motion phase is [t ₀ , t ₁ ), and the time interval corresponding to the second acceleration motion phase is [t ₁ , t ₂ The time interval corresponding to the third acceleration motion phase is [t ₂ , t ₃ ), the time interval corresponding to the uniform motion phase is [t ₃ , t), and the time interval corresponding to the first deceleration motion phase For [t, t _5n4 ), the time interval corresponding to the second deceleration motion phase is [t _5n4 , t _6n4 ), and the time interval corresponding to the third deceleration motion phase is changed to [t _6n4 , t _7n4 ), wherein , t _5n4 = t + T _j2 , t _6n4 = t _5n4 + T _{d -} _2T _j2 , t _7n4 = t _6n4 + T _j2 .

S1038: The initial parameter of the path q ₁ is changed according to the current motion time t, the changed motion track is generated according to the changed initial parameter, and the robot arm is controlled to move with the changed motion track.

Wherein, the changed path q ₁ =q ₁ -v _max [T _v -(tt ₃ )]. Thereafter, the changed motion trajectory can be obtained according to formula (8), and the robot arm is controlled to move with the changed motion trajectory.

In the embodiment of the present invention, if the user's stop command is received during the deceleration phase, the motion track is not changed.

In summary, the method for planning the motion trajectory of the robot arm according to the embodiment of the present invention, after receiving the stop command of the user before detecting the entering the deceleration motion phase, the current and subsequent motion trajectories according to the current motion phase. The re-planning is carried out so that the moving arm stops as soon as possible while maintaining smooth motion and continuous speed, thereby satisfying the user's use requirements.

It should be noted that, in the above embodiment, the speed curve algorithm is an S-type speed curve algorithm, but the idea of the present invention is equally applicable to other speed curve algorithms, and in particular, to a speed curve having multiple motion stages. The algorithms are all within the scope of the present invention and will not be described herein.

It should be noted that, in the above embodiment, before the motion, it is necessary to obtain the coordinates of the end point, and then obtain the path q ₁ according to the coordinates of the starting point, the direction of motion and the coordinates of the end point. The coordinates of the end point provided by a preferred embodiment of the present invention will be described below. Get the method.

Specifically, the endpoint coordinates can be obtained based on the following steps:

S01. Generate a working area of the robot arm based on joint parameters of the robot arm to be operated.

In general, a robotic arm includes a plurality of joints, each joint having joint parameters, which may include joint type, arm length (length of the connecting arm connecting the two joints), range of motion, and the like. Wherein, the joint type may include a rotating joint and a sliding joint, the rotating joint may control the rotation of the connecting arm, and the sliding joint may realize the vertical sliding of the connecting arm. The working area of the robot arm can be controlled by a combination of different joints. Specifically, the working area of the robot arm can be determined by the following steps:

S011, according to the joint type of the robot arm and the relative positional relationship between the joints, a coordinate system for generating each joint is established based on the DH coordinate system.

As shown in FIG. 4, a SCARA (Selective Compliant Articulated Robot for Assembly) type robot arm is exemplified, which has four joints J1, J2, J3, and J4, wherein J1, J2, and J4 are rotating joints, and J1 and J2, J2 and J4 are connected by corresponding connecting arms, and J3 is a sliding joint. Of course, it should be understood that the present invention is also applicable to other types of robot arms, and details are not described herein.

After determining the above joints, it is necessary to establish a coordinate system for each joint. As shown in FIG. 5, a coordinate system of each joint can be established based on the DH (Denavit-Hartenberg) coordinate system, wherein the rotational axes of the rotating joints Ji and their respective The zi axis (the connecting arm of the rotating joint Ji rotates around the zi axis) is parallel to each other, the arm length of the connecting arm is αi, {i|i∈{1, 2, 4}}; the axis of the sliding joint J3 (z3 axis) and J4 The axes (z4 axes) are parallel. After the zi axis is determined, the direction of the first joint pointing to the second joint can be used as the xi direction, and the direction of yi can be determined based on the right hand rule, thus, the coordinate system of each joint is generated.

S012, generating a working area of the robot arm according to a coordinate system of each joint, an active range of each joint, and an arm length between the joints.

In the embodiment of the present invention, after generating the coordinate system of each joint, according to the range of motion of each joint (for the rotating joint, the range of motion is the range of the angle of rotation of the connecting arm rotation, and for the sliding joint, the range of motion is The working range of the arm can be generated by the upper and lower movement range of the connecting arm (as shown in FIG. 6). In this case, a plan view as shown in FIG. 7 can be generated by simply projecting on the plane.

S02: Generate a ray according to the coordinates and the moving direction of the starting point input in advance, and calculate an intersection of the ray and the boundary of the working area to generate a set of intersection points.

Specifically, it may include:

S021: Decompose the working area into at least two arcs, obtain a circle center, a radius, and a central angle range of each arc, and generate an equation of a circle corresponding to each arc.

As can be seen from Figure 7, the working area of the robot arm is

Four arcs are enclosed. The four arcs are located on four circles: ⊙O ₁ , ⊙O ₂ , ⊙O ₃ , ⊙O ₄ . The center O ₁ , O ₄ coincides with the J ₁ axis; O ₂ and O ₃ are the positions of the J ₂ axis when the J ₁ axis is rotated to the positive and negative limits, respectively.

with

The maximum range of motion in the positive/negative direction of the axis J _i , respectively

^{^{{j i | -π≤j i ≤π}}} .

The detailed parameters of these four arcs can be seen in Table 1:

Table 1

among them:

S022, generating a ray according to the coordinates and the moving direction of the starting point input in advance, and calculating an intersection of the ray and each circle.

In the embodiment of the present invention, it is assumed that the starting point is I, the direction vector is n, and ||n|| is a unit vector of the direction vector n. Then the ray can be expressed as R(u)=I+u·||n||.

If the starting point I is in the working area, the ray R(u) must intersect with a circle ⊙O _i

Wherein, when calculating the intersection point, the equation of the ray R(u) can be sequentially connected with the equation of each circle, and obtained by using the parametric equation method.

S023, calculating an arc angle of the intersection point on a circle where the intersection point is located, and when the arc angle angle is within the circle center angle range, determining that the intersection point is located on the arc, and depositing the intersection point A preset collection that generates a collection of intersections.

In the embodiment of the present invention, some of the intersections obtained above intersect with the circle, but are not located on the arc, and thus need to be removed. Specifically, as shown in FIG. 8, the ray IP intersects ⊙O ₁ at point N, respectively; ⊙O ₂ intersects at point K and point M; ⊙O ₄ intersects point J and point L; and ⊙O ₃ does not Intersection. Wherein, the points K, N are on the circle, but are not on the arc of the working area, so after obtaining the intersection point, it is also necessary to check whether the arc angle θ ^{i of} the intersection point satisfies

Specifically, assuming that the point s (x, y) is a point on ⊙O _i, with respect to the point s ⊙O _i θ ⁱ is the angle in radians:

Through the above formula, you can determine which intersections are on the arc and which are not. At this point, those intersections located on the arc are stored in the preset set ξ={k ⁱ } to generate a set of intersections.

It should be noted that, in other embodiments of the present invention, the working area does not necessarily consist of an arc of a circle. For example, the working area may be composed of an elliptical arc or a mixture of arcs of different types of geometric shapes. At this time, similarly, the equations and angular ranges of the geometric shapes corresponding to the arcs may be obtained, and then The method can be used to calculate and generate a set of intersection points. These technical solutions are all within the scope of the present invention and will not be described herein.

S03. Calculate a distance between each intersection point in the intersection set and the starting point, and obtain coordinates of an intersection point corresponding to the minimum distance. The intersection is marked as the end point.

In the embodiment of the present invention, the distance between each intersection point and the starting point I can be calculated by the Euler formula, and then each calculated distance is compared, and the coordinates of the intersection point corresponding to the minimum distance are obtained, and the The intersection point is marked as the end point, at which point the boundary of the desired work area is obtained.

In summary, in the preferred embodiment, by generating the working area of the robot arm, the intersection of the ray generated by the coordinates and the moving direction of the starting point input in advance and the boundary of the working area is calculated, and the acquisition and the starting point have The coordinates of the intersection of the minimum distances are obtained at the end points corresponding to the starting point and the moving direction. The invention is based on a geometric method to determine the boundary of the working area of the robot arm, so that the robot can know the position of the end point in advance before the movement, so that the robot can plan the motion trajectory.

Preferably, for step S022, after the ray is generated, a conventional parametric equation method may be used to find an intersection point, or may be used.

An optimization algorithm based on the ray and circle intersection test is proposed to calculate the intersection of the ray and each circle.

Specifically, as shown in FIG. 9, the parameter equation of the ray is R(u)=I+u·||n||, where ||n|| is a unit length. As shown in Fig. 10(a), first calculate the vector from the starting point I to the center of the circle O _i

vector

length

And vector

Projection along the ||n|| direction

If

And l<0, it means that the starting point is outside ⊙O _i , and the direction of the ray extends away from the ⊙O _i direction, so the ray does not intersect with ⊙O _i (as shown in FIG. 10( b )), and the completion is completed. The first exclusion test. Otherwise, use the Pythagorean theorem to calculate the square of the distance between the center of the circle O _i and the projection: m ² = a ^{2 -} l ² , if

It can be determined that the ray and the circle O _i must not intersect, and the second exclusion test is completed. If the ray and ⊙O _{i are} subjected to two exclusion tests, it can be determined that they must intersect. Next, calculate the intersection of the ray and ⊙O _i : first, calculate the distance

Then determine whether the starting point I is located in ⊙O _i , if

Then, the starting point I is located outside the ⊙O _i . At this time, the ray has two intersections with the circle O _i , respectively

as well as

(As shown in Figure 10 (a)). If

Then, the starting point I is located in the circle O _i . At this time, the ray has an intersection with the circle O _i , which is I+(l+q)·||n|| (as shown in FIG. 10(c)).

In the preferred embodiment, before calculating the intersection of the ray and the circle, it is first determined whether there is an intersection of the ray and the circle by two tests. In the calculation, only the circle having the intersection with the ray is calculated, and there is no need for those points that do not intersect with the ray. The circle is calculated, thus reducing the amount of calculation and improving the calculation efficiency.

Referring to FIG. 11 together, FIG. 11 is a motion trajectory planning apparatus 100 for a mechanical arm according to an embodiment of the present invention, which includes:

a motion control unit 10, configured to control the robot arm to move according to a predetermined motion trajectory according to preset start point coordinates, an end point coordinate, and a speed curve algorithm; wherein the speed curve algorithm is to move the machine according to a preset initial parameter set The trajectory of the arm is planned for at least two phases of motion and a duration is configured for each phase of motion.

The motion time acquisition unit 20 is configured to acquire a current motion time t of the robot arm after receiving a stop command issued by the user, and determine the current motion time t and the duration configured for each motion phase. The current stage of motion of the robotic arm.

a motion trajectory changing unit 30, configured to change a duration of a predetermined motion phase and a predetermined initial parameter in the initial parameter set according to a motion phase in which the motion arm is currently located, generate a modified motion trajectory, and control the Mechanical arm The trajectory moves.

Specifically:

When the motion trajectory changing unit 30 determines that the mechanical arm is currently in the first acceleration motion phase, the first acceleration motion phase, the third acceleration motion phase, the first deceleration motion phase, and the third deceleration The duration of the motion phase is changed to t, and the durations of the second acceleration motion phase, the uniform motion phase, and the second deceleration phase are changed to zero; thereafter, based on the current motion time t, the initial parameter a _max , a _min , v _max and q ₁ are changed, the changed motion trajectory is generated according to the changed initial parameters, and the robot arm is controlled to move with the changed motion trajectory.

specifically:

When the motion trajectory changing unit 30 determines that the mechanical arm is currently in the second acceleration motion phase, change the duration of the second acceleration motion phase and the second deceleration motion phase to tt ₂ , and move the uniform motion The duration of the phase is changed to zero. Thereafter, the initial parameters v _max and q ₁ are changed according to the current motion time t, the changed motion trajectory is generated according to the changed initial parameters, and the robot arm is controlled to be changed. The motion track is moving.

specifically:

When the motion trajectory changing unit 30 determines that the mechanical arm is currently in the third acceleration motion phase, the duration of the uniform motion phase is changed to zero;

The initial parameter q ₁ is changed according to the current motion time t, the changed motion trajectory is generated according to the changed initial parameter, and the robot arm is controlled to move with the changed motion trajectory.

specifically:

When the motion track changing unit 30 determines that the mechanical arm is currently in a uniform motion phase, change the duration of the uniform motion phase to tt ₃ ;

The motion trajectory planning apparatus 100 of the robot arm according to the embodiment of the present invention re-plans the current and subsequent motion trajectories according to the current motion stage after receiving the user's stop command before detecting the entering the deceleration motion phase. The moving arm is stopped as soon as possible while maintaining smooth motion and continuous speed, thereby satisfying the user's use requirements.

Referring to FIG. 12, it should be noted that, in the embodiment of the present invention, in order to obtain the end point coordinates of the mechanical arm, the motion trajectory planning device of the mechanical arm further includes:

The work area generating unit 40 is configured to generate a working area of the mechanical arm based on joint parameters of the mechanical arm to be operated.

The intersection point generation unit 50 is configured to generate a ray according to the coordinates and the movement direction of the start point input in advance, and calculate an intersection of the ray and the boundary of the work area to generate an intersection set.

The end point marking unit 60 is configured to calculate a distance between each intersection point in the intersection point set and the starting point, acquire coordinates of an intersection point corresponding to the minimum distance, and mark the intersection point as an end point.

In the embodiment of the present invention, the working area generating unit 40 generates the working area of the robot arm, and the intersection point generating unit 50 calculates the intersection of the ray generated by the coordinates and the moving direction of the starting point input in advance and the boundary of the working area. The end point marking unit 60 acquires the coordinates of the intersection point having the smallest distance from the starting point, obtains the end point corresponding to the starting point and the moving direction, and then performs the planning of the motion trajectory according to the starting point, the ending point and the preset trajectory planning algorithm. The invention is based on a geometric method to determine the boundary of the working area of the robot arm, so that the robot can know the position of the end point in advance before the movement, so that the robot can plan the motion trajectory.

The present invention also provides a robot comprising the above described motion trajectory planning apparatus 100.

The robot provided by the embodiment of the present invention, after receiving the stop command of the user before detecting the entering the deceleration motion phase, re-plans the current and subsequent motion trajectories according to the current motion phase, so that the motion arm is maintained. Under the premise of smooth motion and continuous speed, stop as soon as possible to meet the user's needs.

The robot provided by the embodiment of the present invention re-plans the entire motion trajectory after receiving the stop command of the user before entering the deceleration motion phase, so that the motion arm stops as soon as possible while maintaining smooth motion and continuous speed. In order to meet the user's needs.

Claims

A method for planning a motion trajectory of a mechanical arm, comprising the steps of:

Controlling the robot arm to move according to a predetermined motion trajectory according to preset start point coordinates, end point coordinates, and speed curve algorithm; wherein the speed curve algorithm plans the motion trajectory of the arm according to a preset initial parameter set to at least Two phases of motion and a duration for each phase of motion;

Obtaining a current motion time of the robot arm after receiving a stop command issued by the user, and determining a current motion phase of the robot arm according to the current motion time and a duration configured for each motion phase;

Generating a modified motion trajectory according to a duration of the motion stage in which the moving arm is currently in motion and a predetermined initial parameter in the initial parameter set, and controlling the mechanical arm to change motion The trajectory moves.
The method for planning a trajectory of a robot arm according to claim 1, wherein the speed curve algorithm is an S-type speed curve algorithm, and the initial parameter set includes at least a maximum acceleration and a maximum speed of the path and the acceleration phase. The initial parameters of the minimum deceleration, the maximum jerk, and the minimum jerk of the deceleration motion phase; the motion phase includes an acceleration motion phase, a uniform motion phase, and a deceleration motion phase, where the acceleration motion phase includes a first acceleration motion phase, The second acceleration motion phase and the third acceleration motion phase include: a first deceleration motion phase, a second deceleration motion phase, and a third deceleration motion phase.
The method of planning a trajectory of a robot arm according to claim 2, wherein said changing a duration of a predetermined motion phase and a predetermined one of said initial parameter sets according to a motion phase in which said arm is currently in motion The initial parameter generates a changed motion trajectory and controls the robot arm to move with the changed motion trajectory, specifically:

When it is determined that the mechanical arm is currently in the first acceleration motion phase, changing the durations of the first acceleration motion phase, the third acceleration motion phase, the first deceleration motion phase, and the third deceleration motion phase to a current exercise time, and changing the duration of the second accelerated motion phase, the uniform velocity motion phase, and the second deceleration motion phase to zero;

And changing, according to the current movement time, the initial parameters of the distance, the maximum speed, the maximum acceleration of the acceleration motion phase, and the minimum deceleration of the deceleration motion phase, and generating the changed according to the changed initial parameters. Motion trajectory, and control the robot arm to move with the changed motion trajectory.
The method of planning a trajectory of a robot arm according to claim 2, wherein said changing a duration of a predetermined motion phase and a predetermined one of said initial parameter sets according to a motion phase in which said arm is currently in motion The initial parameter generates a changed motion trajectory and controls the robot arm to move with the changed motion trajectory, specifically:

When it is determined that the mechanical arm is currently in the second acceleration motion phase, change the duration of the second acceleration motion phase and the second deceleration motion phase to tt 1 and change the duration of the uniform motion phase to zero Where t is the current exercise time, and t 1 -t 0 is a pre-configured duration from the start time t 0 to the end of the first acceleration motion phase;

And modifying the two initial parameters of the distance and the maximum speed according to the current motion time t, generating a changed motion trajectory according to the changed initial parameter, and controlling the robot arm to perform the changed motion trajectory. motion.
The method of planning a trajectory of a robot arm according to claim 2, wherein said changing a duration of a predetermined motion phase and a predetermined one of said initial parameter sets according to a motion phase in which said arm is currently in motion The initial parameter generates a changed motion trajectory and controls the robot arm to move with the changed motion trajectory, specifically:

When it is determined that the mechanical arm is currently in the third acceleration motion phase, the duration of the uniform motion phase is changed to zero;

The initial parameter of the path is changed according to the current motion time, the changed motion track is generated according to the changed initial parameter, and the robot arm is controlled to move with the changed motion track.
The method of planning a trajectory of a robot arm according to claim 2, wherein said changing a duration of a predetermined motion phase and a predetermined one of said initial parameter sets according to a motion phase in which said arm is currently in motion The initial parameter generates a changed motion trajectory and controls the robot arm to move with the changed motion trajectory, specifically:

When it is determined that the mechanical arm is currently in a uniform motion phase, the duration of the uniform motion phase is changed to tt 3 ; wherein t is the current motion time, and t 3 -t 0 is a pre-configured time t from the start timing 0 to the duration of the end of the third acceleration motion phase;

The initial parameter of the path is changed according to the current motion time t, the changed motion track is generated according to the changed initial parameter, and the robot arm is controlled to move with the changed motion track.
The method for planning a motion trajectory of a robot arm according to any one of claims 1 to 6, wherein the control robot arm performs a predetermined motion trajectory according to the preset starting point coordinates, the end point coordinates, and the speed curve algorithm. Before the exercise, it also includes:

Generating a working area of the robot arm based on a joint parameter of the robot arm;

Generating a ray according to a coordinate and a moving direction of the starting point input in advance, and calculating an intersection of the ray and a boundary of the working area to generate a set of intersection points;

Calculating a distance between each intersection point in the intersection set and the starting point, acquiring coordinates of an intersection point corresponding to the minimum distance, and marking the intersection point as an end point.
A motion trajectory planning device for a mechanical arm, comprising:

a motion control unit, configured to control the robot arm to move according to a predetermined motion trajectory according to a preset starting point coordinate, an end point coordinate, and a velocity curve algorithm; wherein the speed curve algorithm is to move the mechanical arm according to a preset initial parameter set The motion trajectory is planned for at least two motion phases, and each motion phase is configured for a duration;

a motion time acquisition unit, configured to acquire a current motion time of the robot arm after receiving a stop command issued by a user, and determine, according to the current motion time and a duration configured for each motion phase, the current arm The stage of exercise

a motion trajectory changing unit, configured to change a duration of a predetermined motion phase and a predetermined initial parameter in the initial parameter set according to a motion phase in which the motion arm is currently in motion, generate a modified motion trajectory, and control the Robotic arm with altered motion The trajectory moves.
The trajectory planning device for a robot arm according to claim 8, further comprising:

a work area generating unit, configured to generate a working area of the mechanical arm based on a joint parameter of the mechanical arm to be operated;

An intersection point generating unit configured to generate a ray according to a coordinate and a moving direction of the starting point input in advance, and calculate an intersection of the ray and a boundary of the working area to generate a set of intersection points;

An end point marking unit is configured to calculate a distance of each intersection point in the intersection point set from the starting point, acquire coordinates of an intersection point corresponding to the minimum distance, and mark the intersection point as an end point.
A robot comprising the motion trajectory planning device of the mechanical arm according to claim 8 or 9.