CN106444738A - Mobile robot path planning method based on dynamic motion primitive learning model - Google Patents

Abstract

The invention discloses a path planning method for a mobile robot based on a learning model of a dynamic motion primitive. First use the handle to control the movement of the robot and record the trajectory of the robot. Then, the recorded trajectory is used as a sample of the dynamic motion primitive model, and the parameters of the dynamic motion primitive model are obtained by establishing the dynamic motion primitive model and using the trajectory samples for training, so as to realize the autonomous path planning of the robot. On this basis, the target position of the robot movement is changed to complete the generalization of the new target. The path planning method of the present invention improves the intelligence level of the mobile robot. When the target position of the robot movement changes, the robot can reach the new target position autonomously, that is, the robot can complete not for a certain specified task, but for other tasks It also has the ability of generalization; and the combination of the online learning feature of the dynamic motion primitive model and its autonomous obstacle avoidance function improves the efficiency of path planning.

Description

Path Planning Method for Mobile Robots Based on Learning Model of Dynamic Motion Primitives

technical field

The invention relates to the field of path planning for mobile robots, in particular to a path planning method for mobile robots based on a learning model of dynamic motion primitives.

Background technique

Path planning is one of the key technologies of mobile robots. To a certain extent, it marks the intelligence level of mobile robots. Being able to quickly find a convenient and collision-free path not only ensures the safety of the mobile robot itself, but also reflects the robot's Efficiency and reliability.

At present, the commonly used robot path planning methods include artificial potential field method, fuzzy logic model, genetic and other models. The artificial potential field method is a relatively mature and efficient planning method in the path planning model, and is widely used for its simple mathematical calculation. However, the traditional artificial potential field method has problems such as local minimum points and unreachable targets. At present, there are many ways to solve the local minimum point, such as heuristic search, random escape method, etc., but these improved artificial potential field methods only exert additional control force on the robot, and do not fundamentally solve the problem. The genetic model is a multi-search model based on genetic and natural selection, which has the advantages of robustness, flexibility, and the search in the population is not easy to fall into a local minimum point. However, the genetic model has problems such as large population size, large search space, easy to fall into local minimum point, and slow convergence speed when performing robot path planning.

The above traditional robot path planning model mainly has the following two problems:

(1) The task is specific, and it only has good performance for a certain task, but does not have the ability to generalize;

(2) Learning is often offline, which leads to retraining and learning for new scenes, and the real-time performance is poor.

Contents of the invention

The technical problem to be solved by the present invention is to propose a path planning method based on a learning model of dynamic motion primitives in view of the poor real-time performance and the single task of the mobile robot in the above path planning method for mobile robots. The ability to search for paths in real time, combined with its autonomous obstacle avoidance function, can effectively improve the efficiency of path planning. In addition, when the robot completes new tasks, it can maintain the characteristics of the original sample trajectory to reach the new target position without retraining samples.

In order to solve the above technical problems, the present invention provides the following technical solutions:

A method for path planning of a mobile robot based on a dynamic motion primitive learning model, characterized in that it mainly includes the following steps:

Step 1: Model the two-dimensional environment of the robot's movement, and simulate the two-dimensional environment interface of the robot's movement. The robot is replaced by a small solid circle, and the obstacles are various plane figures;

Step 2: Use the handle to control the robot so that the robot can avoid collision with obstacles and reach the target point from the starting point;

Step 3: During the process of step 2, collect the robot motion trajectory data as the sample points of the dynamic motion primitive learning model, and the robot motion trajectory data includes displacement, velocity and acceleration;

Step 4: According to the displacement, velocity and acceleration data of the robot's trajectory obtained in step 3, these data are used as training samples, and the samples are trained by the dynamic motion primitive algorithm to obtain the optimal weight value corresponding to the robot's trajectory;

Step 5: Set initial parameters for specific tasks, the initial parameters include the starting point and end point of robot motion, and plan the path after learning through the dynamic motion primitive model according to the optimal weight value obtained in step 4, which has the original sample The characteristics of the trajectory, that is, the starting point and the ending point are consistent, and its running trajectory is roughly the same as the sample trajectory;

Step 6: On the basis of step 5, add circular obstacles, and add coupling terms to the original dynamic equation, so as to construct a dynamic system with obstacle avoidance function, and realize the autonomous avoidance of the dynamic motion primitive learning model dysfunction;

