CN110561419A - arm-shaped line constraint flexible robot track planning method and device - Google Patents
arm-shaped line constraint flexible robot track planning method and device Download PDFInfo
- Publication number
- CN110561419A CN110561419A CN201910733184.5A CN201910733184A CN110561419A CN 110561419 A CN110561419 A CN 110561419A CN 201910733184 A CN201910733184 A CN 201910733184A CN 110561419 A CN110561419 A CN 110561419A
- Authority
- CN
- China
- Prior art keywords
- arm
- flexible robot
- slit
- joint
- deviation
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 239000011159 matrix material Substances 0.000 claims description 48
- 239000013598 vector Substances 0.000 claims description 34
- 238000013507 mapping Methods 0.000 claims description 12
- 238000004590 computer program Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 description 13
- 230000033001 locomotion Effects 0.000 description 10
- 238000010586 diagram Methods 0.000 description 7
- 230000009466 transformation Effects 0.000 description 6
- 230000004888 barrier function Effects 0.000 description 5
- 238000004364 calculation method Methods 0.000 description 4
- 238000009795 derivation Methods 0.000 description 4
- 230000006870 function Effects 0.000 description 4
- 238000005457 optimization Methods 0.000 description 3
- 230000009286 beneficial effect Effects 0.000 description 2
- 238000006243 chemical reaction Methods 0.000 description 2
- 230000007613 environmental effect Effects 0.000 description 2
- 238000012423 maintenance Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 230000001360 synchronised effect Effects 0.000 description 2
- 230000005540 biological transmission Effects 0.000 description 1
- 125000004432 carbon atom Chemical group C* 0.000 description 1
- 238000001816 cooling Methods 0.000 description 1
- 230000008878 coupling Effects 0.000 description 1
- 238000010168 coupling process Methods 0.000 description 1
- 238000005859 coupling reaction Methods 0.000 description 1
- 238000007689 inspection Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
- 238000013519 translation Methods 0.000 description 1
Classifications
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1602—Programme controls characterised by the control system, structure, architecture
- B25J9/161—Hardware, e.g. neural networks, fuzzy logic, interfaces, processor
-
- B—PERFORMING OPERATIONS; TRANSPORTING
- B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
- B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
- B25J9/00—Programme-controlled manipulators
- B25J9/16—Programme controls
- B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
- B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
Landscapes
- Engineering & Computer Science (AREA)
- Robotics (AREA)
- Mechanical Engineering (AREA)
- Automation & Control Theory (AREA)
- Physics & Mathematics (AREA)
- Artificial Intelligence (AREA)
- Evolutionary Computation (AREA)
- Fuzzy Systems (AREA)
- Mathematical Physics (AREA)
- Software Systems (AREA)
- Manipulator (AREA)
- Numerical Control (AREA)
Abstract
The invention discloses a method and a device for planning a track of an arm-shaped line constrained flexible robot. The method comprises the steps of obtaining relative deviation data of the flexible robot, judging whether a tail end point reaches a target position point of a target area according to the obtained relative deviation data and a threshold judgment condition, if so, judging that the tail end point reaches the target position point, otherwise, obtaining 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, obtaining joint control quantity at the next moment according to the expected angular speed of the joints, and driving each joint of the flexible robot to move to reach the target position point. The track planning of the end point is realized, the pose characteristics of the slit inner arm section and the slit outer arm section are combined, the fact that the part of the flexible robot entering the slit does not collide with the slit wall is achieved, the obstacle avoidance function is achieved on the slit outer part, the track planning efficiency of the arm-shaped line constraint flexible robot is improved, and the control precision is considered.
Description
Technical Field
The invention relates to the field of robot control, in particular to a method and a device for planning a track of an arm-shaped line-constrained flexible robot.
Background
The current environment adaptability to the intelligent robot and the overcoming capability of the environment limitation have higher and higher requirements, because the working space of the traditional industrial robot is small, the motion flexibility is insufficient, especially, the environment adaptability to some unstructured environments is weak, and the crossing problem of various barriers in the narrow environment is difficult to complete by facing the limitation of the degree of freedom and the rigid arm rod, the related research on the rope-driven super-redundant robot which can meet the requirements of operation in the narrow space is more and more, the rope-driven super-redundant robot has the advantages of more degree of freedom, small arm rod, strong motion flexibility, good environment adaptability and the like, the crossing task of a plurality of narrow cylindrical spaces can be met, for example, the super-conventional operation areas such as the maintenance of a nuclear power station cooling pipeline, the maintenance of an oil-gas pipeline, the nuclear reactor pipeline inspection and the like.
However, such tight spaces are generally cylindrical, and to perform such tight space tasks, a flexible robot is required to reach a target location without contacting the inner wall of the confined space (i.e., the flexible arm does not collide with environmental obstacles). The traditional research only considers the operation of the tail end of the flexible robot reaching a target position, or simply avoids an environmental barrier through gradient projection, and does not consider the relation of combining a cylindrical narrow space and an arm type. And the traditional optimization method solves the problems of low calculation efficiency or excessively complex optimization equation and optimization index, and is not beneficial to the real-time motion control of the flexible robot. Therefore, an arm-type line constraint flexible robot trajectory planning method capable of improving the planning efficiency of the trajectory passing through the cylindrical slit and simultaneously considering the control precision is needed.
Disclosure of Invention
the present invention is directed to solving, at least to some extent, one of the technical problems in the related art. Therefore, the invention aims to provide an arm-shaped line constraint flexible robot track planning method which can improve the planning efficiency of a track passing through a cylindrical slit and simultaneously give consideration to control precision.
the technical scheme adopted by the invention is as follows:
In a first aspect, the present invention provides a trajectory planning method for an arm-type line-constrained flexible robot, which is applicable to an arm-type line-constrained flexible robot, where a target area is a cylindrical slit, a portion of the flexible robot entering the cylindrical slit is a slit inner arm section, and correspondingly, a portion outside the cylindrical slit is a slit outer arm section, and the method includes:
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.
Further, the threshold determination 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.
further, a specific formula for calculating the desired 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.
Further, the formula for calculating the joint control amount at the next time is specifically:
wherein, thetad(t) represents the desired joint angle at time t,representing the desired angular velocity of the joint at time t.
further, derivation is carried out on the joint angle through the position deviation, and a Jacobian matrix of the arm-shaped line constraint is obtained.
Further, derivation is carried out on joint angles through minimum distance deviation between the slit outer arm section and the obstacle, and an expanded Jacobian matrix of obstacle avoidance constraint is obtained.
Further, 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.
In a second aspect, the present invention further provides an arm-type line constrained flexible robot trajectory planning device, including:
a 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 the 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.
In a third aspect, the present invention provides an arm-type line-constrained flexible robot trajectory planning device, including:
at least one processor, and a memory communicatively coupled to the at least one processor;
Wherein the processor is adapted to perform the method of any of the first aspects by invoking a computer program stored in the memory.
in a fourth aspect, the present invention provides a computer-readable storage medium having stored thereon computer-executable instructions for causing a computer to perform the method of any of the first aspects.
the invention has the beneficial effects that:
The method comprises the steps of constructing a spatial mapping model, obtaining relative deviation data of the flexible robot, judging whether a tail end point reaches a target position point of a target area according to the obtained relative deviation data and a threshold judgment condition, if so, judging that the tail end point reaches the target position point, otherwise, obtaining speed data of the flexible robot at the next moment according to the relative deviation data, calculating the expected angular speed of the joint of the flexible robot, obtaining joint control quantity at the next moment according to the expected angular speed of the joint, and driving each joint of the flexible robot to move to reach the target position point. According to the method, unified modeling is carried out according to the cylindrical slit and the mapping relation from the joint space of the flexible robot to the Cartesian space of the tail end point, the track planning of the tail end point is realized, the pose characteristics of the slit inner arm section and the slit outer arm section are combined, the fact that the part of the flexible robot entering the slit does not collide with the slit wall is realized, and the obstacle avoidance function is realized on the outer part of the slit.
The method can be widely applied to the field of arm-type line constraint flexible robot trajectory planning.
drawings
FIG. 1 is a flowchart illustrating an implementation of a trajectory planning method for an arm-type line-constrained flexible robot according to an embodiment of the present invention;
FIG. 2 is a schematic diagram of a joint coordinate system of a flexible robot according to an embodiment of the method for planning a trajectory of an arm-type line-constrained flexible robot in the present invention;
FIG. 3 is a D-H coordinate system distribution diagram of an arm segment of a flexible robot according to an embodiment of the method for planning a trajectory of an arm-type line-constrained flexible robot in the present invention;
FIG. 4 is a schematic diagram illustrating a method for planning a trajectory of an arm-type line-constrained flexible robot according to an embodiment of the present invention;
Fig. 5 is a block diagram of a trajectory planning device of an arm-type line-constrained flexible robot according to an embodiment of the present invention.
Detailed Description
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the following description will be made with reference to the accompanying drawings. It is obvious that the drawings in the following description are only some examples of the invention, and that for a person skilled in the art, other drawings and embodiments can be derived from them without inventive effort.
Unless defined otherwise, all technical and scientific terms used herein have the same meaning as commonly understood by one of ordinary skill in the art to which this invention belongs. The terminology used in the description of the invention herein is for the purpose of describing particular embodiments only and is not intended to be limiting of the invention.
the first embodiment is as follows:
The embodiment of the invention provides a trajectory planning method for an arm-type line constrained flexible robot, which is suitable for an arm-type line constrained flexible robot (namely, a joint-type line constrained flexible robot, in the embodiment, a joint-type flexible arm is taken as an example), wherein a target area of operation is a cylindrical slit, a part of the flexible robot entering the cylindrical slit is defined as a slit inner arm section, and a part outside the cylindrical slit is defined as a slit outer arm section.
Fig. 1 is a flowchart of an implementation of a trajectory planning method for an arm-type line-constrained flexible robot according to an embodiment of the present invention, and as shown in fig. 1, the method includes the following steps:
S1: and constructing a space mapping model and acquiring relative deviation data of the flexible robot.
In this embodiment, the space mapping model optionally performs unified modeling on the cylindrical slit of the operation target area, the joint space of the flexible robot, and the cartesian space mapping relationship of the end point according to the hand-eye vision system, and jointly establishes an integrated jacobian matrix of "end point pose-arm profile-obstacle avoidance" by combining end point pose constraint, slit inner arm segment arm profile constraint, and slit outer arm segment obstacle avoidance constraint.
In this embodiment, the relative deviation data includes: flexible robot tail end pointAnd the relative pose deviation of the desired point (i.e., the target position point), the position deviation (denoted as Δ P) of the arm line vector of the arm segment in the slit and the desired arm line vectorm,i) The minimum distance deviation (denoted as Δ D) between the outer arm segment of the slit and the obstacleo). Wherein, relative position appearance deviation includes: relative positional deviation (denoted as Δ P)e) And relative attitude deviation (note as)。
S2: and judging whether the terminal point reaches the target position point of the target area or not according to the relative deviation data and the threshold judgment condition.
S21: and when the threshold judgment condition is met, the terminal point is considered to reach the target position point, and the track planning task is finished.
s22: otherwise, continuing to plan the track, 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, 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, and repeating the process until the target position point is reached.
In this embodiment, the speed data at the next time includes: linear and angular velocities of the end point, arm type linear velocity of the slit inner arm section, and obstacle avoidance instantaneous velocity of the slit outer arm section.
In step S22, the specific process of planning the trajectory is as follows.
s221: deriving the joint angle by the position deviation of the arm contour line vector of the arm section in the slit and the expected arm contour line vector to obtain a Jacobian matrix of arm contour line constraint, and recording the Jacobian matrix as JLine。
s222: minimum distance deviation Delta D between slit outer arm section and obstacleoderivation is carried out on the joint angle to obtain an expanded Jacobian matrix of obstacle avoidance constraint, and the expanded Jacobian matrix is recorded as Jd。
s223: jacobian matrix J constrained by arm-line according to the above stepsLineextended jacobian matrix J with obstacle avoidance constraintsdobtain generalized extended YakeRatio matrix, notethe generalized extended Jacobian matrix is subjected to inverse kinematics solution, and the expected angular velocity of the flexible robot joint is obtained through calculation and recorded asThe calculation formula is expressed as:
In the above-mentioned formula (1),indicating the desired angular velocity of the joints of the flexible robot,jacobian matrix J representing arm type line constraintLineExtended Jacobian matrix J with obstacle avoidance constraintsdthe pseudo-inverse of the generalized extended jacobian matrix in between,Representing the generalized velocity of the end point in the end coordinate system,The linear velocity of the arm type is shown,and representing the obstacle avoidance instantaneous speed.
S224: integrating the expected joint angular velocity to obtain the joint control quantity at the next moment, driving each joint of the flexible robot to move until the threshold judgment condition is met, and enabling the running time t to be at the preset maximum running time tfand judging the target position to be reached, and finishing the track planning.
This embodiment fuses arm type line restraint and obstacle avoidance restraint, the combination becomes to have the arm type line, keep away the barrier function in novel generalized extension jacobian matrix of an organic whole, and then can obtain flexible robot's joint angular velocity through decomposing the speed method, the joint angle that obtains flexible robot after carrying out the integral to joint angular velocity's numerical value, and then control flexible robot end point and move to the target position point fast, realize passing through the purpose in narrow and small cylindrical confined space effectively, can improve the precision of trajectory control, the efficiency of trajectory control has also been improved simultaneously.
The calculation process of the present embodiment is described in detail below.
As shown in fig. 2, which is a schematic view of a joint coordinate system of the flexible robot in this embodiment, it can be seen that the flexible robot is a schematic view of a joint coordinate system of the mth sub-joint of the flexible robot, and a wiring disc 1 (circle center O) of the kth sub-joint is assumed2p(m-1)+k) The wire passing holes of the upper driving rope are represented as: a. the2p(m-1)+k,1、A2p(m-1)+k,2、A2p(m-1)+k,3at the center of the circle O2p(m-1)+kthe coordinate system established is expressed as: x2p(m-1)+kY2p(m-1)+kZ2p(m-1)+kWiring disc 2 of the (k + 1) th joint (center O)2p(m-1)+k+1) The drive cord passage holes are represented as: b is2p(m-1)+k+1,1、B2p(m-1)+k+1,2、B2p(m-1)+k+1,3At the center of the circle O2p(m-1)+k+1the coordinate system established is expressed as: x2p(m-1)+k+1Y2p(m-1)+k+1Z2p(m-1)+k+1Line segment A2p(m-1)+k,1B2p(m-1)+k+1,1、A2p(m-1)+k,2B2p(m-1)+k+1,2and A2p(m-1)+k,3B2p(m-1)+k+1,3Each representing the distance of three drive ropes between two wiring disc through-holes, O2p(m-1)represents the joint center of the kth sub-joint, at point O2p(m-1)The coordinate system established is expressed as: x2p(m-1)Y2p(m-1)Z2p(m-1)。
Fig. 3 is a D-H coordinate system distribution diagram of an arm segment of the flexible robot in this embodiment. In this embodiment, the flexible robot is set to be composed of n linked arm segments, the rotation angles of joints in the same arm segment are the same, and it is assumed that each arm segment has four orthogonal sub-joints, and a coordinate system X is provided2pm+1Y2pm+1Z2pm+1The starting coordinate system of the adjacent m +1 arm segment on the joint is established, and the positive kinematic equation of the mth arm segment is expressed by the kinematic recursion relationship as follows:
In the above formula (2), (θ)2m-1,θ2m) Two joint angles of the mth arm segment are shown,A homogeneous transformation matrix representing the mth arm segment,Respectively representing the direction vectors of the homogeneous transformation matrix in the x, y and z axes,Representing the position vector of the homogeneous transformation matrix.
from this it follows that the positive kinematic equation for the entire flexible arm is expressed as:
In the above formula (3), REuler_ZYXrepresents TeRotation matrix in matrix ZYX Euler Angle mode, PeRepresents TeThe position vector of the matrix is then calculated,A homogeneous transformation matrix representing the ith segment, (theta)2i-1,θ2i) Two joint angles of the ith arm segment are shown.
The joint angular velocity of the entire flexible arm can be expressed as:
in the above-mentioned formula (4),Representing the joint angular velocity of the ith arm segment.
the generalized motion velocity of the flexible arm tip can be expressed as:
in the above formula (5), ve＝[vex,vey,vez]T∈R3、ωe＝[ωex,ωey,ωez]T∈R3respectively representing the linear and angular velocities of the flexible arm end point,Indicating the position differential and attitude differential of the end point.
Differentiating the upper part (5) to obtain a velocity level positive kinematic equation of the flexible arm, and expressing the velocity level positive kinematic equation as follows:
in the above formula (6), Jg(Θ)＝[J1 J2 … Jn]∈R6×2na conventional Jacobian matrix representing the flexible arm is a function of the joint angle, and the relationship between the angular velocity of the joint of the flexible arm and the velocity of the movement of the distal point is established, JiIs the transmission ratio of the movement speed of the joint i to the movement speed of the end point of the flexible arm, and Jia column vector of 6x 1.
Assumption xim,iIs the rotation axis of the ith sub-joint of the mth arm segment, Pm,iFor the position vector of the ith sub-joint of the mth arm segment, the jacobian matrix of the mth arm segment can be expressed as:
RefJm＝[RefJm,1 RefJm,2 … RefJm,2p] (7)
Wherein,the subscript in indicates the index from i to n, and Ref includes {0} system and { n } system.
1) when Ref is {0} system:
2) when Ref is { n } system:
And:
RefPi＝RefTi(1:3,4) (10)
Refzi＝RefTi(1:3,3) (11)
nPn＝[0 0 0]T (12)
0T0(1:3,3)＝[0 0 1]T (13)
In the above formulae (10) to (13), zirepresenting a direction vector, P, of the i-th joint based on a particular coordinate systemiThe position vector of the ith joint based on the specific coordinate system is shown, and the superscript shows that the specific coordinate system is selected.
Since the number of the minor joints of the mth arm segment is p, and the corresponding degree of freedom is 2p, the velocity of the end point of the mth arm segment can be expressed as follows according to equation (6):
according to the coupling characteristics of adjacent four degrees of freedom in the same arm segment, equation (14) can be simplified as follows:
assuming {0} is the inertial system, the conventional Jacobian matrix for the entire flexible arm can be simplified as:
wherein,
Comparing the formula (15) with the formula (16), it can be seen that,Dimension of (a) is the Jacobian matrix J of the initial flexible manipulatorm1/p of (1/p) in the actual jacobian inversion process, the computation amount of the jacobian matrix inversion is very large, and the real-time performance of the computation is influenced, so that the embodiment is simplified, and the efficiency of trajectory planning is improved.
As shown in fig. 4, which is a schematic diagram of the method for planning the trajectory of the arm-type line-constrained flexible robot in this embodiment, a coordinate system X of a chassis is established on the chassis of the flexible arm0Y0Z0When the end point of the p-th sub-joint of the nth arm segment enters the slit, the p-1 th sub-joint is used as a matching point, and the tangential direction and the tangential distance between the end point of the arm segment entering the slit and the starting point of the matching point are kept unchanged. Considering the problem of limited joint angle, for example, fixing the degree of freedom of the arm segment in the slit, the flexible arm has a smaller feasible threshold, and the application range is limited due to the influence on the application range. In this embodiment, the arm shape of the arm section in the slit is a straight line, and the linear vector direction of the straight line is parallel to the axis of the slit, that is, the direction of the tail end of the flexible arm relative to the starting point of the section to be entered is consistent with the tangential direction of the planned trajectory, so as to ensure that the euler distance between the flexible arm and the section to be entered is the maximum distance (i.e., the straightened state).
the trajectory planning process of the present embodiment is described in detail below with reference to fig. 4.
after the last section of the flexible arm enters the slit, the position vector of the starting point and the ending point of the section i-1 of the nth arm segment is expressed as:
In the above formula (17), ln,iindicates the length of the ith sub-joint of the nth arm segment,Represents OAOBunit vector of straight track, OADenotes the starting point of the slit, OBIndicating the end point of the slit.
Therefore, it can be concluded that, when the ith sub-joint enters the slit for any mth arm segment, the position vector of the (i-1) th segment is expressed as:
The above is the arm type line vector constraint relation of the slit inner arm section, and the slit outer arm section needs to prevent collision with the obstacle in the environment, in this embodiment, as shown in fig. 4, the environment obstacle is used as PoIs a sphere centerEnveloping the sphere with the radius, and judging whether the Euler distance between the sphere center and the nearest arm section is within the obstacle avoidance distance threshold of the outer arm section of the slit or not, wherein the Euler distance is expressed as:
in the above formula, the first and second carbon atoms are,indicating the Euler distance, d, of the center of the sphere from the nearest arm segmentsafIndicating a safe distance.
The pose of the tail end point of the flexible arm is planned according to the track of the expected point, and the Euler distance constraint is considered, so that the flexible arm is ensured not to contact with the slit wall and collide with external environment barriers when entering the slit.
The trajectory planning of the flexible arm can therefore be decomposed into: (1) by expanding the Jacobian matrix, on one hand, the pose of the tail end point pointing to the expected point is ensured, and on the other hand, the straightening of the arm section in the slit in the motion process and the parallel with the slit plane are ensured; (2) the outer arm section of the slit is ensured not to collide with the barrier by a gradient projection method, and meanwhile, the condition that the section to be entered smoothly enters the slit is met.
the position vector from the end point of the flexible arm to any reference point (taking the end of the ith sub-node of the mth segment as an example for explanation) is expressed as:
rm,i＝Pe-Pm,i＝Rm,i m,ite (20)
Wherein R ism,iA rotation matrix representing the ith sub-joint of the mth arm segment,m,iteA translation vector representing the coordinate system from the ith sub-joint end of the mth arm segment to the end of the flexible arm, and having
By differentiating the expression (20), the following expression can be obtained:
Wherein,0Rm,k,m,1Rm,irespectively represent a posture conversion matrix from a {0} number coordinate system to a {2p (m-1) + k } number coordinate system of a kth sub-joint of the mth segment and a posture conversion matrix from a {2p (m-1) +1} number coordinate system of a 1 st sub-joint of the mth segment to a {2p (m-1) + i } number coordinate system of an ith sub-joint.
writing the above equation (21) in a matrix form, we can obtain:
wherein,0Rm,0A pose transformation matrix representing the initial coordinate system to the m-th arm segment start coordinate system,m,kRm,kA pose transformation matrix representing the kth sub-joint coordinate system of the mth arm segment to the kth sub-joint coordinate system of the mth arm segment, and having:0Rm,0＝m,kRm,k＝eye(3)。
Assuming an arbitrary three-dimensional point P in spaceoHas a three-dimensional coordinate of (x)o,yo,zo) Then any two adjacent points p at the m arm section joint of the flexible armm,jAnd pm,j+1Are respectively expressed as：(xm,j,ym,j,zm,j) And (x)m,j+1,ym,j+1,zm,j+1) Then an arbitrary point PoTo a straight lineexpressed as follows:
traversing all joints of the whole flexible arm, and taking the sub-joint which is closest to the target position point in all distances as a target distance search range, namely:
Order torepresents the minimum distance deviation between the outer arm segment of the slit and the obstacle, DoBy taking the derivative of Θ, we can get:
and has: andIt can be derived that:
The extended Jacobian matrix J of the obstacle avoidance constraint can be obtained by the derivation processdexpressed as:
By combining equations (17), (22) and (27), a Jacobian matrix J based on the arm-type line constraint can be obtainedLineextended jacobian matrix J with obstacle avoidance constraintsdObtaining a generalized extended jacobian matrixexpressed as:
According to the idea of trajectory planning, the relationship between the relative pose deviation of the end point of the flexible arm end point and the expected point (i.e. the target position point), the position deviation of the arm line vector of the inner arm segment of the slit and the expected arm line vector, and the minimum distance deviation of the outer arm segment of the slit and the obstacle is described as follows:
Wherein D isδindicates the desired distance deviation, Dodenotes the distance, Δ D, of the outer arm segment of the slit from the obstacleorepresents the minimum distance deviation of the outer arm segment of the slit from the obstacle, and has the following relationship: kp＝eye(10)、ΔDo＝Do-Dδ、
An inverse kinematics solution is performed according to equation (29) above, and the corresponding joint angular velocity is calculated and expressed as:
In the above-mentioned formula (30),Representing a generalized extended jacobian matrixthe pseudo-inverse of (1).
accordingly, the joint control amount at the next time is obtained from equation (30) and expressed as:
In the above formula, thetad(t) represents the desired joint angle at time t,The joint control amount of the flexible arm at time t can be calculated from equation (31) for the desired angular velocity of the joint at time t
in this embodiment, if the time t of the current time is<tfIf the threshold judgment condition is not met, continuing to plan the track; otherwise, finish the planning process, tfAnd the preset maximum operation time is represented, because the error is very large when the trajectory planning is just started, the trajectory planning will tend to converge gradually only after multiple iterations until the threshold judgment condition is met, but the trajectory planning cannot be operated in an infinite time, so the preset maximum operation time is set, and the trajectory planning process is continuously circulated in the time period until the fault-tolerant range of the target position point is reached.
in the embodiment, the relative pose deviation of the tail end point of the flexible arm and an expected point (namely a target position point), the position deviation of the arm type line vector of the inner arm section of the slit and the expected arm type line vector and the minimum distance deviation of the outer arm section of the slit and an obstacle are synchronized through the generalized extended jacobian matrix, so that the tail end of the flexible robot can not only finish the tracking of the expected pose, but also avoid collision between the part entering the cylindrical narrow slit and the slit wall, and in addition, the obstacle avoidance function of the outer part of the slit of the flexible robot in the movement process can be realized, and the synchronous planning of 'tail end-arm type line-obstacle avoidance' is realized.
And (3) obtaining joint angle data of the flexible robot through a formula (31), judging relative deviation data of the flexible robot according to the planar slit limited space operation task, comparing the relative deviation data with a set threshold value, judging, driving the movement of each joint of the flexible robot until the relative pose deviation of the tail end of the flexible robot and the target position is within a first preset threshold value range, namely reaching the target position, and finishing the track planning task.
In step S2, the threshold value determination condition includes:
1) the terminal point of the flexible robot reaches the target area, the terminal point of the flexible robot is projected on the inner surface of the target area and is located in the target area, and the relative pose deviation is within a first preset threshold range, wherein the first preset threshold range comprises: a relative position deviation threshold and a relative attitude deviation threshold; 2) the position deviation is less than a position deviation threshold; 3) the distance is less than the obstacle avoidance distance threshold. And when the conditions are met, judging that the tail end of the flexible robot reaches the target position, and finishing the planning process.
The threshold determination process is expressed as:Wherein, Δ Peindicates the relative positional deviation, δp1A relative position deviation threshold value is indicated,Representing a relative attitude deviation threshold, δθthreshold value, delta, representing deviation of relative attitudep2indicating a position deviation threshold, δdIndicating an obstacle avoidance distance threshold.
in this embodiment, if the time t of the current time is<tfAnd does not satisfy the threshold judgment conditionthen continuing to plan the track; otherwise, finish the planning process, tfand the preset maximum operation time is represented, because the error is very large when the trajectory planning is just started, the trajectory planning will tend to converge gradually only after multiple iterations until the threshold judgment condition is met, but the trajectory planning cannot be operated in an infinite time, so the preset maximum operation time is set, and the trajectory planning process is continuously circulated in the time period until the fault-tolerant range of the target position point is reached.
In the embodiment, a spatial mapping model is constructed, relative deviation data of the flexible robot is acquired, whether the terminal point reaches a target position point of a target area is judged according to the acquired relative deviation data and a threshold judgment condition, if the threshold judgment condition is met, the terminal point is considered to reach the target position point, otherwise, speed data of the flexible robot at the next moment is acquired according to the relative deviation data, the expected angular speed of the joint of the flexible robot is calculated, and the joint control quantity at the next moment is acquired according to the expected angular speed of the joint, so that each joint of the flexible robot is driven to move to reach the target position point.
Example two:
The present embodiment provides an arm-type line-constrained flexible robot trajectory planning apparatus, configured to execute the method according to the first embodiment, as shown in fig. 5, which is a structural block diagram of the arm-type line-constrained flexible robot trajectory planning apparatus according to the present embodiment, and includes:
The deviation data acquiring module 10: the method is used for constructing a space mapping model and acquiring relative deviation data of the flexible robot, wherein the relative deviation data comprises the following steps: the relative pose deviation of a tail end point and a desired point of the flexible robot, the position deviation of an arm line vector of an inner arm section of the slit and an expected arm line vector, and the minimum distance deviation of an outer arm section of the slit and an obstacle;
the threshold value judging module 20: judging whether the terminal point reaches a target position point of the target area according to the relative deviation data and a threshold judgment condition;
Threshold determination result execution module 30: and when the threshold judgment condition is met, the terminal point is considered to reach the target position point, otherwise, the speed data of the next moment speed of the flexible robot is obtained according to the relative deviation data, the joint expected angular speed of the flexible robot is calculated according to the speed data, the joint control quantity of the next moment is obtained according to the joint expected angular speed, and each joint of the flexible robot is driven 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.
in addition, the invention also provides an arm-type line constraint flexible robot track planning device, which comprises:
At least one processor, and a memory communicatively coupled to the at least one processor;
Wherein the processor is configured to perform the method according to embodiment one by calling the computer program stored in the memory.
In addition, the present invention also provides a computer-readable storage medium, which stores computer-executable instructions for causing a computer to perform the method according to the first embodiment.
according to the method, unified modeling is carried out according to the cylindrical slit and the mapping relation from the joint space of the flexible robot to the Cartesian space of the tail end point, the track planning of the tail end point is realized, the pose characteristics of the slit inner arm section and the slit outer arm section are combined, the fact that the part of the flexible robot entering the slit does not collide with the slit wall is realized, and the obstacle avoidance function is realized on the outer part of the slit. The method can be widely applied to the field of arm-type line constraint flexible robot trajectory planning.
The above embodiments are only used for illustrating the technical solutions of the present invention, and not for limiting the same, although the present invention is described in detail with reference to the foregoing embodiments, those of ordinary skill in the art should understand that: the technical solutions described in the foregoing embodiments may still be modified, or some or all of the technical features may be equivalently replaced; such modifications and substitutions do not depart from the spirit and scope of the present invention, and they should be construed as being included in the following claims and description.
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, thetad(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.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910733184.5A CN110561419B (en) | 2019-08-09 | 2019-08-09 | Arm-shaped line constraint flexible robot track planning method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201910733184.5A CN110561419B (en) | 2019-08-09 | 2019-08-09 | Arm-shaped line constraint flexible robot track planning method and device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN110561419A true CN110561419A (en) | 2019-12-13 |
| CN110561419B CN110561419B (en) | 2021-01-12 |
Family
ID=68774907
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201910733184.5A Active CN110561419B (en) | 2019-08-09 | 2019-08-09 | Arm-shaped line constraint flexible robot track planning method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN110561419B (en) |
Cited By (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111300420A (en) * | 2020-03-16 | 2020-06-19 | 大连理工大学 | Method for solving minimum path of joint space corner of mechanical arm |
| CN112862812A (en) * | 2021-03-10 | 2021-05-28 | 中山大学 | Method, system and device for optimizing operation space trajectory of flexible robot |
| CN112936287A (en) * | 2021-03-19 | 2021-06-11 | 中山大学 | Flexible robot control method and device based on dynamics iterative learning |
| WO2022199610A1 (en) * | 2021-03-23 | 2022-09-29 | 中联重科股份有限公司 | Method and apparatus for compensating boom deflection, and method and apparatus for controlling boom |
Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN102079093A (en) * | 2010-12-27 | 2011-06-01 | 上海大学 | Novel flexible body feeding method |
| CN103717147A (en) * | 2011-05-27 | 2014-04-09 | 伊西康内外科公司 | Robotically-controlled endoscopic accessory channel |
| CN106956260A (en) * | 2017-03-31 | 2017-07-18 | 浙江大学 | A kind of inverse kinematics method of multi-joint mechanical arm flat serpentine track motion |
| US9763683B2 (en) * | 2001-08-28 | 2017-09-19 | Bonutti Skeletal Innovations Llc | Method for performing surgical procedures using optical cutting guides |
| US20180333855A1 (en) * | 2017-05-19 | 2018-11-22 | Stc.Unm | System and methods for multiple-place swarm foraging with dynamic depots |
| CN109303610A (en) * | 2017-07-27 | 2019-02-05 | 赛诺微医疗科技(北京)有限公司 | Surgical instrument clamping device, end effector and the surgery mechanical arm using it |
| CN109991847A (en) * | 2019-01-11 | 2019-07-09 | 天津理工大学 | A kind of Dynamics For Flexible Multibody robot time proximity optimal trajectory planning method |
-
2019
- 2019-08-09 CN CN201910733184.5A patent/CN110561419B/en active Active
Patent Citations (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9763683B2 (en) * | 2001-08-28 | 2017-09-19 | Bonutti Skeletal Innovations Llc | Method for performing surgical procedures using optical cutting guides |
| CN102079093A (en) * | 2010-12-27 | 2011-06-01 | 上海大学 | Novel flexible body feeding method |
| CN103717147A (en) * | 2011-05-27 | 2014-04-09 | 伊西康内外科公司 | Robotically-controlled endoscopic accessory channel |
| CN106956260A (en) * | 2017-03-31 | 2017-07-18 | 浙江大学 | A kind of inverse kinematics method of multi-joint mechanical arm flat serpentine track motion |
| US20180333855A1 (en) * | 2017-05-19 | 2018-11-22 | Stc.Unm | System and methods for multiple-place swarm foraging with dynamic depots |
| CN109303610A (en) * | 2017-07-27 | 2019-02-05 | 赛诺微医疗科技(北京)有限公司 | Surgical instrument clamping device, end effector and the surgery mechanical arm using it |
| CN109991847A (en) * | 2019-01-11 | 2019-07-09 | 天津理工大学 | A kind of Dynamics For Flexible Multibody robot time proximity optimal trajectory planning method |
Cited By (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111300420A (en) * | 2020-03-16 | 2020-06-19 | 大连理工大学 | Method for solving minimum path of joint space corner of mechanical arm |
| CN111300420B (en) * | 2020-03-16 | 2021-08-10 | 大连理工大学 | Method for solving minimum path of joint space corner of mechanical arm |
| CN112862812A (en) * | 2021-03-10 | 2021-05-28 | 中山大学 | Method, system and device for optimizing operation space trajectory of flexible robot |
| CN112862812B (en) * | 2021-03-10 | 2023-04-07 | 中山大学 | Method, system and device for optimizing operation space trajectory of flexible robot |
| CN112936287A (en) * | 2021-03-19 | 2021-06-11 | 中山大学 | Flexible robot control method and device based on dynamics iterative learning |
| WO2022199610A1 (en) * | 2021-03-23 | 2022-09-29 | 中联重科股份有限公司 | Method and apparatus for compensating boom deflection, and method and apparatus for controlling boom |
Also Published As
| Publication number | Publication date |
|---|---|
| CN110561419B (en) | 2021-01-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110561419B (en) | Arm-shaped line constraint flexible robot track planning method and device | |
| CN107490965B (en) | Multi-constraint trajectory planning method for space free floating mechanical arm | |
| CN110228069B (en) | Online obstacle avoidance motion planning method for mechanical arm | |
| CN111300425B (en) | Super-redundant mechanical arm tail end track motion planning method | |
| Lu et al. | Collision-free and smooth joint motion planning for six-axis industrial robots by redundancy optimization | |
| Zhu et al. | A convex optimization approach to smooth trajectories for motion planning with car-like robots | |
| CN109397271B (en) | 7-degree-of-freedom anthropomorphic mechanical arm and control method and system thereof | |
| CN113146600B (en) | Flexible robot trajectory planning method and device based on kinematics iterative learning control | |
| CN110561420B (en) | Arm profile constraint flexible robot track planning method and device | |
| Kabir et al. | Generation of synchronized configuration space trajectories of multi-robot systems | |
| CN111761582B (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN112809682B (en) | Mechanical arm obstacle avoidance path planning method and system and storage medium | |
| CN107538493A (en) | A kind of industrial robot method for planning track for avoiding cable interference constraint | |
| CN110682286A (en) | Real-time obstacle avoidance method for cooperative robot | |
| CN108772835B (en) | Obstacle and physical limit avoiding method | |
| CN105234930B (en) | Control method of motion of redundant mechanical arm based on configuration plane | |
| CN115416016A (en) | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method | |
| Ji et al. | E-RRT*: Path Planning for Hyper-Redundant Manipulators | |
| Su et al. | Pythagorean-Hodograph curves-based trajectory planning for pick-and-place operation of Delta robot with prescribed pick and place heights | |
| CN116872212A (en) | Double-mechanical-arm obstacle avoidance planning method based on A-Star algorithm and improved artificial potential field method | |
| CN116551693A (en) | Rope-driven super-redundancy snake-shaped mechanical arm kinematics inverse solution method | |
| Ata et al. | COLLISION-FREE TRAJECTORY PLANNING FOR MANIPULATORS USING GENERALIZED PATTERN SEARCH. | |
| CN115533920A (en) | Collaborative planning method and system for solving inverse kinematics of rope-driven mechanical arm and computer storage medium | |
| CN113084797B (en) | Dynamic cooperative control method for double-arm redundant mechanical arm based on task decomposition | |
| Bai et al. | Coordinated motion planning of the mobile redundant manipulator for processing large complex components |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |