JP5829968B2 - Articulated robot, joint cooperative control apparatus and method thereof - Google Patents

Description

  The present invention relates to a joint control apparatus and method for an articulated robot, and in particular, it is possible to reduce joint jerk input and obtain joint input satisfying upper and lower limits of joint displacement, speed, and acceleration. The present invention relates to a multi-joint robot, and a joint cooperative control apparatus and method thereof.

  Joint cooperative control is known as a method for operating a target by cooperatively operating a joint of a robot. Here, “joint” mainly refers to the driving joint of the robot, but if the robot is not fixed to the ground, such as a wheeled robot or a humanoid robot, the degree of freedom of its reference coordinate system is also It can be thought of as a virtual joint. The “target” refers to a part of the robot that is to be operated. For example, when the robot includes a manipulator, the target is the hand, and the degree of freedom of the target is the position and posture of the hand. As a target, a point on an arbitrary link, the position of the center of gravity of the entire robot, and the like can be considered.

  Conventionally, in joint cooperative control for articulated robots having redundant degrees of freedom, a method using joint speed as an input is often used.

JP 2009-187521 A

  However, in the method using the joint speed as an input, due to minimizing the square error of the speed of the task with the lowest priority, there may be (A) a trajectory where the speed changes suddenly or an oscillatory trajectory. The amount of change in joint acceleration (hereinafter referred to as jerk) becomes large (that is, the acceleration becomes discontinuous), and (B) the joint speed becomes discontinuous when the task is switched. (That is, it is impossible to obtain input to a joint that necessarily satisfies the upper and lower limit constraints of joint displacement, velocity, and acceleration).

  The above (A) occurs because no limitation is imposed on the acceleration of the joint with respect to the minimization of the speed error. The above (B) occurs when the priority of a task is switched for each step or when a task is added or excluded. In such a case, the object to be optimized Since the function structure is changed to a completely different one, the obtained joint velocity changes discontinuously. When such a problem occurs, the obtained joint velocity input trajectory does not become smooth and cannot be applied to the robot as it is.

  Therefore, with regard to the main joint coordinated control of the robot, (1) the joint input for manipulating the target can be obtained in the multi-joint robot, (2) the upper and lower limit constraints of the joint displacement, velocity, and acceleration are always satisfied There is a strong demand for a technique that can obtain an input of a joint, and (3) an input to a joint can reduce the jerk as much as possible and smooth the acceleration.

  In addition, there exists a technique disclosed by patent document 1 as a technique relevant to this invention. In the technique disclosed in Patent Document 1, although the jerk of the joint is limited, the jerk at the apex of the velocity pattern is only limited, and as small as possible as shown in (3) above. It does not realize the degree of leaps. In addition, the target is a single-axis robot, and the joint and the target have a one-to-one correspondence, so the above (1) is not considered. Furthermore, no consideration is given to the restrictions on joint displacement, velocity, and acceleration shown in (2) above.

  The present invention has been made to solve the above-described problems, and can reduce the jerk input to the joint and obtain a joint input satisfying the upper and lower limits of the joint displacement, speed, and acceleration. It is an object of the present invention to provide a possible multi-joint robot and its joint cooperative control apparatus and method.

  A joint cooperative control device for a multi-joint robot according to the present invention is a joint cooperative control device for a multi-joint robot that operates a target part by cooperatively controlling joints of the multi-joint robot, and starts from the state of the joint at the current time Then, the speed and acceleration of the joint reach 0 at the upper or lower limit of the set displacement of the joint after a predetermined time while satisfying all the upper and lower limit settings of the speed, acceleration, and jerk of the joint. Determining a possible trajectory pattern of the jerk input to the joint, upper and lower limits of jerk input to the joint calculated from the determined trajectory pattern, the state of the joint at the current time, and the joint The upper and lower limit values of the jerk input to the joint calculated from the set speed, acceleration and jerk upper and lower limits of the jerk are used to calculate the upper and lower limit range of the jerk input to the joint. Equality constraints including an upper / lower limit constraint calculation unit, an equality constraint on the jerk input to the joint and the jerk input to the joint, and an inequality constraint on an upper / lower limit range of the jerk input to the joint And a task in which a solution exists among a plurality of setting tasks related to the equality constraint, and an upper and lower limit range of jerk input to the joint calculated by an upper and lower limit constraint calculation unit to the jerk input , And an error of a jerk input to the joint in a task for which no solution exists among a plurality of setting tasks related to the equality constraint as an optimization evaluation function, and the evaluation under the constraint An optimization problem solving unit that solves an optimization problem so as to optimize a function value and calculates a jerk input to the joint. As a result, it is possible to reduce the jerk input to the joint and to obtain a joint input that satisfies the upper and lower limit constraints of the joint displacement, speed, and acceleration.

  Further, the upper / lower limit constraint calculation unit for the jerk input includes, as the stop trajectory pattern, a first stop trajectory pattern in which jerk input to the joint changes from a set minimum value to a set maximum value, and the joint The second stop trajectory pattern in which the jerk input to the joint changes from the set minimum value through zero to the set maximum value, and the jerk input to the joint returns from the set minimum value through zero to the set minimum value Later, after the third stop trajectory pattern changing to the set maximum value and the jerk input to the joint return from the set minimum value to 0 through the set minimum value, to 0 through the set minimum value again to the set maximum value The fourth stop trajectory pattern that changes is determined, and when any stop trajectory pattern is used among the determined first to fourth stop trajectory patterns, the setting is made after the predetermined time has elapsed. The upper or lower limit of the displacement of the joint It is determined whether the velocity and acceleration of the joint can reach zero, and the upper and lower limit values of the jerk input to the joint calculated from the stop trajectory pattern trajectory pattern determined to be reachable, and the current time Upper and lower limits of jerk input to the joint using the joint state and upper and lower limit values of jerk input to the joint calculated from the set speed, acceleration, and jerk constraints of the jerk The range may be calculated.

  Furthermore, the articulated robot may be a robot having a plurality of degrees of freedom arms.

  The articulated joint control method of the articulated robot according to the present invention is a articulated joint control method of the articulated robot that operates the target part by cooperatively controlling the joint of the articulated robot, and starts from the state of the joint at the current time Then, the speed and acceleration of the joint reach 0 at the upper or lower limit of the set displacement of the joint after a predetermined time while satisfying all the upper and lower limit settings of the speed, acceleration, and jerk of the joint. Determining a possible trajectory pattern of the jerk input to the joint, upper and lower limits of jerk input to the joint calculated from the determined trajectory pattern, the state of the joint at the current time, and the joint The upper and lower limit values of the jerk input to the joint calculated from the set speed, acceleration and jerk upper and lower limits of the jerk are used to calculate the upper and lower limit range of the jerk input to the joint. Equal constraint including the upper and lower limit constraint calculation step, the equality constraint of the jerk of the target part and the jerk input to the joint, and the inequality constraint of the upper and lower limit range of the jerk input to the joint And a task in which a solution exists among a plurality of setting tasks related to the equality constraint, and an upper and lower limit range of jerk input to the joint calculated in an upper and lower limit constraint calculation step to the jerk input , And an error of a jerk input to the joint in a task for which no solution exists among a plurality of setting tasks related to the equality constraint as an optimization evaluation function, and the evaluation under the constraint An optimization problem solving step of solving an optimization problem so as to optimize a function value and calculating a jerk input to the joint. As a result, it is possible to reduce the jerk input to the joint and to obtain a joint input that satisfies the upper and lower limit constraints of the joint displacement, speed, and acceleration.

  Furthermore, the upper and lower limit constraint calculation step for the jerk input includes, as the stop trajectory pattern, a first stop trajectory pattern in which jerk input to the joint changes from a set minimum value to a set maximum value, The second stop trajectory pattern in which the jerk input to the joint changes from the set minimum value through 0 to the set maximum value, and the jerk input into the joint returns from the set minimum value through 0 to the set minimum value After that, the third stop trajectory pattern changing to the set maximum value and the jerk input to the joint return from the set minimum value to 0 through the set minimum value, and then through 0 again to the set maximum value. A fourth stop trajectory pattern that changes, and when any stop trajectory pattern is used among the determined first to fourth stop trajectory patterns, the setting is performed after the predetermined time has elapsed. Upper limit of the displacement of the joint or It is determined whether or not the speed and acceleration of the joint can reach zero, and the upper and lower limit values of the jerk input to the joint calculated from the stop trajectory pattern trajectory pattern determined to be reachable and the current time And the upper and lower limit values of the jerk input to the joint calculated from the upper and lower limits of the joint speed, acceleration, and jerk, and the jerk input to the joint. The lower limit range may be calculated.

  The articulated robot according to the present invention is an articulated robot that operates a target part by cooperatively controlling a joint, and starts from the state of the joint at a current time, and sets the speed, acceleration, and jerk of the joint Determines the trajectory pattern of the jerk input to the joint at which the joint velocity and acceleration can reach zero at the upper or lower limit of the joint displacement set after a predetermined time while satisfying all upper and lower limit constraints Calculated from the upper and lower limit values of the jerk input to the joint calculated from the determined trajectory pattern, the state of the joint at the current time, and the set speed, acceleration, and jerk limits of the joint Upper and lower limit values for the jerk input to calculate the jerk input to the joint using the upper and lower limit values of the jerk input to the joint, and the jerk of the target part And said Seki The task is an equality constraint that includes an equality constraint on the input to the joint and an inequality constraint on the upper and lower limit ranges of the input to the joint, and the solution is a set of tasks related to the equality constraint. An existing task and an upper / lower limit range of the jerk input to the joint calculated by the upper / lower limit constraint calculation unit for the jerk input, and among the plurality of setting tasks related to the equation constraint The error of the jerk input to the joint in a task for which no solution exists in is used as an optimization evaluation function, and an optimization problem is solved so that the value of the evaluation function is optimal under the constraint conditions, An optimization problem solving unit for calculating a jerk input to the joint. As a result, it is possible to reduce the jerk input to the joint and to obtain a joint input that satisfies the upper and lower limit constraints of the joint displacement, speed, and acceleration.

  According to the present invention, a multi-joint robot capable of reducing a jerk input to a joint and obtaining a joint input satisfying upper and lower limits of joint displacement, speed, and acceleration, and a joint cooperative control apparatus and method thereof Can be provided.

6 is a flowchart showing upper and lower limit constraint calculation processing for jerk input according to the first embodiment. 3 is a graph showing a trajectory A according to the first embodiment. 3 is a graph showing a trajectory B according to the first embodiment. 3 is a graph showing a trajectory C according to the first embodiment. 4 is a graph showing a trajectory D according to the first embodiment. 5 is a graph showing an example of a trajectory with one degree of freedom according to Embodiment 1 (response of displacement). 5 is a graph showing an example of a trajectory (speed response) with one degree of freedom according to the first embodiment. 6 is a graph showing an example of a trajectory (acceleration response) with one degree of freedom according to the first embodiment. 6 is a graph showing an example of a trajectory with one degree of freedom according to Embodiment 1 (input of jerk). 1 is a schematic diagram of an arm robot according to a first embodiment. 6 is a graph showing an example of a trajectory with three degrees of freedom according to the first embodiment (hand position response (y direction)). 6 is a graph showing an example of a trajectory with three degrees of freedom according to the first embodiment (hand position response (z direction)). 7 is a graph showing an example of a trajectory with three degrees of freedom according to Embodiment 1 (response of joint displacement). 6 is a graph showing an example of a trajectory with three degrees of freedom according to the first embodiment (response of joint speed). 6 is a graph showing an example of a trajectory with three degrees of freedom according to Embodiment 1 (response of joint acceleration). 5 is a graph showing an example of a trajectory with three degrees of freedom according to Embodiment 1 (input of joint jerk). (A) Robot motion generation system according to the present invention (b) It is a schematic configuration diagram of a robot motion generation system according to the prior art.

  Hereinafter, specific embodiments to which the present invention is applied will be described in detail with reference to the drawings. In each drawing, the same or corresponding elements are denoted by the same reference numerals, and redundant description is omitted as necessary for clarification of the description.

  Prior to the description of the embodiment of the present invention, a schematic configuration of the present invention will be described with reference to FIG. FIG. 17A is a diagram showing a schematic configuration of a robot motion generation system according to the present invention. As shown in the figure, the robot motion generation system includes a motion input device 100, a whole body cooperative calculation unit 200, and a robot 300 as an articulated robot. The whole-body cooperation calculation unit 200 includes an upper / lower limit constraint calculation unit 210 for the jerk input and an optimization problem solving unit 220.

  The motion input device 100 acquires motion data including a position / posture to be realized by the robot 300. The whole body cooperation calculation unit 200 calculates upper and lower limits to the jerk input from the obtained motion data, and solves an optimization problem using the calculated jerk upper and lower limits. The robot 300 operates the target of the robot 300 by cooperatively controlling the multi-joint according to the solution to the optimization problem (the jerk input to the joint).

  The whole body cooperative calculation unit 200 controls the joints of the robot 300 to operate the target part. The upper / lower limit constraint calculation unit 210 for the jerk input starts from the state of the joint at the current time and inputs the jerk to the joint while satisfying all the upper / lower constraints set for the joint speed, acceleration, and jerk. Determine the trajectory pattern. The determined trajectory pattern is a trajectory pattern in which the joint speed and acceleration can reach zero at the upper limit or lower limit of the set joint displacement after a predetermined time has elapsed. Furthermore, the upper / lower limit constraint calculation unit 210 for jerk input calculates the upper / lower limit values of jerk input to the joint from the determined trajectory pattern, and the joint state at the current time and the set joint speed The upper and lower limit values of the jerk input to the joint are calculated using the upper and lower limit constraints of acceleration and jerk, and the jerk to the joint is calculated using the upper and lower limit values of the jerk input to the calculated joint. Calculate the upper and lower limits of the input.

  The optimization problem finding unit 220 solves the optimization problem so as to minimize the value of the evaluation function under the constraint conditions, and calculates the jerk input to the joint. Here, the task refers to an equality constraint including an equality constraint on the jerk of the target part and the jerk input to the joint, and an inequality constraint on the upper and lower limits of the jerk input to the joint. Here, the constraint conditions are a task in which a solution exists among a plurality of setting tasks related to equality constraints, and upper and lower limits of the jerk input to the joint calculated by the upper and lower limit constraint calculation unit 210 for the jerk input. Range. In addition, as an evaluation function, an error in jerk input to a joint in a task for which no solution exists among a plurality of setting tasks related to equality constraints is used.

  In the present invention, a joint jerk is set as an input, and upper and lower limit constraints are set to obtain a solution that minimizes the sum of squares of the jerk error (joint jerk input). This corresponds to realizing a smooth trajectory by minimizing the acceleration differential (jump rate) as much as possible. As a result, the acceleration does not become discontinuous, and a smooth trajectory can be obtained even if the task is switched, and joint cooperative control that solves the conventional problem can be realized. The trajectory refers to time-series data such as displacement, speed, acceleration, jerk, etc. that the object or joint follows.

  In the present invention, the whole body cooperative solution is finally derived as an optimization problem. In the process, the determination method of the upper and lower limit constraints of the jerk input to the joint which satisfies the constraints of the joint displacement (movable range), speed, and acceleration as the optimization constraint conditions is a point.

  All joint states (joint displacement, speed, and acceleration at a certain time) need to be set so that the target displacement and the target acceleration can be reached with the target displacement in the shortest time. Then, a predetermined trajectory pattern illustrated as an example is considered. Note that the lower limit of the jerk input to the joint can be obtained by inverting the upper limit, and is handled in the same manner as the upper limit.

  In the present invention, considering these trajectory patterns, a method for obtaining an appropriate upper and lower limit of jerk according to each condition is provided, and a smooth joint trajectory is generated by an optimization calculation considering the upper and lower limits of the obtained jerk. . Further, by using the generated joint trajectory, it is possible to prevent the acceleration / speed from being vibrated in the robot 300 operating in the human environment, and to provide a safe operation even in the external environment.

