CN111857121B - Walking obstacle avoidance method and system for patrol robot based on inertial navigation and laser radar - Google Patents
Walking obstacle avoidance method and system for patrol robot based on inertial navigation and laser radar Download PDFInfo
- Publication number
- CN111857121B CN111857121B CN202010205064.0A CN202010205064A CN111857121B CN 111857121 B CN111857121 B CN 111857121B CN 202010205064 A CN202010205064 A CN 202010205064A CN 111857121 B CN111857121 B CN 111857121B
- Authority
- CN
- China
- Prior art keywords
- patrol
- obstacle
- patrol robot
- angle
- robot
- 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 30
- 238000004364 calculation method Methods 0.000 claims description 5
- 238000012937 correction Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 3
- 230000006870 function Effects 0.000 claims description 3
- 238000006073 displacement reaction Methods 0.000 description 28
- 230000007547 defect Effects 0.000 description 4
- 238000005516 engineering process Methods 0.000 description 3
- 238000009499 grossing Methods 0.000 description 3
- 238000012986 modification Methods 0.000 description 3
- 230000004048 modification Effects 0.000 description 3
- 238000005070 sampling Methods 0.000 description 3
- 230000002159 abnormal effect Effects 0.000 description 2
- 238000013459 approach Methods 0.000 description 2
- 238000001514 detection method Methods 0.000 description 2
- 238000010586 diagram Methods 0.000 description 2
- 206010034719 Personality change Diseases 0.000 description 1
- 230000001133 acceleration Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007423 decrease Effects 0.000 description 1
- 238000013461 design Methods 0.000 description 1
- 230000000694 effects Effects 0.000 description 1
- 230000001788 irregular Effects 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000011218 segmentation Effects 0.000 description 1
- 239000000126 substance Substances 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/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/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
-
- 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/028—Control of position or course in two dimensions specially adapted to land vehicles using signals provided by a source external to the vehicle using a RF signal
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- Aviation & Aerospace Engineering (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Optics & Photonics (AREA)
- Electromagnetism (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention provides a walking obstacle avoidance method and system for a patrol robot based on inertial navigation and laser radar. The walking obstacle avoidance method of the patrol robot based on inertial navigation and laser radar comprises the following steps: a first step of: aiming at the starting point and the destination of the patrol robot, recording the set patrol route track by using a learning mode, and establishing a digital map for subsequent patrol; and a second step of: and navigating by using the established map to patrol the patrol robot along the track of the set patrol route.
Description
Technical Field
The invention relates to the field of navigation, in particular to a walking obstacle avoidance method and system for a patrol robot based on inertial navigation and laser radar.
Background
At present, two methods for calculating the travelling distance of the trolley through a GPS module in intelligent vehicle-mounted hardware are mainly adopted:
One method is based mainly on GPS module longitude and latitude coordinate points and calculated according to an arc length formula between two points on the earth. The method has the following defects: ① When the sampling frequency of the GPS module is lower, the calculated distance error is larger; ② When the GPS signals are bad, such as through environments of tunnels, high buildings, forest shadows and the like, GPS coordinate points drift and are lost, and the deviation between the running distance calculated according to an arc length formula and the actual running distance is larger;
Another approach is to calculate the distance travelled by GPS velocity approximation. The method has the following defects: ① When the GPS sampling frequency is low, the speed change between adjacent sampling points is irregular, so that the running distance calculated according to the GPS speed is inaccurate; ② The GPS speed can generate an abnormal value, and the deviation of the running distance calculated according to the abnormal GPS speed is large;
The traditional obstacle avoidance methods, such as infrared obstacle avoidance, ultrasonic obstacle avoidance, binocular vision navigation systems and the like, have defects and drawbacks. The infrared obstacle avoidance effect is in direct proportion to the number of the installed infrared sensors, so that the cost control, the appearance design and the like are limited, the infrared detection distance is limited, and the far-distance obstacles cannot be found in time and avoided in advance; the ultrasonic obstacle avoidance can not detect the size and shape of the obstacle, and can only detect the obstacle when the obstacle is close to the obstacle, and the obstacle may not be avoided in time; binocular vision navigation also suffers from close detection distance, and is affected at night or in the case of weak light.
Disclosure of Invention
Aiming at the defects in the prior art, the technical problem to be solved by the invention is to provide a navigation method which can accurately calculate the travel distance of the patrol robot through inertial navigation information and has wide application scene by using a laser radar to assist obstacle avoidance.
According to the invention, a walking obstacle avoidance method of a patrol robot based on inertial navigation and laser radar is provided, comprising the following steps:
A first step of: aiming at the starting point and the destination of the patrol robot, recording the set patrol route track by using a learning mode, and establishing a digital map for subsequent patrol;
and a second step of: and navigating by using the established map to patrol the patrol robot along the track of the set patrol route.
Preferably, in the second step, when the patrol robot encounters an obstacle in the middle, the laser radar scans to obtain point cloud data of the obstacle in front of the patrol robot, and the data are clustered to obtain a front obstacle cluster; and determining the turning angle of the robot and whether the robot stops according to the distance between the obstacles and the gaps between the obstacles.
Preferably, in the second step, when the patrol robot encounters an obstacle in the middle, the control unit of the patrol robot uses the azimuth and speed signals output by the satellite navigation device and the distance and angle data of the obstacle obtained by laser radar scanning to bypass the obstacle and then uses the angle correction algorithm to control the motor to return to the original route.
Preferably, in the second step, the control unit of the patrol robot calculates a deviation value between the current position of the patrol car and the nearest point on the digital route by using the azimuth and speed signals output by the satellite navigation device and the distance and angle data of the obstacle obtained by laser radar scanning, calculates a control amount returned to the digital route, and calculates a control amount for walking along the digital route if the patrol car is on the digital route.
Preferably, in the second step, the inertial navigation unit of the patrol robot outputs mileage, speed and course angle information every fixed time after the patrol robot starts walking, when the position coordinates are detected, the map coordinates closest to the position coordinates and the predetermined map coordinates subsequent to the map coordinates are searched, a straight line from the current point to the map point is fitted, an included angle between the straight line and the north direction is calculated, and the included angle is input to the motor for direction control.
Preferably, in the second step:
calculating the distance d between the center point O 'of the obstacle cluster and the origin point O of the coordinates and the included angle phi between the center point O' and the x axis, using Representing obstacle clusters, and calculating a maximum included angle delta alpha between an obstacle cluster closure and a straight line OO'; calculation ofWherein k sf is more than 1, W is the width of the unmanned vehicle, and D sf is the transverse safety distance; calculating D sr＝-ksr·v2/2 a, wherein k sr＞1,Dsr is a braking safety distance;
calculating the repulsive force generated by d (phi) at any angle theta:
Or (b)
A or C is a cluster boundary point:
defining the total force K RF (theta) in the direction of the angle theta as the maximum value of repulsive force generated by each cluster, wherein D m is the set maximum evaluation distance;
Setting a local target point A (d) reached by the patrol robot, wherein theta obj is a target point azimuth angle, and d obj is a distance from the target point to an origin point; the attraction force generated by the target point at the angle theta is as follows:
KCF(θ)＝cos(θ-θobj)，；
setting a cost function that the patrol robot can pass at an angle theta:
KP(θ)＝KCF(θ)/KRF(θ)，
Selection of The angle theta Cur＝argmax(KPC corresponding to the maximum value) is used as the current steering angle of the unmanned vehicle.
Preferably, the patrol robot is stopped when K PC =0.
Preferably, the lidar is a 16-wire lidar.
The present invention is a combined navigation method for estimating the distance travelled by a patrol robot as a preset route by means of acceleration and attitude change of an inertial measurement unit of the patrol robot as a carrier using inertial navigation initial information and by integrating the measured values over time and realizing road obstacle avoidance using a laser radar such as 16-line laser radar, which combines two or more different navigation devices in an appropriate manner, and which obtains a navigation system having higher navigation performance than that of either system alone using complementarity in performance.
Drawings
The invention will be more fully understood and its attendant advantages and features will be more readily understood by reference to the following detailed description, when taken in conjunction with the accompanying drawings, in which:
Fig. 1 schematically shows a flow chart of a walking obstacle avoidance method of a patrol robot based on inertial navigation and lidar according to a preferred embodiment of the invention.
Fig. 2 schematically shows a map navigation schematic of a walking obstacle avoidance method of a patrol robot based on inertial navigation and laser radar according to a preferred embodiment of the present invention.
Fig. 3 schematically shows a motor control schematic of a walking obstacle avoidance method of a patrol robot based on inertial navigation and lidar according to a preferred embodiment of the invention.
Fig. 4 schematically shows an obstacle avoidance algorithm of a walking obstacle avoidance method of a patrol robot based on inertial navigation and lidar according to a preferred embodiment of the invention.
Fig. 5 schematically shows a schematic view of obstacle avoidance areas of a walking obstacle avoidance method of a patrol robot based on inertial navigation and lidar according to a preferred embodiment of the invention.
Fig. 6 schematically shows an example of a patrol route of a walking obstacle avoidance system of a patrol robot based on inertial navigation and laser radar according to a preferred embodiment of the present invention.
Fig. 7 schematically shows a system block diagram of a walking obstacle avoidance system of a patrol robot based on inertial navigation and lidar according to a preferred embodiment of the invention.
It should be noted that the drawings are for illustrating the invention and are not to be construed as limiting the invention. Note that the drawings representing structures may not be drawn to scale. Also, in the drawings, the same or similar elements are denoted by the same or similar reference numerals.
Detailed Description
In order that the invention may be more readily understood, a detailed description of the invention is provided below along with specific embodiments and accompanying figures.
Fig. 1 schematically shows a flow chart of a walking obstacle avoidance method of a patrol robot based on inertial navigation and lidar according to a preferred embodiment of the invention. Preferably, the lidar used is a 16-line lidar.
As shown in fig. 2, the walking obstacle avoidance method of the patrol robot based on inertial navigation and laser radar according to the preferred embodiment of the present invention comprises:
a first step S1: aiming at the starting point and the destination of the patrol robot, recording the set patrol route track by using a learning mode, and establishing a digital map for subsequent patrol;
specifically, for example, as shown in fig. 2, in the first step, the directional distance is recorded And map coordinates Represents the directional distance of the current position relative to the starting point in the north direction,Representing the directional distance between the current position and the starting point in the forward eastern direction, setting M (0) = [0,0], and recording the longitude and latitude coordinates of the starting point: m 0.
Because GPS causes the position deviation to be big because of factors such as shelter from, and inertial navigation system's accumulated error, can calculate relative position through GPS and inertial navigation joint Kalman equation to reduce the error.
Smoothing (segment smoothing) the map: segmentation smoothing further reduces map acquisition errors caused by inaccurate longitude and latitude.
A second step S2: and navigating by using the established map to patrol the patrol robot along the track of the set patrol route.
In the second step, when the patrol robot encounters an obstacle in the middle, the laser radar scans to obtain point cloud data of the obstacle in front of the patrol robot, and the data are clustered to obtain a front obstacle cluster; and determining the turning angle of the robot according to the distance between the obstacles and the gap between the obstacles.
And in the second step, when the patrol robot encounters an obstacle in the middle, the control unit of the patrol robot uses the azimuth and speed signals output by the satellite navigation device and the distance and angle data of the obstacle obtained by laser radar scanning to bypass the obstacle and then uses an angle correction algorithm to control the motor to return to the original route.
For example, in the second step, the control unit of the patrol robot calculates the deviation value between the current position of the patrol car and the nearest point on the digital route by using the azimuth and speed signals output by the satellite navigation device and the distance and angle data of the obstacle obtained by laser radar scanning, and calculates the control amount returned to the digital route, and if the patrol car is on the digital route, calculates the control amount walked along the digital route.
And after the patrol robot starts to walk, the inertial navigation unit of the patrol robot outputs mileage, speed and course angle information at fixed time intervals. At a certain moment T the position coordinates are detectedSearching for position coordinatesThe map coordinates closest to the map coordinates and several map coordinates (k+1, k+2) subsequent to the map coordinates, a straight line from the current point to the map point (k=0,..m) is fitted, an angle θ s between the straight line and the north direction is calculated, and the angle θ s is input to the motor for direction control, as shown in fig. 3.
As shown in fig. 4, the point cloud data of the obstacle in front of the patrol robot is obtained by laser radar scanning, and the data are clustered to obtain front obstacle clusters; and determining the turning angle of the robot and whether the robot stops according to the distance between the obstacles and the gaps between the obstacles.
And (3) calculating a repulsive force field generated by obstacle clustering:
calculating the distance d between the center point O 'of the obstacle cluster and the origin of coordinates (the current position of the patrol robot) O, and the included angle phi between the center point O' and the x-axis (the tangent line of the current position of the patrol robot at the track of the patrol route), using Representing obstacle clusters, and calculating a maximum included angle delta alpha between an obstacle cluster closure and a straight line OO'; calculation ofWherein k sf >, W is the width of the unmanned vehicle, D sf is a parameter related to the width of the unmanned vehicle, and is a transverse safety distance; d sr＝-ksr·v2/2 a is calculated, where k sr＞1,Dsr is the distance braked from the current speed v, which is the braking safety distance.
The repulsive force generated by d (phi) at any angle theta is calculated as follows:
Or (b)
A or C is a cluster boundary point:
The repulsive force is generated by each obstacle cluster at a certain angle theta, and the total force K RF (theta) in the direction of the angle theta is defined as the maximum value of the repulsive force generated by each cluster.
Where D m is the set maximum estimated distance, the obstacle exceeding D m is the minimum repulsive force.
The repulsive force is affected by two aspects:
obstacle distance: the closer the obstacle is to the origin O, the greater the repulsive force; meanwhile, the larger the range of the included angle is affected;
Obstacle clustering scale: the larger the dimension, the larger the repulsive force affects the included angle, i.e. Δα.
For example, considering that the speed of the unmanned vehicle is not fast, the simplified braking safety distance D sf =0.8m is independent of the running speed; maximum evaluation distance D m =10m.
Gravitational field generated by target point:
Setting a local target point A (d) reached by the patrol robot, wherein theta obj is a target point azimuth angle, and d obj is a distance from the target point to an origin point; the attraction force generated by the target point at the angle theta is as follows:
K CF(θ)＝cos(θ-θobj) that changes smoothly as θ approaches θ obj, and that K CF (θ) decreases rapidly as θ moves away from θ obj.
Cost function that unmanned vehicles can pass at a certain angle:
KP(θ)＝KCF(θ)/KRF(θ)，
Selection of The angle theta Cur＝argmax(KPC corresponding to the maximum value) is used as the current steering angle of the unmanned vehicle.
Unmanned vehicle movement decision:
k PC =0, park;
K PC is more than 0, and theta Cur＝argmax(KPC) is selected as the current steering angle of the unmanned vehicle;
speed of:
Here K PCmax＝Dm-Dsr,KPCmax corresponds to the scenario when the attractive force is maximum and the repulsive force is minimum.
Unmanned vehicle obstacle avoidance area division is as shown in fig. 5:
in the parking area, the unmanned vehicle is stopped, and the state=3 is a stopped state;
In a steering area, the unmanned vehicle determines steering direction and speed according to a potential field method, and the state=2 is an obstacle avoidance state;
Outside the steering region, the unmanned vehicle determines the steering direction and speed according to the potential field method, and the state=1 is a normal running state.
Fig. 6 schematically shows an example of a patrol route of a walking obstacle avoidance system of a patrol robot based on inertial navigation and laser radar according to a preferred embodiment of the present invention.
1. Starting point:
1) The patrol car stops at the starting point;
2) The transverse deviation is less than 0.2m, and the longitudinal deviation is less than 0.5m;
3) The azimuth angle is arbitrary;
2. And (3) straight line running:
1) Azimuth guide advancing;
2) Calculating the east-north displacement according to the speed;
3) Calculating the transverse displacement and correcting the transverse displacement;
4) Calculating the width and length of a forward unobstructed road, and giving obstacle detouring angle data (no obstacle, value 0);
3. encountering an obstacle:
1) According to the position of the obstacle, judging and giving steering control data, and implementing steering forward;
2) Calculating the east-north displacement according to the speed;
3) Calculating the transverse displacement and correcting the transverse displacement;
4) Calculating the width and length of a forward unobstructed road and giving obstacle detouring angle data;
4. Around obstacles:
1) According to the position of the obstacle, judging and giving steering control data, and implementing steering forward;
2) Calculating the east-north displacement according to the speed;
3) Calculating the width and length of a forward unobstructed road and giving obstacle detouring angle data;
5. obstacle crossing:
1) Determining that the obstacle is outdated according to the position of the obstacle, calculating and giving steering control data according to the closest point data of the digital route, and implementing steering forward;
2) Calculating the east-north displacement according to the speed;
3) Adjusting the advancing speed according to the east-north displacement deviation until the advancing speed returns to the digital route;
4) Calculating the width and length of a forward unobstructed road and giving obstacle detouring angle data;
6. advancing along a preset route:
1) Calculating east and north displacement according to the speed, and starting azimuth navigation to advance when the displacement returns to the digital route;
2) Calculating the east-north displacement according to the speed;
3) Calculating the transverse displacement and correcting the transverse displacement;
4) Calculating the width and length of a forward unobstructed road and giving obstacle detouring angle data;
7. correcting a turning part:
1) When the linear displacement reaches-2 m at the corner, starting the laser radar to correct the turning point;
2) Calculating the east-north displacement according to the speed;
3) Calculating the transverse displacement and correcting the transverse displacement;
4) Calculating the width and length of a forward unobstructed road and giving obstacle detouring angle data;
8. Steering advances:
1) When the laser radar finishes correcting the turning point, starting the next section of angle guiding, and implementing steering;
2) Calculating the east displacement and the north displacement of the new section according to the speed;
3) Calculating the transverse displacement and correcting the transverse displacement;
4) Calculating the width and length of a forward unobstructed road and giving obstacle detouring angle data;
9. Endpoint stop:
1) When the angle guiding is normal;
2) The northeast displacement calculated according to the speed returns to the starting point;
3) The width and the length of the forward clear road are proper;
4) Stopping;
10. Restarting:
1) When the plurality of patrol is not finished;
2) Starting angle guidance and adjusting to a patrol starting state;
3) Angle guiding and advancing;
4) Calculating the east displacement and the north displacement of the new section according to the speed;
5) Calculating the transverse displacement and correcting the transverse displacement;
6) Calculating the width and length of a forward unobstructed road and giving obstacle detouring angle data;
fig. 7 schematically shows a system block diagram of a walking obstacle avoidance system of a patrol robot based on inertial navigation and lidar according to a preferred embodiment of the invention.
As shown in fig. 7, the walking obstacle avoidance system of the patrol robot based on inertial navigation and laser radar according to the preferred embodiment of the present invention comprises: the system can be used for executing the walking obstacle avoidance method of the patrol robot by the laser radar 1, the industrial personal computer 2, the satellite navigation unit 3 and the control unit 4.
Compared with the prior art, the invention solves the problem that the motor drives the crawler belt to rotate at an inaccurate speed in the travelling process of the patrol robot, greatly reduces the error between the actual gesture of the patrol robot and a preset route, and assists in the calculation of the north-south displacement; in the aspect of obstacle avoidance and obstacle avoidance, the patrol robot can accurately return to the original set route after avoiding the obstacle, and patrol of a complex route can be realized by fine adjustment of control parameters in actual use, so that the patrol robot has higher flexibility and practicability in application.
It should be noted that, unless specifically stated otherwise, the terms "first," "second," "third," and the like in the specification are used merely as a distinction between various components, elements, steps, etc. in the specification, and are not used to denote a logical or sequential relationship between various components, elements, steps, etc.
It will be appreciated that although the invention has been described above in terms of preferred embodiments, the above embodiments are not intended to limit the invention. Many possible variations and modifications of the disclosed technology can be made by anyone skilled in the art without departing from the scope of the technology, or the technology can be modified to be equivalent. Therefore, any simple modification, equivalent variation and modification of the above embodiments according to the technical substance of the present invention still fall within the scope of the technical solution of the present invention.
Claims (6)
1. A walking obstacle avoidance method of a patrol robot based on inertial navigation and laser radar is characterized by comprising the following steps:
A first step of: aiming at the starting point and the destination of the patrol robot, recording the set patrol route track by using a learning mode, and establishing a digital map for subsequent patrol;
And a second step of: navigation is carried out by using the established map so that the patrol robot can patrol along the track of the set patrol route; in the second step, when the patrol robot encounters an obstacle in the middle, the laser radar scans to obtain point cloud data of the obstacle in front of the patrol robot, and the data are clustered to obtain front obstacle clusters; determining the turning angle of the robot and whether the robot stops according to the distance between the obstacles and the gaps between the obstacles;
Further, in the second step:
Calculating the distance d between the central point O 'of the obstacle cluster and the origin O of coordinates, and forming an included angle between the line between the central point O' and the origin O of coordinates and the x axis Wherein the origin of coordinates O is the current position of the patrol robot, the x-axis is the tangent line of the current position of the patrol robot at the track of the patrol route, andRepresenting obstacle clusters, and calculating a maximum included angle delta alpha between an obstacle cluster closure and a straight line OO'; calculation ofWherein k sf is more than 1, W is the width of the patrol robot, and D sf is the transverse safety distance; calculating D sr＝-ksr·v2/2 a, wherein k sr＞1,Dsr is a braking safety distance and v is a current speed;
Calculation of Repulsive force generated at an arbitrary angle θ:
Or (b)
Wherein D m is the set maximum evaluation distance;
A or C is a cluster boundary point:
Defining the total force K RF (theta) in the direction of the angle theta as the maximum value of repulsive force generated by each cluster;
Setting a local target point A (d) reached by the patrol robot, wherein theta obj is a target point azimuth angle, and d obj is a distance from the target point to an origin point; the attraction force generated by the target point at the angle theta is as follows:
KCF(θ)＝cos(θ-θobj)，
setting a cost function that the patrol robot can pass at an angle theta:
KP(θ)＝KCF(θ)/KRF(θ)，
Selection of The angle theta Cur＝argmax(KPC corresponding to the maximum value) is used as the current steering angle of the patrol robot.
2. The walking obstacle avoidance method of claim 1 wherein in the second step, when the patrol robot encounters an obstacle in transit, the control unit of the patrol robot uses the azimuth and speed signals output by the satellite navigation device and the distance and angle data of the obstacle obtained by laser radar scanning to bypass the obstacle and then uses an angle correction algorithm to control the motor to return to the original route.
3. The walking obstacle avoidance method of claim 1 wherein in the second step, the control unit of the patrol robot calculates the deviation value of the current position of the patrol car from the nearest point on the digital route using the azimuth and speed signals output from the satellite navigation device and the distance and angle data of the obstacle scanned by the laser radar, and calculates the control amount returned to the digital route, and if the patrol car is on the digital route, calculates the control amount walked along the digital route.
4. The walking obstacle avoidance method of claim 1 wherein in the second step, the inertial navigation unit of the patrol robot outputs mileage, speed, heading angle information at fixed time intervals after the patrol robot starts walking, when the position coordinates are detected, the map coordinates closest to the position coordinates and the predetermined map coordinates subsequent to the map coordinates are found, a straight line from the current point to the map point is fitted, an included angle between the straight line and the north direction is calculated, and the included angle is input to the motor for directional control.
5. The walking obstacle avoidance method of patrol robot of claim 1 wherein the patrol robot is stopped at K PC =0.
6. The walking obstacle avoidance method of patrol robots according to claim 1 or 2, wherein the lidar is a 16-line lidar.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010205064.0A CN111857121B (en) | 2020-03-20 | 2020-03-20 | Walking obstacle avoidance method and system for patrol robot based on inertial navigation and laser radar |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202010205064.0A CN111857121B (en) | 2020-03-20 | 2020-03-20 | Walking obstacle avoidance method and system for patrol robot based on inertial navigation and laser radar |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN111857121A CN111857121A (en) | 2020-10-30 |
| CN111857121B true CN111857121B (en) | 2024-11-01 |
Family
ID=72985044
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202010205064.0A Active CN111857121B (en) | 2020-03-20 | 2020-03-20 | Walking obstacle avoidance method and system for patrol robot based on inertial navigation and laser radar |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN111857121B (en) |
Families Citing this family (4)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN112294197A (en) * | 2020-11-04 | 2021-02-02 | 深圳市普森斯科技有限公司 | Sweeping control method of sweeper, electronic device and storage medium |
| CN112894758A (en) * | 2021-04-01 | 2021-06-04 | 广东南牧机械设备有限公司 | Robot cleaning control system, method and device and computer equipment |
| CN113237488B (en) * | 2021-05-14 | 2024-07-05 | 徕兄健康科技(威海)有限责任公司 | Navigation system and navigation method based on binocular vision and curb finding technology |
| CN114355944B (en) * | 2022-01-05 | 2023-11-28 | 天津华宁电子有限公司 | Mining working face double-vehicle control system |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107817509A (en) * | 2017-09-07 | 2018-03-20 | 上海电力学院 | Crusing robot navigation system and method based on the RTK Big Dippeves and laser radar |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN1331641C (en) * | 2004-07-14 | 2007-08-15 | 华南理工大学 | Security ensuring and patrolling robot |
| TWI396830B (en) * | 2008-11-28 | 2013-05-21 | Univ Nat Taiwan | Patrol device and patrol path planning method for the same |
| CN102096413B (en) * | 2010-12-23 | 2012-05-30 | 中国民航大学 | Security patrol robot system and control method thereof |
| CN110370284A (en) * | 2019-08-07 | 2019-10-25 | 北京凌天世纪控股股份有限公司 | A kind of automatic control system of explosion-proof autonomous crusing robot |
| CN110716549A (en) * | 2019-11-04 | 2020-01-21 | 中国船舶重工集团公司第七一六研究所 | Autonomous navigation robot system for map-free area patrol and navigation method thereof |
-
2020
- 2020-03-20 CN CN202010205064.0A patent/CN111857121B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN107817509A (en) * | 2017-09-07 | 2018-03-20 | 上海电力学院 | Crusing robot navigation system and method based on the RTK Big Dippeves and laser radar |
Also Published As
| Publication number | Publication date |
|---|---|
| CN111857121A (en) | 2020-10-30 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111857121B (en) | Walking obstacle avoidance method and system for patrol robot based on inertial navigation and laser radar | |
| CN108628324B (en) | Unmanned vehicle navigation method, device, equipment and storage medium based on vector map | |
| GB2619417A (en) | Motion graph construction and lane level route planning | |
| Lee et al. | Development of a self-driving car that can handle the adverse weather | |
| CN111176298B (en) | Unmanned vehicle track recording and tracking method | |
| CN111006667B (en) | Automatic driving track generation system under high-speed scene | |
| US11015940B2 (en) | Systems and methods for longitudinal position correction of a vehicle using mapped landmarks | |
| CN110955236A (en) | Curvature correction path sampling system for autonomous vehicle | |
| WO2020135772A1 (en) | Generation method and generation system for dynamic target line during automatic driving of vehicle, and vehicle | |
| US20210114625A1 (en) | System and method for autonomous collision avoidance | |
| CN114442101B (en) | Vehicle navigation method, device, equipment and medium based on imaging millimeter wave radar | |
| Babak et al. | Control of autonomous ground vehicles: A brief technical review | |
| US20230384441A1 (en) | Estimating three-dimensional target heading using a single snapshot | |
| CN114200945B (en) | Safety control method of mobile robot | |
| US20230242149A1 (en) | Radar object classification based on radar cross-section data | |
| Valldorf et al. | Advanced Microsystems for Automotive Applications 2007 | |
| JP2024095712A (en) | Output device, control method, program, and storage medium | |
| US10845814B2 (en) | Host vehicle position confidence degree calculation device | |
| US20230415739A1 (en) | Systems and methods for controlling longitudinal acceleration based on lateral objects | |
| Cremean et al. | Model-based estimation of off-highway road geometry using single-axis ladar and inertial sensing | |
| US12055623B2 (en) | Estimating target heading using a single snapshot | |
| WO2017216856A1 (en) | Inter-vehicle distance estimation method and inter-vehicle distance estimation device | |
| US20230311931A1 (en) | Systems and methods for control state estimation of vehicles | |
| US20230415781A1 (en) | Systems and methods for controlling longitudinal acceleration based on lateral objects | |
| CN114661040B (en) | System, method and controller for graph-based path planning of a host vehicle |
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 |