CN110561419B - 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 PDF

Info

Publication number
CN110561419B
CN110561419B CN201910733184.5A CN201910733184A CN110561419B CN 110561419 B CN110561419 B CN 110561419B CN 201910733184 A CN201910733184 A CN 201910733184A CN 110561419 B CN110561419 B CN 110561419B
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.)
Active
Application number
CN201910733184.5A
Other languages
Chinese (zh)
Other versions
CN110561419A (en
Inventor
徐文福
彭键清
牟宗高
梁斌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Graduate School Harbin Institute of Technology
Original Assignee
Shenzhen Graduate School Harbin Institute of Technology
Priority date (The priority date 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 date listed.)
Filing date
Publication date
Application filed by Shenzhen Graduate School Harbin Institute of Technology filed Critical Shenzhen Graduate School Harbin Institute of Technology
Priority to CN201910733184.5A priority Critical patent/CN110561419B/en
Publication of CN110561419A publication Critical patent/CN110561419A/en
Application granted granted Critical
Publication of CN110561419B publication Critical patent/CN110561419B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

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

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

Arm-shaped line constraint flexible robot track planning method and device
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:
Figure BDA0002161320070000021
wherein the content of the first and second substances,
Figure BDA0002161320070000022
representing a desired angular velocity of a joint of the flexible robot,
Figure BDA0002161320070000023
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,
Figure BDA0002161320070000024
representing the generalized velocity of the end point in an end coordinate system,
Figure BDA0002161320070000025
the linear velocity of the arm type is indicated,
Figure BDA0002161320070000026
and representing the obstacle avoidance instantaneous speed.
Further, the formula for calculating the joint control amount at the next time is specifically:
Figure BDA0002161320070000027
wherein, thetad(t) represents the desired joint angle at time t,
Figure BDA0002161320070000028
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: the relative pose deviation of the tail end point and the expected point (namely the target position point) of the flexible robot, and the position deviation (noted as delta P) of the arm line vector of the arm segment in the slit and the expected 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
Figure BDA0002161320070000051
)。
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 constraintsdObtaining a generalized extended Jacobian matrix, and marking as
Figure BDA0002161320070000052
The 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 as
Figure BDA0002161320070000053
The calculation formula is expressed as:
Figure BDA0002161320070000054
in the above-mentioned formula (1),
Figure BDA0002161320070000055
indicating the desired angular velocity of the joints of the flexible robot,
Figure BDA0002161320070000056
elegance representing arm-type line constraintComparable matrix JLineExtended Jacobian matrix J with obstacle avoidance constraintsdThe pseudo-inverse of the generalized extended jacobian matrix in between,
Figure BDA0002161320070000057
representing the generalized velocity of the end point in the end coordinate system,
Figure BDA0002161320070000058
the linear velocity of the arm type is shown,
Figure BDA0002161320070000059
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:
Figure BDA0002161320070000061
in the above formula (2), (θ)2m-1,θ2m) Two joint angles of the mth arm segment are shown,
Figure BDA0002161320070000062
a homogeneous transformation matrix representing the mth arm segment,
Figure BDA0002161320070000063
respectively representing the direction vectors of the homogeneous transformation matrix in the x, y and z axes,
Figure BDA0002161320070000064
represents the sameThe position vectors of the matrix are transformed uniformly.
From this it follows that the positive kinematic equation for the entire flexible arm is expressed as:
Figure BDA0002161320070000071
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,
Figure BDA0002161320070000072
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:
Figure BDA0002161320070000073
in the above-mentioned formula (4),
Figure BDA0002161320070000074
representing the joint angular velocity of the ith arm segment.
The generalized motion velocity of the flexible arm tip can be expressed as:
Figure BDA0002161320070000075
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,
Figure BDA0002161320070000076
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:
Figure BDA0002161320070000077
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 content of the first and second substances,
Figure BDA0002161320070000078
the subscript in indicates the index from i to n, and Ref includes {0} system and { n } system.
1) When Ref is {0} system:
Figure BDA0002161320070000081
2) when Ref is { n } system:
Figure BDA0002161320070000082
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):
Figure BDA0002161320070000083
according to the coupling characteristics of adjacent four degrees of freedom in the same arm segment, equation (14) can be simplified as follows:
Figure BDA0002161320070000084
assuming {0} is the inertial system, the conventional Jacobian matrix for the entire flexible arm can be simplified as:
Figure BDA0002161320070000085
wherein the content of the first and second substances,
Figure BDA0002161320070000086
comparing the formula (15) with the formula (16), it can be seen that,
Figure BDA0002161320070000091
dimension of (a) is the Jacobian matrix J of the initial flexible manipulator m1/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 actual jacobian matrix inversion process is simplifiedAnd the efficiency of the track 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:
Figure BDA0002161320070000092
in the above formula (17), ln,iIndicates the length of the ith sub-joint of the nth arm segment,
Figure BDA0002161320070000093
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:
Figure BDA0002161320070000094
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 center
Figure BDA0002161320070000095
Enveloping 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:
Figure BDA0002161320070000096
in the above formula, the first and second carbon atoms are,
Figure BDA0002161320070000097
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
Figure BDA0002161320070000101
By differentiating the expression (20), the following expression can be obtained:
Figure BDA0002161320070000102
wherein the content of the first and second substances,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:
Figure BDA0002161320070000111
wherein the content of the first and second substances,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 line
Figure BDA0002161320070000112
Expressed as follows:
Figure BDA0002161320070000113
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:
Figure BDA0002161320070000114
order to
Figure BDA0002161320070000115
Represents the minimum distance deviation between the outer arm segment of the slit and the obstacle, DoBy taking the derivative of Θ, we can get:
Figure BDA0002161320070000116
and has:
Figure BDA0002161320070000121
Figure BDA0002161320070000122
and
Figure BDA0002161320070000123
it can be derived that:
Figure BDA0002161320070000124
the extended Jacobian matrix J of the obstacle avoidance constraint can be obtained by the derivation processdExpressed as:
Figure BDA0002161320070000125
combination formula (17), formula (22), formula (I)27) The Jacobian matrix J based on the arm-type line constraint can be obtainedLineExtended jacobian matrix J with obstacle avoidance constraintsdObtaining a generalized extended jacobian matrix
Figure BDA0002161320070000126
Expressed as:
Figure BDA0002161320070000127
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:
Figure BDA0002161320070000128
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δ、
Figure BDA0002161320070000129
An inverse kinematics solution is performed according to equation (29) above, and the corresponding joint angular velocity is calculated and expressed as:
Figure BDA0002161320070000131
in the above-mentioned formula (30),
Figure BDA0002161320070000132
representing a generalized extended jacobian matrix
Figure BDA0002161320070000133
The pseudo-inverse of (1).
Accordingly, the joint control amount at the next time is obtained from equation (30) and expressed as:
Figure BDA0002161320070000134
in the above formula, thetad(t) represents the desired joint angle at time t,
Figure BDA0002161320070000135
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
Figure BDA0002161320070000136
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:
Figure BDA0002161320070000141
wherein, Δ PeIndicates the relative positional deviation, δp1A relative position deviation threshold value is indicated,
Figure BDA0002161320070000142
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<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, 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 target position point of the flexible robot, the position deviation of an arm profile line vector and an expected arm profile line vector of the slit inner arm section 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 at the next time 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:
Figure FDA0002771374590000011
wherein the content of the first and second substances,
Figure FDA0002771374590000012
representing a desired angular velocity of a joint of the flexible robot,
Figure FDA0002771374590000013
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,
Figure FDA0002771374590000014
representing the generalized velocity of the end point in an end coordinate system,
Figure FDA0002771374590000015
the linear velocity of the arm type is indicated,
Figure FDA0002771374590000016
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:
Figure FDA0002771374590000021
wherein, thetad(t) represents the desired joint angle at time t,
Figure FDA0002771374590000022
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 target position point of the flexible robot, the position deviation of an arm profile line vector and an expected arm profile line vector of the slit inner arm section 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 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.
CN201910733184.5A 2019-08-09 2019-08-09 Arm-shaped line constraint flexible robot track planning method and device Active CN110561419B (en)

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 CN110561419A (en) 2019-12-13
CN110561419B true 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)

