WO2018107851A1

WO2018107851A1 - Method and device for controlling redundant robot arm

Info

Publication number: WO2018107851A1
Application number: PCT/CN2017/103286
Authority: WO
Inventors: 阳方平
Original assignee: 广州视源电子科技股份有限公司
Priority date: 2016-12-16
Filing date: 2017-09-25
Publication date: 2018-06-21
Also published as: CN106625666B; CN106625666A

Abstract

A method and device for controlling a redundant robot arm. The method comprises: acquiring information about a current point where a redundant robot arm is located and information about a target point; according to the current point information and the target point information, determining a trajectory function corresponding to a motion trajectory of the redundant robot arm moving from the current point to the target point; establishing an equation corresponding to the trajectory function by using a redundancy spatial vector as an independent variable; and solving the equation corresponding to the trajectory function according to an objective approaching method, so as to obtain the position and speed of each joint in the redundant robot arm corresponding to the motion trajectory. By means of the technical solution, on the premise of obtaining an optimal global solution satisfying a design goal, the search space for a multi-objective optimization problem of a redundant robot arm can be reduced, the occurrence of dimension explosion in a multi-objective solution process can be avoided, the computation required in a redundant robot arm control process can be simplified, and the reaction speed of the redundant robot arm can be improved.

Description

Redundant mechanical arm control method and device

Technical field

The present invention relates to the field of robot technology, and in particular, to a control method and apparatus for a redundant mechanical arm.

Background technique

In recent years, with the improvement of artificial intelligence technology and mechanical control technology and the complexity of tasks performed by robots, redundant mechanical arms (manipulators with more than 6 joints) have been applied to robots to complete. A variety of more complex tasks.

When the redundant manipulator performs a Cartesian space-specific task, there is an infinite group solution in the joint space. In the specific application, a set of optimal solutions can be selected according to the optimization index such as joint speed, joint torque or obstacle distance to determine the redundant machine. The movement of the arm. When the robot performs more complex tasks, it is usually necessary to optimize multiple targets of the redundant manipulator at the same time. At present, the methods commonly used for multi-objective optimization can be classified into a weighted function method and a Pareto frontier method. However, although the weighting function method can use the weighting coefficient to weight the optimization target to transform the multi-objective optimization problem into a single-objective optimization problem, it is difficult to judge the influence of the weighting coefficient change on the optimization result. A solution needs to constantly change the value of the weighting coefficient. The calculation steps are cumbersome and have certain limitations. The computational complexity of the Pareto frontier method increases sharply with the increase of the degree of freedom of the robot arm. For a long time, this kind of method also has the problems of complicated calculation and complicated steps.

Summary of the invention

In view of this, an embodiment of the present invention provides a control method and apparatus for a redundant mechanical arm to solve The multi-objective solving method in the prior art calculates a complicated technical problem with complicated steps.

In a first aspect, an embodiment of the present invention provides a method for controlling a redundant mechanical arm, including:

Obtaining current point information and target point information where the redundant robot arm is located;

Determining, according to the current point information and the target point information, a trajectory function corresponding to a motion trajectory of the redundant robot arm moving from a current point to a target point;

Establishing an equation corresponding to the trajectory function by using a redundancy space vector as an independent variable;

The equation corresponding to the trajectory function is solved according to the target approaching method, and the position and velocity of each joint in the redundant mechanical arm corresponding to the motion trajectory are obtained.

In a second aspect, an embodiment of the present invention further provides a control device for a redundant mechanical arm, including:

An information acquiring unit, configured to acquire current point information and target point information where the redundant robot arm is located;

a trajectory unit, configured to be connected to the information acquiring unit, configured to determine, according to the current point information and the target point information, a trajectory function corresponding to a motion trajectory of the redundant mechanical arm moving from a current point to a target point;

a redundancy processing unit, connected to the trajectory unit, configured to establish an equation corresponding to the trajectory function by using a redundancy space vector as an independent variable; and solving an equation corresponding to the trajectory function according to a target approach method, to obtain the motion The position and velocity of each joint in the redundant robot arm corresponding to the trajectory.

The technical solution for controlling the redundant mechanical arm provided by the embodiment of the present invention obtains current point information and target point information of the redundant mechanical arm, and determines that the redundant mechanical arm moves from the current point to the current point information and the target point information. The trajectory function corresponding to the motion trajectory of the target point is constructed by using the redundancy space vector as the independent variable, and the equation is solved by the target approach method to obtain the position of each joint in the redundant mechanical arm corresponding to the motion trajectory. speed. By adopting the above technical solution, the embodiment of the present invention can reduce the search space of the multi-objective optimization problem of the redundant manipulator under the premise of obtaining the global optimal solution satisfying the design goal, and avoid the occurrence of the dimensional explosion problem in the multi-objective solution process. Simplify redundant machinery The amount of calculation required during the arm control process increases the reaction speed of the redundant manipulator.

DRAWINGS

Other features, objects, and advantages of the present invention will become more apparent from the Detailed Description of Description

