CN103914068A - Service robot autonomous navigation method based on raster maps - Google Patents

Abstract

The invention belongs to the technical field of robot navigation and relates to a service robot autonomous navigation method based on raster maps. According to the method, the information of the environment where a robot is located is acquired in real time through 2D laser radar, environment feature extraction is conducted, raster map establishment is conducted according to the acquired information by means of the SLAM technique, autonomous positioning is conducted on the robot in real time at the same time, path planning is conducted on the robot according to navigation task requirements on this basis, and then tracking control is conducted according to the planned path to enable the robot to accomplish the navigation task. Compared with the prior art, the method has the advantages that three key techniques in navigation are improved so that the method can meet actual requirements of service robot navigation better, and then the optimal feasible scheme is obtained; environment information is processed in real time to generate dynamic maps, and then autonomous navigation is achieved effectively, and navigation accuracy and efficiency are improved greatly.

Description

A kind of service robot autonomous navigation method based on grating map

Technical field

The invention belongs to robot and field of navigation technology, relate to a kind of service robot autonomous navigation method based on grating map.

Background technology

Autonomous mobile robot is an important branch in robot research field, and it is widely applied in many fields such as military, civilian, scientific researches.In the last few years, along with the development of computing machine, sensor and network technology, the emphasis of people's research also stationary machine arm, the mechanical arm from structural formula environment turned to the autonomous mobile robot in non-structure circumstances not known.Traditional mechanical arm space environment modeling and the method for teaching campaign cannot meet the new task that autonomous mobile robot is faced.Autonomous navigation technology is the core that mobile robot studies, and is the gordian technique that realizes autonomous, mainly comprises autonomous location, path planning, tracking control etc.At present, for the existing a lot of research of autonomous mobile robot navigation problem, but existing technology is still perfect not.Autonomous location is the basic link of Mobile Robotics Navigation, mobile robot will complete navigation task, just need to know in real time self pose with respect to external environment, conventional autonomous location technology mainly comprises relative positioning technology, absolute fix technology and combined orientation technology.

Relative positioning is also referred to as reckoning, mainly comprise telemetry and inertial navigation method, its advantage is not rely on external environmental information, can provide independence and complete navigation information completely, but the error of the sensors such as odometer, gyroscope can attract cumulative errors.Absolute fix is to utilize outside reference system, realizes location by the absolute position of measuring mobile robot, mainly comprises network positions, road sign location, map match location, its advantage is that positioning precision is higher, there are not cumulative errors, but technical sophistication, and cost is higher.Integrated positioning is that relative positioning is combined and positioned with absolute technology, and common way is to utilize relative positioning to carry out pose estimation, utilizes absolute fix to proofread and correct positioning result.

Path planning is the basis of mobile robot tracking control, is one of most important task in Mobile Robotics Navigation.Mobile robot path planning mainly can be divided into template matches path planning, path planning based on environmental model and the path planning three types based on behavior.Template matches path planning is that robot current state is compared with the example in past template base, finds out an Optimum Matching example, revises the path in this example, thereby obtains a new path.Path planning based on environmental model is the most ripe at present method, can be divided into the known global path planning of environmental information and environmental information the unknown or the unknown local paths planning of part completely according to the integrated degree of Information.Method based on behavior is to be proposed in his containment type structure by BROOKS, and using it for and solving mobile robot path planning problem is a kind of new development trend.

According to the difference of controlling target, mobile robot's tracking control problem comprises a Stabilization, track following problem, path trace problem.Point Stabilization refers to CONTROLLER DESIGN, makes mobile robot arrive and be stabilized in final state arbitrarily from original state arbitrarily, its objective is a Feedback Control Laws of acquisition, and it is progressive stable making an equilibrium point of mobile robot's closed-loop system.Track following problem refers to by FEEDBACK CONTROL, makes robot from arbitrary initial position, can both follow pre-set desired trajectory.Path trace problem refers to that mobile robot, with given speed or acceleration, follows pre-set expected path.

