CN115674195A - Online arm shape planning method for flexible mechanical arm - Google Patents
Online arm shape planning method for flexible mechanical arm Download PDFInfo
- Publication number
- CN115674195A CN115674195A CN202211283940.7A CN202211283940A CN115674195A CN 115674195 A CN115674195 A CN 115674195A CN 202211283940 A CN202211283940 A CN 202211283940A CN 115674195 A CN115674195 A CN 115674195A
- Authority
- CN
- China
- Prior art keywords
- arm
- flexible mechanical
- mechanical arm
- node
- shape
- 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 52
- 230000004888 barrier function Effects 0.000 claims abstract description 22
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 17
- 238000005070 sampling Methods 0.000 claims description 8
- 238000005452 bending Methods 0.000 claims description 3
- 230000008569 process Effects 0.000 description 11
- 238000004364 calculation method Methods 0.000 description 10
- 238000010586 diagram Methods 0.000 description 6
- 150000001875 compounds Chemical class 0.000 description 4
- 238000010276 construction Methods 0.000 description 3
- 238000013507 mapping Methods 0.000 description 3
- 238000013459 approach Methods 0.000 description 2
- 230000007274 generation of a signal involved in cell-cell signaling Effects 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- QEIQEORTEYHSJH-UHFFFAOYSA-N Armin Natural products C1=CC(=O)OC2=C(O)C(OCC(CCO)C)=CC=C21 QEIQEORTEYHSJH-UHFFFAOYSA-N 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000007246 mechanism Effects 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
Images
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses an online arm shape planning method for a flexible mechanical arm, which comprises the steps of obtaining an initial arm shape and a target arm shape of the flexible mechanical arm through a flexible mechanical arm inverse kinematics model according to an initial tail end pose and a target tail end pose of the flexible mechanical arm; and determining a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape from a space containing the flexible mechanical arm and the barrier based on a fast-expanding random tree algorithm to form a collision-free path planning scheme of the flexible mechanical arm and generate an online collision-free map. The method solves the problem of online planning of the arm shape of the flexible mechanical arm with the deformation characteristic.
Description
Technical Field
The application belongs to the technical field of automatic control of robots, relates to flexible mechanical arm shape control, and particularly relates to the problem of flexible mechanical arm shape planning in a motion process in a specific environment.
Background
On the premise of giving an initial state and a target state, the flexible mechanical arm can solve and calculate a driving space solution of the flexible mechanical arm through an inverse kinematics model, and then the flexible mechanical arm is controlled to move to a specified position. However, during the movement of a specific environment (for example, a space has an obstacle), how to perform arm shape planning to control the arm shape of the flexible mechanical arm so that the flexible mechanical arm does not collide with the environment during the movement and can complete the movement from the initial state to the target state is a problem to be solved.
The application document with the application number of CN202110500442.2 discloses a redundant mechanical arm obstacle avoidance trajectory planning method based on an improved fast-expansion random tree.
However, since there is a linearized mathematical relationship between the spatial arm shape and the joint angle of the rigid mechanical arm, there is no universality in the process of mapping the obstacle to the joint space of the mechanical arm, so the above redundant mechanical arm obstacle avoidance trajectory planning method is only applicable to the rigid mechanical arm, that is, the method cannot be applied to a flexible mechanical arm device whose mechanical arm body is deformable. Moreover, because the method belongs to off-line track planning, a free-collision map describing the spatial position relationship between the mechanical arm and the barrier must be constructed first, so that arm shape planning can be performed in the two-dimensional grid map by using the existing planning scheme, and the acquisition of the collision-free space is a link which is time-consuming and unnecessary in calculation, so that the defect of large calculation amount exists.
Disclosure of Invention
The invention aims to provide an online arm shape planning method for a flexible mechanical arm, which solves the problem of arm shape planning of the flexible mechanical arm with deformation characteristic under the guidance of a tail end pose in an obstacle space.
In order to achieve the purpose, the invention adopts the following technical scheme to realize.
The flexible arm online arm shape planning method provided by the invention adopts RRT to plan a collision-free path of an arm shape space and simultaneously generates an online collision-free map; and (4) combining the flexible arm inverse kinematics model to obtain a flexible arm control signal, and realizing collision-free movement of the flexible arm in the terminal pose guiding process.
Based on the analysis, the online arm shape planning method for the flexible mechanical arm provided by the invention specifically comprises the following steps of:
s1, obtaining an initial arm shape and a target arm shape of the flexible mechanical arm through an inverse kinematics model of the flexible mechanical arm according to the initial tail end pose and the target tail end pose of the flexible mechanical arm;
s2, determining a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape from a space containing the flexible mechanical arm and the barrier based on a fast-expanding random tree algorithm, forming a collision-free path planning scheme of the flexible mechanical arm, and simultaneously generating an online collision-free map; taking the arm shape of a flexible mechanical arm as a node in a random tree to be generated; the method comprises the following steps:
s20, initializing a random tree;
s21, sampling is carried out in a space containing the flexible mechanical arm and the barrier, and a random node q is obtained rand ；
S22, searching for a random node q in a random tree rand Nearest near node q near ；
S23 based on random node q rand Near node q near And the set extension distance is obtained to obtain a candidate node q pre-new ；
S24 judging candidate node q pre-new And near node q near Whether the area between covers the flexible mechanical arm and the barrier; if the candidate node q pre-new And near node q near If the area between the two nodes is not covered by the flexible mechanical arm and the barrier, the candidate node is taken as an effective node q new Adding the tree into a random tree and entering step S25; otherwise, returning to the step S21;
s25, judging the target arm shape and the generated effective node q new Whether or not to satisfy q goal -q new ||≥q err If not, obtaining a connection primerA plurality of effective arm shape sequences between the initial arm shape and the target arm shape form a collision-free path planning scheme of the flexible mechanical arm by using the obtained plurality of arm shape sequences, and simultaneously, an online collision-free map comprising the plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape is constructed; if yes, go to step S26;
s26, increasing the iteration frequency by 1, and judging whether the maximum iteration frequency is reached; if so, the collision-free path planning fails, the step S20 is returned, and the random tree is initialized again; otherwise, returning to the step S21, and simultaneously constructing the node q new And its previously generated online collision-free map of valid nodes and initial arms.
In the above step S1, the flexible manipulator inverse kinematics model is not particularly limited, and the flexible manipulator inverse kinematics model used herein is as follows:
in the formula (X) j ,Y j ,Z j ) Representing an end pose;
represents an arm deformation amount of the flexible robot arm, wherein theta j Is the bending angle of the j-th segment flexible arm,
is the rotation angle of the flexible arm of the j-th segment. The arm shape of the flexible mechanical arm is represented by the arm shape variation of the flexible mechanical arm.
According to the initial end pose and the target end pose of the flexible mechanical arm, the initial arm shape and the target arm shape of the flexible mechanical arm can be obtained by solving the inverse kinematics model.
In the step S2, the purpose is to determine a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape based on a rapid-expansion Random tree algorithm (RRT algorithm) and spatial information (including size and position information of the flexible mechanical arm and the obstacle) of the flexible mechanical arm and the obstacle, so as to form a collision-free path planning scheme for the flexible mechanical arm.
In the fast-expanding random tree algorithm, T = (V, E) denotes a random tree, V denotes a node set in the random tree, and E denotes an edge between nodes in the node set. The method is based on the initial arm shape, the target arm shape, the flexible mechanical arm and the barrier and some initialization parameters (including the set expansion probability p, the expansion distance d and the final node error q) of the RRT algorithm in the space containing the flexible mechanical arm and the barrier err And the maximum number of iterations N RRT ) And then obtaining candidate nodes according to the traditional RRT algorithm execution idea (SampleFree, nerest and Steer), and judging the effectiveness of the candidate nodes.
In the step S21, based on the current arm shape, the target arm shape, the spatial information of the flexible mechanical arm and the obstacle, and the set expansion probability, sampling is performed on the flexible mechanical arm on-line collision-free map by using a SampleFree () function, so as to obtain a random node q rand . The current arm shape is an effective node obtained in the previous iteration; at the initial moment, the current arm shape is the initial arm shape.
In the above step S22, the random node q is obtained based on the random tree and the step S21 rand Obtaining a random node q through a nerest () function rand Nearest near node q near 。
In the above step S23, the node q is based on the random node q rand Near node q near And the set extended distance d is obtained through a Steer () function to be positioned at the node q rand And node q near Candidate node q therebetween pre-new 。
In the above step S24, the candidate node q is selected pre-new In the validity judging process, the flexible mechanical arm, the space information of the barrier and the node q are combined near Then, the candidate node q can be paired pre-new The effectiveness judgment can be greatly carried outAnd the judgment efficiency is improved. If the candidate node q pre-new And node q near If the flexible mechanical arm and the barrier are not covered in the middle area, adding the candidate node as an effective node into the random tree, and entering the step S25; otherwise, the generated node is not qualified, and the step S21 needs to be returned to for sampling again.
In the above steps S25 and S26, in the process of using the target arm as the guide, the candidate node to be searched gradually approaches the target arm q according to the preset precision goal And finally-form an RRT random tree T = (V, E). The termination conditions include whether the maximum number of iterations is reached (for determining whether planning has failed) and whether the target arm shape and the generated valid node (i.e., valid arm shape) satisfy | | q goal -q new ||≥q err (for determining whether planning is complete).
In step S25, when the target arm shape and the generated valid node (i.e., valid arm shape) satisfy | | q goal -q new ||≥q err It is indicated that the approximation of the generated effective arm shape to the target arm shape cannot meet the set precision requirement, and an effective arm shape which is closer to the target arm shape needs to be further generated. When not satisfied, | | q goal -q new ||＜q err And the generated effective arm shape is close to the target arm shape and meets the set precision requirement, and the searching of the effective arm shape can be stopped. The flexible mechanical arm driving signal can be further obtained based on a collision-free path planning scheme of the flexible mechanical arm, and the flexible mechanical arm can be driven. Meanwhile, an online collision-free map containing a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape can be constructed, so that the online collision-free map is used for assisting an operator to check the movement of the flexible mechanical arm and completing the closed-loop control of the flexible mechanical arm.
In step S26, it is determined whether the planning has failed. When the maximum number of iterations is reached, which indicates that the current planning scheme fails, the planning needs to be restarted from the initial arm shape of the flexible mechanical arm. When the maximum iteration times are not reached, the flexible mechanical arm path can be planned continuously; in this case, the current valid section can be obtained directly based onPoint q new Acquiring a real-time driving signal of the flexible mechanical arm to complete the real-time driving of the flexible mechanical arm; meanwhile, the node q can be constructed new The online collision-free map of the effective nodes and the initial arm shapes generated before is used for assisting an operator to check the movement of the flexible mechanical arm and completing the closed-loop control of the flexible mechanical arm; and then, returning to the step S21, continuously searching an effective arm shape sequence meeting the requirement in the exploration area until the tail end of the flexible mechanical arm reaches the target tail end pose. In the step, the flexible mechanical arm on-line collision-free map is generated while the effective arm-shaped node is generated, so that a large amount of calculation time cost can be saved, and the planning efficiency is improved.
Based on a flexible mechanical arm collision-free path planning scheme, a flexible mechanical arm driving signal is obtained through a flexible mechanical arm inverse kinematics model. In the invention, based on a flexible mechanical arm collision-free path planning scheme, the arm shape variable between two adjacent arm shapes can be obtained, and then the rotation angular velocity is obtained through the inverse kinematics relationship between a joint space and a driving space, as shown in the following formula:
in the formula (I), the compound is shown in the specification,
the total variation of the rotation angles of the single-section flexible mechanical arm around the horizontal rotating shaft and the vertical rotating shaft of the universal joint respectively;
the corresponding rotation angular speed is represented in a quantitative mode by the flexible mechanical arm driving space; t is t C 、t T Representing the corresponding time of two adjacent arms;
arm shape variable n of flexible robot arm respectively representing adjacent two arm shapes j Representing the number of universal joints of a single-section flexible mechanical arm。
In the invention, the flexible mechanical arm is driven to move by driving wires in the flexible mechanical arm. Therefore, based on the rotation angular velocity of the flexible mechanical arm, the rotation speed (i.e. the driving signal) of the screw rod driving motor connected with the driving wire can be obtained according to the following formula, and the driving of the flexible mechanical arm is further realized:
in the formula, D represents a screw lead, namely the linear distance of the nut on the screw rod which rotates for one circle.
In the formula (I), the compound is shown in the specification,
and the length variation of the mth driving wire arranged on the jth section of flexible mechanical arm in a specified time period between two adjacent arm shapes is shown.
Similar to the real-time driving signal generation process given above, the method is based on the current effective node q new And the previous effective node, according to the explanation given above, the rotation speed of the screw rod driving motor in the designated time period between the two adjacent arm shapes can be obtained, and the flexible mechanical arm can be driven.
The invention mainly aims at the problem of obstacle avoidance of the barrier space of the flexible mechanical arm, and adopts an online arm shape planning method in the arm shape space, and the online arm shape planning method of the flexible mechanical arm provided by the invention has the following beneficial effects:
1) Firstly, acquiring an initial arm shape and a target arm shape of a flexible mechanical arm based on an initial tail end pose and a target tail end pose of the flexible mechanical arm; then, a plurality of effective arm shape sequences of the initial arm shape and the target arm shape of the flexible mechanical arm are obtained through a fast expansion random tree algorithm to form a collision-free path planning scheme of the local flexible mechanical arm, so that the problem of online planning of the arm shape of the flexible mechanical arm with deformation characteristics is solved;
2) According to the invention, a complete free-collision map does not need to be constructed in advance, and a real-time online collision-free map can be constructed by combining the previously generated effective arm nodes and the initial arm shape while the effective arm shape nodes are obtained, so that a large amount of calculation time cost is saved;
3) The method gradually approaches the target arm shape by continuously constructing the local effective arm shape, so that the accuracy of the collision-free path planning scheme of the flexible mechanical arm can be improved;
4) According to the method, the operator can be assisted to check the movement of the flexible mechanical arm through the constructed real-time online collision-free map, and the closed-loop control of the flexible mechanical arm is completed.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings needed to be used in the embodiments will be briefly described below, it should be understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and for those skilled in the art, other related drawings can be obtained according to the drawings without inventive efforts.
FIG. 1 is a schematic diagram of a physical entity of a flexible mechanical arm; wherein, (a) is a schematic overall structure, (b) is a schematic male ring diagram, and (c) is a schematic female ring diagram.
Fig. 2 shows the relationship between the arm shape space of the flexible manipulator and the arm shape configuration sequence space.
Fig. 3 is a schematic flow chart of an online arm shape planning method for a flexible manipulator.
Fig. 4 is a diagram of a motion vector of the flexible mechanical arm.
Fig. 5 is an obstacle avoidance scene of the multi-segment flexible mechanical arm constructed in the application example.
Fig. 6 shows the results of the online arm shape planning of a segment of flexible mechanical arm in a multi-obstacle space in an application example.
Detailed Description
In order to make the objects, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below with reference to the drawings in the embodiments of the present invention, and it is obvious that the described embodiments are some, but not all embodiments of the present invention. The components of embodiments of the present invention generally described and illustrated in the figures herein may be arranged and designed in a wide variety of different configurations.
Thus, the following detailed description of the embodiments of the present invention, as presented in the figures, is not intended to limit the scope of the invention, as claimed, but is merely representative of selected embodiments of the invention. All other embodiments, which can be obtained by a person skilled in the art without inventive step based on the embodiments of the present invention, are within the scope of protection of the present invention.
The flexible mechanical arm physical entity addressed in the following embodiments, as shown in fig. 1, includes a flexible arm first section 1, a flexible arm second section 2, and an elastic support 7 passing through the flexible arm first section and the flexible arm second section; the second segment of the flexible arm ends in an end joint 5 (for characterizing the end pose). The first section 1 of flexible arm is the same with the second section 2 of flexible arm structure, all includes a plurality of universal joints 3 and establishes ties three drive silk together with a plurality of universal joints. Two adjacent universal joints are connected in a Hooke hinge mode. One end of a driving wire in the first section of the flexible mechanical arm penetrates through the base 6 to be connected with a corresponding driving motor, and the other end of the driving wire is fixedly connected with the connecting body 4. One end of a driving wire in the second section 2 of the flexible arm penetrates through the base 6 to be connected with a corresponding driving motor, and the other end of the driving wire is fixedly connected with the tail end joint 5.
As shown in fig. 1, the gimbal 3 joint has two structures: the male ring 31 and the female ring 32 are arranged in a staggered mode. Bosses 312 are respectively arranged on the ring surfaces at the two sides of the male ring 31, and the boss positions on the two ring surfaces are mutually vertical; grooves 322 are respectively formed in the ring surfaces of the two sides of the female ring 32, and the positions of the grooves in the two ring surfaces are perpendicular to each other, so that the two sides of the universal joint and the adjacent universal joint rotate along a horizontal rotating shaft (A shaft) and a vertical rotating shaft (B shaft). The groove arranged on the ring surface of the female ring is matched with the boss arranged on the ring surface of the male ring opposite to the groove. The end surfaces of the two sides of the female ring and the end surfaces of the two sides of the male ring are designed by inclined planes, so that the male ring can have a certain rotation angle around the center of the groove; specifically, the end surfaces on two sides of the female ring and the end surfaces on two sides of the male ring are both provided with positioning surfaces which incline from the middle to two sides, so that the first flexible arm/the second flexible arm formed by the female ring and the male ring can move in a larger moving range.
The connecting body 4 and the end joint 5 are matched with the end surface structure of a universal joint (a male ring 31 or a female ring 32) connected with the connecting body. When the male ring 31 is connected with the connecting body 4/the tail end joint 5, the end surface of the male ring is consistent with the end surface of the female ring in structure; when the female ring 32 is connected with the connecting body 4/the end joint 5, the end surface of the female ring is consistent with the end surface structure of the male ring.
Example 1
Here, the relevant definitions of the flexible robot arm shape planning problem are unified first. Definition of
For a global arm space of the flexible manipulator without any constraints, j =1,2 represents two segments of the flexible manipulator.
Representing the total effective free arm space of the flexible robot arm in the obstacle space, C in the conventional arm planning scheme free All need to be acquired in advance, Q = (Q) 1 ,q 2 ,...,q s ) Is defined as a space C free A sequence of all the arm configurations within the arm,
s is the number of total sequences. The traditional arm planning scheme needs to continue to search for solutions to Q from Q init →q goal A number of valid arm sequences of q init ,q goal ∈C free And defining the sequence as
T is the total number of valid arm shape sequences to complete the planned arm shape sequence. The scheme of the invention adopts an online mode and can directly follow
In looking for Q free . As shown in fig. 2, a block diagram of the relationship between each arm shape space and the arm shape configuration sequence space is shown, where an ellipse represents the arm shape space and a rectangle represents the arm shape configuration sequence space. The traditional off-line arm shape planning scheme needs to complete the calculation processes from (1), (2), (4) and (5), firstly determines the global arm shape space C of the flexible mechanical arm without any constraint, and then determines the total effective free arm shape space C of the flexible mechanical arm in the obstacle space free Then determining C free All arms in (1) configure sequence space Q, and then search Q for a solution Q init →q goal A plurality of valid arm sequences Q free 。
The online arm shape planning method of the flexible mechanical arm only needs to pass through (1), (3) and (5), so that not only is a great amount of calculation time from (1) to (2) saved, but also the (5) can be directly obtained by searching from the more compact (3), and therefore, the searching efficiency and the searching accuracy are improved when the arm shape planning scheme is greatly reduced in calculation.
The invention provides an online arm shape planning method of a flexible mechanical arm by combining an algorithm RRT, which comprises the following steps as shown in figure 3:
s1, according to the initial tail end pose and the target tail end pose of the flexible mechanical arm, the initial arm shape and the target arm shape of the flexible mechanical arm are obtained through a flexible mechanical arm inverse kinematics model.
In this embodiment, the flexible manipulator inverse kinematics model used is as follows:
wherein (X) j ,Y j ,Z j ) Representing an end pose;
denotes an arm deformation amount of the flexible robot arm, wherein θ j Is the bending angle of the j-th segment of flexible arm,
is the rotation angle of the flexible arm of the j-th segment. The arm shape of the flexible mechanical arm is represented by the arm shape variation of the flexible mechanical arm.
According to the initial end position and pose of the flexible mechanical arm
And object end position and pose
And solving the inverse kinematics model to obtain the initial arm shape and the target arm shape of the flexible mechanical arm.
S2, determining a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape from a space containing the flexible mechanical arm and the barrier based on a fast-expansion random tree algorithm, forming a collision-free path planning scheme of the flexible mechanical arm, and simultaneously generating an online collision-free map.
The method aims to determine a plurality of effective arm shape sequences connecting an initial arm shape and a target arm shape based on a fast expansion random tree algorithm and space information (including sizes and position information of the flexible mechanical arm and an obstacle) of the flexible mechanical arm and the obstacle, and form a collision-free path planning scheme of the flexible mechanical arm.
In the fast-expanding random tree algorithm, T = (V, E) denotes a random tree, V denotes a node set in the random tree, and E denotes an edge between nodes in the node set. The invention takes the arm shape of the flexible mechanical arm as a node in a tree, and is based on the initial arm shape, the target arm shape, the space information of the flexible mechanical arm and an obstacle and some initialization parameters (including the set expansion probability p, the expansion distance d and the final node error q) of the RRT algorithm err And the maximum number of iterations N RRT ) Then obtaining candidate nodes according to the traditional RRT algorithm execution thought (Samplefree, nerest and Steer), and carrying out operation on the candidate nodesAnd (6) judging the effectiveness. The RRT algorithm pseudo code is shown in Table 1.
With reference to table 1, the present process specifically comprises the following sub-steps:
s20 initializes the random tree.
Here, the method includes the steps of calculating a random tree space (i.e. a space containing the flexible mechanical arm and the obstacle) and initialization parameters (including the set expansion probability p, the expansion distance d and the final node error q) err And the maximum number of iterations N RRT ) Initialization is carried out, and an initial arm-shaped node and a target arm-shaped node are introduced into the random tree.
S21, sampling is carried out in a space containing the flexible mechanical arm and the barrier, and a random node q is obtained rand 。
In the step, based on the current arm shape, the target arm shape, the space information of the flexible mechanical arm and the obstacle and the set expansion probability, sampling is carried out on an online collision-free map of the flexible mechanical arm through a SampleFree () function, and a random node q is obtained rand . The current arm shape is an effective node obtained in the previous iteration; at the initial moment, the current arm shape is the initial arm shape.
S22, searching for a node q in the random tree rand Nearest near node q near 。
In this step, a random node q is obtained based on the random tree and step S21 rand Obtaining a random node q through a nerest () function rand Nearest near node q near 。
S23 based on random node q rand Near node q near And the set extension distance is obtained to obtain a candidate node q pre-new 。
In this step, based on the random node q rand Near node q near And the set extension distance d is obtained through a Steer () function and positioned at the node q rand And node q near Candidate node q therebetween pre-new 。
S24 judging candidate node q pre-new And near node q near Whether the area between covers the flexible mechanical arm and the barrier; if the candidate node q pre-new And near node q near If the flexible mechanical arm and the barrier are not covered in the middle area, the candidate node is taken as an effective node q new Adding the tree into a random tree, and entering a step S25; otherwise, the procedure returns to step S21.
At the pair candidate node q pre-new In the effective free arm shape space of the more compact flexible mechanical arm
In combination with the flexible mechanical arm, the space information of the barrier and the node q near Then, the candidate node q can be paired pre-new And effectiveness judgment is carried out, and the judgment efficiency can be greatly improved. If the candidate node q pre-new And node q near If the flexible mechanical arm and the barrier are not covered in the middle area, adding the candidate node as an effective node into the random tree, and entering the step S25; otherwise, the generated node does not meet the requirement, and the step S21 needs to be returned to, and sampling is performed again.
S25, judging the target arm shape and the generated effective node q new Whether or not to satisfy q goal -q new ||≥q err If not, obtaining a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape, forming a collision-free path planning scheme of the flexible mechanical arm by using the obtained plurality of arm shape sequences, and simultaneously constructing an online collision-free map comprising the plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape; if yes, the process proceeds to step S26.
When the target arm shape and the generated effective node (namely the effective arm shape) meet | | | q goal -q new ||≥q err It is indicated that the approximation of the generated effective arm shape to the target arm shape cannot meet the set precision requirement, and an effective arm shape which is closer to the target arm shape needs to be further generated. When not satisfied, | | q goal -q new ||＜q err And the generated effective arm shape is close to the target arm shape and meets the set precision requirement, and the searching of the effective arm shape can be stopped. The obtained V is a plurality of discrete effective parts for connecting the initial arm shape and the target arm shapeAn arm-shaped sequence.
The flexible mechanical arm driving signal can be further obtained based on a collision-free path planning scheme of the flexible mechanical arm, and the flexible mechanical arm can be driven. Meanwhile, an online collision-free map containing a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape can be constructed, and the online collision-free map is used for assisting an operator to check the movement of the flexible mechanical arm and completing the closed-loop control of the flexible mechanical arm.
S26, increasing the iteration frequency by 1, and judging whether the maximum iteration frequency is reached; if yes, the collision-free path planning fails, the step S20 is returned, and the random tree is initialized again; otherwise, returning to the step S21, and simultaneously constructing the node q new And their previously generated online collision-free maps of valid nodes and initial arms.
This step is to determine whether the planning fails. After the iteration number is increased by 1 (i.e. k ← k + 1), when the maximum iteration number is reached, it indicates that the current planning scheme is failed, and planning needs to be restarted from the initial arm shape of the flexible mechanical arm. When the maximum iteration times are not reached, the flexible mechanical arm path can be planned continuously; in this case, the current valid node q can be obtained based on the current valid node q new Acquiring a real-time driving signal of the flexible mechanical arm to complete the real-time driving of the flexible mechanical arm; meanwhile, the method can also construct a node q new The online collision-free map of the effective nodes and the initial arm shapes generated before is used for assisting an operator to check the movement of the flexible mechanical arm and finishing the closed-loop control of the flexible mechanical arm; and then, returning to the step S21, continuously searching an effective arm shape sequence meeting the requirement in the exploration area until the tail end of the flexible mechanical arm reaches the target tail end pose. The V obtained at this time is a plurality of discrete effective arm shape sequences connecting the initial arm shape and the current effective node.
The flexible mechanical arm driving signal can be further obtained based on a collision-free path planning scheme of the flexible mechanical arm, and the flexible mechanical arm can be driven.
TABLE 1 Flexible mechanical arm on-line arm shape planning RRT algorithm pseudo code
Based on a collision-free path planning scheme of the flexible mechanical arm, an arm shape variable between two adjacent arm shapes can be obtained. As shown in fig. 4, the rotational angular velocity is then obtained from the inverse kinematics relationship of the joint space and the drive space, as shown in the following equation:
in the formula (I), the compound is shown in the specification,
the total variation of the rotation angles of the single-section flexible mechanical arm around the horizontal rotating shaft and the vertical rotating shaft of the universal joint respectively;
the method is a quantitative representation of the driving space of the flexible mechanical arm for corresponding rotation angular velocity; t is t C 、t T Representing the corresponding time of two adjacent arms;
the arm shape variables, n, of the flexible arm representing the shape of two adjacent arms respectively j Representing the number of single segment flexible mechanical arm universal joints.
In the invention, the flexible mechanical arm is driven to move by driving wires in the flexible mechanical arm. Therefore, based on the rotation angular velocity of the flexible mechanical arm, the rotation speed (i.e. the driving signal) of the screw rod driving motor connected with the driving wire can be obtained according to the following formula, and the driving of the flexible mechanical arm is further realized:
wherein D represents the lead of the lead screw, i.e. the nut on the lead screw rotates one turnThe linear distance traveled;
representing the drive wire speed.
In the formula (I), the compound is shown in the specification,
the length variation of the m-th driving wire arranged on the j-th section of flexible mechanical arm in the time period between two adjacent arm shapes is shown, and m =1,2,3.
r is the radius of a reference circle where the via holes through which the 3 driving wires of each section of flexible mechanical arm pass.
by substituting the equations (6), (7) and (8) into the equation (5)
The expression (c).
And then the rotating speed of a screw rod driving motor connected with the driving wire, namely a driving signal of the flexible mechanical arm can be obtained through a formula (4), so that the flexible mechanical arm is driven.
Similar to the real-time driving signal generation process given above, the method is based on the current effective node q new And the previous effective node, according to the explanation given above, the rotation speed of the screw rod driving motor in the designated time period between the two adjacent arm shapes can be obtained, and the flexible mechanical arm can be driven. Furthermore, the end pose of the flexible mechanical arm is monitored in real time (for example, by an IMU inertial measurement unit), so that the effectiveness of the flexible mechanical arm online arm shape planning method provided by the invention can be detected.
Application example
In order to verify the effectiveness of the online arm-shaped planning method for the flexible mechanical arm, an obstacle avoidance scene of a multi-segment flexible arm in a multi-obstacle space is constructed as shown in fig. 5 (parameters are shown in tables 2 and 3), the flexible mechanical arm needs to complete movement from an initial end pose to a target end pose, and two obstacles in a double-segment arm-shaped avoidance space need to be planned in the movement process. According to the online arm shape planning method for the flexible mechanical arm provided by the embodiment 1, online planning of the flexible mechanical arm is performed, fig. 6 shows an online arm shape planning result of one section of the flexible mechanical arm in a multi-obstacle space, a red curve in the diagram is a movement track of the corresponding section of the flexible mechanical arm in the arm shape space after the planning is completed, and sporadic black block areas are results of online map building.
Table 4 shows the comparison of the computing time of the present application example and the conventional offline scheme. See CN202110500422.2 for traditional off-line scheme implementation. As can be seen from Table 4, the application example can greatly shorten the arm shape planning time of the flexible mechanical arm, improve the arm shape planning efficiency and meet the online real-time planning requirement of the flexible mechanical arm.
TABLE 2 two-segment Flexible robot arm parameters
TABLE 3 obstacle parameters
TABLE 4 comparison of the time-consuming calculations for two different arm planning schemes
Note: the above-mentioned mean plan refers to the mean total planning time calculated by 10 times of simulation;
the mapping average refers to the average time of mapping links calculated by 10 times of simulation.
Therefore, the traditional arm shape planning scheme is only suitable for rigid mechanical arms with limited degrees of freedom or multi-redundancy mechanical arms, and the method depends on the rotation angle data of the rigid joint angle of the mechanical arm, so that the method cannot be suitable for the flexible mechanical arm in the invention; the traditional arm-shaped planning scheme has the advantages that a free-collision map describing the spatial position relationship between the mechanical arm and the barrier is required to be constructed firstly, the arm-shaped planning can be carried out in the two-dimensional grid map by utilizing the existing planning scheme, the calculation is time-consuming, the arm-shaped planning is completed by utilizing an online mechanism, the construction of a local map can be realized, the extra time for constructing the grid map is not required, and therefore, a large amount of time for constructing the grid map by calculation is saved.
In summary, the online arm shape planning method for the flexible manipulator provided by the invention has the following greatest characteristics:
1) The traditional off-line collision-free map construction is improved into an on-line construction method, so that the overall planning efficiency is improved;
2) The problem of planning the arm shape of a flexible mechanical arm with similar deformation characteristics is solved.
The present invention has been described in terms of the preferred embodiment, and it is not intended to be limited to the embodiment. Any modification, equivalent replacement, or improvement made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (6)
1. An online arm shape planning method for a flexible mechanical arm is characterized by comprising the following steps:
s1, obtaining an initial arm shape and a target arm shape of the flexible mechanical arm through an inverse kinematics model of the flexible mechanical arm according to the initial tail end pose and the target tail end pose of the flexible mechanical arm;
s2, determining a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape from a space containing the flexible mechanical arm and the barrier based on a fast-expanding random tree algorithm, forming a collision-free path planning scheme of the flexible mechanical arm, and simultaneously generating an online collision-free map; taking the arm shape of a flexible mechanical arm as a node in a random tree to be generated; the method comprises the following steps:
s20, initializing a random tree;
s21, sampling is carried out in a space containing the flexible mechanical arm and the barrier, and a random node q is obtained rand ；
S22, searching for a node q in the random tree rand Nearest near node q near ；
S23 based on random node q rand Near node q near And the set extension distance to obtain a candidate node q pre-new ；
S24 judging candidate node q pre-new And near node q near Whether the area between covers the flexible mechanical arm and the barrier; if the candidate node q pre-new And near node q near If the area between the two nodes is not covered by the flexible mechanical arm and the barrier, the candidate node is taken as an effective node q new Adding the tree into a random tree and entering step S25; otherwise, returning to the step S21;
s25 judging validity of target arm shape and generationNode q new Whether or not to satisfy q goal -q new ||≥q err If the current position does not meet the preset requirement, obtaining a plurality of effective arm shape sequences connecting the initial arm shape and the target arm shape, forming a collision-free path planning scheme of the flexible mechanical arm by using the obtained arm shape sequences, and simultaneously constructing an online collision-free map comprising the effective arm shape sequences connecting the initial arm shape and the target arm shape; if yes, go to step S26;
s26, increasing the iteration frequency by 1, and judging whether the maximum iteration frequency is reached; if so, the collision-free path planning fails, the step S20 is returned, and the random tree is initialized again; otherwise, returning to the step S21, and simultaneously constructing the node q new And their previously generated online collision-free maps of valid nodes and initial arms.
2. The method for planning the arm shape of the flexible mechanical arm on line according to claim 1, wherein in step S1, the inverse kinematics model of the flexible mechanical arm is as follows:
wherein (X) j ,Y j ,Z j ) Representing an end pose;
denotes an arm deformation amount of the flexible robot arm, wherein θ j Is the bending angle of the j-th segment of flexible arm,
is the rotation angle of the j-th flexible arm; and characterizing the arm shape of the flexible mechanical arm by the arm shape deformation of the flexible mechanical arm.
3. The method of claim 1, wherein in step S21, sampling is performed on the flexible robot on-line collision-free map through SampleFree () function based on the current arm shape, the target arm shape, the spatial information between the flexible robot and the obstacle, and the set extension probability to obtain a random node q rand 。
4. The method for planning the arm shape of a flexible mechanical arm on line according to claim 1, wherein in step S22, the random node q is obtained based on the random tree and step S21 rand Obtaining a random node q through a nerest () function rand Nearest near node q near 。
5. The method for planning the arm shape of a flexible mechanical arm on line according to claim 1, wherein in step S23, the method is based on a random node q rand Near node q near And the set extension distance d is obtained through a Steer () function and positioned at the node q rand And node q near Candidate node q therebetween pre-new 。
6. The method for planning the arm shape of a flexible mechanical arm on line according to claim 1, wherein in step S25, the driving signal of the flexible mechanical arm is obtained through an inverse kinematics model of the flexible mechanical arm based on a collision-free path planning scheme of the flexible mechanical arm.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211283940.7A CN115674195B (en) | 2022-10-20 | Online arm shape planning method for flexible mechanical arm |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202211283940.7A CN115674195B (en) | 2022-10-20 | Online arm shape planning method for flexible mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115674195A true CN115674195A (en) | 2023-02-03 |
| CN115674195B CN115674195B (en) | 2024-06-04 |
Family
ID=
Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008023630A (en) * | 2006-07-19 | 2008-02-07 | Toyota Motor Corp | Arm-guiding moving body and method for guiding arm |
| WO2011152265A1 (en) * | 2010-05-31 | 2011-12-08 | 独立行政法人産業技術総合研究所 | Direct acting extensible and retractable arm mechanism, and robot arm provided with direct acting extensible and retractable arm mechanism |
| CN106695802A (en) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | Improved RRT<*> obstacle avoidance motion planning method based on multi-degree-of-freedom mechanical arm |
| CN108237534A (en) * | 2018-01-04 | 2018-07-03 | 清华大学深圳研究生院 | A kind of space collision free trajectory method of continuous type mechanical arm |
| CN109571466A (en) * | 2018-11-22 | 2019-04-05 | 浙江大学 | A kind of seven freedom redundant mechanical arm dynamic obstacle avoidance paths planning method based on quick random search tree |
| CN111251306A (en) * | 2020-03-18 | 2020-06-09 | 广东省智能机器人研究院 | Mechanical arm path planning method with chassis error |
| US10946519B1 (en) * | 2018-11-30 | 2021-03-16 | X Development Llc | Offline computation and caching of precalculated joint trajectories |
| US20220066456A1 (en) * | 2016-02-29 | 2022-03-03 | AI Incorporated | Obstacle recognition method for autonomous robots |
| CN114290332A (en) * | 2021-12-28 | 2022-04-08 | 哈尔滨工业大学 | Serial mechanical arm path planning method and system applied to GIS pipeline detection |
| CN114986501A (en) * | 2022-05-28 | 2022-09-02 | 麻航翔 | Mechanical arm path planning method and system and mechanical arm |
| CN115179287A (en) * | 2022-07-13 | 2022-10-14 | 苏州中科行智智能科技有限公司 | Path planning method of mechanical arm |
Patent Citations (11)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2008023630A (en) * | 2006-07-19 | 2008-02-07 | Toyota Motor Corp | Arm-guiding moving body and method for guiding arm |
| WO2011152265A1 (en) * | 2010-05-31 | 2011-12-08 | 独立行政法人産業技術総合研究所 | Direct acting extensible and retractable arm mechanism, and robot arm provided with direct acting extensible and retractable arm mechanism |
| US20220066456A1 (en) * | 2016-02-29 | 2022-03-03 | AI Incorporated | Obstacle recognition method for autonomous robots |
| CN106695802A (en) * | 2017-03-19 | 2017-05-24 | 北京工业大学 | Improved RRT<*> obstacle avoidance motion planning method based on multi-degree-of-freedom mechanical arm |
| CN108237534A (en) * | 2018-01-04 | 2018-07-03 | 清华大学深圳研究生院 | A kind of space collision free trajectory method of continuous type mechanical arm |
| CN109571466A (en) * | 2018-11-22 | 2019-04-05 | 浙江大学 | A kind of seven freedom redundant mechanical arm dynamic obstacle avoidance paths planning method based on quick random search tree |
| US10946519B1 (en) * | 2018-11-30 | 2021-03-16 | X Development Llc | Offline computation and caching of precalculated joint trajectories |
| CN111251306A (en) * | 2020-03-18 | 2020-06-09 | 广东省智能机器人研究院 | Mechanical arm path planning method with chassis error |
| CN114290332A (en) * | 2021-12-28 | 2022-04-08 | 哈尔滨工业大学 | Serial mechanical arm path planning method and system applied to GIS pipeline detection |
| CN114986501A (en) * | 2022-05-28 | 2022-09-02 | 麻航翔 | Mechanical arm path planning method and system and mechanical arm |
| CN115179287A (en) * | 2022-07-13 | 2022-10-14 | 苏州中科行智智能科技有限公司 | Path planning method of mechanical arm |
Non-Patent Citations (6)
| Title |
|---|
| CHENG FANG; JINOH LEE; ARASH AJOUDANI; CHENGXU ZHOU; NIKOS G. TSAGARAKIS; DARWIN G. CALDWELL: "RRT-based motion planning with sampling in Redundancy space for robots with anthropomorphic arms", 《2017 IEEE INTERNATIONAL CONFERENCE ON ADVANCED INTELLIGENT MECHATRONICS (AIM)》, 24 August 2017 (2017-08-24) * |
| ZHONG JIANNING; LIU JIAXIN; DIAN SONGYI; GUO RUI: "Beetle Antennae Search guided RRT* for path planning of GIS inspection and maintenance robot", 《2021 6TH INTERNATIONAL CONFERENCE ON AUTOMATION, CONTROL AND ROBOTICS ENGINEERING (CACRE)》, 10 August 2021 (2021-08-10) * |
| 孙逸武: "含狭窄通道场景的智能小车路径规划算法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》, no. 8, 15 August 2021 (2021-08-15) * |
| 肖权，郭斌，向国菲，马丛俊，廖亮，佃松宜: "基于随机采样的多段柔性机械臂路径规划", 《组合机床与自动化加工技术》, no. 3, 31 March 2023 (2023-03-31) * |
| 阳涵疆: "基于采样的采摘机器人机械臂避障算法研究", 《中国优秀硕士学位论文全文数据库信息科技辑》, no. 1, 15 January 2018 (2018-01-15) * |
| 马丛俊 赵 涛 向国菲 任江涛 陈元科 佃松宜: "基于逆运动学的柔性机械臂末端定位控制", 《机械工程学报》, vol. 57, no. 13, 31 July 2021 (2021-07-31) * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110228069B (en) | Online obstacle avoidance motion planning method for mechanical arm | |
| Palmer et al. | Real-time method for tip following navigation of continuum snake arm robots | |
| JP3207728B2 (en) | Control method of redundant manipulator | |
| CN110653805B (en) | Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space | |
| CN113146600B (en) | Flexible robot trajectory planning method and device based on kinematics iterative learning control | |
| CN111300425B (en) | Super-redundant mechanical arm tail end track motion planning method | |
| CN112276953B (en) | Arm type collaborative planning method of continuous super-redundant mechanical arm, terminal equipment and storage medium | |
| CN110561419B (en) | Arm-shaped line constraint flexible robot track planning method and device | |
| Chitrakaran et al. | Setpoint regulation of continuum robots using a fixed camera | |
| CN111761582A (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN106329399B (en) | A kind of control method and controller of transmission line of electricity bolt fastening machine people | |
| CN114147708B (en) | Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm | |
| CN112936273B (en) | Speed stage kinematics modeling method for rope-driven flexible mechanical arm | |
| CN114670191A (en) | Seven-degree-of-freedom elbow offset mechanical arm track planning method | |
| Zaichenko et al. | Development of a geomechatronic complex for the geotechnical monitoring of the contour of a mine working | |
| CN113334381B (en) | Control method of movable decoupling continuum robot | |
| Sharma et al. | A framework for robot motion planning with sensor constraints | |
| Chen et al. | Optimizing the obstacle avoidance trajectory and positioning error of robotic manipulators using multigroup ant colony and quantum behaved particle swarm optimization algorithms | |
| Ji et al. | E-RRT*: Path Planning for Hyper-Redundant Manipulators | |
| CN115674195A (en) | Online arm shape planning method for flexible mechanical arm | |
| Burgard et al. | Introduction to mobile robotics | |
| CN115674195B (en) | Online arm shape planning method for flexible mechanical arm | |
| CN110727271A (en) | Robot motion primitive determining method and device | |
| CN113650010B (en) | Motion control method and system of table tennis robot and storage medium | |
| Guochen et al. | Path planning based on Q-learning and three-segment method for aircraft fuel tank inspection robot |
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 |