1 is a schematic flow chart of a method for controlling a redundant mechanical arm according to Embodiment 1 of the present invention;

2 is a schematic flow chart of a method for controlling a redundant mechanical arm according to Embodiment 2 of the present invention;

FIG. 3 is a schematic diagram of a coordinate system of a nine-degree-of-freedom redundant robot arm according to Embodiment 2 of the present invention; FIG.

4 is a structural block diagram of a control device for a redundant mechanical arm according to Embodiment 3 of the present invention.

detailed description

The present invention will be further described in detail below with reference to the accompanying drawings and embodiments. It is understood that the specific embodiments described herein are merely illustrative of the invention and are not intended to limit the invention. It should also be noted that, for ease of description, only some, but not all, of the present invention are shown in the drawings.

Embodiment 1

Embodiment 1 of the present invention provides a control method for a redundant mechanical arm. The method can be performed by a control device of a redundant robot arm, wherein the device can be implemented by hardware and/or software, and can generally be integrated in a control module for controlling the robot. 1 is a schematic flow chart of a method for controlling a redundant mechanical arm according to Embodiment 1 of the present invention. As shown in FIG. 1, the method includes:

S110. Acquire current point information and target point information where the redundant robot arm is located.

In this embodiment, the current point information and the target point information of the redundant robot arm may be redundant mechanical arm ends. The current point information and the target point information of the terminal may also be the current point information and the target point information of the joints of the redundant robot arm, which are not limited herein. Considering the practicability of the current point information and the target point information, optionally, the current point information of the redundant robot arm may include current point information of the end of the arm and current point information of each joint of the redundant manipulator, target point information Target point information at the end of the robot arm can be included. The current point information (target point information) may include information such as position coordinates, angular coordinates, linear velocity, angular velocity, linear acceleration, and/or angular acceleration of the end of the arm or the joint of the robot arm at the current (target) position.

Exemplarily, when acquiring the current point information of the redundant manipulator, the current point information of the redundant manipulator can be obtained directly by the sensor or by using the calculation result of the last moment as the current point information of the redundant manipulator; When the target point information of the redundant manipulator is used, the target point information to which the end of the redundant manipulator needs to be moved can be calculated according to the current point information of the end of the redundant manipulator and the motion track information of the end of the redundant manipulator. Taking the current point information as the angular coordinate, angular velocity and angular acceleration as an example, the angular information and angular velocity information of a joint of the redundant mechanical arm at the current position can be obtained by an encoder installed at the joint position, and the angular acceleration information can be differentiated. The operation is obtained; or, the target point information of each joint of the robot arm calculated at the previous time can be directly used as the current point information of each joint of the robot arm at the current time.

S120. Determine, according to the current point information and the target point information, a trajectory function corresponding to a motion trajectory of the redundant robot arm moving from a current point to a target point.

In this embodiment, the trajectory function corresponding to the motion trajectory of the redundant mechanical arm moving from the current point to the target point may include a trajectory corresponding to each joint motion trajectory of the redundant mechanical arm when the end of the redundant mechanical arm moves from the current point to the target point. Function, the trajectory function of each joint of the redundant manipulator can be one or more, that is, the trajectory function corresponding to each joint motion trajectory of the redundant manipulator can be described as a whole by a total trajectory function, or can be used for each joint The motion trajectory is described in the form of one or several trajectory functions. Here, it should be pointed out that the trajectory function corresponding to the motion trajectory of each joint of the redundant manipulator may include an equation and/or an inequality. For example, the trajectory function of a joint of the redundant manipulator may include the position coordinates of the joint, The correspondence between speed and/or acceleration and other variables (such as position coordinates, velocity and/or acceleration at the end of the arm) may also include coordinates of the joint position, moving speed, and/or moving angular velocity when the redundant manipulator moves normally. The range of values, etc., is not limited here.

Specifically, when determining a trajectory function corresponding to the motion trajectory of the redundant mechanical arm moving from the current point to the target point, the trajectory function of the end of the redundant mechanical arm may be first determined according to the current point information and the target point information at the end of the redundant mechanical arm. Then, according to the connection relationship between the end of the redundant manipulator and each joint, the relationship between the motion path of each joint and the motion track of the end of the redundant manipulator is determined, and then the redundant robot arm is determined according to the trajectory function of the end of the redundant manipulator. Trajectory function.

S130. Establish an equation corresponding to the trajectory function by using a redundancy space vector as an independent variable.

Specifically, the correspondence between the redundancy vector and the coordinates of each joint position, speed, or acceleration may be first determined, and then the position vector, speed, or position in the trajectory function corresponding to each joint is replaced by the redundancy space vector according to the correspondence. Acceleration and other independent variables, and then establish the equation corresponding to the trajectory function of each joint of the redundant manipulator with the redundancy space vector as the independent variable. The redundancy space vector may be any vector of the redundancy vector space, and the redundancy vector space may be an existing vector space or a custom space, which is not limited herein.

S140. Solve an equation corresponding to the trajectory function according to a target approaching method, and obtain a position and a velocity of each joint in the redundant mechanical arm corresponding to the motion trajectory.

In this embodiment, after obtaining the position and speed of each joint of the redundant mechanical arm, the acceleration of each joint of the redundant mechanical arm can be further determined as needed. Illustratively, when there is a finite set of solutions for the equation corresponding to the trajectory function of a joint of a redundant manipulator, all solutions of the equation can be obtained, and then a set of solutions of the equations can be obtained according to actual needs or according to the set selection rules. The target position of the joint at the next moment At the same time, the moving speed and acceleration of the joint when moving from the current position to the target position are determined. When there is an infinite group solution for the equation corresponding to the trajectory function of a joint of a redundant manipulator, a set of solutions of the equation can be arbitrarily obtained and the target position of the joint at the next moment is obtained by the set of solutions and moved from the current position to The moving speed and acceleration of the target position; first, the constraint condition of the equation solution can be set first, then the solution of the equation that satisfies the constraint condition is obtained, and the movement process of the joint from the current time to the next moment is guided by the set of solutions, There are no restrictions. In order to ensure that the joints of the redundant manipulator can move efficiently, reducing the output torque and motion of each joint, avoiding the singularity and avoiding the limit of joint motion, it is preferable to obtain the redundancy corresponding to the motion trajectory according to the solution of the equation satisfying the constraint condition. The position, velocity and acceleration of the joints in the remaining manipulators.

The control method of the redundant mechanical arm provided by the first embodiment of the present invention acquires current point information and target point information of the redundant mechanical arm, and determines that the redundant mechanical arm moves from the current point to the current point information and the target point information. The trajectory function corresponding to the motion trajectory of the target point is constructed by using the redundancy space vector as the independent variable, and the equation is solved by the target approach method to obtain the position of each joint in the redundant mechanical arm corresponding to the motion trajectory. speed. By adopting the above technical solution, the present embodiment can reduce the search space of the multi-objective optimization problem of the redundant manipulator under the premise of satisfying the global optimal solution of the design target, and avoid the occurrence of the dimensional explosion problem in the multi-objective solution process, simplifying The amount of calculation required during the redundant manipulator control process increases the reaction speed of the redundant manipulator.

Embodiment 2

2 is a schematic flow chart of a method for controlling a redundant mechanical arm according to Embodiment 2 of the present invention. The embodiment is optimized on the basis of the foregoing embodiment. Further, the acquiring the current point information and the target point information where the redundant mechanical arm is located includes: acquiring the speed of the end of the redundant mechanical arm at the target point; According to the speed of the end of the redundant manipulator at the target current point, the joint of the redundant manipulator The angular, joint angular velocity and joint angular acceleration are mapped to the redundancy space.

Further, the determining, according to the current point information and the target point information, a trajectory function corresponding to a motion trajectory of the redundant robot arm moving from a current point to a target point comprises: determining the redundancy according to an inverse kinematics equation The optimization objective function and constraints corresponding to the motion trajectory of the remaining manipulator moving from the current point to the target point.

Further, the solving the equation corresponding to the trajectory function according to the target approaching method, and obtaining the position and velocity of each joint in the redundant mechanical arm corresponding to the motion trajectory includes: in the redundancy space, according to the optimization target a function, a minimum target value corresponding to the optimization target, a corresponding constraint condition, and a preset weighting coefficient, and an equation corresponding to the trajectory function is solved by an auxiliary vector and a single target optimization algorithm to obtain the redundancy corresponding to the motion trajectory The position and speed of the joints in the robot arm.

Correspondingly, as shown in FIG. 2, the control method of the redundant mechanical arm provided by this embodiment includes:

S210. Acquire a speed of the end of the redundant mechanical arm at the target point.

Preferably, the obtaining the speed of the end of the redundant mechanical arm at the current point comprises: according to the posture of the redundant mechanical arm end at the current point and the movement of the redundant mechanical arm end The pose of the next sample point in the trajectory determines the velocity of the end of the redundant robot arm at the next sample point. The position of the end of the redundant manipulator refers to the position and posture of the redundant manipulator. The position can be represented by position coordinates, and the posture can be expressed by angle. Illustratively, information such as the angular velocity and/or angular acceleration of the redundant manipulator at the current point can be obtained from an encoder installed at the end of the redundant manipulator, or directly based on the calculation result of the redundant manipulator at the previous moment. The angular velocity and/or angular acceleration of the end of the mechanical arm at the current point, and then the angular velocity of the redundant manipulator at the next sampling point according to the angle of the redundant manipulator at the current point and the angle of the next sampling, thereby obtaining redundancy The speed of the arm at the next sampling point.

S220, according to the speed of the redundant mechanical arm end at the target point, the redundant mechanical arm Joint angle, joint angular velocity, and joint angular acceleration are mapped to the redundancy space.

Exemplarily, the relationship between the end speed of the redundant manipulator and the joint angular velocity of the redundant manipulator (ie, the trajectory function of each joint of the redundant manipulator with angular velocity as an independent variable) can be expressed as:

among them,

For the speed of the end of the arm,

v _i (i=x, y, z) represents a linear velocity, and ω _i (i = θ, Ψ, Φ) represents an angular velocity;

For the joint angular velocity of the arm,

N is the degree of freedom of the redundant manipulator, N=6+r, r>1; J∈R ^6×N is the Jacobian matrix.

Assuming that α is an arbitrary r (r=n-6>1) column in the Jacobian matrix J, so that the remaining 6 columns constitute a non-singular matrix J ^* , then equation (1) can be rewritten as:

Thus, any joint angular velocity that satisfies equation (2)

Can be further rewritten as:

among them,

Is a special solution that satisfies equation (3).

Is a homogeneous solution of equation (3), and

make

Where i=1, 2, . . . , r, α _j is the jth column of the matrix α;

Is a column matrix, and its pre-r(r=N-6) behaves as 0;

It is a column matrix, and its ith behavior is 1, and the other behaviors of the first r rows except the ith row are 0. Defining the redundancy space vector k=(k ₁ ,...,k _r ) ^T is any vector of the redundancy vector space, then:

Assumed angular velocity

Expressed as S(k), the angle θ is represented by P(k), angular acceleration

Represented by A(k), the angular velocity of the redundant manipulator joint

Can be expressed as a function of the redundancy space vector k:

The expression of the redundant manipulator joint angle θ with respect to the redundancy space vector k is:

Redundant manipulator joint angular acceleration The expression for the redundancy space vector k is:

Where θ ₀ is the joint angle of the previous moment,

For the joint angular velocity at the previous moment, Δt is the sampling step size.

S230. Determine, according to an inverse kinematics equation, an optimization objective function and a constraint condition corresponding to a motion trajectory of the redundant mechanical arm moving from a current point to a target point.

In this embodiment, the optimization objective function and the constraint condition corresponding to the motion trajectory of the redundant mechanical arm moving from the current point to the target point can be flexibly set according to requirements, for example, the range of the redundant mechanical arm movement can be restricted to avoid Obstacle, or limit the bending angle of the redundant manipulator to avoid the joints reaching their motion limits. In view of the practicability of each constraint, optionally, the optimization objective function includes a minimum joint motion amplitude of the redundant mechanical arm or the redundant mechanical arm is minimally affected by a corresponding gear gap; the constraint condition includes : constraining the joint speed of the redundant mechanical arm within a preset speed range or constraining the joint angle of the redundant mechanical arm within a preset angular range. Illustratively, the optimal objective function for the minimum joint motion of the redundant manipulator can be:

The optimal objective function for the redundant manipulator to be minimally affected by the corresponding gear clearance can be:

The constraint that constrains the joint speed of the redundant manipulator to a preset speed range may be:

The constraint that constrains the joint angle of the redundant manipulator within a predetermined range of angles may be: θ _min ≤ θ ≤ θ _max .

S240. The equation corresponding to the trajectory function is established by using a redundancy space vector as an independent variable.

In this embodiment, the optimization objective function and the constraint equation of the redundant robot arm trajectory can be established based on the redundancy space, and the optimization objective function and the constraint equation of the redundant mechanical arm can be expressed as joint angle, joint angular velocity and/or The joint angular acceleration is an expression of the independent variable. Therefore, by substituting the above equations (7), (8), and (9) into the expression, the optimal objective function and the constraint equation with the redundant space vector k as the independent variable can be obtained. . For example, an optimization objective function with minimal joint motion amplitude can be further expressed as an equation for the redundancy space vector k:

The optimization objective function with respect to the minimum influence of the mechanical arm on the corresponding gear gap can be further expressed as the equation of the redundancy space vector k:

The constraint on the joint angular velocity limit can be further expressed as an equation with the redundant space vector k as an independent variable:

The constraint on the joint angle limit can be further expressed as an equation with the redundancy space vector k as an independent variable:

S250. In the redundancy space, according to the optimization objective function, the minimum target value corresponding to the optimization target, the corresponding constraint condition, and the preset weighting coefficient, the equation corresponding to the trajectory function is solved by the auxiliary vector and the single target optimization algorithm. Obtaining each joint in the redundant mechanical arm corresponding to the motion trajectory The location and speed.

Exemplarily, the single-objective optimization algorithm for solving the equation corresponding to the trajectory function can be flexibly determined according to needs, and is not limited herein. Preferably, the single target optimization algorithm comprises: a Newton-Eulerian algorithm, a Nelder-Mead simplex algorithm, an interior point algorithm, a genetic algorithm and/or a Pattern Search algorithm.

The Pareto frontier is a solution set for multi-objective optimization problems, which is defined as follows: On the multi-objective feasible domain Ω, there is a point x ^* ∈ Ω, if there is no point x ∈ Ω, so that f(x) ≤ f(x ^* ) is established, and there is at least a point x' ∈ Ω such that the strict inequality f(x') > f(x ^* ) holds, then x ^* is a Pareto optimal solution. Taking the double-objective optimization problem as an example, the equation corresponding to the trajectory function is as follows:

In the above formula, F(x)={f ₁ (x) f ₂ (x)} is the optimization objective function,

For the minimum target value corresponding to the optimization objective function, A is a matrix constraint condition, b is a vector constraint condition, ω is a weighting coefficient vector, and γ is an auxiliary vector. The weighting coefficient vector ω can be set by the user or the developer as needed, and the auxiliary vector γ is used to convert the original multi-objective optimization into a single-objective optimization problem.

In this embodiment, the inner point method can be used to solve the equation (12) to obtain the optimal solution of the equation, and then the joints of the redundant mechanical arm and the optimal solution are obtained by the equations (7), (8) and (9). Corresponding angles, angular velocities, and angular angular velocities, resulting in the position and velocity of the joints of the redundant manipulator at the next moment.

Taking a redundant manipulator with nine degrees of freedom as an example (assuming that the DH coordinates of the nine-degree-of-freedom redundant manipulator are as shown in Fig. 3, the parameter table of the nine-degree-of-freedom redundant manipulator link DH is shown in Table 1) The process of solving the joint position and speed of the redundant manipulator can be:

Table 1

θ_i θ _i	d_i(mm)d _i (mm)	α_i α _i	a_i(mm)a _i (mm)	范围range
00	d₁ d ₁	90°90°	00	1550≤d₁≤82761550 ≤ d ₁ ≤ 8276
θ₂ θ ₂	00	00	a₂＝2200a ₂ =2200	-90≤θ₂≤90-90≤θ ₂ ≤90

θ₃ θ ₃	00	-90°-90°	00	-90≤θ₃≤90-90≤θ ₃ ≤90
θ₄ θ ₄	d₄＝2800d ₄ = 2800	90°90°	α₄＝-375α ₄ =-375	-180≤θ₄≤180-180≤θ ₄ ≤180
θ₅ θ ₅	00	-90°-90°	α₅＝-a₄＝375α ₅ =-a ₄ =375	-180≤θ₅≤180-180≤θ ₅ ≤180
θ₆ θ ₆	d₄＝2800d ₄ = 2800	90°90°	00	-180≤θ₆≤180-180≤θ ₆ ≤180
θ₇ θ ₇	00	-90°-90°	00	-90≤θ₇≤90-90≤θ ₇ ≤90
θ₈ θ ₈	d₈＝1650d ₈ =1650	90°90°	00	-90≤θ₈≤90-90≤θ ₈ ≤90
θ₉ θ ₉	00	00	a_h＝350a _h =350	-90≤θ₉≤90-90≤θ ₉ ≤90

First, the speed of the end of the redundant manipulator is converted to the reference of the coordinate system of the link 3:

Then, define the wrist coordinate system. Considering the simplicity of the calculation, the wrist coordinate system can be defined according to the structural characteristics of the redundant mechanical arm. The specific coordinates and direction of the wrist coordinate system can be flexibly set according to requirements, which is not limited herein. Exemplarily, the wrist coordinate system can be defined as a coordinate system parallel to the coordinate end system of the redundant manipulator and the origin coincides with the origin of the coordinate system of the connecting rod 8. At this time, when the speed of the end of the wrist and the redundant manipulator is connected When the rod 3 coordinate system is a reference, the angular velocity and the linear velocity of the two have the following relationship:

³ ω _w = ³ ω _n (15)

³ v _w = ³ v _h - ³ R _w ( ^w ω _h × ^w P _h )= ³ v _h - ³ ω _h × ³ P _h = ³ v _h - ³ ω _h × ^a _h ³ x _h (16)

Where ³ ω _w is the angular velocity in the wrist coordinate system with reference to the coordinate system of the connecting rod 3; ³ ω _h is the angular velocity in the coordinate system of the redundant manipulator referenced by the coordinate system of the connecting rod 3; ³ v _w The linear velocity in the wrist coordinate system with reference to the coordinate system of the connecting rod 3; ³ v _h is the linear velocity in the coordinate system of the redundant manipulator referenced by the coordinate system of the connecting rod 3; ³ x _h is the connection The rod 3 coordinate system is the value of the unit vector of the Z direction in the redundant robot arm end coordinate system of the reference:

In the above formula, c _i represents cos(θ _i ), s _i represents sin(θ _i ), c _ij represents cos(θ _i +θ _j ), and s _ij represents sin(θ _i +θ _j ).

Definition ³ J _w is the joint angular velocity

Speed with wrist

The Jacques matrix has:

The Jacobian matrix can be found by the vector product method as follows:

³ J _w =[J ₁ J ₂ J ₃ J ₄ J ₅ J ₆ J ₇ J ₈ J ₉ ] (19)

among them,

In the present embodiment, any three columns in the equation (19) may be taken as α, and J ^* is the remaining columns other than the three columns constituting α. Illustratively, J ₅ , J ₆ and J _{7 are taken} as α, and the remaining columns (J ₁ , J ₂ , J ₃ , J ₄ , J ₈ and J ₉ ) constitute J ^* , and can be obtained by the formula (5):

among them,

J ₂₁ = a ₂ s _{3 -} d ₆ c _{5 -} d ₄ - a ₅ s _{5 -} d ₇ c ₅ c ₇ + d ₇ s ₅ c ₆ s ₇ ,

J ₂₃ = a ₂ c ₃ + a ₄ c ₄ + a ₅ c ₄ c _{5 -} d ₆ c ₄ s _{5 -} d ₇ c ₄ s ₅ c ₇ + d ₇ s ₄ s ₆ s ₇ -d ₇ c ₄ c ₅ c ₆ s ₇ ,

J ₃₁ =-d ₄ -d ₇ (c ₅ c ₇ -s ₅ c ₆ s ₇ )-d ₆ c ₅ -a ₅ s ₅ ,

J ₃₃ = a ₄ c ₄ + d ₇ (s ₇ (s ₄ s ₆ -c ₄ c ₅ c ₆ )-c ₄ s ₅ c ₇ )+a ₅ c ₄ c ₅ -d ₆ c ₄ s ₃ ,

J ₄₁ =d ₇ (s ₇ (c ₄ s ₆ +s ₄ c ₅ c ₆ )+s ₄ s ₅ c ₇ )-a ₄ s ₄ -a ₅ s ₄ c ₅ +d ₆ s ₄ s ₅ ,

J ₄₂ =d ₇ (s ₇ (s ₄ s ₆ -c ₄ c ₅ c ₆ )-c ₄ s ₅ c ₇ )+a ₄ c ₄ +a ₅ c ₄ c ₅ -d ₆ c ₄ s ₅ ,

_{_{_{J 84 = s 7 (s 4}}} s 6 -c 4 c 5 c 6) -c 4 s 5 c 7,

_{_{_{J 85 = -s 7 (c 4}}} s 6 + s 4 c 5 c 6) -s 4 s 5 c 7,

J ₈₆ =c ₅ c ₇ -s ₅ c ₆ s ₇ ,

J ₉₄ =c ₈ (s ₄ c ₆ +c ₄ c ₅ s ₆ )-s ₈ (c ₇ (s ₄ s ₆ -c ₄ c ₅ c ₆ ))+c ₄ s ₅ s ₇ ,

J ₉₅ =s ₈ (c ₇ (s ₄ s ₆ +s ₄ c ₅ c ₆ ))-s ₄ s ₅ s ₇ -c ₈ (c ₄ c ₆ -s ₄ c ₅ s ₆ ),

J ₉₆ = s ₈ (c ₅ s _{7 -} s ₅ c ₆ c ₇ ) + c ₈ s ₅ s ₆ .

make

with

From equation (20):

among them,

Matrix

The ith element.

Available from formula (6):

Where α = (J ₅ J ₆ J ₇ ).

Therefore, in the equations (21) to (26)

(j=1, 2, 3, 4, 8 and 9)

(i=5, 6 and 7, j=1, 2, 3, 4, 8 and 9) instead, in the equations (21) to (26)

(k=1, 2, 3, 4, 5, and 6) can be obtained by replacing -J _ik (i=5,6 and 7, k=1, 2, 3, 4, 5, and 6)

with

Value. Take

As an example, the equations (21) to (26)

(j=1, 2, 3, 4, 8 and 9)

(j=1, 2, 3, 4, 8 and 9) instead, you can get:

In the formulas (21) to (26)

(k=1, 2, 3, 4, 5, and 6) Substituting -J _5k (k = 1, 2, 3, 4, 5, and 6) gives:

Known by equation (19)

So available:

among them,

Matrix

The ith element. The same can be concluded:

among them,

Joint angular velocity

A function expressed as a redundancy vector (k ₅ , k ₆ , k ₇ ):

Substituting equation (28) into equation (8), the joint angle θ can be obtained as a function of the redundancy vector (k ₅ , k ₆ , k ₇ ) as an independent variable:

Where θ ₀ is the current joint angle and Δt is the sampling step size.

Substituting equation (28) into equation (10-1) and equation (29) into equation (10-2) can obtain the equations with the smallest joint motion and the minimum influence of the mechanical arm on the corresponding gear gap:

Where H _i (θ(k ₅ , k ₆ , k ₇ )) is the element corresponding to the second row and the fourth column of the link transformation matrix ⁰ T _i (θ(k ₅ , k ₆ , k ₇ )):

⁰ T _i (θ(k ₅ , k ₆ , k ₇ ))= ⁰ T ₁ (d ₁ )· ¹ T ₂ (θ ₂ )... ^i-1 T _i (θ _i ), (i=1,..., 9) (32)

Where ^i-1 T _i (θ _i ) is the Denavit-Hartenberg matrix:

Wherein, d _i , α _i and a _i are Denavit-Hartenberg parameters, and specific values thereof are shown in Table 1.

Substituting equation (28) into equation (11-1) and substituting equation (29) into equation (11-2) can obtain joint angular velocity limits and joint angle constraints.

From formula (33):

From the equation (34), the matrix A and the vector b in the equation (12) can be further obtained as follows:

Thus, multi-objective optimization problems can be translated into:

By the formula (30), (31) and (35) can be determined redundancy vector _{_{(k 5, k 6, k}} 7) the optimum value, the redundancy vector _{_{(k 5, k 6, k}} 7) The optimal value is substituted into equations (28) and (29) to obtain the angular velocity and angle of the joints of the redundant manipulator at the next sampling point, thereby obtaining the position and velocity of each joint of the redundant manipulator.

The control method of the redundant mechanical arm provided by the second embodiment of the present invention obtains the speed of the end of the redundant mechanical arm at the target point, and the joint angle, the joint angular velocity of the redundant mechanical arm and the speed of the end of the redundant mechanical arm at the target point The joint angular acceleration is mapped to the redundancy space. According to the inverse kinematics equation, the optimal objective function and constraints corresponding to the motion trajectory of the redundant manipulator moving from the current point to the target point are determined, and the redundant space vector is used as the independent variable to establish the motion. The equation corresponding to the trajectory is solved by the auxiliary vector and the single-objective optimization algorithm to solve the equation corresponding to the trajectory function, and the position and velocity of each joint in the redundant mechanical arm corresponding to the motion trajectory are obtained. In this embodiment, by using the above technical solution, a mathematical module for multi-objective constrained optimization problem is established, which can provide a theoretical basis for solving a multi-objective optimization problem, and reduce redundant manipulators under the premise of satisfying the global optimal solution of the design target. The search space of the target optimization problem avoids the occurrence of dimensional explosion problems in the multi-objective solution process, simplifies the calculation amount required in the redundant robot arm control process, and improves the reaction speed of the redundant mechanical arm.

Embodiment 3

Embodiment 3 of the present invention provides a control device for a redundant mechanical arm. The device can be implemented by hardware and/or software, generally integrated in a control module for controlling the robot, and the control of the redundant manipulator can be realized by performing a control method of the redundant manipulator. 4 is a structural block diagram of a control device for a redundant mechanical arm according to the embodiment. As shown in FIG. 4, the device includes:

The information acquiring unit 410 is configured to acquire current point information and target point information where the redundant robot arm is located;

The trajectory unit 420 is connected to the information acquiring unit, and configured to determine, according to the current point information and the target point information, a trajectory function corresponding to a motion trajectory of the redundant mechanical arm moving from a current point to a target point;

The redundancy processing unit 430 is connected to the trajectory unit, and is configured to establish an equation corresponding to the trajectory function by using a redundancy space vector as an independent variable; and solving an equation corresponding to the trajectory function according to a target proximity method, to obtain the The position and velocity of each joint in the redundant robot arm corresponding to the motion trajectory.

The control device of the redundant mechanical arm provided by the third embodiment of the present invention obtains the current point information and the target point information of the redundant mechanical arm through the information acquiring unit, and determines the redundancy according to the acquired current point information and the target point information by the trajectory unit. The trajectory function corresponding to the motion trajectory of the robot arm moving from the current point to the target point is established by the redundancy processing unit with the redundancy space vector as the independent variable, and the equation is solved by the target approach method to obtain the motion. The position and velocity of each joint in the redundant robot arm corresponding to the trajectory. By adopting the above technical solution, the embodiment of the present invention can reduce the search space of the multi-objective optimization problem of the redundant manipulator under the premise of obtaining the global optimal solution satisfying the design goal, and avoid the occurrence of the dimensional explosion problem in the multi-objective solution process. Simplify the amount of calculations required during redundant manipulator control and increase the response speed of redundant manipulators.

Further, the information acquiring unit 410 is specifically configured to: acquire a speed of the end of the redundant mechanical arm at the target point; and the redundant machine according to a speed of the redundant mechanical arm end at the target point The joint angle of the arm, the joint angular velocity, and the joint angular acceleration are mapped to the redundancy space.

Further, the acquiring the speed of the end of the redundant mechanical arm at the current point comprises: according to the posture of the redundant mechanical arm end at the current point and the end of the redundant mechanical arm The pose of the next sample point in the trajectory determines the velocity of the end of the redundant robot arm at the next sample point.

Further, the trajectory unit 420 is specifically configured to determine an optimization objective function and a constraint condition corresponding to a motion trajectory of the redundant mechanical arm moving from a current point to a target point according to an inverse kinematics equation.

Further, the optimization objective function includes a minimum joint motion amplitude of the redundant mechanical arm or the redundant mechanical arm is minimally affected by a corresponding gear gap; the constraint condition includes: a joint of the redundant mechanical arm The speed constraint is within a preset speed range or the joint angle of the redundant robot arm is constrained within a preset angle range.