Above three gordian techniquies are the problems that must solve in robot autonomous navigation procedure, but due to the complicacy of service robot environment of living in, some current technology can not meet the demands, therefore need these technology to improve, or study new airmanship.

Summary of the invention

In order to address the above problem, the object of the invention is to for the deficiencies in the prior art, a kind of service robot autonomous navigation method based on grating map is provided.

The present invention is based on the service robot autonomous navigation method of grating map, comprise autonomous location, path planning, track following, it is characterized in that: gather in real time the residing environmental information of robot by 2D laser radar, carry out environmental characteristic extraction, utilize simultaneous localization and mapping technology to carry out the establishment of grating map to the information gathering, in real time robot is independently located simultaneously, and in position fixing process, sensor error is proofreaied and correct in real time, require robot to carry out path planning according to navigation task on this basis, then follow the tracks of control according to the path of planning, make robot complete navigation task, specifically comprise the following steps:

The present invention is achieved by the following technical solutions, the present invention includes following steps:

Step 1: utilize scrambler to gather the rotating speed of the each wheel of robot, utilize 2D laser radar to gather environmental information, thereby obtain the relative distance of service robot and surrounding environment;

Step 2: simultaneous localization and mapping

Step 2.1: independently location

Step 2.1.1: according to each wheel speed of encoder feedback and serve robot architecture, set up robot kinematics's model;

Step 2.1.2: be Markov process according to robot pose forecasting process, set up robot pose predicated error model;

Step 2.1.3: the environmental information gathering according to 2D laser radar, adopt randomized hough transform least-squares algorithm to extract local environment linear feature and some feature, set up the observation model based on environmental characteristic;

Step 2.1.4: according to the relation of 2D laser radar raw data and observation model, set up robot observational error model;

Step 2.1.5: the location algorithm by EKF is independently located.

Step 2.2: map building

Step 2.2.1: by local environment feature by the prediction of robustness, thereby generate global context feature;

Step 2.2.2: take the minimum value of service robot shared projected area in two dimensional surface as grid, and take this grid as unit by global context feature rasterizing, generated the grating map of service robot environment of living in;

Described global context grid refers to: barrier grid tag is 1, and blank grid tag is 0;

Step 3: path planning

Step 3.1: according to navigation task requirement, starting point and the impact point of path planning is set;

Step 3.2: adopt one-dimensional coding mode to represent selectable path in grating map, and set up the fitness function that has clear and definite physical significance, and then adopt genetic algorithm to carry out path planning;

Step 3.3: adopt polynomial curve method under polar coordinates to carry out smoothing processing to the path generating, thereby obtain being applicable to the smooth paths that robotic tracking controls;

Step 4: follow the tracks of and control

Step 4.1: set up kinematics model and the position and attitude error state equation of robot under certain movement constraint condition according to moveable robot movement performance;

Step 4.1: become the mobile robot trace tracking control unit of State Feedback Approach while designing based on backstepping, and utilize based on Lyapunov stability theory, the global stability of contrail tracker is analyzed;

Step 4.2: according to the motion path of the service robot of cooking up, and the current motion state of robot, adopting contrail tracker, control completes navigation task.

The present invention's beneficial effect compared with the existing technology: with respect to existing mobile robot autonomous navigation method, the invention has the advantages that: three gordian techniquies that first the present invention is directed in navigation are improved, it is more suitable in the actual demand of service robot navigation, thereby obtains best feasible program; Secondly the present invention processes environmental information in real time, produces dynamic map, thereby effectively realizes independent navigation, and the precision of navigation and efficiency all improve greatly.

Accompanying drawing explanation

Fig. 1 is service robot autonomous navigation method schematic diagram

Fig. 2 is the autonomous positioning flow figure of step 2.1 in Fig. 1

Fig. 3 is that the map of step 2.2 in Fig. 1 upgrades process flow diagram

Fig. 4 is the genetic algorithm path planning process flow diagram of step 3 in Fig. 1

Fig. 5 is the path planning schematic diagram of step 3 in Fig. 1

Fig. 6 is the path code mode schematic diagram of step 3 in Fig. 1

Fig. 7 is the tracking control flow chart of step 4 in Fig. 1

Fig. 8 is the tracking control system structure of step 4 in Fig. 1

Embodiment

Below in conjunction with accompanying drawing, the present invention is described in detail, it should be pointed out that described embodiment is only intended to be convenient to the understanding of the present invention, and it is not played to any restriction effect.

Fig. 1 is that explanation is according to the process flow diagram of the enforcement key step of the service robot autonomous navigation method based on grating map of the present invention.With reference to Fig. 1, the main flow process of the method is:

Step 1: control moves in known experimental situation, gathers and through the environmental information of feature extraction, carries out map building according to 2D laser radar, obtains global context grating map;

Step 2: carry out autonomous locating module, determine the pose of robot in global coordinate system, and require target setting point according to navigation task;

Step 3: robot is in global context grating map, and according to self pose and impact point position, execution route planning module, obtains the optimal path from robot to impact point, i.e. the path point sequence of series of discrete, and send to path trace module;

Step 4: in execution route tracking module process, the environmental information real-time update global context map gathering according to 2D laser radar.Autonomous location, and detect on path whether have barrier, if exist barrier to perform step 3, follow the tracks of otherwise continue execution route, until arrive impact point.

Fig. 2 is the autonomous positioning flow figure of step 2.1 in Fig. 1.With reference to Fig. 2, the main flow process of autonomous location is:

Step 2.1.1: first 500 line scramblers and 2D laser radar are installed on respectively around server, then 500 line scramblers are set take 30 milliseconds of rotating speeds as the each wheel of periodic feedback; 2D laser radar is set take 30 milliseconds as sampling period collection environmental information again, thereby obtains the relative distance of service robot and its surrounding environment;

Step 2.1.2: setting up robot kinematics's model according to 500 line scramblers and the each wheel construction of service robot is:

In formula [x (k) y (k) θ (k)] ^tfor the pose in the k of robot moment; L (k) is carved into the camber line distance that the k+1 moment moves during from k for robot; θ (k) is the angle of robot coordinate system and global coordinate system; Δ θ (k) is carved into the variable quantity of k+1 moment direction of motion during from k for robot.

Step 2.1.3: analyzing the error that adopts kinematics model to carry out robot pose prediction and to introduce, is Markov process according to robot pose forecasting process, obtains the pose prediction covariance matrix P of robot (k)=[p _ij].

Step 2.1.4: the observation model of setting up robot according to the position relationship of service robot and global context feature is:

$Z_j\left(k\right)=\left[\begin{array}{c}{\mathrm{\λ}}_j\\ {\mathrm{\δ}}_j\end{array}\right]=\left[\begin{array}{c}|{\mathrm{\ρ}}_j-\sqrt{x^2\left(k\right)+y^2\left(k\right)}\cos ({\mathrm{\θ}}_j-\arctan (y\left(k\right)/x\left(k\right)))|\\ {\mathrm{\θ}}_j-\mathrm{\θ}\left(k\right)\end{array}\right]$

Z in formula _j(k)=[λ _jδ _j] ^tbe the parameter of j environmental characteristic in robot coordinate system; (ρ _j, θ _j) be the parameter of j environmental characteristic in global coordinate system.

Step 2.1.5: according to the relation of 2D laser radar raw data and observation model, the observational error covariance matrix that obtains j environmental characteristic of robot is R _j;

Step 2.1.6: according to the kinematics model of service robot and observation model, upgrade four step processing procedures by filter forecasting, observation prediction, characteristic matching, state, can obtain more accurate locating information.

Fig. 3 is that the map of step 2.2 in Fig. 1 upgrades process flow diagram.

Fig. 4 is the genetic algorithm path planning process flow diagram of step 3 in Fig. 1, and Fig. 5 is the path planning schematic diagram of step 3 in Fig. 1.With reference to Fig. 4, Fig. 5, the main flow process of path planning is:

Step 3.1: according to path code mode as shown in Figure 6, path code form is:

y 1

y 2

y 3

……

y n

Step 3.2: calculate population at individual fitness [f, p]=objf (start, destination), f is each ideal adaptation degree, and p is cumulative probability;

Step 3.3: according to navigation task requirement, adopt above-mentioned improved genetic algorithms method to carry out path planning, generated the path point sequence from starting point to impact point;

Step 3.4: the path point sequence generating is carried out to key point optimization, obtain the key point sequence from starting point to impact point;

Step 3.5: adopt polynomial curve method under polar coordinates to carry out smoothing processing to the path being represented by key point sequence, polynomial curve is:

In formula for the polar coordinates of each point on curve; R is the radius that Curves substitutes arc; Φ is curve changing value.Can obtain final smooth paths.

Fig. 7 is the tracking control flow chart of step 4 in Fig. 1.The main flow process of track following is:

Step 4.1: considering that after the suffered constraint of service robot, robot position and attitude error system state equation is:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e={\mathrm{\ω}}_{\mathrm{cz}}{y}_e-v_{\mathrm{ry}}{\sin \mathrm{\θ}}_e\\ {\stackrel{\·}{y}}_e=-{\mathrm{\ω}}_{\mathrm{cz}}{x}_e-v_{\mathrm{cy}}+v_{\mathrm{ry}}\cos {\mathrm{\θ}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e={\mathrm{\ω}}_{\mathrm{rz}}-{\mathrm{\ω}}_{\mathrm{cz}}\end{array}\right.$

[x in formula _ey _eθ _e] ^tduring for k, be engraved in the position and attitude error vector in mobile robot's coordinate system; [v _cxv _cyω _cz] ^tfor the control vector of robotic tracking control device; [v _rxv _ryω _rz] ^tfor robot reference velocity and angular velocity.

Be defined as and find bounded speed controlled quentity controlled variable v based on robot kinematics's model track following _yand ω _zfor:

$v_y=v_y(x_e,y_e,{\mathrm{\θ}}_e,v_{\mathrm{ry}},{\mathrm{\ω}}_{\mathrm{rz}},{\stackrel{\·}{v}}_{\mathrm{ry}},{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rz}})$

${\mathrm{\ω}}_z={\mathrm{\ω}}_z(x_e,y_e,{\mathrm{\θ}}_e,v_{\mathrm{ry}},{\mathrm{\ω}}_{\mathrm{rz}},{\stackrel{\·}{v}}_{\mathrm{ry}},{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rz}})$

Step 4.2: construct Lyapunov function by substep and carry out design control law, the control law of robot is:

$\left\{\begin{array}{c}{v}_y=v_{\mathrm{ry}}{\cos \mathrm{\θ}}_e+k_1\cos \left(\arctan \left({\mathrm{\ω}}_z\right)\right)\frac{1}{1+{\mathrm{\ω}}_z^2}{\stackrel{\·}{\mathrm{\ω}}}_z{x}_e+k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right){\mathrm{\ω}}_z{y}_e\\ -k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right)v_{\mathrm{ry}}s{\mathrm{in\θ}}_e+k_1(y_e+k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right)x_e)\\ {\mathrm{\ω}}_z={\mathrm{\ω}}_{\mathrm{rz}}-{2k}_3{x}_e{v}_{\mathrm{ry}}\cos \left(\frac{{\mathrm{\θ}}_e}{2}\right)+k_4\sin \left(\frac{{\mathrm{\θ}}_e}{2}\right)\end{array}\right.$

K in formula ₁, k ₂, k ₃, k ₄for being greater than zero constant.

Claims (4)

