CN110561419A - arm-shaped line constraint flexible robot track planning method and device - Google Patents

Abstract

The invention discloses a trajectory planning method and device for a flexible robot constrained by an arm profile. It relates to the field of robot control, wherein, the method obtains the relative deviation data of the flexible robot, and judges whether the end point reaches the target position point in the target area according to the obtained relative deviation data and the threshold judgment condition. When the threshold judgment condition is met, the end point is considered to have arrived. The target position point, otherwise, according to the relative deviation data, obtain the velocity data of the next moment of the flexible robot and calculate the expected angular velocity of the joints of the flexible robot, and obtain the joint control amount at the next moment according to the expected angular velocity of the joints, to drive the joints of the flexible robot to move to target location point. Realize the trajectory planning of the end point, and combine the pose characteristics of the slit inner arm segment and the slit outer arm segment to realize that the part of the flexible robot entering the slit does not collide with the slit wall and the part outside the slit realizes the obstacle avoidance function , which improves the trajectory planning efficiency of the arm-shaped line-constrained flexible robot and takes into account the control accuracy.

Description

Trajectory planning method and device for flexible robot constrained by arm shape line

technical field

The invention relates to the field of robot control, in particular to a trajectory planning method and device for a flexible robot constrained by an arm shape line.

Background technique

Nowadays, there are higher and higher requirements for the environmental adaptability of intelligent robots and the ability to overcome environmental restrictions. Due to the small working space and insufficient mobility of traditional industrial robots, especially the weak adaptability to some unstructured environments, In addition, in the face of the limitation of degrees of freedom and the difficulty of passing through various obstacles in a narrow environment with a rigid arm, there are more and more related researches on rope-driven ultra-redundant robots that can meet the requirements of operations in narrow spaces. Yu robot has the advantages of many degrees of freedom, small arm, strong movement flexibility, and good environmental adaptability. It can meet many tasks of traversing narrow cylindrical spaces, such as maintenance of cooling pipelines in nuclear power plants, maintenance of oil and gas pipelines, and inspection of nuclear reactor pipelines. Extraordinary work area.

However, such narrow spaces are usually cylindrical. To perform such narrow space tasks, the soft robot needs to be able to reach the target position without contacting the inner wall of the confined space (that is, the flexible arm does not collide with environmental obstacles) . Traditional research only considers the end of the flexible robot to reach the target position, or simply avoids environmental obstacles through gradient projection, without considering the relationship between the small cylindrical space and the arm shape. Moreover, the traditional optimization method to solve such problems has low calculation efficiency or the optimization equation and optimization index are too complex, which is not conducive to the real-time motion control of flexible robots. Therefore, it is necessary to propose a trajectory planning method for an arm-shaped line-constrained flexible robot that can improve the efficiency of trajectory planning through cylindrical slits while taking into account the control accuracy.

Contents of the invention

The present invention aims to solve one of the technical problems in the related art at least to a certain extent. Therefore, the object of the present invention is to provide a trajectory planning method for an arm-shaped line-constrained flexible robot that can improve the efficiency of trajectory planning through a cylindrical slit while taking into account the control accuracy.

The technical scheme adopted in the present invention is:

In the first aspect, the present invention provides a trajectory planning method for a flexible robot constrained by an arm profile, which is suitable for a flexible robot constrained by an arm profile. The target area is a cylindrical slit, and the flexible robot enters the part of the cylindrical slit is the inner arm section of the slit, and correspondingly, the part outside the cylindrical slit is the outer arm section of the slit, including:

Construct a space mapping model and obtain the relative deviation data of the soft robot, the relative deviation data includes: the relative pose deviation between the end point and the expected point of the soft robot, the arm shape line vector of the arm segment in the slit and the expected arm shape The position deviation of the line vector, the minimum distance deviation between the outer arm section of the slit and the obstacle;

According to the relative deviation data and the threshold judgment condition, it is judged whether the end point reaches the target position point of the target area;

When the threshold judgment condition is met, it is considered that the end point reaches the target position point;

Otherwise, acquire the velocity data of the flexible robot at the next moment according to the relative deviation data, calculate the expected angular velocity of the joints of the flexible robot according to the velocity data, and acquire the joint control amount at the next moment according to the expected angular velocity of the joints , to drive each joint of the flexible robot to move to the target position point;

The speed data at the next moment includes: the linear velocity and angular velocity of the end point, the arm shape linear velocity of the arm section inside the slit, and the obstacle avoidance instantaneous velocity of the arm section outside the slit for obstacle avoidance.

Further, the threshold judgment conditions include: the end point of the flexible robot arrives in the target area and the relative pose deviation is within a first preset threshold range, the position deviation is less than a position deviation threshold, the The minimum distance deviation between the outer arm segment of the slit and the obstacle is smaller than the obstacle avoidance distance threshold.