<Embodiment 1 of the Invention>
Hereinafter, with reference to FIG. 1 to FIG. 16, a joint cooperative control device and method for an articulated robot according to the present embodiment will be described in detail. In the joint cooperative control method according to the present embodiment, instead of the method using the input to the joint as a speed, a method is used in which the degree of jerk is obtained by advancing differentiation from the speed, and this control method is called jerk decomposition control. . The reason why the differentiation is advanced to the jerk is that the upper and lower limits are set for the amount of change in acceleration and the acceleration is to be changed smoothly.

(Decision control)
The jerk decomposition control will be described.
The subject of the directive orbit y _r, one dot of y _r (is in the formula is a variable having a "-" one of the top, the first floor is a differential variables.), Tsudotto of y _r (2 to the top in the formula Is a variable having two “·” s and is a second-order differentiated variable), the state feedback law jerk (y three dots (upper part in the equation) shown in the following equation (1)) And the third-order differentiated variable))), the target can be made to follow the target trajectory.

In the following equation, y, y one dot, and y two dot are vectors indicating the displacement, velocity, and acceleration of the object at the current time, respectively, and y's three-dot is the object to be realized at the current time. It is a vector indicating the jerk, and K _d , K _v , and K _a are feedback gains for displacement, velocity, and acceleration deviations, respectively. Since this feedback law becomes a third-order response, stability is guaranteed by calculating and setting an appropriate gain in advance.

  The relationship between the target jerk (y three dots) and the joint jerk (x three dots) is shown in the following equation (2) using the Jacobian matrix A regarding the target velocity and the joint velocity. In the following equation, x, x one dot, and x two dots are vectors indicating the displacement, velocity, and acceleration of the joint at the current time, respectively. It is a vector indicating the jerk.

  Since the state quantity x, one dot of x, and two dots of x at the current time are all known, the jerk task is an equality constraint condition shown in the following equation (3). Although details will be described later, when setting a plurality of tasks, a plurality of equality constraints can be considered. In the actual robot 300, there are restrictions on joint displacement and speed. For this reason, it is necessary to consider the displacement and speed constraints of the joint. Therefore, in addition to tasks as equations, constraints as inequalities are also added as tasks, and a method for solving as an optimization problem under this task is known (Non-Patent Document: Bernard Faverjon and Pierre Tournassoud. "A Local Based Approach for Path Planning of Manipulators", With a High Number of Degrees of Freedom. In In Procs. Of IEEE International Conference on Robotics and Automation, pp. 1152-1159, 1987.).

However, the three dots of b in Expression (3) are expressed by the following Expression (4).

If there are multiple tasks, multiple equality constraints can be considered. For example, when there are three tasks, the equality constraint condition shown in the following formula (5) is satisfied.

Further, regarding the joint jerk input, upper and lower limit constraints are set as shown in the following equation (6). This method of setting the upper and lower limits of jerk input to the joint realizes the upper and lower limits (movable range) of the joint displacement, the upper and lower limits of the speed, and the upper and lower limits of the acceleration. It will be described later.

  In addition, priorities are set for tasks as in the conventional method, and tasks up to a task with a solution are included in the constraint condition. For example, among the three tasks shown in Equation (5), there is a solution that implements the tasks up to the first equality constraint and the second equality constraint with high priority, but the third equality There are no tasks that satisfy the constraints. In this case, the error of the task jerk of the third equality constraint for which no solution exists is used as an evaluation function for optimization. Then, the objective function of the optimization problem has a quadratic form consisting of the sum of squares of jerk error shown in the following equation (7).

  In other words, the first equality constraint task and the second equality constraint task shown in Equation (5) are satisfied, and the joint jerk input upper and lower limit constraints shown in Equation (6) are satisfied. This is an equation represented by the sum of squared errors of the jerk related to the task of the third equality constraint, and is a problem of minimizing the objective function shown in Equation (7). By finding such a quadratic programming problem, it is possible to obtain joint jerk input.

  Therefore, by using the jerk to the joint as an input, a solution that minimizes the sum of squares of the jerk error can be obtained. This corresponds to realizing a smooth trajectory by minimizing the acceleration differential. For example, when the priority of a task is changed for each step, or when a task is added or deleted, it is the joint jerk that changes discontinuously. By setting joint upper and lower limit constraints, joint acceleration will not be discontinuous (beyond the upper and lower limit constraints), resulting in a smooth trajectory even when switching tasks be able to.

  Here, the problem is how to set the upper and lower limit constraints for the jerk input to the joint. First, although arbitrary upper / lower limits can be given for each step as the jerk input to the joint, what is actually desired to be limited is not only the joint jerk of the robot 300 but also torque, speed, displacement (movable) Range). Since the torque depends on the inertia matrix of the robot 300, the present embodiment will be described by taking as an example a case where a constraint on joint acceleration is added.

  Further, the jerk input to the joint is obtained as a result of the optimization calculation and is not understood in advance. Only the upper and lower limit constraints of the jerk input to the joint at the current time are guaranteed. That is, there is a problem of how to provide upper and lower limit constraints on the degree of jerk input to the joint to satisfy the upper and lower limit constraints of the joint displacement, velocity, and acceleration. This will be specifically described below.

(How to determine upper and lower limit constraints for joint jerk input: joint upper and lower limit constraints)
Hereinafter, how to calculate the upper and lower limit constraints of the jerk input to the joint at the current time to satisfy the upper and lower limits of the displacement, velocity, and acceleration for one joint will be described. The case of multi-joint can be handled in the same way by thinking independently.

  The displacement, velocity, and acceleration of a joint at the current time are x, x one dot and x two dot, respectively, and the input to the joint is the jerk (x three dots). In the whole body cooperative control, the jerk input (x three dots) is one element of the variable of the optimization problem, and the upper and lower limit constraints of the jerk (x three dots) set for each step are always satisfied. However, it should be noted that it is not known in advance what value is input as a result of optimization.

As state variables at time k discretized by the discrete time Δt, displacement, velocity, and acceleration are x _k , x _k one dot, x _k two dots, respectively, and joint jerk (three _k dots of x _k ) ) As input. However, k = 0 is the current time. The following formula (8) shows an update formula for the state at time k + 1.

Here, at all times k, it is desired that all of displacement, velocity, acceleration, and jerk meet the upper and lower limit constraints shown in the following equation (9).

However, in the formula (9), constraint value _x l of the upper and lower _limit, x u, one dot _{x l,} one dot of _{x u,} the _{x l} Tsudotto, Tsudotto of _{x u,} three dots _{x l,} of _{x u} Three dots are all constants, and appropriate values are set in advance. In addition, the one-dot of one dot <0 _{<x u} of _{x l,} Tsudotto of _{x l} <0 _{<x u} of Tsudotto, to be a three-dot, the three-dot <0 _{<x u} of _{x l.}

As is guaranteed to meet sure all the inequality (9), the following expression represented by (10), jerk upper and lower limit constraint at the current time (x _0l three dots, the x _0u Three Finding dots) becomes a problem.

In the following, the calculation method of the upper limit (x _0u three dots) of the jerk input to the joint will be described as an example. However, the upper limit and the lower limit are interchanged to change the lower limit of the jerk input to the joint (three dots of x _0l ). ) Can be considered in exactly the same way.

(Determining upper and lower limit constraints for joint jerk input: Basic concept)
As described above, the joints have upper and lower limits on displacement, speed, and acceleration. However, it is not sufficient to satisfy these momentarily near the current time. Even if these upper and lower limit constraints are satisfied in the vicinity of the current time, there is no guarantee that the upper and lower limit constraints can be similarly satisfied at future times. Also, a trajectory that vibrates in the vicinity of the upper limit of displacement is not a desirable trajectory even if the upper limit constraint is satisfied, and is preferably avoided.

Now, it is assumed that you assign a value in the jerk input of at the current time to the joint (x ₀ of the three-dot). In addition, when the predetermined time has elapsed from the current time under the entered jerk input to this joint, this joint satisfies all the upper / lower limits of the set speed, acceleration, and jerk, while the upper limit of displacement is exceeded. It is assumed that there is a trajectory that stops without vibration or overshoot (stop trajectory). Here, “stop at the upper limit of displacement” means that at the upper limit of displacement (x = x _u ) as the target position, the target speed is 0 (one dot of x = 0) and the target acceleration is 0 (two dots of x = 0) is reached.

At this time, largest of jerk input to the joint that would be realized a stop track (x ₀ Three dots), which is the upper limit of the jerk input at the current time (x _0u Three dots). Various trajectories that can be stopped without vibration at the upper limit of displacement of the joint are conceivable. For example, when the purpose is to stop the joint in the shortest time, the following four trajectory patterns (trajectories A, B, C, D).

