CN113759904A - Mobile robot autonomous navigation method based on fusion algorithm - Google Patents

Mobile robot autonomous navigation method based on fusion algorithm Download PDF

Info

Publication number
CN113759904A
CN113759904A CN202110977169.2A CN202110977169A CN113759904A CN 113759904 A CN113759904 A CN 113759904A CN 202110977169 A CN202110977169 A CN 202110977169A CN 113759904 A CN113759904 A CN 113759904A
Authority
CN
China
Prior art keywords
mobile robot
algorithm
fusion algorithm
path planning
navigation
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.)
Pending
Application number
CN202110977169.2A
Other languages
Chinese (zh)
Inventor
李晓
施允堃
张志泽
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Guilin University of Electronic Technology
Original Assignee
Guilin University of Electronic Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Guilin University of Electronic Technology filed Critical Guilin University of Electronic Technology
Priority to CN202110977169.2A priority Critical patent/CN113759904A/en
Publication of CN113759904A publication Critical patent/CN113759904A/en
Pending legal-status Critical Current

Links

Images

Classifications

    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0212Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
    • G05D1/0221Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0257Control of position or course in two dimensions specially adapted to land vehicles using a radar
    • GPHYSICS
    • G05CONTROLLING; REGULATING
    • G05DSYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
    • G05D1/02Control of position or course in two dimensions
    • G05D1/021Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/0274Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Automation & Control Theory (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

The invention discloses a mobile robot autonomous navigation method based on a Gmapping-DWA fusion algorithm, which realizes three problems of autonomous navigation, namely positioning, map construction and path planning. Aiming at the problem of SLAM of a mobile robot in an unknown environment, an improved particle filter algorithm based on a Bayesian filter theory is provided for establishing a real-time grid map, and the established map has better global consistency. The invention provides a navigation control decision scheme combining global path planning and local dynamic obstacle avoidance, which optimizes a global path and a local dynamic path by respectively adopting an A-x algorithm and a DWA dynamic window method, and improves the overall path optimization efficiency. And finally, the simulation result shows that the proposed path planning algorithm is effective, the planned path is safe and smooth, and the experimental result also verifies that the proposed fusion algorithm autonomous navigation scheme has higher precision and good map consistency and also has stronger dynamic obstacle avoidance capability.

Description

Mobile robot autonomous navigation method based on fusion algorithm
Technical Field
The invention relates to the technical field of mobile robot positioning and autonomous navigation, in particular to a mobile robot autonomous navigation method based on a fusion algorithm.
Background
The problem of navigation of mobile robots has been one of the research hotspots, in particular autonomous navigation. There are three problems with autonomous navigation, namely Localization and Mapping (SLAM) and path planning. Aiming at the problem of SLAM of a mobile robot in an unknown environment, the invention provides an improved particle filter algorithm based on a Bayesian filter theory to establish a real-time grid map, and the constructed map has better global consistency. The invention provides a navigation control decision scheme combining global path planning and local Dynamic obstacle avoidance, which optimizes a global path and a local Dynamic path by respectively adopting an A-x algorithm and a Dynamic Window Approach (DWA), thereby improving the overall path optimization efficiency. In order to reduce system errors, a least square method is adopted to calibrate the odometer, meanwhile, a piecewise linear interpolation method is adopted to inhibit laser radar distortion, and processed data have a good error inhibition effect. Finally, simulation is carried out in a Gazebo environment under the ROS framework, and the result shows that the proposed path planning algorithm is effective, and the planned path is safe and smooth. The experimental result also verifies that the provided fusion algorithm autonomous navigation scheme has higher precision and good map consistency, and simultaneously has stronger dynamic obstacle avoidance capability.
Disclosure of Invention
The invention aims to provide a mobile robot autonomous navigation method based on a fusion algorithm, and aims to solve the technical problem that the autonomous navigation efficiency of a mobile robot in the prior art is poor.
In order to achieve the above object, the present invention provides a mobile robot autonomous navigation method based on fusion algorithm, comprising the following steps:
the mobile robot positions and constructs a picture in real time;
acquiring a navigation target, and performing dynamic path planning by adopting a fusion algorithm;
preprocessing data, inhibiting distortion of a laser radar and calibrating a milemeter;
and (5) system simulation and experimental verification.
In the positioning and mapping process of the mobile robot, under the navigation frame of ROS, a particle filter algorithm improved based on Bayesian filter is adopted for positioning and mapping.
In the process of obtaining a navigation target and planning a path, a navigation control decision scheme combining global path planning and local dynamic obstacle avoidance is adopted.
And performing the global path planning by using an A-x algorithm, and performing the local dynamic obstacle avoidance by using a DWA algorithm.
Wherein, the system simulation and experimental verification adopt a Gazebo simulation environment under the ROS framework.
In the process of system simulation and experimental verification, a piecewise linear interpolation method is used for distortion suppression processing of the laser radar, and a least square method is used for odometer calibration.
According to the autonomous navigation method of the mobile robot based on the fusion algorithm, on the basis of the Gmapping algorithm, the improved particle filter algorithm based on the Bayesian filter theory is used for positioning, then a map is constructed by adopting a grid-occupied map method, and the constructed map has good global consistency. And then, global path planning is carried out by using an A-x algorithm, and local dynamic obstacle avoidance is carried out by using a DWA algorithm, so that the overall optimization efficiency of the system is improved, and verification is carried out in a Gazebo simulation environment. In order to reduce system errors, an error accumulation method is adopted to restrain yaw angle drift, a least square method is adopted to calibrate the odometer, and a piecewise linear interpolation method is adopted to restrain laser radar distortion. The system simulation and experiment results show that the scheme is effective, autonomous navigation of the mobile robot is realized, high precision and good map consistency are achieved, and meanwhile the scheme has strong dynamic obstacle avoidance capability.
Drawings
In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings used in the description of the embodiments or the prior art will be briefly described below, it is obvious that the drawings in the following description are only some embodiments of the present invention, and for those skilled in the art, other drawings can be obtained according to the drawings without creative efforts.
Fig. 1 is a flow chart diagram of an autonomous navigation method of a mobile robot based on a fusion algorithm according to the invention.
Fig. 2 is a block diagram of the autonomous navigation system of the mobile robot according to the present invention.
Fig. 3 is a flow chart of a gmaping composition algorithm of the autonomous navigation method of the mobile robot based on the fusion algorithm.
Fig. 4 is a flowchart of the DWA algorithm of the present invention.
FIG. 5 is a block diagram of an experimental hardware configuration of an embodiment of the present invention.
FIG. 6 is a flowchart of a main experimental procedure according to an embodiment of the present invention.
Fig. 7 is a diagram of SLAM simulation results of the gmaping algorithm according to the embodiment of the present invention.
Fig. 8 is a simulation diagram of global path planning according to an embodiment of the present invention.
Fig. 9 is a simulation diagram of local path planning according to an embodiment of the present invention.
Fig. 10 is a map and global path planning effect diagram constructed based on an experimental environment according to an embodiment of the present invention.
FIG. 11 is a diagram illustrating the autonomous navigation effect based on the constructed map according to the embodiment of the present invention.
Fig. 12 is a schematic diagram of the dynamic obstacle avoidance effect based on the constructed map according to the embodiment of the present invention.
Detailed Description
Reference will now be made in detail to embodiments of the present invention, examples of which are illustrated in the accompanying drawings, wherein like or similar reference numerals refer to the same or similar elements or elements having the same or similar function throughout. The embodiments described below with reference to the drawings are illustrative and intended to be illustrative of the invention and are not to be construed as limiting the invention.
Referring to fig. 1, the present invention provides a method for autonomous navigation of a mobile robot based on a fusion algorithm, comprising the following steps:
s1: the mobile robot positions and constructs a picture in real time;
s2: acquiring a navigation target, and performing dynamic path planning by adopting a fusion algorithm;
s3: preprocessing data, inhibiting distortion of a laser radar and calibrating a milemeter;
s4: and (5) system simulation and experimental verification.
In the positioning and mapping process of the mobile robot, an improved particle filter algorithm based on the Bayesian filter theory is adopted for positioning and mapping under the navigation framework of the ROS.
In the process of obtaining the navigation target and planning the path, a fusion algorithm is adopted to plan the path, and the path planning comprises global path planning and local path planning.
And performing the global path planning by using an A-x algorithm, and performing the local dynamic obstacle avoidance by using a DWA algorithm.
Simulation experiments verify that a Gazebo environment under an ROS framework is adopted.
Before system simulation and experimental debugging and verification, distortion suppression processing is carried out on the laser radar by adopting a piecewise linear interpolation method, and odometer calibration is carried out by adopting a least square method.
The present invention will be further described below.
Step 1: the principle analysis is carried out on the SLAM problem based on the Bayesian filtering theory, a Gmapping algorithm based on improved particle filtering is selected as a mobile robot SLAM scheme through comparative research, and positioning and drawing are carried out under the navigation frame of ROS (please refer to a flow chart of the Gmapping algorithm in FIG. 2):
based on the Bayesian filtering theory, the mathematical expression of the SLAM problem is given as follows:
P(x1:t|u1:t,z1:t)=ξP(zt|xt)P(xt|xt-1)P(x1:t-1|u1:t-1,z1:t-1) (1)
wherein x is1:tIs the positioning of the mobile robot; u. of1:tRepresenting odometer data; z is a radical of1:tRepresenting each observed data obtained. Because the odometer data fluctuate greatly with each measurement, the observation model of the gmaping algorithm uses laser radar data as input:
P(xt|xt-1,ut)=ξP(zt|xt,m) (2)
wherein m is a map constructed by the mobile robot; ξ is the normalization factor. In order to solve the problem of reduced particle diversity, the Gmapping algorithm adopts a selective resampling method, and the sampling formula is as follows:
Figure BDA0003227942650000041
where i is the number of iterations, NeffIs the resampling threshold. When N is presenteffIf the particle variance is smaller than a certain threshold, the particle variance is too large, and resampling is performed.
SLAM mapping is performed by using an occupancy grid mapping method, and each grid is represented by a random variable:
Figure BDA0003227942650000042
wherein P (m ═ 1) indicates that the grid is in an idle (Non-Occupied) state, and at this time, indicates a Non-obstacle probability; p (m ═ 0) represents the probability of being in an Occupied (Occupied) state, i.e., the probability of having an obstacle; z represents each time a lidar observation is obtained.
Obtaining a construction formula of the grid map through recursion:
Figure BDA0003227942650000043
wherein m is+And m-Showing the grid state before and after observation.
Step 2: and determining a navigation target, planning a feasible path by applying an A-x algorithm, obtaining track information from a starting point to a destination, and planning a local dynamic motion strategy of the mobile robot by adopting a DWA algorithm. (FIG. 3 is the autonomous navigation frame diagram of the mobile robot)
Firstly, an A-algorithm is used in global path planning of the mobile robot, wherein a path evaluation formula is as follows:
F(n)=G(n)+H(n) (6)
where F (n) is a cost estimate from the initial node to the target node through the current search node, G (n) is an actual cost from the initial node to the current search node in the state space, and H (n) is an estimated cost from the current node to the target state, and hereinafter, their function values are referred to as F value, G value, and H value, respectively. The optimal path searching steps are as follows:
(1) initialization: the map is rasterized, and each grid has two states: idle and occupied.
(2) Pretreatment: two node sets openList and closeList are set, all searched nodes on a path are accommodated, and the single-step moving cost is calculated by adopting a Manhattan method.
(3) Searching: and starting to search adjacent child nodes by taking the starting node as a parent node, and calculating corresponding F value, G value and H value.
(4) And repeating the steps for continuously searching, checking and updating, and ending the search when the target node appears in the closed List.
Secondly, obtaining a local target pose value from the global path planning, and carrying out the path planning of the local real-time dynamic obstacle avoidance of the mobile robot by adopting a DWA algorithm, wherein an evaluation function is as follows (fig. 4 is a DWA algorithm flow chart):
G(v,w)=σ(α·heading(v,w)+β·dist(v,w)+γvelocity(v,w)) (7)
wherein α, β, γ represent weight coefficients, respectively. The orientation estimation function heading (v, w) is used for estimating the angle difference between the target and the predicted position; the distance between the robot and the nearest barrier on the current track is dist (v, w); velocity (v, w) indicates the velocity of the current trajectory. The path calculation formula is as follows:
Figure BDA0003227942650000051
wherein, [ x ]k,ykk]TRepresenting the pose at time k, epsilon is random noise. The local optimal path searching steps are as follows:
(1) initialization: and acquiring the position and attitude data of the laser radar and the robot.
(2) Patterning: and creating a global cost map and a local cost map, and screening to obtain a local target pose.
(3) Evaluation: and predicting a track space, and performing collision detection to obtain an optimal path.
(4) And repeating the operations until the path planning is completed.
And step 3: preprocessing data before experiment and debugging the system.
Referring to fig. 5 to 12, the present invention provides an embodiment:
first, the suppression of yaw angle drift of the MPU 6050. Since the MPU6050 is only a 3-axis accelerometer plus a 3-axis gyroscope sensor and does not have a magnetometer for data fusion compensation, drift of the yaw angle is inevitably generated, and if the drift is not suppressed, the accumulation of the drift will cause a large error. And (3) carrying out drift suppression by adopting an error accumulation method:
Figure BDA0003227942650000061
wherein e isyIs an error accumulation variable; e.g. of the typevIs the data mean; t is the total sampling time; t is tsIs the sampling interval.
And then the odometer calibration is carried out by using a least square method. If the pose of the mobile robot obtained from the laser radar odometer is
Figure BDA0003227942650000062
The pose of the mobile robot obtained from the wheel type odometer is A11=A22=A33=(uix,uiy,u) Then, there are:
Figure BDA0003227942650000063
obtaining a transformation matrix by a least squares method
Figure BDA0003227942650000064
Figure BDA0003227942650000065
Wherein the content of the first and second substances,
Figure BDA0003227942650000066
and finally, inhibiting the motion distortion of the laser radar by adopting a piecewise linear interpolation method. The odometer pose of each laser beam is taken as an approximate starting point, i.e. the odometer pose, and all laser beam data is transmitted from the map coordinate system to the odometer coordinate system.
Turning to fig. 5, fig. 5 is a block diagram of the overall hardware result of an example implementation. Fig. 6 is a system overall software flow chart.
Referring to fig. 7, fig. 7 is a diagram of SLAM simulation results of the gmaping algorithm, the left side is a top view of the simulation environment, and the right side is a global map constructed by using the gmaping algorithm.
Fig. 8 is a global path planning simulation diagram, where the left side of the first row is the case when the deviation of the given initial pose is too large, the right side is the pose estimation divergence after navigating for a certain distance, the left side of the second row is the case when the initial pose is given reasonably, and the right side is the odometer and path diagram for successful navigation.
Fig. 9 is a simulation diagram of local path planning, the diagram of adding obstacles on the left side in real time illustrates the diagram of successfully avoiding obstacles on the right side.
Fig. 10 shows a map constructed by the mobile robot based on the experimental environment on the left side and a global path planning effect map on the right side.
Fig. 11 is a schematic diagram of an autonomous navigation effect based on a constructed map, where the left side of the first row is a picture when a navigation function is started, the right side of the first row is a picture when a pose is adjusted to be ready to bypass a corner, the left side of the second row is a situation when an obstacle is avoided, and the right side of the second row is a situation when a target point is successfully reached.
Fig. 12 is a diagram of dynamic obstacle avoidance effect of the mobile robot based on a constructed map.
While the invention has been described with reference to a preferred embodiment, it will be understood by those skilled in the art that various changes in form and detail may be made therein without departing from the spirit and scope of the invention as defined by the appended claims.

Claims (6)

1. A mobile robot autonomous navigation method based on a fusion algorithm is characterized by comprising the following steps:
the mobile robot positions and constructs a picture in real time;
acquiring a navigation target, and performing dynamic path planning by adopting a fusion algorithm;
preprocessing data, inhibiting distortion of a laser radar and calibrating a milemeter;
and (5) system simulation and experimental verification.
2. The fusion algorithm-based mobile robot autonomous navigation method of claim 1, characterized in that in the mobile robot positioning and mapping process, under the navigation framework of ROS, an improved particle filter algorithm is adopted for positioning and mapping.
3. The autonomous mobile robot navigation method based on fusion algorithm as claimed in claim 1, wherein a navigation control decision scheme combining global path planning and local dynamic obstacle avoidance is adopted in the process of obtaining the navigation target and performing path planning.
4. The fusion algorithm-based autonomous mobile robot navigation method of claim 3, wherein the global path planning is performed using the a-x algorithm and the local dynamic obstacle avoidance is performed using the DWA algorithm.
5. The fusion algorithm-based mobile robot autonomous navigation method of claim 1, wherein system simulation and experimental verification adopt Gazebo simulation environment under ROS architecture.
6. The autonomous navigation method of mobile robot based on fusion algorithm as claimed in claim 5, wherein during the process of system simulation and experimental debugging, distortion suppression processing is performed on the laser radar by using a piecewise linear interpolation method, and odometer calibration is performed by using a least square method.
CN202110977169.2A 2021-08-24 2021-08-24 Mobile robot autonomous navigation method based on fusion algorithm Pending CN113759904A (en)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110977169.2A CN113759904A (en) 2021-08-24 2021-08-24 Mobile robot autonomous navigation method based on fusion algorithm

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110977169.2A CN113759904A (en) 2021-08-24 2021-08-24 Mobile robot autonomous navigation method based on fusion algorithm