Further, the specific formula for calculating the expected angular velocity of the flexible robot joint is:

in, Indicates the expected angular velocity of the joints of the flexible robot, represents the pseudoinverse of the generalized extended Jacobian matrix between the Jacobian matrix of the arm type constraint and the extended Jacobian matrix of the obstacle avoidance constraint, represents the generalized velocity of the end point in the end coordinate system, represents the linear velocity of the arm type, Indicates the instantaneous speed of the obstacle avoidance.

Further, the formula for calculating the joint control amount at the next moment is specifically:

Among them, Θ _d (t) represents the desired joint angle at time t, Indicates the expected angular velocity of the joint at time t.

Further, the position deviation is derived from the joint angle to obtain the Jacobian matrix constrained by the arm shape line.

Further, the minimum distance deviation between the outer arm segment of the slit and the obstacle is derived from the joint angle to obtain the extended Jacobian matrix of the obstacle avoidance constraint.

Further, the arm shape of the arm segment in the slit is a straight line, and the line vector direction of the straight line is parallel to the axis of the slit.

In the second aspect, the present invention also provides a trajectory planning device for a flexible robot constrained by an arm shape line, including:

Obtaining deviation data module: used to construct a space mapping model and obtain the relative deviation data of the flexible robot, the relative deviation data includes: the relative pose deviation between the end point and the expected point of the flexible robot, the arm of the arm segment in the slit The position deviation between the model line vector and the expected arm model line vector, and the minimum distance deviation between the outer arm segment of the slit and the obstacle;

Threshold judgment module: for judging whether the end point reaches the target position point of the target area according to the relative deviation data and the threshold judgment condition;

Threshold judgment result execution module: used to consider that the end point has reached the target position point when the threshold judgment condition is satisfied, otherwise, obtain the speed data of the next moment of the flexible robot according to the relative deviation data, and according to the The velocity data calculates the expected angular velocity of the joints of the flexible robot, and obtains the joint control amount at the next moment according to the expected angular velocity of the joints to drive the joints of the flexible robot to reach the target position point, and the velocity data of the next moment Including: the linear velocity and angular velocity of the end point, the arm shape linear velocity of the arm section inside the slit, and the obstacle avoidance instantaneous velocity of the arm section outside the slit.

In a third aspect, the present invention provides a trajectory planning device for a flexible robot constrained by an arm shape line, including:

at least one processor, and a memory communicatively coupled to the at least one processor;

Wherein, the processor is used to execute the method according to any one of the first aspect by invoking the computer program stored in the memory.

In a fourth aspect, the present invention provides a computer-readable storage medium, the computer-readable storage medium stores computer-executable instructions, and the computer-executable instructions are used to make a computer execute the method described in any one of the first aspect. method.

The beneficial effects of the present invention are:

The present invention constructs a space mapping model and obtains the relative deviation data of the flexible robot, and judges whether the end point has reached the target position point in the target area according to the obtained relative deviation data and the threshold judgment condition. When the threshold judgment condition is met, the end point is considered to have reached the target Otherwise, according to the relative deviation data, obtain the velocity data of the flexible robot's next moment speed and calculate the expected angular velocity of the joints of the flexible robot, and obtain the joint control amount at the next moment according to the expected angular velocity of the joints, to drive the joints of the flexible robot to move to the target location point. The present invention performs unified modeling according to the mapping relationship between the cylindrical slit and the joint space of the flexible robot to the Cartesian space of the end point, realizes the trajectory planning of the end point, and combines the pose characteristics of the slit inner arm segment and the slit outer arm segment , to realize that the part of the flexible robot entering the slit does not collide with the slit wall and the part outside the slit realizes the obstacle avoidance function. Compared with the prior art, the trajectory planning efficiency of the arm-shaped line-constrained flexible robot is improved while taking into account the control accuracy.

The invention can be widely used in the field of trajectory planning of flexible robots constrained by arm type lines.

Description of drawings

Fig. 1 is the implementation flow chart of a specific embodiment of the trajectory planning method of the flexible robot with arm profile constraints in the present invention;

Fig. 2 is a schematic diagram of the joint coordinate system of the flexible robot in a specific embodiment of the trajectory planning method for the flexible robot constrained by the arm shape line of the present invention;

Fig. 3 is a distribution diagram of the D-H coordinate system of an arm segment of the flexible robot according to a specific embodiment of the trajectory planning method of the flexible robot constrained by the arm shape line in the present invention;

Fig. 4 is a planning schematic diagram of a specific embodiment of the trajectory planning method of the flexible robot constrained by the middle arm shape line of the present invention;

Fig. 5 is a structural block diagram of a specific embodiment of a trajectory planning device for a flexible robot constrained by a line of the middle arm of the present invention.

Detailed ways

In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the specific implementation manners of the present invention will be described below with reference to the accompanying drawings. Obviously, the accompanying drawings in the following description are only some embodiments of the present invention, and those skilled in the art can obtain other accompanying drawings based on these drawings and obtain other implementations.

Unless otherwise defined, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the technical field of the invention. The terms used herein in the description of the present invention are for the purpose of describing specific embodiments only, and are not intended to limit the present invention.

Embodiment one:

Embodiment 1 of the present invention provides a trajectory planning method for an arm-shaped line-constrained flexible robot. This embodiment is applicable to an arm-shaped line-constrained flexible robot (that is, a joint-shaped line-constrained flexible robot. ), the target area of the operation is the cylindrical slit, and the part of the soft robot entering the cylindrical slit is defined as the inner arm segment of the slit, and the part outside the cylindrical slit is the outer arm segment of the slit.

Fig. 1 is the implementation flowchart of the trajectory planning method for the arm-shaped line constrained flexible robot provided by the embodiment of the present invention. As shown in Fig. 1, the method includes the following steps:

S1: Construct a spatial mapping model and obtain relative deviation data of the soft robot.

In this embodiment, the spatial mapping model can optionally be unified modeling of the cylindrical slit of the operation target area, the joint space of the flexible robot, and the Cartesian spatial mapping relationship of the end point according to the hand-eye vision system, combined with the end point pose Constraints, arm shape line constraints inside the slit, and obstacle avoidance constraints outside the slit, jointly establish a Jacobian matrix integrating "end point pose-arm shape line-obstacle avoidance".

In this embodiment, the relative deviation data includes: the relative pose deviation between the end point of the flexible robot and the expected point (i.e. the target position point), the position deviation between the arm shape vector of the arm segment in the slit and the expected arm shape vector (record is ΔP _m,i ), the minimum distance deviation between the outer arm segment of the slit and the obstacle (denoted as ΔD _o ). Among them, relative pose deviation includes: relative position deviation (denoted as ΔP _e ) and relative attitude deviation (denoted as ).

S2: According to the relative deviation data and the threshold judgment condition, judge whether the end point reaches the target position point in the target area.

S21: When the threshold judgment condition is met, it is considered that the end point has reached the target position point, and the trajectory planning task ends.

S22: Otherwise, continue trajectory planning, obtain the speed data of the flexible robot at the next moment according to the relative deviation data, calculate the joint expected angular velocity of the flexible robot according to the speed data, and obtain the joint control amount at the next moment according to the joint expected angular velocity to drive Each joint of the flexible robot moves, and the process is repeated until it reaches the target position.

In this embodiment, the speed data at the next moment includes: the linear velocity and angular velocity of the end point, the arm shape linear velocity of the arm segment inside the slit, and the obstacle avoidance instantaneous speed of the arm segment outside the slit for obstacle avoidance.

In step S22, the specific process of trajectory planning is as follows.

S221 : Deriving the position deviation between the arm shape line vector of the arm segment in the slit and the expected arm shape line vector with respect to the joint angle to obtain a Jacobian matrix of the arm shape line constraint, denoted as J _Line .

S222: The minimum distance deviation ΔD _o between the outer arm segment of the slit and the obstacle is derived from the joint angle to obtain the extended Jacobian matrix of the obstacle avoidance constraint, denoted as J _d .

S223: According to the Jacobian matrix J _Line constrained by the arm shape line of the above steps and the extended Jacobian matrix J _d of the obstacle avoidance constraint, the generalized extended Jacobian matrix is obtained, denoted as By solving the inverse kinematics of the generalized extended Jacobian matrix, the expected angular velocity of the joint of the flexible robot is calculated, denoted as The calculation formula is expressed as:

In the above formula (1), Indicates the expected angular velocity of the joints of the flexible robot, Represents the pseudoinverse of the generalized extended Jacobian matrix between the Jacobian matrix J _Line of the arm type line constraint and the extended Jacobian matrix J _d of the obstacle avoidance constraint, represents the generalized velocity of the end point in the end coordinate system, Indicates the linear velocity of the arm type, Indicates the instantaneous speed of obstacle avoidance.

S224: Integrate it according to the expected joint angular velocity to obtain the joint control amount at the next moment, and drive each joint of the flexible robot to move until the threshold judgment condition is met, and the running time t is within the preset maximum running time t _f , and then judge to reach the target position to complete the trajectory planning.

In this embodiment, the arm profile constraints and obstacle avoidance constraints are combined to form a new generalized extended Jacobian matrix with arm profile and obstacle avoidance functions, and then the joint angular velocity of the flexible robot can be obtained by the decomposition velocity method. The value of the joint angular velocity is integrated to obtain the joint angle of the flexible robot, and then the end point of the flexible robot is controlled to move quickly to the target position, so as to achieve the purpose of effectively passing through the narrow cylindrical confined space, which can improve the accuracy of trajectory control and improve the efficiency of trajectory control.

The calculation process of this embodiment will be described in detail below.

As shown in Figure 2, it is a schematic diagram of the joint coordinate system of the flexible robot in this embodiment. It can be seen from the figure that it is a schematic diagram of the joint coordinate system of the k-th sub-joint of the m-th segment of the flexible robot. It is assumed that the wiring disc 1 of the k-th sub-joint is (Circle center O _2p(m-1)+k ) The wire-passing hole of the driving rope is expressed as: A _2p(m-1)+k,1 , A _2p(m-1)+k,2 , A _{2p(m -1)+k,3} , the coordinate system established with the center O _2p(m-1)+k is expressed as: X _2p(m-1)+k Y _2p(m-1)+k Z _{2p(m-1 )+k} , the wiring disc 2 of the k+1th joint (circle center O _2p(m-1)+k+1 ) is expressed as: B _{2p(m-1)+k+1, 1.} B _{2p(m-1)+k+1,2} and B _{2p(m-1)+k+1,3} , the coordinate system established with the center O _2p(m-1)+k+1 is expressed as: X _2p(m-1)+k+1 Y _2p(m-1)+k+1 Z _2p(m-1)+k+1 , line segment A _2p(m-1)+k,1 B _{2p(m -1)+k+1,1} , A _2p(m-1)+k,2 B _{2p(m-1)+k+1,2} and A _2p(m-1)+k,3 B _{2p(m -1)+k+1, 3} respectively represent the distances between the three driving ropes in the two wiring disc holes, O _2p(m-1) represents the joint center of the kth sub-joint, the point O _2p( The coordinate system established by _m-1) is expressed as: X _2p(m-1) Y _2p(m-1) Z _2p(m-1) .

As shown in FIG. 3 , it is a distribution diagram of the DH coordinate system of one arm segment of the flexible robot in this embodiment. In this embodiment, it is assumed that the flexible robot is composed of n linked arm segments, and the rotation angle of each joint in the same arm segment is the same, assuming that there are four orthogonal sub-joints in each arm segment, and the coordinate system X _2pm+1 Y _2pm+1 Z _2pm+1 is the initial coordinate system established on this joint by the adjacent m+1 arm segment. According to the kinematic recursive relationship, the positive kinematic equation of the mth arm segment is expressed as:

In the above formula (2), (θ _2m-1 ,θ _2m ) represents the two joint angles of the m-th arm segment, represents the homogeneous transformation matrix of the m-th arm segment, represent the direction vectors of the homogeneous transformation matrix on the x, y, and z axes respectively, represents the position vector of this homogeneous transformation matrix.

From this, the positive kinematic equation of the whole flexible arm is expressed as:

In the above formula (3), R _{Euler_ZYX} represents the rotation matrix of T _e matrix ZYX Euler angle mode, P _e represents the position vector of T _e matrix, Represents the homogeneous transformation matrix of the i-th segment, (θ _2i-1 ,θ _2i ) represents the two joint angles of the i-th arm segment.

The joint angular velocity of the whole flexible arm can be expressed as:

In the above formula (4), Indicates the joint angular velocity of the i-th arm segment.

The generalized motion velocity at the end of the flexible arm can be expressed as:

In the above formula (5), v _e ＝[v _ex , _{ve ey} , _{ve ez} ] ^T ∈R ³ , ω _e ＝[ω _ex ,ω _ey ,ω _ez ] ^T ∈R ³ represent the lines of the end points of the flexible arm respectively velocity and angular velocity, Indicates the position differential and attitude differential of the end point.

Differentiate the above (5) to obtain the velocity-level positive kinematics equation of the flexible arm, which is expressed as:

In the above formula (6), J _g (Θ)=[J ₁ J ₂ … J _n ]∈R ^6×2n represents the regular Jacobian matrix of the flexible arm, which is a function of the joint angle, and the joint angular velocity and The relationship between the motion speeds of the end points, J _i is the transmission ratio of the motion speed of the joint i to the motion speed of the end point of the flexible arm, and J _i is a 6x1 column vector.

Suppose ξ _m,i is the rotation axis of the i-th sub-joint of the m-th arm segment, and P _m,i is the position vector of the i-th sub-joint of the m-th arm segment, then the Jacobian matrix of the m-th arm segment can be expressed as:

^Ref J _m ＝[ ^Ref J _m,1 ^Ref J _m,2 ... ^Ref J _m,2p ] (7)

in, The subscript in means from i to n, and Ref includes {0} series and {n} series.

1) When Ref is {0} series:

2) When Ref is {n} series:

and:

^Ref P _i = ^Ref T _i (1:3,4) (10)

^Ref z _i = ^Ref T _i (1:3,3) (11)

ⁿ P _n = [0 0 0] ^T (12)

⁰ T ₀ (1:3,3)＝[0 0 1] ^T (13)

In the above formulas (10) to (13), z _i represents the direction vector of the i-th joint based on a specific coordinate system, P _i represents the position vector of the i-th joint based on a specific coordinate system, and the superscript indicates the selection specific coordinate system.

Since the number of facet joints of the m-th arm segment is p, and the corresponding degrees of freedom are 2p, according to formula (6), the velocity of the end point of the m-th arm segment can be expressed as:

According to the coupling characteristics of the adjacent four degrees of freedom in the same arm segment, formula (14) can be simplified as:

Assuming that the {0} system is an inertial system, the general Jacobian matrix of the entire flexible arm can be simplified as:

in,

Comparing formula (15) and formula (16), it can be seen that, The dimension of is the 1/p of the Jacobian matrix J _m of the initial flexible manipulator. In the actual Jacobian inversion process, the calculation amount of the Jacobian matrix inversion is very large, which will affect the real-time performance of the operation. Therefore, in this embodiment Simplify to improve the efficiency of trajectory planning.

As shown in Figure 4, it is a schematic diagram of the trajectory planning method of the arm-shaped line constrained flexible robot in this embodiment. The coordinate system X ₀ Y ₀ Z ₀ of the chassis is established on the chassis of the flexible arm. The end point of the joint enters the slit, and the p-1th sub-joint is used as the matching point, and the tangential direction and tangential distance between the end point of the arm segment entering the slit and the starting point of the matching point remain unchanged. Considering the limited joint angle, for example, the method of fixing the degree of freedom of the arm segment in the slit will cause the feasible threshold of the flexible arm to become smaller, which affects its applicable range and limits its application range. In this embodiment, the arm shape of the arm section in the slit is a straight line, and the line vector direction of the straight line is parallel to the axis of the slit, that is, the direction of the end of the flexible arm relative to the starting point of the segment to be entered is consistent with the tangent direction of the planned trajectory , to ensure that the Euler distance between them is the maximum distance (that is, the straight state).

The trajectory planning process of this embodiment will be described in detail below with reference to FIG. 4 .

When the last section of the flexible arm enters the slit, the position vectors between the starting point of the i-1 section of the nth arm section and the end point of the last section are expressed as:

In the above formula (17), l _n,i represents the length of the i-th sub-joint of the n-th arm segment, Represents the unit vector of the linear trajectory of O _A O _B , O _A represents the starting point of the slit, and O _B represents the end point of the slit.

Therefore, it can be deduced that when the i-th sub-joint of any m-th arm section enters the slit, the position vector of the i-1th section is expressed as:

The above is the vector constraint relationship of the arm shape line of the arm section inside the slit, and the arm section outside the slit needs to prevent collision with obstacles in the environment. In this embodiment, as shown in Figure 4, an _o is the center of the ball Set the envelope for the radius ball. By judging whether the Euler distance between the center of the ball and the nearest arm segment is within the obstacle avoidance distance threshold of the outer arm segment of the slit, the Euler distance is expressed as:

In the above formula, Indicates the Euler distance between the center of the sphere and the nearest arm segment, and d _saf indicates the safety distance.

The pose of the end point of the flexible arm is planned according to the trajectory of the desired point, taking into account the above-mentioned Euler distance constraints, so as to ensure that the flexible arm enters the slit without contacting the slit wall or colliding with external environmental obstacles .

Therefore, the trajectory planning of the flexible arm can be decomposed into: (1) By extending the Jacobian matrix, on the one hand, the pose of the end point pointing to the desired point is ensured, and on the other hand, the arm segment inside the slit is straightened and parallel to the slit plane ; (2) Use the gradient projection method to ensure that the outer arm segment of the slit does not collide with obstacles, and at the same time meet the conditions for the segment to enter the slit smoothly.

The position vector from the end point of the flexible arm to any reference point (taking the end of the i-th child node in the m-th section as an example for illustration) is expressed as:

r _m,i ＝P _e -P _m,i ＝R _m,i ^m,i t _e (20)

Among them, R _m,i represents the rotation matrix of the i-th sub-joint of the m-th arm segment, ^m,i t _e represents the translation vector from the end of the i-th sub-joint of the m-th arm segment to the coordinate system of the end of the flexible arm, and

Differentiate formula (20), the following expression can be obtained:

Among them, ⁰ R _{m, k} , ^{m, 1} R _{m, i} represent the attitude transformation matrix from the {0} coordinate system to the {2p(m-1)+k} coordinate system of the kth sub-joint in the mth segment And the attitude transformation matrix from the {2p(m-1)+1} coordinate system of the first sub-joint in the m segment to the {2p(m-1)+i} coordinate system of the i-th sub-joint.

Write the above formula (21) in the form of a matrix to get:

Among them, ⁰ R _m,0 represents the attitude transformation matrix from the initial coordinate system to the starting point coordinate system of the m-th arm segment, ^m,k R _m,k represents the k-th sub-joint coordinate system of the m-th arm segment to the k-th sub-joint of the m-th segment The attitude transformation matrix of the coordinate system, and has: ⁰ R _m,0 = ^m,k R _m,k =eye(3).

Assuming that the three-dimensional coordinates of any three-dimensional point P _o in space are (x _o , y _o , z _o ), then the three-dimensional coordinates of any two adjacent points p _m,j and p _m,j+1 The coordinates are expressed as _: (x _m,j ,y _m,j ,z _m,j ) and (x _m,j+1 ,y _m,j+1 ,z _m,j+1 ), then any point P _o to the straight line The Euler distance of is expressed as:

Traverse all joints of the entire flexible arm, and take the sub-joint closest to the target position point in all distances as the target distance search range, that is:

make Indicates the minimum distance deviation between the outer arm section of the slit and the obstacle, and taking the derivative of D _o to Θ, we can get:

and have: and It can be concluded that:

With the above derivation process, it can be obtained that the extended Jacobian matrix _Jd of the obstacle avoidance constraint is expressed as:

Combining Equation (17), Equation (22) and Equation (27), the Jacobian matrix J _Line based on the arm type line constraint and the extended Jacobian matrix J _d of the obstacle avoidance constraint can be obtained to obtain the generalized extended Jacobian matrix Expressed as:

According to the idea of trajectory planning, the relative pose deviation between the end point of the flexible arm end point and the expected point (i.e. the target position point), the position deviation between the arm shape vector of the arm segment in the slit and the expected arm shape vector in this embodiment and the relationship between the minimum distance deviation between the outer arm segment of the slit and the obstacle is described as:

Among them, D _δ represents the expected distance deviation, D _o represents the distance between the slit outer arm segment and the obstacle, ΔD _o represents the minimum distance deviation between the slit outer arm segment and the obstacle, and has the following relationship: K _p =eye( 10), ΔD _o =D _o -D _δ ,

According to the above formula (29), the inverse kinematics solution method is used to calculate the corresponding joint angular velocity, which is expressed as:

In the above formula (30), Represents the generalized extended Jacobian matrix pseudo-inverse of .

Correspondingly, according to formula (30), the joint control amount at the next moment can be expressed as:

In the above formula, Θ _d (t) represents the desired joint angle at time t, is the expected angular velocity of the joint at time t, and the joint control amount of the flexible arm at time t can be calculated by formula (31)

In this embodiment, if the current time t<t _f and the threshold judgment condition is not satisfied, the trajectory planning will continue; otherwise, the planning process will end, and t _f represents the preset maximum running time, because the error at the beginning of the trajectory planning It is very large, and only multiple iterations will gradually converge until the threshold judgment condition is met, but it cannot run for an infinite time, so the preset maximum running time is set, and the trajectory planning process is continuously cycled within this time period until it reaches The tolerance range of the target location point.

In this embodiment, through the generalized extension of the Jacobian matrix at the end point of the flexible arm end point and the relative pose deviation of the expected point (i.e. the target position point), the arm shape vector of the arm segment in the slit and the expected arm shape vector The position deviation and the minimum distance deviation between the outer arm section of the slit and the obstacle are synchronized, so that the end of the flexible robot can not only complete the tracking of the expected pose, but also the part entering the narrow cylindrical slit does not collide with the slit wall , in addition, it can also realize the obstacle avoidance function of the outer part of the flexible robot slit during the movement process, and realize the synchronous planning of "end-arm shape line-obstacle avoidance".

The joint angle data of the flexible robot is obtained by formula (31), and the relative deviation data of the flexible robot is judged according to the task of the planar slit-constrained space, and compared with the set threshold value, and the joint angle of each joint of the flexible robot is driven. Move until the relative pose deviation between the end of the flexible robot and the target position is within the first preset threshold range, that is, the target position is reached, and the trajectory planning task ends.

In step S2, the threshold judgment conditions include:

1) When the end point of the flexible robot arrives in the target area, it means that the surface projection of the end point of the flexible robot in the target area is located in the target area and the relative pose deviation is within the first preset threshold range. The first preset threshold range includes: relative The position deviation threshold and the relative attitude deviation threshold; 2) the position deviation is less than the position deviation threshold; 3) the distance is less than the obstacle avoidance distance threshold. When the above conditions are met at the same time, it is judged that the end of the flexible robot has reached the target position, and the planning process ends.