And orbital A: Stop-orbit by the jerk input of the "three dots of the three-dot → x _u of x _l" B: Stop-orbit by the jerk input of the "three dots of the three-dot → 0 → x _u of x _l" C: stop-orbit by the jerk input of "x Three dot → 0 → x _l Three dot → x _u Three dots of _l" D: Three dots "x _l → 0 → x _l of the three-dot → 0 → stop by the jerk input of the three-dot "of x _u

  These four trajectory patterns use a combination of a section in which the maximum or always minimum jerk is input and a section in which the state is at the upper speed limit or upper acceleration limit and the jerk input is zero. It realizes a trajectory that stops in the shortest time.

FIG. 1 is a flowchart showing a process of calculating upper and lower limit constraints on the jerk input at the current time. In the upper / lower limit constraint calculation for the jerk input, it is determined in which order of the four trajectory patterns A, B, C, and D whether the stop can be realized by any stop trajectory (S110, S130, S150). When feasible stop track is found, it sets the jerk input for realizing the stop start (x ₀ Three dots), as the upper limit of the jerk input to the joint at the current time (x _0u Three dots) (S120, S140, S160). Details of the stop trajectory determination process will be described later.

  By realizing the range of jerk input to the joint according to this process from the initial state where it can be stopped (usually the stopped state), it always operates within the range where the stop trajectory can be realized. Operation within constraints is always possible. Prior to the description of each of the track patterns A, B, C, and D, processing common to the track patterns A, B, C, and D will be described.

(Method of determining upper and lower limit constraints for joint jerk input: Necessary condition for jerk restriction common to all trajectory patterns)
Necessary conditions for jerk input that are common to the four trajectory patterns A, B, C, and D will be described. First, upper and lower limit constraints ( _xl three dots, x _u three dots) are set in advance in the jerk input itself. Acceleration, velocity, Tsudotto upper and lower limit value of the displacement is _{x l,} Tsudotto of _{x u,} one dot _{x l,} one dot of _{x u,} x l, because it is _{x u,,} for not exceeding each jerk The upper and lower limits are expressed by the following equation (11).

And the condition shown by the following formula | equation (12) becomes a necessary condition of the upper and lower limits of jerk.

Further, at time 1 after Δt from the current time, each of acceleration, speed, and displacement needs to be within upper limit constraints as shown by the following equation (13).

From these, the upper limit value of the jerk input to the joint at time 0 can be calculated, and can be obtained as shown by the following equation (14).

(Determining upper and lower limit constraints for joint jerk input: orbital pattern A)
The orbit A (stopped by the jerk input of "x _l Three dot → x _u Three dots of") will be described.

First, how to determine the trajectory A will be described. FIG. 2 illustrates the trajectory A. The trajectory A is first stopped by inputting the minimum jerk (x ₁ three dots) for a predetermined time t ₁ and then giving the maximum jerk (x _u three dots) for a predetermined time t _2. It is a pattern. The current time is time 0, time 1 after step time Δt, time 1 after t ₁ second, and time 3 after t ₂ seconds. The state variables at the current time are x ₀ , x ₀ one dot, x ₀ two dots, and the current time jerk input is x ₀ three dots.

At time 3, the stop condition shown in the following formula (15) is necessary. If a variable is set as a three dot x ₀ , t ₁ , t ₂ , the solution of the variable can be obtained from equation (15).

First, the variable t ₁ is a solution of a sixth-order polynomial shown in the following equation (16).

However, _a 0 ~a ₆ in the formula (16) are shown in the following equation (17).

The solution of the polynomial shown in the above equation (16) can be obtained by a numerical solution method using the Newton method or the like. If there is no solution, t ₁ = 0. Using this _{t 1,} as a solution of equation (18) below, it is possible to obtain the _{t 2.}

However, _b 0 ~b ₂ in the formula (18) are shown in the following equation (19).

From _t 1, _{t 2} or more, the three dots _{x 0,} as shown in the following equation (20) can be obtained.

Next, it is determined whether or not the stop trajectory A thus determined satisfies the speed and acceleration constraints. That is, it is determined whether or not the vehicle can be stopped according to the trajectory A.
First, when the time la is the time when the acceleration is 0 (two dots of x _la = 0), the time tla from the time 1 to the time la is expressed by the following equation (21).

  Here, when tla <0, since the maximum value of the speed is not taken in this trajectory, the process proceeds to the next confirmation of acceleration.

On the other hand, if it is tla> 0 is represented by the following equation (22), to calculate the speed _{(one-dot x la)} at that time. Result of the calculation, when a one-dot one-dot> x _u of x _la is track A obtained is determined not to satisfy the speed constraints.

Next, the minimum value of acceleration ( _two dots of x2) is obtained. If it is Tsudotto of x ₂ in Tsudotto <x _l is the trajectory A obtained is determined not to satisfy the acceleration constraint.

As a result of the above determination, when it is determined that all the constraints are satisfied, the trajectory A becomes the shortest stop trajectory. For this reason, the maximum value (three dots of _x0u ) of jerk input can be obtained as shown in the following equation (23).

  If the state is changed so that the trajectory A can be realized in the initial state and can be stopped on any of the trajectories A, B, C, and D, the trajectory A itself cannot be realized.

(Determining upper and lower limit constraints for joint jerk input: orbital pattern B)
Next, a description will be given of the track B (stopped by the jerk input of "x _l Three dot → 0 → x _u Three dots of"). First, how the trajectory B is determined will be described. FIG. 3 illustrates the trajectory B.

In the trajectory B, the stop condition shown in the following formula (24) is required at time 4.

Further, as shown in the following equation (25), the acceleration takes a minimum value between time 2 and time 3.

The three dots of variable _{x 0,} putting a t _1, t 2, _{t 3,} and stop condition shown in the above formula (25), a first condition of the formula (25) from, variable You can find a solution.

The solution of the variable t ₃ can be obtained by the following equation (26).

The solution of the variable t ₁ can be obtained as a solution of the following equation (27).

However, _a 0 ~a ₄ in the formula (27) are shown in the following equation (28).

The solution of the variable t ₂ can be obtained from t ₁ and t ₃ by the following equation (29).

Solution of the variable (x ₀ Three dots), the other variables, obtained as described above, can be obtained by equation (30) below.

Next, it is determined whether or not the stop trajectory B determined in this way satisfies the speed constraint. That is, it is determined whether or not the vehicle can be stopped according to the trajectory B.
First, when the time t1b is a time when the trajectory B takes the maximum value of the speed, the time tlb is expressed by the following equation (31).

  Here, when t1b <0, the trajectory B can be stopped because the trajectory does not take the maximum value of the speed.