Further, the single target optimization algorithm includes: a Newton-Eulerian algorithm, a Nelder-Mead simplex algorithm, an interior point algorithm, a genetic algorithm, and/or a Pattern Search algorithm.

The control device of the redundant mechanical arm provided by this embodiment can execute the control method of the redundant mechanical arm provided by any embodiment of the present invention, and has the corresponding functional modules and beneficial effects of the control method for executing the redundant mechanical arm. For technical details not fully described in this embodiment, reference may be made to the control method of the redundant robot arm provided by any embodiment of the present invention.

Note that the above are only the preferred embodiments of the present invention and the technical principles applied thereto. Those skilled in the art will appreciate that the present invention is not limited to the specific embodiments described herein and can be made by those skilled in the art Various obvious changes, modifications, and substitutions are made without departing from the scope of the invention. Therefore, the present invention has been described in detail by the above embodiments, but the present invention is not limited to the above embodiments, and other equivalent embodiments may be included without departing from the inventive concept. The scope is determined by the scope of the appended claims.

Claims

A method for controlling a redundant mechanical arm, comprising:

Obtaining current point information and target point information where the redundant robot arm is located;

Determining, according to the current point information and the target point information, a trajectory function corresponding to a motion trajectory of the redundant robot arm moving from a current point to a target point;

Establishing an equation corresponding to the trajectory function by using a redundancy space vector as an independent variable;

The equation corresponding to the trajectory function is solved according to the target approaching method, and the position and velocity of each joint in the redundant mechanical arm corresponding to the motion trajectory are obtained.
The method for controlling a redundant robot arm according to claim 1, wherein the acquiring current point information and target point information of the redundant robot arm comprises:

Obtaining a velocity of the end of the redundant mechanical arm at the target point;

The joint angle, joint angular velocity, and joint angular acceleration of the redundant robot arm are mapped to the redundancy space according to the speed of the redundant robot arm end at the target point.
The control method of the redundant robot arm according to claim 2, wherein the obtaining the speed of the end of the redundant mechanical arm at the current point comprises:

Determining the end of the redundant manipulator according to the pose of the end of the redundant manipulator at the current point and the pose of the next sample point of the end of the redundant manipulator in the motion trajectory The speed of a sample point.
The control method of the redundant robot arm according to claim 1 or 2, wherein the determining, by the current point information and the target point information, that the redundant robot arm moves from a current point to a target point The trajectory function corresponding to the motion trajectory includes:

And determining, according to the inverse kinematics equation, an optimization objective function and a constraint condition corresponding to the motion trajectory of the redundant mechanical arm moving from the current point to the target point.
A method of controlling a redundant robot arm according to claim 4, wherein said optimizing The objective function includes a minimum degree of joint motion of the redundant mechanical arm or the redundant mechanical arm is minimally affected by a corresponding gear gap; the constraint condition includes constraining the joint speed of the redundant mechanical arm to a preset The joint angle of the redundant manipulator is constrained within a predetermined range of angles within the speed range.
The control method of the redundant robot arm according to claim 5, wherein the equation corresponding to the trajectory function is solved according to a target approach method, and each joint in the redundant mechanical arm corresponding to the motion trajectory is obtained. The location and speed include:

In the redundancy space, according to the optimization objective function, the minimum target value corresponding to the optimization target, the corresponding constraint condition and the preset weighting coefficient, the equation corresponding to the trajectory function is solved by the auxiliary vector and the single target optimization algorithm, and obtained The motion trajectory corresponds to the position and velocity of each joint in the redundant robot arm.
The control method of a redundant robot arm according to claim 6, wherein the single target optimization algorithm comprises: a Newton-Eulerian algorithm, a Nelder-Mead simplex algorithm, an interior point algorithm, a genetic algorithm, and/or a Pattern. Search algorithm.
A control device for a redundant mechanical arm, comprising:

An information acquiring unit, configured to acquire current point information and target point information where the redundant robot arm is located;

a trajectory unit, configured to be connected to the information acquiring unit, configured to determine, according to the current point information and the target point information, a trajectory function corresponding to a motion trajectory of the redundant mechanical arm moving from a current point to a target point;

a redundancy processing unit, connected to the trajectory unit, configured to establish an equation corresponding to the trajectory function by using a redundancy space vector as an independent variable; and solving an equation corresponding to the trajectory function according to a target approach method, to obtain the motion The position and velocity of each joint in the redundant robot arm corresponding to the trajectory.
A control device for a redundant robot arm according to claim 8, wherein said information The acquiring unit is specifically configured to: acquire a velocity of the end of the redundant mechanical arm at the current point; and combine an articulated angle and an articulated angular velocity of the redundant mechanical arm according to a speed of the redundant mechanical arm end at the current point And joint angular acceleration is mapped to the redundancy space.
The control device of the redundant robot arm according to claim 8 or 9, wherein the trajectory unit is specifically configured to determine the movement of the redundant mechanical arm from the current point to the target point according to an inverse kinematics equation. The optimization objective function and constraints corresponding to the trajectory.