CN114147708B - Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm - Google Patents

Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm Download PDF

Info

Publication number
CN114147708B
CN114147708B CN202111424954.1A CN202111424954A CN114147708B CN 114147708 B CN114147708 B CN 114147708B CN 202111424954 A CN202111424954 A CN 202111424954A CN 114147708 B CN114147708 B CN 114147708B
Authority
CN
China
Prior art keywords
path
mechanical arm
point
joint
search
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202111424954.1A
Other languages
Chinese (zh)
Other versions
CN114147708A (en
Inventor
季雅文
姜向远
马思乐
栾义忠
马晓静
陈纪旸
孙文旭
张振强
李进
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shandong University
Original Assignee
Shandong University
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shandong University filed Critical Shandong University
Priority to CN202111424954.1A priority Critical patent/CN114147708B/en
Publication of CN114147708A publication Critical patent/CN114147708A/en
Application granted granted Critical
Publication of CN114147708B publication Critical patent/CN114147708B/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1664—Programme controls characterised by programming, planning systems for manipulators characterised by motion, path, trajectory planning
    • B25J9/1666—Avoiding collision or forbidden zones
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J18/00—Arms
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1674—Programme controls characterised by safety, monitoring, diagnostic
    • B25J9/1676—Avoiding collision or forbidden zones
    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Abstract

The invention relates to the technical field of automatic control, in particular to a planning method for an obstacle avoidance path of a mechanical arm. An improved BAS algorithm-based mechanical arm obstacle avoidance track planning method comprises the following steps: establishing an optimized evaluation model of the attitude planning result; acquiring the starting point and the end point pose of the tail end of the mechanical arm, solving the joint angles of the starting point and the end point by using an inverse kinematics analysis method, and determining the position of the mechanical arm in space; establishing a collision detection model of the mechanical arm and the obstacle; and according to the collision detection result, combining with an optimization evaluation model, and generating a mechanical arm obstacle avoidance path by using a BAS4-AO algorithm. The mechanical arm obstacle avoidance path planning method based on the improved longhorn beetle whisker search algorithm can effectively simplify an initial path, eliminate jitter in the path and ensure the safety of the mechanical arm in the operation process.

Description

Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm
Disclosure of Invention
The invention relates to the technical field of automatic control, in particular to a planning method for an obstacle avoidance path of a mechanical arm.
Background
In recent years, with the progress of technology, the manufacturing industry of China has raised intelligent and unmanned research with robots as the core. According to development needs, robots are required to avoid barriers automatically according to complex environmental conditions in many cases, and therefore wide application prospects are brought to robot obstacle avoidance path planning technologies.
The improvement and even combination of the existing path planning algorithm into a more efficient composite algorithm to adapt to more complex path planning tasks is the current mainstream research direction. The current intelligent algorithms for path planning optimization include firefly algorithm (Firefly Algorithm, FA), particle swarm algorithm (Particle Swarm optimization, PSO), genetic algorithm (Genetic Algorithm, GA), artificial bee colony algorithm (Artificial Bee Colony, ABC), rapid-search random tree algorithm (RRT), ant colony Algorithm (ACO), etc. Ginger is far from imitating natural midget cattle predation, and an optimization algorithm with meta-heuristic property, high randomness and rapid convergence is provided, namely a longhorn cattle whisker search algorithm (Beetle Antennae Search, BAS). As shown in fig. 3, in nature, predation of longicorn mainly depends on antennae on both sides of the head, and smell cells in antennae can sense the concentration of scattered pheromones in the air. As the smell cells on different antennae sense the concentration difference in two directions, the longicorn can move to the side with larger concentration, so that the position is continuously updated, and finally, the food is found. According to the predatory behavior of the longhorns, the searching process of the BAS algorithm is mainly controlled by two parameters, namely a direction and a step length. By setting random directions and reducing search steps, optimization can be guaranteed within a larger search range, and rapid convergence can be achieved when optimization is finished. Meanwhile, due to the limitation of strong attitude randomness and self-attenuation of step length, the influence of initial parameter setting on an optimization result is large, and adjustment and improvement are needed when the robot attitude planning field is applied.
The six-degree-of-freedom mechanical arm has very wide application in industrial production and has great research value. However, most of the currently used path planning algorithms are mainly random sampling path planning algorithms, and although proper motion paths can be successfully found, the algorithm operation efficiency is not high due to the fact that a large-scale random search is performed.
Disclosure of Invention
The invention aims to provide an improved BAS algorithm-based mechanical arm obstacle avoidance track planning method, which effectively improves the path exploration and obstacle avoidance capacity of an algorithm in three-dimensional gesture planning.
In order to achieve the above purpose, the invention adopts the following technical scheme: the mechanical arm obstacle avoidance path planning method based on the improved longhorn beetle whisker search algorithm comprises the following steps:
establishing an optimized evaluation model of the attitude planning result;
acquiring the starting point and the end point pose of the tail end of the mechanical arm, solving the joint angles of the starting point and the end point by using an inverse kinematics analysis method, and determining the position of the mechanical arm in space;
establishing a collision detection model of the mechanical arm and the obstacle;
and according to the test parameters obtained by collision detection, combining an optimization evaluation model, and generating a mechanical arm obstacle avoidance path by using an improved BAS4-AO algorithm.
Further, the optimization evaluation model comprises a function for optimizing the joint angle of the mechanical arm, and the expression is as follows:
Figure BDA0003377845940000021
wherein θ (θ) t ) The sum of angle differences of the mechanical arm joint after N search iterations is obtained; variable θ i t Representing the joint angle at the sampling instant t at the path point i; m represents the total number of all path points in the path planning process.
Further, the optimization evaluation model includes a function for optimizing the run path length, expressed as:
Figure BDA0003377845940000022
wherein L (P) represents the moving distance of the mechanical arm end effector in space, and the variable P i t Representing the position of the end point of the manipulator at the sampling time t on the path point i; k represents the number of path points included in the path.
Further, the time optimization function expression completed by the algorithm is:
T＝T end -T start
wherein T is start Starting point for algorithm optimization, T end A time point when the manipulator operation is completed; total number of search iterations
Figure BDA0003377845940000023
T S Is the sampling interval.
Further, in the collision detection model, a convex hull simplified model is used as a robot arm model, and a shaft envelope model is used as an obstacle model.
Further, the generation of the robot arm obstacle avoidance path by using the improved BAS4-AO algorithm comprises the following steps:
(1) Initializing various parameters;
(2) Generating two mutually orthogonal path point search directions, wherein the calculation formula of the search directions is as follows:
Figure BDA0003377845940000031
Figure BDA0003377845940000032
a 1 and a 2 Is two proportionality constants, and rand (]Random value on the table;
Figure BDA0003377845940000033
is a random unit vector, ">
Figure BDA0003377845940000034
Is the direction vector of the target point relative to the current path point; />
Figure BDA0003377845940000035
Representing a left to right direction vector; />
Figure BDA0003377845940000036
Representing a bottom-up direction vector;
(3) Path Point spatial location Generation
When iteration at the t-th moment is carried out, the spatial position coordinates of four judgment points around the spatial coordinate P are as follows:
Figure BDA0003377845940000037
Figure BDA0003377845940000038
Figure BDA0003377845940000039
Figure BDA00033778459400000310
wherein the search step delta * Is a dynamic adjustment factor, decays with the search iteration loop;
then, pose information of the next iteration is generated:
Figure BDA00033778459400000311
R t+1 ＝R t +δ * *(R g -R t )*λ 2
wherein, L (P) is taken as the fitness evaluation standard of four judgment points, and a sign function min (·) is used for extracting the point with the minimum fitness function value in the four judgment points; matrix R g R is the rotation matrix of the target point t For the rotation matrix of the current point, lambda 2 As a matching coefficient, delta * As R g To R t Is a step of approximation;
(4) Collision detection and search step adjustment
Performing collision detection on the mechanical arm shaft body and the obstacle, and if no collision exists, directly transferring to the step (2) to execute the next iteration cycle; if collision occurs, the global search step is attenuated, the path point information is cleared, and a new path point is searched again.
The collision detection process is to project the mechanical arm convex hull simplified model and the obstacle model on three coordinate planes, and determine whether collision occurs by judging whether the projections are coincident.
(5) Performing joint angle optimization judgment
Solving the joint angle of the mechanical arm according to the spatial coordinates of the end effector of the mechanical arm, and judging the joint angle theta of the end effector of the current mechanical arm t And the joint angle theta of the target point g If the angle difference is smaller than the threshold value tau, returning to the step (2) to continuously generate the path point; if the threshold value is smaller than the threshold value, the step (6) is carried out;
(6) Local variable step search of joint angles:
Figure BDA0003377845940000041
Figure BDA0003377845940000042
Figure BDA0003377845940000043
Figure BDA0003377845940000044
Figure BDA0003377845940000045
wherein θ (θ) t ) As an adaptability evaluation standard of the joint angle;
the update of the local variable step is as follows:
Figure BDA0003377845940000046
Figure BDA0003377845940000047
in the middle of
Figure BDA0003377845940000048
M is the maximum iteration number of the local attenuation iteration for the minimum search step length;
(7) Determination of Path Point precision
Judging the space position distance between the current path point and the target path point, returning to the step (6) if the distance is not smaller than the precision threshold mu, and entering the step (8) if the distance is smaller than the precision threshold mu;
(8) Path pruning
Calculating the modulus of each path point under the angle change of each joint, and determining the point where the minimum joint change is located, wherein the specific formula is as follows:
Figure BDA0003377845940000049
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA00033778459400000410
representing an ith point in the planned path closest to the joint angle of the current point in the process of carrying out positive sequence or reverse sequence calculation on the planned path point;
marking the currently calculated path points and the points with the minimum joint change, smoothing the path points among the marked points, and replacing the joint angles of the path points among the marked points by using the joint angles of the points with the current minimum joint change.
According to the mechanical arm obstacle avoidance path planning method based on the improved longhorn beetle whisker search algorithm, provided by the invention, two mutually orthogonal search directions are generated, the space position coordinates of four judgment points are obtained, the global search capability can be improved, the initial path can be effectively simplified, the shake in the path is eliminated, and the safety of the mechanical arm in the operation process is ensured.
Drawings
FIG. 1 is a flow chart of a robot arm obstacle avoidance path planning method based on an improved longhorn beetle whisker search algorithm in an embodiment of the invention;
FIG. 2 is a model diagram of a model diagram a of a model diagram b of a coordinate system of a model diagram An Chuan manufactured MOTOMAN-ES165D manipulator with 6 degrees of freedom;
FIG. 3 is a schematic illustration of the position of the robotic arm and the distribution of obstacles of the present invention;
FIG. 4 is a schematic illustration of the present invention modeling of a robotic arm using convex hull structure approximation;
FIG. 5 is a schematic illustration of the present invention modeling of an obstacle approximated using an axis envelope method;
FIG. 6 is a simplified diagram of a decision point optimization portion employed in the present invention;
fig. 7 is a path information diagram of initial path generation;
FIG. 8 is a plot of the angular change of the joint generated by the initial path, a being the first 3 angular changes of the joint near the base, b being the last 3 angular changes of the joint near the end effector;
FIG. 9 is a pruned path information graph;
fig. 10 is a graph showing the variation trend of the joint angle of the path after trimming, a is the first 3 joint angle variations near the base, and b is the last 3 joint angle variations near the end effector.
Detailed Description
In order that the invention may be readily understood, a more particular description thereof will be rendered by reference to specific embodiments that are illustrated in the appended drawings. Preferred embodiments of the present invention are shown in the drawings. This invention may, however, be embodied in many different forms and should not be construed as limited to the embodiments set forth herein. Rather, these embodiments are provided so that this disclosure will be thorough and complete.
The embodiment 1 provides a mechanical arm obstacle avoidance path planning method based on an improved longhorn beetle whisker search algorithm, which can be applied to mechanical arm gesture planning, and the flow is shown in fig. 1, and specifically comprises the following steps:
firstly, establishing a mechanical arm mathematical model by utilizing a rotation theory, and establishing an optimized evaluation model of a gesture planning result so as to obtain an optimal mechanical arm joint angle and running path length
In this embodiment, a MOTOMAN-ES165D mechanical arm manufactured by Anchuan is used as a research object, and a mathematical model is built by using a rotation theory, and specific parameters are shown in FIG. 2.
For a serial manipulator with 6 degrees of freedom, the joint movements of the serial manipulator are integrated into a whole, so that an exponential product formula for representing the positive kinematic model of the manipulator can be obtained:
Figure BDA0003377845940000061
in the formula g st (θ) represents the pose matrix of the rigid body coordinate system of the end effector in the base coordinate system after the mechanical arm is articulated, g st (0) And (3) representing an initial pose matrix of a rigid body coordinate system of the end effector in a base coordinate system at zero time of each joint, wherein theta and xi respectively represent the joint angle and the joint rotation of the corresponding joint. Through an exponential product formula, a positive kinematic model represented by the kinematic rotations of all joints of the mechanical arm can be established.
The initial pose of the robotic arm is expressed as:
Figure BDA0003377845940000062
the exponential product formula of the mechanical arm can be obtained and can be expressed as follows:
Figure BDA0003377845940000063
wherein the mathematical expression formula of homogeneous coordinates is as follows:
n x ＝c 1 c 23 c 5 -s 5 (c 1 c 4 s 23 +s 1 s 4 )
n y ＝c 23 c 5 s 1 -s 5 (c 1 c 4 -c 4 s 1 s 23 )
n z ＝-c 3 (c 5 s 2 +c 2 c 4 s 5 )-s 3 (c 2 c 5 -c 4 s 2 s 5 )
o x ＝c 6 (c 1 s 23 s 4 -c 4 s 1 )+s 6 (c 5 s 1 s 4 +c 1 (c 4 c 5 s 23 +c 23 s 5 ))
o y ＝c 1 (c 4 c 6 -c 5 s 4 s 6 )+s 1 (c 6 s 23 s 4 +s 6 (c 4 c 5 s 23 +c 23 s 5 ))
o z ＝c 23 (c 6 s 4 +c 4 c 5 s 6 )-s 23 s 5 s 6
a x ＝s 1 (c 5 c 6 s 4 +c 4 s 6 )+c 1 (c 4 c 5 c 6 s 23 -s 4 s 6 s 23 +c 23 c 6 s 5 )
a y ＝c 6 (c 4 c 5 s 1 s 23 -c 1 c 5 s 4 +c 23 s 1 s 5 )-s 6 (c 1 c 4 +s 1 s 23 s 4 )
a z ＝c 23 (c 4 c 5 c 6 -s 4 s 6 )-c 6 s 23 s 5
p x ＝d 6 (c 1 s 2 s 3 -c 1 c 2 c 3 )c 4 s 5 -d 6 s 1 s 4 s 5 -(c 1 c 2 s 3 +c 1 s 2 c 3 )(d 4 +d 6 c 5 )+a 3 c 1 c 2 c 3 -a 3 c 1 s 2 s 3 +a 2 c 1 c 2
p y ＝d 6 (s 1 s 2 s 3 -s 1 c 2 c 3 )c 4 s 5 -d 6 c 1 s 4 s 5 -(s 1 c 2 s 3 +s 1 s 2 c 3 )(d 4 +d 6 c 5 )+a 3 s 1 c 2 c 3 -a 3 s 1 s 2 s 3 +a 2 s 1 c 2
p z ＝d 6 (s 2 c 3 +c 2 s 3 )c 4 s 5 +d 1 +(s 2 s 3 -c 2 c 3 )(d 4 +d 6 c 5 )-a 3 c 2 s 3 -a 2 s 2
wherein, the symbol c represents a cosine function in the trigonometric function, the symbol s represents a sine function, d represents a relative distance between the corresponding joints, s 23 Representation sin (θ) 2 +θ 3 ) And c 23 Represent cos (θ) 2 +θ 3 )。
Similarly, due to the six degrees of freedom serial mechanical arm, the fitness evaluation function is as follows:
Figure BDA0003377845940000071
Figure BDA0003377845940000072
in the formula, θ (θ t ) The sum of angle differences of six joints of the six-degree-of-freedom mechanical arm after N search iterations is the sum of angle differences of all path points; l (P) represents the moving distance of the mechanical arm end effector in space, and k represents the pathThe number of path points included in the list. M is the total number of path points.
R 6 Meaning that the variable is a vector of 6 elements, each element being a real number, belonging to the real number domain R (R 6 Can be solved into a vector containing 6 elements, where each element is a real number), R 3 And the same is true.
Step two, reading the positions of the starting point and the end point of the tail end of the mechanical arm, and solving the joint angles of the starting point and the end point by using an inverse kinematics analysis method
After the pose of the tail end of the mechanical arm is obtained, all joint angles of the mechanical arm are required to be obtained to be combined with a mechanical arm model established before, and the position of the mechanical arm in space is determined. Therefore, it is important to solve the joint angle by inverse kinematics.
Step three, establishing a mechanical arm and obstacle collision detection model
In the practical implementation process, the spatial position relationship between the mechanical arm and the obstacle is shown in fig. 3. The obstacle data are shown in table 1 below.
TABLE 1 Barrier model data
Figure BDA0003377845940000073
Because the actual mechanical arm model is complex, a corresponding convex hull model is established in the implementation process, and the implementation process is specifically shown in fig. 4. The simplified model of the obstacle adopts the envelope of the shaft body, and the expansion is also carried out in consideration of actual errors, and is shown in a specific view in fig. 5.
Generating an effective mechanical arm obstacle avoidance path by utilizing the improved BAS4-AO algorithm, wherein the effective mechanical arm obstacle avoidance path is specifically as follows:
(1) Initializing various parameters
The range of the respective joint angles is shown in table 2 below according to the arm parameters of the crash test.
TABLE 2 effective ranges for angles of joints of mechanical arms
Figure BDA0003377845940000081
The selection of algorithm-related parameters is shown in table 3 below based on experimental experience.
Table 3 path planning parameter configuration combinations
Figure BDA0003377845940000082
(2) Path point search direction generation
Compared with two judgment points of the traditional BAS algorithm, the improved BAS4-AO algorithm has four judgment points with different positions, as shown in fig. 6, so that two mutually orthogonal direction vectors are needed.
The search direction for each path point is calculated according to the following formula:
Figure BDA0003377845940000083
Figure BDA0003377845940000084
wherein a is 1 And a 2 Is two proportionality constants, and rand (]Random value on the same. In this embodiment, since a 1 Is 1, a 2 And if the result is 10, the search direction in the test is greatly influenced by the connecting line direction from the current point to the target point.
Figure BDA0003377845940000085
Representing a left to right direction vector; />
Figure BDA0003377845940000086
Representing the bottom-up direction vector.
Normalizing the direction vector:
Figure BDA0003377845940000091
Figure BDA0003377845940000092
Figure BDA0003377845940000093
Figure BDA0003377845940000094
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA0003377845940000095
in v (·) is an inverse kinematics equation of three-dimensional coordinate conversion into joint angle for the target guiding portion with joint information at the target point; wherein the original random direction->
Figure BDA0003377845940000096
Still remaining, constant vector x g Representing the coordinates of the target point.
(3) Path Point spatial location Generation
When iteration at the t-th moment is carried out, the spatial position coordinates of four judgment points around the spatial coordinate P are as follows:
Figure BDA0003377845940000097
Figure BDA0003377845940000098
Figure BDA0003377845940000099
Figure BDA00033778459400000910
wherein the search step delta * Is a dynamic adjustment factor that decays with the search iteration loop. In this embodiment, the search step size δ * The initial value was 0.8.
Then, pose information of the next iteration is generated:
Figure BDA00033778459400000911
R t+1 ＝R t +δ * *(R g -R t )*λ 2
wherein, L (P) is taken as the fitness evaluation standard of four judgment points, and a sign function min (·) is used for extracting the point with the minimum fitness function value in the four judgment points. Matrix R g R is the rotation matrix of the target point t For the rotation matrix of the current point, lambda 2 As a matching coefficient, delta * Herein as R g To R t Is equal to the global step size.
(4) Collision detection and search step adjustment
Performing collision detection on the mechanical arm shaft body and the obstacle, and if no collision exists, directly transferring to the step (2) to execute the next iteration cycle; if collision occurs, the global search step is attenuated, the path point information is cleared, and a new path point is searched again. The collision detection process is to project the mechanical arm convex hull simplified model and the obstacle model on three coordinate planes, and determine whether collision occurs by judging whether the projections are coincident.
The global search step numerical decay model is represented as follows:
Figure BDA0003377845940000101
wherein lambda is 1 Is a constant and the test was set to 0.998.
(5) Performing joint angle optimization judgment
Solving the joint angle of the mechanical arm according to the space coordinates of the end effector of the mechanical arm, and judgingCurrent mechanical arm end effector joint angle θ t And the joint angle theta of the target point g Whether the angle difference between them is smaller than a threshold τ; if not smaller than the threshold tau, returning to the step (2) to continuously generate the path point; if the joint angle is smaller than the threshold tau, the step (6) is started to optimize the joint angle.
(6) Local variable step exploration of joint angles
At the current manipulator end effector joint angle θ t And the joint angle theta of the target point g When the angle difference between them is smaller than the threshold τ, the search is shifted to the attitude control section that is dominant in angle optimization, specifically as follows:
Figure BDA0003377845940000102
Figure BDA0003377845940000103
Figure BDA0003377845940000104
Figure BDA0003377845940000105
Figure BDA0003377845940000106
wherein the iteration principle is similar to the iteration of the coordinate point, and the space coordinate P is calculated t Replaced by joint angle theta t The fitness evaluation function applies L (P) t ) Replaced by θ (θ) t )。
The update of the local variable step is as follows:
Figure BDA0003377845940000107
Figure BDA0003377845940000108
in the middle of
Figure BDA0003377845940000109
For the minimum search step, m is the maximum number of iterations of the local decay iteration. In such local iterative exploration, once a valid path point is searched, the local iteration is skipped and the global step is used again for optimization.
(7) Determination of Path Point precision
And (3) judging the spatial position distance between the current path point and the target path point, returning to the step (6) if the distance is not smaller than the precision threshold value mu, and entering the step (8) if the distance is smaller than the precision threshold value mu.
(8) Path pruning
The path information and joint change information generated by the trial are shown in fig. 7 and 8. As is evident from fig. 8, there is a strong fluctuation in the joint angle. Therefore, in order to ensure the safety of the operation of the mechanical arm, after the path is iteratively generated, singular points possibly existing in the path need to be removed through preprocessing.
In this embodiment, the modulus under the angle change of each joint at each path point and the subsequent points is calculated in sequence, and the point where the minimum joint change is located is determined. The specific formula is as follows:
Figure BDA0003377845940000111
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure BDA0003377845940000112
and the i-th point in the planning path closest to the joint angle of the current point in the process of carrying out positive sequence or reverse sequence calculation on the planning path point is represented.
Marking the currently calculated path points and the points with the minimum joint change, smoothing the path points among the marked points, and replacing the joint angles of the path points among the marked points by using the joint angles of the points with the current minimum joint change. The method is shown in fig. 9 and 10 for the path information and the joint change information after the path pruning.
Example 2 in order to compare the difference between the BAS4-AO algorithm and the existing RRT algorithm and BAS algorithm after the improvement of the present invention, the present example calculates experimental data of path planning for the robot arm using three algorithms under the same condition for a plurality of times, and compares the experimental data, and the results are shown in table 4.
Table 4 optimized comparison of the improved BAS4-AO algorithm and RRT algorithm, BAS algorithm
Figure BDA0003377845940000113
Compared with the traditional RRT algorithm, the time efficiency of the improved BAS4-AO algorithm is shortened by 87.62%, the path length is shortened by 29.83%, and the overall performance is superior to that of the application of the RRT algorithm in the gesture planning of the manipulator, so that the improved BAS4-AO algorithm has better path optimization capability. In addition, the four whiskers (four judgment points) in the invention have more selectable path directions compared with the original two whiskers of the BAS algorithm, have better global optimizing effect, shorten the path length by 18.87%, and improve the global searching capability by 13% compared with the original BAS algorithm.
The six-degree-of-freedom mechanical arm has very wide application in industrial production and has great research value. However, most of the currently used path planning algorithms are mainly random sampling path planning algorithms, and although proper motion paths can be successfully found, the algorithm operation efficiency is not high due to the fact that a large-scale random search is performed. The invention provides an improved BAS algorithm, namely BAS4-AO algorithm. Firstly, introducing a fitness function and a target position searching guide, so that searching is more targeted, and the running time of an algorithm is reduced; then, a mode of combining three-dimensional Cartesian coordinates and joint angle search is adopted, and the operation stability of the mechanical arm is ensured from multiple angles. As can be seen from the actual test results, compared with the RRT algorithm, the BAS4-AO not only can form a proper motion planning path, but also has faster operation efficiency and better operation result.

Claims (3)

1. The mechanical arm obstacle avoidance path planning method based on the improved longhorn beetle whisker search algorithm is characterized by comprising the following steps of:
establishing an optimized evaluation model of the attitude planning result; the method comprises the following steps of optimizing a function of the joint angle of the mechanical arm, wherein the expression is as follows:
Figure QLYQS_1
wherein->
Figure QLYQS_2
The sum of angle differences of the mechanical arm joint after N search iterations is obtained; variable->
Figure QLYQS_3
Is represented at the route pointiUp-sampling timetJoint angle at;Mrepresenting the total number of all path points in the path planning process; />
Figure QLYQS_4
Is a vector containing 6 elements, where each element is a real number;
a function for optimizing the length of a travel path, expressed as:
Figure QLYQS_5
in the method, in the process of the invention,
Figure QLYQS_6
representing the distance of movement of the end effector of the manipulator in space, variable +.>
Figure QLYQS_7
Representing the position of the end point of the manipulator at the sampling time t on the path point i; k represents the number of path points included in the path; />
Figure QLYQS_8
Is a vector containing 3 elements, where each element is a real number;
acquiring the starting point and the end point pose of the tail end of the mechanical arm, solving the joint angles of the starting point and the end point by using an inverse kinematics analysis method, and determining the position of the mechanical arm in space;
establishing a collision detection model of the mechanical arm and the obstacle;
according to a collision detection result, combining with an optimization evaluation model, generating a mechanical arm obstacle avoidance path by using an improved BAS4-AO algorithm, wherein the method comprises the following steps:
(1) Initializing various parameters;
(2) Generating two mutually orthogonal path point search directions, wherein the calculation formula of the search directions is as follows:
Figure QLYQS_9
；
Figure QLYQS_11
and->
Figure QLYQS_14
Is two proportionality constants, +.>
Figure QLYQS_15
Is uniformly distributed in->
Figure QLYQS_12
Random value on the table; />
Figure QLYQS_13
Is a vector of random units and is a vector of random units,
Figure QLYQS_16
is the direction vector of the target point relative to the current path point; />
Figure QLYQS_17
Representing left to right direction vectors；/>
Figure QLYQS_10
Representing a bottom-up direction vector;
before calculating the search direction, the direction vector needs to be normalized, and the formula is as follows:
Figure QLYQS_18
；
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure QLYQS_19
for a target guiding part with joint information at the target point, < >>
Figure QLYQS_20
Converting the three-dimensional coordinates into inverse kinematics equations of joint angles; wherein the original random direction->
Figure QLYQS_21
Still reserved, constant vector->
Figure QLYQS_22
Representing coordinates of the target point;
(3) Path Point spatial location Generation
When iteration at the t-th moment is carried out, the spatial position coordinates of four judgment points around the spatial coordinate P are as follows:
Figure QLYQS_23
；
wherein the search step size
Figure QLYQS_24
Is a dynamic adjustment factor, decays with the search iteration loop;
then, pose information of the next iteration is generated:
Figure QLYQS_25
；
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure QLYQS_27
as an fitness evaluation criterion for four decision points, the sign function +.>
Figure QLYQS_30
The method is used for extracting the point with the minimum fitness function value from the four judgment points; matrix->
Figure QLYQS_32
For the rotation matrix of the target point +.>
Figure QLYQS_26
For the rotation matrix of the current point, in +.>
Figure QLYQS_29
As a matching coefficient +.>
Figure QLYQS_31
As->
Figure QLYQS_33
To->
Figure QLYQS_28
Is a step of approximation;
(4) Collision detection and search step adjustment
Performing collision detection on the mechanical arm shaft body and the obstacle, and if no collision exists, directly transferring to the step (2) to execute the next iteration cycle; if collision occurs, the global searching step length is attenuated, the path point information is cleared, and new path points are searched again;
(5) Performing joint angle optimization judgment
Solving the joint angle of the mechanical arm according to the spatial coordinates of the end effector of the mechanical arm, and judging the joint angle of the end effector of the current mechanical arm
Figure QLYQS_34
And target point joint angle>
Figure QLYQS_35
Whether the angle difference between them is smaller than a threshold +.>
Figure QLYQS_36
If it is not less than threshold +.>
Figure QLYQS_37
Returning to the step (2) to continuously generate the path points; if less than threshold->
Figure QLYQS_38
Then go to step (6);
(6) Local variable step search of joint angles:
Figure QLYQS_39
;
Figure QLYQS_40
;
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure QLYQS_41
as an adaptability evaluation standard of the joint angle;
the update of the local variable step is as follows:
Figure QLYQS_42
；
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure QLYQS_43
m is the maximum iteration number of the local attenuation iteration for the minimum search step length;
(7) Determination of Path Point precision
Judging the space position distance between the current path point and the target path point, if the distance is not less than the accuracy thresholdValue of
Figure QLYQS_44
Returning to step (6) if less than the accuracy threshold +.>
Figure QLYQS_45
Step (8) is entered;
(8) Path pruning
Calculating the modulus of each path point under the angle change of each joint, and determining the point where the minimum joint change is located, wherein the specific formula is as follows:
Figure QLYQS_46
；
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure QLYQS_47
representing an ith point in the planned path closest to the joint angle of the current point in the process of carrying out positive sequence or reverse sequence calculation on the planned path point;
marking the currently calculated path points and the points with the minimum joint change, smoothing the path points among the marked points, and replacing the joint angles of the path points among the marked points by using the joint angles of the points with the current minimum joint change.
2. The method for planning the obstacle avoidance path of the mechanical arm based on the improved longhorn beetle whisker search algorithm according to claim 1, wherein the time optimization function expression completed by the algorithm is as follows:
Figure QLYQS_48
；
wherein, the liquid crystal display device comprises a liquid crystal display device,
Figure QLYQS_49
starting point for algorithm optimization, < > for>
Figure QLYQS_50
A time point when the manipulator operation is completed; number of search iterations
Figure QLYQS_51
，/>
Figure QLYQS_52
Is the sampling interval.
3. The method for planning the obstacle avoidance path of the mechanical arm based on the improved longhorn beetle whisker search algorithm according to claim 1, wherein in the collision detection model, a convex hull simplified model is used as a mechanical arm model, and a shaft envelope model is used as an obstacle model.
CN202111424954.1A 2021-11-26 2021-11-26 Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm Active CN114147708B (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202111424954.1A CN114147708B (en) 2021-11-26 2021-11-26 Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202111424954.1A CN114147708B (en) 2021-11-26 2021-11-26 Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm

Publications (2)

Publication Number Publication Date
CN114147708A CN114147708A (en) 2022-03-08
CN114147708B true CN114147708B (en) 2023-06-30

Family

ID=80458102

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202111424954.1A Active CN114147708B (en) 2021-11-26 2021-11-26 Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm

Country Status (1)

Country Link
CN (1) CN114147708B (en)

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN115014375B (en) * 2022-06-06 2023-11-03 北京京深深向科技有限公司 Collision detection method and device, electronic equipment and storage medium
CN115648220A (en) * 2022-11-15 2023-01-31 华侨大学 Mechanical arm joint space obstacle avoidance path planning method based on minimum cost reduction
CN116522234B (en) * 2023-02-10 2023-11-17 东莞理工学院 Joint angle prediction method, device, equipment and medium

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112975961A (en) * 2021-02-20 2021-06-18 华南理工大学 Picking mechanical arm motion planning method based on CTB-RRT algorithm

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN109866222B (en) * 2019-02-26 2020-09-01 杭州电子科技大学 Mechanical arm motion planning method based on longicorn stigma optimization strategy
CN111173573B (en) * 2020-01-08 2022-07-19 上海电力大学 Identification method for power object model of steam turbine regulating system
CN111844023A (en) * 2020-06-28 2020-10-30 合肥工业大学 Six-degree-of-freedom robot inverse solution method based on longicorn whisker algorithm
CN113352319B (en) * 2021-05-08 2022-10-21 上海工程技术大学 Redundant mechanical arm obstacle avoidance trajectory planning method based on improved fast expansion random tree

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN112975961A (en) * 2021-02-20 2021-06-18 华南理工大学 Picking mechanical arm motion planning method based on CTB-RRT algorithm

Also Published As

Publication number Publication date
CN114147708A (en) 2022-03-08

Similar Documents

Publication Publication Date Title
CN114147708B (en) Mechanical arm obstacle avoidance path planning method based on improved longhorn beetle whisker search algorithm
CN110228069B (en) Online obstacle avoidance motion planning method for mechanical arm
CN109571466B (en) Seven-degree-of-freedom redundant mechanical arm dynamic obstacle avoidance path planning method based on rapid random search tree
CN109960880B (en) Industrial robot obstacle avoidance path planning method based on machine learning
CN110653805B (en) Task constraint path planning method for seven-degree-of-freedom redundant manipulator in Cartesian space
CN111251295A (en) Visual mechanical arm grabbing method and device applied to parameterized parts
CN110561419B (en) Arm-shaped line constraint flexible robot track planning method and device
Yang et al. Real-time optimal navigation planning using learned motion costs
CN111716352B (en) Power distribution network live working mechanical arm navigation obstacle avoidance method and system
CN115416016A (en) Mechanical arm obstacle avoidance path planning method based on improved artificial potential field method
Liu et al. Improved RRT path planning algorithm for humanoid robotic arm
Tarbouriech et al. Bi-objective motion planning approach for safe motions: Application to a collaborative robot
Zhang et al. Safe and efficient robot manipulation: Task-oriented environment modeling and object pose estimation
CN113043278B (en) Mechanical arm track planning method based on improved whale searching method
Xu et al. Efficient object manipulation to an arbitrary goal pose: Learning-based anytime prioritized planning
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
CN116872212A (en) Double-mechanical-arm obstacle avoidance planning method based on A-Star algorithm and improved artificial potential field method
Alatartsev et al. Improving the sequence of robotic tasks with freedom of execution
Hsu Obstacle avoidance path scheme of snake robot based on bidirectional fast expanding random tree algorithm
CN114454160B (en) Mechanical arm grabbing control method and system based on kernel least square soft Belman residual error reinforcement learning
Li et al. Research on the agricultural machinery path tracking method based on deep reinforcement learning
Zha et al. Robot motion planning method based on incremental high-dimensional mixture probabilistic model
Tang et al. Picking trajectory planning of citrus based on improved immune algorithm and binocular vision
CN111912411A (en) Robot navigation positioning method, system and storage medium
Lin et al. A search strategy for motion planning of unmanned crawler crane

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