CN116263605B - Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization - Google Patents
Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization Download PDFInfo
- Publication number
- CN116263605B CN116263605B CN202310432909.3A CN202310432909A CN116263605B CN 116263605 B CN116263605 B CN 116263605B CN 202310432909 A CN202310432909 A CN 202310432909A CN 116263605 B CN116263605 B CN 116263605B
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- track
- time
- max
- real
- 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.)
- Active
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 46
- 238000000034 method Methods 0.000 title claims abstract description 28
- 230000001133 acceleration Effects 0.000 claims description 14
- 238000009499 grossing Methods 0.000 claims description 7
- 230000003068 static effect Effects 0.000 claims description 6
- 238000001514 detection method Methods 0.000 claims description 3
- 239000011159 matrix material Substances 0.000 claims description 3
- 230000017105 transposition Effects 0.000 claims description 3
- 230000006870 function Effects 0.000 description 17
- 238000010586 diagram Methods 0.000 description 7
- 230000009977 dual effect Effects 0.000 description 3
- 238000004088 simulation Methods 0.000 description 2
- 241000282412 Homo Species 0.000 description 1
- 241000282414 Homo sapiens Species 0.000 description 1
- 238000013473 artificial intelligence Methods 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000005070 sampling Methods 0.000 description 1
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/0214—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory in accordance with safety or protection criteria, e.g. avoiding hazardous areas
-
- 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
-
- 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/0257—Control of position or course in two dimensions specially adapted to land vehicles using a radar
-
- 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/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
- G05D1/0274—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means using mapping information stored in a memory device
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
- Y02T10/00—Road transport of goods or passengers
- Y02T10/10—Internal combustion engine [ICE] based vehicles
- Y02T10/40—Engine management systems
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Aviation & Aerospace Engineering (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 smooth and real-time collision avoidance method of a mobile robot based on nonlinear optimization. Firstly, inputting necessary information; then generating an initial track, simulating the motion of the mobile robot by using a local path planning module and a tracking control module for the target point position, and further generating the initial track; then optimizing the nonlinear track at the rear end to obtain an optimal track; and finally, inputting the optimal track into a control module of the mobile robot, and updating the motion route of the mobile robot in real time. The invention can greatly reduce the scale of nonlinear optimization problem, the initial track can be converged to a smooth, safe and dynamic feasible track in real time, the obtained optimal track can maximize the probability of reaching the target point, the complexity of the optimization problem is greatly reduced, and the requirements of smoothness and instantaneity are met.
Description
Technical Field
The invention relates to a robot movement control method in the field of mobile robots, in particular to a mobile robot smooth and real-time collision avoidance method based on nonlinear optimization.
Background
With the continuous development of artificial intelligence and mobile robot technology, it has been a trend that mobile robots gradually replace human beings to complete complex and heavy work. There is an increasing demand for autonomous mobile robots capable of safe movement in high dynamic environments and performing complex tasks by humans, typical applications such as unmanned automobiles, home service mobile robots, automated guided vehicles, etc. In these applications, it is a key difficulty to design a safe, smooth, real-time collision avoidance method, that is, on the premise of collision avoidance, to make the mobile robot move rapidly and stably in a dynamic environment. Autonomous mobile robots often carry heavy or sensitive loads, so mobile robots need to have accurate collision avoidance capabilities. The collision avoidance algorithm should consider constraints such as kinematics and safety of the mobile robot, and plan the optimal smooth track in real time. It should be noted that, the track is based on the path, and time sequence information is added, that is, speed planning is performed based on the path planning. Only then can the underlying control module accurately track the nominal trajectory output by the planning module.
Conventional path planning methods, such as Dijkstra and a based on graph search, and probabilistic road graph and fast-expansion random tree based on sampling, find the optimal path (usually the shortest path) under the condition of a known map, but these shortest paths cannot meet the smoothness requirement. The ROS navigation pack now in widespread use contains a time elastic band method (TEB). The TEB adds time information to the path in consideration of constraints such as incomplete kinematics and dynamics, but in order to improve efficiency, the TEB converts constraint conditions into soft constraints, and when dynamic obstacles occur, the TEB may fall into local optimum, so that requirements such as safety and smoothness cannot be met. The hundred-degree Apollo unmanned system adopts an optimization-based collision avoidance method for an automatic parking scene, and skillfully models vehicle dynamics and collision avoidance constraint into a large nonlinear model prediction control problem only comprising continuous variables by introducing dual variables and corresponding nonlinear constraint conditions. The method has the defects of large calculated amount, can be applied to automatic parking with low real-time requirement, and cannot meet the real-time requirement of collision avoidance of the mobile robot in a high dynamic environment.
Disclosure of Invention
In order to solve the problems in the background technology, the invention provides a smooth and real-time collision avoidance method for an autonomous mobile robot in a high-dynamic complex environment, which can avoid dynamic obstacles, minimize smoothness indexes and the running time of the mobile robot and overcome the problems in the existing method.
The technical scheme of the invention is as follows:
step one: inputting necessary information;
step two: generating an initial track at the front end;
for a given target point position, simulating the motion of the mobile robot by using a local path planning module and a tracking control module to generate an initial track;
Step three: optimizing the nonlinear track at the rear end to obtain an optimal track;
Step four: and (3) inputting the optimal track obtained in the step (III) into a control module of the mobile robot, and updating the motion route of the mobile robot in real time, so as to realize smooth and real-time collision avoidance of the mobile robot.
In the first step, the input information includes: surrounding environment information, target point positions, mobile robot positioning positions and mobile robot movement capability information of the static map and real-time laser data; wherein the static map includes a grid map, a map resolution, map origin coordinates, and obstacle information.
Obstacle information is known and can be obtained by means of lidar.
The mobile robot is provided with a laser radar.
In the second step, specifically: selecting a path with the maximum probability of reaching the target point position from an existing offline generated path library by using a local path planning module as a tracking planning path, calculating a control instruction required by the tracking planning path by using a tracking control module, simulating the state of the mobile robot in a period of time according to the control instruction output by the tracking control module based on a kinematic model of the mobile robot and the known initial state of the mobile robot, further generating a state quantity x ws and a control quantity u ws of the mobile robot, and generating an initial track by using the state quantity x ws and the control quantity u ws as a hot start initial value of the nonlinear track optimization in the step three.
And in the second step, the frequency calculated by the tracking control module is higher than the frequency selected by the local path planning module.
In the third step, specifically:
s1, establishing the following trajectory optimization model with bounding boxes, which contains nonlinear constraint conditions, as an objective function and constraint conditions of the objective function:
s.t.x0＝xS,xK+1＝xF
xk+1＝f(xk,uk),h(xk,uk)≤0
xk∈Bk
for k＝0,…K
And x is k＝[Xk,Yk,θk]T,uk＝[vk,wk]T
Δuk＝(uk﹣uk-1)/τ
Wherein min represents minimization, p is a weight coefficient between control cost and running time, Q and Q Δ are weight matrixes of control quantity and control quantity smoothness set in advance respectively, deltau k represents a smoothness parameter of a track, and T represents matrix transposition; τ is the step length of the time step in simulation, τ >0; x k、uk represents the state quantity and the control quantity of the mobile robot at the kth time step respectively; x S represents the x-axis position of the mobile robot on the map at the beginning, and x F represents the x-axis position of the mobile robot on the map when reaching the target point position; f () represents a kinematic constraint function, h () represents a motion capability constraint function, B k represents bounding box parameters, for represents a loop, k=0 to K, traversing all K summations, K represents ordinal numbers of time steps, K represents the total number of time steps; x k、Yk represents the X-axis position and the y-axis position of the mobile robot on the map at the kth time step, theta k represents the included angle between the X-axis (the front direction) of the mobile robot body coordinate system and the X-axis of the global coordinate system at the kth time step, and v k、wk is the linear speed and the angular speed of the mobile robot on the map at the kth time step, respectively;
S2, under the condition of giving a bounding box, solving and iterative optimization are carried out on an objective function and constraint conditions thereof by adopting a nonlinear numerical optimization tool, and finally, the optimal time step length tau, the state quantity set x and the control quantity set u of the mobile robot are obtained, wherein:
x＝[x0,x1,…,xK+1],u＝[u0,u1,…,uK]
Wherein x and u are state quantity set and control quantity set of the mobile robot, K+1 steps are shared, and letters are bold;
S3, generating a new track as an optimal track according to the optimal time step tau, the state quantity set x and the control quantity set u of the mobile robot.
The objective function of the invention can enable the mobile robot to reach the target point as soon as possible, and simultaneously reduce the control cost.
Meanwhile, the method for meeting collision avoidance constraint by adopting bounding box iteration adjustment strategy can improve the solving efficiency of the objective function and meet the real-time requirement.
In the step S1 of the third step, the kinematic constraint function f () is specifically:
Xk+1＝Xk+τvkcos(θk+0.5τwk)
Yk+1＝Yk+τvksin(θk+0.5τwk)
θk+1＝θk+τwk
in the step S1, the motion capability constraint function h () specifically includes:
.vk∈[0,vmax],wk∈[-wmax,wmax],avk∈[-avmax,avmax],awk∈[-awmax,awmax]
Where v max is the maximum linear velocity, w max is the maximum angular velocity, av max is the maximum linear acceleration, aw max is the maximum angular acceleration, v k is the mobile robot linear velocity at the kth time step, w k is the mobile robot angular velocity at the kth time step, av k is the mobile robot linear acceleration at the kth time step, aw k is the mobile robot angular acceleration at the kth time step.
In the step S1 of the third step, x k∈Bk represents that a bounding box is set as a safety area of the position of the mobile robot, and a bounding box B k is set as follows:
||xk﹣xws,k||≤dxk,||yk﹣yws,k||≤dyk
Wherein x ws,k、yws,k is the position of the mobile robot in the initial trajectory at the kth time step, and dx k、dyk is the lengths of the bounding box in the x-axis and y-axis directions, respectively.
In the step S2, after each solving, iterative optimization is performed according to the following mode:
inputting the obtained optimal track into a mobile robot, and performing collision detection on each point on the optimal track by using a laser radar of the mobile robot:
If an obstacle collision is detected, the track is unsafe, the bounding box is contracted and updated to B k＝β*Bk, wherein beta is the change rate, and 0< beta < 1 is satisfied;
If no obstacle collision is detected, the trajectory is safe and the iterative optimization is terminated.
Therefore, the invention also provides an iteration method for determining the bounding box with proper size, so that smoothness and running time of the track are not excessively sacrificed while collision avoidance is ensured.
The method mainly decouples the collision avoidance problem into initial track generation at the front end and nonlinear track optimization at the rear end.
The probability of the mobile robot reaching the target point is maximized at the initial trajectory generation, so that the mobile robot tends to pass through a wider area, thereby providing more options for successfully bypassing the obstacle during navigation. Since smoothness and the movement time to reach the target point are not considered, the front-end initial trajectory needs to be further optimized.
In the back-end nonlinear track optimization, the bounding box is adopted to ensure the track safety, the size of the bounding box is adjusted in an iterative mode, and the back-end nonlinear track optimization process is accelerated and accurately optimized. By introducing the bounding box, the dual variables and related constraint conditions thereof can be removed, so that the scale of the nonlinear optimization problem is greatly reduced, and the initial track can be converged to a smooth, safe and dynamically feasible track in real time.
Compared with the prior art, the invention has the following advantages:
according to the method, the collision avoidance problem is decoupled into front-end initial track generation and back-end track nonlinear optimization, and the obtained optimal track can maximize the probability of reaching the target point and meet the smoothness requirement.
The method and the device provided by the invention adopt the bounding box to ensure the safety of the track, and iteratively determine the size of the bounding box, so that the complexity of the optimization problem is greatly reduced, and the real-time requirement can be met.
Drawings
FIG. 1 is a schematic diagram of a mobile robot state;
FIG. 2 is a schematic diagram of collision detection and bounding box update;
FIG. 3 is a schematic diagram of a test scenario in one embodiment;
FIG. 4 is a path diagram corresponding to an initial trajectory of a front end in one embodiment;
FIG. 5 is a path diagram of a back-end nonlinear trajectory optimization in one embodiment;
FIG. 6 is a graph of linear velocity versus front end initial trajectory and back end nonlinear trajectory optimization in one embodiment;
FIG. 7 is a graph comparing angular velocity after optimization of a front-end initial trajectory and a back-end nonlinear trajectory in one embodiment;
FIG. 8 is a schematic diagram of 25 target points in one embodiment;
FIG. 9 is a graph of time versus which iterative solution and direct solution are needed in one embodiment.
Detailed description of the preferred embodiments
The design method in the invention is further described below with reference to the accompanying drawings:
in this embodiment, the mobile robot performs autonomous collision avoidance in a test scenario, where the test scenario used in this embodiment is shown in fig. 3, and the specific steps are as follows:
Step one: inputting necessary information
And inputting information such as surrounding environment containing static map and real-time laser data, target point positions, mobile robot positioning, mobile robot motion capability and the like. The dimensions of the test scene were 10m long and 5m wide. The resolution of the grid map is 5cm, the left lower corner of the map is the origin, the coordinates of the origin are [ -2.0, -2.0], the black part in the grid map indicates that the grid is occupied, the white part indicates that the grid is idle, and no obstacle exists.
The laser radar is installed on the mobile robot, so that laser data can be acquired in real time. The small circles in the figure represent target points with coordinates of [10.0,3.0]. The initial position of the mobile robot is [3.0,3.0], which faces to the right and is in a static state. The motion capabilities of the mobile robot are as follows: linear velocity v e 0,1 m/s, linear acceleration av e-1, 1 m/s 2, angular velocity w e-1, 1 rad/s, angular acceleration aw e-1, 1 rad/s 2.
Step two: front end initial trajectory generation
And (3) setting a target point, simulating the motion of the mobile robot by using a local path planning module and a tracking control module, wherein the planning frequency is 10HZ, and the control frequency is 100HZ.
Based on a kinematic model of the mobile robot, the initial state of the mobile robot is known, the state of the mobile robot in a period of time is simulated according to a control instruction output by the tracking control module, and the generated mobile robot state x ws and the control input u ws are simulated.
The paths corresponding to the generated front initial trajectory are shown in fig. 4, and the linear velocity and the angular velocity of the front initial trajectory are shown in solid lines in fig. 5 and 6, respectively.
Step three: back-end nonlinear trajectory optimization
The initial trajectory generated by the second step cannot meet the smoothness requirement and is further smoothed and optimized.
The purpose of the back-end nonlinear trajectory optimization is to find a control instruction sequence with the total time step number of K, so that the mobile robot moves from the initial state x S to the end state x F, and the optimization function J is minimized while obstacles are avoided.
S1, establishing the following trajectory optimization model with bounding boxes, which contains nonlinear constraint conditions, as an objective function and constraint conditions of the objective function:
s.t.x0＝xS,xK+1＝xF
xk+1＝f(xk,uk),h(xk,uk)≤0
xk∈Bk
for k＝0,…K
And x is k＝[Xk,Yk,θk]T,uk＝[vk,wk]T
Δuk＝(uk﹣uk-1)/τ
Wherein min represents minimization, p is a weight coefficient between control cost and running time, Q and Q Δ are weight matrices of control quantity and control smoothness, deltau k represents a smoothness parameter of the track, and T represents matrix transposition; τ is the step length of the time step in simulation, τ >0; x k、uk represents the state quantity and the control quantity of the mobile robot at the kth time step respectively; x S represents the x-axis position of the mobile robot on the map at the beginning, and x F represents the x-axis position of the mobile robot on the map when reaching the target point position; f () represents a kinematic constraint function, h () represents a motion capability constraint function, B k represents bounding box parameters, for represents a loop, traversing all K represents ordinals of time steps, K represents the total number of time steps; x k、Yk represents the X-axis position and the y-axis position of the mobile robot on the map at the kth time step, theta k represents the included angle between the X-axis (the front direction) of the mobile robot body coordinate system and the X-axis of the global coordinate system at the kth time step, and v k、wk is the linear speed and the angular speed of the mobile robot on the map at the kth time step, respectively;
As shown in fig. 1, the mobile robot body coordinate system is specifically a planar coordinate system with the center of rotation of the robot as the origin, the forward direction being the positive x-axis direction, and the left direction being the positive y-axis direction.
The global coordinate system is a two-dimensional coordinate system established based on the robot map, and is a plane coordinate system with the origin of the map, the horizontal right axis as the x axis and the upward axis as the y axis.
The constraint condition of the objective function mainly comprises the following four parts: (1) initial and final state constraints, (2) kinematic constraints, (3) motion capability constraints, and (4) collision avoidance constraints.
(1) Initial and final state constraints
The state x 0、xK+1 of the mobile robot at the initial time and the end time is required to be the same as the initial state x S、xF of the front end initial trajectory, that is, x 0＝xS、xK+1＝xF.
(2) Kinematic constraints
Discretizing a continuous kinematic equation of the mobile robot, wherein the state of the mobile robot meets x k+1＝f(xk,uk), and the specific expression is as follows:
Xk+1＝Xk+τvkcos(θk+0.5τwk)
Yk+1＝Yk+τvksin(θk+0.5τwk)
θk+1＝θk+τwk
(3) Exercise capacity constraints
The linear speed v k, the angular speed w k, the linear acceleration av k and the angular acceleration aw k of the mobile robot are all required to be in the motion capability range, and h (x k,uk) is less than or equal to 0, and the specific expression is as follows:
vk∈[0,vmax],wk∈[-wmax,wmax],avk∈[-avmax,avmax],awk∈[-awmax,awmax]
Where v max is the maximum linear velocity, w max is the maximum angular velocity, av max is the maximum linear acceleration, aw max is the maximum angular acceleration.
(4) Collision avoidance restraint
According to the method, the bounding box with the proper size is adopted as the safety area of the position of the mobile robot, so that collision avoidance constraint is met, and the computational complexity of track smooth optimization is reduced. I.e., satisfying x k∈Bk,Bk denotes a bounding box, defined as:
||xk﹣xws,k||≤dxk,||yk﹣yws,k||≤dyk
Wherein x ws,k、yws,k is the position of the mobile robot in the initial trajectory at the kth time step, and dx k、dyk is the lengths of the bounding box in the x-axis and y-axis directions, respectively.
There are k bounding boxes in total, the bounding box is not centered around the center of the robot, and the center of the kth bounding box is the position at the kth time step in the initial trajectory.
S2, under the condition of giving a bounding box, solving and iterative optimization are carried out on an objective function and constraint conditions thereof by adopting a nonlinear numerical optimization tool, and finally, the optimal time step length tau, the state quantity set x and the control quantity set u of the mobile robot are obtained, wherein:
x＝[x0,x1,…,xK+1],u＝[u0,u1,…,uK]
wherein x and u are state quantity set and control quantity set of the mobile robot, K+1 steps are taken, and letters are bold.
The size of the bounding box is determined by adopting an iterative method, so that smoothness and running time of a track are not excessively sacrificed while collision avoidance is ensured.
The objective function is solved using a nonlinear numerical optimization tool IPOPT, as shown in FIG. 2, and iteratively optimized after each solution as follows:
Inputting the obtained optimal track to a mobile robot, and acquiring laser data points by the mobile robot by using a laser radar of the mobile robot to accurately collision detect each point on the optimal track:
if the collision of the obstacle is detected, the track is unsafe, the bounding box is reduced, parameters of the bounding box are reduced, and the parameters are updated to be B k＝β*Bk, wherein beta is a preset change rate, and 0< beta < 1 is satisfied;
If no obstacle collision is detected, the trajectory is safe and the iterative optimization is terminated.
The update process of the bounding box is shown in fig. 2.
The CPU of the computer used for the test is Intel i5-7400 3.0GHZ 4, the memory is 8GB, and the programming language is C++. The total time required by the front end and the rear end is less than 0.1 second, so that the real-time requirement is met. Fig. 5 shows a path diagram after the back-end nonlinear trajectory optimization, and the smoothness is slightly improved compared with the path corresponding to the front-end initial trajectory in fig. 4.
Fig. 6 and 7 show that the linear velocity and the angular velocity of the mobile robot after the front end initial trajectory and the rear end nonlinear trajectory are optimized, and it can be seen that the linear velocity and the angular velocity of the initial trajectory are severely changed and are very unstable, and the optimized linear velocity and angular velocity become very stable. In addition, after the back-end nonlinear optimization, the time required for the mobile robot to run from the starting point to the target point is reduced from about 19 seconds to 14 seconds, and the efficiency is greatly improved.
To demonstrate the computational efficiency of the proposed iterative trajectory smoothing algorithm, the embodiments compare the computational time required for iterative and direct solutions using the same front-end initial trajectory and IPOPT tools. The initial pose of the mobile robot and the 25 target locations selected on the map are shown in fig. 8.
As can be seen from fig. 9, the computation time required for direct solution is about 20 times that of the iterative trajectory smoothing algorithm, mainly because no dual variables and corresponding nonlinear equations and inequality constraints are introduced to accurately represent collision avoidance, thereby greatly reducing the scale of the optimization problem.
Claims (6)
1. A smooth and real-time collision avoidance method of a mobile robot based on nonlinear optimization is characterized by comprising the following steps:
step one: inputting information;
Step two: initial trajectory generation: for a given target point position, simulating the motion of the mobile robot by using a local path planning module and a tracking control module to generate an initial track;
In the second step, specifically: selecting a path with the maximum probability of reaching the target point position from a path library generated offline by using a local path planning module as a tracking planning path, calculating a control instruction required by tracking the planning path by using a tracking control module, simulating the state of the mobile robot in a period of time according to the control instruction output by the tracking control module, further generating a state quantity x ws and a control quantity u ws of the mobile robot, and generating an initial track by using the state quantity x ws and the control quantity u ws as initial values of nonlinear track optimization in the step three;
step three: optimizing a rear-end nonlinear track;
step four: inputting the track obtained in the step three into a control module of the mobile robot, and updating the motion route of the mobile robot in real time, so as to realize smooth and real-time collision avoidance of the mobile robot;
in the first step, the input information includes: surrounding environment information, target point positions, mobile robot positioning positions and mobile robot movement capability information of the static map and real-time laser data; the static map comprises a grid map, map resolution and map origin coordinates;
in the third step, specifically:
s1, establishing the following trajectory optimization model with bounding boxes, which contains nonlinear constraint conditions, as an objective function and constraint conditions of the objective function:
s.t.x0＝xS,xK+1＝xF
xk+1＝f(xk,uk),h(xk,uk)≤0
xk∈Bk
for k＝0,…K
And x is k＝[Xk,Yk,θk]T,uk＝[vk,wk]T
Δuk＝(uk﹣uk-1)/t
Wherein min represents minimization, p is a weight coefficient between control cost and running time, Q and Q Δ are weight matrixes of control quantity and control quantity smoothness respectively, deltau k represents a smoothness parameter of the track, and T represents matrix transposition; t is the step length of the simulated time steps, t >0; x k、uk represents the state quantity and the control quantity of the mobile robot at the kth time step respectively; x S represents the x-axis position of the mobile robot on the map at the beginning, and x F represents the x-axis position of the mobile robot on the map when reaching the target point position; f () represents a kinematic constraint function, h () represents a motion capability constraint function, B k represents bounding box parameters, for represents a cycle, k=0 to K, K represents ordinal numbers of time steps, and K represents the total number of time steps; x k、Yk represents the X-axis position and the y-axis position of the mobile robot on the map at the kth time step, theta k represents the included angle between the X-axis of the mobile robot body coordinate system and the X-axis of the global coordinate system at the kth time step, and v k、wk is the linear speed and the angular speed of the mobile robot on the map at the kth time step respectively;
S2, under the condition of giving a bounding box, solving and iterative optimization are carried out on an objective function and constraint conditions thereof by adopting a nonlinear numerical optimization tool, and finally, the optimal time step length t, the state quantity set x and the control quantity set u of the mobile robot are obtained, wherein:
x＝[x0,x1,…,xK+1],u＝[u0,u1,…,uK]
wherein x and u are a state quantity set and a control quantity set of the mobile robot;
S3, generating a new track as an optimal track according to the optimal time step length t, the state quantity set x and the control quantity set u of the mobile robot.
2. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 1, wherein: the mobile robot is provided with a laser radar.
3. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 1, wherein: in the step S1 of the third step, the kinematic constraint function f () is specifically:
Xk+1＝Xk+tvkcos(θk+0.5twk)
Yk+1＝Yk+tvksin(θk+0.5twk)
θk+1＝θk+twk。
4. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 1, wherein: in the step S1, the motion capability constraint function h () specifically includes:
vk∈[0,vmax],wk∈[-wmax,wmax],avk∈[-avmax,avmax],awk∈[-awmax,awmax]
Where v max is the maximum linear velocity, w max is the maximum angular velocity, av max is the maximum linear acceleration, aw max is the maximum angular acceleration, v k is the mobile robot linear velocity at the kth time step, w k is the mobile robot angular velocity at the kth time step, av k is the mobile robot linear acceleration at the kth time step, aw k is the mobile robot angular acceleration at the kth time step.
5. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 1, wherein: in the step S1 of the third step, x k∈Bk represents that a bounding box is set as a safety area of the position of the mobile robot, and a bounding box B k is set as follows:
||xk﹣xws,k||≤dxk,||yk﹣yws,k||≤dyk
Wherein x ws,k、yws,k is the position of the mobile robot in the initial trajectory at the kth time step, and dx k、dyk is the lengths of the bounding box in the x-axis and y-axis directions, respectively.
6. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 1, wherein: in the step S2, after each solving, iterative optimization is performed according to the following mode:
Inputting the obtained track to a mobile robot, and performing collision detection on each point on the track by the mobile robot through a laser radar of the mobile robot:
If an obstacle collision is detected, the track is unsafe, the bounding box is contracted and updated to B k＝b*Bk, wherein B is the change rate, and 0< B <1 is satisfied;
If no obstacle collision is detected, the trajectory is safe and the iterative optimization is terminated.
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310432909.3A CN116263605B (en) | 2023-04-21 | 2023-04-21 | Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202310432909.3A CN116263605B (en) | 2023-04-21 | 2023-04-21 | Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116263605A CN116263605A (en) | 2023-06-16 |
| CN116263605B true CN116263605B (en) | 2024-06-21 |
Family
ID=86723134
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202310432909.3A Active CN116263605B (en) | 2023-04-21 | 2023-04-21 | Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN116263605B (en) |
Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109960261A (en) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | A kind of dynamic barrier preventing collision method based on collision detection |
Family Cites Families (9)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN106873600A (en) * | 2017-03-31 | 2017-06-20 | 深圳市靖洲科技有限公司 | It is a kind of towards the local obstacle-avoiding route planning method without person bicycle |
| CN113031592A (en) * | 2021-02-25 | 2021-06-25 | 杭州国辰机器人科技有限公司 | Robot path smoothing method and system based on fifth-order Bezier curve |
| CN113664832A (en) * | 2021-09-01 | 2021-11-19 | 上海节卡机器人科技有限公司 | Robot collision prediction method, computer storage medium and electronic device |
| CN114227686A (en) * | 2021-12-28 | 2022-03-25 | 深圳市优必选科技股份有限公司 | Robot obstacle avoidance control method and device, terminal equipment and storage medium |
| CN114442621A (en) * | 2022-01-17 | 2022-05-06 | 浙江大学 | Autonomous exploration and mapping system based on quadruped robot |
| CN114442491A (en) * | 2022-04-08 | 2022-05-06 | 杭州国辰机器人科技有限公司 | Local path planning and tracking control method, device and medium for indoor robot |
| CN115525063A (en) * | 2022-08-12 | 2022-12-27 | 南京航空航天大学 | Trajectory planning method for unmanned helicopter |
| CN115816446A (en) * | 2022-11-24 | 2023-03-21 | 广西大学 | Cooperative motion control method for mobile mechanical arm in hilly and mountainous areas |
| CN115840451A (en) * | 2022-12-12 | 2023-03-24 | 畅加风行(苏州)智能科技有限公司 | Narrow passage unmanned towing obstacle avoidance turning track planning method |
-
2023
- 2023-04-21 CN CN202310432909.3A patent/CN116263605B/en active Active
Patent Citations (1)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN109960261A (en) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | A kind of dynamic barrier preventing collision method based on collision detection |
Non-Patent Citations (1)
| Title |
|---|
| 基于滚动优化原理的类车机器人路径跟踪控制;丛岩峰;安向京;陈虹;喻再涛;;吉林大学学报(工学版);20120131;第42卷(第1期);全文 * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN116263605A (en) | 2023-06-16 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Cherubini et al. | Autonomous visual navigation and laser-based moving obstacle avoidance | |
| Li et al. | A practical trajectory planning framework for autonomous ground vehicles driving in urban environments | |
| Fridovich-Keil et al. | Safely probabilistically complete real-time planning and exploration in unknown environments | |
| Miller et al. | Optimal planning for target localization and coverage using range sensing | |
| Blekas et al. | RL-based path planning for an over-actuated floating vehicle under disturbances | |
| Zhou et al. | A hybrid path planning and formation control strategy of multi-robots in a dynamic environment | |
| Azam et al. | N 2 C: neural network controller design using behavioral cloning | |
| Liu et al. | Deep reinforcement learning for mobile robot path planning | |
| Quang et al. | Design a nonlinear MPC controller for autonomous mobile robot navigation system based on ROS | |
| CN116263605B (en) | Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization | |
| Backes et al. | Automated rover positioning and instrument placement | |
| Francis et al. | Real-time path planning module for autonomous vehicles in cluttered environment using a 3D camera | |
| CN117055556A (en) | Multi-robot formation path planning method and device, electronic equipment and storage medium | |
| Frew et al. | Trajectory generation for constant velocity target motion estimation using monocular vision | |
| Pacis et al. | Enhancing functionality and autonomy in man-portable robots | |
| Bai et al. | Model predictive visual trajectory-tracking control of wheeled mobile robots | |
| Daryina et al. | Parametric optimization of unmanned vehicle controller by PSO algorithm | |
| Han et al. | A distributed range-only collision avoidance approach for low-cost large-scale multi-robot systems | |
| Björnberg | Shared control for vehicle teleoperation with a virtual environment interface | |
| Sivarathri et al. | Kinematic modes of vision-based heterogeneous uav-agv system | |
| Febbo et al. | A Comprehensive Trajectory Planner for a Person-Following ATV | |
| Chen et al. | An Intelligent Navigation Control Approach for Autonomous Unmanned Vehicles via Deep Learning-Enhanced Visual SLAM Framework | |
| Kanjanawanishkul | Coordinated path following for mobile robots using a virtual structure strategy with model predictive control | |
| Yoshida et al. | “Give me the purple ball”-he said to HRP-2 N. 14 | |
| Lee | Efficient perception, planning, and control algorithm for vision-based automated vehicles |
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 | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |