Walking obstacle avoidance method and system for patrol robot based on inertial navigation and laser radar
Through the combination of inertial navigation and 16-line lidar, a digital map is established and the driving distance is calculated, which solves the problems of low sampling frequency of GPS module and the short distance and high cost of traditional obstacle avoidance methods, and achieves the high patrol robot in complex environments Accurate navigation and obstacle avoidance.
Patent Information
- Application Number
- CN202010205064.0
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2020-03-20
- Publication Date
- 2024-11-01
- Estimated Expiration
- Not applicable · inactive patent
AI Technical Summary
In the prior art, the low sampling frequency or poor signal of the GPS module leads to large error in driving distance calculation. Traditional obstacle avoidance methods such as infrared, ultrasound, and binocular visual navigation have problems such as short detection distance, high cost and poor flexibility.
Inertial navigation information combined with 16-line lidar is used to create a digital map by learning and recording patrol route trajectory, and use inertial navigation and lidar scanning data to calculate the driving distance and obstacle avoidance paths to achieve accurate navigation and obstacle avoidance.
It greatly reduces the error between the actual attitude of the patrol robot and the predetermined route, improves the accuracy and flexibility of obstacle avoidance, and can accurately return to the original route in complex environments, and is suitable for a wide range of navigation scenarios.
Smart Images

Figure CN111857121B_ABST
Abstract
Description
Technical Field
[0001] The invention relates to the field of navigation, and in particular to a walking obstacle avoidance method and system for a patrol robot based on inertial navigation and laser radar. Background Art
[0002] At present, there are two main methods for calculating the travel distance of a car through the GPS module in the smart car hardware:
[0003] One method is mainly based on the GPS module's longitude and latitude coordinate points, and is calculated according to the arc length formula between two points on the earth. The disadvantages of this method are: ① When the GPS module sampling frequency is low, the calculated distance error is large; ② When the GPS signal is poor, such as passing through tunnels, high-rise buildings, and tree-lined environments, the GPS coordinate points drift or are lost. At this time, the driving distance calculated according to the arc length formula has a large deviation from the actual driving distance;
[0004] Another method is to calculate the driving distance approximately by GPS speed. The disadvantages of this method are: ① When the GPS sampling frequency is low, the speed changes between adjacent sampling points are irregular, resulting in inaccurate driving distance calculated based on GPS speed; ② GPS speed itself will produce abnormal values, and the driving distance calculated based on abnormal GPS speed has a large deviation;
[0005] Traditional obstacle avoidance methods, such as infrared obstacle avoidance, ultrasonic obstacle avoidance, and binocular vision navigation systems, all have shortcomings and drawbacks. The effect of infrared obstacle avoidance is proportional to the number of installed infrared sensors, which has certain limitations on cost control and appearance design. In addition, the infrared detection distance is limited, and it is not possible to detect and avoid distant obstacles in time; ultrasonic obstacle avoidance cannot detect the size and shape of obstacles, and can only detect obstacles when they are close, so it may not be possible to avoid them in time; binocular vision navigation also has the problem of short detection distance, and will be affected at night or in weak light. Summary of the invention
[0006] The technical problem to be solved by the present invention is to provide an accurate navigation method with a wide range of application scenarios that accurately calculates the travel distance of a patrol robot by using inertial navigation information and uses a laser radar to assist in obstacle avoidance in response to the above-mentioned defects in the prior art.
[0007] According to the present invention, a patrol robot walking obstacle avoidance method based on inertial navigation and laser radar is provided, comprising:
[0008] The first step: for the patrol robot’s starting point and destination, use the learning method to record the set patrol route trajectory and establish a digital map for subsequent patrols;
[0009] The second step: using the established map to navigate the patrol robot along the set patrol route.
[0010] Preferably, in the second step, when the patrol robot encounters an obstacle on the way, the laser radar scans to obtain point cloud data of the obstacle in front of the patrol robot, and the data is clustered to obtain the front obstacle cluster; the robot's turning angle and whether to stop are determined according to the distance of the obstacle and the gap between the obstacles.
[0011] Preferably, in the second step, when the patrol robot encounters an obstacle on the way, 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 the laser radar scanning, bypasses the obstacle, and then uses the angle correction algorithm to control the motor to return to the original route.
[0012] Preferably, in the second step, 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 the laser radar scanning to calculate the deviation between the current position of the patrol car and the nearest point on the digital route, and then calculates the control amount for returning to the digital route. If the patrol car is on the digital route, the control amount for walking along the digital route is calculated.
[0013] Preferably, in the second step, after the patrol robot starts walking, the inertial navigation unit of the patrol robot outputs mileage, speed, and heading angle information at fixed intervals. When the position coordinates are detected, the map coordinates closest to the position coordinates and the subsequent predetermined map coordinates of the map coordinates are found, a straight line from the current point to the map point is fitted, the angle between the straight line and the north direction is calculated, and this angle is input to the motor for direction control.
[0014] Preferably, in the second step:
[0015] Calculate the distance d between the center point O' of the obstacle cluster and the coordinate origin O, and the angle φ between it and the x-axis. represents the obstacle clustering, and calculates the maximum angle Δα between the obstacle clustering closure and the straight line OO'; calculates where k sf >1, W is the width of the unmanned vehicle, D sf is the lateral safety distance; calculate D sr =-k sr ·v 2 / 2a, where k sr >1,D sr is the braking safety distance;
[0016] Calculate the repulsive force generated by d(φ) at any angle θ:
[0017]
[0018]
[0019] or
[0020] A or C is the cluster boundary point:
[0021]
[0022] Define the total force K in the direction of angle θ RF (θ) is the maximum repulsive force generated by each cluster, where D m is the set maximum evaluation distance;
[0023] Set the local target point A(d) reached by the patrol robot, θ obj is the azimuth of the target point, d obj is the distance between the target point and the origin; the gravitational force generated by the target point at angle θ is:
[0024] K CF (θ) = cos(θ - θ obj ),
[0025] Set the cost function for the patrol robot to pass at angle θ:
[0026] K P (θ) = K CF (θ) / K RF (θ),
[0027] choose The angle θ corresponding to the maximum Cur = argmax(K PC ) as the current steering angle of the unmanned vehicle.
[0028] Preferably, in K PC =0, the patrol robot stops.
[0029] Preferably, the laser radar is a 16-line laser radar.
[0030] The present invention utilizes the initial inertial navigation information, with the help of the acceleration and posture change of the inertial measurement unit of the patrol robot as a carrier, and estimates the travel distance of the patrol robot through time-integrated measurement values as a preset route and uses a laser radar such as a 16-line laser radar to achieve a combined navigation method for road obstacle avoidance. The combined navigation combines two or more different navigation devices in an appropriate manner, utilizes the complementarity of their performance, and obtains a navigation system with higher navigation performance than using any system alone. BRIEF DESCRIPTION OF THE DRAWINGS
[0031] A more complete understanding of the present invention and its attendant advantages and features will be more readily appreciated by reference to the following detailed description taken in conjunction with the accompanying drawings, in which:
[0032] Figure 1 The flowchart of the walking obstacle avoidance method of a patrol robot based on inertial navigation and laser radar according to a preferred embodiment of the present invention is schematically shown.
[0033] Figure 2 A schematic diagram of map navigation of a patrol robot walking obstacle avoidance method based on inertial navigation and laser radar according to a preferred embodiment of the present invention is schematically shown.
[0034] Figure 3 A motor control schematic diagram of a patrol robot walking obstacle avoidance method based on inertial navigation and laser radar according to a preferred embodiment of the present invention is schematically shown.
[0035] Figure 4 The obstacle avoidance algorithm of the patrol robot walking obstacle avoidance method based on inertial navigation and laser radar according to the preferred embodiment of the present invention is schematically shown.
[0036] Figure 5 A schematic diagram of an obstacle avoidance area of a patrol robot walking obstacle avoidance method based on inertial navigation and laser radar according to a preferred embodiment of the present invention is schematically shown.
[0037] Figure 6 An example of a patrol route of a patrol robot walking obstacle avoidance system based on inertial navigation and laser radar according to a preferred embodiment of the present invention is schematically shown.
[0038] Figure 7 The system block diagram of the patrol robot walking obstacle avoidance system based on inertial navigation and laser radar according to the preferred embodiment of the present invention is schematically shown.
[0039] It should be noted that the drawings are used to illustrate the present invention, rather than to limit the present invention. Note that the drawings showing the structures may not be drawn to scale. In addition, in the drawings, the same or similar elements are marked with the same or similar reference numerals. DETAILED DESCRIPTION
[0040] In order to make the contents of the present invention clearer and easier to understand, the contents of the present invention are described in detail below in conjunction with specific embodiments and drawings.
[0041] Figure 1 The flowchart of 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 is schematically shown. Preferably, the laser radar used is a 16-line laser radar.
[0042] like Figure 2As shown, the patrol robot walking obstacle avoidance method based on inertial navigation and laser radar according to the preferred embodiment of the present invention includes:
[0043] The first step S1: for the starting point and destination of the patrol robot, the set patrol route trajectory is recorded by using a learning method to establish a digital map for subsequent patrols;
[0044] Specifically, for example, Figure 2 As shown, in the first step, the directed distance is recorded and map coordinates Indicates the directed distance of the current position relative to the starting point in the true north direction. Indicates the directed distance of the current position relative to the starting point in the east direction, sets M(0) = [0, 0], and records the latitude and longitude coordinates of the starting point: M0.
[0045] Since GPS has large position deviations due to factors such as occlusion, as well as the cumulative error of the inertial navigation system, the relative position can be inferred by combining GPS and inertial navigation with the Kalman equation to reduce the error.
[0046] Smoothing the map (segmented smoothing): Segmented smoothing further reduces map collection errors caused by inaccurate longitude and latitude.
[0047] The second step S2: using the established map to navigate the patrol robot along the set patrol route trajectory.
[0048] In the second step, when the patrol robot encounters an obstacle on the way, the laser radar scans to obtain the point cloud data of the obstacle in front of the patrol robot, and the data is clustered to obtain the obstacle cluster in front; the robot's turning angle is determined according to the distance of the obstacle and the gap between the obstacles.
[0049] In the second step, when the patrol robot encounters an obstacle on the way, 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 the laser radar scanning, bypasses the obstacle, and then uses the angle correction algorithm to control the motor to return to the original route.
[0050] For example, in the second step, 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 the laser radar scanning to calculate the deviation between the current position of the patrol car and the nearest point on the digital route, and then calculates the control amount for returning to the digital route. If the patrol car is on the digital route, the control amount for walking along the digital route is calculated.
[0051] After the patrol robot starts walking, the patrol robot's inertial navigation unit outputs mileage, speed, and heading angle information at fixed intervals. At a certain time T, the position coordinates are detected. Finding and location coordinates The nearest map coordinate, as well as the map coordinates following this map coordinate (k+1, k+2, ..... k+M), fit a straight line from the current point to the map point (k=0, ...M), and calculate the angle θ between the straight line and the true north direction s , this angle θ s Input to the motor for direction control, such as Figure 3 shown.
[0052] like Figure 4 As shown, the point cloud data of the obstacles in front of the patrol robot are obtained by laser radar scanning, and the data are clustered to obtain the front obstacle clusters; the robot's turning angle and whether to stop are determined according to the distance of the obstacles and the gaps between the obstacles.
[0053] Calculation of the repulsive field generated by obstacle clustering:
[0054] Calculate the distance d between the center point O' of the obstacle cluster and the coordinate origin (the current position of the patrol robot) O, and the angle φ between it and the x-axis (the tangent of the current position of the patrol robot at the patrol route trajectory), and use represents the obstacle clustering, and calculates the maximum angle Δα between the obstacle clustering closure and the straight line OO'; calculates where 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 the lateral safety distance; calculate D sr =-k sr ·v 2 / 2a, where k sr >1,D sr is the braking distance from the current speed v, and is the braking safety distance.
[0055] The repulsive force generated by d(φ) at any angle θ is defined as follows:
[0056]
[0057]
[0058] or
[0059] A or C is the cluster boundary point:
[0060]
[0061] At a certain angle θ, each obstacle cluster generates a repulsive force, and the total force K in the direction of angle θ is defined. RF (θ) is the maximum repulsive force generated by each cluster.
[0062] Here D m The maximum evaluation distance is set, exceeding D m The obstacles are moved with minimum repulsion.
[0063] Repulsion is affected by two factors:
[0064] Obstacle distance: The closer the obstacle is to the origin O, the greater the repulsive force; at the same time, the larger the impact angle range;
[0065] Obstacle clustering scale: The larger the scale, the larger the repulsive influence angle, that is, the larger Δα.
[0066] For example, considering that the speed of the unmanned vehicle is not fast, the simplified braking safety distance D sf =0.8m, regardless of driving speed; maximum evaluation distance D m =10m.
[0067] The gravitational field generated by the target point:
[0068] Set the local target point A(d) reached by the patrol robot, θ obj is the azimuth of the target point, d obj is the distance between the target point and the origin; the gravitational force generated by the target point at angle θ is:
[0069] K CF (θ) = cos(θ - θ obj ), when θ approaches θ obj When θ is far away from θ obj Time K CF (θ) decreases rapidly.
[0070] The cost function for the driverless car to pass at a certain angle:
[0071] K P (θ) = K CF (θ) / K RF (θ),
[0072] choose The angle θ corresponding to the maximum Cur = argmax(K PC ) as the current steering angle of the unmanned vehicle.
[0073] Autonomous vehicle motion decision-making:
[0074] K PC =0, stop;
[0075] K PC >0, select θ Cur = argmax(K PC ) as the current steering angle of the unmanned vehicle;
[0076] speed:
[0077] Here K PCmax =D m -D sr , K PCmax Corresponding to the situation when gravity is maximum and repulsion is minimum.
[0078] Unmanned vehicle obstacle avoidance area division, such as Figure 5 As shown:
[0079] In the parking area, the unmanned vehicle stops, state = 3, which is the stop state;
[0080] In the turning area, the unmanned vehicle determines the turning direction and speed according to the potential field method, and the state = 2, which is the obstacle avoidance state;
[0081] Outside the turning area, the unmanned vehicle determines the turning direction and speed according to the potential field method, and the state = 1, which is the normal driving state.
[0082] Figure 6 An example of a patrol route of a patrol robot walking obstacle avoidance system based on inertial navigation and laser radar according to a preferred embodiment of the present invention is schematically shown.
[0083] 1. Starting point:
[0084] 1) The patrol car stops at the starting point;
[0085] 2) The lateral deviation is less than 0.2m and the longitudinal deviation is less than 0.5m;
[0086] 3) Arbitrary azimuth angle;
[0087] 2. Straight-line driving:
[0088] 1) Azimuth guidance;
[0089] 2) Calculate the eastward and northward displacements based on the velocity;
[0090] 3) Calculate the lateral displacement and correct the lateral displacement;
[0091] 4) Calculate the width and length of the unobstructed road ahead and provide obstacle avoidance angle data (no obstacle, value 0);
[0092] 3. Encountering obstacles:
[0093] 1) According to the location of the obstacle, determine and give the steering control data, and implement the steering forward;
[0094] 2) Calculate the eastward and northward displacements based on the velocity;
[0095] 3) Calculate the lateral displacement and correct the lateral displacement;
[0096] 4) Calculate the width and length of the unobstructed road ahead and provide obstacle avoidance angle data;
[0097] 4. Go around obstacles:
[0098] 1) According to the location of the obstacle, determine and give the steering control data, and implement the steering forward;
[0099] 2) Calculate the eastward and northward displacements based on the velocity;
[0100] 3) Calculate the width and length of the forward unobstructed road and provide obstacle avoidance angle data;
[0101] 5. Overcoming obstacles:
[0102] 1) Determine that the obstacle has passed according to its location, calculate the steering control data according to the nearest point data of the digital route, and implement the steering forward;
[0103] 2) Calculate the eastward and northward displacements based on the velocity;
[0104] 3) Adjust the forward speed according to the east and north displacement deviations until returning to the digital route;
[0105] 4) Calculate the width and length of the unobstructed road ahead and provide obstacle avoidance angle data;
[0106] 6. Move along the preset route:
[0107] 1) Calculate the east and north displacements based on the speed. When the displacements have returned to the digital route, start azimuth navigation to move forward;
[0108] 2) Calculate the eastward and northward displacements based on the velocity;
[0109] 3) Calculate the lateral displacement and correct the lateral displacement;
[0110] 4) Calculate the width and length of the unobstructed road ahead and provide obstacle avoidance angle data;
[0111] 7. Correction at turns:
[0112] 1) When the linear displacement reaches -2m at the corner, start the laser radar to correct the turning point;
[0113] 2) Calculate the eastward and northward displacements based on the velocity;
[0114] 3) Calculate the lateral displacement and correct the lateral displacement;
[0115] 4) Calculate the width and length of the unobstructed road ahead and provide obstacle avoidance angle data;
[0116] 8. Turn forward:
[0117] 1) When the laser radar corrects the turning point, the next angle guidance is started and the steering is implemented;
[0118] 2) Calculate the eastward and northward displacements of the new section based on the velocity;
[0119] 3) Calculate the lateral displacement and correct the lateral displacement;
[0120] 4) Calculate the width and length of the unobstructed road ahead and provide obstacle avoidance angle data;
[0121] 9. End stop:
[0122] 1) When the angle guidance is normal;
[0123] 2) The eastward and northward displacements calculated based on the velocity have returned to the starting point;
[0124] 3) The width and length of the forward clear road are appropriate;
[0125] 4) Stop;
[0126] 10. Restart:
[0127] 1) When multiple patrols have not been completed;
[0128] 2) Start the angle guidance and adjust it to the patrol start state;
[0129] 3) Angle-guided advancement;
[0130] 4) Calculate the eastward and northward displacements of the new section based on the velocity;
[0131] 5) Calculate the lateral displacement and correct the lateral displacement;
[0132] 6) Calculate the width and length of the forward unobstructed road and provide obstacle avoidance angle data;
[0133] Figure 7 The system block diagram of the patrol robot walking obstacle avoidance system based on inertial navigation and laser radar according to the preferred embodiment of the present invention is schematically shown.
[0134] like Figure 7 As shown, the patrol robot walking obstacle avoidance system based on inertial navigation and laser radar according to a preferred embodiment of the present invention includes: a laser radar 1, an industrial computer 2, a satellite navigation unit 3 and a control unit 4. This system can be used to execute the above-mentioned patrol robot walking obstacle avoidance method.
[0135] Compared with the prior art, the present invention solves the problem of inaccurate speed of the motor-driven crawler tracks of the patrol robot during its movement, greatly reduces the error between the actual posture of the patrol robot and the predetermined route, and assists in the calculation of the north-south displacement; in terms of obstacle avoidance, the patrol robot can accurately return to the original set route after avoiding obstacles, and in actual use, fine-tuning the control parameters can realize patrolling of complex routes, which has high flexibility and practicality in application.
[0136] It should be noted that, unless otherwise specified, the terms "first", "second", "third", etc. in the specification are only used to distinguish the various components, elements, steps, etc. in the specification, and are not used to indicate the logical relationship or sequential relationship between the various components, elements, steps, etc.
[0137] It is to be understood that, although the present invention has been disclosed as a preferred embodiment, the above embodiment is not intended to limit the present invention. For any person skilled in the art, without departing from the scope of the technical solution of the present invention, the technical content disclosed above can be used to make many possible changes and modifications to the technical solution of the present invention, or modified into equivalent embodiments of equivalent changes. Therefore, any simple modification, equivalent change and modification made to the above embodiments according to the technical essence of the present invention without departing from the content of the technical solution of the present invention still falls within the scope of protection of the technical solution of the present invention.
Claims
1. A patrol robot walking obstacle avoidance method based on inertial navigation and laser radar, characterized in that include: The first step: for the patrol robot’s starting point and destination, use the learning method to record the set patrol route trajectory and establish a digital map for subsequent patrols; The second step: using the established map to navigate the patrol robot along the set patrol route trajectory; In the second step, when the patrol robot encounters an obstacle on the way, the laser radar scans to obtain the point cloud data of the obstacle in front of the patrol robot, and clusters the data to obtain the cluster of obstacles in front; the robot's turning angle and whether to stop are determined according to the distance of the obstacle and the gap between the obstacles; And, in the second step execute: Calculate the distance d between the center point O' of the obstacle cluster and the coordinate origin O, and the angle between the line between the center point O' and the coordinate origin O and the x-axis The coordinate origin O is the current position of the patrol robot, and the x-axis is the tangent line of the patrol robot's current position on the patrol route trajectory. Represents obstacle clustering, and calculates the maximum angle Δα between the obstacle cluster closure and the straight line OO'; calculate where k sf >1, W is the patrol robot width, D sf is the lateral safety distance; calculate D sr =-k sr ·v 2 / 2a, where k sr >1,D sr is the braking safety distance, v is the current speed; calculate The repulsive force generated at any angle θ is: or Where D m is the set maximum evaluation distance; A or C is the cluster boundary point: Define the total force K in the direction of angle θ RF (θ) is the maximum repulsive force generated by each cluster; Set the local target point A(d) reached by the patrol robot, θ obj is the azimuth of the target point, d obj is the distance between the target point and the origin; the gravitational force generated by the target point at angle θ is: K CF (θ)=cos(θ-θ obj ), Set the cost function for the patrol robot to pass at angle θ: K P (θ)=K CF (θ) / K RF (i), choose The angle θ corresponding to the maximum Cur = argmax(K PC ) as the current steering angle of the patrol robot.
2. The patrol robot walking obstacle avoidance method according to claim 1, characterized in that: In the second step, when the patrol robot encounters an obstacle on the way, 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 the laser radar scan to bypass the obstacle and then use the angle correction algorithm to control the motor to return to the original route.
3. The patrol robot walking obstacle avoidance method according to claim 1, characterized in that: In the second step, 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 obstacles obtained by the laser radar scanning to calculate the deviation between the current position of the patrol car and the nearest point on the digital route, and then calculates the control amount for returning to the digital route. If the patrol car is on the digital route, the control amount for walking along the digital route is calculated.
4. The patrol robot walking obstacle avoidance method according to claim 1, characterized in that: In the second step, after the patrol robot starts walking, the inertial navigation unit of the patrol robot outputs mileage, speed, and heading angle information at fixed intervals. When the position coordinates are detected, the map coordinates closest to the position coordinates and the subsequent predetermined map coordinates of the map coordinates are found, and a straight line from the current point to the map point is fitted, and the angle between the straight line and the north direction is calculated. This angle is input to the motor for direction control.
5. The patrol robot walking obstacle avoidance method according to claim 1, characterized in that: In K PC =0, the patrol robot stops.
6. The patrol robot walking obstacle avoidance method according to claim 1 or 2, characterized in that: The laser radar is a 16-line laser radar.