Families Citing this family (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111300420B (en) * 2020-03-16 2021-08-10 大连理工大学 Method for solving minimum path of joint space corner of mechanical arm
CN112862812B (en) * 2021-03-10 2023-04-07 中山大学 Method, system and device for optimizing operation space trajectory of flexible robot
CN112936287B (en) * 2021-03-19 2022-09-27 中山大学 Flexible robot control method and device based on dynamics iterative learning
CN113021408B (en) * 2021-03-23 2022-08-05 中联重科股份有限公司 Method and device for compensating deflection of arm support and method and device for controlling arm support

Family Cites Families (7)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US7708741B1 (en) * 2001-08-28 2010-05-04 Marctec, Llc Method of preparing bones for knee replacement surgery
US8992422B2 (en) * 2006-03-23 2015-03-31 Ethicon Endo-Surgery, Inc. Robotically-controlled endoscopic accessory channel
CN102079093A (en) * 2010-12-27 2011-06-01 上海大学 Novel flexible body feeding method
CN106956260B (en) * 2017-03-31 2019-02-19 浙江大学 A kind of inverse kinematics method of multi-joint mechanical arm flat serpentine track movement
US10850395B2 (en) * 2017-05-19 2020-12-01 Stc.Unm System and methods for multiple-place swarm foraging with dynamic depots
CN109303610B (en) * 2017-07-27 2021-05-18 赛诺微医疗科技(北京)有限公司 Surgical instrument clamping mechanism, end effector and surgical mechanical arm adopting same
CN109991847B (en) * 2019-01-11 2022-04-22 天津理工大学 Approximate time optimal trajectory planning method for flexible multi-body robot

Also Published As

Publication number Publication date
CN110561419A (en) 2019-12-13

Similar Documents

Publication Publication Date Title
CN110561419B (en) Arm-shaped line constraint flexible robot track planning method and device
Zhu et al. A convex optimization approach to smooth trajectories for motion planning with car-like robots
US8560122B2 (en) Teaching and playback method based on control of redundancy resolution for robot and computer-readable medium controlling the same
CN111300425B (en) Super-redundant mechanical arm tail end track motion planning method
Best et al. Real-time reciprocal collision avoidance with elliptical agents
CN110561420B (en) Arm profile constraint flexible robot track planning method and device
CN109397271B (en) 7-degree-of-freedom anthropomorphic mechanical arm and control method and system thereof
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
CN113146600B (en) Flexible robot trajectory planning method and device based on kinematics iterative learning control
CN107538493A (en) A kind of industrial robot method for planning track for avoiding cable interference constraint
CN108772835B (en) Obstacle and physical limit avoiding method
CN114147708B (en) Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm
CN105234930B (en) Control method of motion of redundant mechanical arm based on configuration plane
Jia et al. MDA+ RRT: A general approach for resolving the problem of angle constraint for hyper-redundant manipulator
CN115416016A (en) Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method
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
Muhammed et al. Towards Comparison and Real Time Implementation of Path Planning Methods for 2R Planar Manipulator with Obstacles Avoidance.
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.
Ji et al. E-RRT*: Path Planning for Hyper-Redundant Manipulators
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
Gu et al. Dexterous obstacle-avoidance motion control of Rope Driven Snake Manipulator based on the bionic path following

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