CN113146600B

CN113146600B - Flexible robot trajectory planning method and device based on kinematics iterative learning control

Info

Publication number: CN113146600B
Application number: CN202110318327.3A
Authority: CN
Inventors: 彭键清; 朱望茹; 韩瑜
Original assignee: Sun Yat Sen University
Current assignee: Sun Yat Sen University
Priority date: 2021-03-25
Filing date: 2021-03-25
Publication date: 2022-12-20
Anticipated expiration: 2041-03-25
Also published as: CN113146600A

Abstract

The invention discloses a flexible robot track planning method and device based on kinematics iterative learning control. The method comprises the steps of performing segmental linkage kinematic modeling on the mechanical arm from the tail end to the joint and from the joint to the rope length, and further comprising inner ring passive quadratic programming control-real-time control and outer ring ILC control-track cycle control. And the passive quadratic programming control comprises the step of respectively obtaining kinematic equations of operation space tracking and rope space tracking by optimizing an objective function. The ILC control track level optimization model can facilitate the system to realize perfect tracking, and control input in an iteration process to converge to an optimal state, and the method comprises the steps of establishing the track level optimization model and searching zero space parameters. The method realizes the dual requirements of higher iteration speed and control precision.

Description

Flexible robot trajectory planning method and device based on kinematics iterative learning control

Technical Field

The invention relates to the field of rope-driven super-redundant flexible robot control, in particular to a flexible robot trajectory planning method and device based on kinematics iterative learning control.

Technical Field

The flexible robot integrates the structure of the snake-shaped robot and the drive of the continuous robot, is more compact than the traditional snake-shaped robot, and has higher positioning precision than the continuous soft robot. And the characteristics make the robot very suitable for closed space operation, in particular to the fields of minimally invasive operation, nuclear reactor pipelines, disaster debris and the like. The flexible robot can be applied to detection in narrow spaces such as nuclear power stations and the like, has high movement precision and load capacity, can flexibly move in narrow and complex environments, and has wide application prospects in the field of autonomous charging of electric vehicles.

In order to meet the requirement of flexible control of the tail end and the arm type, the active and passive hybrid drive segmented linkage type flexible arm becomes the best choice. The configuration adopts an active-passive hybrid driving form of 'discrete rigid connecting rod + linkage mechanism + rope', the degrees of freedom in the same direction in each arm section are coupled together, and the motion control of each section is carried out through the driving rope on the outer edge. The rigidity of the mechanical arm with the configuration can be greatly improved. In addition, the included angles between each adjacent facet joint of the articular segment are strictly equal, and equal curvature bending of the arm segment can be realized.

Due to the characteristics of multiple degrees of freedom, system nonlinearity, strong dynamic coupling and the like of the rope-driven super-redundant robot, the track planning and control of the super-redundant robot system are quite complex. In order to improve the control accuracy of the tail end of the flexible robot, the invention provides a trajectory following planning method based on kinematics iterative learning control, which has higher iterative speed and control accuracy.

Disclosure of Invention

In view of the defects of the prior art, the invention aims to provide a flexible robot trajectory planning method based on kinematics iterative learning control, and aims to improve the motion control precision and the iteration speed of a mechanical arm capable of realizing an arm section equal-curvature bending configuration.

In order to achieve the above purpose, the invention adopts the following technical scheme,

a flexible robot track planning method and a device based on kinematics iterative learning control are provided, the method comprises the following steps:

determining model parameters of the flexible robot;

modeling the flexible mechanical arm in a segmented linkage manner;

the method comprises the steps of analyzing each section of active and passive hybrid drive segmented linkage flexible mechanical arm, and establishing an initial coordinate system on a joint of the mechanical arm.

It should be noted that, when the starting coordinate system is established, a positive kinematic equation of the flexible robot is obtained:

it should be noted that the rope connection point a between the coordinate systems is obtained by a homogeneous transformation matrix between the coordinate systems _i-1 、B _i And B _i-1 Then a mapping between the drive space and the joint space of the individual joints can be obtained.

It should be noted that the method also includes obtaining the change of the length of the rope by differentiating the mapping relationship between the driving space and the joint space, and then further obtaining the change of the joint angle, and finally obtaining the Jacobian matrix from the joint space to the terminal cartesian space of the whole flexible robot.

It should be noted that the method also includes the step of deriving the end velocity of each segment according to the relationship between the number of joints in each segment and the nominal degree of freedom, so as to obtain the Jacobian matrix of the whole flexible robot and further obtain the generalized velocity of the end effector of the flexible robot.

It should be noted that an iterative learning control method is also included, which is an inner loop passive quadratic programming control-real time control (real-time control) and an outer loop ILC control-trajectory level control (trajectory control).

It should be noted that, according to the kinematic redundancy of the end operating space-joint space, the operating space error equation of motion can be obtained by optimizing the objective function.

It should be noted that, according to the kinematic redundancy between joint space and rope space, by optimizing the objective function, the rope space error equation of motion can be obtained.

It should be noted that, according to the actually obtained error library of the required rope length and the actual length, a track-level objective function optimization model can be established.

It should be noted that the method further includes searching the joint-operation space Jacobian matrix according to a series of designed zero space search parameters to obtain an extended tracking function of passive quadratic programming.

The invention has the beneficial effects that:

1. the structure adopts an active-passive hybrid driving mode of 'discrete rigid connecting rod + linkage mechanism + rope', the degrees of freedom in the same direction in each arm section are coupled together, and the motion control of each section is carried out through the driving rope on the outer edge. The rigidity of the mechanical arm with the configuration can be greatly improved. In addition, the included angles between each adjacent facet joint of the articular segment are strictly equal, and equal curvature bending of the arm segment can be realized.

2. The proposed iterative learning based on kinematics is divided into two layers of control, the periodic control of the track is realized through the control of the outer ring ILC, the optimized parameters are transmitted to the inner ring passive quadratic programming control, and the real-time control is realized. The combination of internal and external cyclic control not only improves the accuracy of track tracking, but also considers the reduction of execution space error and operation space tracking error, and simultaneously solves the motion redundancy and driving redundancy.

Drawings

FIG. 1 is a coordinate system relationship diagram of the j-th joint of the mth segment of the flexible robot of the present invention;

FIG. 2 is a D-H coordinate system diagram of a flexible robot of the mth segment of the present invention;

FIG. 3 is a schematic diagram of a flexible robot parameter description of the present invention;

fig. 4 is a kinematic-based ILC-passive quadratic programming framework proposed by the present invention.

DETAILED DESCRIPTION OF EMBODIMENT (S) OF INVENTION

The present invention will be further described with reference to the accompanying drawings, and it should be noted that the present embodiment is based on the technical scheme, and a detailed implementation manner and a specific operation process are provided, but the protection scope of the present invention is not limited to the present embodiment.

The invention relates to a flexible robot trajectory planning method and a device based on kinematics iterative learning control, wherein the method comprises the following steps:

determining model parameters of the flexible robot;

modeling the flexible mechanical arm in a segmented linkage manner;

It should be noted that, when the initial coordinate system is established, a positive kinematic equation of the flexible robot is obtained:

it should be noted that the rope connection point a between the coordinate systems is obtained by a homogeneous transformation matrix between the coordinate systems _i-1 、B _i And B _i-1 Then a mapping relationship between the drive space and the joint space of the individual joint can be obtained。

The method further comprises the step of deducing the terminal speed of each segment according to the relation between the number of joints in each segment and the nominal degree of freedom, so that a Jacobian matrix of the whole flexible robot can be obtained, and further the generalized speed of the flexible robot terminal actuator can be obtained.

It should be noted that, according to the kinematic redundancy of joint space-rope space, the rope space error equation of motion can be obtained by optimizing the objective function.

It should be noted that, according to the actually obtained error library of the required rope length and the actual length, a trajectory-level objective function optimization model can be established.

The first embodiment is as follows:

the embodiment of the invention provides a modeling method based on a flexible robot, and in the embodiment, an articulated flexible arm is taken as an example.

S1: determining model parameters of the flexible robot.

Suppose that the segmented linkage type mechanical arm studied by people has n segments, each segment has p small joints, the coordinate relation of the jth single joint of the mth segment is shown as figure 1, and let i =2[ p ] (mm-1)+j]-1, plane

Plane surface

Respectively representing the wiring disk k and the wiring disk k +1, line segments

Respectively showing the distance of three driving ropes between the two wiring disc wire through holes, O _2p(m-1) Representing the center of the gimbal.

The flexible mechanical arm is composed of np/2 linkage joint sections, and the rotation angles of all joints in the same linkage joint section are the same. Within each joint segment there are four orthogonal sub-joints and the overall flexible arm coordinate system is shown in figure 2. X _2pm+1 Y _2pm+1 Z _2pm+1 The coordinate system is the initial coordinate system established on the joint by adjacent m +1 arm segments, and the D-H parameter table of the whole flexible arm is shown in Table 1.

The flexible robot consists of np joint sections, each linkage joint section is internally provided with 2 orthogonal sub-joints, the forward kinematic equation of the flexible robot is as follows:

s2: and modeling the segmented linkage type flexible mechanical arm.

And analyzing each section of active and passive hybrid drive segmented linkage flexible mechanical arm, and establishing an initial coordinate system on a joint of the mechanical arm to obtain a positive kinematic equation of the flexible robot.

S21: tip-to-joint modeling is performed.

The rope distribution between each gimbal is as shown in fig. 3, and the driving rope passing through the m-th section must also pass through the (m-1) -th section. The homogeneous transformation matrix between the # i-1 and # i coordinate systems can be expressed as:

wherein, the first and the second end of the pipe are connected with each other,

ψ _m representing the angle between the string hole where the starting string is located in the mth segment and the centerline of the reel and the X-axis.

Thus, the rope connection point A in the coordinate systems # i-1 and # i ^i-1 And B ^i-1 Is:

further, B in the # i-1 coordinate system can be obtained by ⁱ 3-D coordinates of points:

then, the mapping relationship between the drive space and the joint space of a single joint can be expressed as:

by differentiating (5), we can obtain the change in cord length, i.e.:

wherein the content of the first and second substances,

further, the change in joint angle can be expressed as:

thus, the JACOBIAN matrix from the joint space to the rope space of the entire flexible robot can be represented as:

s22: modeling of joint to cord length is performed.

The joint velocity of the entire flexible robot can be expressed as:

suppose xi _m,i Is the rotation axis of the ith joint of the mth segment, and P _m，i Is the location vector of the ith joint of the mth segment, i.e., the Jacobian matrix of the mth segment can be expressed as:

^Ref J _m ＝[ ^Ref J _m,1 ^Ref J _m,2 … ^Ref J _m,2p ]

(10)

i＝1,…,2p, ^Ref P _i ＝ ^Ref T _i (1:3,4), ^Ref z _i ＝ ^Ref T _i (1:3,3), ⁿ P _n ＝[0 0 0] ^T , ⁰ T ₀ (1:3,3)＝[0 0 1] ^T .

since the number of joints in the mth segment is p and the nominal degree of freedom is 2p, the end velocity of the mth segment can be expressed as:

assuming {0} is an inertial system, the JACOBIAN matrix for the entire flexible robot can be expressed as:

J _q (Θ)＝[ ⁰ J ₁ ⁰ J ₂ … ⁰ J _n ]∈R ^6×2n (12)

wherein the content of the first and second substances, ⁰ J _i is the transmission ratio of the speed of the ith joint to the speed of the end effector.

Thus, the generalized velocity of a flexible robotic end effector may be expressed as:

wherein v is _e ＝[v _ex ,v _ey ,v _ez ] ^T ∈R ³ ,ω _e ＝[ω _ex ,ω _ey ,ω _ez ] ^T ∈R ³ Respectively, the linear and angular velocities of the end effector.

S3: and performing iterative learning control based on kinematics, namely ILC-passive quadratic programming control.

As shown in fig. 4, the proposed kinematics-based ILC runs in two time dimensions, and the whole iterative learning control framework is divided into two categories:

inner loop passive quadratic programming control-real time control for tracking the required trajectory in real time, running at each instant time point (faster time period); (ii) a

Outer loop ILC control-track cycle control, which runs on every repeated track, improves track following performance (slower time period) by iterating as the tracks are executed repeatedly.

The inner-loop real-time reactive power control component utilizes the proposed passive quadratic programming controller to execute a trajectory tracking task, and the controller simultaneously solves motion redundancy and driving redundancy.

The outer loop track level control component executes an optimized parameter searching task from the last iteration process to the next iteration process, so that the track tracking accuracy is improved.

Kinematic redundancy is extracted from multiple coupling relations of rope-joint-tail end, and a parameter exploration method of constrained space is provided. Through exploration and optimization of the ILC, different joint angles can generate the same operation space movement, so that the performance of the passive quadratic programming controller is improved in the process of repeating the same movement. After each iteration is completed, the task performance is quantified by an objective function P (u) that takes into account the norm of the execution space error and the operating space tracking error.

Thus, by using the ILC framework, the trajectory optimization problem can be solved using the parameter u as an optimization variable. And according to the inner loop ILC optimization framework, u is updated after each iteration and is fed back to the outer loop passive quadratic programming controller. The trajectory optimization method takes full advantage of the ILC characteristics in resolving kinematic redundancies and predicts that these changes will improve the accuracy of P (u).

S31: and carrying out passive quadratic programming control on the external circulation.

The quadratic programming problem for the entire system can be described as:

wherein the content of the first and second substances,

is the tracking error in the working space and,

is the tracking error in drive space and is a constant coefficient.

S311: the operating space is tracked.

According to the kinematic redundancy of the end operating space-joint space, the tracking function can be described as minimizing the difference from the angular velocity of the reference tracking point, i.e. the objective function is:

here, K _p Is a positive definite diagonal matrix, J _q Is the Jacobian matrix of tip to joint,

is the inverse of a weighted Jacobian matrix, the weighted matrix being W _q 。

The kinematic equation of the operation space error obtained from (15) is:

here, e _X ＝X-X _d Is the operating space error.

S312: the rope space is tracked.

Tracking functions based on kinematic redundancy of joint space-rope space

Can be described as minimizing

The difference value between the reference tracking point rope length change value and the reference tracking point rope length change value, namely the target function is as follows:

wherein the content of the first and second substances,

here, K _p Is a positive definite diagonal matrix, J _L Is a jacobian matrix of joints to ropes,

is the inverse of a weighted Jacobian matrix, the weighted matrix being W _L 。

The kinematic equation of the operation space error obtained from (15) is:

here, e _L ＝L-L _d Is the operating space error.

S32: ILC control is performed on the inner loop.

Research on ILC has been conducted for a long time, provided that the initial conditions given for each iteration are the same as the expected trajectory for each iteration. The ILC improves task performance in the next iteration by information in a particular iteration. The ILC updates the control inputs over a limited time interval using the designed learning rules so that the system can achieve perfect tracking and the control inputs converge to an optimal state in an iterative process.

For flexible robot applications, the control input is typically a rope tension. Due to the constraint of rope tension and drive redundancy, the conventional ILC cannot be used to directly address the rope tension required for the flexible robot. However, numerical gradient-based and optimization-based methods have difficulty achieving real-time control of the high-dimensional system.

To solve this problem, the ILC is combined with a passive quadratic programming controller and iterative learning is performed at the trajectory level to obtain a set of spatial parameters to achieve an update of the passive quadratic programming controller in real time, instead of iteratively learning the rope tension in real time.

The outer ILC model can be equivalent to an unconstrained nonlinear optimization problem, that is, the optimization result of the null space is as follows:

here, P (u) is a trajectory error, and each trajectory equation (23) is run only once.

S321: and establishing a track-level optimization model.

Considering the tracking error of the end operation space and the rope tracking error, a track level objective function can be established as follows:

P(u)＝|E _t (u)| _F ·|E _L (u)| _F (24)

wherein |. Non chlorine _F The number of the norm is represented,

in order to handle the spatial tracking error,

is the end pose error at the ith moment,

in order to drive the spatial tracking error,

is the rope length error at the ith time.

e is caused by rope deformation, static friction force, rope hole position installation errors, rope initial position calibration errors and the like, and can be obtained through an error model library obtained through multiple experimental measurements. For a given desired trajectory, the actual trajectory can be measured by a 3D high precision external sensor, and the rope length can be obtained indirectly by a motor. The required rope length is compared with the actual rope length at any time to obtain a rope length error library.

S322: a null space search is performed.

In order to obtain the tracking effect of the joint space under the best ILC optimization, a series of zero-space search parameters are designed to search for the Jacobian J of the joint-operation space _q This search strategy is in fact a tracking function (15) that extends passive quadratic programming,

independent of u, the objective function is:

wherein the content of the first and second substances,

here, the difference from the conventional method of optimizing an objective function in real time is that

Was obtained by ILC.

The control accuracy can be improved by searching for kinematic redundancy, so that:

here, D (u) is a diagonal matrix.

S33: and (5) passive quadratic programming optimization.

Although the kinematic redundancy decomposition offers a great possibility for task performance optimization, the relationship between these parameters of the null-space optimization and performance optimization functions is complex, which makes it very difficult to adjust these parameters manually. As can be seen from (22) and (23), the ILC can solve the unconstrained nonlinear optimization problem through various optimization methods, and can solve the optimal set of spatial parameters through Particle Swarm Optimization (PSO) search.

The ILC framework is shown using PSO search as its optimization method and also showing the nested call relationship between the ILC framework and the passive quadratic programming controller. In addition, the framework has universality, and other optimization methods can be easily realized by changing u.

Various modifications may be made by those skilled in the art based on the above teachings and concepts, and all such modifications are intended to be included within the scope of the present invention as defined in the appended claims.

Claims

1. The flexible robot trajectory planning method based on kinematics iterative learning control is characterized by comprising the following steps:

determining model parameters of the flexible robot;

modeling the flexible robot in a segmented linkage manner;

analyzing each section of active and passive hybrid drive segmented linkage flexible robot, and establishing an initial coordinate system on a joint of the flexible robot; the method comprises inner loop passive quadratic programming control-real-time control and outer loop ILC control-track level control; obtaining an operation space error motion equation by optimizing an objective function according to the kinematic redundancy of the terminal operation space and the joint space; searching a joint-operation space Jacobian matrix according to a series of designed zero space search parameters to obtain an extended tracking function of passive quadratic programming; wherein, according to the kinematic redundancy of the terminal operating space-joint space, the tracking function can be described as minimizing the difference from the angular velocity of the reference tracking point, i.e. the objective function is:

wherein the content of the first and second substances,

is the inverse of the weighted Jacobian matrix, the articular Jacobian weighting matrix is W _q 。

2. The flexible robot trajectory planning method based on kinematics iterative learning control according to claim 1, wherein after the initial coordinate system is established, a positive kinematics equation of the flexible robot is obtained:

wherein n is the number of the segments of the flexible robot mechanical arm, p is the number of the small segments of each segment of the mechanical arm, and j is the number of the single joints.

3. The flexible robot trajectory planning method based on kinematics iterative learning control according to claim 2, wherein a rope connection point A between coordinate systems is obtained through a homogeneous transformation matrix between coordinate systems _i-1 、B _i And B _i-1 Then a mapping between the drive space and the joint space of the individual joints is obtained.

4. The flexible robot trajectory planning method based on kinematics iterative learning control according to claim 3, further comprising differentiating the mapping relationship between the driving space and the joint space to obtain the change of the rope length, and then obtaining the change of the joint angle, and finally obtaining a Jacobian matrix from the joint space of the whole flexible robot to the terminal Cartesian space.

5. The method for planning the trajectory of the flexible robot based on the iterative learning control of kinematics as recited in claim 4, further comprising deriving the terminal velocity of each segment according to the relationship between the number of joints in the segment and the nominal degree of freedom, and obtaining a Jacobian matrix of the whole flexible robot to obtain the generalized velocity of the end effector of the flexible robot.

6. The flexible robot trajectory planning method based on kinematics iterative learning control according to claim 1, further comprising obtaining a rope space error motion equation by optimizing an objective function according to the kinematics redundancy of joint space-rope space; wherein the function is tracked according to the kinematic redundancy of joint space rope space

Can be described as minimizing

The difference value between the reference tracking point rope length change value and the reference tracking point rope length change value, namely the objective function, is as follows:

wherein the content of the first and second substances,

here, K _p Is a positive definite diagonal matrix, J _L Is the jacobian matrix of the joints to the ropes,

is the inverse of the weighted Jacobian matrix, and the rope length Jacobian weighted matrix is W _L 。

7. The flexible robot trajectory planning method based on kinematics iterative learning control according to claim 1, wherein a trajectory-level objective function optimization model is established according to a practically obtained error library of required rope length and actual length.