Publications (1)

Publication Number Publication Date
CN113759904A true CN113759904A (en) 2021-12-07

Family

ID=78791024

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110977169.2A Pending CN113759904A (en) 2021-08-24 2021-08-24 Mobile robot autonomous navigation method based on fusion algorithm

Country Status (1)

Country Link
CN (1) CN113759904A (en)

Cited By (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114089774A (en) * 2022-01-14 2022-02-25 中国科学院微电子研究所 AGV path planning method and device in storage environment

Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111486855A (en) * 2020-04-28 2020-08-04 武汉科技大学 Indoor two-dimensional semantic grid map construction method with object navigation points

Patent Citations (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111486855A (en) * 2020-04-28 2020-08-04 武汉科技大学 Indoor two-dimensional semantic grid map construction method with object navigation points

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
刘琴;明振: "基于激光雷达的安保机器人自主定位与导航算法研究", 《中国安全防范技术与应用》, 30 June 2018 (2018-06-30) *
吴正;黄辉;邹安安: "Turtlebot2机器人环境地图构建与多目标点自主导航的研究", 《电脑与信息技术》, 28 February 2019 (2019-02-28) *
宋书亮: "室内移动机器人SLAM算法设计", 《中国优秀硕士学位论文全文数据库 信息科技辑》, pages 1 - 50 *
常灯祥: "基于ROS的无人驾驶汽车自主导航研究", 《中国优秀硕士学位论文全文数据库 工程科技Ⅱ辑》, pages 1 - 59 *

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114089774A (en) * 2022-01-14 2022-02-25 中国科学院微电子研究所 AGV path planning method and device in storage environment
CN114089774B (en) * 2022-01-14 2022-04-12 中国科学院微电子研究所 AGV path planning method and device in storage environment

Similar Documents

Publication Publication Date Title
EP3470787B1 (en) Multi-sensor fusion for robust autonomous flight in indoor and outdoor environments with a rotorcraft micro-aerial vehicle (mav)
CN112762957B (en) Multi-sensor fusion-based environment modeling and path planning method
CN111650929B (en) Self-adaptive sliding mode control method and system and mobile robot controller
Yin et al. Robust derivative unscented Kalman filter under non-Gaussian noise
CN115993825A (en) Unmanned vehicle cluster control system based on air-ground cooperation
CN111578926A (en) Map generation and navigation obstacle avoidance method based on automatic driving platform
CN111025229B (en) Underwater robot pure orientation target estimation method
CN113759904A (en) Mobile robot autonomous navigation method based on fusion algorithm
CN114608585A (en) Method and device for synchronous positioning and mapping of mobile robot
CN114326810A (en) Obstacle avoidance method of unmanned aerial vehicle in complex dynamic environment
CN116774247A (en) SLAM front-end strategy based on multi-source information fusion of EKF
CN115808933A (en) Unmanned aerial vehicle path planning method based on quantum seagull algorithm
CN114815899A (en) Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor
CN112904855B (en) Follow-up robot local path planning method based on improved dynamic window
Housein et al. Extended Kalman filter sensor fusion in practice for mobile robot localization
Yang et al. SLAM self-cruise vehicle based on ROS platform
Wang et al. Dynamic path planning algorithm for autonomous vehicles in cluttered environments
Bacha et al. A new robust cooperative-reactive Filter for vehicle localization: The Extended Kalman Particle Swarm ‘EKPS’
Kuncara et al. Enhancing accuracy in field mobile robot state estimation with GNSS and encoders
Pathiranage et al. T–S fuzzy model adopted SLAM algorithm with linear programming based data association for mobile robots
Hadji et al. A review: Simultaneous localization and mapping algorithms
Yang et al. SLAM Based on Double Layer Unscented Kalman Filter
Indelman et al. Real-time mosaic-aided aerial navigation: II. Sensor fusion
Yang et al. SLAM Based on Double Layer Cubature Kalman Filter
CN113961016B (en) Unmanned aerial vehicle dynamic target track planning method and system based on A-algorithm

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