CN102528802A - Motion driving method for robot with nine degrees of freedom - Google Patents

Abstract

The invention provides a motion driving method for a robot with nine degrees of freedom. The method includes realizing motion path planning and gesture planning of a tail-end hand of the robot by the aid of information, which is provided by a user, of positions and gestures of the tail-end hand at multiple points in a cartesian space, and obtaining positions of the tail-end hand of the robot in an integral driving process and gestures of the tail-end hand of the robot in the integral driving process; solving a joint space from a first degree of freedom to a sixth degree of freedom of the robot at a certain point of time by the aid of the obtained information of the positions of the tail-end hand of the robot, and solving a joint space from a seventh degree of freedom to a ninth degree of freedom of the robot at the point of time by the aid of the obtained information of the gestures of the tail-end hand of the robot; and driving the robot to move by the aid of the obtained joint spaces of the ninth degrees of freedom. The motion driving method for the robot has the advantages that the user only needs to provide the positions and the gestures of the tail-end hand of the robot at a few of points in the space, accordingly, the robot can be driven, and the motion driving method is simple in implementation and high-efficiency in running.

Description

Motion driving method of nine-degree-of-freedom robot

Technical Field

The invention relates to robot motion control, in particular to a motion driving method of a nine-degree-of-freedom robot.

Background

With the continuous development of scientific technology and productivity, the application of the robot in various fields is wider, for example, the application of the robot is seen everywhere in a production workshop of a large automobile manufacturer. In most cases, one needs a robot to be able to do soA discipline for studying how a robot moves is called robot kinematics, where a given action, such as welding in the production of a car, is known as robot kinematics. Robot kinematics can be divided into two categories: forward and reverse. The problem to be solved by forward kinematics is that the joint space Q (θ) — [ θ ] of the known robot₁，θ₂，…，θ_n]And solving the position and the posture of the tail end of the robot. The problem to be solved by inverse kinematics is to know the position and the posture of the tail end of the robot and find the joint space of the robot. The solution of inverse kinematics is not unique and its solving process is more complex than that of forward kinematics.

In the prior art, various methods for solving inverse kinematics have been proposed, such as projection method and analytic method as mentioned in reference 1 "advanced robot control, Tantan Min, education Press, 5 months 2007". These methods have been used in existing 6-degree-of-freedom industrial robots (also called full-degree-of-freedom robots, whose joints are similar to the waist, shoulder, elbow and wrist joints of the human body), and which rely on them to perform mechanical movements quickly and accurately.

Nowadays, it has become possible to apply robots in the field of photography in addition to the traditional industrial production field. The robot with the camera has wide application prospect in some special scenes, such as scenes with high risk. And the camera shooting action finished by the robot has the advantages of stability, difficult shake and the like compared with manual work. However, considering that it is unlikely that a professional in the photography industry, such as a director, will have knowledge about robot control, a robot applied in the photography field needs to provide a simple and easy-to-operate interface to a user, and after the user inputs limited information through the interface, the robot should be able to complete a prescribed motion within a prescribed time according to the information to achieve a predetermined posture. For example, a director inputs several key points on a touch screen, which require the lens of a camera to reach, and the time of the camera reaching the key points, and the robot should be able to drive the camera to realize a corresponding track according to the information, and further generate a joint space for completing the track. Furthermore, a 6-degree-of-freedom robot with complete degrees of freedom cannot meet the space requirements of a camera robot due to the limitation of the motion space. Such as: the infinite rotation around the lens and other operations cannot be realized through 6 degrees of freedom, so in order to meet the rotation around the camera and special shooting requirements in the shooting process, 2 degrees of freedom need to be added behind the wrist joint of the camera robot, and one sliding degree of freedom is added in front of all joints to increase the motion space of the robot. Therefore, the imaging robot needs to adopt a 9-degree-of-freedom robot.

Due to the characteristics of the robot in the field of photography, the inverse kinematics solution method in the prior art cannot be directly applied to the photography robot with 9 degrees of freedom.

Disclosure of Invention

The invention aims to overcome the defect that the existing inverse kinematics solution method cannot be directly applied to a camera robot with 9 degrees of freedom, thereby providing a robot motion driving method which can be fast and efficient.

In order to achieve the above object, the present invention provides a motion driving method of a robot, the robot including 9 degrees of freedom, a moving joint of the robot having a first degree of freedom, a shoulder joint having a second degree of freedom, an elbow joint having a third degree of freedom, a wrist joint having a fourth degree of freedom, a hand joint having fifth, sixth and seventh degrees of freedom, and a rotation joint having eighth and ninth degrees of freedom; the method comprises the following steps:

step 1), realizing motion trajectory planning and posture planning of the robot tail end hand by using information of positions and postures of the robot tail end hand on a plurality of points in a Cartesian space, which is provided by a user, and obtaining the position of the robot tail end hand in the whole driving process and the posture of the robot tail end hand in the whole driving process;

step 2), solving joint space of the robot from a first degree of freedom to a sixth degree of freedom at a certain time point by using the position information of the robot end hand obtained in the step 1), and solving joint space of the robot from a seventh degree of freedom to a ninth degree of freedom at the time point by using the posture information of the robot end hand obtained in the step 1); the resulting joint space of nine degrees of freedom is used to drive the motion of the robot.

In the above technical solution, further comprising:

and 3) simulating by using the joint space information of the robot with nine degrees of freedom at a plurality of time points, which is obtained in the step 2), and selecting a better value from the joint space information of the nine degrees of freedom at the plurality of time points as a control point according to a simulation result.

In the above technical solution, in the step 1), the motion trajectory planning includes:

and performing curve fitting on the position information of the terminal hand of the robot provided by the user on a plurality of points in a Cartesian space to obtain at least one motion trail curve.

In the above technical scheme, the curve fitting adopts a B-spline curve method, or a Bezier curve method, or a method combining a B-spline curve and a Bezier curve.

In the above technical solution, in the step 1), the attitude planning includes:

step a), respectively taking two adjacent points of known attitude information in Cartesian space as an initial position and a target position, modifying the attitude information of the initial position and the target position from an Euler angle form to a quaternion form, and respectively using Q₁And Q₂Represents;

step b), solving the attitude between the initial position and the target position at any moment, namely: q_n＝Q₁+(Q₂-Q₁)×(t_n-t₁)；

And c) converting the posture obtained in the step b) back to an Euler angle form.

In the above technical solution, the step 2) includes:

step 2-1), selecting position information and attitude information at a plurality of time points from the position information and attitude information in the whole driving process obtained in the step 1);

step 2-2), respectively substituting the position information on the plurality of time points obtained in the step 2-1) into a calculation formula of a gradient projection method to obtain joint spaces of the robots from the first degree of freedom to the sixth degree of freedom in the plurality of times;

step 2-3), calculating the adjustment amount of the attitude according to the joint space of the robot from the first degree of freedom to the sixth degree of freedom calculated in the step 2-2) and the attitude information of the robot at the plurality of time points obtained in the step 2-1), and calculating the joint space of the robot from the seventh degree of freedom to the ninth degree of freedom at the plurality of time points according to the adjustment amount of the attitude.

In the above technical solution, in the step 2-2), the calculation formula of the gradient projection method is as follows:

in the above-mentioned formula,representing joint velocity, J is the Jacobian matrix, J⁺Is a pseudo-inverse matrix of J,is the first derivative of the position with respect to time,

is a set of free vectors, and alpha represents an amplification factor; wherein,

β_W，β_D，β_L，β_Rfor the weighting coefficients, they are of a value of [0, 1 ]]The number of (1);

w is a function of the degree of operability, $W=\sqrt{\det \left({\mathrm{JJ}}^T\right)};$

d (theta) is an obstacle avoidance function,theta represents a certain attitude in the joint space, i represents the number of the capsule-like bounding box, j represents the number of the obstacle, d₀A threshold representing a safe distance, η being a coefficient;

l (theta) is a joint constraint function,

a_i＝(θ_imax+θ_imin) 2 is the median value of the allowable range of each joint, theta_imaxIndicates the maximum value of the i-th joint angle, θ_iminRepresents the minimum value of the ith joint angle, and n is the number of joints;

is a 9-dimensional vector, and the vector is a vector,

lambda denotes

Representing the gradient values.

In the above technical solution, the step 2-3) includes:

step 2-3-1), calculating the attitude R of the robot at a certain moment from the joint space from the first degree of freedom to the sixth degree of freedom calculated in the step 2-2)₁Obtaining the attitude R at the target position from the attitude information generated by the attitude planning of the step 1)₂Calculating the posture R of the tail end hand of the robot, which needs to be adjusted from a certain moment to a target position:

$R=R_1^T{R}_2$

step 2-3-2), solving an equivalent rotating shaft by utilizing rotation transformation;

<math> <mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>Rot</mi> <mrow> <mo>(</mo> <mi>f</mi> <mo>,</mo> <msub> <mi>&theta;</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> </mrow> </math>

<math> <mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

f_xf_yf_zthe equivalent rotating shaft is a Cartesian space rotation transformation equivalent rotating shaft and corresponds to the seventh, eighth and ninth degrees of freedom of the tail end hand of the robot; theta_tIs the attitude R at time t₁And R₂The angle difference of (a); vers theta_t＝1-cosθ_t；

And 2-3-3) calculating the adjustment quantity of the robot relative to the initial posture by the equivalent rotating shaft, namely the joint space of the seventh, eighth and ninth degrees of freedom.

In the above technical solution, the step 3) includes:

step 3-1), solving a simulation result of PTP motion by using the speed and the acceleration of the shaft;

and 3-2) solving the fitting similarity between the simulation result of the PTP movement and the target curve, and selecting a better value from the joint space information of the nine degrees of freedom at the plurality of time points as a control point according to the fitting similarity.

In the above technical solution, the step 3-1) includes:

step 3-1-1), obtaining the longest acceleration time of the ith joint of the robot in PTP motion according to the speed and acceleration parameters of the shaft;

wherein, t_aiIndicates the acceleration completion time of the ith joint,

represents the maximum speed of the axis motor of the robot,represents a maximum acceleration of a shaft motor of the robot;

the distance of the transformation is represented by,

(end joint space θ)_iEnd) - (starting point joint space θ)_iStart)

Step 3-1-2), calculating the longest finish time of acceleration, the longest finish time of uniform speed and the longest finish time of deceleration of the whole robot according to the result obtained in the previous step;

t_di＝t_ei-t_ai

maxT_a＝max(t_a1，t_a2，…，t_ai)

maxT_d＝max(t_d1，t_d2，…，t_di)

maxT_e＝max(t_e1，t_e2，…，t_ei)

wherein, t_eiIndicates the end time, t_diRepresenting the time of completion of the uniform speed;

3-1-3), calculating the running speed and running acceleration of each joint under the condition of uniform acceleration and uniform speed;

$A_i=\frac{V_i}{\max T_a}$

wherein, V_iIs the running speed of the joint i, A_iIs the running acceleration of joint i;

step 3-1-4), calculating an expression theta of joint space along with time in PTP operation according to the calculation result_tSubstituting into the kinematics positive solution to obtain a simulation result F_ptp(θ_{1sta rt}，θ_iEndT); wherein,

<math> <mrow> <msub> <mi>&theta;</mi> <mi>t</mi> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>A</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <msup> <mi>t</mi> <mn>2</mn> </msup> </mtd> <mtd> <mn>0</mn> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>a</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <mi>t</mi> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfrac> <msup> <msub> <mi>V</mi> <mi>i</mi> </msub> <mn>2</mn> </msup> <msub> <mi>A</mi> <mi>i</mi> </msub> </mfrac> </mtd> <mtd> <mi>max</mi> <msub> <mi>T</mi> <mi>a</mi> </msub> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>d</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> <mo>-</mo> <mfrac> <msub> <mi>A</mi> <mi>i</mi> </msub> <mn>2</mn> </mfrac> <mo>&CenterDot;</mo> <msup> <mrow> <mo>(</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> <mo>-</mo> <mi>t</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> <mtd> <mi>max</mi> <msub> <mi>T</mi> <mi>d</mi> </msub> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow> </math>

in the above technical solution, the step 3-2) includes:

step 3-2-1), calculating the similarity degree by utilizing the principle of autocorrelation

Step 3-2-2), calculating

Value and threshold of

Comparing, if the curve fitting time is less than the preset time, judging that the curve fitting is successful, otherwise, supplementing a key point at the middle point of the target curve in time, dividing the target curve into two sections, and then repeatedly executing the step 3-1) until the whole curve fitting is completed; the key points to be used in the fitting process are the control points.