On the other hand, if it is t1b> 0 is represented by the following equation (32), calculates the _{(one-dot x 1b)} that time speed. Result of the calculation, when a one-dot one-dot> x _u of x _1b is obtained trajectory B is determined not to satisfy the speed constraints.

As a result of the above determination, when it is determined that the constraint is satisfied, the trajectory B becomes the shortest stop trajectory. For this reason, the maximum value (three dots of _x0u ) of jerk input can be obtained as shown in the following equation (33).

(Determining upper and lower limit constraints for joint jerk input: trajectory pattern C)
Next, a description will be given orbit C (stopped by the jerk input of the "three dots of the three-dot → x _u of the three-dot → 0 → x _l of x _l"). First, how to determine the trajectory C will be described. FIG. 4 illustrates the trajectory C.

In the trajectory C, the stop condition shown in the following formula (34) is required at time 5.

Further, as shown in the following equation (35), the speed takes the maximum value between time 2 and time 3.

When placing the variable three dots _{x 0,} and _{t 1, t 2, t 3} , t 4, and stop condition shown in the above formula (34), first and second condition of the formula (35) From this, the solution of the variable can be obtained.

Solutions of variable t ₄ can be obtained by equation (36) below.

The solution of the variable t ₃ can be obtained by the following equation (37).

The solution of the variable t ₁ can be obtained as a solution of a quadratic equation shown in the following equation (38).

However, _a 0 ~a ₂ in the formula (38) are shown in the following equation (39).

The solution of the variable t ₂ can be obtained from t ₁ , t ₃ , t ₄ by the following equation (40).

Solution of the variable (x ₀ Three dots) can be obtained by equation (41) below.

Next, it is determined whether or not the stop trajectory C thus determined satisfies the acceleration constraint. That is, it is determined whether or not the vehicle can be stopped according to the trajectory C.
Limitations in orbit C is the acceleration at time 4 (Tsudotto of x ₄₎ is that it does not exceed the acceleration limit (Tsudotto of x _l). If it is Tsudotto of x ₄ of Tsudotto <x _l, it is determined that the track C is not satisfied constraints. On the other hand, when it is determined that this restriction is satisfied, the trajectory C becomes the shortest stop trajectory. Therefore, the maximum value of jerk input (three dots of _x0u ) can be obtained as shown in the following formula (42).

(Method for determining upper and lower limit constraints for joint jerk input: orbital pattern D)
Next, a description will be given orbit D (stopped by the jerk input of "x _l Three dot → 0 → x _l Three dot → 0 → x _u Three dots of"). First, how to determine the trajectory D will be described. FIG. 5 illustrates the trajectory D.

In the trajectory D, the stop condition shown in the following formula (43) is required at time 6.

Further, as shown in the following formula (44), the speed takes the maximum value between time 2 and time 3.

Further, as shown in the following formula (45), the acceleration takes the minimum value between time 4 and time 5.

When placing the variable three dots _{x 0,} and _{t 1, t 2, t 3} , t 4, t 5, and stop condition shown in the above formula (43), the first and 2 in the formula (45) The solution of the variable can be obtained from the first condition and the first condition of the above equation (45).

Solutions of the variables t ₃ , t ₅ , t ₄ can be obtained by the following equation (46).

The solution of the variable t ₁ can be obtained as a solution of a quadratic equation shown in the following equation (47).

However, _a 0 ~a ₂ in the formula (47) are shown in the following equation (48).

The solution of the variable t ₂ can be obtained from t ₁ , t ₃ , t ₄ , t ₅ by the following equation (49).

Solution of the variable (x ₀ Three dots) can be obtained by equation (50) below.

The stop trajectory D thus determined is not limited. For this reason, the maximum value of the jerk input (three dots of _x0u ) can be obtained as shown in the following equation (51).

(Operation check: Operation check with 1 degree of freedom)
Next, the above-described jerk limitation was implemented for the one-degree-of-freedom variable x, and its operation was confirmed. In variable x, upper and lower limits of displacement, speed, and acceleration are set. The target displacement x _r of the joint is calculated jerk enter (x Three dots) as shown in equation (52) below.

  The process shown in FIG. 1 is performed for each step to calculate the upper and lower limits of the jerk, and the jerk jerk input (three dots of x) is limited by the upper and lower limits. However, the gains Kd, Kv, and Ka of the above equation (52) are designed to be appropriate values in advance based on the response of the tertiary system.

The obtained trajectories are illustrated in FIGS. FIG. 6 shows the displacement response. FIG. 7 shows the speed response. FIG. 8 shows the acceleration response. FIG. 9 shows jerk input. The upper and lower limits of displacement are set to ± 1, the upper and lower limits of speed are set to ± 2, the upper and lower limits of acceleration are set to ± 10, and the upper and lower limits of jerk are set to ± 1000. In addition, the target displacement _{x r,} the time 3 to 1.1, until the time 8 -1.1, is set to 0 to time 10. The target speed and target acceleration are both set to zero. As can be seen from these figures, the operation is performed while satisfying all the upper and lower limits of the displacement, velocity, and acceleration (shown as Upper limit and Lower limit in the figure).

(Operation check: Operation check with 3 degrees of freedom)
Next, an operation check is performed on a robot having a three-degree-of-freedom arm using jerk decomposition control, and the result will be described. FIG. 10 shows a schematic configuration example of an articulated robot 300 (hereinafter simply referred to as robot 300) according to the present embodiment. Here, a case where the robot 300 is an arm robot having a three-degree-of-freedom arm will be described as an example. FIG. 2 shows the arrangement of the axes of the three-degree-of-freedom arm of the robot 300.

  The robot 300 includes a controller 310. Further, the robot 300 includes an arm 320, and this target is manipulated with respect to a hand 330 provided at the tip of the arm 320. The arm 320 includes a plurality of joints (joints 321, 322, and 323) and links that connect the joints. Each joint includes an actuator for controlling the joint angle and an encoder for detecting the joint angle. The controller 310 may be configured to be included in the robot 300 or may be configured to be connected to the outside and transmit a command to the robot 300.

  The controller 310 has the function of the whole body cooperative calculation unit 200 described above. That is, the controller 310 includes programs that realize the functions of the upper and lower limit constraint calculation unit 210 and the optimization problem solving unit 220 for the jerk input, execute each process in each unit, and input the jerk input to the joint. calculate. The controller 310 controls each joint (joints 321, 322, and 323) based on the calculated jerk input to the joint to coordinately control the multi-joint, thereby moving the hand 330 of the arm 320. Let

  That is, the controller 310 controls the actuator so as to follow the calculated jerk target of each joint. More specifically, the controller 310 calculates the time series data of the target jerk of each joint, and controls each joint so as to follow the calculated target jerk, thereby operating the target of the robot 300.

  The controller 310 includes a CPU (Central Processing Unit) that performs control processing, arithmetic processing, and the like as a main hardware configuration, a ROM (Read Only Memory) that stores a control program executed by the CPU, an arithmetic program, and the like. ) And a RAM (Random Access Memory) that temporarily stores processing data and the like. The CPU, ROM, and RAM are connected to each other by a data bus.

  Further, the joints (joints 321, 322, and 323) of the robot 300 illustrated in FIG. 10 are rotary joints in the y-axis direction. In the example shown in the figure, since the position of the hand 330 on the xz plane is designated and this position is realized by using three joints, there is a redundant problem.

  First, it is assumed that the robot 300 is in the initial posture (all joints (joints 321, 322, 323) angle = 0) shown in FIG. As for the task setting, as a task with the highest priority, as shown in FIG. 10B, the target displacement (target position) of the hand 330 is stepped to the coordinates (2.0, 0.5). Gave. Further, 0 was given to the command speed and command acceleration of the hand 330. Next, as a task with the highest priority, a command for setting the speed of each joint (joints 321, 322, 323) to 0 is given. Further, the lower limit of the displacement of the joint 321 is limited to −0.65 [rad], and in order to reach the target position of the hand 330, the joint 321 always takes the lower limit displacement.

  FIGS. 11 and 12 show the displacement response of the target (hand 330) when the robot 300 is operated in accordance with the calculated joint jerk input. As shown in the figure, it can be confirmed that it has converged to the commanded displacement.

  13 to 15 show the displacement, velocity, and acceleration responses of each joint (joints 321, 322, and 323). In each figure, the joint 321 is shown as “Joint 0”, the joint 322 as “Joint 1”, and the joint 323 as “Joint 2”. FIG. 16 shows the jerk input of the joints (joints 321, 322, and 323). As shown in FIGS. 13-16, it can confirm that it is operating, satisfy | filling the upper and lower limit restrictions set to each joint (joint 321,322,323), respectively.

  In addition, since the jerk input of the joints (joints 321, 322, and 323) always satisfies the upper and lower limits, a smooth trajectory can be obtained. In particular, as shown in FIG. 13, the joint 321 hits the lower limit of displacement in the vicinity of 0.9 seconds. However, since the robot 300 has redundant degrees of freedom, the joint 322 and the joint 323 are displaced. Thus, the target position of the hand 330 can be realized.

  As described above, in a robot that operates a target by coordinating a plurality of joints, it is possible to minimize joint jerk input and to satisfy joint displacement, speed, and acceleration upper and lower limits. Realized the method.

