CN113589809B - Work track planning method and device for obstacle-avoidance excavator working device - Google Patents
Work track planning method and device for obstacle-avoidance excavator working device Download PDFInfo
- Publication number
- CN113589809B CN113589809B CN202110842844.0A CN202110842844A CN113589809B CN 113589809 B CN113589809 B CN 113589809B CN 202110842844 A CN202110842844 A CN 202110842844A CN 113589809 B CN113589809 B CN 113589809B
- Authority
- CN
- China
- Prior art keywords
- excavator
- working device
- joint
- track
- motion
- 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 87
- 230000033001 locomotion Effects 0.000 claims abstract description 114
- 230000009466 transformation Effects 0.000 claims description 26
- 239000011159 matrix material Substances 0.000 claims description 25
- 230000001133 acceleration Effects 0.000 claims description 22
- 238000004364 calculation method Methods 0.000 claims description 20
- 230000010349 pulsation Effects 0.000 claims description 18
- 230000009471 action Effects 0.000 claims description 9
- 230000004888 barrier function Effects 0.000 claims description 9
- 230000011218 segmentation Effects 0.000 claims description 8
- 238000009412 basement excavation Methods 0.000 claims description 5
- 238000012216 screening Methods 0.000 claims description 5
- 230000008569 process Effects 0.000 abstract description 20
- 230000002265 prevention Effects 0.000 abstract description 2
- 239000002245 particle Substances 0.000 description 19
- 238000005457 optimization Methods 0.000 description 16
- 230000003044 adaptive effect Effects 0.000 description 14
- 238000006243 chemical reaction Methods 0.000 description 8
- 230000002068 genetic effect Effects 0.000 description 7
- 238000009434 installation Methods 0.000 description 7
- 238000013519 translation Methods 0.000 description 6
- 238000011161 development Methods 0.000 description 5
- 230000018109 developmental process Effects 0.000 description 5
- 230000006978 adaptation Effects 0.000 description 4
- 239000012636 effector Substances 0.000 description 4
- 230000035772 mutation Effects 0.000 description 4
- 230000008859 change Effects 0.000 description 2
- 238000007405 data analysis Methods 0.000 description 2
- 238000006073 displacement reaction Methods 0.000 description 2
- 230000003137 locomotive effect Effects 0.000 description 2
- 238000004519 manufacturing process Methods 0.000 description 2
- 238000011160 research Methods 0.000 description 2
- 238000010187 selection method Methods 0.000 description 2
- 241000282414 Homo sapiens Species 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 238000005265 energy consumption Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 230000007613 environmental effect Effects 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000000926 separation method Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0231—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means
- G05D1/0238—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors
- G05D1/024—Control of position or course in two dimensions specially adapted to land vehicles using optical position detecting means using obstacle or wall sensors in combination with a laser
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0276—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle
- G05D1/0278—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using satellite positioning signals, e.g. GPS
Landscapes
- Engineering & Computer Science (AREA)
- Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Operation Control Of Excavators (AREA)
Abstract
The invention discloses a working track planning method and a working track planning device for an obstacle-avoidance excavator, wherein the method comprises the following steps: generating a motion obstacle avoidance path point of an excavator working device; planning a motion track of the excavator working device based on the motion obstacle avoidance path points to generate an optimal path; and controlling the movement of the excavator working device based on the optimal path. The invention not only enables the working device of the excavator to realize obstacle avoidance and collision prevention in the motion process, but also meets the operation constraint of each node of the working device of the excavator, and can reach the aim of optimizing time, thereby improving the working efficiency of the working device of the excavator and relieving the limit of the excavating operation on the dependence of professional talents.
Description
Technical Field
The invention belongs to the technical field of operation track planning of excavating arms, and particularly relates to an operation track planning method and device for an excavator working device capable of avoiding barriers.
Background
With the continuous progress of society, in order to better serve human beings, the requirements on the working quality of the excavator are also urgent to be improved, and how to improve the working efficiency and the working reliability of the excavator is urgent. And the track planning is carried out on the excavator, so that the excavator can rapidly and stably complete the excavating task, the problems can be effectively solved, and the overall performance of the excavator is improved. The main content of the track planning research is that firstly, according to the environmental information, an obstacle avoidance path point is obtained through an obstacle avoidance algorithm, then, the problems of displacement, speed and acceleration of each joint of the excavator arm are planned on the basis, and finally, the tail end of the excavator arm is enabled to finish operation under a certain working condition. The track planning of the excavator arm is a practical application of the problems of forward kinematics and reverse kinematics of the excavator arm, so that when the track planning of the excavator arm is performed, a certain principle is required to be followed, and a reasonable method is adopted to avoid abrupt changes of displacement, speed and acceleration of the excavator arm.
Therefore, the track optimization research of the excavator arm is an important component for intelligent transformation and upgrading of the excavator arm, the intelligent development of the excavator can promote the industrial structure adjustment of the excavator, and the intelligent development of the excavator also needs to drive the development of other industries, so that the intelligent development of the excavator arm has extremely important significance for the realization of the automation technology in the industrial field and even the comprehensive development of national economy.
The Chinese patent application with publication number of CN107186713A discloses that an optimal motion time sequence is solved by a path point mechanical arm multi-axis motion planning optimization method, and the time sequence is brought into a mechanical arm joint motion track equation, so that an optimal planning curve of the path point mechanical arm multi-axis continuous motion is obtained. The obstacle avoidance problem in the movement process of the mechanical arm is not considered in the scheme, and a large risk exists in the practical application process.
The chinese patent application publication No. CN108356819a discloses that the search step length of the conventional a-algorithm is set to be a variable step length, and a collision detection algorithm based on a separation axis is used to determine whether the mechanical arm collides with the environment in each step of the search, and search is performed in a multidimensional space of the mechanical arm with six degrees of freedom, so as to obtain a collision-free motion path. In the scheme, the optimization process such as time optimization, energy consumption optimization and the like is not performed.
Disclosure of Invention
Aiming at the problems, the invention provides the working track planning method and the working track planning device for the working device of the excavator, which not only enable the working device of the excavator to realize obstacle avoidance and collision prevention in the motion process, but also meet the operation constraint of each node of the working device of the excavator, and simultaneously achieve the aim of optimizing time, thereby improving the working efficiency of the working device of the excavator and relieving the limit of the excavating operation on the dependence of professional talents.
In order to achieve the technical purpose and achieve the technical effect, the invention is realized by the following technical scheme:
in a first aspect, the present invention provides a method for planning an operation track of an obstacle avoidance excavator working device, including:
generating a motion obstacle avoidance path point of an excavator working device;
planning a motion track of the excavator working device based on the motion obstacle avoidance path points to generate an optimal path;
and controlling the movement of the excavator working device based on the optimal path.
Optionally, the method for generating the motion obstacle avoidance path point includes:
obtaining position information and shape information of an obstacle;
and planning an obstacle avoidance path point based on the coordinate information of the movement starting point and the movement target point of the excavator working device, the position information and the shape information of the obstacle.
Optionally, the planning of the obstacle avoidance path point based on the coordinate information of the movement starting point and the movement target point of the excavator working device, the position information and the shape information of the obstacle is specifically:
setting the grid position and the area of the barrier according to the position information and the shape information of the barrier;
according to the coordinate information of the motion starting point and the motion target point at the tail end of the excavator working device, rasterizing the motion space of the excavator working device, and taking the center of the grid as a node;
and (3) performing obstacle avoidance path planning according to the priority of the nodes based on the grid position and area of the obstacle and the movement space of the rasterized excavation working device by using an A-scale algorithm.
Optionally, the a-algorithm calculates the priority of each node by the following function:
F(n)＝G(n)+H(n)
where F (n) is the comprehensive priority of node n, G (n) is the cost of node n from the start point, H (n) is the predicted cost of node n from the end point, and when the next node to be traversed is selected, the node with the smallest comprehensive priority value is always selected.
Optionally, the method for generating the optimal path includes:
based on the motion obstacle avoidance path points, establishing a cubic polynomial by utilizing interpolation function segmentation;
And solving each coefficient of the cubic polynomial by taking the shortest action time as an objective function and taking the constraint condition that the motion speed, the acceleration and the pulsation curve do not exceed the corresponding constraint ranges to obtain an optimal path.
Optionally, the third order polynomial is:
P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0
wherein P is ji (t) represents an i-th track expression of a j-th joint, in which j=1, 2,3, …, N in the mechanical arm of N joints, the track of each joint may be divided into i=1, 2,3, …, N-1 when the excavator working device has N intermediate points in total from the start point to the target point; s is(s) ji3 Representing the coefficients of the third order term in the j-th joint, i-th track, s ji2 Representing coefficients of quadratic terms in the j-th joint, i-th track, s ji1 Representing the coefficients of the j-th joint, the primary term in the i-th track, s ji0 The coefficients of constant terms in the j-th joint and i-th track are represented;
the objective function is:
the constraint conditions are as follows:
wherein g is the total time taken by the working device to move, P j ' i (t)，P j ” i (t)，P j ” i 't' represents the trajectory function P respectively ji (t) first, second, third derivatives, V j,max Is the speed constraint condition of the j-th joint, W j,max K being the acceleration constraint of the j-th joint j,max Is the pulsation constraint condition of the j-th joint, h j,i ＝t j,i+1 -t j,i The time required for the j-th joint to track in the i-th segment.
Optionally, before the step of establishing the cubic polynomial by using interpolation function segmentation, the method further comprises:
selecting an optimal path point by taking the motion obstacle avoidance path point as a constraint condition and taking the minimum sum of angle variation of each joint of the excavator working device as a target;
the cubic polynomial is built in segments by interpolation function based on the screened optimal path point.
Optionally, the method for screening the optimal path point specifically includes:
rotating and translating the coordinate system where the working device of the excavator is positioned to obtain a homogeneous transformation matrix T of the ith coordinate system relative to the coordinate system of the vehicle body i 0 ；
The method comprises the steps of obtaining joint variables in an excavator working device according to the position of the tail end of the excavator working device by using an inverse kinematics method, wherein in the inverse kinematics method, an objective function min (|alpha) is obtained i -α i-1 |+|β i -β i-1 |+|θ i -θ i-1 I), wherein alpha i ，β i ，θ i The positions of the tail ends of the working devices of the excavator are based on the data in a GPS coordinate system and the data in a local horizontal coordinate system.
Optionally, the homogeneous transformation matrix T i 0 The calculation formula of (2) is as follows:
in a second aspect, the present invention provides an obstacle avoidance excavator working device working path planning apparatus, comprising:
The obstacle avoidance unit is used for generating a motion obstacle avoidance path point of the excavator working device;
the planning unit is used for planning the motion trail of the excavator working device based on the motion obstacle avoidance path points to generate an optimal path;
and a control unit for controlling the movement of the excavator working device based on the optimal path.
Compared with the prior art, the invention has the beneficial effects that:
in the process of solving the optimized path, the invention takes the motion speed, the acceleration and the pulsation speed of the working device as constraint conditions and takes the shortest motion time as an optimization target, thereby greatly improving the working efficiency of the excavator and relieving the limit of the excavator operation on the dependence of professional talents while realizing the autonomous obstacle avoidance and excavation of the excavator.
Drawings
In order that the invention may be more readily understood, a more particular description of the invention will be rendered by reference to specific embodiments that are illustrated in the appended drawings, in which:
FIG. 1 is a flow chart of an implementation of one embodiment of the present invention;
FIG. 2 is a schematic view of the installation of sensors according to one embodiment of the invention;
FIG. 3 is a coordinate system of an excavator arm according to an embodiment of the present invention;
fig. 4 is a flowchart of an algorithm a according to an embodiment of the present invention;
FIG. 5 is a flow chart of a particle swarm algorithm according to an embodiment of the invention;
FIG. 6 is a flowchart of a genetic algorithm according to an embodiment of the present invention;
wherein: the system comprises a 1-laser radar, a 2-long-distance millimeter wave radar, a 3-industrial personal computer, a 4-dip angle sensor, a 5-length sensor, a 6-inertial navigation system and a controller, and a 7-GPS antenna.
Detailed Description
The present invention will be described in further detail with reference to the following examples in order to make the objects, technical solutions and advantages of the present invention more apparent. It should be understood that the detailed description and specific examples are intended for purposes of illustration only and are not intended to limit the scope of the invention.
The principle of application of the invention is described in detail below with reference to the accompanying drawings.
Example 1
The embodiment of the invention provides a working track planning method for an obstacle-avoidance excavator working device, which specifically comprises the following steps:
step (1) generating a motion obstacle avoidance path point of an excavator working device;
step (2) planning a motion track of the excavator working device based on the motion obstacle avoidance path points to generate an optimal path;
and (3) controlling the movement of the working device of the excavator based on the optimal path.
In a specific implementation manner of the embodiment of the present invention, the method for generating the motion obstacle avoidance path point includes:
Obtaining position information and shape information of an obstacle;
planning an obstacle avoidance path point based on the coordinate information of a movement starting point and a movement target point of the excavator working device, the position information and the shape information of the obstacle;
in the specific implementation process, in order to collect the position information and the shape information of the obstacle, the arrangement of the sensors needs to be performed, and the arrangement manner shown in fig. 2 may be adopted:
1) The right front side of the excavator is provided with: 2 laser radars 1, 1 long-distance millimeter wave radar 2;
2) Symmetrically installing the two sides of the excavator body: 1 laser radar 1 each;
3) And (3) rear installation of the excavator: 2 laser radars 1, 1 long-distance millimeter wave radar 2;
4) Cab top mounting: 1 laser radar 1;
5) The interior installation of the excavating locomotive: 1 exchanger, 1 industrial personal computer 3;
6) Excavator arm installation: 1 laser radar 1;
7) The excavator working device is installed: an inclination sensor 4 and a length sensor 5; the excavator working device includes: boom, stick, and bucket;
8) The excavator is provided with an inertial navigation system (adopting a GPS coordinate system), a controller 6 and a GPS antenna 7.
The method comprises the following steps of:
Setting the grid position and the area of the barrier according to the position information and the shape information of the barrier;
according to the coordinate information of the motion starting point and the motion target point at the tail end of the excavator working device, rasterizing the motion space of the excavator working device, and taking the center of the grid as a node;
and carrying out obstacle avoidance path planning according to the priority of the nodes based on the grid position and the area of the obstacle and the movement space of the rasterized excavation working device by using an A-scale algorithm, wherein the A-scale algorithm calculates the priority of each node by the following functions:
F(n)＝G(n)+H(n)
where F (n) is the comprehensive priority of node n, G (n) is the cost of node n from the start point, H (n) is the predicted cost of node n from the end point, and when the next node to be traversed is selected, the node with the smallest comprehensive priority value is always selected.
The flow chart of algorithm a is shown in fig. 4, and the specific steps are as follows:
step 1: initializing an open list and a close list to be empty, and adding a starting point into the open list; the open list is ended if empty, otherwise, the open list is continued;
step 2: calculating the F value of each node in the open list, wherein F=G+H, and the calculation of H uses a Manhattan method to find out the node n with the minimum F value in the open list, and updating the father node of each node according to the F value;
Step 3: removing the node n from the open list and adding the node n to the close list;
step 4: if the node n is the target node, a path is obtained, the path is traced back from the end point to the starting point, and otherwise, the path is continued;
step 5: if the node n has no child node, returning to the step 2, otherwise continuing;
step 6: searching nodes adjacent to the node n, skipping nodes in the close list and unreachable nodes, adding the adjacent nodes into the open list, and returning to the step 2.
Step 7: the nodes in the open list constitute feasible path points that correspond to the center of each grid.
In a specific implementation manner of the embodiment of the present invention, the method for generating the optimal path includes:
selecting an optimal path point by taking the motion obstacle avoidance path point as a constraint condition and taking the minimum sum of angle variation of each joint of the excavator working device as a target;
based on the optimal path point, establishing a cubic polynomial by utilizing interpolation function segmentation;
and solving each coefficient of the cubic polynomial by taking the shortest action time as an objective function and taking the constraint condition that the motion speed, the acceleration and the pulsation curve do not exceed the corresponding constraint ranges to obtain an optimal path.
The third order polynomial is:
P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0
wherein P is ji (t) represents the ith track expression of the jth joint, i.e., a track function; j=1, 2,3, …, N in the mechanical arm of the N joints, when the excavator working device has N intermediate points from the starting point to the target point, the track of each joint may be divided into the i=1, 2,3, …, N-1 segments; s is(s) ji3 Representing the coefficients of the third order term in the j-th joint, i-th track, s ji2 Representing coefficients of quadratic terms in the j-th joint, i-th track, s ji1 Representing the coefficients of the j-th joint, the primary term in the i-th track, s ji0 The coefficients of constant terms in the j-th joint and i-th track are represented;
the objective function is:
the constraint conditions are as follows:
wherein g is the total time taken by the working device to move, P j ' i (t)，P j ” i (t)，P j ” i 't' represents the trajectory function P respectively ji (t) first, second, third derivatives, V j,max Is the speed constraint condition of the j-th joint, W j,max K being the acceleration constraint of the j-th joint j,max Is the pulsation constraint condition of the j-th joint, h j,i ＝t j,i+1 -t j,i The time required for the j-th joint to track in the i-th segment.
In a specific implementation manner of the embodiment of the present invention, the method for screening the optimal path point specifically includes:
rotating and translating the coordinate system where the working device of the excavator is positioned to obtain a homogeneous transformation matrix T of the ith coordinate system relative to the coordinate system of the vehicle body i 0 ；
The method comprises the steps of obtaining joint variables in an excavator working device according to the position of the tail end of the excavator working device by using an inverse kinematics method, wherein in the inverse kinematics method, an objective function min (|alpha) is obtained i -α i-1 |+|β i -β i-1 |+|θ i -θ i-1 I), wherein alpha i ，β i ，θ i The positions of the tail ends of the working devices of the excavator are based on the data in a GPS coordinate system and the data in a local horizontal coordinate system.
The homogeneous transformation matrix T i 0 The calculation formula of (2) is as follows:
specifically, the generation process of the optimal path is as follows:
1) Excavator working device pose and motion description:
the coordinate system of the working device of the excavator is shown in fig. 3, wherein the establishment of the coordinate system accords with the right-hand rule, and after the coordinate system is established, the coordinate system o can be established through 4 steps i-1 x i-1 y i-1 z i-1 Transform to the next coordinate system o i x i y i z i ：
Step 1: around z i-1 Shaft rotation theta i Degree, x i-1 And x i The axes become the same direction, and this transformation matrix is denoted as Rot (Z, θ) i ) Z represents rotation about the Z axis;
step 2: along z i-1 Axial translation d i In units of (2) such thatx i-1 And x i The axes are collinear, and the transformation matrix is denoted as Trans (0, d) i )；
Step 3: along x i-1 Axial translation a i The origin of the two coordinate systems is superimposed, and the transformation matrix is denoted as Trans (. Alpha.) i ,0,0)；
Step 4: around x i Shaft rotation a i Degree, z i-1 And z i The axes are collinear, the two coordinate systems coincide with each other, and the transformation matrix is denoted Rot (X, alpha) i ) X represents rotation about the X axis;
relation between the (i-1) th coordinate system and the (i) th coordinate system can be used as matrixRepresentation, wherein:
according to the translation matrix and the rotation homogeneous transformation formula, the method can be as follows:
thus (2)
Homogeneous transformation matrix T of ith coordinate system relative to vehicle body coordinate system i 0 Can be expressed by the product of homogeneous transformation matrixes of continuous coordinate systems, namely
In the embodiment of the invention, the joint variables of the excavator working device are obtained according to the position of the tail end of the excavator working device (namely the tail end of the tooth tip of the bucket) by using an inverse kinematics method. Since the excavator working device has only three joints, the following steps are available:
according to the above, can obtain
Wherein E is an identity matrix.
The position of the tail end of the excavator working device is based on the conversion of data in a GPS coordinate system into data in a local horizontal coordinate system, and the specific conversion process is as follows:
the vehicle position and the surrounding obstacle position adopt a coordinate position-an earth coordinate system under an absolute coordinate system. Because the GPS data read through the GPS antenna and inertial navigation is a geographic coordinate system (WGS 84), the GPS data needs to be analyzed and coordinate transformed to be converted into rectangular coordinate system data which points to the right east along the X axis, points to the right north along the Y axis, and the origin is positioned at the mass center of the inertial navigation unit; the parsing and the coordinate transformation can be realized through the calculation of a Gaussian forward calculation formula. The specific method comprises the following steps:
The inertial navigation unit can output the acquired GPS data to the industrial personal computer, and the GPS data adopts a GPRMC format (namely minimum positioning information) and comprises information such as time date, longitude and latitude, heading, magnetic declination and the like; creating a GPS data structure body in a system, and carrying out data analysis on the obtained GPRMC data to obtain longitude (L) and latitude (B) data; because the acquired GPS data are the earth coordinates and cannot be directly used for an excavator, the L, B coordinates are projected by a Gaussian projection method (Gaussian forward calculation formula) to obtain Gaussian plane rectangular coordinates, and the achievable precision of the Gaussian forward calculation formula is 0.1 meter, wherein the specific formula is as follows:
X＝m 0 +m 2 l 2 +m 4 l 4
Y＝n 1 l+n 3 l 3 +n 5 l 5
wherein l is warp beam, m 0 ,n 1 ,m 2 ,. are undetermined coefficients that are a function of dimension B; (X, Y) is the transformed coordinates. And then converting the Gaussian plane rectangular coordinate into a local horizontal coordinate, wherein the obtained coordinate system has positive y-axis east and positive x-axis north, is not uniform with the definition of a customary coordinate system, and can obtain a universal coordinate system through coordinate conversion. The conversion formula is as follows:
by the method, GPS data in the WGS84 coordinate system is converted into a local horizontal coordinate system with the X axis pointing to the east and the Y axis pointing to the north, and the origin is located in the mass center of the sensor.
According to the converted GPS data of the vehicle body, the angles of all joints of the working device of the excavator can be acquired through sensor data, and the tail end position and the tail end gesture of the working device of the excavator can be obtained through positive kinematics of the mechanical arm.
In the motion inverse solution calculation process of the excavator working device based on the obtained path points, the optimization function is that the sum of the angle change amounts of all joints is minimum, namely the motion path is shortest, namely min (|alpha) i -α i-1 |+|β i -β i-1 |+|θ i -θ i-1 I), constraint is path planning as described above, where α i ，β i ，θ i Respectively represent the angles of the motion of the three joints in the ith section of track,and then carrying out optimization solution by adopting a particle swarm optimization algorithm, wherein a flow chart of the particle swarm optimization algorithm is shown in fig. 5. The algorithm comprises the following specific steps:
step 1: initializing a population of particles, including random positions and velocities;
step 2: evaluating the fitness of each particle;
step 3: for each particle, comparing the adaptive value with the best position pbest (namely, the local optimal position) passed by the particle, and if the adaptive value is better, taking the particle as the current best position gbest (namely, the global optimal position);
step 4: adjusting the speed and the position of the particles according to the steps 2 and 3;
step 5: and if the iteration termination condition is not met, continuing to step 2.
The iteration termination condition is selected as the optimal position searched by the maximum iteration times or the particle swarm before the maximum iteration times, namely the result is converged, and the optimal path point is screened out.
2) Calculating a trajectory function of an excavator work device
The cubic polynomial interpolation function (namely, the cubic spline interpolation function) is the lowest order polynomial capable of ensuring continuous position, speed and acceleration, and therefore, the method is one of widely used and simple in calculation. Therefore, the three-time multi-pattern bar function method is adopted to plan the motion trail of the excavator in the joint space, so that the controlled variable is related to the motion time, and the calculation process is simple and convenient during trail planning.
The general form of a cubic spline polynomial trajectory can be expressed as:
P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0
wherein P is ji (t) represents an i-th track expression of the j-th joint, j=1, 2,3, …, N in the mechanical arm of the N joints, and when the mechanical arm has N intermediate points (including a start point and an end point) in total from the start point to the target point, the track of each joint may be divided into i=1, 2,3, …, N-1 segments. s is(s) ji3 Representing the third order in the j-th joint and the i-th trackCoefficient s of (2) ji2 Representing coefficients of quadratic terms in the j-th joint, i-th track, s ji1 Representing the coefficients of the j-th joint, the primary term in the i-th track, s ji0 The coefficients of the constant term in the j-th joint, i-th track are shown. Since the end effector is the result of the co-action of the individual joints, the path of motion of the end effector is also divided into n-1 segments, since each joint has n-1 segments of trajectory.
3) Excavator working device track planning based on adaptive value function
The final goal of trajectory planning is to find P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0 And ensure that each coefficient obtained is substituted into P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0 And each curve in the obtained joint angle, speed, acceleration and pulsation curves can meet the corresponding constraint conditions.
For industrial application of the excavator, the motion speed of the excavator is closely related to the production efficiency, and in order to maximally improve the efficiency of the excavator, the invention optimizes the action time of the excavator to the minimum. In the process of solving the track function expression, the joint tracks meet the kinematic constraint condition, and the shortest total motion time is ensured. The essence of this problem is how to reasonably solve the problem of time intervals between two adjacent nodes when the excavator work device is in operation, so that the total motion time of the sum of all the time intervals is minimized. In addition, when the track planning is performed by using the optimized time, it is necessary to ensure that the speed, acceleration and pulsation curves of the track cannot exceed the corresponding constraint ranges, otherwise, the optimized time is invalid, and the track planning must be performed again.
The method optimizes all joints simultaneously, and is characterized in that the solving process is complex, but the movement time of all joints is the same, the action continuity is good, and the control is simple.
The following definition is made for a boom with N joints:
the optimization objective function is:
the constraint conditions are as follows:
wherein g is the total time taken by the working device to move, P j ' i (t)，P j ” i (t)，P j ” i 't' represents the trajectory function P respectively ji (t) first, second, third derivatives, V j,max Is the speed constraint condition of the j-th joint, W j,max K being the acceleration constraint of the j-th joint j,max Is the pulsation constraint condition of the j-th joint, h j,i ＝t j,i+1 -t j,i For the time required for the j-th joint to track in the i-th segment,
and taking the total time as an optimized adaptive value function, and carrying out optimizing solution. The invention utilizes genetic algorithm to solve, see figure 6. If the solution searched by the genetic algorithm meets the constraint condition, the iteration is continued after the adaptive value is calculated, and if the solution searched by the genetic algorithm does not meet the constraint condition, the adaptive value is set to be a great number.
The method comprises the following specific steps:
step 1: creating and initializing a certain fixed number of time populations, wherein individuals in the populations are the movement time of each joint;
step 2: updating each coefficient of the track function by the time population;
Step 3: bringing time intervals in a time population to P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0 Calculate P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0 Maximum value of the correlation constraint;
step 4: judging whether the obtained result value is larger than the range of the constraint condition, if not, executing the step 5, otherwise, executing the step 6;
step 5: based onCalculating the adaptation value of each body in the population, namely calculating the total time of the motion trail, and continuously executing the step 7;
step 6: assigning the fitness to a larger number which is far larger than the maximum value of the initializing time population, and continuing to execute the step 7;
step 7: judging whether a termination condition is met, wherein the termination condition is that the iteration times are maximum values of fixed times or the adaptation values in the iteration are converged to be unchanged. Otherwise, executing the step 8, and executing the step 11 if yes;
step 8: the selection operation is performed according to a classical roulette selection method of the selection operation, namely, the selection is performed through probability that partial population accounts for the whole body. Specifically, the time is the most used as an adaptive value function, and the shorter and the better the time is, the better the selection result is under the premise of meeting constraint conditions;
step 9: cross operation, randomly selecting part of individuals in the population, grouping two individuals, and carrying out binary coding position random exchange to generate a part of new individuals;
Step 10: a mutation operation, namely randomly selecting part of individuals in the population, carrying out the random mutation operation of individual numerical values, and continuously executing the step 2;
step 11: outputting an optimal track function and population individuals which meet constraint conditions.
Example 2
The embodiment of the invention provides an operation track planning system of an obstacle-avoidance excavator working device, which comprises the following components:
the obstacle avoidance unit is used for generating a motion obstacle avoidance path point of the excavator working device;
the planning unit is used for planning the motion trail of the excavator working device based on the motion obstacle avoidance path points to generate an optimal path;
and a control unit for controlling the movement of the excavator working device based on the optimal path.
In a specific implementation manner of the embodiment of the present invention, the method for generating the motion obstacle avoidance path point includes:
obtaining position information and shape information of an obstacle;
planning an obstacle avoidance path point based on the coordinate information of a movement starting point and a movement target point of the excavator working device, the position information and the shape information of the obstacle;
in the specific implementation process, in order to collect the position information and the shape information of the obstacle, the arrangement of the sensors needs to be performed, and the arrangement manner shown in fig. 2 may be adopted:
1) The right front side of the excavator is provided with: 2 laser radars 1, 1 long-distance millimeter wave radar 2;
2) Symmetrically installing the two sides of the excavator body: 1 laser radar 1 each;
3) And (3) rear installation of the excavator: 2 laser radars 1, 1 long-distance millimeter wave radar 2;
4) Cab top mounting: 1 laser radar 1;
5) The interior installation of the excavating locomotive: 1 exchanger, 1 industrial personal computer 3;
6) Excavator arm installation: 1 laser radar 1;
7) The excavator working device is installed: an inclination sensor 4 and a length sensor 5; the excavator working device includes: boom, stick, and bucket;
8) The excavator is provided with an inertial navigation system (adopting a GPS coordinate system), a controller 6 and a GPS antenna 7.
The method comprises the steps of planning obstacle avoidance path points based on coordinate information of a motion starting point and a motion target point of an excavator working device, position information and shape information of an obstacle, and specifically comprises the following steps:
setting the grid position and the area of the barrier according to the position information and the shape information of the barrier;
according to the coordinate information of the motion starting point and the motion target point at the tail end of the excavator working device, rasterizing the motion space of the excavator working device, and taking the center of the grid as a node;
And carrying out obstacle avoidance path planning according to the priority of the nodes based on the grid position and the area of the obstacle and the movement space of the rasterized excavation working device by using an A-scale algorithm, wherein the A-scale algorithm calculates the priority of each node by the following functions:
F(n)＝G(n)+H(n)
where F (n) is the comprehensive priority of node n, G (n) is the cost of node n from the start point, H (n) is the predicted cost of node n from the end point, and when the next node to be traversed is selected, the node with the smallest comprehensive priority value is always selected.
The flow chart of algorithm a is shown in fig. 4, and the specific steps are as follows:
step 1: initializing an open list and a close list to be empty, and adding a starting point into the open list; the open list is ended if empty, otherwise, the open list is continued;
step 2: calculating the F value of each node in the open list, wherein F=G+H, and the calculation of H uses a Manhattan method to find out the node n with the minimum F value in the open list, and updating the father node of each node according to the F value;
step 3: removing the node n from the open list and adding the node n to the close list;
step 4: if the node n is the target node, a path is obtained, the path is traced back from the end point to the starting point, and otherwise, the path is continued;
step 5: if the node n has no child node, returning to the step 2, otherwise continuing;
Step 6: searching nodes adjacent to the node n, skipping nodes in the close list and unreachable nodes, adding the adjacent nodes into the open list, and returning to the step 2.
Step 7: the nodes in the open list constitute feasible waypoints, and each waypoint is then sent to the path planning model, which corresponds to the center of each grid.
In a specific implementation manner of the embodiment of the present invention, the method for generating the optimal path includes:
selecting an optimal path point by taking the motion obstacle avoidance path point as a constraint condition and taking the minimum sum of angle variation of each joint of the excavator working device as a target;
based on the optimal path point, establishing a cubic polynomial by utilizing interpolation function segmentation;
and solving each coefficient of the cubic polynomial by taking the shortest action time as an objective function and taking the constraint condition that the motion speed, the acceleration and the pulsation curve do not exceed the corresponding constraint ranges to obtain an optimal path.
The third order polynomial is:
P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0
wherein P is ji (t) represents an i-th track expression of a j-th joint, in which j=1, 2,3, …, N in the mechanical arm of N joints, the track of each joint may be divided into i=1, 2,3, …, N-1 when the excavator working device has N intermediate points in total from the start point to the target point; s is(s) ji3 Representing the coefficients of the third order term in the j-th joint, i-th track, s ji2 Representing coefficients of quadratic terms in the j-th joint, i-th track, s ji1 Representing the coefficients of the j-th joint, the primary term in the i-th track, s ji0 The coefficients of constant terms in the j-th joint and i-th track are represented;
the objective function is:
the constraint conditions are as follows:
wherein g is the total time taken by the working device to move, P j ' i (t)，P j ” i (t)，P j ” i 't' represents the trajectory function P respectively ji (t) first, second, third derivatives, V j,max Is the speed constraint condition of the j-th joint, W j,max K being the acceleration constraint of the j-th joint j,max Is the pulsation constraint condition of the j-th joint, h j,i ＝t j,i+1 -t j,i The time required for the j-th joint to track in the i-th segment.
In a specific implementation manner of the embodiment of the present invention, the method for screening the optimal path point specifically includes:
rotating and translating the coordinate system where the working device of the excavator is positioned to obtain a homogeneous transformation matrix T of the ith coordinate system relative to the coordinate system of the vehicle body i 0 ；
The method comprises the steps of obtaining joint variables in an excavator working device according to the position of the tail end of the excavator working device by using an inverse kinematics method, wherein in the inverse kinematics method, an objective function min (|alpha) is obtained i -α i-1 |+|β i -β i-1 |+|θ i -θ i-1 I), wherein alpha i ，β i ，θ i The positions of the tail ends of the working devices of the excavator are based on the data in a GPS coordinate system and the data in a local horizontal coordinate system.
The homogeneous transformation matrix T i 0 The calculation formula of (2) is as follows:
specifically, the generation process of the optimal path is as follows:
1) Excavator working device pose and motion description:
the coordinate system of the working device of the excavator is shown in fig. 3, wherein the establishment of the coordinate system accords with the right-hand rule, and after the coordinate system is established, the coordinate system o can be established through 4 steps i-1 x i-1 y i-1 z i-1 Transform to the next coordinate system o i x i y i z i ：
Step 1: around z i-1 Shaft rotation theta i Degree, x i-1 And x i The axes become the same direction, and this transformation matrix is denoted as Rot (Z, θ) i ) Z represents rotation about the Z axis;
step 2: along z i-1 Axial translation d i In units of (2), x i-1 And x i The axes are collinear, and the transformation matrix is denoted as Trans (0, d) i )；
Step 3: along x i-1 Axial translation a i The origin of the two coordinate systems is superimposed, and the transformation matrix is denoted as Trans (. Alpha.) i ,0,0)；
Step 4: around x i Shaft rotation a i Degree, z i-1 And z i The axes are collinear, the two coordinate systems coincide with each other, and the transformation matrix is denoted Rot (X, alpha) i ) X represents rotation about the X axis;
relation between the (i-1) th coordinate system and the (i) th coordinate system can be used as matrix Representation, wherein:
according to the translation matrix and the rotation homogeneous transformation formula, the method can be as follows:
/>
thus (2)
Homogeneous transformation matrix T of ith coordinate system relative to vehicle body coordinate system i 0 Can be expressed by the product of homogeneous transformation matrixes of continuous coordinate systems, namely
In the embodiment of the invention, the joint variables of the excavator working device are obtained according to the position of the tail end of the excavator working device (namely the tail end of the tooth tip of the bucket) by using an inverse kinematics method. Since the excavator working device has only three joints, the following steps are available:
according to the above, can obtain
Wherein E is an identity matrix.
The position of the tail end of the excavator working device is based on the conversion of data in a GPS coordinate system into data in a local horizontal coordinate system, and the specific conversion process is as follows:
the vehicle position and the surrounding obstacle position adopt a coordinate position-an earth coordinate system under an absolute coordinate system. Because the GPS data read through the GPS antenna and inertial navigation is a geographic coordinate system (WGS 84), the GPS data needs to be analyzed and coordinate transformed to be converted into rectangular coordinate system data which points to the right east along the X axis, points to the right north along the Y axis, and the origin is positioned at the mass center of the inertial navigation unit; the parsing and the coordinate transformation can be realized through the calculation of a Gaussian forward calculation formula. The specific method comprises the following steps:
The inertial navigation unit can output the acquired GPS data to the industrial personal computer, and the GPS data adopts a GPRMC format (namely minimum positioning information) and comprises information such as time date, longitude and latitude, heading, magnetic declination and the like; creating a GPS data structure body in a system, and carrying out data analysis on the obtained GPRMC data to obtain longitude (L) and latitude (B) data; because the acquired GPS data are the earth coordinates and cannot be directly used for an excavator, the L, B coordinates are projected by a Gaussian projection method (Gaussian forward calculation formula) to obtain Gaussian plane rectangular coordinates, and the achievable precision of the Gaussian forward calculation formula is 0.1 meter, wherein the specific formula is as follows:
X＝m 0 +m 2 l 2 +m 4 l 4
Y＝n 1 l+n 3 l 3 +n 5 l 5
wherein l is warp beam, m 0 ,n 1 ,m 2 ,. are undetermined coefficients that are a function of dimension B; (X, Y) is the transformed coordinates. And then converting the Gaussian plane rectangular coordinate into a local horizontal coordinate, wherein the obtained coordinate system has positive y-axis east and positive x-axis north, is not uniform with the definition of a customary coordinate system, and can obtain a universal coordinate system through coordinate conversion. The conversion formula is as follows:
by the method, GPS data in the WGS84 coordinate system is converted into a local horizontal coordinate system with the X axis pointing to the east and the Y axis pointing to the north, and the origin is located in the mass center of the sensor.
According to the converted GPS data of the vehicle body, the angles of all joints of the working device of the excavator can be acquired through sensor data, and the tail end position and the tail end gesture of the working device of the excavator can be obtained through positive kinematics of the mechanical arm.
In the motion inverse solution calculation process of the excavator working device based on the obtained path points, the optimization function is that the sum of the angle change amounts of all joints is minimum, namely the motion path is shortest, namely min (|alpha) i -α i-1 |+|β i -β i-1 |+|θ i -θ i-1 I), constraint is path planning as described above, where α i ，β i ，θ i The angles of the motion of the ith section of track of the three joints are respectively represented, and then the particle swarm optimization algorithm is adopted for optimization solution, wherein the flow chart of the particle swarm optimization algorithm is shown in fig. 5. The algorithm comprises the following specific steps:
step 1: initializing a population of particles, including random positions and velocities;
step 2: evaluating the fitness of each particle;
step 3: for each particle, comparing the adaptive value with the best position pbest (namely, the local optimal position) passed by the particle, and if the adaptive value is better, taking the particle as the current best position gbest (namely, the global optimal position);
step 4: adjusting the speed and the position of the particles according to the steps 2 and 3;
step 5: and if the iteration termination condition is not met, continuing to step 2.
The iteration termination condition is selected as the optimal position searched by the maximum iteration times or the particle swarm before the maximum iteration times, namely the result is converged, and the optimal path point is screened out.
2) Calculating a trajectory function of an excavator work device
The cubic polynomial interpolation function (namely, the cubic spline interpolation function) is the lowest order polynomial capable of ensuring continuous position, speed and acceleration, and therefore, the method is one of widely used and simple in calculation. Therefore, the three-time multi-pattern bar function method is adopted to plan the motion trail of the excavator in the joint space, so that the controlled variable is related to the motion time, and the calculation process is simple and convenient during trail planning.
The general form of a cubic spline polynomial trajectory can be expressed as:
P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0
wherein P is ji (t) represents an i-th track expression of the j-th joint, j=1, 2,3, …, N in the mechanical arm of the N joints, and when the mechanical arm has N intermediate points (including a start point and an end point) in total from the start point to the target point, the track of each joint may be divided into i=1, 2,3, …, N-1 segments. s is(s) ji3 Representing the coefficients of the third order term in the j-th joint, i-th track, s ji2 Representing coefficients of quadratic terms in the j-th joint, i-th track, s ji1 Representing the coefficients of the j-th joint, the primary term in the i-th track, s ji0 The coefficients of the constant term in the j-th joint, i-th track are shown. Since the end effector is the result of the co-action of the individual joints, the path of motion of the end effector is also divided into n-1 segments, since each joint has n-1 segments of trajectory.
3) Excavator working device track planning based on adaptive value function
The final goal of trajectory planning is to find P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0 And ensure that each coefficient obtained is substituted into P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0 And each curve in the obtained joint angle, speed, acceleration and pulsation curves can meet the corresponding constraint conditions.
For industrial application of the excavator, the motion speed of the excavator is closely related to the production efficiency, and in order to maximally improve the efficiency of the excavator, the invention optimizes the action time of the excavator to the minimum. In the process of solving the track function expression, the joint tracks meet the kinematic constraint condition, and the shortest total motion time is ensured. The essence of this problem is how to reasonably solve the problem of time intervals between two adjacent nodes when the excavator work device is in operation, so that the total motion time of the sum of all the time intervals is minimized. In addition, when the track planning is performed by using the optimized time, it is necessary to ensure that the speed, acceleration and pulsation curves of the track cannot exceed the corresponding constraint ranges, otherwise, the optimized time is invalid, and the track planning must be performed again.
The method optimizes all joints simultaneously, and is characterized in that the solving process is complex, but the movement time of all joints is the same, the action continuity is good, and the control is simple.
The following definition is made for a boom with N joints:
V j,max is the speed constraint condition of the j-th joint, W j,max K being the acceleration constraint of the j-th joint j,max Is the pulsation constraint of the j-th joint. h is a j,i ＝t j,i+1 -t j,i The time required for the j-th joint to track in the i-th segment.
The optimization objective function is:
the constraint conditions are as follows:
wherein g is the total time taken by the working device to move, P j ' i (t)，P j ” i (t)，P j ” i 't' represents the trajectory function P respectively ji (t) first, second, third derivatives, V j,max Is the speed constraint condition of the j-th joint, W j,max K being the acceleration constraint of the j-th joint j,max Is the pulsation constraint condition of the j-th joint, h j,i ＝t j,i+1 -t j,i The time required for the j-th joint to track in the i-th segment.
And taking the total time as an optimized adaptive value function, and carrying out optimizing solution. The invention utilizes genetic algorithm to solve, see figure 6. If the solution searched by the genetic algorithm meets the constraint condition, the iteration is continued after the adaptive value is calculated, and if the solution searched by the genetic algorithm does not meet the constraint condition, the adaptive value is set to be a great number.
The method comprises the following specific steps:
step 1: creating and initializing a certain fixed number of time populations, wherein individuals in the populations are the movement time of each joint;
step 2: updating P by time population ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0 Each coefficient;
step 3: bringing time intervals in the time population into the track function, and calculating the maximum value of the track function related constraint condition;
step 4: judging whether the obtained result value is larger than the range of the constraint condition, if not, executing the step 5, otherwise, executing the step 6;
step 5: by passing throughCalculating the adaptation value of each body in the population, namely calculating the total time of the motion trail, and continuously executing the step 7;
step 6: assigning the fitness to a larger number which is far larger than the maximum value of the initializing time population, and continuing to execute the step 7;
step 7: judging whether a termination condition is met, wherein the termination condition is that the iteration times are maximum values of fixed times or the adaptation values in the iteration are converged to be unchanged. Otherwise, executing the step 8, and executing the step 11 if yes;
step 8: the selection operation is performed according to a classical roulette selection method of the selection operation, namely, the selection is performed through probability that partial population accounts for the whole body. Specifically, the time is the most used as an adaptive value function, and the shorter and the better the time is, the better the selection result is under the premise of meeting constraint conditions;
Step 9: cross operation, randomly selecting part of individuals in the population, grouping two individuals, and carrying out binary coding position random exchange to generate a part of new individuals;
step 10: a mutation operation, namely randomly selecting part of individuals in the population, carrying out the random mutation operation of individual numerical values, and continuously executing the step 2;
step 11: outputting an optimal track function and population individuals which meet constraint conditions.
The foregoing has shown and described the basic principles and main features of the present invention and the advantages of the present invention. It will be understood by those skilled in the art that the present invention is not limited to the embodiments described above, and that the above embodiments and descriptions are merely illustrative of the principles of the present invention, and various changes and modifications may be made without departing from the spirit and scope of the invention, which is defined in the appended claims. The scope of the invention is defined by the appended claims and equivalents thereof.
Claims (6)
1. The working track planning method for the obstacle-avoidance excavator working device is characterized by comprising the following steps of:
generating a motion obstacle avoidance path point of an excavator working device;
planning a motion track of the excavator working device based on the motion obstacle avoidance path points to generate an optimal path;
Controlling movement of the excavator working device based on the optimal path;
the generation method of the optimal path comprises the following steps:
based on the motion obstacle avoidance path points, establishing a cubic polynomial by utilizing interpolation function segmentation;
taking the shortest action time as an objective function, taking the motion speed, the acceleration and the pulsation curve not exceeding the corresponding constraint range as constraint conditions, and solving various coefficients of a cubic polynomial to obtain an optimal path;
the third order polynomial is:
P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0
wherein P is ji (t) represents the ith track expression of the jth joint, j=1, 2,3, …, N in the mechanical arm of the N joints, N being counted as N intermediate points from the starting point to the target point of the excavator working deviceAt the point, the trajectory of each joint may be divided into sections i=1, 2,3, …, n-1; s is(s) ji3 Representing the coefficients of the third order term in the j-th joint, i-th track, s ji2 Representing coefficients of quadratic terms in the j-th joint, i-th track, s ji1 Representing the coefficients of the j-th joint, the primary term in the i-th track, s ji0 The coefficients of constant terms in the j-th joint and i-th track are represented;
the objective function is:
the constraint conditions are as follows:
wherein g is the total time used by the movement of the working device, P' ji (t)，P″ ji (t)，P″′ ji (t) represents the trajectory functions P respectively ji (t) first, second, third derivatives, V j,max Is the speed constraint condition of the j-th joint, W j,max K being the acceleration constraint of the j-th joint j,max Is the pulsation constraint condition of the j-th joint, h j,i ＝t j,i+1 -t j,i The time required for the j-th joint to track in the i-th segment;
the step of establishing the cubic polynomial by utilizing interpolation function segmentation further comprises the following steps:
selecting an optimal path point by taking the motion obstacle avoidance path point as a constraint condition and taking the minimum sum of angle variation of each joint of the excavator working device as a target;
the third-order polynomial is established in sections by using an interpolation function based on the screened optimal path point;
the optimal path point screening method specifically comprises the following steps:
rotating and translating the coordinate system where the working device of the excavator is positioned to obtain the ith seatHomogeneous transformation matrix T of standard system relative to vehicle body coordinate system i 0 ；
The method comprises the steps of obtaining joint variables in an excavator working device according to the position of the tail end of the excavator working device by using an inverse kinematics method, wherein in the inverse kinematics method, an objective function min (|alpha) is obtained i -α i-1 |+|β i -β i-1 |+|θ i -θ i-1 I), wherein alpha i ，β i ，θ i Respectively representing the angles of the motion of the three joints in the ith section of track; the position of the end of the excavator work device is based on converting data in a GPS coordinate system to data in a local horizontal coordinate system.
2. The method for planning the working track of the working device of the obstacle avoidance excavator according to claim 1, wherein the method for generating the moving obstacle avoidance path point comprises the following steps:
obtaining position information and shape information of an obstacle;
and planning an obstacle avoidance path point based on the coordinate information of the movement starting point and the movement target point of the excavator working device, the position information and the shape information of the obstacle.
3. The method for planning the working track of the obstacle avoidance excavator working device according to claim 2, wherein the obstacle avoidance path point is planned based on the coordinate information of the movement starting point and the movement target point of the excavator working device, the position information and the shape information of the obstacle, specifically:
setting the grid position and the area of the barrier according to the position information and the shape information of the barrier;
according to the coordinate information of the motion starting point and the motion target point at the tail end of the excavator working device, rasterizing the motion space of the excavator working device, and taking the center of the grid as a node;
and (3) performing obstacle avoidance path planning according to the priority of the nodes based on the grid position and area of the obstacle and the movement space of the rasterized excavation working device by using an A-scale algorithm.
4. A method of working track planning for an obstacle avoidance excavator working device according to claim 3 wherein the a-algorithm calculates the priority of each node by the following function:
F(n)＝G(n)+H(n)
where F (n) is the comprehensive priority of node n, G (n) is the cost of node n from the start point, H (n) is the predicted cost of node n from the end point, and when the next node to be traversed is selected, the node with the smallest comprehensive priority value is always selected.
5. The method for planning the working track of the working device of the obstacle avoidance excavator, which is characterized by comprising the following steps of: the homogeneous transformation matrix T i 0 The calculation formula of (2) is as follows:
6. an obstacle avoidance excavator working device operation track planning device, comprising:
the obstacle avoidance unit is used for generating a motion obstacle avoidance path point of the excavator working device;
the planning unit is used for planning the motion trail of the excavator working device based on the motion obstacle avoidance path points to generate an optimal path;
a control unit for controlling the movement of the excavator working device based on the optimal path;
the generation method of the optimal path comprises the following steps:
based on the motion obstacle avoidance path points, establishing a cubic polynomial by utilizing interpolation function segmentation;
Taking the shortest action time as an objective function, taking the motion speed, the acceleration and the pulsation curve not exceeding the corresponding constraint range as constraint conditions, and solving various coefficients of a cubic polynomial to obtain an optimal path;
the third order polynomial is:
P ji (t)＝s ji3 t 3 +s ji2 t 2 +s ji1 t+s ji0
wherein P is ji (t) represents an i-th track expression of a j-th joint, in which j=1, 2,3, …, N in the mechanical arm of N joints, the track of each joint may be divided into i=1, 2,3, …, N-1 when the excavator working device has N intermediate points in total from the start point to the target point; s is(s) ji3 Representing the coefficients of the third order term in the j-th joint, i-th track, s ji2 Representing coefficients of quadratic terms in the j-th joint, i-th track, s ji1 Representing the coefficients of the j-th joint, the primary term in the i-th track, s ji0 The coefficients of constant terms in the j-th joint and i-th track are represented;
the objective function is:
the constraint conditions are as follows:
wherein g is the total time used by the movement of the working device, P' ji (t)，P″ ji (t)，P″′ ji (t) represents the trajectory functions P respectively ji (t) first, second, third derivatives, V j,max Is the speed constraint condition of the j-th joint, W j,max K being the acceleration constraint of the j-th joint j,max Is the pulsation constraint condition of the j-th joint, h j,i ＝t j,i+1 -t j,i When required for the j-th joint in the i-th trackA compartment;
the step of establishing the cubic polynomial by utilizing interpolation function segmentation further comprises the following steps:
selecting an optimal path point by taking the motion obstacle avoidance path point as a constraint condition and taking the minimum sum of angle variation of each joint of the excavator working device as a target;
the third-order polynomial is established in sections by using an interpolation function based on the screened optimal path point;
the optimal path point screening method specifically comprises the following steps:
rotating and translating the coordinate system where the working device of the excavator is positioned to obtain a homogeneous transformation matrix T of the ith coordinate system relative to the coordinate system of the vehicle body i 0 ；
The method comprises the steps of obtaining joint variables in an excavator working device according to the position of the tail end of the excavator working device by using an inverse kinematics method, wherein in the inverse kinematics method, an objective function min (|alpha) is obtained i -α i-1 |+|β i -β i-1 |+|θ i -θ i-1 I), wherein alpha i ，β i ，θ i Respectively representing the angles of the motion of the three joints in the ith section of track; the position of the end of the excavator work device is based on converting data in a GPS coordinate system to data in a local horizontal coordinate system.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110842844.0A CN113589809B (en) | 2021-07-26 | 2021-07-26 | Work track planning method and device for obstacle-avoidance excavator working device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110842844.0A CN113589809B (en) | 2021-07-26 | 2021-07-26 | Work track planning method and device for obstacle-avoidance excavator working device |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113589809A CN113589809A (en) | 2021-11-02 |
| CN113589809B true CN113589809B (en) | 2024-04-05 |
Family
ID=78250057
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110842844.0A Active CN113589809B (en) | 2021-07-26 | 2021-07-26 | Work track planning method and device for obstacle-avoidance excavator working device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113589809B (en) |
Families Citing this family (3)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN113954081A (en) * | 2021-12-07 | 2022-01-21 | 中国煤炭科工集团太原研究院有限公司 | Working arm track planning method and system of anchor rod support robot |
| CN114265364B (en) * | 2021-12-21 | 2023-10-03 | 江苏师范大学 | Monitoring data processing system and method of industrial Internet of things |
| CN118170146A (en) * | 2024-05-09 | 2024-06-11 | 山东科技大学 | Excavator running control method based on extended artificial potential field |
Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105302147A (en) * | 2015-11-06 | 2016-02-03 | 太原科技大学 | Series connection mechanism locus planning method |
| CN105583858A (en) * | 2014-11-10 | 2016-05-18 | 库卡罗伯特有限公司 | Flexible cycle time-optimized sharing of a working space for robots |
| CN107186713A (en) * | 2017-05-23 | 2017-09-22 | 江苏安格尔机器人有限公司 | A kind of mechanical arm multiaxial motion plan optimization method for crossing path point |
| CN108356819A (en) * | 2018-01-17 | 2018-08-03 | 西安交通大学 | Based on the industrial machinery arm Collision Free Path Planning for improving A* algorithms |
| CN108621158A (en) * | 2018-04-27 | 2018-10-09 | 上海师范大学 | A kind of time optimal trajectory planning control method and device about mechanical arm |
| CN108705532A (en) * | 2018-04-25 | 2018-10-26 | 中国地质大学（武汉） | A kind of mechanical arm obstacle-avoiding route planning method, equipment and storage device |
| CN108789413A (en) * | 2018-07-04 | 2018-11-13 | 杭州电子科技大学 | Mechanical arm free boundary motion planning method based on three sections of cubic polynomials |
| CN108839019A (en) * | 2018-06-25 | 2018-11-20 | 广州视源电子科技股份有限公司 | The determination method and apparatus of the motion path of mechanical arm |
| CN110370256A (en) * | 2019-06-25 | 2019-10-25 | 达闼科技（北京）有限公司 | Robot and its paths planning method, device and controller |
| CN110455290A (en) * | 2019-07-17 | 2019-11-15 | 爱克斯维智能科技(苏州)有限公司 | A kind of optimal trajectory planning method of intelligent hydraulic excavator |
| CN110595482A (en) * | 2019-10-28 | 2019-12-20 | 深圳市银星智能科技股份有限公司 | Path planning method and device with obstacle avoidance weight and electronic equipment |
| CN111152224A (en) * | 2020-01-10 | 2020-05-15 | 温州大学 | Dual-optimization robot motion trajectory optimization method |
| CN112692826A (en) * | 2020-12-08 | 2021-04-23 | 佛山科学技术学院 | Industrial robot track optimization method based on improved genetic algorithm |
Family Cites Families (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| US9566742B2 (en) * | 2012-04-03 | 2017-02-14 | Massachusetts Institute Of Technology | Methods and apparatus for computer-assisted spray foam fabrication |
-
2021
- 2021-07-26 CN CN202110842844.0A patent/CN113589809B/en active Active
Patent Citations (13)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105583858A (en) * | 2014-11-10 | 2016-05-18 | 库卡罗伯特有限公司 | Flexible cycle time-optimized sharing of a working space for robots |
| CN105302147A (en) * | 2015-11-06 | 2016-02-03 | 太原科技大学 | Series connection mechanism locus planning method |
| CN107186713A (en) * | 2017-05-23 | 2017-09-22 | 江苏安格尔机器人有限公司 | A kind of mechanical arm multiaxial motion plan optimization method for crossing path point |
| CN108356819A (en) * | 2018-01-17 | 2018-08-03 | 西安交通大学 | Based on the industrial machinery arm Collision Free Path Planning for improving A* algorithms |
| CN108705532A (en) * | 2018-04-25 | 2018-10-26 | 中国地质大学（武汉） | A kind of mechanical arm obstacle-avoiding route planning method, equipment and storage device |
| CN108621158A (en) * | 2018-04-27 | 2018-10-09 | 上海师范大学 | A kind of time optimal trajectory planning control method and device about mechanical arm |
| CN108839019A (en) * | 2018-06-25 | 2018-11-20 | 广州视源电子科技股份有限公司 | The determination method and apparatus of the motion path of mechanical arm |
| CN108789413A (en) * | 2018-07-04 | 2018-11-13 | 杭州电子科技大学 | Mechanical arm free boundary motion planning method based on three sections of cubic polynomials |
| CN110370256A (en) * | 2019-06-25 | 2019-10-25 | 达闼科技（北京）有限公司 | Robot and its paths planning method, device and controller |
| CN110455290A (en) * | 2019-07-17 | 2019-11-15 | 爱克斯维智能科技(苏州)有限公司 | A kind of optimal trajectory planning method of intelligent hydraulic excavator |
| CN110595482A (en) * | 2019-10-28 | 2019-12-20 | 深圳市银星智能科技股份有限公司 | Path planning method and device with obstacle avoidance weight and electronic equipment |
| CN111152224A (en) * | 2020-01-10 | 2020-05-15 | 温州大学 | Dual-optimization robot motion trajectory optimization method |
| CN112692826A (en) * | 2020-12-08 | 2021-04-23 | 佛山科学技术学院 | Industrial robot track optimization method based on improved genetic algorithm |
Non-Patent Citations (2)
| Title |
|---|
| 《基于时间最优的机器人关节轨迹规划方法》;连黎明;《组合机床与自动化加工技术》;20180930(第9期);57-60 * |
| 《机器人轨迹规划与控制及其在工业包装中的应用研究》;王银仲;《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》;全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113589809A (en) | 2021-11-02 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113589809B (en) | Work track planning method and device for obstacle-avoidance excavator working device | |
| CN112378408B (en) | Path planning method for realizing real-time obstacle avoidance of wheeled mobile robot | |
| Deo et al. | Multimodal trajectory prediction conditioned on lane-graph traversals | |
| Stahl et al. | Multilayer graph-based trajectory planning for race vehicles in dynamic scenarios | |
| CN109782779B (en) | AUV path planning method in ocean current environment based on population hyperheuristic algorithm | |
| CN110927740A (en) | Mobile robot positioning method | |
| CN111610786B (en) | Mobile robot path planning method based on improved RRT algorithm | |
| CN113050640B (en) | Industrial robot path planning method and system based on generation of countermeasure network | |
| CN110866927A (en) | Robot positioning and composition method based on EKF-SLAM algorithm combined with dotted line characteristics of foot | |
| Li et al. | A mobile robot path planning algorithm based on improved A* algorithm and dynamic window approach | |
| Yu et al. | A hybrid path planning method for an unmanned cruise ship in water quality sampling | |
| CN111578926A (en) | Map generation and navigation obstacle avoidance method based on automatic driving platform | |
| Liu et al. | Deep reinforcement learning for mobile robot path planning | |
| WO2023057185A1 (en) | Coordination of multiple robots using graph neural networks | |
| Yu et al. | A traversal multi-target path planning method for multi-unmanned surface vessels in space-varying ocean current | |
| Hassan et al. | Path planning and trajectory tracking control for two-wheel mobile robot | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| Feng et al. | A hybrid motion planning algorithm for multi-robot formation in a dynamic environment | |
| CN116009558A (en) | Mobile robot path planning method combined with kinematic constraint | |
| CN114815899A (en) | Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor | |
| CN112904855B (en) | Follow-up robot local path planning method based on improved dynamic window | |
| Zhang et al. | Intelligent path planning by an improved RRT algorithm with dual grid map | |
| XIANG et al. | Three-dimensional path planning of manipulator based on improved RRT algorithm | |
| Wang et al. | Dynamic path planning algorithm for autonomous vehicles in cluttered environments | |
| Yu et al. | PathRL: An End-to-End Path Generation Method for Collision Avoidance via Deep Reinforcement Learning |
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 |