CN113741414A - Safe motion planning method and device based on mobile robot contour - Google Patents
Safe motion planning method and device based on mobile robot contour Download PDFInfo
- Publication number
- CN113741414A CN113741414A CN202110647515.0A CN202110647515A CN113741414A CN 113741414 A CN113741414 A CN 113741414A CN 202110647515 A CN202110647515 A CN 202110647515A CN 113741414 A CN113741414 A CN 113741414A
- Authority
- CN
- China
- Prior art keywords
- robot
- constraint
- path
- convex polygon
- information
- 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
Links
- 238000000034 method Methods 0.000 title claims abstract description 68
- 238000005457 optimization Methods 0.000 claims abstract description 25
- 230000004888 barrier function Effects 0.000 claims abstract description 9
- 230000001133 acceleration Effects 0.000 claims description 16
- 238000012545 processing Methods 0.000 claims description 15
- 238000005070 sampling Methods 0.000 claims description 6
- 238000005265 energy consumption Methods 0.000 claims description 5
- 238000005516 engineering process Methods 0.000 abstract description 6
- 230000007613 environmental effect Effects 0.000 abstract 1
- 238000004458 analytical method Methods 0.000 description 3
- 238000001514 detection method Methods 0.000 description 3
- 238000010586 diagram Methods 0.000 description 3
- 238000010845 search algorithm Methods 0.000 description 2
- 238000013461 design Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000005259 measurement Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000006467 substitution reaction Methods 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0221—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving a learning process
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/0223—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory involving speed control of the vehicle
Landscapes
- Engineering & Computer Science (AREA)
- Aviation & Aerospace Engineering (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Automation & Control Theory (AREA)
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
- Manipulator (AREA)
Abstract
The invention discloses a safe motion planning method and device based on a mobile robot contour, which are used for planning a track based on the existing environment map information and robot positioning information to obtain an initial path. And then, considering kinematic constraint, two-point boundary value constraint and manifold constraint of the robot, and performing path optimization based on the initial path to complete the motion planning of the robot. The method fully considers the outline of the mobile robot and the outline of the environmental barrier when constructing the optimal control proposition of the trajectory planning, solves the optimal trajectory by applying the collision-free constraint, is safe and efficient, and can greatly reduce the collision risk of the mobile robot caused by the motion planning technology.
Description
Technical Field
The invention belongs to the field of mobile robot motion planning, and particularly relates to a safe motion planning method and device based on a mobile robot contour.
Background
The motion planning technology has the function of starting and stopping in the autonomous navigation technology, is connected with the environment map information in an up-connection mode, is connected with the tracking control in a down-connection mode, and is one of the core technologies for realizing the autonomous navigation function. However, in the current motion planning technology, a graph search method, a sampling method, and the like are often used, and a path from a starting point to an end point is planned by simplifying a mobile robot into a mass point. Although the existing method can find a geometric path, the existing method cannot effectively cope with dynamic obstacles in the environment under the condition of meeting the kinematic constraint. In addition, the existing method does not fully consider the outline and the shape of the mobile robot, so that the robot has certain potential safety hazard in the autonomous navigation process. Therefore, designing a safe and effective motion planning method and device which conform to the kinematic constraints of the robot is crucial to the autonomous navigation implementation of the mobile robot.
Disclosure of Invention
In view of this, the invention provides a safe motion planning method and device based on a mobile robot contour, which can improve the safety of the motion planning of the mobile robot.
In order to solve the above-mentioned technical problems, the present invention has been accomplished as described above.
A safe motion planning method based on mobile robot outline comprises the following steps:
planning a track based on the existing environment map information and the robot positioning information to obtain an initial path;
the kinematic constraint, the two-point boundary value constraint and the manifold constraint of the robot are considered, path optimization is carried out based on the obtained initial path, and the motion planning of the robot is completed;
the manifold constraint comprises obstacle collision constraint conditions, specifically: representing the outline of the robot as a convex polygon and representing the outline of the obstacle as more than one convex polygon; at any moment, each vertex of the robot convex polygon outline is positioned outside the obstacle convex polygon, and each vertex of the obstacle convex polygon is positioned outside the robot convex polygon outline.
Furthermore, the judgment mode that the vertex of the convex polygon outline of the robot is positioned outside the convex polygon of the obstacle is as follows:
let the vertex be pvThe convex polygon T is formed by vertexes V1…VnComposition is carried out; judging the vertex as pvThe way of whether it is outside the convex polygon T is: with the vertex pvTowards vertex V in sequence1…VnConnecting lines are made, and n connecting lines form n triangles which are represented as pvVnV1、pvVkVk+1K is 1,2,. cndot.n-1; if the total area of the n triangles is larger than the area of the convex polygon T, the vertex p is determinedvOutside the convex polygon T.
Further, the two-point boundary value constraint includes two parts, namely a start point constraint and an end point constraint:
the origin constraint is expressed as:
the endpoint constraint is expressed as:
Wherein x ism(t) and ym(t) positional information of the robot at time t, vm(t) and am(t) respectively representing the speed and acceleration information of the robot moving at the moment t; thetam(t) is the yaw angle information of the robot at the time t; deltam(t) and ωm(t) respectively representing the steering angle and the steering angular speed of the wheels of the robot at the moment t; t is tfIndicating a stop time; (x)m0,ym0) Indicating positional coordinate information of the robot at an initial time, thetam0Representing yaw direction information of the robot at the initial moment, vm0And deltam0Respectively representing the speed and the steering angle information of the robot at the initial moment; (x)mf,ymf) Position coordinate information representing the time at which the robot stops, and θmfAnd yaw direction information indicating a time when the robot stops.
Further, the manifold constraints further include an extreme motion capability constraint:
wherein v ism(t) and am(t) represents the velocity and acceleration information, respectively, of the robot movement at time t, δm(t) and ωm(t) respectively representing the steering angle and the steering angular speed of the wheels of the robot at the moment t; v. ofmaxIs the maximum value of the moving speed, delta, of the robot in the whole moving processmaxIs the maximum value which can be reached by the steering angle of the wheels of the robot in the whole moving process, amaxAnd ωmaxRespectively, the maximum values of the acceleration and the wheel steering angular velocity of the robot during the whole movement.
Further, performing trajectory planning based on the existing environment map information and the robot positioning information, and obtaining an initial path as follows: rapidly planning a geometric path which is free of collision and connects a starting point and an end point by a graph searching and sampling algorithm and the like on the basis of the existing environment map information and the robot positioning information; and then, referring to dynamic barrier information in the environment, and planning the speed at which the robot runs along the geometric path by adopting a speed decision method, thereby completing the front-end trajectory planning and obtaining an initial path.
Further, in the path optimization process, the path quality is measured by weighting and measuring the corresponding passing time, energy consumption and path smoothness of the path, and the weighted path quality is used as an optimized objective function.
A safe motion planning device based on a mobile robot outline comprises an initial path planning unit and a path optimization unit;
the initial path planning unit is used for planning a track based on the existing environment map information and the robot positioning information to obtain an initial path;
the path optimization unit is used for considering kinematic constraint, two-point boundary value constraint and manifold constraint of the robot, optimizing a path based on the initial path and finishing the motion planning of the robot;
the manifold constraint comprises obstacle collision constraint conditions, specifically: representing the contour of the robot as a convex polygon, and representing the contour of the obstacle as more than one convex polygon; at any moment, each vertex of the robot convex polygon outline is positioned outside the obstacle convex polygon, and each vertex of the obstacle convex polygon is positioned outside the robot convex polygon outline.
Further, the path optimization unit comprises a constraint condition processing module and an optimization module;
the constraint condition processing module is used for processing kinematic constraint, two-point boundary value constraint and manifold constraint; judging a vertex p according to the collision constraint condition of the barriervIf it is outside the convex polygon T, using the vertex pvTowards each vertex V of the convex polygon T in turn1…VnConnecting lines are made, and n connecting lines form n triangles which are represented as pvVnV1、pvVkVk+1K is 1,2,. cndot.n-1; if the total area of the n triangles is larger than the area of the convex polygon T, the vertex p is determinedvOutside the convex polygon T;
and the optimization module is used for optimizing the initial path according to the constraint condition and the set objective function to complete the motion planning of the robot.
Further, the two-point boundary value constraint used by the constraint condition processing module comprises two parts, namely a start point constraint and an end point constraint:
the origin constraint is expressed as:
the endpoint constraint is expressed as:
Wherein x ism(t) and ym(t) positional information of the robot at time t, vm(t) and am(t) respectively representing the speed and acceleration information of the robot moving at the moment t; thetam(t) is the yaw angle information of the robot at the time t; deltam(t) and ωm(t) respectively representing the steering angle and the steering angular speed of the wheels of the robot at the moment t; t is tfIndicating a stop time; (x)m0,ym0) Indicating positional coordinate information of the robot at an initial time, thetam0Representing yaw direction information of the robot at the initial moment, vm0And deltam0Respectively representing the speed and the steering angle information of the robot at the initial moment; (x)mf,ymf) Position coordinate information representing the time at which the robot stops, and θmfYaw direction information indicating a stopping time of the robot;
manifold constraints further include motion capability extremum constraints:
wherein v ismaxIs the maximum value of the moving speed, delta, of the robot in the whole moving processmaxIs the maximum value which can be reached by the steering angle of the wheels of the robot in the whole moving process, amaxAnd ωmaxRespectively, the maximum values of the acceleration and the wheel steering angular velocity of the robot during the whole movement.
Further, the initial path planning unit comprises a path decision module and a speed decision module;
the path decision module is used for quickly planning a geometric path which is free of collision and connects a starting point and an end point through a graph searching and sampling algorithm and the like on the basis of the existing environment map information and the robot positioning information;
and the speed decision module is used for referring to dynamic barrier information in the environment and planning the speed at which the robot runs along the geometric path by adopting a speed decision method so as to complete the front-end trajectory planning and obtain an initial path.
Has the advantages that:
(1) when the method is used for optimizing the path, particularly considering the collision constraint of the obstacle, the convex polygon is used for representing the outlines of the robot and the obstacle, and the collision relation between the robot and the obstacle is converted into the judgment of the position relation of the point and the edge, so that the collision judgment is realized, and the scheme is simple and effective. According to the scheme, the outline and the shape of the mobile robot are fully considered, most potential safety hazards of the robot in the autonomous navigation process are avoided, the motion planning process and the motion planning result are safer, the collision risk is lower, and the autonomous navigation safety of the robot is improved. The method is a uniform, safe and efficient motion planning solution.
(2) The contour vertex of the robot is connected with the vertex of the convex polygon of the obstacle to obtain a plurality of triangles, and the area sum of the triangles is compared with the area of the convex polygon to judge whether the contour vertex of the robot is outside the convex polygon of the obstacle or not, so that the judgment process is simpler and more accurate, and the motion planning and optimization process is more efficient.
(3) The two-point boundary value constraint simultaneously considers the starting point constraint and the end point constraint, wherein the end point constraint simultaneously considers the problem of a motion path from the starting point to the end point, and the motion path is constrained and limited, so that the path planning and the track optimization of the robot can be completed more accurately and efficiently, and the collision risk of the mobile robot caused by the motion planning technology is further reduced.
Drawings
Fig. 1 is a schematic diagram of the vertex coordinates of a mobile robot.
FIG. 2 is a schematic diagram of the position relationship between convex polygons and dots.
Fig. 3 is a schematic diagram of a safe movement planning device module arrangement.
Detailed Description
The invention discloses a safe motion planning scheme based on a mobile robot outline, which has the core idea that: in the process of optimizing the path, the scheme of the invention sets the collision constraint condition of the barrier, expresses the outline of the robot as a convex polygon and expresses the outline of the barrier as more than one convex polygon; at any moment, each vertex of the robot convex polygon outline is positioned outside the obstacle convex polygon, and each vertex of the obstacle convex polygon is positioned outside the robot convex polygon outline; through the setting of the obstacle collision constraint condition, the outline and the shape of the mobile robot are fully considered in the path optimization scheme, most potential safety hazards of the robot in the autonomous navigation process are avoided, the motion planning process and the motion planning result are safer, the collision risk is lower, and the autonomous navigation safety of the robot is improved.
The invention is described in detail below by way of example with reference to the accompanying drawings.
The safe motion planning method based on the mobile robot outline of the embodiment of the invention specifically comprises the following steps:
the method comprises the following steps: and planning a track based on the existing environment map information and the robot positioning information to obtain an initial path.
The method comprises the following substeps:
step 1.1, a geometric path capable of avoiding obstacles is found by utilizing a path backtracking algorithm.
This step belongs to front-end geometric path decision. On the basis of the known environment map information and the robot positioning information (the starting point and the end point information), a geometric path which is free of collision and connects the starting point and the end point can be quickly planned by adopting path backtracking algorithms such as a graph search algorithm, a sampling algorithm and the like. Taking a graph search algorithm as an example, nodes are expanded outwards from a starting point, and a kinematic model of the mobile robot needs to be considered when the nodes are expanded until the expansion nodes contain end points.
Step 1.2, determining the running speed of the robot
In the step, dynamic barrier information in the environment is referenced, and a speed decision method is adopted to figure out the speed at which the robot runs along the geometric path, so that the front-end track planning is completed, and the initial path is obtained. For example, a graph search method may be used to find a function mapping relationship between distance and time, and then determine the speed at which the mobile robot moves along the current path from the starting point to the end point by the distance-time derivative. The method can determine the running speed of the robot.
And 1.3, storing the information such as the existing geometric path, speed and the like as initialization information for subsequent track optimization.
Step two: and (4) considering kinematic constraint, two-point boundary value constraint and manifold constraint of the robot, and performing path optimization based on the initial path obtained in the step one to complete the motion planning of the robot.
And 2.1, on the basis of completing the front-end trajectory planning in the first step, further considering the kinematic constraint, the two-point boundary value constraint, the manifold constraint, the objective function and other information of the mobile robot to construct a trajectory planning proposition of the mobile robot. Particularly obstacle constraints in manifold constraints.
(1) Kinematic constraint of mobile robot
The kinematic model of the mobile robot m can be expressed as:
in the formula: x is the number ofm(t) and ym(t) indicates positional information of the robot at time t. v. ofm(t) and amAnd (t) represents the speed and acceleration information of the robot moving at the moment t respectively, and the speed and the acceleration information take the robot to advance as a positive direction. ThetamAnd (t) the yaw angle information of the robot at the time t, wherein the yaw angle takes the anticlockwise rotation as the positive direction. Deltam(t) and ωm(t) represents the steering angle and steering angular velocity of the robot wheels at time t, respectively, both of which are positive directions with a left turn. Wherein s ism(t)＝[xm(t),ym(t),θm(t),vm(t),δm(t)]Represents the state quantity, mu, of the robot at time tm(t)＝[am(t),ωm(t)]Representing the control quantity of the robot at the time t. L ismThe wheelbase between the front and rear wheels of the robot. t is an element of 0, tf]Representing time, and ending time tfAnd may be constant or variable.
(2) Two-point boundary value constraint
The two-point boundary value constraint mainly includes a start point constraint (t ═ 0) and an end point constraint (t ═ t)f) Two parts.
Wherein the origin constraint may be expressed as:
wherein (x)m0,ym0) Indicating positional coordinate information of the robot at an initial time, thetam0Representing yaw direction information of the robot at the initial moment, vm0And deltam0Respectively, the speed and steering angle information of the robot at the initial moment.
xm(t) and ym(t) positional information of the robot at time t, vm(t) and am(t) respectively representing the speed and acceleration information of the robot moving at the moment t; thetam(t) is the yaw angle information of the robot at the time t; deltam(t) and ωm(t) denotes the steering angle and steering angular velocity of the robot wheels at time t, respectively. The above initial motion state of the robot can be obtained by the corresponding sensor.
The endpoint constraint can be expressed as:
Wherein x ism(t) and ym(t) positional information of the robot at time t, vm(t) and am(t) respectively representing the speed and acceleration information of the robot moving at the moment t; thetam(t) is the yaw angle information of the robot at the time t; deltam(t) and ωm(t) respectively representing the steering angle and the steering angular speed of the wheels of the robot at the moment t; t is tfIndicating a stop time; (x)mf,ymf) Position coordinate information representing the time at which the robot stops, and θmfAnd yaw direction information indicating a time when the robot stops. The state variable θ can be known from the formula (1)m(t) differentiable, the state variable is continuous. In the same initial time state, the same initial time state as the ending time state thetam(tf) End time state θ compared to 0m(tf) 2n pi means that the motion track of the robot needs to make n additional rounds. Therefore, the state variable θm(tf) The end point constraint of (a) is written as equation (3), so that it can be expressed that the specific motion path is also considered while the end point constraint is considered.
(3) Manifold constraint
Manifold constraints generally include extreme kinematic constraints of the robot, obstacle collision constraints, and the like. The obstacle collision constraint condition specifically comprises the following steps: representing the outline of the robot as a convex polygon and representing the outline of the obstacle as more than one convex polygon; at any moment, each vertex of the robot convex polygon outline is positioned outside the obstacle convex polygon, and each vertex of the obstacle convex polygon is positioned outside the robot convex polygon outline.
The extreme constraint of motion capability is expressed as:
wherein v ism(t) and am(t) represents the speed and acceleration of the robot movement at time t, respectivelyDegree information, δm(t) and ωm(t) respectively representing the steering angle and the steering angular speed of the wheels of the robot at the moment t; v. ofmaxIs the maximum value of the moving speed, delta, of the robot in the whole moving processmaxIs the maximum value which can be reached by the steering angle of the wheels of the robot in the whole moving process, amaxAnd ωmaxRespectively, the maximum values of the acceleration and the wheel steering angular velocity of the robot during the whole movement.
Restraint of obstacle collision:
robot collision detection constraint of the present invention: firstly, the robot is completely constrained in a polygonal outline on the basis of fully considering the safety of the robot. If the robot centroid coordinates at any time are known, the abscissa and ordinate of each vertex of the polygon outline can be obtained. During specific implementation, the outer contour is determined according to the shape of the robot and the specific shape of the robot. For the robot of fig. 1, the robot is constrained within a rectangular outline; in practice, the outer contour of the robot may be other shapes such as a 6-sided polygon.
In the formula: lf (t), rf (t), rr (t), lr (t) respectively indicate vertices of the rectangular outline in the counterclockwise direction, i.e., a left front vertex, a right rear vertex, and a left rear vertex. L is the length of the rectangular profile and W is the width of the rectangular profile.
For the obstacles in the two-dimensional coordinate environment, the number of the obstacles is generally multiple, and the obstacles can be represented as different numbers of polygons with different shapes by using algorithms such as clustering, minimum convex hull generation and the like. It is noted that polygons generally include two broad categories, namely: concave polygons and convex polygons. However, a concave polygon must be decomposed into a finite number of convex polygons.
The present embodiment will construct collision detection constraints using convex polygonal obstacles and robot rectangular contours. If the outer contour of the robot is in other shapes, the following method for constructing the collision detection constraint condition still applies.
Through geometric analysis, it can be known that the collision of the rectangle and the convex polygon only includes two cases, one is that the vertex of the rectangle collides with the outline of the convex polygon first, and the other is that the vertex of the convex polygon collides with the outline of the rectangle first. Therefore, if the mobile robot can guarantee that the following two conditions are satisfied at each time, it can be guaranteed that the robot does not collide with the obstacle at each time. The two conditions are:
1) at any time, the vertices of the rectangular outline of the robot are outside the convex polygon of the obstacle.
2) At any moment, the vertices of the convex polygon of the obstacle are outside the rectangular outline of the robot.
In the case that the coordinates of the robot contour vertex and the obstacle convex polygon vertex are known, the above two conditions can be summarized as a problem, namely: as shown in FIG. 2, the decision point pvWhether (x, y) is located in a convex polygon (vertex v)1…vn) Of the outer part of (1). An effective method is to determine whether a point is outside the polygon by calculating the area of the polygon. Particularly, a vertex p is determinedvIf it is outside the convex polygon T, using the vertex pvTowards each vertex V of the convex polygon T in turn1…VnConnecting lines are made, and n connecting lines form n triangles which are represented as pvVnV1、pvVkVk+1K is 1,2, …, n-1; if the total area of the n triangles is larger than the area of the convex polygon T, the vertex p is determinedvOutside the convex polygon T.
Expressed by the formula:
in the formula:
is represented by pn、vn、v1Triangles with verticesThe shape area of the utility model is as follows,
indicating that when k takes different values, corresponding to the area of the triangle,
representing the area of the convex polygon.
The area of any polygon is calculated by using a vector product method. To be provided with
For example, there are:
in summary, a complete obstacle collision avoidance constraint can be established, namely:
in the formula: j denotes an obstacle number. RobOutObs indicates that the vertex of the rectangular outline of the robot is positioned outside the convex polygon of the obstacle, and ObsOutRob indicates that the vertex of the convex polygon of the obstacle is positioned outside the rectangular outline of the robot.
(4) Objective function
In the path optimization process, the path quality is measured by weighting the corresponding passing time of the path, energy consumption and the smoothness of the path and is used as an optimized objective function. Expressed as:
Jm＝ρ1Jm1+ρ2Jm2+ρ3Jm3 (10)
in the formula: rho1、ρ2、ρ3Respectively corresponding to time Jm1Energy consumption Jm2Smoothness of trajectory Jm3And the weight coefficient of the performance index is equal. It should be noted here that the objective function is a comprehensive track quality measure including a plurality of indexes.However, only part of the indexes can be selected as the measurement standard on the premise of not having high requirements on some indexes of the track.
In summary, the trajectory planning optimal control problem can be described as;
the existing motion planning method based on the mobile robot contour only needs to solve a formula (11), so that the robot is enabled to be in a time domain t epsilon [0, t ∈ [ ]f]In, the control quantity mu is adoptedmAnd (t) minimizing the objective function (10) to obtain a safe and effective track.
And 2.2, optimizing the path based on the initial path obtained in the step one to complete the motion planning of the robot.
In the discrete module, for an optimal control problem, commonly used methods include a direct method, an analytic method, a dynamic programming method and the like. Since the analytical method is generally suitable for the case where the controlled variable is unconstrained or the controlled variable is a closed set, the above-mentioned optimal control problem cannot be well solved. The dynamic programming rule is not suitable for solving the optimal control proposition because of low processing constraint efficiency and low solving speed of the large-scale optimal control problem. The direct method is very suitable for solving the large-scale and constrained multi-optimal control problem, and therefore, the direct method can be used as a solving method of the optimal control proposition.
To solve the optimal control problem using the direct method, equation (10) is described as a more standard and uniform mathematical form, namely:
in the formula: t is an element of 0, tf]Time, s (t), μ (t) state quantities and control quantities of the robot, JNZFor the objective function of the optimal control problem, f is an algebraic part in a state equation, Γ represents a two-point boundary value constraint and a manifold constraint of the robot, and when Γ (s (t), and μ (t)) ═ 0, Γ representsTwo-point boundary value constraint, namely equation constraint formula (2) and formula (3); when Γ (s (t), μ (t)) < 0, manifold constraints, i.e., inequality constraints equation (4) and equation (9), are expressed.
To discretize the control variables and the state variables, the time t is first determinedfIs equally divided into NeqFinite elements of equal segment length, namely:
in the formula: 1,2, …, Neq，
For the start time of the planning of the trajectory,
for the end of the planning of the trajectory,
indicating the end time of the ith time segment.
Then, all finite elements are required
Re-representing the state quantity and the control quantity by adopting a Lagrange interpolation method, namely;
in the formula: tau is equal to [0,1 ∈]，
And K is a segmented interpolation order.
Lagrangian basis function, s, representing state variable s (t)i,jRepresents discrete configuration points for the state variables s (t).
Lagrangian basis function, μ (t), for the control variable μi,jAre discrete configuration points of the state variable μ (t).
Lagrange basis function
Can be described as:
in the formula: tau isk∈[0,1]Can determine finite elements
Relative position of upper configuration point.
In addition, since the state variable s (t) needs to be kept continuous, the following constraints need to be added at the connection point of each finite element to ensure the continuity of the state variable. Namely:
in the formula: 1,2, …, Neq-1，si+1,0Is the first configuration point, s, on the (i + 1) th finite elementi,KIs the K configuration point on the ith finite element.
In summary, substituting equations (13) - (16) into equation (12) can obtain a complete nonlinear programming problem. Namely:
in the formula: 1,2, …, Neq，ii＝1,2,…,Neq-1, j-0, 1, …, K being the duration of the finite element.
And in an iterative solution module, combining the track planning result in the step one and the nonlinear programming problem in the discrete module, substituting the known conditions into a nonlinear programming solver to solve, and thus, realizing the safe motion planning method based on the mobile robot contour.
As shown in fig. 3, a safe motion planning apparatus based on mobile robot contour includes an initial path planning unit and a path optimizing unit;
the initial path planning unit comprises a path decision module and a speed decision module and is used for planning a track based on the existing environment map information and the robot positioning information to obtain an initial path;
the path decision module is used for quickly planning a geometric path which is free of collision and connects a starting point and an end point through a graph searching and sampling algorithm and the like on the basis of the existing environment map information and the robot positioning information;
and the speed decision module is used for referring to dynamic barrier information in the environment and planning the speed at which the robot runs along the geometric path by adopting a speed decision method so as to complete the front-end trajectory planning and obtain an initial path.
The path optimization unit comprises a constraint condition processing module and an optimization module, and is used for considering kinematic constraint, two-point boundary value constraint and manifold constraint of the robot, optimizing the path based on the initial path and finishing the motion planning of the robot.
And the constraint condition processing module is used for constructing kinematic constraint, two-point boundary value constraint and manifold constraint. The two-point boundary value constraint used by the constraint condition processing module comprises a starting point constraint and an end point constraint; manifold constraints include obstacle collision constraints and motion capability extremum constraints. The constraint condition processing module is also constructed with an objective function, and in the process of path optimization, the path quality is measured by weighting the corresponding passing time of the path, the energy consumption and the smoothness of the path and is used as the optimized objective function.
Specifically, the kinematic constraint, the two-point boundary value constraint, the manifold constraint and the objective function constructed by the constraint condition processing module are the same as those of the constraint and objective function processing described in the method above, and are not described herein again.
And the optimization module is used for optimizing the initial path according to the constraint condition and the set objective function to complete the motion planning of the robot.
The above embodiments only describe the design principle of the present invention, and the shapes and names of the components in the description may be different without limitation. Therefore, a person skilled in the art of the present invention can modify or substitute the technical solutions described in the foregoing embodiments; such modifications and substitutions do not depart from the spirit and scope of the present invention.
Claims (10)
1. A safe motion planning method based on mobile robot outline is characterized by comprising the following steps:
planning a track based on the existing environment map information and the robot positioning information to obtain an initial path;
the kinematic constraint, the two-point boundary value constraint and the manifold constraint of the robot are considered, path optimization is carried out based on the initial path, and the motion planning of the robot is completed;
the manifold constraint includes an obstacle collision constraint condition, specifically: representing the outline of the robot as a convex polygon and representing the outline of the obstacle as more than one convex polygon; at any moment, each vertex of the robot convex polygon outline is positioned outside the obstacle convex polygon, and each vertex of the obstacle convex polygon is positioned outside the robot convex polygon outline.
2. The method of claim 1, wherein the vertices of the robot convex polygon outline are located outside the obstacle convex polygon, and the vertices of the obstacle convex polygon are located outside the robot convex polygon outline determined by:
let the vertex be pvThe convex polygon T is formed by vertexes V1…VnComposition is carried out; judging the vertex as pvThe way of whether it is outside the convex polygon T is: with the vertex pvTowards vertex V in sequence1…VnConnecting lines are made, and n connecting lines form n triangles which are represented as pvVnV1、pvVkVk+1K is 1,2, …, n-1; if the total area of the n triangles is larger than the area of the convex polygon T, the vertex p is determinedvOutside the convex polygon T.
3. The method of claim 1, wherein the two-point boundary value constraint comprises two parts, a start point constraint and an end point constraint:
the origin constraint is expressed as:
the endpoint constraint is expressed as:
Wherein x ism(t) and ym(t) positional information of the robot at time t, vm(t) and am(t) respectively representing the speed and acceleration information of the robot moving at the moment t; thetam(t) is the yaw angle information of the robot at the time t; deltam(t) and ωm(t) respectively representing the steering angle and the steering angular speed of the wheels of the robot at the moment t; t is tfIndicating a stop time; (x)m0,ym0) Indicating positional coordinate information of the robot at an initial time, thetam0Representing yaw direction information of the robot at the initial moment, vm0And deltam0Respectively representing the speed and the steering angle information of the robot at the initial moment; (x)mf,ymf) Position coordinate information representing the time at which the robot stops, and θmfAnd yaw direction information indicating a time when the robot stops.
4. The method of claim 1, wherein the manifold constraints further comprise a motion capability extremum constraint:
wherein v ism(t) and am(t) represents the velocity and acceleration information, respectively, of the robot movement at time t, δm(t) and ωm(t) respectively representing the steering angle and the steering angular speed of the wheels of the robot at the moment t; v. ofmaxIs the maximum value of the moving speed, delta, of the robot in the whole moving processmaxIs the maximum value which can be reached by the steering angle of the wheels of the robot in the whole moving process, amaxAnd ωmaxRespectively, the maximum values of the acceleration and the wheel steering angular velocity of the robot during the whole movement.
5. The method of claim 1, wherein the trajectory planning is performed based on the existing environment map information and the robot positioning information, and the initial path is obtained by: planning a geometric path which is free of collision and connects a starting point and a terminal point through a path backtracking algorithm on the basis of the existing environment map information and the robot positioning information; and then, referring to dynamic barrier information in the environment, and planning the speed at which the robot runs along the geometric path by adopting a speed decision method, thereby completing the front-end trajectory planning and obtaining an initial path.
6. The method of claim 1, wherein in the path optimization process, path quality is measured by weighting and weighting the trajectory with respect to transit time, energy consumption, trajectory smoothness, and as an objective function of optimization.
7. A safe motion planning device based on a mobile robot outline is characterized by comprising an initial path planning unit and a path optimizing unit;
the initial path planning unit is used for planning a track based on the existing environment map information and the robot positioning information to obtain an initial path;
the path optimization unit is used for considering kinematic constraint, two-point boundary value constraint and manifold constraint of the robot, optimizing a path based on the initial path and finishing the motion planning of the robot;
the manifold constraint includes an obstacle collision constraint condition, specifically: representing the contour of the robot as a convex polygon, and representing the contour of the obstacle as more than one convex polygon; at any moment, each vertex of the robot convex polygon outline is positioned outside the obstacle convex polygon, and each vertex of the obstacle convex polygon is positioned outside the robot convex polygon outline.
8. The apparatus of claim 7, wherein the path optimization unit comprises a constraint processing module and an optimization module;
the constraint condition processing module is used for processing kinematic constraint, two-point boundary value constraint and manifold constraint; judging a vertex p according to the collision constraint condition of the barriervIf it is outside the convex polygon T, using the vertex pvTowards each vertex V of the convex polygon T in turn1…VnConnecting lines are made, and n connecting lines form n triangles which are represented as pvVnV1、pvVkVk+1K is 1,2,. cndot.n-1; if the total area of the n triangles is larger than the area of the convex polygon T, the vertex p is determinedvOutside the convex polygon T;
and the optimization module is used for optimizing the initial path according to the constraint condition and the set objective function to complete the motion planning of the robot.
9. The apparatus of claim 8, wherein the two-point boundary value constraint used by the constraint processing module comprises two parts, a start point constraint and an end point constraint:
the origin constraint is expressed as:
the endpoint constraint is expressed as:
Wherein x ism(t) and ym(t) positional information of the robot at time t, vm(t) and am(t) respectively representing the speed and acceleration information of the robot moving at the moment t; thetam(t) is the yaw angle information of the robot at the time t; deltam(t) and ωm(t) respectively representing the steering angle and the steering angular speed of the wheels of the robot at the moment t; t is tfIndicating a stop time; (x)m0,ym0) Indicating positional coordinate information of the robot at an initial time, thetam0Representing yaw direction information of the robot at the initial moment, vm0And deltam0Respectively representing the speed and the steering angle information of the robot at the initial moment; (x)mf,ymf) Position coordinate information representing the time at which the robot stops, and θmfYaw direction information indicating a stopping time of the robot;
the manifold constraints further include a motion capability extremum constraint:
wherein v ismaxIs the maximum value of the moving speed, delta, of the robot in the whole moving processmaxIs the maximum value which can be reached by the steering angle of the wheels of the robot in the whole moving process, amaxAnd ωmaxRespectively, the maximum values of the acceleration and the wheel steering angular velocity of the robot during the whole movement.
10. The apparatus of claim 7, wherein the initial path planning unit comprises a path decision module and a speed decision module;
the path decision module is used for quickly planning a geometric path which is free of collision and connects a starting point and an end point through a graph searching algorithm, a sampling algorithm and the like on the basis of the existing environment map information and the robot positioning information;
and the speed decision module is used for referring to dynamic barrier information in the environment and planning the speed at which the robot runs along the geometric path by adopting a speed decision method, so that the front-end trajectory planning is completed and the initial path is obtained.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110647515.0A CN113741414A (en) | 2021-06-08 | 2021-06-08 | Safe motion planning method and device based on mobile robot contour |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110647515.0A CN113741414A (en) | 2021-06-08 | 2021-06-08 | Safe motion planning method and device based on mobile robot contour |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN113741414A true CN113741414A (en) | 2021-12-03 |
Family
ID=78728443
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110647515.0A Pending CN113741414A (en) | 2021-06-08 | 2021-06-08 | Safe motion planning method and device based on mobile robot contour |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113741414A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116061178A (en) * | 2022-12-28 | 2023-05-05 | 墨影科技(南京)有限公司 | Control method of feeding and discharging robot based on path planning |
Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010039839A (en) * | 2008-08-06 | 2010-02-18 | Ihi Corp | Mobile robot and travel speed control method for mobile robot |
| CN108120442A (en) * | 2017-12-12 | 2018-06-05 | 北京理工大学 | A kind of multi-rotor unmanned aerial vehicle flight path generation method based on Second-order cone programming |
| CN108955695A (en) * | 2018-08-22 | 2018-12-07 | 洛阳中科龙网创新科技有限公司 | A kind of global path planning method for farmland robot |
| CN111562787A (en) * | 2020-05-28 | 2020-08-21 | 长沙中联重科环境产业有限公司 | Robot full-coverage path planning region dividing method, device, medium and equipment |
| CN112161622A (en) * | 2020-08-28 | 2021-01-01 | 深圳市优必选科技股份有限公司 | Robot footprint planning method and device, readable storage medium and robot |
-
2021
- 2021-06-08 CN CN202110647515.0A patent/CN113741414A/en active Pending
Patent Citations (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| JP2010039839A (en) * | 2008-08-06 | 2010-02-18 | Ihi Corp | Mobile robot and travel speed control method for mobile robot |
| CN108120442A (en) * | 2017-12-12 | 2018-06-05 | 北京理工大学 | A kind of multi-rotor unmanned aerial vehicle flight path generation method based on Second-order cone programming |
| CN108955695A (en) * | 2018-08-22 | 2018-12-07 | 洛阳中科龙网创新科技有限公司 | A kind of global path planning method for farmland robot |
| CN111562787A (en) * | 2020-05-28 | 2020-08-21 | 长沙中联重科环境产业有限公司 | Robot full-coverage path planning region dividing method, device, medium and equipment |
| CN112161622A (en) * | 2020-08-28 | 2021-01-01 | 深圳市优必选科技股份有限公司 | Robot footprint planning method and device, readable storage medium and robot |
Non-Patent Citations (2)
| Title |
|---|
| JIE LIU 等: "Research on Cooperative Trajectory Planning and Tracking Problem for Multiple Carrier Aircraft on the Deck", IEEE SYSTEMS JOURNAL, vol. 14, no. 02, pages 3027 - 3038, XP011790958, DOI: 10.1109/JSYST.2019.2932783 * |
| 黎自强 等: "一种新的凸多边形不干涉算法", 计算机工程与应用, vol. 44, no. 01, pages 11 - 13 * |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN116061178A (en) * | 2022-12-28 | 2023-05-05 | 墨影科技(南京)有限公司 | Control method of feeding and discharging robot based on path planning |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111971574B (en) | Deep learning based feature extraction for LIDAR localization of autonomous vehicles | |
| CN110320933B (en) | Unmanned aerial vehicle obstacle avoidance movement planning method under cruise task | |
| CN111771141B (en) | LIDAR positioning for solution inference using 3D CNN network in autonomous vehicles | |
| CN109885883B (en) | Unmanned vehicle transverse motion control method based on GK clustering algorithm model prediction | |
| CN107702716B (en) | Unmanned driving path planning method, system and device | |
| CN111771135B (en) | LIDAR positioning using RNN and LSTM for time smoothing in autonomous vehicles | |
| CN111427370B (en) | Sparse pose adjustment-based Gmapping mapping method for mobile robot | |
| Xiong et al. | Application improvement of A* algorithm in intelligent vehicle trajectory planning | |
| CN108121205A (en) | A kind of paths planning method, system and medium for a variety of scenes of parking | |
| CN111142091B (en) | Automatic driving system laser radar online calibration method fusing vehicle-mounted information | |
| CN111707272A (en) | Underground garage automatic driving laser positioning system | |
| CN112269965A (en) | Continuous curvature path optimization method under incomplete constraint condition | |
| CN107357168B (en) | Unmanned vehicle obstacle avoidance method based on opportunity constraint model predictive control | |
| CN112578673B (en) | Perception decision and tracking control method for multi-sensor fusion of formula-free racing car | |
| CN113701780A (en) | Real-time obstacle avoidance planning method based on A-star algorithm | |
| CN117222915A (en) | System and method for tracking an expanded state of a moving object using a composite measurement model | |
| CN113741414A (en) | Safe motion planning method and device based on mobile robot contour | |
| Wang et al. | Research on AGV task path planning based on improved A* algorithm | |
| Zhao et al. | Adaptive non-linear joint probabilistic data association for vehicle target tracking | |
| CN111123953A (en) | Particle-based mobile robot group under artificial intelligence big data and control method thereof | |
| Huang et al. | B-splines for purely vision-based localization and mapping on non-holonomic ground vehicles | |
| CN117055556A (en) | Multi-robot formation path planning method and device, electronic equipment and storage medium | |
| Song et al. | Research on Target Tracking Algorithm Using Millimeter‐Wave Radar on Curved Road | |
| CN113379915B (en) | Driving scene construction method based on point cloud fusion | |
| Wang et al. | A Path Planning Framework Based on an Improved Weighted Heuristic RRT and Optimization Strategy |
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 |