Step 7: On the basis of step 5, change the target position of the robot movement, and only change the parameters of the target position without retraining samples, and the robot can still reach the new target point position autonomously, that is, the robot can complete untargeted A specific task, but also has the ability to generalize to other tasks.

In the above technical solution, in step 1, the two-dimensional environment of the robot's movement is modeled, and the requirements for modeling are: the range of motion of the mobile robot is in a limited two-dimensional space; The size expands outwards, and the robot is regarded as a particle; the obstacles are composed of various plane figures, the number is limited, and these obstacles will not change and move during the movement of the robot.

In the above technical solution, the specific process of step 2 is as follows:

Step 2-1: Read the data of the robot handle. When the handle is pushed up and down or left and right, the interface will display the displacement, speed and acceleration of the robot in the modeling environment in real time;

Step 2-2: Remote control handle, artificially plan an optimal path for the robot to reach the end point from the starting point. Considering that the robot can only move back and forth and left and right, the planned path is also the path of front and back or left and right movement. Planning The resulting trajectory is also called the sample trajectory;

Step 2-3: When planning the path, avoid obstacles, and use the method of data saving to record the displacement, velocity and acceleration of the sample trajectory, and use it as sample data.

In the above technical solution, step 4 includes the following specific steps:

Step 4-1: Establish the mathematical model of dynamic motion primitives: dynamic motion primitives are generally used to form discrete motions. For a single degree of freedom displacement y, introduce a linear differential equation with constant coefficients and call it a dynamic system. This system serves as the basis for motor learning:

In the formula:

x and v are the displacement and velocity of the system, respectively; x ₀ and g are the initial position and target position, respectively; τ is the time stretch factor; K is the elastic coefficient of the spring; D is the damping coefficient of the system in a critical state; f is the nonlinearity functions for generating arbitrarily complex movements;

Step 4-2: Set the initial parameters, the starting point x ₀ and the target point g of the robot movement, the time constant τ, the elastic coefficient K of the spring, and the damping coefficient D when the system is in a critical state; the nonlinear function f is used to form any complex The motion of , define f as:

In the formula:

ψ _i (s) is a radial basis kernel function, i represents the i-th radial basis kernel function ψ _i (s), and its value ranges from 1 to N, where N represents the number of radial basis kernel functions; The basic kernel function is defined as:

ψ _i (s)＝exp(-h _i (sc _i ) ² ) (4)

In the formula:

c _i is the center of the radial basis kernel function, h _i >0 and determines the width of the kernel function; where h _N ＝h _N-1 , i=1,...N, α is any normal constant;

The function f in formula (3) does not depend on the time parameter, but on the phase variable s, the expression of s is:

In the formula:

s is a function of time t, α is any normal constant, and τ is a time expansion factor; from equation (5), it can be seen that s is monotonically decreasing from 1 to 0, so equation (5) is called a regular system;

Step 4-3: Substitute the sample data obtained in step 3 into formula (1) and formula (2), because the regular system is integrable, that is, s can be calculated according to the parameter τ, so the nonlinear disturbance in the training sample f'(s) can be expressed as:

According to the minimum error criterion function J to solve the optimal weight value w _i , where the expression of the minimum error criterion function is:

J＝∑s (f′( _s )-f(s)) ² (7)

When J is minimized, _wi is the best weight value.

In the above technical solution, step 5 includes the following specific steps:

Step 5-1: When the robot performs the specified task, set the start position and end position of the robot;

Step 5-2: The sample data is two-dimensional, that is, it includes data in the x-axis direction and data in the y-axis direction. The data in the x-axis direction is trained according to step 4 to obtain the best data in the x-axis direction. The weight value is substituted into the starting point and end point value in step 5-1 to calculate the displacement, velocity and acceleration in the x direction after learning through the dynamic motion primitive model;

Step 5-3: Train the data in the y-axis direction according to step 4 to obtain the optimal weight value in the y-axis direction, substitute the starting point and end point values in step 5-1, and calculate the dynamic motion basis in the y-direction Displacement, velocity and acceleration after meta-model learning;

Step 5-4: Read in the data obtained in step 5-2 and step 5-3, obtain the motion data in the x-axis and y-axis directions respectively, and output the motion trajectory simulation diagram on the two-dimensional plane, that is, complete the Path Planning for Mobile Robots by Learning Models of Dynamic Motion Primitives.

In the above technical solution, the obstacle added in step 6 is a circle with (0.4,0.4) as the center coordinates and a radius of 0.1m.

The method of the present invention begins to study with a simple linear dynamic system (a group of differential equations), converts the simple linear dynamic system into a nonlinear system through the conversion system, and forms any complex motion through the attractor, so that it can be compared Simple study of nonlinear systems. Among them, the advantage of using differential equations is that the error can be automatically corrected, and the differential equations are formed in a fixed format. According to this fixed format, only a simple change of a target parameter is required to adapt to the new environment. , that is, it can generalize new targets; the method based on dynamic motion primitive learning is online learning, and can track the target position in real time without re-learning for new situations. Therefore, in terms of obstacle avoidance, autonomous obstacle avoidance is realized by constructing a dynamic system with obstacle avoidance function, and the combination of online learning features of the dynamic motion primitive model and its autonomous obstacle avoidance function improves the efficiency of path planning.

Compared with prior art, the beneficial effect of the present invention is:

(1) The present invention proposes a mobile robot path planning method based on a dynamic motion primitive learning model. The learning model has the ability of generalization. When the robot completes a new task, it can keep the original sample track without retraining the sample. The feature reaches the new target location.

(2) The path planning model proposed by the present invention is real-time when searching for a path, and combining with its autonomous obstacle avoidance function can effectively improve the efficiency of path planning.

Description of drawings

Fig. 1 is the schematic diagram of the path planning process of the mobile robot based on the dynamic motion primitive learning model of the present invention;

Fig. 2 is the two-dimensional environment of simulating robot motion among the present invention; Wherein the linear track represents the sample track; Graphics of various shapes (such as rectangle, circle, ellipse) represent the obstacle in the two-dimensional environment;

Fig. 3 is the simulation diagram of autonomous obstacle avoidance of dynamic motion primitive learning model among the present invention;

Fig. 4 is the emulation diagram of the generalization promotion ability that dynamic motion primitive learning model has among the present invention;

Fig. 5 is a comparison diagram of the training sample trajectory and the trajectory learned by the dynamic motion primitive model.

detailed description

In order to further illustrate the technical solution of the present invention, the present invention will be described in detail below in conjunction with accompanying drawings 1-5.

Step 1: Simulate the two-dimensional environment interface of the robot movement, the robot on the interface is replaced by a small solid circle, and the obstacles are various plane figures; set the two-dimensional environment interface of the robot movement as a square (both length and width are 1m), The robot is replaced by a small solid circle with a diameter of 5mm.

Step 2: Use OPENCV (Open Source Computer Vision Library) to realize the control of the robot by the handle, so that the robot can avoid collision with obstacles from the starting point to reach the target point;

Step 2-1: Write a host computer software based on the MFC (Microsoft Foundation Classes) interface. This software can read the data of the robot handle. When the handle is pushed up and down or left and right, the interface can display the robot in the modeling environment in real time. Movement displacement, velocity and acceleration;

Step 2-2: Remote control handle, artificially plan an optimal path for the robot to reach the end point from the starting point. Considering that the robot can only move back and forth and left and right, the planned path is also the path of front and back or left and right movement. Planning The resulting trajectory is also called the sample trajectory;

Step 2-3: When planning the path, avoid obstacles, and use the method of data saving to record the displacement, velocity and acceleration of the sample trajectory, and use it as sample data;

In step 2, the upper computer software based on MFC is used, and the robot can be controlled by manipulating the handle. The displacement of the push rod of the handle is set as the speed of the robot when it is moving, and the range of controlling the moving speed of the robot is -5mm/s～5mm/s.

Step 3: During the process of step 2, collect the robot motion trajectory data as the sample points of the dynamic motion primitive learning model, wherein the robot motion trajectory data includes the size of its displacement, velocity and acceleration values;

Step 4: According to the displacement, velocity and acceleration of the robot trajectory obtained in step 3, these data are used as the training samples of the DMP learning model, and the optimal weight value corresponding to the robot trajectory is obtained by training the samples;

Step 4-1: Establish a mathematical model of dynamic motion primitives. Dynamic motion primitives are generally used to form discrete motions. For a single degree of freedom displacement y, a linear differential equation with a constant coefficient is introduced and called a dynamic system. This system serves as the basis for motion learning:

In the formula:

x and v are the displacement and velocity of the system, respectively; x ₀ and g are the initial position and target position, respectively; τ is the time stretch factor; K is the elastic coefficient of the spring; D is the damping coefficient of the system in a critical state; f is the nonlinearity functions for generating arbitrarily complex movements;

Step 4-2: Set the initial parameters, the starting point x ₀ and the target point g of the robot movement, the time constant τ, the elastic coefficient K of the spring, and the damping coefficient D when the system is in a critical state; the nonlinear function f is used to form any complex The motion of is defined as:

In the formula:

ψ _i (s) is a radial basis kernel function, i represents the i-th radial basis kernel function ψ _i (s), and its value ranges from 1 to N, where N represents the number of radial basis kernel functions; The basic kernel function is defined as:

ψ _i (s)＝exp(-h _i (sc _i ) ² ) (4)

In the formula:

c _i is the center of the radial basis kernel function, h _i >0 and determines the width of the kernel function; where h _N ＝h _N-1 , i=1,...N, α is any normal constant;

The function f in formula (3) does not depend on the time parameter, but on the phase variable s, the expression of s is:

In the formula:

s is a function of time t, α is any normal constant, and τ is a time expansion factor; from equation (5), it can be seen that s is monotonically decreasing from 1 to 0, so equation (5) is called a regular system;

Step 4-3: Substitute the sample data obtained in step 3 into the above formula, because the regular system is integrable, that is, s can be calculated according to the parameter τ, so the nonlinear disturbance f'(s) in the training sample can be expressed as become:

According to the minimum error criterion function J to solve the optimal weight value w _i , where the expression of the minimum error criterion function is:

J＝∑s (f′( _s )-f(s)) ² (7)

When J takes the minimum, _wi is the best weight value;

Step 5: Set the initial parameters (starting point and end point of the robot motion) for a specific task, and plan the path learned by the dynamic motion primitive model according to the optimal weight value obtained in step 4, which has the characteristics of the original sample trajectory;

Step 5-1: When the robot performs the specified task, set the start position and end position of the robot;

Step 5-2: The sample data is two-dimensional (data in the x-axis direction and data in the y-axis direction), and the data in the x-axis direction is trained according to step 4 to obtain the optimal weight value in the x-axis direction , by substituting the start and end values in step 5-1, the displacement, velocity and acceleration in the x direction after learning through the dynamic motion primitive model can be calculated;

Step 5-3: Train the data in the y-axis direction according to step 4 to obtain the optimal weight value in the y-axis direction, and substitute the starting point and end point values in step 5-1 to calculate the dynamic weight in the y-axis direction. Displacement, velocity and acceleration after learning the motion primitive model;

Step 5-4: Read the data obtained in step 5-2 and step 5-3 into MATLAB to obtain the motion data in the directions of x-axis and y-axis, and output the motion trajectory simulation diagram on the two-dimensional plane, namely Complete the path planning of the mobile robot based on the dynamic motion primitive learning model.

Step 6: On the basis of step 5, add circular obstacles, and add coupling terms to the original dynamic equation, so as to construct a dynamic system with obstacle avoidance function, and realize the autonomy of the dynamic motion primitive learning model Obstacle avoidance function;

Step 6-1: On the basis of step 5-4, add a circular obstacle, where the obstacle is a circle with (0.4,0.4) as the center coordinates and a radius of 0.1m;

Step 6-2: On the basis of the dynamic system equation given in step 4-1, add the coupling item P(x,v) to construct a dynamic system with obstacle avoidance function, where the coupling item P(x,v) The expression is:

In the formula:

so for the axis, is the rotation matrix for the rotation angle, vector is the position of the obstacle, γ and β are constants, θ is the angle between the distance vector between the point on the trajectory and the obstacle and the relative velocity of that point on the trajectory;

Step 6-3: Given the initial value of the constant term in the coupling term P(x, v) formula, where γ=8, The rotation matrix R is expressed as:

Step 6-4: By building a dynamic system with obstacle avoidance function and adding the obstacles described in step 6-1, the robot can still avoid obstacles to reach the target point, and the dynamic system with obstacle avoidance function The mathematical expression of is:

It can be seen from Figure 3 that the dynamic motion primitive model has the function of autonomous obstacle avoidance;

Step 7: On the basis of step 5, change the target position of the robot movement, and only change the parameters of the target position without retraining samples, the robot can still reach the new target point position autonomously, that is, the robot can complete different For a specific task, it also has the ability to generalize to other tasks.

Step 7-1: On the basis of step 5-4, change the position of the robot target point to (0.5,0.5), substitute into step 4, and obtain the trajectory after learning the dynamic motion primitive model without retraining samples ;

Step 7-2: On the basis of step 5-4, change the position of the robot target point to (0.8,0.8), and substitute it into step 4 to obtain the trajectory after learning the dynamic motion primitive model without retraining samples ;

Step 7-3: As can be seen from Figure 4, in steps 7-1 and 7-2, the robot can reach the new target position and maintain the characteristics of the original sample trajectory, thus proving that the dynamic motion primitive learning model has Generalization ability;

In summary, the present invention implements path planning for robots based on a dynamic motion primitive learning model. The combination of online learning features of the learning model and its autonomous obstacle avoidance function improves the efficiency of path planning, and the model has generalization capabilities. The proposal of the present invention improves the intelligence of the mobile robot, and provides reference for the mobile robot in related fields such as path planning, obstacle avoidance and navigation.

Claims (6)

1. the method for planning path for mobile robot based on dynamic motion primitive learning model, it is characterised in that mainly include Following steps：

Step 1：The two-dimensional environment of robot motion is modeled, the two-dimensional environment interface of dummy robot's motion, robot Replacing by little filled circles, barrier is various planar graphs；

Step 2：Utilize the manipulation to robot for the handle, make robot can avoid colliding with barrier from starting point and reach target Point；

Step 3：During step 2 is carried out, gather robot motion trace data as dynamic motion primitive learning model Sample point, described robot motion's track data includes displacement, speed and acceleration；

Step 4：These data are made by the displacement during robot motion's track obtaining according to step 3, speed and acceleration information It for training sample, is trained obtaining the best weights corresponding to robot motion's track to sample by dynamic motion primitive algorithm Weight values；

Step 5：Arranging initial parameter for particular task, described initial parameter includes the beginning and end of robot motion, root The best weights weight values obtaining according to step 4, the path after cooking up by the study of dynamic motion basic-element model, this path has former state The characteristic of this track, i.e. beginning and end are consistent, and its running orbit is roughly the same with sample trace；

Step 6：On the basis of step 5, add circular barrier, and in original kinetics equation, add coupling terms, Thus build the dynamic system with barrier avoiding function, it is achieved the automatic obstacle avoiding function of dynamic motion primitive learning model；

Step 7：On the basis of step 5, change the target location of robot motion, on the premise of not re-training sample, Only changing the parameter of target location, robot remains to independently reach new aiming spot, i.e. robot can complete not pin To a certain appointed task, and other task is also had to the ability of extensive popularization.

2. the method for planning path for mobile robot based on dynamic motion primitive learning model according to claim 1, its It is characterised by：In step 1 being modeled the two-dimensional environment of robot motion, the requirement of modeling is：The activity of mobile robot Scope is at a limited two-dimensional space；With move robot size as benchmark, by the size outward expansion of barrier, by machine Device people regards a particle as；Barrier is made up of various planar graphs, Limited Number, and in robot moving process these Barrier will not change and move.

3. the method for planning path for mobile robot based on dynamic motion primitive learning model according to claim 1, its It is characterised by：Step 2 detailed process is as follows：

Step 2-1：The data of read machine people's handle, when handle to up and down or left and right promote when, this interface shows machine in real time Displacement, speed and the acceleration that device people moves in modeling environment；

Step 2-2：Remote-control handle, artificial cooks up the optimal path that a robot can reach home from starting point, it is considered to To robot typically can only before and after and side-to-side movement, before and after the path therefore planning out is also or the path of side-to-side movement, Planning track out is also referred to as sample trace；

Step 2-3：When path planning, avoiding obstacles, and by the method for data preservation by the displacement of sample trace, speed The value of degree and acceleration is recorded, and as sample data.

4. the method for planning path for mobile robot based on dynamic motion primitive learning model according to claim 1, its It is characterised by：Step 4 comprises the following specific steps that：