The threshold judgment process is expressed as: Among them, ΔP _e represents the relative position deviation, δ _p1 represents the relative position deviation threshold, Indicates the threshold of relative attitude deviation, δ _θ indicates the threshold of relative attitude deviation, δ _p2 indicates the threshold of position deviation, and δ _d indicates the threshold of obstacle avoidance distance.

In this embodiment, if the current time t<t _f and the threshold judgment condition is not satisfied, the trajectory planning will continue; otherwise, the planning process will end, and t _f represents the preset maximum running time, because the error at the beginning of the trajectory planning It is very large, and only multiple iterations will gradually converge until the threshold judgment condition is met, but it cannot run for an infinite time, so the preset maximum running time is set, and the trajectory planning process is continuously cycled within this time period until it reaches The tolerance range of the target location point.

In this embodiment, by constructing a space mapping model and obtaining the relative deviation data of the flexible robot, according to the obtained relative deviation data and the threshold judgment condition, it is judged whether the end point has reached the target position point in the target area. When the threshold judgment condition is satisfied, the end point is considered to have arrived. The target position point, otherwise, according to the relative deviation data, obtain the velocity data of the next moment of the flexible robot and calculate the expected angular velocity of the joints of the flexible robot, and obtain the joint control amount at the next moment according to the expected angular velocity of the joints, to drive the joints of the flexible robot to move to target location point.

Embodiment two:

This embodiment provides a trajectory planning device for a flexible robot constrained by an arm shape line, which is used to execute the method described in Embodiment 1. As shown in FIG. 5 , it is the structure of the trajectory planning device for a flexible robot constrained by an arm shape line in this embodiment. Block diagram, including:

Obtaining deviation data module 10: used to construct a space mapping model and obtain relative deviation data of the flexible robot, the relative deviation data includes: the relative pose deviation between the end point and the expected point of the flexible robot, the arm shape vector and the expected position of the arm segment in the slit The position deviation of the arm shape line vector, the minimum distance deviation between the outer arm section of the slit and the obstacle;

Threshold judgment module 20: used for judging whether the end point reaches the target position point of the target area according to the relative deviation data and the threshold judgment condition;

Threshold judgment result execution module 30: used to consider that the end point has reached the target position point when the threshold judgment condition is satisfied, otherwise, obtain the speed data of the next moment of the flexible robot according to the relative deviation data, and calculate the joint of the flexible robot according to the speed data Expected angular velocity, according to the expected angular velocity of the joint, obtain the joint control amount at the next moment to drive the joints of the flexible robot to reach the target position point. The speed data at the next moment includes: the linear velocity and angular velocity of the end point, the The linear speed of the arm section of the arm section, and the instantaneous speed of obstacle avoidance of the arm section outside the slit.

In addition, the present invention also provides a trajectory planning device for an arm-shaped line-constrained flexible robot, including:

at least one processor, and a memory communicatively coupled to the at least one processor;

Wherein, the processor is used to execute the method described in Embodiment 1 by invoking the computer program stored in the memory.

In addition, the present invention also provides a computer-readable storage medium, where computer-executable instructions are stored in the computer-readable storage medium, where the computer-executable instructions are used to make a computer execute the method as described in Embodiment 1.

The present invention performs unified modeling according to the mapping relationship between the cylindrical slit and the joint space of the flexible robot to the Cartesian space of the end point, realizes the trajectory planning of the end point, and combines the pose characteristics of the slit inner arm segment and the slit outer arm segment , to realize that the part of the flexible robot entering the slit does not collide with the slit wall and the part outside the slit realizes the obstacle avoidance function. Compared with the prior art, the trajectory planning efficiency of the arm-shaped line-constrained flexible robot is improved while taking into account the control accuracy. The invention can be widely used in the field of trajectory planning of flexible robots constrained by arm type lines.

The above embodiments are only used to illustrate the technical solutions of the present invention, and are not intended to limit them. Although the present invention has been described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: it can still implement the foregoing embodiments. Modifications to the technical solutions described in the examples, or equivalent replacement of some or all of the technical features; and these modifications or replacements do not make the essence of the corresponding technical solutions depart from the scope of the technical solutions of the embodiments of the present invention, and they shall cover Within the scope of the claims and description of the present invention.

Claims (10)

1. A trajectory planning method for an arm-type line constrained flexible robot is suitable for the arm-type line constrained flexible robot, a target area is a cylindrical slit, the part of the flexible robot entering the cylindrical slit is a slit inner arm section, correspondingly, the part outside the cylindrical slit is a slit outer arm section, and the trajectory planning method is characterized by comprising the following steps:

Constructing a spatial mapping model and acquiring relative deviation data of the flexible robot, wherein the relative deviation data comprises: the relative pose deviation of the tail end point and the expected point of the flexible robot, the position deviation of an arm line vector of the slit inner arm section and an expected arm line vector, and the minimum distance deviation of the slit outer arm section and the obstacle;

Judging whether the tail end point reaches a target position point of the target area or not according to the relative deviation data and a threshold judgment condition;

when a threshold judgment condition is met, the terminal point is considered to reach the target position point;

otherwise, acquiring speed data of the flexible robot at the next moment according to the relative deviation data, calculating expected angular speed of joints of the flexible robot according to the speed data, and acquiring joint control quantity at the next moment according to the expected angular speed of the joints to drive each joint of the flexible robot to move to reach a target position point;

the speed data of the next time instant includes: the linear velocity and the angular velocity of the tail end point, the arm type linear velocity of the slit inner arm section and the obstacle avoidance instantaneous velocity of the slit outer arm section for obstacle avoidance.

2. the trajectory planning method for the arm-type line constrained flexible robot according to claim 1, characterized in that: the threshold judgment condition includes: the tail end point of the flexible robot reaches the target area, the relative pose deviation is within a first preset threshold range, the position deviation is smaller than a position deviation threshold, and the minimum distance deviation between the slit outer arm section and the obstacle is smaller than an obstacle avoidance distance threshold.

3. The trajectory planning method for the arm-type line constrained flexible robot according to claim 1, characterized in that: the specific formula for calculating the expected angular velocity of the flexible robot joint is as follows:

wherein,Representing a desired angular velocity of a joint of the flexible robot,a pseudo-inverse of a generalized extended jacobian matrix between a jacobian matrix representing an arm-type line constraint and an extended jacobian matrix representing an obstacle avoidance constraint,Representing the generalized velocity of the end point in an end coordinate system,The linear velocity of the arm type is indicated,And representing the obstacle avoidance instantaneous speed.

4. The trajectory planning method for the arm-type line constrained flexible robot according to claim 3, characterized in that: the formula for calculating the joint control amount at the next moment is specifically as follows:

wherein, theta_d(t) represents the desired joint angle at time t,Represents the desired angular velocity of the joint at time t, and Δ t represents the time interval.

5. the trajectory planning method for the arm-type line constrained flexible robot according to claim 3, characterized in that: and deriving the joint angle by the position deviation to obtain a Jacobian matrix constrained by the arm profile.

6. The trajectory planning method for the arm-type line constrained flexible robot according to claim 3, characterized in that: and deriving a joint angle by the minimum distance deviation between the slit outer arm section and the obstacle to obtain the expanded Jacobian matrix of obstacle avoidance constraint.

7. The trajectory planning method for the arm-type line-constrained flexible robot according to any one of claims 1 to 6, characterized in that: the arm type of the inner arm section of the slit is a straight line, and the line vector direction of the straight line is parallel to the axis of the slit.

8. An arm-type line constraint flexible robot trajectory planning device, comprising:

and a relative deviation data acquisition module: the flexible robot relative deviation data acquisition unit is used for constructing a spatial mapping model and acquiring the relative deviation data of the flexible robot, wherein the relative deviation data comprises: the relative pose deviation of the tail end point and the expected point of the flexible robot, the position deviation of an arm line vector of the slit inner arm section and an expected arm line vector, and the minimum distance deviation of the slit outer arm section and the obstacle;

a threshold value judging module: the terminal point is used for judging whether the terminal point reaches a target position point of a target area or not according to the relative deviation data and a threshold judgment condition;

A threshold judgment result execution module: and when a threshold judgment condition is met, considering that the terminal point reaches a target position point, otherwise, acquiring speed data of the next moment speed of the flexible robot according to the relative deviation data, calculating an expected angular speed of a joint of the flexible robot according to the speed data, and acquiring a joint control quantity of the next moment according to the expected angular speed of the joint to drive each joint of the flexible robot to move to reach the target position point, wherein the speed data of the next moment speed comprises: the linear velocity and the angular velocity of the tail end point, the arm type linear velocity of the slit inner arm section and the obstacle avoidance instantaneous velocity of the slit outer arm section for obstacle avoidance.

9. An arm-type line constraint flexible robot trajectory planning device, comprising:

at least one processor; and a memory communicatively coupled to the at least one processor;

wherein the processor is operable to perform the method of any one of claims 1 to 7 by invoking a computer program stored in the memory.

10. A computer-readable storage medium having stored thereon computer-executable instructions for causing a computer to perform the method of any one of claims 1 to 7.