CN117369467A - Spline curve optimization-based robot track optimization method and device - Google Patents
Spline curve optimization-based robot track optimization method and device Download PDFInfo
- Publication number
- CN117369467A CN117369467A CN202311447483.5A CN202311447483A CN117369467A CN 117369467 A CN117369467 A CN 117369467A CN 202311447483 A CN202311447483 A CN 202311447483A CN 117369467 A CN117369467 A CN 117369467A
- Authority
- CN
- China
- Prior art keywords
- path
- track
- robot
- local
- planning
- 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 44
- 238000005457 optimization Methods 0.000 title claims description 33
- 238000009499 grossing Methods 0.000 claims description 8
- 230000000903 blocking effect Effects 0.000 claims description 4
- 239000011159 matrix material Substances 0.000 claims description 4
- 238000004140 cleaning Methods 0.000 abstract description 3
- 230000008447 perception Effects 0.000 abstract description 3
- 238000012216 screening Methods 0.000 abstract 2
- 230000004888 barrier function Effects 0.000 abstract 1
- 230000002498 deadly effect Effects 0.000 abstract 1
- 238000013507 mapping Methods 0.000 abstract 1
- 238000007781 pre-processing Methods 0.000 abstract 1
- 230000006870 function Effects 0.000 description 11
- 230000009286 beneficial effect Effects 0.000 description 2
- 230000001133 acceleration Effects 0.000 description 1
- 238000010276 construction Methods 0.000 description 1
- 230000036461 convulsion Effects 0.000 description 1
- 125000004122 cyclic group Chemical group 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000003825 pressing Methods 0.000 description 1
Landscapes
- Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
Abstract
The invention relates to and discloses a local environment perception method and a device of an indoor walking intelligent body, wherein the method comprises the following steps: collecting point cloud data and photos of the current environment of the walking intelligent body in real time; preprocessing and screening point clouds of key intervals; identifying and obtaining a dynamic obstacle point cloud; screening out important barriers of a key scene; determining a life cycle of the three-dimensional voxels; determining the time for clearing the three-dimensional voxels according to the life cycle; mapping the three-dimensional voxels to the existing two-dimensional grid map to form a deadly obstacle, and obtaining an updated grid map after expansion for the cleaning robot to carry out path planning and control. According to the invention, the life cycle of the voxels is determined based on the obstacles with different movement characteristics, the self-adaptive view model is designed, different requirements of the inside and outside of the view on cleaning are realized, the formulated perception strategy is closer to the real environment, and the path planned by the robot is more reasonable.
Description
Technical Field
The invention belongs to the technical field of mobile robots, and particularly relates to a spline curve optimization-based robot track optimization method and device.
Background
In a complex environment, the mobile cleaning robot needs to dynamically plan a path in real time under the conditions of obstacles, narrow channels or crowds and the like, so that the robot can quickly and safely adjust the path to smoothly pass through the obstacles, the narrow channels, the crowds and the like. At present, the track precision and the track speed of the robot planning are smooth, and a lifting space exists, so that the phenomena of unreasonable planning paths, larger track tracking errors, unsmooth speed and the like can occur particularly in a complex environment.
Disclosure of Invention
The technical purpose is that: in order to solve the problem of path planning of a robot in a complex environment, the invention provides a method and a device for optimizing a robot path based on spline curve optimization, which can improve the instantaneity, the safety and the smoothness of the path planning of a mobile robot.
The technical scheme is as follows: in order to achieve the technical purpose, the invention adopts the following technical scheme:
a robot track optimization method based on spline curve optimization comprises the following steps:
(1) Global planning, namely planning a global path, namely a movable path from a starting point to an end point by using a hybird A algorithm;
(2) The method comprises the steps of locally planning, wherein in the process that a robot walks along the global path, a local sensing device arranged on the robot is used for detecting the surrounding environment in real time, and when a blocking obstacle appears on the global path, a local path bypassing the obstacle is planned again in real time by utilizing a hybird A algorithm;
(3) Track planning, wherein a robot walks along the local path, and in the walking process, a cubic B spline curve algorithm and an L-BFGS iterative algorithm are used for carrying out smoothing and optimizing treatment on the local planning path to obtain a local track, and the robot walks along the local track;
(4) Tracking the track, namely tracking the real-time walking track of the robot by using a pure tracking method, returning to the step (2) and re-planning the local path if an obstacle appears in the walking process of the robot.
Preferably, the step (3) specifically includes:
parameterizing the local path as a cubic uniform B-spline;
for the cubic uniform B-spline, n+1 control points { P 0 ,P 1 ,…,P n And corresponding node vector t 0 ,t 1 ,…,t n+1+k ]Determining kB-spline curves;
optimizing the control point { P } 0 ,P 1 ,…,P n Control point { P }, get k ,P k+1 ,…,P n-k And optimizing an objective function by using a Newton method to obtain a local track.
Preferably, the step (3) specifically includes the steps of:
the collision item optimization, namely selecting a point on the local path at a certain interval, wherein the point corresponds to a vector P of a projection point on the original path pointing to the point, and pushing away along the direction of the vector P when the distance between the path planned by A and the obstacle is smaller than the safety distance;
track smoothness optimization, namely, minimizing secondary and tertiary conduction of b spline track control points, and realizing reduction of overall curvature of the track;
and (3) optimizing the numerical value, namely optimizing an objective function by using an L-BFGS iterative algorithm, and adjusting the track control points to obtain the track with smooth collision or not as a local track.
Preferably, in the step (3), the objective function is:
f total ＝σ 1 f s +σ 2 f c
wherein f s Representing the smoothing cost, f c Representing the cost of the collision.
Preferably, in the step (3), the LGBFS iterative algorithm achieves a relatively accurate estimate through a series of iterations, as follows:
wherein: x is x k+1 The value of the parameter obtained after the (k+1) th iteration; x is x k To represent the parameter value of the kth iteration; i.e. the current parameter vector, a k A step size representing the kth iteration; h k An inverse of the Hessian matrix representing the kth iteration;representing the parameter value x k Gradient vector at.
A spline curve optimization-based robot trajectory optimization device, comprising:
the global planning module is used for planning a global path, namely a movable path from a starting point to an end point by using a hybird A algorithm;
a local path planning module comprising: the path planning unit is used for detecting the surrounding environment in real time through a local sensing device arranged on the robot in the process that the robot walks along the global path, and when the blocking obstacle appears on the global path, the local path bypassing the obstacle is re-planned in real time by utilizing a hybird A algorithm; the track planning unit is used for smoothing and optimizing the local planning path by using a cubic B spline curve algorithm and an L-BFGS iterative algorithm in the process of the robot walking along the local path to obtain a local track;
the track tracking module is used for tracking the real-time walking track of the robot by using a pure tracking method, and returning to the local path planning module to plan the local path again if an obstacle appears in the walking process of the robot.
The beneficial effects are that: compared with the prior art, the invention has the following beneficial effects:
the method and the system can improve the real-time performance, the safety and the track smoothness of the path planning of the mobile robot.
Drawings
Figure 1 is a flow chart of a method of robot trajectory optimization based on spline curve optimization,
fig. 2 is a flow chart of the algorithm of hybird a;
FIG. 3 is a schematic diagram of an optimized trajectory.
Detailed Description
Embodiments of the present invention will be described in detail below with reference to the accompanying drawings.
The invention provides a robot track optimization method based on spline curve optimization.
1. Introduction of software architecture and local planning implementation principle
As shown in fig. 1, the global path planning is to use hybird a rule to draw a path from a starting point to an end point. The path planning of the local planning is the safety guarantee of the robot in the process of tracking the global path, the surrounding environment of the robot is detected in real time by utilizing the local perception, and when the global path is monitored to be blocked by an obstacle, the path bypassing the obstacle is re-planned in real time by utilizing the hybird A algorithm. The trajectory planning is to smooth the path by utilizing a cubic B spline curve, and the smoothness and the safety of the trajectory are improved by a Newton method. Trajectory tracking is the robot tracking of local trajectories using pure tracking algorithms.
2. Local planning implementation technical principle
The local planning plays an important role in planning a control system, and the invention plans a local path which meets the constraint of kinematics and dynamics, is smooth and far away from an obstacle, and deviates from a global path as little as possible for a controller to track according to real-time local dynamic map information.
When the robot tracking global path encounters an obstacle, the obstacle detouring path planned by the hybird a algorithm is parameterized as a uniform B-spline, since the B-spline curve has a continuous, smooth characteristic. And obtaining a safety path far away from the obstacle and meeting kinematic constraint through gradient information and the curvature constraint optimization path. The minimum snap principle is utilized to complete speed planning to obtain a track which can be tracked by the controller;
2.1, hybird a principle of algorithm
The hybird a algorithm may plan an initial path around the obstacle, as shown in fig. 2, with the following pseudocode implemented:
the method comprises the steps of initializing a queue to be searched and an expanded queue, and storing unexpanded nodes into an open_set of the queue to be searched;
the value h (n) of the heuristic search function with respect to the node n is preset;
the initial point in the queue to be searched is X_s;
setting a cost function g (x_s) =0 reaching an initial point, and setting the cost g (n) reaching other nodes in the graph to infinity;
and (3) circulation:
if the queue open_set to be searched is empty, returning to FALSE; ending execution;
removing a node 'n' with a minimum cost function f (n) =g (n) +h (n) in the queue open_set to be searched;
marking node "n" as used and adding to the close_set queue;
if the node 'n' is the end point or the current node can directly pass through by utilizing the dubin curve, the parent node is gradually tracked from the end point until the start point is found. Returning TRUE; ending execution;
the neighbor node "m" of the cyclic expansion node "n" that is not searched:
if g (m) =infinity
g (m) =g (n) +cnm, cnm being the cost of node "n" reaching "m";
pressing m nodes into an open_set path queue;
if g (m) > g (n) +Cnm;
g(m)＝g(n)+Cnm；
the cycle is ended;
the cycle is ended;
2.2, cubic homogeneous B-spline
The cubic B spline curve is one of the common curve interpolation methods, has the characteristics of smoothness, continuity, convex hull property, adjustability and the like, and can be used for generating high-quality paths and tracks. Given n+1 control points P 0 ,P 1 ,…,P n And corresponding node vector t 0 ,t 1 ,…,t n+1+k ]The k-th order B-spline curve can be determined as follows:
p(t)＝s(t) T M k q m
wherein: s (t) = [1, t 2 ,…,t k ],t∈[t k ,t n+1 ]，M k Represents a constant matrix of order k+1, q m ＝[P m-k P m-k+1 P m-k+2 …P m ]，m∈[k,n+1]。
The b-spline curve shape is determined by the control points, i.e. the shape of the path is controlled.
2.3 track optimization
In order to ensure the state constraint of the track boundary, the ending control point of the path is unchanged, and the optimized control point is { P } k， P k+1 ，…，P n-k }. According to the method, the objective function is optimized by utilizing the Newton method, and the local path planning track is obtained. The overall objective function of trajectory optimization is:
f total ＝σ 1 f s +σ 2 f c
wherein: f (f) s Representing the smoothing cost, f c Representing collision cost, sigma 1 Weight coefficient, sigma, representing smoothing cost 2 A weight coefficient representing the collision cost.
2.3.1 Collision term optimization
As shown in fig. 3, the blue path is a global path, the red path is a hybird a path, and a point is selected from the hybird a path at a certain interval, and corresponds to a vector P pointing to the point at a projection point on the global path. Planned obstacle detouring path and obstacle distance d in hybird a i Less than the safety distance df, the push away is along the direction of vector p.
Wherein: d, d i Representing the distance between the ith point on the hybird A planning path and the obstacle;
j c the cost value of the collision distance of the i-th point is expressed and used for measuring whether collision danger exists.
2.3.2 track smoothness optimization
The track of the B spline curve is called convex hull property in the polygonal range of the control point connecting line, and can be used for ensuring the safety and feasibility of the track. Minimizing the secondary and tertiary leads of the b-spline track control points can achieve the reduction of the overall curvature of the track, and the formula is as follows:
A i represents the i-th point acceleration, J i Indicating the i-th point jerk.
2.3.3 numerical optimization
Objective function f total After the construction is completed, the invention uses the Newton iteration method of the L-BFGS iteration algorithm to utilize the second derivative information of the objective function, quickly converges to the minimum point of the objective function, and optimizes a smooth collision-free path. The L-BFGS requires a series of iterations to achieve a relatively accurate estimate, as follows:
wherein: x is x k+1 The value of the parameter obtained after the (k+1) th iteration; x is x k To represent the parameter value of the kth iteration; i.e. the current parameter vector, a k A step size representing the kth iteration; h k An inverse of the Hessian matrix representing the kth iteration;representing the parameter value x k Gradient vector at.
The three optimization modes can be independently carried out and do not interfere with each other. An example of an optimization trajectory achieved by the method of the present invention is shown in fig. 3.
The foregoing description is merely illustrative of the preferred embodiments of the present invention, and it should be noted that the scope of the present invention is not limited thereto, and any person skilled in the art should be able to substitute or change the technical solution according to the present invention and the inventive concept thereof within the scope of the present invention.
Claims (6)
1. The robot track optimizing method based on spline curve optimization is characterized by comprising the following steps:
(1) Global planning, namely planning a global path, namely a movable path from a starting point to an end point by using a hybird A algorithm;
(2) The method comprises the steps of locally planning, wherein in the process that a robot walks along the global path, a local sensing device arranged on the robot is used for detecting the surrounding environment in real time, and when a blocking obstacle appears on the global path, a local path bypassing the obstacle is planned again in real time by utilizing a hybird A algorithm;
(3) Track planning, wherein a robot walks along the local track, and in the walking process, a cubic B spline curve algorithm and a Newton method are used for carrying out smoothing and optimizing treatment on the local planning track to obtain a local track, and the robot walks along the local track;
(4) Tracking the track, namely tracking the real-time walking track of the robot by using a pure tracking method, returning to the step (2) and re-planning the local path if an obstacle appears in the walking process of the robot.
2. The method for optimizing a robot trajectory based on spline optimization of claim 1, wherein the step (3) specifically comprises:
parameterizing the local path as a cubic uniform B-spline;
for the cubic uniform B-spline, n+1 control points { P 0 ,P 1 ,…,P n And corresponding node vector t 0 ,t 1 ,…,t n+1+k ]Determining a k-order B spline curve;
optimizing the control point { P } 0 ,P 1 ,…,P n Control point { P }, get k ,P k+1 ,…,P n-k And optimizing an objective function by using a Newton method to obtain a local track.
3. The method for optimizing a robot trajectory based on spline optimization according to claim 2, wherein the step (3) specifically comprises the steps of:
the collision item optimization, namely selecting a point on the local path at a certain interval, wherein the point corresponds to a vector P of a projection point on the original path pointing to the point, and pushing away along the direction of the vector P when the distance between the path planned by A and the obstacle is smaller than the safety distance;
track smoothness optimization, namely, minimizing secondary and tertiary conduction of b spline track control points, and realizing reduction of overall curvature of the track;
and (3) optimizing the numerical value, namely optimizing an objective function by using an L-BFGS iterative algorithm, and adjusting the track control points to obtain the track with smooth collision or not as a local track.
4. The method for optimizing a robot trajectory based on spline optimization of claim 3, wherein in the step (3), the objective function is:
f total ＝σ 1 f s +σ 2 f c
wherein f s Representing the smoothing cost, f c Representing the cost of the collision.
5. A method for optimizing a robot trajectory based on spline optimization according to claim 3, wherein in the step (3), the LGBFS iterative algorithm achieves a relatively accurate estimate through a series of iterations, the formula is as follows:
wherein: x is x k+1 The value of the parameter obtained after the (k+1) th iteration; x is x k To represent the parameter value of the kth iteration; i.e. the current parameter vector, a k A step size representing the kth iteration; h k An inverse of the Hessian matrix representing the kth iteration;representing the parameter value x k Gradient vector at.
6. The utility model provides a robot orbit optimizing device based on spline curve optimization which characterized in that includes:
the global planning module is used for planning a global path, namely a movable path from a starting point to an end point by using a hybird A algorithm;
a local path planning module comprising: the path planning unit is used for detecting the surrounding environment in real time through a local sensing device arranged on the robot in the process that the robot walks along the global path, and when the blocking obstacle appears on the global path, the local path bypassing the obstacle is re-planned in real time by utilizing a hybird A algorithm; the track planning unit is used for smoothing and optimizing the local planning path by using a cubic B spline curve algorithm and an L-BFGS iterative algorithm in the process of the robot walking along the local path to obtain a local track;
the track tracking module is used for tracking the real-time walking track of the robot by using a pure tracking method, and returning to the local path planning module to plan the local path again if an obstacle appears in the walking process of the robot.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311447483.5A CN117369467A (en) | 2023-10-31 | 2023-10-31 | Spline curve optimization-based robot track optimization method and device |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202311447483.5A CN117369467A (en) | 2023-10-31 | 2023-10-31 | Spline curve optimization-based robot track optimization method and device |
Publications (1)
| Publication Number | Publication Date |
|---|---|
| CN117369467A true CN117369467A (en) | 2024-01-09 |
Family
ID=89389072
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202311447483.5A Pending CN117369467A (en) | 2023-10-31 | 2023-10-31 | Spline curve optimization-based robot track optimization method and device |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117369467A (en) |
Cited By (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117705123A (en) * | 2024-02-01 | 2024-03-15 | 戴盟(深圳)机器人科技有限公司 | Track planning method, device, equipment and storage medium |
-
2023
- 2023-10-31 CN CN202311447483.5A patent/CN117369467A/en active Pending
Cited By (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN117705123A (en) * | 2024-02-01 | 2024-03-15 | 戴盟(深圳)机器人科技有限公司 | Track planning method, device, equipment and storage medium |
| CN117705123B (en) * | 2024-02-01 | 2024-04-09 | 戴盟(深圳)机器人科技有限公司 | Track planning method, device, equipment and storage medium |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109782779B (en) | AUV path planning method in ocean current environment based on population hyperheuristic algorithm | |
| Gao et al. | Gradient-based online safe trajectory generation for quadrotor flight in complex environments | |
| CN109597864B (en) | Method and system for real-time positioning and map construction of ellipsoid boundary Kalman filtering | |
| CN117369467A (en) | Spline curve optimization-based robot track optimization method and device | |
| CN112432648A (en) | Real-time planning method for safe motion trail of mobile robot | |
| CN103901891A (en) | Dynamic particle tree SLAM algorithm based on hierarchical structure | |
| CN113110455A (en) | Multi-robot collaborative exploration method, device and system for unknown initial state | |
| CN117270545B (en) | Convolutional neural network-based substation wheel type inspection robot and method | |
| CN115407352A (en) | Robot positioning and mapping system and method based on particle filtering and mapping optimization | |
| CN115356991A (en) | Multi-target motion fusion control method and system based on motion state optimization | |
| CN114596360A (en) | Double-stage active instant positioning and graph building algorithm based on graph topology | |
| CN117705123B (en) | Track planning method, device, equipment and storage medium | |
| CN111596668A (en) | Mobile robot anthropomorphic path planning method based on reverse reinforcement learning | |
| Yevsieiev et al. | Building a traffic route taking into account obstacles based on the A-star algorithm using the python language | |
| CN116136417B (en) | Unmanned vehicle local path planning method facing off-road environment | |
| Cheng et al. | Visibility optimization using variational approaches | |
| CN116009558A (en) | Mobile robot path planning method combined with kinematic constraint | |
| CN116817913A (en) | New path planning method utilizing turning penalty factors and twin road network improvement | |
| CN116604557A (en) | Double-arm robot cooperative collision avoidance planning optimization method in unstructured environment | |
| CN116466701A (en) | Unmanned surface vessel energy consumption minimum track planning method | |
| Deng et al. | Robotic manipulation planning using dynamic RRT | |
| CN114815899A (en) | Unmanned aerial vehicle three-dimensional space path planning method based on 3D laser radar sensor | |
| Zhang et al. | FGIP: A Frontier-Guided Informative Planner for UAV Exploration and Reconstruction | |
| Yang et al. | Sampling-efficient path planning and improved actor-critic-based obstacle avoidance for autonomous robots | |
| CN117733308B (en) | Ultrasonic welding robot path planning method and device |
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 |