The effects of the present invention will be further described with reference to FIG.
In recent years, sensors that can easily acquire the position and posture of a person have been put into practical use. Many attempts have been made to drive the robot 300 from motion data obtained using the motion input device 100 including such a sensor. FIG. 17 shows a schematic configuration of a motion generation system of the robot 300 using such a motion input device 100. FIG. 17A is a schematic configuration diagram of a motion generation system including the whole body cooperative calculation unit 200 according to the present invention. FIG. 17B is a schematic configuration diagram of a motion generation system including the whole body cooperative calculation unit 500 according to the related art.

  As shown in FIG. 17 (b), in the motion generation system using the conventional joint cooperative control technology, a configuration in which a discontinuous input value due to a sensor error or the like is corrected (the input value Discontinuity correction processing 400) is required, and a configuration for smoothing excessive acceleration discontinuity points and excessive speed (command value smoothing processing 600) is also required in the subsequent stage.

  On the other hand, as shown in FIG. 17A, in the motion generation system using the whole body cooperative calculation unit 200 according to the present invention, the state feedback law is included in the whole body cooperative control in the whole body cooperative calculation unit 200. Therefore, the previous processing is not necessary. Furthermore, since the continuity at the acceleration level is guaranteed in consideration of the joint jerk, it is not necessary to provide a subsequent filtering process and can be used directly as a joint drive command value. Accordingly, it is possible to expect a significant simplification of the system, and it is possible to configure a direct system in which some measured motion target value is applied to the robot 300 as it is.

  That is, in the motion generation system using the whole-body cooperative calculation unit 200 according to the present invention, the system alone is corrected to a trajectory that smoothly drives the joints of the robot 300 even from discontinuous and poor data input and output. be able to. As a result, it is possible to provide safe and safe operation command values for the robot 300 itself and the external environment (human beings and things), which is very useful.

  Note that the present invention is not limited to the above-described embodiment, and can be changed as appropriate without departing from the spirit of the present invention. For example, in the above-described embodiment, the case where the robot 300 is an arm robot including a three-degree-of-freedom arm has been described as an example, but the present invention is not limited to this. That is, as the robot 300, the present invention may be applied to a humanoid robot (20 or more joints) or an arm robot with wheels (8 or more joints).

  In addition, the calculation method of the jerk upper and lower limits for realizing the upper and lower limit constraints of displacement, speed, and acceleration is a general one, and may be applied to, for example, industrial robot arm and automobile motion control. Good.

  In the above-described embodiment, the controller 310 included in the robot 300 has been described as a configuration having the function of the whole body cooperation calculation unit 200. However, the present invention is not limited to this, and the function of the whole body cooperation calculation unit 200 is described. It is good also as a structure provided independently from the robot 300 as a joint coordination control apparatus.

100 motion input device,
200 Whole body cooperation calculation part,
210 Upper / lower limit constraint calculation part for jerk input,
220 Optimization Problem Solver,
300 robots,
310 controller,
320 arms,
330 minions,
321, 322, 323 joints,
400 Discontinuity correction processing of input values,
500 whole body cooperation calculation part,
600 Smoothing of command value,

Claims (6)

