CN113759900B - Method and system for track planning and real-time obstacle avoidance of inspection robot based on obstacle region prediction - Google Patents
Method and system for track planning and real-time obstacle avoidance of inspection robot based on obstacle region prediction Download PDFInfo
- Publication number
- CN113759900B CN113759900B CN202110923744.0A CN202110923744A CN113759900B CN 113759900 B CN113759900 B CN 113759900B CN 202110923744 A CN202110923744 A CN 202110923744A CN 113759900 B CN113759900 B CN 113759900B
- Authority
- CN
- China
- Prior art keywords
- obstacle
- potential field
- dynamic
- elliptical
- model
- 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 35
- 238000007689 inspection Methods 0.000 title claims description 69
- 239000013598 vector Substances 0.000 claims abstract description 33
- 230000003068 static effect Effects 0.000 claims abstract description 23
- 230000006870 function Effects 0.000 claims description 42
- 230000004888 barrier function Effects 0.000 claims description 19
- 239000011159 matrix material Substances 0.000 claims description 14
- 238000004590 computer program Methods 0.000 claims description 6
- 238000005259 measurement Methods 0.000 claims description 6
- 238000012546 transfer Methods 0.000 claims description 6
- 238000004364 calculation method Methods 0.000 claims description 3
- 230000007704 transition Effects 0.000 claims description 2
- 238000004422 calculation algorithm Methods 0.000 abstract description 11
- 230000008859 change Effects 0.000 abstract description 2
- 238000005265 energy consumption Methods 0.000 abstract description 2
- 230000008569 process Effects 0.000 description 5
- 230000007613 environmental effect Effects 0.000 description 4
- 238000005381 potential energy Methods 0.000 description 3
- 238000010276 construction Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 230000005484 gravity Effects 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 239000000126 substance Substances 0.000 description 2
- 206010063385 Intellectualisation Diseases 0.000 description 1
- 240000007651 Rubus glaucus Species 0.000 description 1
- 235000011034 Rubus glaucus Nutrition 0.000 description 1
- 235000009122 Rubus idaeus Nutrition 0.000 description 1
- 238000005299 abrasion Methods 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- XAGFODPZIPBFFR-UHFFFAOYSA-N aluminium Chemical compound [Al] XAGFODPZIPBFFR-UHFFFAOYSA-N 0.000 description 1
- 229910052782 aluminium Inorganic materials 0.000 description 1
- 238000004458 analytical method Methods 0.000 description 1
- 230000006399 behavior Effects 0.000 description 1
- 238000001514 detection method Methods 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 238000005868 electrolysis reaction Methods 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005188 flotation Methods 0.000 description 1
- 230000006872 improvement Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000004519 manufacturing process Methods 0.000 description 1
- 230000008447 perception Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000000007 visual effect Effects 0.000 description 1
Images
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/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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
-
- 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
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a method and a system for track planning and real-time obstacle avoidance of a patrol robot based on obstacle region prediction, wherein a two-dimensional high-precision grid map is established through a two-dimensional high-precision laser radar and a depth camera, a dynamic obstacle reachable region prediction model based on state updating is established, the movement characteristics and the geometric dimensions of obstacles are fully considered, adjustment allowance is reserved, a dynamic obstacle reachable region is determined in advance, a multistep elliptical enveloping potential field, a novel Sigmoid square obstacle potential field and a circular obstacle potential field are defined to correct a logarithmic Lyapunov gravitational field model, a real-time obstacle avoidance algorithm under an obstacle space is obtained, an expected running angle and an expected speed vector of the robot are obtained through the obstacle avoidance algorithm, a dynamic obstacle avoidance path of the robot is finally obtained in real time, the traditional static obstacle-based obstacle avoidance method is perfected, the obstacle avoidance path length of the patrol robot under complex environments such as static and dynamic is shorter, the energy consumption is lower, the safety is higher, and the maximum running angle change amplitude is smaller.
Description
Technical Field
The invention mainly relates to the technical field of flow industry and obstacle avoidance trajectory planning, in particular to a method and a system for planning and real-time obstacle avoidance of a patrol robot trajectory based on obstacle region prediction.
Background
The autonomous inspection mobile robot has the advantages of low price, flexible operation, wide movement range and the like, is successfully applied to the electric power field and the security field, and has huge application potential in the process industry fields such as aluminum electrolysis, flotation and the like. The research trend of the mobile inspection robot in the future is intellectualization and autonomy, but safe real-time obstacle avoidance is one of key technologies for improving the autonomous planning capability of the mobile inspection robot, but compared with a moving platform such as an unmanned plane, an underwater mobile robot and the like, the factory environment where the mobile inspection robot is located is more complex, if operators, large-scale static obstacles and the like exist, and the autonomous obstacle avoidance capability of the mobile inspection robot is more required. With the progress of China towards industry 4.0 and manufacturing China 2025, the contradiction between increasingly complex and changeable industrial environments and insufficient autonomous planning capability of the inspection mobile robot is increasingly highlighted, and the industrial inspection mobile robot becomes a bottleneck for restricting the industrial inspection robot in China to develop autonomously and intelligently.
The autonomous obstacle avoidance problem of the industrial inspection robot is that the obstacle avoidance behavior of the inspection mobile machine is independently planned, decided and controlled according to information of an industrial site or environmental information such as obstacle information, target information and the like sensed in real time from a sensor (such as a depth camera, a laser radar and the like), so that the inspection robot moves towards a target point and avoids various obstacles. The difficulty of this problem is the non-structural nature of the environment (such as the presence of non-protruding areas and many types of obstacles), the dynamic nature of the environment (such as the presence of dynamic obstacles such as operators and other inspection robots, moving objects, etc.), the non-deterministic nature of the environmental information (such as the difficulty in obtaining part of the environmental information, the lack of some local information in a short time due to the long time of obtaining the environmental information, etc.). Through years of development, the autonomous obstacle avoidance method of the inspection robot has achieved various results, but most of the autonomous obstacle avoidance method is only suitable for simple industrial environments such as no-obstacle or static obstacle, and the autonomous obstacle avoidance method of the inspection robot still needs to be further researched in complex industrial environments with static and dynamic obstacle.
Disclosure of Invention
The method and the system for planning the track of the inspection robot and avoiding the obstacle in real time based on the prediction of the obstacle area solve the technical problem that the conventional inspection robot cannot avoid the obstacle in real time in a complex factory environment.
In order to solve the technical problems, the track planning and real-time obstacle avoidance method for the inspection robot based on obstacle region prediction provided by the invention comprises the following steps:
establishing a dynamic obstacle reachable area prediction model based on state updating;
based on a dynamic obstacle reachable region prediction model, an elliptical reachable region under the predicted step number is obtained;
establishing a multi-step elliptical envelope potential field model based on the elliptical reachable region under the predicted step number;
based on the static obstacle model, a square obstacle potential field model and a round obstacle potential field model of a novel Sigmoid function are established;
obtaining an expected running angle and an expected speed vector of the inspection robot based on a multi-step elliptical envelope potential field model, a square obstacle potential field model of a novel Sigmoid function, a circular obstacle potential field model and a logarithmic Lyapunov gravitational field model;
based on the expected traveling angle and the expected speed vector, an obstacle avoidance path of the robot is obtained.
Further, based on the dynamic obstacle reachable region prediction model, obtaining the elliptical reachable region under the preset step number comprises the following steps:
initializing a motion speed variance, a measurement noise variance and an initial variance transfer matrix of the dynamic barrier;
according to a p-step prediction variance equation, the variance of the centroid of the dynamic obstacle in the motion direction and the vertical motion direction under the prediction step number is obtained, wherein the p-step prediction variance equation is specifically:
wherein P is k+j|k For the variance matrix of the predicted step number j at the k moment, P (0) is the initial variance transfer matrix, A j Updating a matrix for the state transition of the predicted step number j, wherein Q is a measurement covariance matrix;
according to the mass center updating equation of the dynamic obstacle, obtaining the mass center position of the dynamic obstacle under the predicted step number;
and constructing an ellipse reachable region under the preset step number according to the barycenter position of the dynamic obstacle and the variance of the barycenter of the dynamic obstacle in the moving direction and the vertical moving direction under the predicted step number.
Further, according to the barycenter position of the dynamic obstacle and the variance of the barycenter of the dynamic obstacle in the moving direction and the vertical moving direction under the predicted step number, constructing the ellipse reachable region under the preset step number specifically comprises:
and taking the centroid position of the previous step of the preset step number as a center, and taking the 3-time variance of the centroid of the dynamic obstacle in the moving direction and the vertical moving direction as a long and short axis under the preset step number to construct an ellipse reachable region.
Further, based on the ellipse reachable region at the predicted step number, establishing the multi-step ellipse envelope potential field model includes:
expanding the ellipse reachable area under the predicted step number by a preset radius;
obtaining an elliptical envelope potential field according to the elliptical reachable region in the predicted step number after the outward expansion of the preset radius;
establishing a multi-step elliptical envelope potential field model according to the elliptical envelope potential field, wherein the multi-step elliptical envelope potential field model specifically comprises:
wherein U is 1 For a multi-step elliptical envelope potential field,
U AB (M 1 )、U CD (M 1 ) Respectively any point M in multi-step elliptical envelope potential field 1 In oval reachable area N 1 N 2 Elliptical reachable region P 1 P 2 Potential field generated by line AB and line CD, and ellipse reaching region P 1 P 2 And oval reachable area N 1 N 2 The ellipse reachable areas corresponding to the predicted step numbers j=1 and j=p are respectively, and the straight line AB and the straight line CD are the ellipse reachable areas P 1 P 2 And oval reachable area N 1 N 2 Is included in the envelope common tangent of (a).
Further, based on the static obstacle model, a specific formula for establishing a square obstacle potential field model of the novel Sigmoid function is as follows:
wherein U is 2 (M 2 ) Any point M in square obstacle potential field model of novel Sigmoid function 2 Square barrier potential field, k ri Gain coefficient of novel Sigmoid function, r is influence coefficient related to influence range of square obstacle, S i Representing square shapeMathematical expression of function of ith surface of obstacle, h c (M 2 ) Any point M in square obstacle potential field model of novel Sigmoid function 2 Square obstacle function expression of (c).
Further, based on the static obstacle model, a specific formula for establishing the circular obstacle potential field model is as follows:
wherein U is 3 (M 3 ) Is any point M in a circular barrier potential field model 3 Lambda is the circular barrier potential field coefficient, r c Radius of circular obstacle, h c (M 3 ) Is any point M in a circular barrier potential field model 3 Circular barrier function expression of ρ 0 Is the influence distance of the circular obstacle.
Further, obtaining the expected driving angle and the expected speed vector of the inspection robot based on the multi-step elliptical envelope potential field model, the square obstacle potential field model, the circular obstacle potential field model and the logarithmic Lyapunov gravitational field model of the novel Sigmoid function comprises:
correspondingly obtaining a multistep elliptical envelope potential field, a novel square obstacle potential field of the Sigmoid function, a circular obstacle potential field and a logarithmic Lyapunov vector gravitation according to the multistep elliptical envelope potential field model, the novel square obstacle potential field of the Sigmoid function, the circular obstacle potential field and the logarithmic Lyapunov gravitational field model;
respectively solving negative gradients of a multistep elliptical envelope potential field, a square obstacle potential field of the novel Sigmoid function and a circular obstacle potential field, and correspondingly obtaining a multistep elliptical envelope potential field repulsive force, a square obstacle potential field repulsive force of the novel Sigmoid function and a circular obstacle potential field repulsive force;
and obtaining the expected running angle and the expected speed vector of the inspection robot according to the multi-step elliptical envelope potential field repulsive force, the square obstacle potential field repulsive force of the novel Sigmoid function, the circular obstacle potential field repulsive force and the logarithmic Lyapunov vector attractive force.
Further, based on the expected traveling angle and the expected speed vector, a calculation formula for obtaining the obstacle avoidance path of the robot is as follows:
θ(t+1)＝θ(t)+ω θ △t
wherein q is d (t+1) and q d (t) represents the positions of the inspection robots at time t+1 and time t, v x And v y The speed of the inspection robot in the x and y directions is represented, theta (t+1) and theta (t) represent the direction angles of the inspection robot at the time t+1 and the time t, and omega θ Indicating the angular velocity of the inspection robot.
The invention provides a patrol robot track planning and real-time obstacle avoidance system based on obstacle region prediction, which comprises the following components:
the method comprises the steps of a memory, a processor and a computer program stored in the memory and capable of running on the processor, wherein the steps of the method for planning the track of the inspection robot and avoiding the obstacle in real time based on the prediction of the obstacle area are realized when the processor executes the computer program.
Compared with the prior art, the invention has the advantages that:
according to the method and the system for planning the track of the inspection robot and avoiding the obstacle in real time based on the prediction of the obstacle area, the expected running angle and the expected speed vector of the inspection robot and the obstacle avoidance path based on the expected running angle and the expected speed vector of the inspection robot are obtained, the technical problem that the existing inspection robot cannot avoid the obstacle in real time under the complex factory environment is solved, the movement characteristics and the geometric dimensions of the obstacle are fully considered and the margin is reserved and determined in advance by establishing the prediction model of the dynamic obstacle reachable area based on the static obstacle model, the square obstacle potential field model of the novel Sigmoid function, the round obstacle potential field model and the logarithmic Lyapunov potential field model based on the multistep elliptical enveloping potential field model, the expected running angle and the expected speed vector of the inspection robot, the obstacle avoidance path of the inspection robot is obtained, the movement characteristics and the geometric dimensions of the obstacle are fully considered and the margin are reserved by establishing the prediction model of the dynamic obstacle reachable area based on the state update, and then the round obstacle reachable area based on the multistep elliptical enveloping potential field model, the expected running angle and the obstacle can be improved under the complex factory environment, the actual running angle and the obstacle avoidance path of the inspection robot is more stable, and the obstacle avoidance path of the inspection robot can be improved in real time under the complex environment, and the environment is realized under the condition of the complex environment.
Drawings
Fig. 1 is a flowchart of a method for track planning and real-time obstacle avoidance of an inspection robot based on obstacle region prediction according to an embodiment of the present invention;
fig. 2 is a flowchart of a method for track planning and real-time obstacle avoidance of an inspection robot based on obstacle region prediction according to a second embodiment of the present invention;
FIG. 3 shows a predicted time domain T according to a second embodiment of the present invention p Centroid reachable region schematic of dynamic obstacle of=4Δt;
FIG. 4 shows a predicted time domain T according to a second embodiment of the present invention p Elliptical envelope dynamic obstacle reachable region =3 Δt;
FIG. 5 is a side view of a multi-step elliptical envelope potential field based on dynamic obstacle accessible region prediction in accordance with a second embodiment of the present invention;
FIG. 6 is a top view of a multi-step elliptical envelope potential field based on dynamic obstacle accessible region prediction in accordance with a second embodiment of the present invention;
fig. 7 is a schematic diagram of a geodetic coordinate system of a inspection robot according to a second embodiment of the present invention;
fig. 8 is a block diagram of a track planning and real-time obstacle avoidance system for an inspection robot based on obstacle region prediction according to the present invention.
Reference numerals:
10. a memory; 20. a processor.
Detailed Description
The present invention will be described more fully hereinafter with reference to the accompanying drawings, in which preferred embodiments are shown, for the purpose of illustrating the invention, but the scope of the invention is not limited to the specific embodiments shown.
Embodiments of the invention are described in detail below with reference to the attached drawings, but the invention can be implemented in a number of different ways, which are defined and covered by the claims.
Example 1
Referring to fig. 1, a method for track planning and real-time obstacle avoidance of an inspection robot based on obstacle region prediction according to an embodiment of the present invention includes:
step S101, establishing a dynamic obstacle reachable area prediction model based on state updating;
step S102, based on a dynamic obstacle reachable region prediction model, an elliptical reachable region under the predicted step number is obtained;
step S103, establishing a multi-step elliptical envelope potential field model based on the elliptical reachable region under the predicted step number;
step S104, based on the static obstacle model, a square obstacle potential field model and a circular obstacle potential field model of a novel Sigmoid function are established;
step S105, obtaining expected running angles and expected speed vectors of the inspection robot based on a multi-step elliptical envelope potential field model, a square obstacle potential field model of a novel Sigmoid function, a circular obstacle potential field model and a logarithmic Lyapunov gravitational field model;
step S106, obtaining the obstacle avoidance path of the robot based on the expected traveling angle and the expected speed vector.
According to the method for planning and real-time obstacle avoidance of the inspection robot track based on the obstacle region prediction, the expected running angle and expected speed vector of the inspection robot and the obstacle avoidance path based on the expected running angle and expected speed vector of the inspection robot are obtained, the technical problem that the existing inspection robot cannot avoid the obstacle in real time in the complex factory environment is solved, the movement characteristics and geometric dimensions of the obstacle are fully considered and the margin is reserved and adjusted in advance by establishing the dynamic obstacle reachable region prediction model based on the state update, the square obstacle potential field model based on the novel Sigmoid function, the circular obstacle potential field model based on the multistep elliptical enveloping potential field model, the circular obstacle potential field model and the logarithmic Lyapunov potential field model, the expected running angle and expected speed vector of the inspection robot are obtained, the technical problem that the existing inspection robot cannot avoid the obstacle in real time in the complex factory environment is solved, and then the dynamic obstacle reachable region based on the state update is determined, the dynamic obstacle reachable region prediction model is defined, the circular obstacle potential field algorithm is enabled to have a higher dynamic obstacle potential field with a higher dynamic obstacle potential field, the expected running angle and the expected speed vector is improved in real time under the complex factory environment, and the actual environment is improved, and the actual running angle of the inspection robot is improved.
Example two
Referring to fig. 2, the method for planning and real-time obstacle avoidance of a patrol robot based on obstacle region prediction provided by the second embodiment of the invention includes the establishment of an initial world grid map, a static obstacle map layer and a dynamic obstacle map layer, the construction of a multi-step elliptical envelope potential field model based on the reachable region prediction of a dynamic obstacle centroid reachable region, the construction of a square obstacle potential field and a circular potential field based on a novel Sigmoid function to correct a logarithmic Lyapunov gravity vector field, the expected speed vector and the expected driving angle of a total potential field force solver are applied on a raspberry dispatch industrial personal computer, then the solution result is sent to a lower computer STM32 singlechip through a serial port, and the singlechip returns the odometer and positioning information to an ROS operation system so as to facilitate the next solution. The method specifically comprises the following steps:
step 1: and building a two-dimensional ultra-high-precision world grid map in an actual industrial scene, and acquiring map layer information of the movement obstacle in real time. And building an ultra-high-precision grid map and a high-speed and low-speed moving obstacle grid map updated in real time in a complex industrial environment according to the multi-thread ultra-high-precision laser radar data or the red-yellow-blue three-channel data containing depth information of the light in the obbe.
Since the static obstacle can be cut into circular or square obstacles, the positions of which are fixed, all the circular and square obstacles can be mapped by a program and arranged into a global static grid map, and once the static grid map is arranged, the static map layer does not need to be updated as long as the obstacles are not moved artificially; for a moving object, the robot can greatly detect the local appearance information of the moving object through the laser radar and the binocular camera or the monocular depth three-way camera, so that the occupied area of the moving object on an obstacle map and the corresponding position coordinates are changed, and each moving object is stored in a relatively independent map layer as an irrelevant object.
Specifically, the embodiment scans an actual scene through a laser radar or an Orb medium-light depth camera, establishes a two-dimensional high-precision grid map, and establishes a static obstacle model on the basis of an obstacle obtained by scanning, wherein the static obstacle model comprises a square obstacle map model and a circular obstacle map model; wherein the square obstacle map model is represented as having a center position at (x r ,y r ) The side length is l r1 ,l r2 Then a function h can be defined with respect to the coordinates (x, y) r (x, y), specifically:
wherein the circular obstacle map model is represented as centered at (x c ,y c ) Radius r c Then a function h with respect to the coordinates (x, y) can be obtained c (x, y) is as follows:
when h r (x,y)>0,h r (x,y)＝0,h r (x,y)<0, respectively represent the outer area, the surface area and the inner area of the square barrier. When h c (x,y)>0,h c (x,y)＝0,h c (x,y)<0, respectively represent the outer region, the surface region and the inner region of the circular obstacle.
Step 2: for dynamic obstacles in intelligent factories, such as operators, various mobile robots, etc. In order to describe the dynamic obstacle more effectively, the center of mass of the dynamic obstacle is taken as the center of a circle, and the radius r is used at a certain moment c Is representative of the current area of the dynamic obstacle and makes the following assumptions: the movement speed of dynamic obstacle is in the prediction time domain T p The =p Δt is constant but with uncertain gaussian noise. Defining a state vector of a dynamic obstacle as
Wherein x is 0 ,y 0 Is the centroid position of the dynamic obstacle, +.>
Is a dynamic obstacle x 0 Speed component of direction, +.>
Is a dynamic obstacle y 0 The state equation and the measurement equation of the dynamic barrier are as follows:
wherein:
Discretizing a state equation and a measurement equation of the dynamic obstacle to obtain a state update equation under each time step, so that in the obstacle grid map layer, the inspection robot updates the dynamic obstacle substance core area in real time according to the obstacle substance core position obtained by laser radar scanning and the dynamic obstacle state update equation.
x obs_d (k+1)＝Ax obs_d (k)+ω s (k) (4)
Where a is the state update matrix of the dynamic obstacle,
is the state vector of the obstacle, and each item from left to right in the square brackets comprises the abscissa of the centroid of the dynamic obstacle, the ordinate of the centroid of the dynamic obstacle and the x of the dynamic obstacle under the perception of the robot 0 Directional velocity, dynamic obstacle y 0 A directional velocity; Δt is the control step time of the robot, μ s -N (0, Q), Q being a state update covariance matrix, the form being as follows: />
Step 3: and (3) according to a state updating equation of the mass center area of the dynamic obstacle in the step (2), providing a prediction algorithm of the reachable area of the dynamic obstacle.
The equation is updated based on the state of the dynamic obstacle first, and the future motion state of the dynamic obstacle is predicted according to the real-time detected obstacle information. Then set the prediction time domain T p =p Δt, p being a positive integer. Constructing a prediction time domain based on optimal prediction theoryT p P-step prediction variance equation of p delta t, and finally obtaining the reachable area of the dynamic obstacle centroid of p steps of the predicted steps.
Prediction step number j=1, 2 … p, prediction error
Wherein->
Optimal prediction->
Due to
A gaussian sequence of zero mean. The p-step prediction variance formula is as follows:
a in the p-step prediction variance formula j P(0)A j For the transfer term caused by the variance matrix, +.>
For the variance increase caused by noise, P (0) is the initial variance transfer matrix, which is of the form:
calculating the variance sigma of the centroid of the dynamic obstacle in the moving direction by a p-step prediction variance formula x And variance sigma in the vertical motion direction y Then the reachable area of its centroid is available (x 0 (k),y 0 (k) With a centre, a major axis of 3 sigma y Short axis of 3 sigma x Is represented by an ellipse as shown in fig. 3. Based on the analysis, a dynamic obstacle reachable area prediction algorithm based on state update is proposed as follows:
step 4: and (3) establishing a multi-step elliptical envelope potential field model based on dynamic obstacle reachable region prediction according to the dynamic obstacle reachable region centroid prediction algorithm provided in the step (3). Firstly, because the obstacle has a certain size, in order to improve the calculation efficiency, the reachable area of the dynamic obstacle is firstly expanded by r c . Let the position of the centroid of the dynamic obstacle be O 1 And is the predicted step number j=1 ellipse P 1 P 2 Center of P 1 And P 2 For its focal point, its major and minor axes are noted as 2a 1 ,2b 1 The focal length is recorded as 2c 1 The method comprises the steps of carrying out a first treatment on the surface of the Predictive step number j=p ellipse N 1 N 2 Is of focus N 1 And N 2 Its major and minor axes are denoted as 2a 2 ,2b 2 The focal length is recorded as 2c 2 The method comprises the steps of carrying out a first treatment on the surface of the Two straight lines around the ellipse are envelope common tangents to the predicted reachable region; dynamic obstacle uses radius r at a certain moment c Is described by the circular envelope of (2) the reachable region is extended by r c . Ellipse P using geometric knowledge 1 P 2 The major axis and the minor axis of (a) are respectively re-corrected to 2 (a) 1 +r c ),2(b 1 +r c ) The focal length is re-calculated as
Ellipse N 1 N 2 The major axis and the minor axis of (a) are re-calculated to be 2 (a) 2 +r c ),2(b 2 +r c ) The focal length becomes +.>
Envelope common tangent readjustment of the predicted reachable region is noted as AB and CD; let a point location in space be denoted M (x, y), the elliptical envelope potential field is described as follows:
wherein: d, d 1 ＝|2(a 1 +r c )-||MN 1 ||-||MN 2 |||，d 2 ＝|||AB||-||MB||-||MA|||，
And U AB (M) are respectively ellipses N 1 N 2 And the potential field generated by the straight line AB portion, MN 1 I is the robot to focus N 1 Distance of MN 2 I is the robot to focus N 2 Distance d of (d) 0 Is the influence distance, k, of the potential field of the dynamic barrier f And k AB Respectively is ellipse N 1 N 2 And ellipse P 1 P 2 Influence coefficient of the repulsive field. Similarly, the multi-step elliptical envelope potential energy of a dynamic barrier is as follows:
negative gradient is calculated on the envelope potential energy, so that repulsive force can be obtained:
wherein: o (O) 2 Is the focus N 1 And focus N 2 Is recorded as the midpoint of (1)
Psi and lambda respectively represent the included angles between the connecting lines from the points M (x, y) to the two ends of the AB and the AB;ellipse N 1 N 2 Long axis of 2a 2 Short axis of 2b 2 ；/>
Are respectively->
F AB The side view and the top view of the multi-step elliptical envelope potential field predicted based on the dynamic obstacle reachable region according to the embodiment are shown in fig. 5 and 6 respectively.
Similarly, the total repulsive force of the dynamic potential field is:
step 5: in the obstacle avoidance process of the robot, the robot can meet not only dynamic obstacles, but also various dynamic obstacles. Generally, an irregular static obstacle can be divided into a square obstacle and a circular obstacle by a laser semantic division and visual target detection method, and according to the step 1, a square obstacle potential field model of a novel Sigmoid function is further built on the basis of building a static obstacle model. Repulsive force function F of square obstacle 2 Negative gradient as a function of repulsive potential energy:
wherein k is ri Is the gain coefficient of the novel Sigmoid function, and the coefficient r is related to the influence range of square obstacles, S i Mathematical expression of a function representing the i-th face of a square obstacle, h c Is square obstacle function expression, r i ＝[sinβ i -sinα i ,cosα i -cosβ i ] T Is the sum of two unit vectors formed by two endpoints on the ith surface of the square obstacle and the position of the robot respectively, and the direction points to the outer side of the square obstacle.
Step 6: on the basis of establishing a static obstacle model, a circular obstacle potential field is further established according to the step 1, wherein the circular obstacle potential field is defined as follows:
wherein ρ is 0 Is the distance of influence of a circular obstacle.
Step 7: according to steps 4-6, a multi-step elliptical envelope potential field repulsive force F 1 Square obstacle potential field repulsive force F of novel Sigmoid function 2 Potential field repulsive force F of circular obstacle 3 Logarithmic Lyapunov vector attraction force F attr Total potential field force F total ＝F 1 +F 2 +F 3 +F attr ＝[F totalx ,F totaly ] T Referring to fig. 7, a desired velocity vector v of the robot in the total potential field d And the desired travel angle
Can be determined by solving the following system of equations:
||v d ||＝v d ,arctan(v d )＝arctan(F total )＝θ d (18)
wherein the logarithmic Lyapunov gravity vector field is expressed as
Indicating the Euclidean distance between the current position of the inspection robot and the position of the target point, < >>
Step 8: and 7, resolving the expected running angle and the expected speed vector of the robot in real time in a ros operation system, then controlling an instruction to the STM32 singlechip through a serial port, controlling the pose of the robot in real time through proportional integral feedback and forward feedback by the robot, and finally obtaining the obstacle avoidance path of the robot through a track deduction formula.
The track deduction formula is as follows:
θ(t+1)＝θ(t)+ω θ △t (20)
wherein [ v ] x ,v y ] T ，q d And (t), ω represents the speed of the inspection robot in the geodetic coordinate system, the position of the robot at time t, and the angular velocity, respectively.
Compared with the traditional method, the real-time obstacle avoidance algorithm provided by the embodiment considers the movement trend and the geometric dimension of the obstacle, so that the path length of the inspection robot in the obstacle avoidance process is shorter, the energy consumption is lower, and the path smoothness and the maximum driving angle change amplitude are smaller. Under the condition of the performance constraint of the inspection robot, the algorithm can more effectively realize the obstacle avoidance task of the inspection robot in a complex factory environment. Meanwhile, the robot can avoid the obstacle safely locally, and meanwhile, the problems that the path is far away and the obstacle is not avoided timely can be solved. The average linear acceleration and the average angular velocity of the robot are larger than the corresponding indexes of the obstacle avoidance algorithm under the dynamic window algorithm, so that higher performance requirements are provided for the voltage driving module of the inspection mobile robot, and the connecting device is more prone to abrasion. Based on this, the proposed algorithm is more reasonable and practical.
Referring to fig. 8, an inspection robot trajectory planning and real-time obstacle avoidance system based on obstacle region prediction according to an embodiment of the present invention includes:
the system comprises a memory 10, a processor 20 and a computer program stored in the memory 10 and capable of running on the processor 20, wherein the steps of the inspection robot track planning and real-time obstacle avoidance method based on obstacle region prediction, which are proposed in the embodiment, are realized when the processor 20 executes the computer program.
The specific working process and working principle of the inspection robot trajectory planning and real-time obstacle avoidance system based on the obstacle region prediction in this embodiment may refer to the working process and working principle of the inspection robot trajectory planning and real-time obstacle avoidance method based on the obstacle region prediction in this embodiment.
The above is only a preferred embodiment of the present invention, and is not intended to limit the present invention, but various modifications and variations can be made to the present invention by those skilled in the art. Any modification, equivalent replacement, improvement, etc. made within the spirit and principle of the present invention should be included in the protection scope of the present invention.
Claims (6)
1. The method for planning the track of the inspection robot and avoiding the obstacle in real time based on the prediction of the obstacle area is characterized by comprising the following steps:
establishing a dynamic obstacle reachable area prediction model based on state updating;
based on the dynamic obstacle reachable region prediction model, an elliptical reachable region under the predicted step number is obtained;
establishing a multi-step elliptical envelope potential field model based on the elliptical reachable region at the predicted number of steps, wherein establishing the multi-step elliptical envelope potential field model based on the elliptical reachable region at the predicted number of steps comprises:
expanding the ellipse reachable area under the predicted step number by a preset radius;
obtaining an elliptical envelope potential field according to the elliptical reachable region in the predicted step number after the outward expansion of the preset radius;
establishing a multi-step elliptical envelope potential field model according to the elliptical envelope potential field, wherein the multi-step elliptical envelope potential field model specifically comprises:
wherein U is 1 For a multi-step elliptical envelope potential field,
U AB (M 1 )、U CD (M 1 ) Respectively any point M in multi-step elliptical envelope potential field 1 In oval reachable area N 1 N 2 Elliptical reachable region P 1 P 2 Potential field generated by line AB and line CD, and ellipse reaching region P 1 P 2 And oval reachable area N 1 N 2 The ellipse reachable areas corresponding to the predicted step numbers j=1 and j=p are respectively, and the straight line AB and the straight line CD are the ellipse reachable areas P 1 P 2 And oval reachable area N 1 N 2 Is a common envelope tangent of (a);
based on the static obstacle model, a square obstacle potential field model and a circular obstacle potential field model of the novel Sigmoid function are established, wherein based on the static obstacle model, the specific formula of the square obstacle potential field model of the novel Sigmoid function is established as follows:
wherein U is 2 (M 2 ) Any point M in square obstacle potential field model of novel Sigmoid function 2 Square barrier potential field, k ri Gain coefficient of novel Sigmoid function, r is influence coefficient related to influence range of square obstacle, S i Mathematical expression of a function representing the i-th face of a square obstacle, h c (M 2 ) Any point M in square obstacle potential field model of novel Sigmoid function 2 Square obstacle function expression of (a);
based on the static barrier model, a specific formula for establishing a circular barrier potential field model is as follows:
wherein U is 3 (M 3 ) Is any point M in a circular barrier potential field model 3 Lambda is the circular barrier potential field coefficient, r c Radius of circular obstacle, h c (M 3 ) Is any point M in a circular barrier potential field model 3 Circular barrier function expression of ρ 0 Is the influence distance of the circular obstacle;
obtaining an expected running angle and an expected speed vector of the inspection robot based on the multi-step elliptical envelope potential field model, a square obstacle potential field model of a novel Sigmoid function, a circular obstacle potential field model and a logarithmic Lyapunov gravitational field model;
and obtaining the obstacle avoidance path of the robot based on the expected traveling angle and the expected speed vector.
2. The method for planning and real-time obstacle avoidance of a tour inspection robot trajectory based on obstacle region prediction according to claim 1, wherein obtaining an elliptical reachable region under a preset number of steps based on the dynamic obstacle reachable region prediction model comprises:
initializing a motion speed variance, a measurement noise variance and an initial variance transfer matrix of the dynamic barrier;
obtaining variances of the mass centers of the dynamic barriers in the motion direction and the vertical motion direction under the prediction steps according to a p-step prediction variance equation, wherein the p-step prediction variance equation is specifically:
wherein P is k+j|k For the variance matrix of the predicted step number j at the k moment, P (0) is the initial variance transfer matrix, A j Updating a matrix for the state transition of the predicted step number j, wherein Q is a measurement covariance matrix;
according to the mass center updating equation of the dynamic obstacle, obtaining the mass center position of the dynamic obstacle under the predicted step number;
and constructing an ellipse reachable region under the preset step number according to the barycenter position of the dynamic obstacle and the variance of the barycenter of the dynamic obstacle in the moving direction and the vertical moving direction under the predicted step number.
3. The method for planning and real-time obstacle avoidance of a tour inspection robot according to claim 2, wherein the constructing an ellipse reachable region under a preset number of steps according to the barycenter position of the dynamic obstacle under the predicted number of steps and the variance of the barycenter of the dynamic obstacle in the moving direction and the vertical moving direction is specifically as follows:
and taking the centroid position of the previous step of the preset step number as a center, and taking the 3-time variance of the centroid of the dynamic obstacle in the moving direction and the vertical moving direction as a long and short axis under the preset step number to construct an ellipse reachable region.
4. The method for planning and real-time obstacle avoidance of a patrol robot based on obstacle region prediction according to claim 3, wherein obtaining the expected travel angle and the expected speed vector of the patrol robot based on the multi-step elliptical envelope potential field model, the square obstacle potential field model of the novel Sigmoid function, the circular obstacle potential field model, and the logarithmic Lyapunov gravitational field model comprises:
correspondingly obtaining a multistep elliptical envelope potential field, a square obstacle potential field of the novel Sigmoid function, a circular obstacle potential field and a logarithmic Lyapunov vector gravitation according to the multistep elliptical envelope potential field model, the square obstacle potential field of the novel Sigmoid function, the circular obstacle potential field and the logarithmic Lyapunov gravitational field model;
respectively solving negative gradients of the multistep elliptical enveloping potential field, the square obstacle potential field of the novel Sigmoid function and the circular obstacle potential field, and correspondingly obtaining multistep elliptical enveloping potential field repulsive force, the square obstacle potential field repulsive force of the novel Sigmoid function and the circular obstacle potential field repulsive force;
and obtaining the expected running angle and the expected speed vector of the inspection robot according to the multi-step elliptical envelope potential field repulsive force, the square obstacle potential field repulsive force of the novel Sigmoid function, the circular obstacle potential field repulsive force and the logarithmic Lyapunov vector attractive force.
5. The method for planning and real-time obstacle avoidance of a patrol robot based on obstacle region prediction according to claim 4, wherein the calculation formula for obtaining the obstacle avoidance path of the robot based on the expected travel angle and the expected speed vector is:
θ(t+1)＝θ(t)+ω θ Δt
wherein q is d (t+1) and q d (t) represents the positions of the inspection robots at time t+1 and time t, v x And v y The speed of the inspection robot in the x and y directions is represented, theta (t+1) and theta (t) represent the direction angles of the inspection robot at the time t+1 and the time t, and omega θ Indicating the angular velocity of the inspection robot.
6. An inspection robot trajectory planning and real-time obstacle avoidance system based on obstacle region prediction, the system comprising:
memory (10), a processor (20) and a computer program stored on the memory (10) and executable on the processor (20), characterized in that the processor (20) implements the steps of the method according to any of the preceding claims 1 to 5 when executing the computer program.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110923744.0A CN113759900B (en) | 2021-08-12 | 2021-08-12 | Method and system for track planning and real-time obstacle avoidance of inspection robot based on obstacle region prediction |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110923744.0A CN113759900B (en) | 2021-08-12 | 2021-08-12 | Method and system for track planning and real-time obstacle avoidance of inspection robot based on obstacle region prediction |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113759900A CN113759900A (en) | 2021-12-07 |
| CN113759900B true CN113759900B (en) | 2023-05-02 |
Family
ID=78789080
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110923744.0A Active CN113759900B (en) | 2021-08-12 | 2021-08-12 | Method and system for track planning and real-time obstacle avoidance of inspection robot based on obstacle region prediction |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113759900B (en) |
Families Citing this family (7)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114384919B (en) * | 2022-01-17 | 2023-06-27 | 北京格睿能源科技有限公司 | Vehicle obstacle avoidance path planning method and system based on large obstacle form information |
| CN114578827B (en) * | 2022-03-22 | 2023-03-24 | 北京理工大学 | Distributed multi-agent cooperative full-coverage path planning method |
| CN114610040A (en) * | 2022-04-02 | 2022-06-10 | 天津大学 | Autonomous obstacle avoidance learning control method and device applied to unmanned operating system |
| CN114637302B (en) * | 2022-04-15 | 2022-10-18 | 安徽农业大学 | Automatic advancing obstacle avoidance method and system based on computer vision |
| CN115454061B (en) * | 2022-08-31 | 2024-03-29 | 安徽机电职业技术学院 | Robot path obstacle avoidance method and system based on 3D technology |
| CN115309169B (en) * | 2022-10-11 | 2022-12-20 | 天地科技股份有限公司 | Underground unmanned vehicle control method and device |
| CN117474190B (en) * | 2023-12-28 | 2024-02-27 | 磐石浩海(北京)智能科技有限公司 | Automatic inspection method and device for cabinet |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111337941A (en) * | 2020-03-18 | 2020-06-26 | 中国科学技术大学 | Dynamic obstacle tracking method based on sparse laser radar data |
Family Cites Families (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN103558856A (en) * | 2013-11-21 | 2014-02-05 | 东南大学 | Service mobile robot navigation method in dynamic environment |
| CN108827312B (en) * | 2018-08-08 | 2021-10-08 | 清华大学 | Cooperative game path planning method based on neural network and artificial potential field |
| WO2020206071A1 (en) * | 2019-04-02 | 2020-10-08 | Brain Corporation | Systems, apparatuses, and methods for cost evaluation and motion planning for robotic devices |
| CN112904842B (en) * | 2021-01-13 | 2022-07-15 | 中南大学 | Mobile robot path planning and optimizing method based on cost potential field |
-
2021
- 2021-08-12 CN CN202110923744.0A patent/CN113759900B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN111337941A (en) * | 2020-03-18 | 2020-06-26 | 中国科学技术大学 | Dynamic obstacle tracking method based on sparse laser radar data |
Also Published As
| Publication number | Publication date |
|---|---|
| CN113759900A (en) | 2021-12-07 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN113759900B (en) | Method and system for track planning and real-time obstacle avoidance of inspection robot based on obstacle region prediction | |
| Asadi et al. | An integrated UGV-UAV system for construction site data collection | |
| Xu et al. | Behavior‐based formation control of swarm robots | |
| Saravanakumar et al. | Multipoint potential field method for path planning of autonomous underwater vehicles in 3D space | |
| Shiltagh et al. | Optimal path planning for intelligent mobile robot navigation using modified particle swarm optimization | |
| Baek et al. | Optimal path planning of a target-following fixed-wing UAV using sequential decision processes | |
| Williams et al. | Dynamic obstacle avoidance for an omnidirectional mobile robot | |
| Li et al. | Simultaneous obstacle avoidance and target tracking of multiple wheeled mobile robots with certified safety | |
| WO2020136978A1 (en) | Path determination method | |
| Xu et al. | The mobile robot path planning with motion constraints based on Bug algorithm | |
| Do Quang et al. | An approach to design navigation system for omnidirectional mobile robot based on ROS | |
| Steiner et al. | Open-sector rapid-reactive collision avoidance: Application in aerial robot navigation through outdoor unstructured environments | |
| KR20120129002A (en) | Underwater robot and Method for controlling the same | |
| Bayer et al. | Speeded up elevation map for exploration of large-scale subterranean environments | |
| Wang et al. | A fuzzy logic path planning algorithm based on geometric landmarks and kinetic constraints | |
| Tang et al. | Obstacle Avoidance Motion in Mobile Robotics | |
| Wang et al. | Potential-based obstacle avoidance in formation control | |
| Lai et al. | Hierarchical incremental path planning and situation-dependent optimized dynamic motion planning considering accelerations | |
| KR20140086246A (en) | Method and system for formation control of multiple mobile robots | |
| Vasseur et al. | Navigation of car-like mobile robots in obstructed environments using convex polygonal cells | |
| Pandey et al. | Trajectory Planning and Collision Control of a Mobile Robot: A Penalty‐Based PSO Approach | |
| Wang et al. | Obstacle detection and obstacle-surmounting planning for a wheel-legged robot based on Lidar | |
| Sinha et al. | A∗ WRBAS: Space Mobile Robotics Control Conceptual Model Using IoRT Reinforcement Learning and Tracking with Noise Estimation Using EKF | |
| Yang et al. | Research and implementation of automatic navigation and driving of tracked robot in tunnel based on slam | |
| Wu et al. | Application of 6-Dof Robot Motion Planning in Fabrication |
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 |