1. the service robot autonomous navigation method based on grating map, comprise autonomous location, path planning, track following, it is characterized in that: gather in real time the residing environmental information of robot by 2D laser radar, carry out environmental characteristic extraction, utilize simultaneous localization and mapping technology to carry out the establishment of grating map to the information gathering, in real time robot is independently located simultaneously, and in position fixing process, sensor error is proofreaied and correct in real time, require robot to carry out path planning according to navigation task on this basis, then follow the tracks of control according to the path of planning, make robot complete navigation task, specifically comprise the following steps:

Step 1: control moves in known experimental situation, gathers and through the environmental information of feature extraction, carries out map building according to 2D laser radar, obtains global context grating map; Described global context grid refers to that barrier grid tag is 1, and blank grid tag is 0;

Step 2: carry out autonomous locating module, determine the pose of robot in global coordinate system, and require target setting point according to navigation task;

Step 3: robot is in global context grating map, and according to self pose and impact point position, execution route planning module, obtains the optimal path from robot to impact point, i.e. the path point sequence of series of discrete, and send to path trace module;

Step 4: in execution route tracking module process, the environmental information real-time update global context map gathering according to 2D laser radar; Autonomous location, and detect on path whether have barrier, if exist barrier to perform step 3, follow the tracks of otherwise continue execution route, until arrive impact point.

2. a kind of service robot autonomous navigation method based on grating map according to claim 1, is characterized in that: the concrete steps of step 2 are as follows:

Step 2.1: independently location

Step 2.1.1: first 500 line scramblers and 2D laser radar are installed on respectively around server, then 500 line scramblers are set take 30 milliseconds of rotating speeds as the each wheel of periodic feedback; 2D laser radar is set take 30 milliseconds as sampling period collection environmental information again, thereby obtains the relative distance of service robot and its surrounding environment;

Step 2.1.2: setting up robot kinematics's model according to 500 line scramblers and the each wheel construction of service robot is:

In formula [x (k) y (k) θ (k)] ^tfor the pose in the k of robot moment; L (k) is carved into the camber line distance that the k+1 moment moves during from k for robot; θ (k) is the angle of robot coordinate system and global coordinate system; Δ θ (k) is carved into the variable quantity of k+1 moment direction of motion during from k for robot.

Step 2.1.3: analyzing the error that adopts kinematics model to carry out robot pose prediction and to introduce, is Markov process according to robot pose forecasting process, obtains the pose prediction covariance matrix P of robot (k)=[p _ij].

Step 2.1.4: the observation model of setting up robot according to the position relationship of service robot and global context feature is:

$Z_j\left(k\right)=\left[\begin{array}{c}{\mathrm{\λ}}_j\\ {\mathrm{\δ}}_j\end{array}\right]=\left[\begin{array}{c}|{\mathrm{\ρ}}_j-\sqrt{x^2\left(k\right)+y^2\left(k\right)}\cos ({\mathrm{\θ}}_j-\arctan (y\left(k\right)/x\left(k\right)))|\\ {\mathrm{\θ}}_j-\mathrm{\θ}\left(k\right)\end{array}\right]$

Z in formula _j(k)=[λ _jδ _j] ^tbe the parameter of j environmental characteristic in robot coordinate system; (ρ _j, θ _j) be the parameter of j environmental characteristic in global coordinate system;

Step 2.1.5: according to the relation of 2D laser radar raw data and observation model, the observational error covariance matrix that obtains j environmental characteristic of robot is R _j;

Step 2.1.6: according to the kinematics model of service robot and observation model, upgrade four step processing procedures by filter forecasting, observation prediction, characteristic matching, state, can obtain more accurate locating information;

Step 2.2: map building

Step 2.2.1: by local environment feature by the prediction of robustness, thereby generate global context feature;

Step 2.2.2: take the minimum value of service robot shared projected area in two dimensional surface as grid, and take this grid as unit by global context feature rasterizing, generated the grating map of service robot environment of living in.

3. a kind of service robot autonomous navigation method based on grating map according to claim 1, is characterized in that: the concrete steps of step 3 are as follows:

Step 3.1: path code form is:

y ₁ y ₂ y ₃ …… y _n

Step 3.2: calculate population at individual fitness [f, p]=objf (start, destination), f is each ideal adaptation degree, and p is cumulative probability;

Step 3.3: according to navigation task requirement, adopt above-mentioned improved genetic algorithms method to carry out path planning, generated the path point sequence from starting point to impact point;

Step 3.4: the path point sequence generating is carried out to key point optimization, obtain the key point sequence from starting point to impact point;

Step 3.5: adopt polynomial curve method under polar coordinates to carry out smoothing processing to the path being represented by key point sequence, polynomial curve is:

In formula for the polar coordinates of each point on curve; R is the radius that Curves substitutes arc; Φ is curve changing value.Can obtain final smooth paths.

4. a kind of service robot autonomous navigation method based on grating map according to claim 1, is characterized in that: the concrete steps of step 4 are as follows:

Step 4.1: considering that after the suffered constraint of service robot, robot position and attitude error system state equation is:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_e={\mathrm{\ω}}_{\mathrm{cz}}{y}_e-v_{\mathrm{ry}}{\sin \mathrm{\θ}}_e\\ {\stackrel{\·}{y}}_e=-{\mathrm{\ω}}_{\mathrm{cz}}{x}_e-v_{\mathrm{cy}}+v_{\mathrm{ry}}\cos {\mathrm{\θ}}_e\\ {\stackrel{\·}{\mathrm{\θ}}}_e={\mathrm{\ω}}_{\mathrm{rz}}-{\mathrm{\ω}}_{\mathrm{cz}}\end{array}\right.$

[x in formula _ey _eθ _e] ^tduring for k, be engraved in the position and attitude error vector in mobile robot's coordinate system; [v _cxv _cyω _cz] ^tfor the control vector of robotic tracking control device; [v _rxv _ryω _rz] ^tfor robot reference velocity and angular velocity.

Be defined as and find bounded speed controlled quentity controlled variable v based on robot kinematics's model track following _yand ω _zfor:

$v_y=v_y(x_e,y_e,{\mathrm{\θ}}_e,v_{\mathrm{ry}},{\mathrm{\ω}}_{\mathrm{rz}},{\stackrel{\·}{v}}_{\mathrm{ry}},{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rz}})$

${\mathrm{\ω}}_z={\mathrm{\ω}}_z(x_e,y_e,{\mathrm{\θ}}_e,v_{\mathrm{ry}},{\mathrm{\ω}}_{\mathrm{rz}},{\stackrel{\·}{v}}_{\mathrm{ry}},{\stackrel{\·}{\mathrm{\ω}}}_{\mathrm{rz}})$

Step 4.2: construct Lyapunov function by substep and carry out design control law, the control law of robot is:

$\left\{\begin{array}{c}{v}_y=v_{\mathrm{ry}}{\cos \mathrm{\θ}}_e+k_1\cos \left(\arctan \left({\mathrm{\ω}}_z\right)\right)\frac{1}{1+{\mathrm{\ω}}_z^2}{\stackrel{\·}{\mathrm{\ω}}}_z{x}_e+k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right){\mathrm{\ω}}_z{y}_e\\ -k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right)v_{\mathrm{ry}}s{\mathrm{in\θ}}_e+k_1(y_e+k_1\sin \left(\arctan \left({\mathrm{\ω}}_z\right)\right)x_e)\\ {\mathrm{\ω}}_z={\mathrm{\ω}}_{\mathrm{rz}}-{2k}_3{x}_e{v}_{\mathrm{ry}}\cos \left(\frac{{\mathrm{\θ}}_e}{2}\right)+k_4\sin \left(\frac{{\mathrm{\θ}}_e}{2}\right)\end{array}\right.$

K in formula ₁, k ₂, k ₃, k ₄for being greater than zero constant.