An articulated joint control device for an articulated robot that controls the joints of the articulated robot in a coordinated manner.
Starting from the state of the joint at the current time, while satisfying all the upper and lower limit settings of the joint speed, acceleration, and jerk, the upper limit or the lower limit of the joint displacement set after a predetermined time has passed. Determine the trajectory pattern of the jerk input to the joint at which the joint velocity and acceleration can reach zero, upper and lower limit values of jerk input to the joint calculated from the determined trajectory pattern, and the current time And the upper and lower limit values of the jerk input to the joint calculated from the upper and lower limits of the joint speed, acceleration, and jerk, and the jerk input to the joint. Upper and lower limit constraint calculation part to jerk input to calculate the lower limit range,
The task includes a constraint including an equality constraint of the jerk of the target part and the jerk input to the joint, and a constraint as an inequality of upper and lower limit ranges of the jerk input to the joint, and the equation constraint The constraint condition includes a task in which a solution exists among a plurality of setting tasks, and an upper and lower limit range of the jerk input to the joint calculated by the upper and lower limit constraint calculation unit to the jerk input, and the like An error in the jerk input to the joint in a task in which a solution does not exist among a plurality of setting tasks related to expression constraints is used as an optimization evaluation function, and the value of the evaluation function is optimized under the constraint conditions An optimization problem solving unit that solves an optimization problem and calculates a jerk input to the joint; and
Joint joint control device for articulated robots. The upper and lower limit constraint calculation unit for the jerk input is:
As the trajectory pattern, the first trajectory pattern in which the jerk input to the joint changes from the set minimum value to the set maximum value, and the jerk input to the joint changes from the set minimum value to 0 through the set maximum value. A second trajectory pattern that changes to a predetermined maximum value after the jerk input to the joint returns from the set minimum value to 0 through the set minimum value, and the joint The fourth trajectory pattern that changes from 0 to 0 from the set minimum value to the set minimum value and then changes to 0 and again to the set maximum value is determined. Which of the fourth trajectory patterns is used, and whether the joint velocity and acceleration can reach zero at the upper or lower limit of the set joint displacement after the predetermined time has elapsed. Can be reached Calculated from the upper and lower limit values of the jerk input to the joint calculated from the determined trajectory pattern, the state of the joint at the current time, and the set speed, acceleration, and jerk limits of the joint at the current time Calculating upper and lower limit ranges of jerk input to the joint using upper and lower limits of jerk input to the joint;
The joint cooperative control apparatus of the articulated robot according to claim 1. The articulated robot is a robot having a plurality of degrees of freedom arms.
The joint cooperative control apparatus of the articulated robot according to claim 1 or 2. A joint joint control method for an articulated robot in which joints of the articulated robot are cooperatively controlled to operate a target part,
Starting from the state of the joint at the current time, while satisfying all the upper and lower limit settings of the joint speed, acceleration, and jerk, the upper limit or the lower limit of the joint displacement set after a predetermined time has passed. Determine the trajectory pattern of the jerk input to the joint at which the joint velocity and acceleration can reach zero, upper and lower limit values of jerk input to the joint calculated from the determined trajectory pattern, and the current time And the upper and lower limit values of the jerk input to the joint calculated from the upper and lower limits of the joint speed, acceleration, and jerk, and the jerk input to the joint. Upper and lower limit constraint calculation process to jerk input to calculate the lower limit range,
The task includes a constraint including an equality constraint of the jerk of the target part and the jerk input to the joint, and a constraint as an inequality of upper and lower limit ranges of the jerk input to the joint, and the equation constraint The constraint condition includes a task in which a solution exists among a plurality of setting tasks, and upper and lower limit ranges of the jerk input to the joint calculated in the upper and lower limit constraint calculation step to the jerk input, and the like An error in the jerk input to the joint in a task in which a solution does not exist among a plurality of setting tasks related to expression constraints is used as an optimization evaluation function, and the value of the evaluation function is optimized under the constraint conditions An optimization problem solving step of solving an optimization problem and calculating a jerk input to the joint,
A joint control method for an articulated robot. The upper and lower limit constraint calculation process for the jerk input is as follows:
As the trajectory pattern, the first trajectory pattern in which the jerk input to the joint changes from the set minimum value to the set maximum value, and the jerk input to the joint changes from the set minimum value to 0 through the set maximum value. A second trajectory pattern that changes to a predetermined maximum value after the jerk input to the joint returns from the set minimum value to 0 through the set minimum value, and the joint The fourth trajectory pattern that changes from 0 to 0 from the set minimum value to the set minimum value and then changes to 0 and again to the set maximum value is determined. Which of the fourth trajectory patterns is used, and whether the joint velocity and acceleration can reach zero at the upper or lower limit of the set joint displacement after the predetermined time has elapsed. Can be reached Calculated from the upper and lower limit values of the jerk input to the joint calculated from the determined trajectory pattern, the state of the joint at the current time, and the set speed, acceleration, and jerk limits of the joint at the current time Calculating upper and lower limit ranges of jerk input to the joint using upper and lower limits of jerk input to the joint;
The joint cooperative control method of the articulated robot according to claim 4. An articulated robot that operates a target part by cooperatively controlling joints,
Starting from the state of the joint at the current time, while satisfying all the upper and lower limit settings of the joint speed, acceleration, and jerk, the upper limit or the lower limit of the joint displacement set after a predetermined time has passed. Determine the trajectory pattern of the jerk input to the joint at which the joint velocity and acceleration can reach zero, upper and lower limit values of jerk input to the joint calculated from the determined trajectory pattern, and the current time And the upper and lower limit values of the jerk input to the joint calculated from the upper and lower limits of the joint speed, acceleration, and jerk, and the jerk input to the joint. Upper and lower limit constraint calculation part to jerk input to calculate the lower limit range,
The task includes a constraint including an equality constraint of the jerk of the target part and the jerk input to the joint, and a constraint as an inequality of upper and lower limit ranges of the jerk input to the joint, and the equation constraint The constraint condition includes a task in which a solution exists among a plurality of setting tasks, and an upper and lower limit range of the jerk input to the joint calculated by the upper and lower limit constraint calculation unit to the jerk input, and the like An error in the jerk input to the joint in a task in which a solution does not exist among a plurality of setting tasks related to expression constraints is used as an optimization evaluation function, and the value of the evaluation function is optimized under the constraint conditions An optimization problem solving unit that solves an optimization problem and calculates a jerk input to the joint; and
Articulated robot.

Images