CN117369467A - Spline curve optimization-based robot track optimization method and device - Google Patents

Spline curve optimization-based robot track optimization method and device Download PDF

Info

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
Application number
CN202311447483.5A
Other languages
Chinese (zh)
Inventor
王振斌
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Yijiahe Technology R & D Co ltd
Original Assignee
Shenzhen Yijiahe Technology R & D Co ltd
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Yijiahe Technology R & D Co ltd filed Critical Shenzhen Yijiahe Technology R & D Co ltd
Priority to CN202311447483.5A priority Critical patent/CN117369467A/en
Publication of CN117369467A publication Critical patent/CN117369467A/en
Pending legal-status Critical Current

Links

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

Spline curve optimization-based robot track optimization method and device
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 s2 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 s2 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 s2 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.
CN202311447483.5A 2023-10-31 2023-10-31 Spline curve optimization-based robot track optimization method and device Pending CN117369467A (en)

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)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117705123A (en) * 2024-02-01 2024-03-15 戴盟(深圳)机器人科技有限公司 Track planning method, device, equipment and storage medium

Cited By (2)

* Cited by examiner, † Cited by third party
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