CN115674195B - Online arm shape planning method for flexible mechanical arm - Google Patents
Online arm shape planning method for flexible mechanical arm Download PDFInfo
- Publication number
- CN115674195B CN115674195B CN202211283940.7A CN202211283940A CN115674195B CN 115674195 B CN115674195 B CN 115674195B CN 202211283940 A CN202211283940 A CN 202211283940A CN 115674195 B CN115674195 B CN 115674195B
- 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.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 55
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 17
- 230000008569 process Effects 0.000 claims description 19
- 238000005070 sampling Methods 0.000 claims description 7
- 238000005452 bending Methods 0.000 claims description 3
- 238000004364 calculation method Methods 0.000 description 11
- 238000010586 diagram Methods 0.000 description 4
- 238000012952 Resampling Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000010276 construction Methods 0.000 description 2
- 230000007246 mechanism Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 238000004088 simulation Methods 0.000 description 2
- 230000004888 barrier function Effects 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
- 210000001503 joint Anatomy 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
Landscapes
- Manipulator (AREA)
Abstract
The invention discloses a planning method for an online arm shape of 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 reverse kinematics model of the flexible mechanical arm according to the initial end pose and the target end pose of the flexible mechanical arm; and determining a plurality of effective arm sequences for connecting the initial arm shape and the target arm shape from a space containing the flexible mechanical arm and the obstacle based on a rapid expansion random tree algorithm to form a collision-free path planning scheme of the flexible mechanical arm, and generating an online collision-free map. The invention solves the problem of online planning of the arm shape of the flexible mechanical arm with deformation characteristics.
Description
Technical Field
The application belongs to the technical field of automatic control of robots, relates to arm shape control of flexible mechanical arms, and particularly relates to the problem of arm shape planning of the flexible mechanical arms in a motion process in a specific environment.
Background
On the premise of giving an initial state and a target state, the driving space solution of the flexible mechanical arm can be calculated through solving the inverse kinematics model, and then the flexible arm is controlled to move to a specified position. However, how to perform arm shape planning to control the arm shape of the flexible mechanical arm in the motion process of a specific environment (such as that a space has an obstacle) so that the flexible mechanical arm cannot collide with the environment in the motion process, and meanwhile, the motion from an initial state to a target state can be completed is a problem yet to be solved.
The application document with the application number of CN202110500442.2 discloses a redundant mechanical arm obstacle avoidance track planning method based on an improved rapid expansion random tree, wherein a rigid redundant mechanical arm forward and backward kinematics model is firstly established, then a mechanical arm joint angle in a target state is solved through the inverse kinematics model, then an obstacle is mapped to a joint space of the mechanical arm, a collision-free space of the redundant mechanical arm in the joint space is obtained, a search tree is established in the collision-free space, random sampling nodes are conditionally selected according to a selection rule, and finally a track is planned in the collision-free space.
However, since there is a linear 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 redundant mechanical arm obstacle avoidance trajectory planning method is only applicable to the rigid mechanical arm, that is, the method cannot be applicable to the flexible mechanical arm device with the deformable mechanical arm body. In addition, since the method belongs to offline trajectory planning, a free-collision map describing the spatial position relationship between the mechanical arm and the obstacle must be constructed first, so that the arm shape planning can be performed in the two-dimensional grid map by using the existing planning scheme, the acquisition of collision-free space is a time-consuming and unnecessary link for calculation, and the defect of large calculation amount exists.
Disclosure of Invention
The invention aims to provide a linear arm shape planning method for a flexible mechanical arm, which solves the problem of flexible mechanical arm shape planning with deformation characteristics when the tail end pose is guided downwards in an obstacle space.
In order to achieve the above purpose, the present invention is realized by adopting the following technical scheme.
According to the online arm shape planning method of the flexible arm, RRT is applied to plan a collision-free path of an arm shape space, and an online collision-free map is generated; and combining the inverse kinematics model of the flexible arm to obtain a control signal of the flexible arm, so that the flexible arm can move without collision in the process of guiding the pose of the tail end.
Based on the analysis, the flexible mechanical arm online arm shape planning method provided by the invention specifically comprises the following steps:
S1, obtaining an initial arm shape and a target arm shape of a flexible mechanical arm through a reverse kinematics model of the flexible mechanical arm according to the initial end pose and the target end pose of the flexible mechanical arm;
S2, determining a plurality of effective arm sequences for connecting the initial arm shape and the target arm shape from a space containing the flexible mechanical arm and the obstacle based on a rapid expansion random tree algorithm, forming a collision-free path planning scheme of the flexible mechanical arm, and generating an online collision-free map; taking a flexible mechanical arm shape as a node in a random tree to be generated; the method comprises the following sub-steps:
s20, initializing a random tree;
S21, sampling in a space containing the flexible mechanical arm and the obstacle to obtain a random node q rand;
s22, searching a near node q near closest to a random node q rand in the random tree;
S23, obtaining a candidate node q pre-new based on the random node q rand, the near node q near and the set expansion distance;
S24, judging whether the area between the candidate node q pre-new and the near node q near covers the flexible mechanical arm and the obstacle; if the area between the candidate node q pre-new and the near node q near is not covered by the flexible mechanical arm and the obstacle, taking the candidate node as an effective node q new, adding the effective node into a random tree, and entering into step S25; otherwise, returning to the step S21;
S25, judging whether a target arm shape and a generated effective node q new meet the requirements of q goal-qnew||≥qerr, if not, obtaining a plurality of effective arm shape sequences connected between an initial arm shape and the target arm shape, forming a collision-free path planning scheme of the flexible mechanical arm by the obtained plurality of arm shape sequences, and simultaneously constructing an online collision-free map containing the plurality of effective arm shape sequences connected between the initial arm shape and the target arm shape; if so, go to step S26;
s26, increasing the iteration number by 1, and judging whether the maximum iteration number is reached; if yes, no collision path planning fails, returning to the step S20, and initializing the random tree again; otherwise, returning to step S21, an online collision-free map including the node q new and the valid nodes and the initial arm shape generated before it is constructed at the same time.
In the above step S1, the flexible mechanical arm inverse kinematics model is not particularly limited, and the flexible mechanical arm inverse kinematics model used here is as follows:
Wherein, (X j,Yj,Zj) represents the terminal pose; Represents the arm deformation quantity of the flexible mechanical arm, wherein theta j is the bending angle of the j-th section flexible arm, and is/> Is the rotation angle of the j-th flexible arm. In the invention, the arm shape of the flexible mechanical arm is represented by the arm shape variable quantity of the flexible mechanical arm.
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 can be obtained by solving the inverse kinematics model.
In the step S2, a plurality of effective arm sequences for connecting the initial arm shape and the target arm shape are determined based on a fast-expansion random tree algorithm (Rapidly-Exploring Random Trees, RRT algorithm) and space information (including size and position information of the flexible mechanical arm and the obstacle) of the flexible mechanical arm, so as to form a collision-free path planning scheme of the flexible mechanical arm.
In the fast expansion random tree algorithm, t= (V, E) represents a random tree, V represents a node set in the random tree, and E represents edges between nodes in the node set. In the space containing the flexible mechanical arm and the obstacle, the invention acquires the candidate node based on the initial arm shape, the target arm shape, the flexible mechanical arm and the obstacle and some initialization parameters of the RRT algorithm (including the set expansion probability p, the expansion distance d, the final node error q err and the maximum iteration number N RRT) and then executes thought (SAMPLEFREE, NEAREST, STEER) according to the traditional RRT algorithm, and judges the effectiveness of the candidate node.
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 linear collision-free map of the flexible mechanical arm through 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 step S22, the closest node q near to the random node q rand is obtained by the Nearest () function based on the random tree and the random node q rand obtained in step S21.
In step S23 described above, the candidate node q pre-new located between the node q rand and the node q near is obtained by the Steer () function based on the random node q rand, the near node q near, and the set expansion distance d.
In the step S24, in the validity judging process of the candidate node q pre-new, the validity judging can be performed on the candidate node q pre-new by combining the flexible mechanical arm and the obstacle space information and the node q near, so that the judging efficiency can be greatly improved. If the area between the candidate node q pre-new and the node q near is not covered by the flexible mechanical arm and the obstacle, taking the candidate node as an effective node, adding the effective node into a random tree, and entering into step S25; otherwise, the generated node is not in accordance with the requirement, and the step S21 is needed to be returned to for resampling.
In the above steps S25 and S26, in the process of guiding the target arm shape, the candidate node that is searched gradually approaches the target arm shape q goal according to the preset precision, and finally, RRT random tree t= (V, E) is formed. The termination conditions here include whether the maximum number of iterations is reached (for determining whether the planning has failed) and the target arm satisfying ||q goal-qnew||≥qerr (for determining whether the planning has been completed) with the generated active node (i.e., active arm).
In step S25, when the target arm shape and the generated effective node (i.e., the effective arm shape) satisfy ||q goal-qnew||≥qerr, it indicates that the approximation of the generated effective arm shape to the target arm shape cannot meet the set precision requirement, and further generation of an effective arm shape that is more approximate to the target arm shape is required. When not satisfied, i.e., ||q goal-qnew||＜qerr, it indicates that the approximation of the generated effective arm to the target arm has satisfied the set accuracy requirement, and the search for the effective arm may be stopped. The drive signal of the flexible mechanical arm can be further acquired based on a collision-free path planning scheme of the flexible mechanical arm, and the drive of the flexible mechanical arm is completed. Meanwhile, an online collision-free map comprising a plurality of effective arm sequences connected between the initial arm shape and the target arm shape can be constructed and used for assisting an operator in checking the movement of the flexible mechanical arm, so that closed-loop control of the flexible mechanical arm is completed.
In step S26, it is determined whether the planning has failed. When the maximum number of iterations is reached, indicating that the current planning scheme is unsuccessful, it is necessary to restart planning from the initial arm shape of the flexible manipulator. When the maximum iteration number is not reached, the flexible mechanical arm path can be continuously planned; at this time, the driving real-time signal of the flexible mechanical arm can be directly obtained based on the obtained current effective node q new, so that the real-time driving of the flexible mechanical arm is completed; meanwhile, an online collision-free map comprising the node q new and the effective node and the initial arm shape generated before the node q new can be constructed and used for assisting an operator in checking the movement of the flexible mechanical arm to finish closed-loop control of the flexible mechanical arm; and then returning to the step S21, continuously searching the effective arm shape sequence meeting the requirements in the exploration area until the tail end of the flexible mechanical arm reaches the target tail end pose. The method generates the effective arm node and simultaneously generates the online collision-free map of the flexible mechanical arm, so that a large amount of calculation time cost can be saved, and the planning efficiency is improved.
Based on a collision-free path planning scheme of the flexible mechanical arm, a flexible mechanical arm driving signal is obtained through a flexible mechanical arm inverse kinematics model. According to the invention, the collision-free path planning scheme based on the flexible mechanical arm can obtain the arm deformation quantity between two adjacent arm shapes, and then the rotation angular velocity is obtained through the inverse kinematics relation between the joint space and the driving space, as shown in the following formula:
In the method, in the process of the invention, The total variation of the rotation angle of the single-section flexible mechanical arm around the universal water-saving horizontal rotating shaft and the vertical rotating shaft is respectively calculated; /(I)The corresponding rotation angular speed is the quantitative representation of the flexible mechanical arm driving space; t C、tT represents the corresponding moments of two adjacent arms; /(I)The arm deformation amounts of two adjacent arm-shaped flexible mechanical arms are respectively represented, and n j represents the number of universal joints of the 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 speed of the flexible mechanical arm, the rotation speed (namely, a driving signal) of the screw rod driving motor connected with the driving wire can be obtained according to the following formula, so that the flexible mechanical arm is driven:
in the formula, D represents lead of the screw rod, namely the linear distance of the screw rod for the screw nut to walk after the screw nut rotates for one circle.
In the method, in the process of the invention,And the variable quantity of the length of the driving wire in a specified time period between two adjacent arm shapes of the mth driving wire arranged on the jth flexible mechanical arm is represented.
The generation process of the real-time driving signal given above is similar, and the rotating speed of the screw rod driving motor in the appointed time period between the two adjacent arm shapes can be obtained based on the current effective node q new and the previous effective node thereof according to the explanation given above, so that the driving of the flexible mechanical arm is realized.
The invention mainly aims at solving the obstacle avoidance problem of the obstacle space of the flexible mechanical arm by adopting an online arm shape planning method in the arm shape space, and the online arm shape planning method of the flexible mechanical arm has the following beneficial effects:
1) Firstly, acquiring an initial arm shape and a target arm shape of a flexible mechanical arm based on the initial end pose and the target end pose of the flexible mechanical arm; then, a plurality of effective arm sequences of the initial arm shape and the target arm shape of the flexible mechanical arm are obtained through a rapid expansion random tree algorithm, so that a collision-free path planning scheme of the local flexible mechanical arm is formed, and the problem of online planning of the arm shape of the flexible mechanical arm with deformation characteristics is solved;
2) The invention does not need to construct a complete free-collision map in advance, and can construct a real-time online collision-free map by combining the effective arm node and the initial arm generated before the effective arm node when the effective arm node is obtained, thereby saving a great amount of calculation time cost;
3) According to the invention, the local effective arm shape is continuously constructed to gradually approach the target arm shape, so that the accuracy of a collision-free path planning scheme of the flexible mechanical arm can be improved;
4) The invention can assist operators to check the movement of the flexible mechanical arm through the constructed real-time online collision-free map, and complete the closed-loop control of the flexible mechanical arm.
Drawings
In order to more clearly illustrate the technical solutions of the embodiments of the present invention, the drawings that are needed in the embodiments will be briefly described below, it being understood that the following drawings only illustrate some embodiments of the present invention and therefore should not be considered as limiting the scope, and other related drawings may be obtained according to these drawings without inventive effort for a person skilled in the art.
FIG. 1 is a schematic diagram of a physical structure of a flexible mechanical arm; wherein, (a) is an overall structure schematic, (b) is a male ring schematic, and (c) is a female ring schematic.
Fig. 2 is a diagram showing the relationship between the arm space and the arm configuration sequence space of the flexible mechanical arm.
Fig. 3 is a schematic flow chart of a method for planning the shape of a linear arm of the flexible mechanical arm.
Fig. 4 is a flexible robotic arm motion vector diagram.
Fig. 5 is a multi-barrier avoidance scene of the multi-section flexible mechanical arm constructed in the application example.
Fig. 6 is an online arm shape planning result of a section of flexible mechanical arm in a multi-obstacle space in an application example.
Detailed Description
For the purpose of making the objects, technical solutions and advantages of the embodiments of the present invention more apparent, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention, and it is apparent that the described embodiments are some embodiments of the present invention, but not all embodiments of the present invention. The components of the 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 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 made by those skilled in the art based on the embodiments of the invention without making any inventive effort, are intended to be within the scope of the invention.
The following embodiments are directed to a flexible mechanical arm physical entity, as shown in fig. 1, comprising 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, which are connected together via a connector 4; the second section of the flexible arm terminates in an end joint 5 (used to characterize the end pose). The first section 1 of the flexible arm and the second section 2 of the flexible arm have the same structure and each comprises a plurality of universal joints 3 and three driving wires for connecting the universal joints in series. Two adjacent universal joints are connected through 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 joint 3 has two structures: the male ring 31 and the female ring 32, and the male ring 31 and the female ring 32 are staggered. Bosses 312 are respectively arranged on the annular surfaces on two sides of the male ring 31, and the positions of the bosses on the two annular surfaces are mutually perpendicular; grooves 322 are respectively formed on the annular surfaces on two sides of the female ring 32, and the positions of the grooves on the two annular surfaces are mutually perpendicular, 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 grooves formed on the ring surface of the female ring are matched with the bosses arranged on the opposite ring surface of the male ring. The end surfaces on both sides of the female ring and the end surfaces on both 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 two side end surfaces of the female ring and the two side end surfaces of the male ring are respectively provided with a positioning surface inclined from the middle to the two sides, so that the first flexible arm/second flexible arm formed by the female ring and the male ring can move to have a larger moving range.
The end face structures of the connecting body 4 and the end joint 5 and the universal joint (the male ring 31 or the female ring 32) connected with the connecting body are matched. When the male ring 31 is connected with the connector 4/the tail end joint 5, the end face of the male ring is consistent with the end face structure of the female ring; when the female ring 32 is connected with the connector 4/the terminal joint 5, the end face of the female ring is consistent with the end face structure of the male ring.
Example 1
Here, the relevant definition of the flexible robot arm shape planning problem is unified first. Definition of the definitionFor the global arm-shaped space of the flexible mechanical arm without any constraint, j=1, 2 represents two segments of the flexible mechanical arm. /(I)Representing the total effective free arm space of the flexible robotic arm within the obstacle space, C free needs to be acquired in advance in full in a conventional arm planning scheme, q= (Q 1,q2,...,qs) is defined as all arm configuration sequences within space C free,/>S is the total number of sequences. The traditional arming scheme requires that several valid arming sequences capable of solving Q init→qgoal be continually sought from Q, Q init,qgoal∈Cfree, and defined as/>To complete the planned arm sequences, t is the total number of effective arm sequences. In the scheme of the invention, an online mode is adopted, and the method can directly select from/>Is to find Q free. As shown in fig. 2, a block diagram showing the relationship between each arm space and the arm arrangement sequence space is shown, wherein an ellipse represents the arm space and a rectangle represents the arm arrangement sequence space. The traditional offline arm planning scheme needs to complete the calculation process from ①②④⑤, firstly, the global arm space C of the flexible mechanical arm under no constraint is determined, then all the effective free arm space C free of the flexible mechanical arm in the obstacle space is determined, then all the arm configuration sequence spaces Q in C free are determined, and then a plurality of effective arm sequences Q free capable of solving Q init→qgoal are searched from the Q.
The flexible mechanical arm online arm planning method provided by the invention only needs to pass through ①③⑤, so that a large amount of calculation time from ① to ② is saved, ⑤ can be directly obtained by directly exploring from more compact ③, and therefore, the searching efficiency and the searching accuracy are improved when the arm planning scheme is calculated.
The invention provides a flexible mechanical arm online arm shape planning method in combination with an algorithm RRT, which is shown in figure 3 and comprises the following steps:
s1, obtaining an initial arm shape and a target arm shape of the flexible mechanical arm through a reverse motion model of the flexible mechanical arm according to the initial end pose and the target end pose of the flexible mechanical arm.
In this embodiment, the flexible mechanical arm inverse kinematics model used is as follows:
Wherein, (X j,Yj,Zj) represents the terminal pose; Represents the arm deformation quantity of the flexible mechanical arm, wherein theta j is the bending angle of the j-th section flexible arm, and is/> Is the rotation angle of the j-th flexible arm. In the invention, the arm shape of the flexible mechanical arm is represented by the arm shape variable quantity of the flexible mechanical arm.
According to the initial tail end pose of the flexible mechanical armAnd target terminal pose/>And obtaining the initial arm shape and the target arm shape of the flexible mechanical arm by solving the inverse kinematics model.
S2, determining a plurality of effective arm sequences for connecting the initial arm shape and the target arm shape from a space containing the flexible mechanical arm and the obstacle based on a rapid expansion random tree algorithm, forming a collision-free path planning scheme of the flexible mechanical arm, and generating an online collision-free map.
The method aims at determining a plurality of effective arm sequences between the initial arm shape and the target arm shape based on a rapid expansion random tree algorithm and flexible mechanical arm and obstacle space information (comprising flexible mechanical arm and obstacle size and position information), and forming a collision-free path planning scheme of the flexible mechanical arm.
In the fast expansion random tree algorithm, t= (V, E) represents a random tree, V represents a node set in the random tree, and E represents edges between nodes in the node set. According to the invention, the flexible mechanical arm shape is taken as a node in a tree, the candidate node is obtained based on the initial arm shape, the target arm shape, the space information of the flexible mechanical arm and the obstacle and some initialization parameters (including the set expansion probability p, the expansion distance d, the final node error q err and the maximum iteration number N RRT) of the RRT algorithm, and then the candidate node is obtained according to the conventional RRT algorithm execution thought (SAMPLEFREE, NEAREST, STEER), and the effectiveness of the candidate node is judged. The RRT algorithm pseudocode is shown in table 1.
In combination with table 1, this step specifically comprises the following sub-steps:
S20 initializes the random tree.
Here, initializing a random tree space (i.e., a space containing flexible mechanical arms and obstacles) and initialization parameters (including a set expansion probability p, an expansion distance d, a final node error q err, and a maximum number of iterations N RRT) is included, and introducing an initial arm node and a target arm node into the random tree.
S21, sampling is carried out in a space containing the flexible mechanical arm and the obstacle, and a random node q rand is obtained.
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 the linear collision-free map of the flexible mechanical arm through SAMPLEFREE () function, and a random node q rand is obtained. 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 finds the closest node q near in the random tree to the random node q rand.
In this step, a near node q near Nearest to the random node q rand is obtained by a Nearest () function based on the random tree and the random node q rand obtained in step S21.
S23 obtains a candidate node q pre-new based on the random node q rand, the near node q near, and the set expansion distance.
In this step, a candidate node q pre-new located between the node q rand and the node q near is obtained by a Steer () function based on the random node q rand, the near node q near, and the set expansion distance d.
S24, judging whether the area between the candidate node q pre-new and the near node q near covers the flexible mechanical arm and the obstacle; if the area between the candidate node q pre-new and the near node q near is not covered by the flexible mechanical arm and the obstacle, taking the candidate node as an effective node q new, adding the effective node into a random tree, and entering into step S25; otherwise, the process returns to step S21.
In the process of judging the validity of the candidate node q pre-new, the method is in the effective free arm shape space of the more compact flexible mechanical armAnd the candidate node q pre-new can be effectively judged by combining the space information of the flexible mechanical arm and the obstacle and the node q near, so that the judging efficiency can be greatly improved. If the area between the candidate node q pre-new and the node q near is not covered by the flexible mechanical arm and the obstacle, taking the candidate node as an effective node, adding the effective node into a random tree, and entering into step S25; otherwise, the generated node is not in accordance with the requirement, and the step S21 is needed to be returned to for resampling.
S25, judging whether a target arm shape and a generated effective node q new meet the requirements of q goal-qnew||≥qerr, if not, obtaining a plurality of effective arm shape sequences connected between an initial arm shape and the target arm shape, forming a collision-free path planning scheme of the flexible mechanical arm by the obtained plurality of arm shape sequences, and simultaneously constructing an online collision-free map containing the plurality of effective arm shape sequences connected between the initial arm shape and the target arm shape; if so, the process advances to step S26.
When the target arm shape and the generated effective node (i.e., the effective arm shape) meet the requirement of q goal-qnew||≥qerr, the approximation of the generated effective arm shape to the target arm shape is shown to not meet the set precision requirement, and the effective arm shape which is more approximate to the target arm shape needs to be further generated. When not satisfied, i.e., ||q goal-qnew||＜qerr, it indicates that the approximation of the generated effective arm to the target arm has satisfied the set accuracy requirement, and the search for the effective arm may be stopped. The V obtained is a number of discrete effective arm sequences connecting the initial arm and the target arm.
The drive signal of the flexible mechanical arm can be further acquired based on a collision-free path planning scheme of the flexible mechanical arm, and the drive of the flexible mechanical arm is completed. Meanwhile, an online collision-free map comprising a plurality of effective arm sequences connected between the initial arm shape and the target arm shape can be constructed and used for assisting an operator in checking the movement of the flexible mechanical arm, so that closed-loop control of the flexible mechanical arm is completed.
S26, increasing the iteration number by 1, and judging whether the maximum iteration number is reached; if yes, no collision path planning fails, returning to the step S20, and initializing the random tree again; otherwise, returning to step S21, an online collision-free map including the node q new and the valid nodes and the initial arm shape generated before it is constructed at the same time.
This step is to determine whether the planning failed. After the iteration number increases by 1 (i.e., k + 1), when the maximum iteration number is reached, indicating that the current planning scheme is unsuccessful, it is necessary to restart planning from the initial arm shape of the flexible manipulator. When the maximum iteration number is not reached, the flexible mechanical arm path can be continuously planned; at this time, the driving real-time signal of the flexible mechanical arm can be directly obtained based on the obtained current effective node q new, so that the real-time driving of the flexible mechanical arm is completed; meanwhile, an online collision-free map comprising the node q new and the effective node and the initial arm shape generated before the node q new can be constructed and used for assisting an operator in checking the movement of the flexible mechanical arm to finish closed-loop control of the flexible mechanical arm; and then returning to the step S21, continuously searching the effective arm shape sequence meeting the requirements 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 number of discrete effective arm sequences connecting between the initial arm and the current effective node.
The drive signal of the flexible mechanical arm can be further acquired based on a collision-free path planning scheme of the flexible mechanical arm, and the drive of the flexible mechanical arm is completed.
Table 1 flexible mechanical arm online arm shape planning RRT algorithm pseudocode
Based on a collision-free path planning scheme of the flexible mechanical arm, the arm deformation quantity between two adjacent arm shapes can be obtained. As shown in fig. 4, the rotational angular velocity is then obtained by the inverse kinematics of the joint space and the drive space, as shown in the following equation:
In the method, in the process of the invention, The total variation of the rotation angle of the single-section flexible mechanical arm around the universal water-saving horizontal rotating shaft and the vertical rotating shaft is respectively calculated; /(I)The corresponding rotation angular speed is the quantitative representation of the flexible mechanical arm driving space; t C、tT represents the corresponding moments of two adjacent arms; /(I)The arm deformation amounts of two adjacent arm-shaped flexible mechanical arms are respectively represented, and n j represents the number of universal joints of the 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 speed of the flexible mechanical arm, the rotation speed (namely, a driving signal) of the screw rod driving motor connected with the driving wire can be obtained according to the following formula, so that the flexible mechanical arm is driven:
wherein D represents lead of the screw rod, namely the linear distance of the screw rod for the screw nut to walk after the screw nut rotates for one circle; representing the drive wire speed.
In the method, in the process of the invention,And the variable quantity of the length of the driving wire of the mth driving wire arranged on the jth flexible mechanical arm in the time period between two adjacent arm shapes is represented, and m=1, 2 and 3.
R is the radius of the reference circle where the through holes for 3 driving wires of each section of flexible mechanical arm pass through.
Δθ j is the variation of the flexible mechanical arm deformation in fig. 4, and the relationship between the variation and the rotational angular velocity is as follows:
substituting the formulas (6), (7) and (8) into (5) to obtain Is an expression of (2).
And 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 the formula (4), so that the flexible mechanical arm is driven.
The generation process of the real-time driving signal given above is similar, and the rotating speed of the screw rod driving motor in the appointed time period between the two adjacent arm shapes can be obtained based on the current effective node q new and the previous effective node thereof according to the explanation given above, so that the driving of the flexible mechanical arm is realized. The terminal pose of the flexible mechanical arm is further monitored in real time (for example, through an IMU inertial measurement unit) to obtain the terminal pose, and the effectiveness of the online arm shape planning method of the flexible mechanical arm can be detected.
Application example
In order to verify the effectiveness of the online arm shape planning method of the flexible mechanical arm provided by the invention, an obstacle avoidance scene (parameters are shown in tables 2 and 3) of a multi-section flexible arm in a multi-obstacle space is constructed as shown in fig. 5, the flexible mechanical arm needs to complete the movement from an initial end pose to a target end pose, and the movement process needs to plan two obstacles in the double-section arm shape avoidance space. According to the online arm shape planning method for the flexible mechanical arm provided in embodiment 1, online arm shape planning is performed for the flexible mechanical arm, fig. 6 shows online arm shape planning results of one section of flexible mechanical arm in a multi-obstacle space, a red curve in the figure is a motion track of the corresponding section of flexible mechanical arm in the arm shape space after planning is completed, and a sporadic black block area is a result of online map building, so that an online planning mechanism is not required to construct a global free-collision map, and overall calculation time is greatly reduced.
Table 4 shows the comparison of the calculated time for the present application example and the conventional offline scheme. See CN202110500422.2 for a traditional offline scheme implementation. From table 4, it can be seen that 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 requirement of online real-time planning of the flexible mechanical arm.
Table 2 two-segment flexible mechanical arm parameters
TABLE 3 Barrier parameters
Table 4 comparison of time consuming calculations for two different arm planning schemes
Note that: the average scheme refers to the average total planning time of 10 simulation calculations;
The average time of the drawing is the average time of the drawing 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 relies on the rotation angle data of the rigid joint angles of the mechanical arms, so that the method cannot be suitable for the flexible mechanical arms aimed at in the invention; the traditional arm planning scheme is required to construct a free-collision map describing the spatial position relation between the mechanical arm and the obstacle, so that the arm planning can be performed in the two-dimensional grid map by using the existing planning scheme, the calculation is quite time-consuming, the local map can be constructed while the arm planning is completed by using an online mechanism, and additional time is not required to be spent for constructing the map, so that a great amount of time for constructing the grid map by calculation is saved.
In summary, the online arm shape planning method of the flexible mechanical arm provided by the invention is characterized by comprising the following steps:
1) The traditional offline collision-free map construction is improved to an online construction method, so that the overall planning efficiency is improved;
2) The problem of flexible mechanical arm shape planning similar to that with deformation characteristics is solved.
The above is only a preferred embodiment of the present invention, and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (4)
1. The online arm shape planning method of the flexible mechanical arm is characterized by comprising the following steps of:
s1, obtaining an initial arm shape and a target arm shape of a flexible mechanical arm through a reverse kinematics model of the flexible mechanical arm according to the initial end pose and the target end pose of the flexible mechanical arm; the flexible mechanical arm inverse kinematics model is as follows:
(1)
(2)
In the method, in the process of the invention, Representing the terminal pose; /(I)Representing the arm deformation amount of the flexible mechanical arm, wherein/>For the bending angle of the j-th flexible arm,/>A rotation angle of the j-th flexible arm; characterizing the arm shape of the flexible mechanical arm by the arm deformation amount of the flexible mechanical arm;
s2, determining a plurality of effective arm sequences for connecting the initial arm shape and the target arm shape from a space containing the flexible mechanical arm and the obstacle based on a rapid expansion random tree algorithm, forming a collision-free path planning scheme of the flexible mechanical arm, and generating an online collision-free map; taking a flexible mechanical arm shape as a node in a random tree to be generated; the method comprises the following sub-steps:
S20, initializing a random tree;
S21, sampling in a space containing the flexible mechanical arm and the obstacle to obtain a random node q rand; the specific implementation mode is as follows: 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 the flexible mechanical arm on-line collision-free map through SAMPLEFREE () function, so as to obtain a random node q rand;
S22, searching a near node q near closest to a random node q rand in the random tree;
S23, obtaining a candidate node q pre-new based on the random node q rand, the near node q near and the set expansion distance;
S24, judging whether the area between the candidate node q pre-new and the near node q near covers the flexible mechanical arm and the obstacle; if the area between the candidate node q pre-new and the near node q near is not covered by the flexible mechanical arm and the obstacle, taking the candidate node as an effective node q new, adding the effective node into a random tree, and entering into step S25; otherwise, returning to the step S21;
S25, judging whether the target arm shape and the generated effective node q new meet the requirement Q goal denotes the target arm node, q err denotes the end node error; if the initial arm shape and the target arm shape are not met, a plurality of effective arm shape sequences which are connected with the initial arm shape and the target arm shape are obtained, a collision-free path planning scheme of the flexible mechanical arm is formed by the plurality of obtained arm shape sequences, and meanwhile an online collision-free map which comprises the plurality of effective arm shape sequences which are connected with the initial arm shape and the target arm shape is constructed; if so, go to step S26;
s26, increasing the iteration number by 1, and judging whether the maximum iteration number is reached; if yes, no collision path planning fails, returning to the step S20, and initializing the random tree again; otherwise, returning to step S21, an online collision-free map including the node q new and the valid nodes and the initial arm shape generated before it is constructed at the same time.
2. The flexible mechanical arm online arm shape planning method according to claim 1, wherein in step S22, a proximal node q near closest to the random node q rand is obtained through a Nearest () function based on the random tree and the random node q rand obtained in step S21.
3. The flexible mechanical arm online arm shape planning method according to claim 1, wherein in step S23, candidate node q pre-new located between node q rand and node q near is obtained by Steer () function based on random node q rand, near node q near, and set expansion distance d.
4. The flexible mechanical arm online arm shape planning method according to claim 1, wherein in step S25, a flexible mechanical arm driving signal is obtained through a flexible mechanical arm inverse kinematics model 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 | 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 | 2022-10-20 | Online arm shape planning method for flexible mechanical arm |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN115674195A CN115674195A (en) | 2023-02-03 |
| CN115674195B true CN115674195B (en) | 2024-06-04 |
Family
ID=85067208
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202211283940.7A Active CN115674195B (en) | 2022-10-20 | 2022-10-20 | Online arm shape planning method for flexible mechanical arm |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN115674195B (en) |
Citations (10)
| 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 |
| 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 |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US11927965B2 (en) * | 2016-02-29 | 2024-03-12 | AI Incorporated | Obstacle recognition method for autonomous robots |
-
2022
- 2022-10-20 CN CN202211283940.7A patent/CN115674195B/en active Active
Patent Citations (10)
| 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 |
| 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 |
|---|
| Beetle Antennae Search guided RRT* for path planning of GIS inspection and maintenance robot;Zhong Jianning; Liu Jiaxin; Dian Songyi; Guo Rui;《2021 6th International Conference on Automation, Control and Robotics Engineering (CACRE)》;20210810;全文 * |
| RRT-based motion planning with sampling in Redundancy space for robots with anthropomorphic arms;Cheng Fang; Jinoh Lee; Arash Ajoudani; Chengxu Zhou; Nikos G. Tsagarakis; Darwin G. Caldwell;《2017 IEEE International Conference on Advanced Intelligent Mechatronics (AIM)》;20170824;全文 * |
| 含狭窄通道场景的智能小车路径规划算法研究;孙逸武;《中国优秀硕士学位论文全文数据库信息科技辑》;20210815(第8期);全文 * |
| 基于逆运动学的柔性机械臂末端定位控制;马丛俊 赵 涛 向国菲 任江涛 陈元科 佃松宜;《机械工程学报》;20210731;第57卷(第13期);全文 * |
| 基于采样的采摘机器人机械臂避障算法研究;阳涵疆;《中国优秀硕士学位论文全文数据库信息科技辑》;20180115(第1期);全文 * |
| 基于随机采样的多段柔性机械臂路径规划;肖权，郭斌，向国菲，马丛俊，廖亮，佃松宜;《组合机床与自动化加工技术》;20230331(第3期) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN115674195A (en) | 2023-02-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN106737671B (en) | The anthropomorphic motion planning method of the bilayer of seven degrees of freedom copy man mechanical arm | |
| CN113352319B (en) | Redundant mechanical arm obstacle avoidance trajectory planning method based on improved fast expansion random tree | |
| CN111300425B (en) | Super-redundant mechanical arm tail end track motion planning method | |
| CN110228069A (en) | A kind of online avoidance motion planning method of mechanical arm | |
| CN110653805B (en) | Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space | |
| CN111761582B (en) | Mobile mechanical arm obstacle avoidance planning method based on random sampling | |
| CN109397271B (en) | 7-degree-of-freedom anthropomorphic mechanical arm and control method and system thereof | |
| CN107116540B (en) | A kind of robot collision checking method surrounding structure based on SCS | |
| CN113146600B (en) | Flexible robot trajectory planning method and device based on kinematics iterative learning control | |
| CN110561419B (en) | Arm-shaped line constraint flexible robot track planning method and device | |
| Cortés et al. | Probabilistic motion planning for parallel mechanisms | |
| CN112276953B (en) | Arm type collaborative planning method of continuous super-redundant mechanical arm, terminal equipment and storage medium | |
| CN114147708B (en) | Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm | |
| CN115416016A (en) | Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method | |
| Cheng et al. | Robot arm path planning based on improved RRT algorithm | |
| CN115674195B (en) | Online arm shape planning method for flexible mechanical arm | |
| CN114505862A (en) | Building 3D printing mobile mechanical arm station planning method and system | |
| CN114055467A (en) | Space pose online simulation system based on five-degree-of-freedom robot | |
| CN116572248B (en) | Redundant mechanical arm collision avoidance and path planning method for joint space planning | |
| CN117415806A (en) | Obstacle avoidance track planning method for mechanical arm for loading and unloading drill rod under variable target condition | |
| Liu et al. | A planning method for safe interaction between human arms and robot manipulators | |
| CN113246138B (en) | Visual servo compensation control method based on rope-driven multi-joint redundancy driving mechanical arm | |
| CN113650010B (en) | Motion control method and system of table tennis robot and storage medium | |
| Liu et al. | Vision-based path following of snake-like robots | |
| 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 | ||
| GR01 | Patent grant |