The invention has the advantages that:

by adopting the robot motion driving method, a user only needs to give the positions and postures of the tail end hand of the robot on a few points in space, the robot can be driven, and the method is simple to realize and efficient to operate.

Drawings

Fig. 1 is a schematic view of 9 degrees of freedom of a 9 degree of freedom robot according to the present invention;

FIG. 2 is a flow diagram of a robot motion driving method in one embodiment;

fig. 3 is a schematic view of a 9-degree-of-freedom robot with seventh, eighth and ninth degrees of freedom at a rotation joint according to the present invention;

FIG. 4 is a schematic diagram of an obstacle avoidance structure in one embodiment;

FIG. 5 is a schematic diagram of the acceleration, deceleration, and uniform velocity process of a joint, according to one embodiment.

Detailed Description

The invention is described below with reference to the accompanying drawings and the detailed description.

Before describing the present invention, some concepts involved in the present invention are explained in a unified manner for the understanding.

1. 9 degrees of freedom of the robot: referring to fig. 1, a mobile joint of a robot has a first degree of freedom, a shoulder joint has a second degree of freedom, an elbow joint has a third degree of freedom, a wrist joint has a fourth degree of freedom, a hand joint has fifth, sixth and seventh degrees of freedom, and a rotation joint has eighth and ninth degrees of freedom.

2. Cartesian space: refers to a linear space defined using a cartesian coordinate system. Three-dimensional cartesian space is used in this application to represent the motion space of a robot in a broad sense.

3. Joint space: assuming that the robot has n degrees of freedom, the robot joint coordinate system is an n-dimensional space, and the joint space is a set composed of a series of n-dimensional spaces, and the result of the kinematic positive solution of each feature in the set is the same (i.e. the positions and postures of the robot ends solved by each feature in the joint space through forward kinematics are the same).

4. Gradient projection method: the basic formula of the gradient projection method is:

wherein,

is the time derivative of the angle to be determined, J is the Jacobian matrix, J⁺Is a pseudo-inverse matrix of J,is a position andthe first derivative of the attitude vector with respect to time,

for a set of free vectors (a free vector is an arbitrary vector that satisfies the dimension), α represents a scalar coefficient.

Free vector

Which can be expressed as a vector with respect to the joint space theta, the derivative over time of the free vector yields the following equation:

wherein,

representing free vectors

For the gradient value of joint space [. sup. ]]^TRepresenting a transposed matrix.

From the above equations (1) and (2), the extended equation (3) is obtained:

5. posture: refers to the pose orientation of the end of the robot hand.

6. Forward kinematics: knowing the joint coordinates of each joint of the robot, the pose of each end of the robot is determined, and the mapping from joint space to cartesian space at the end of the robot is called forward kinematics.

After the above concepts are collectively described, a motion driving method of an image pickup robot of the present invention is explained in the following embodiments.

In the background art, it has been mentioned that the user needs to provide some basic information for the camera robot, such as several key points in the motion track of the camera lens, the time when the camera lens reaches these key points, the time when the camera lens stops at some key points, etc. The information of the key points comprises the position coordinates of the points on the x, y and z axes in the cartesian space and the attitude vector (Roll, Pitch, Yaw) of the tail end of the robot hand on the points. The information of these key points is essentially the position and posture of the camera lens (i.e. the end of the hand of the camera robot), and it is known to those skilled in the art that to realize the motion driving of the camera robot, the information of the angle change of all joints of the camera robot along with time needs to be known. How the present embodiment realizes the motion driving of the image pickup robot will be described below with reference to fig. 2 on the basis of the above basic information.

And step 1, realizing the motion trail planning and the posture planning of the camera lens.

The imaging robot according to the present application is a 9-degree-of-freedom robot shown in fig. 1, and when calculating joint velocities of 9 degrees of freedom from the position and orientation of the end of the imaging robot, it is necessary to calculate a (9 × 6) matrix and a (6 × 9) pseudo-inverse matrix corresponding thereto, which takes a large amount of calculation time. Referring to fig. 3, in the imaging robot according to the present invention, since the axes of the eighth degree of freedom, the ninth degree of freedom, and the seventh degree of freedom in the hand joint are aligned at one point and the three rotational directions of the three-dimensional cartesian space are mapped, the imaging robot can change the attitude only by the seventh degree of freedom, the eighth degree of freedom, and the ninth degree of freedom, and similarly, the positional change of the imaging robot is related to only the first six degrees of freedom out of 9 degrees of freedom. Based on the above structural characteristics of the camera robot, the position and the attitude of the camera robot can be considered separately in this embodiment, that is, the motion trajectory planning and the attitude planning of the camera lens in the camera robot are solved separately, so that the complexity of calculation and solution can be reduced, the calculation efficiency is improved, and the real-time completion of the related calculation is ensured.

In view of the above, step 1 comprises the following steps:

step 1-1, the key points provided to the camera robot by the user via the relevant interface are actually discrete points in cartesian space, and therefore it is first necessary to transform these discrete points into a continuous curve representing the motion trajectory of the camera lens, this transformation process is also called motion trajectory planning.

The motion trail planning can adopt a related method in the prior art, such as a method adopting a B spline curve, and the method not only can ensure the continuity of motion, but also has the advantages of simple and efficient equation solution; or the method of the Bezier curve is adopted, the method is widely used in computer drawing, and has the advantage of intuitive operation which is not possessed by a B-spline curve method. In this embodiment, as a preferred implementation manner, the B-spline curve method and the bezier curve method described above may be combined, and a continuous curve formed by discrete points is obtained from the position coordinates of the discrete key points on the x, y, and z axes. It was previously mentioned that the discrete points provided by the user are in cartesian space, and thus the continuous curve generated by the discrete points is in cartesian space. Since there may be a plurality of curves formed by several points, there may be a plurality of results of the motion trajectory planning. In summary, through the motion trajectory planning, a plurality of continuous curves including the key points can be generated from the position coordinates of a plurality of discrete key points input by the user, and the continuous curves can be expressed by a function.

And 1-2, performing attitude planning on the camera robot.

It was mentioned earlier that the information of the key points input by the user includes attitude information (Roll, Pitch, Yaw) in addition to the position information mentioned in step 1-1. However, the number of key points input by the user is limited, and the posture of a certain point between two adjacent key points cannot be directly obtained. The purpose of pose planning is to calculate pose information for these intermediate points.

In the attitude planning process, the key point with the previous time in the adjacent key points is taken as the initial position, the key point with the later time is taken as the target position, and the attitude of the robot at the initial position is taken as R₁Indicating the attitude at the target position by R₂And (4) showing. R₁And R₂All include Roll, Pitch, and Yaw components. In accordance with the common general knowledge in the art, the attitude information of the start position and the target position can be first rewritten into quaternion (see reference 2, page 190 of 3D Math Primer for Graphics and Gate Development, Fletcher Dunn, Wordware Publishing, Inc), namely R₁→Q₁，R₂→Q₂Then, Q at any time is obtained_n＝Q₁+(Q₂-Q₁)×(t_n-t₁) (ii) a Finally, the quaternion Q is obtained_nAnd converted to the euler angle form of Roll, Pitch, Yaw.

Through the attitude planning, the attitude represented by (Roll, Pitch, Yaw) at any time in the whole driving process can be obtained.

And 1-3, storing results of motion trajectory planning and posture planning.

The motion trajectory planning result and the attitude planning result obtained in the previous steps 1-1 and 1-2 are continuous functions, so that the robot driving process does not need much information, and only needs to take a plurality of discrete points on the continuous functions. Since the robot control has a minimum time

Thus, the total time of robot movement can be divided into

Is then substituted into the functions generated in the previous step 1-1 and step 1-2 for representing the motion trail and postureAnd obtaining the positions and postures of the tail end of the robot hand on a plurality of discrete points. The information is saved. In this step, the

The specific value of the integral multiple can be determined according to the precision in actual operation.

And 2, solving the joint space of the camera robot.

As mentioned earlier, the position of the camera robot is considered separately from the pose for the convenience of calculation solution. The generation of the joint space of the first 6 degrees of freedom of the imaging robot is related to the position information of the end hand of the imaging robot, and the generation of the joint space of the last 3 degrees of freedom is related to the posture information of the end hand of the imaging robot. And are therefore described separately below.

And 2-1, solving joint space of the front 6 degrees of freedom of the camera robot.

On the basis of the known position information of the end hand of the robot, the inverse kinematics of the robot is used for solving an array corresponding to the position information in joint space, namely the joint space with the first 6 degrees of freedom.

The inverse kinematics solution must have four constraints: 1) the safety of the robot is ensured; 2) collision is avoided; 3) joint constraint is avoided; 4) joint singularity and algorithm singularity are avoided.

The kinematic equations of the robot can be expressed as (see the aforementioned reference 1, page 79):

X＝f(θ) (4)

in the above formula, X ∈ R^mIndicating the position of the robot in cartesian space (because of the separation of position and attitude, X mentioned here is only position information, slightly different from the attitude and position represented by X in the formula (1) above); theta is formed by RⁿRepresents the joint space of the robot, wherein n denotes the number of joints, m is the dimension of the target vector,in the present application, m refers to cartesian spatial dimensions, which specifically include 6 dimensions (x, y, z, roll.

By converting the above equation (4), the Cartesian space velocity representing the image pickup robot can be obtained

Velocity of joint

The following formula for the relationship:

<math> <mrow> <mover> <mi>X</mi> <mo>&CenterDot;</mo> </mover> <mo>=</mo> <mi>J</mi> <mover> <mi>&theta;</mi> <mo>&CenterDot;</mo> </mover> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>5</mn> <mo>)</mo> </mrow> </mrow> </math>

the Jacobian matrix in the above formula is known

，J∈R^m×nVelocity of the end of the hand of a given camera robot in Cartesian space

(which can be obtained by first deriving the position information in cartesian space generated in step 1 with respect to time), then solving equation (5) can obtain the following inverse solution of robot kinematics:

the above formula is the order mentioned beforeThe basic formula of the ladder projection method, equation (1), is a general solution form of equation (5), where J⁺Is Moore-Penrose pseudo-inverse of Jacobian matrix J, and alpha belongs to R^IIs a scalar coefficient of the number of bits,

is a free vector which can be arbitrarily chosen. The first term on the right of the equation is the special solution of equation (5) and the second term is the homogeneous solution. In a physical sense, the first term defines the motion of the robot hand, and the second term defines joint space self-motion that does not affect the motion of the hand. Joint space self-motion means that in solving a given target value after determining the function mapping, the series of joints can change due to redundancy without affecting the mapping result.

From the foregoing description, it can be known that the basic formula of the staircase projection method can be further expanded, and the expanded formula is shown in formula (3).

As can be seen by observing the associated formula for the gradient projection method, J, J⁺、

I is known or can be calculated from existing data, and therefore joint velocity is calculated

Is that how to obtain the free vectorAnd a scalar coefficient α called an amplification coefficient.

Solving for free vectors

In solving for free vector

When considering the aforementioned reverse movementFour constraints involved in the mathematical solution: the safety of the robot is ensured; collision is avoided; joint constraint is avoided; joint singularity and algorithm singularity are avoided. Corresponding objective functions can be established according to these constraints, which are described separately below.

1. In order to avoid joint singularity and algorithm singularity, a function of the operability is introduced in the application. The operability is defined by Yoshikawa in the process of researching the working capability of the redundant degree of freedom robot, the operability can be represented by W, and the calculation formula of the operability is as follows:

$W=\sqrt{\det \left({\mathrm{JJ}}^T\right)}---\left(6\right)$

the most direct relationship between W and J can be expressed as the product of m singular values of J obtained by singular value decomposition of J

W＝σ₁·σ₂·…·σ_m (7)

In the formula sigma₁≥σ₂≥…≥σ_mMore than or equal to 0 is the singular value of the J matrix. According to the matrix theory, all m singular values of the J matrix with full rank of rows are larger than zero, and the singular values of the J matrix with reduced rank have zero value, when W is 0, the corresponding J matrix is reduced rank, namely, the robot is in joint singularity, which is the most undesirable in the motion planning of the robot. To avoid moving to a position where joints are singular, the degree of operability must be greater than zero. The larger the value of W, the farther the joint is from the position where the singularity occurs, and the better the operability. Therefore, zero can be regarded as a repulsive force source of W, and the relation is introduced into the motion control of the robot with redundant degrees of freedom, which is beneficial to avoiding motion to jointThe position of the generator.

2. To avoid collision problems, an obstacle avoidance function may be set. Considering that the robot basically has a chain bar as its basic structure, the simplest capsule-like structure as shown in fig. 4 can be used to surround it in this embodiment. When the distance between the obstacle and the capsule-shaped structure of the robot is less than a certain value, the following obstacle avoidance function D (θ) ═ D (θ, B) can be obtained:

<math> <mrow> <msub> <mi>D</mi> <mi>ij</mi> </msub> <mrow> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mi>&eta;</mi> <mrow> <mo>(</mo> <mfrac> <mn>1</mn> <mrow> <msub> <mi>d</mi> <mi>ij</mi> </msub> <mrow> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> </mrow> </mrow> </mfrac> <mo>-</mo> <mfrac> <mn>1</mn> <msub> <mi>d</mi> <mn>0</mn> </msub> </mfrac> <mo>)</mo> </mrow> <mo>,</mo> <msub> <mi>d</mi> <mi>ij</mi> </msub> <mrow> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> </mrow> <mo>&le;</mo> <msub> <mi>d</mi> <mn>0</mn> </msub> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> <mo>,</mo> <msub> <mi>d</mi> <mi>ij</mi> </msub> <mrow> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> </mrow> <mo>></mo> <msub> <mi>d</mi> <mn>0</mn> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>8</mn> <mo>)</mo> </mrow> </mrow> </math>

in the above formula, θ represents a certain posture in the joint space, B represents an obstacle, i represents the number of the capsule-like bounding box, j represents the number of the obstacle, and d₀The threshold representing the safe distance, η, is a coefficient whose value is usually taken to be 1.

3. In order to avoid the problem of joint constraint, the joint needs to be ensured to be in a constraint middle position as much as possible. The method for optimizing the joint motion is to set the middle position (theta) of the joint motion range_imax+θ_imin) With/2 as a reference, the joint constraint function is constructed as follows:

<math> <mrow> <mi>L</mi> <mrow> <mo>(</mo> <mi>&theta;</mi> <mo>)</mo> </mrow> <mo>=</mo> <mfrac> <mn>1</mn> <mi>n</mi> </mfrac> <msubsup> <mi>&Sigma;</mi> <mrow> <mi>i</mi> <mo>=</mo> <mn>1</mn> </mrow> <mi>n</mi> </msubsup> <msup> <mrow> <mo>(</mo> <mfrac> <mrow> <mi>&theta;</mi> <msub> <mrow> <mo>-</mo> <mi>a</mi> </mrow> <mi>i</mi> </msub> </mrow> <mrow> <msub> <mi>a</mi> <mi>i</mi> </msub> <mo>-</mo> <msub> <mi>&theta;</mi> <mrow> <mi>i</mi> <mi>max</mi> </mrow> </msub> </mrow> </mfrac> <mo>)</mo> </mrow> <mn>2</mn> </msup> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>9</mn> <mo>)</mo> </mrow> </mrow> </math>

in the formula a_i＝(θ_imax+θ_imin) 2 is the median value of the allowable range of each joint, theta_imaxIndicates the maximum value of the i-th joint angle, θ_iminRepresents the value at which the ith joint angle is minimum, and n is the number of joints. The optimization of the range of motion of the joint is to make the above-mentioned joint constraint function L (theta)_i) The value of (c) is minimal.

As mentioned above, the four constraints also include a constraint condition for ensuring the safety of the robot itself, and this constraint condition is solved substantially at the same time as the above three constraints are solved.

In addition to the three objective functions of the above formula (6), formula (8), and formula (9), the first degree of freedom (the first degree of freedom may be referred to as a guide degree of freedom or a sliding degree of freedom because the moving joint substantially slides on the guide rail) in the moving joint of the imaging robot is considered to have a great influence on the solution, and the unit of the sliding degree of freedom and the rotational degree of freedom are not in one order of magnitude, and therefore, the target functions are used here

It is treated as a free vector alone. This free vector has an effect only on the guideway.

After the above four objective functions are generated, how to establish a unified optimization index is discussed below.

The solution of the self-motion and the special solution of the formula 3 are ensured to be in an order of magnitude, so that the four objective functions are physically kept consistent in meaning, and the fact that the four constraints are meaningful when the functions are exerted can be ensured, so that each objective function needs a parameter to be allocated to the same physical level. Since the operability of the robot is deteriorated near the position where the joints are strange, it is necessary to pay attention to these areas as forbidden areas. The effect of the operationality as an index for joint avoidance and singular optimization of the algorithm is not obvious, because W has a large value around "zero". Therefore, 1/W (theta) is selected as a parameter for measuring the influence of joint singularity on the self-movement. Similarly, the target optimization for obstacle avoidance is much better than others with (1/D (θ)). The four motion optimizations are represented by potential energy field functions with the same trend, thus having the same physical meaning.

Not only the trend of the potential energy field but also the order of magnitude has to be taken into account in the potential energy field function. For example, an appropriate operability value W is selected₀(W＝W₀Corresponding to a boundary value near the singular position of the joint), when W > W₀When the robot is far away from the joint singularity, the influence of the joint singularity is not needed to be considered, namely the self-motion of the joint space of the robot is not needed to be adjusted to increase W (theta). Conversely, if the factor by which W (θ) increases is still taken into account, it may affect the implementation of obstacle avoidance or other motion optimization. Meanwhile, the obstacle avoidance problem should also take the range of the potential force field into consideration, different from the former, the optimization of the motion range of each joint should be always required in the motion planning, and because the change range of the L (theta) is limited, the influence on the motion optimization of the former is small, the following unified optimization index can be established

Wherein beta is_W，β_D，β_L，β_RAs weighting coefficients, they may be based on requirementsIs defined as a value of [0, 1 ]]The number of (2).

Solving the amplification factor alpha

Will unify the free vectors

After the determination, the amplification factor α is considered. The amplification factor directly controls the effect of path tracking optimization. If the size is small, the optimization effect cannot be reflected, and the constraint condition is damaged; if the result is large, the result of the self-motion is larger than the basic solution, so that the result is wrong and the inverse kinematics solution is not satisfied.

The principle of selecting the amplification factor is to ensure

Decreasing with time, for the chosen optimization index function

Due to the fact that

By selecting the amplification factor alpha to satisfy or influence

Trend of change, for this order

Where λ < 0, then corresponds toIs/are as follows

Decreases with time T. α is such that the above formula holds

In finding the free vector