Step 4-1：Set up the Mathematical Modeling of dynamic motion primitive：Dynamic motion primitive is generally used to be formed discrete motion, right In single free degree displacement y, introduce with constant coefficients linear differential equation referred to as dynamic system, this system conduct Basis to motor learning：

$\mathrm{\τ}\stackrel{\·}{v}=K(g-x)-Dv-K(g-x_0)s+Kf\left(s\right)---\left(1\right)$

$\mathrm{\τ}\stackrel{\·}{x}=v---\left(2\right)$

In formula：

X and v is displacement and the speed of system respectively；x₀It is initial position and target location respectively with g；τ is time contraction-expansion factor；K It is the coefficient of elasticity of spring；D is the damped coefficient that system is under critical condition；F is nonlinear function, is used for generating arbitrarily multiple Miscellaneous motion；

Step 4-2：Initial parameter is set, starting point x of robot motion₀With impact point g, timeconstantτ, the elastic system of spring Number K, system is in the damped coefficient D under critical condition；Nonlinear function f is for forming arbitrarily complicated motion, and definition f is：

$f\left(s\right)=\frac{{\mathrm{\Σ}}_{i=1}^N{\mathrm{\ψ}}_i\left(s\right)w_i}{{\mathrm{\Σ}}_{i=1}^N{\mathrm{\ψ}}_i\left(s\right)}s---\left(3\right)$

In formula：

ψ_iS () is Radial basis kernel function, i represents i-th Radial basis kernel function ψ_iS (), its span is 1 to N, and wherein N represents The number of Radial basis kernel function；Radial basis kernel function is defined as：

ψ_i(s)=exp (-h_i(s-c_i)²) (4)

In formula：

c_iIt is the center of Radial basis kernel function, h_i>0 and determine kernel function width；Wherein h_N=h_N-1, i=1 ... N, α are any normal number；

Function f in formula (3) is not dependent on time parameter, and is depending on phase variant s, and the expression-form of s is：

$\mathrm{\τ}\stackrel{\·}{s}=-\mathrm{\α}s---\left(5\right)$

In formula：

S is the function with regard to time t, and α is any normal number, and τ is time contraction-expansion factor；Equation (5) is understood that s is single by 1 to 0 Tune successively decreases, and therefore equation (5) is referred to as cannoncial system；

Step 4-3：The sample data obtaining in step 3 is substituted in formula (1) and formula (2), because cannoncial system is to amass Point, i.e. s can calculate according to parameter τ, so nonlinear disturbance f ' (s) in training sample can be expressed as：

$f^{\′}\left(s\right)=\frac{\mathrm{\τ}\stackrel{\·}{v}+Dv}{K}-(g-x)+(g-x_0)s---\left(6\right)$

Solve best weights weight values w according to minimum error principle function J_i, wherein the expression formula of minimum error principle function is：

J=∑_s(f′(s)-f(s))²(7)

W when J takes minimum_iIt is exactly optimal weighted value.

5. the method for planning path for mobile robot based on dynamic motion primitive learning model according to claim 1, its It is characterised by：Step 5 comprises the following specific steps that：

Step 5-1：When robot performs specifying of task, start position and the final position of robot are set；

Step 5-2：Sample data is two-dimentional, namely includes the data on x-axis direction and the data on y-axis direction, by x-axis side Data upwards are trained according to step 4, obtain the best weights weight values on x-axis direction, substitute into the starting point in step 5-1 and end Point value, calculates x side upwardly through the displacement after the study of dynamic motion basic-element model, speed and acceleration；

Step 5-3：Data on y-axis direction are trained according to step 4, obtain the best weights weight values on y-axis direction, substitute into Beginning and end value in step 5-1, calculate y side upwardly through dynamic motion basic-element model study after displacement, speed and Acceleration；

Step 5-4：Read in the data obtaining in step 5-2 and step 5-3, respectively obtain the motion number of x-axis and y-axis both direction According to exporting the track emulation figure of motion on two dimensional surface, i.e. complete based on dynamic motion primitive learning model to mobile machine The path planning of people.

6. the method for planning path for mobile robot based on dynamic motion primitive learning model according to claim 1, its It is characterised by：The barrier being added in step 6 is to be central coordinate of circle with (0.4,0.4), and radius is the circle of 0.1m.