After the coefficient alpha is amplified, the coefficients can be substituted into the formula (6) to calculate the joint velocity of the front 6 axes (i.e. the front 6 degrees of freedom) of the camera robot

After obtaining the joint velocity of the first 6 degrees of freedom, the method can be based on

And obtaining the angle information of the specific moment, and obtaining the position and the posture of the tail end of the robot according to the forward kinematics.

Step 2-2, obtaining joint space with last 3 degrees of freedom

And 2-2-1, according to the attributes of the joints of the robot, adjusting the seventh, eighth and ninth degrees of freedom in the joints without changing positions, so that the 3 degrees of freedom are required to adjust the posture. After the joint space with the front 6 degrees of freedom is obtained, the posture R of the terminal hand of the camera robot at a certain moment can be solved by combining forward kinematics₁And the attitude information generated by the attitude planning of step 1 can obtain the attitude at the target position as R₂Then, the posture R of the robot end hand to be adjusted from a certain moment to the target position is:

$R=R_1^T{R}_2---\left(11\right)$

and 2-2-2, solving the equivalent rotating shaft by utilizing rotation transformation.

<math> <mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>Rot</mi> <mrow> <mo>(</mo> <mi>f</mi> <mo>,</mo> <msub> <mi>&theta;</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> <mo>=</mo> </mrow> </math>

<math> <mrow> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>12</mn> <mo>)</mo> </mrow> </mrow> </math>

Wherein f ═ f_xf_yf_z]^TThe universal rotation transformation equivalent rotating shaft corresponds to the seventh, eighth and ninth degrees of freedom of the tail end hand of the robot; theta_tIs the attitude R at time t₁And R₂The angle difference of (a); vers theta_t＝1-cosθ_t。

Due to θ in equation (12)_tThe trigonometric functions are known, and the posture adjustment amount R can be calculated by the formula (11), so that the equivalent rotating shafts f of the seventh, eighth and ninth degrees of freedom of the robot tail end hand can be calculated_x、f_y、f_z。

Step 2-2-3, calculating f_x、f_y、f_zThen, the adjustment amounts of the robot with respect to the initial attitude, that is, the joint spaces of the seventh, eighth, and ninth degrees of freedom can be obtained.

Through the steps, all information of complete joint space changing along with time can be obtained. The calculated values of the joint space theta over time are supplemented to the data structure generated in step 1-3.

And 3, simulating by using joint space information with nine degrees of freedom, comparing the curve obtained after simulation with different fitting curves, and selecting an optimal value as a control point in the driving process.

Through the previous steps, joint spaces of the camera robot are obtained, and the robot can be driven to move by the joint spaces. It is mentioned above that the joint space corresponds to the position of the end hand of the camera robot in the cartesian space, and the position of the end hand of the camera robot in the cartesian space is the discrete point mentioned in step 1-3. The larger the number of these discrete points is, the more joint spaces corresponding to these discrete points are, and the more control information is transmitted to the imaging robot. Theoretically, the more joint space information, the more advantageous the control accuracy of the imaging robot to be driven. However, in the actual driving process, there is a running process for starting, stopping or changing the speed of the device (such as a servo motor) for driving the robot, and it is easy to understand that when the camera robot is from a standstill to a certain joint speed, the camera robot is inevitably subjected to a process of accelerating first and then keeping the speed constant. However, if the externally input control information is too frequent, the previous joint speed of the robot is not reached, a new acceleration or deceleration process is performed, and the joint motion is consumed in the continuous acceleration or deceleration process, so that the operation speed of each joint of the robot is likely to fail to reach the actual value.

For the above reasons, it is therefore necessary to select the values obtained in the previous step, and the number of discrete points to be selected should be as small as possible, while ensuring that the driving process is accurate.

In the embodiment, the process of solving the motion trail of the robot by joint speed is realized by adopting the simulation PTP motion. In the process, the joint space theta obtained in the step 2 is selected through a simulation algorithm, and the minimum control points required by the robot driving are obtained.

As a common knowledge of those skilled in the art, there are three modes of joint linkage during robot driving: asynchronous, synchronous, and fully synchronous. In this embodiment, the simulation PTP process uses a fully synchronous co-working mode.

PTP movement is in three stages, acceleration, constant speed and deceleration. Wherein t is 0 to t is t_a(t_aReferred to as acceleration time) is an acceleration stage, t is t ═ t_aTo t ═ t_d(t_dMean uniform velocity time) is a uniform velocity stage, and finally t is t ═ t_e(t_eIndicating the deceleration completion time) is the deceleration phase. As shown in FIG. 5, the PTP movement of the complete synchronization and the same work needs to ensure that all joints of the robot move t_a1＝t_a2＝…＝t_ai，t_d1＝t_d2＝…＝t_di，t_e1＝t_e2＝…＝t_ei. In this embodiment, the necessary control points are obtained by half-inserting the key points based on the fully synchronous synchronization mode. The method comprises the following concrete implementation steps:

step 3-1, solving simulation result F of PTP motion by using speed and acceleration of shaft_ptp(θ₁，θ₂，t)＝X。

3-1-1, obtaining the longest acceleration time of the ith joint of the robot in PTP motion according to the speed and acceleration parameters of the shaft;

wherein,

which represents the maximum speed of the shaft motor,

represents the maximum acceleration of the shaft motor;

the distance of the transformation is represented by,

and 3-1-2, calculating the longest acceleration completion time, the longest uniform speed completion time and the longest deceleration completion time of the whole robot according to the result obtained in the previous step.

Specifically, first, the completion time of the uniform velocity and deceleration at the ith joint by the robot can be calculated by the following equations (15) and (16)

t_di＝t_ei-t_ai (16)

Wherein, t_eiIndicating the time of completion of deceleration, t_diRepresenting the time of completion of the uniform speed;

then calculating the longest finish time of acceleration, the longest finish time of uniform speed and the longest finish time of deceleration of the whole robot by the following formula

maxT_a＝max(t_a1，t_a2，…，t_ai)

maxT_d＝max(t_d1，t_d2，…，t_di) (17)

maxT_e＝max(t_e1，t_e2，…，t_ei)

And 3-1-3, calculating the running speed and running acceleration of each joint under the condition of uniform acceleration and uniform speed.

$A_i=\frac{V_i}{\max T_a}---\left(19\right)$

Wherein, V_iIs the running speed of the joint i, A_iIs the running acceleration of joint i.

Step 3-1-4, calculating an expression theta of joint space along with time in PTP operation according to the calculation result_t

<math> <mrow> <msub> <mi>&theta;</mi> <mi>t</mi> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>A</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <msup> <mi>t</mi> <mn>2</mn> </msup> </mtd> <mtd> <mn>0</mn> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>a</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <mi>t</mi> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfrac> <msup> <msub> <mi>V</mi> <mi>i</mi> </msub> <mn>2</mn> </msup> <msub> <mi>A</mi> <mi>i</mi> </msub> </mfrac> </mtd> <mtd> <mi>max</mi> <msub> <mi>T</mi> <mi>a</mi> </msub> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>d</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> <mo>-</mo> <mfrac> <msub> <mi>A</mi> <mi>i</mi> </msub> <mn>2</mn> </mfrac> <mo>&CenterDot;</mo> <msup> <mrow> <mo>(</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> <mo>-</mo> <mi>t</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> <mtd> <mi>max</mi> <msub> <mi>T</mi> <mi>d</mi> </msub> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>-</mo> <mo>-</mo> <mo>-</mo> <mrow> <mo>(</mo> <mn>20</mn> <mo>)</mo> </mrow> </mrow> </math>

Will be the joint space theta_tSubstituting into the kinematics positive solution to obtain a simulation result F_ptp(θ_1start，θ_iEnd，t)。

Step 3-2, calculating the correlation degree of the fitting

And (3) solving the fitting correlation by utilizing the ratio of the difference of the Cartesian space distance of the target curve (such as Bezieer) obtained by PTP motion at the corresponding time and the motion trajectory planning in the step (1) to the Cartesian space length of the target curve. The calculation formula of the fitting correlation is as follows:

calculated by the above formula

Is at a threshold value

And thirdly, the curve fitting is considered to be successful. Otherwise, a key point is supplemented at the middle point of the target curve in time. Dividing the target curve into two sections to solve PTP fitting, then repeating the step 3-1 until the whole curve is fitted, and if the fitting is not completed, adjusting the threshold value

Finally, it should be noted that the above embodiments are only used for illustrating the technical solutions of the present invention and are not limited. Although the present invention has been described in detail with reference to the embodiments, it will be understood by those skilled in the art that various changes may be made and equivalents may be substituted without departing from the spirit and scope of the invention as defined in the appended claims.

Claims (11)

1. A motion driving method of a robot comprises 9 degrees of freedom, wherein a moving joint of the robot has a first degree of freedom, a shoulder joint has a second degree of freedom, an elbow joint has a third degree of freedom, a wrist joint has a fourth degree of freedom, a hand joint has a fifth degree of freedom, a sixth degree of freedom and a seventh degree of freedom, and a self-rotation joint has an eighth degree of freedom and a ninth degree of freedom; the method comprises the following steps:

step 1), realizing motion trajectory planning and posture planning of the robot tail end hand by using information of positions and postures of the robot tail end hand on a plurality of points in a Cartesian space, which is provided by a user, and obtaining the position of the robot tail end hand in the whole driving process and the posture of the robot tail end hand in the whole driving process;

step 2), solving joint space of the robot from a first degree of freedom to a sixth degree of freedom at a certain time point by using the position information of the robot end hand obtained in the step 1), and solving joint space of the robot from a seventh degree of freedom to a ninth degree of freedom at the time point by using the posture information of the robot end hand obtained in the step 1); the resulting joint space of nine degrees of freedom is used to drive the motion of the robot.

2. The motion driving method of a robot according to claim 1, further comprising:

and 3) simulating by using the joint space information of the robot with nine degrees of freedom at a plurality of time points, which is obtained in the step 2), and selecting a better value from the joint space information of the nine degrees of freedom at the plurality of time points as a control point according to a simulation result.

3. The method of driving a robot according to claim 1 or 2, wherein in the step 1), the motion trajectory planning comprises:

and performing curve fitting on the position information of the terminal hand of the robot provided by the user on a plurality of points in a Cartesian space to obtain at least one motion trail curve.

4. The method of claim 3, wherein the curve fitting is performed by a B-spline method, a Bezier method, or a combination of a B-spline and a Bezier curve.

5. The method of driving a robot according to claim 1 or 2, wherein in the step 1), the pose planning comprises:

step a) Respectively taking two adjacent points of known attitude information in Cartesian space as an initial position and a target position, modifying the attitude information of the initial position and the target position from an Euler angle form to a quaternion form, and respectively using Q₁And Q₂Represents;

step b), solving the attitude between the initial position and the target position at any moment, namely: q_n＝Q₁+(Q₂-Q₁)×(t_n-t₁)；

And c) converting the posture obtained in the step b) back to an Euler angle form.

6. The motion driving method of a robot according to claim 1 or 2, wherein the step 2) comprises:

step 2-1), selecting position information and attitude information at a plurality of time points from the position information and attitude information in the whole driving process obtained in the step 1);

step 2-2), respectively substituting the position information on the plurality of time points obtained in the step 2-1) into a calculation formula of a gradient projection method to obtain joint spaces of the robots from the first degree of freedom to the sixth degree of freedom in the plurality of times;

step 2-3), calculating the adjustment amount of the attitude according to the joint space of the robot from the first degree of freedom to the sixth degree of freedom calculated in the step 2-2) and the attitude information of the robot at the plurality of time points obtained in the step 2-1), and calculating the joint space of the robot from the seventh degree of freedom to the ninth degree of freedom at the plurality of time points according to the adjustment amount of the attitude.

7. The method of claim 6, wherein in the step 2-2), the gradient projection method is calculated by the following formula:

in the above-mentioned formula,

representing joint velocity, J is the Jacobian matrix, J⁺Is a pseudo-inverse matrix of J,

is the first derivative of the position with respect to time,

is a set of free vectors, and alpha represents an amplification factor; wherein,

β_W，β_D，β_L，β_Rfor the weighting coefficients, they are of a value of [0, 1 ]]The number of (1);

w is a function of the degree of operability, $W=\sqrt{\det \left({\mathrm{JJ}}^T\right)};$

d (theta) is an obstacle avoidance function,

theta represents a certain attitude in the joint space, i represents the number of the capsule-like bounding box, j represents the number of the obstacle, d₀A threshold representing a safe distance, η being a coefficient;

l (theta) is a joint constraint function,a_i＝(θ_imax+θ_imin) Per 2 is the allowable range of each jointMedian value of the circumference, θ_imaxIndicates the maximum value of the i-th joint angle, θ_iminRepresents the minimum value of the ith joint angle, and n is the number of joints;

is a 9-dimensional vector, and the vector is a vector,

lambda denotes

Representing the gradient values.

8. The motion driving method of a robot according to claim 6, wherein the step 2-3) comprises:

step 2-3-1), calculating the attitude R of the robot at a certain moment from the joint space from the first degree of freedom to the sixth degree of freedom calculated in the step 2-2)₁Obtaining the attitude R at the target position from the attitude information generated by the attitude planning of the step 1)₂Calculating the posture R of the tail end hand of the robot, which needs to be adjusted from a certain moment to a target position:

$R=R_1^T{R}_2$

step 2-3-2), solving an equivalent rotating shaft by utilizing rotation transformation;

<math> <mrow> <mi>R</mi> <mrow> <mo>(</mo> <mi>i</mi> <mo>)</mo> </mrow> <mo>=</mo> <mi>Rot</mi> <mrow> <mo>(</mo> <mi>f</mi> <mo>,</mo> <msub> <mi>&theta;</mi> <mi>t</mi> </msub> <mo>)</mo> </mrow> </mrow> </math>

<math> <mrow> <mo>=</mo> <mfenced open='[' close=']'> <mtable> <mtr> <mtd> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>-</mo> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>y</mi> </msub> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mi>f</mi> <mi>x</mi> </msub> <msub> <mrow> <mi>sin</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>f</mi> <mi>z</mi> </msub> <msub> <mi>vers&theta;</mi> <mi>t</mi> </msub> <mo>+</mo> <msub> <mrow> <mi>cos</mi> <mi>&theta;</mi> </mrow> <mi>t</mi> </msub> </mtd> <mtd> <mn>0</mn> </mtd> </mtr> <mtr> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>0</mn> </mtd> <mtd> <mn>1</mn> </mtd> </mtr> </mtable> </mfenced> </mrow> </math>

f_xf_yf_zthe equivalent rotating shaft is a Cartesian space rotation transformation equivalent rotating shaft and corresponds to the seventh, eighth and ninth degrees of freedom of the tail end hand of the robot; theta_tIs the attitude R at time t₁And R₂The angle difference of (a); vers theta_t＝1-cosθ_t；

And 2-3-3) calculating the adjustment quantity of the robot relative to the initial posture by the equivalent rotating shaft, namely the joint space of the seventh, eighth and ninth degrees of freedom.

9. The motion driving method of a robot according to claim 2, wherein the step 3) comprises:

step 3-1), solving a simulation result of PTP motion by using the speed and the acceleration of the shaft;

and 3-2) solving the fitting similarity between the simulation result of the PTP movement and the target curve, and selecting a better value from the joint space information of the nine degrees of freedom at the plurality of time points as a control point according to the fitting similarity.

10. The motion driving method of a robot according to claim 9, wherein the step 3-1) comprises:

step 3-1-1), obtaining the longest acceleration time of the ith joint of the robot in PTP motion according to the speed and acceleration parameters of the shaft;

wherein,

which represents the maximum speed of the shaft motor,

represents the maximum acceleration of the shaft motor;

the distance of the transformation is represented by,

step 3-1-2), calculating the longest finish time of acceleration, the longest finish time of uniform speed and the longest finish time of deceleration of the whole robot according to the result obtained in the previous step;

t_di＝t_ei-t_ai

maxT_a＝max(t_a1，t_a2，…，t_ai)

maxT_d＝max(t_d1，t_d2，…，t_di)

maxT_e＝max(t_e1，t_e2，…，t_ei)

wherein, t_eiIndicates the end time, t_diRepresenting the time of completion of the uniform speed;

3-1-3), calculating the running speed and running acceleration of each joint under the condition of uniform acceleration and uniform speed;

$A_i=\frac{V_i}{\max T_a}$

wherein, V_iIs the running speed of the joint i, A_iIs the running acceleration of joint i;

step 3-1-4), calculating an expression theta of joint space along with time in PTP operation according to the calculation result_tSubstituting into the kinematics positive solution to obtain a simulation result F_ptp(θ_{1sta rt}，θ_iEnd，t) (ii) a Wherein,

<math> <mrow> <msub> <mi>&theta;</mi> <mi>t</mi> </msub> <mo>=</mo> <mfenced open='{' close=''> <mtable> <mtr> <mtd> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <msub> <mi>A</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <msup> <mi>t</mi> <mn>2</mn> </msup> </mtd> <mtd> <mn>0</mn> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>a</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <mi>t</mi> <mo>-</mo> <mfrac> <mn>1</mn> <mn>2</mn> </mfrac> <mfrac> <msup> <msub> <mi>V</mi> <mi>i</mi> </msub> <mn>2</mn> </msup> <msub> <mi>A</mi> <mi>i</mi> </msub> </mfrac> </mtd> <mtd> <mi>max</mi> <msub> <mi>T</mi> <mi>a</mi> </msub> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>d</mi> </msub> </mtd> </mtr> <mtr> <mtd> <msub> <mi>V</mi> <mi>i</mi> </msub> <mo>&CenterDot;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> <mo>-</mo> <mfrac> <msub> <mi>A</mi> <mi>i</mi> </msub> <mn>2</mn> </mfrac> <mo>&CenterDot;</mo> <msup> <mrow> <mo>(</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> <mo>-</mo> <mi>t</mi> <mo>)</mo> </mrow> <mn>2</mn> </msup> </mtd> <mtd> <mi>max</mi> <msub> <mi>T</mi> <mi>d</mi> </msub> <mo>&lt;</mo> <mi>t</mi> <mo>&le;</mo> <mi>max</mi> <msub> <mi>T</mi> <mi>e</mi> </msub> </mtd> </mtr> </mtable> </mfenced> <mo>.</mo> </mrow> </math>

11. the motion driving method of a robot according to claim 10, wherein the step 3-2) comprises:

step 3-2-1), calculating the similarity degree by utilizing the principle of autocorrelation

Step 3-2-2), calculating

Value and threshold of

Comparing, if the curve fitting time is less than the preset time, judging that the curve fitting is successful, otherwise, supplementing a key point at the middle point of the target curve in time, dividing the target curve into two sections, and then repeatedly executing the step 3-1) until the whole curve fitting is completed; the key points to be used in the fitting process are the control points.

Images