CN116263605A - 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
- CN116263605A CN116263605A CN202310432909.3A CN202310432909A CN116263605A CN 116263605 A CN116263605 A CN 116263605A CN 202310432909 A CN202310432909 A CN 202310432909A CN 116263605 A CN116263605 A CN 116263605A
- Authority
- CN
- China
- Prior art keywords
- mobile robot
- track
- time
- real
- representing
- 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.)
- Granted
Links
- 238000005457 optimization Methods 0.000 title claims abstract description 49
- 238000000034 method Methods 0.000 title claims abstract description 30
- 230000001133 acceleration Effects 0.000 claims description 14
- 238000009499 grossing Methods 0.000 claims description 9
- 239000011159 matrix material Substances 0.000 claims description 6
- 230000003068 static effect Effects 0.000 claims description 5
- 238000001514 detection method Methods 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
- 238000009825 accumulation Methods 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
Images
Classifications
-
- G—PHYSICS
- G05—CONTROLLING; REGULATING
- G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
- G05D1/00—Control of position, course, altitude or attitude of land, water, air or space vehicles, e.g. using automatic pilots
- G05D1/02—Control of position or course in two dimensions
- G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
- G05D1/0212—Control of position or course in two dimensions specially adapted to land vehicles with means for defining a desired trajectory
- G05D1/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 a path library generated offline in advance by using a local path planning module as a tracking planning path, calculating a control instruction required by the tracking planning path by a tracking control module, and 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 initial state of the known mobile robot so as to generate the mobile machineHuman state quantity x ws And a control quantity u ws As a hot start initial value of the nonlinear trajectory optimization in the step three, a state quantity x is used for ws And a control quantity u ws An initial trajectory is generated.
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.x 0 ＝x S ,x K+1 ＝x F
x k+1 ＝f(x k ,u k ),h(x k ,u k )≤0
x k ∈B k
for k＝0,…K
and x is k ＝[X k ,Y k ,θ k ] T ,u k ＝[v k ,w k ] T
Δu k ＝(u k ﹣u k-1 )/τ
Wherein min represents minimization, p is the weight coefficient between control cost and run time, Q and Q Δ Weight matrix of control quantity and control quantity smoothness respectively set in advance, deltau k A smoothness parameter representing the trajectory, T representing the matrix transpose; τ is the step length of the time step in simulation, τ>0；x k 、u k Respectively representing the state quantity and the control quantity of the mobile robot at the kth time step; x is x S Representing the x-axis position of the mobile robot on the map at the beginning, x F Representing 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 loop, k=Traversing all K accumulation, wherein K represents the ordinal number of the time steps, and K represents the total number of the time steps; x is X k 、Y k Respectively representing the x-axis position and the y-axis position of the mobile robot on the map at the kth time step, theta k Representing the included angle between the x-axis (forward direction) of the mobile robot body coordinate system and the x-axis of the global coordinate system at the kth time step, v k 、w k The linear speed and the angular speed of the mobile robot on the map at the kth time step are 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＝[x 0 ,x 1 ,…,x K+1 ],u＝[u 0 ,u 1 ,…,u K ]
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:
X k+1 ＝X k +τv k cos(θ k +0.5τw k )
Y k+1 ＝Y k +τv k sin(θ k +0.5τw k )
θ k+1 ＝θ k +τw k
in the step S1, the motion capability constraint function h () specifically includes:
。v k ∈[0,v max ],w k ∈[-w max ,w max ],av k ∈[-av max ,av max ],aw k ∈[-aw max ,aw max ]
wherein v is max At maximum linear velocity, w max At maximum angular velocity av max For maximum linear acceleration aw max For maximum angular acceleration, v k Representing the linear velocity, w, of the mobile robot at the kth time step k Indicating the angular velocity, av, of the mobile robot at the kth time step k Representing the linear acceleration of the mobile robot, aw, at the kth time step k The angular acceleration of the mobile robot at the kth time step is indicated.
In the step S1 of the third step, x k ∈B k Indicating that a bounding box B is arranged as a safe area of the position of the mobile robot k The settings are expressed as:
||x k ﹣x ws,k ||≤dx k ,||y k ﹣y ws,k ||≤dy k
wherein x is ws,k 、y ws,k Respectively the positions of the mobile robot in the initial track at the kth time step, dx k 、dy k The length of the bounding box in both 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 the track is updated to B k ＝β*B k Wherein beta is the rate of change, satisfying 0<β＜1；
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 ]]Toward the right, in a stationary 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 [ -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, knowing the initial state of the mobile robot, simulating according to a control instruction output by a tracking control moduleThe state of the mobile robot in a period of time simulates the generated state x of the mobile robot ws And control input u ws 。
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 a total time step number K so that the mobile robot is started from an initial state x S Move to end state x F While avoiding obstacles, the optimization function J is minimized.
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.x 0 ＝x S ,x K+1 ＝x F
x k+1 ＝f(x k ,u k ),h(x k ,u k )≤0
x k ∈B k
for k＝0,…K
and x is k ＝[X k ,Y k ,θ k ] T ,u k ＝[v k ,w k ] T
Δu k ＝(u k ﹣u k-1 )/τ
Wherein min represents minimization, p is the weight coefficient between control cost and run time, Q and Q Δ For controlling the amount and controlling the weight matrix of the smoothness, deltau k A smoothness parameter representing the trajectory, T representing the matrix transpose; τ is the step length of the time step in simulation, τ>0；x k 、u k Separate tableShowing the state quantity and the control quantity of the mobile robot at the kth time step; x is x S Representing the x-axis position of the mobile robot on the map at the beginning, x F Representing 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 Representing bounding box parameters, for representing a cycle, traversing all K, K representing ordinal numbers of time steps, K representing a total number of time steps; x is X k 、Y k Respectively representing the x-axis position and the y-axis position of the mobile robot on the map at the kth time step, theta k Representing the included angle between the x-axis (forward direction) of the mobile robot body coordinate system and the x-axis of the global coordinate system at the kth time step, v k 、w k The linear speed and the angular speed of the mobile robot on the map at the kth time step are 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
State x of mobile robot at initial time and final time 0 、x K+1 Initial and final state x of initial trajectory with front end S 、x F Identical, i.e. x 0 ＝x S 、x K+1 ＝x F 。
(2) Kinematic constraints
Discretizing a continuous kinematic equation of the mobile robot, wherein the state of the mobile robot meets x k+1 ＝f(x k ,u k ) The specific expression is:
X k+1 ＝X k +τv k cos(θ k +0.5τw k )
Y k+1 ＝Y k +τv k sin(θ k +0.5τw k )
θ k+1 ＝θ k +τw k
(3) Exercise capacity constraints
Linear velocity v of mobile robot k Angular velocity w k Linear acceleration av k And angular acceleration aw k All are required to be within the range of motion capability to satisfy h (x k ,u k ) The specific expression is less than or equal to 0:
v k ∈[0,v max ],w k ∈[-w max ,w max ],av k ∈[-av max ,av max ],aw k ∈[-aw max ,aw max ]
wherein v is max At maximum linear velocity, w max At maximum angular velocity av max For 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. satisfy x k ∈B k ，B k Representing bounding boxes, defined as:
||x k ﹣x ws,k ||≤dx k ,||y k ﹣y ws,k ||≤dy k
wherein x is ws,k 、y ws,k Respectively the positions of the mobile robot in the initial track at the kth time step, dx k 、dy k The length of the bounding box in both 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＝[x 0 ,x 1 ,…,x K+1 ],u＝[u 0 ,u 1 ,…,u K ]
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.
Solving an objective function by using a nonlinear numerical optimization tool IPOPT, as shown in FIG. 2, and performing iterative optimization after each solving according to the following mode:
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 an obstacle collision 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 ＝β*B k Wherein beta is a predetermined rate of change, satisfying 0<β＜1；
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 (8)
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 highest 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 a tracking control module, and simulating the state of the mobile robot in a period of time according to the control instruction output by the tracking control module so as to generate a state quantity x of the mobile robot ws And a control quantity u ws As the initial value of the nonlinear trajectory optimization in the step three, the state quantity x is used for ws And a control quantity u ws Generating an initial track;
step three: optimizing a rear-end nonlinear track;
step four: and (3) inputting the obtained track 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.
2. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 1, wherein: 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 comprises a grid map, a map resolution, and map origin coordinates.
3. 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.
4. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 1, wherein: 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.x 0 ＝x S ,x K+1 ＝x F
x k+1 ＝f(x k ,u k ),h(x k ,u k )≤0
x k ∈B k
for k＝0,…K
and x is k ＝[X k ,Y k ,θ k ] T ,u k ＝[v k ,w k ] T
Δu k ＝(u k ﹣u k-1 )/τ
Wherein min represents minimization and p is the control cost and fortuneWeight coefficients, Q and Q between line times Δ Weight matrix of control quantity and control quantity smoothness, respectively, deltau k A smoothness parameter representing the trajectory, T representing the matrix transpose; τ is the step size of the simulated time step, τ>0；x k 、u k Respectively representing the state quantity and the control quantity of the mobile robot at the kth time step; x is x S Representing the x-axis position of the mobile robot on the map at the beginning, x F Representing 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 Representing bounding box parameters, for representing a cycle, k=0 to K, K representing ordinal numbers of time steps, K representing the total number of time steps; x is X k 、Y k Respectively representing the x-axis position and the y-axis position of the mobile robot on the map at the kth time step, theta k Representing the included angle between the x-axis of the robot body coordinate system and the x-axis of the global coordinate system in the kth time step, v k 、w k The linear speed and the angular speed of the mobile robot on the map at the kth time step are 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＝[x 0 ,x 1 ,…,x K+1 ],u＝[u 0 ,u 1 ,…,u K ]
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 tau, the state quantity set x and the control quantity set u of the mobile robot.
5. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 4, wherein: in the step S1 of the third step, the kinematic constraint function f () is specifically:
X k+1 ＝X k +τv k cos(θ k +0.5τw k )
Y k+1 ＝Y k +τv k sin(θ k +0.5τw k )
θ k+1 ＝θ k +τw k 。
6. the nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 4, wherein: in the step S1, the motion capability constraint function h () specifically includes:
v k ∈[0,v max ],w k ∈[-w max ,w max ],av k ∈[-av max ,av max ],aw k ∈[-aw max ,aw max ]
wherein v is max At maximum linear velocity, w max At maximum angular velocity av max For maximum linear acceleration aw max For maximum angular acceleration, v k Representing the linear velocity, w, of the mobile robot at the kth time step k Indicating the angular velocity, av, of the mobile robot at the kth time step k Representing the linear acceleration of the mobile robot, aw, at the kth time step k The angular acceleration of the mobile robot at the kth time step is indicated.
7. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 4, wherein: in the step S1 of the third step, x k ∈B k Indicating that a bounding box B is arranged as a safe area of the position of the mobile robot k The settings are expressed as:
||x k ﹣x ws,k ||≤dx k ,||y k ﹣y ws,k ||≤dy k
wherein x is ws,k 、y ws,k Respectively the positions of the mobile robot in the initial track at the kth time step, dx k 、dy k The length of the bounding box in both the x-axis and y-axis directions, respectively.
8. The nonlinear optimization-based mobile robot smoothing and real-time collision avoidance method as claimed in claim 4, 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 the track is updated to B k ＝β*B k Wherein beta is the rate of change, satisfying 0<β＜1；
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 | 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 | Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN116263605A true CN116263605A (en) | 2023-06-16 |
| CN116263605B CN116263605B (en) | 2024-06-21 |
Family
ID=
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018176593A1 (en) * | 2017-03-31 | 2018-10-04 | 深圳市靖洲科技有限公司 | Local obstacle avoidance path planning method for unmanned bicycle |
| CN109960261A (en) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | A kind of dynamic barrier preventing collision method based on collision detection |
| CN113031592A (en) * | 2021-02-25 | 2021-06-25 | 杭州国辰机器人科技有限公司 | Robot path smoothing method and system based on fifth-order Bezier curve |
| 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 |
| WO2023029901A1 (en) * | 2021-09-01 | 2023-03-09 | 节卡机器人股份有限公司 | Robot collision prediction method, computer storage medium, and electronic device |
| 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 |
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| WO2018176593A1 (en) * | 2017-03-31 | 2018-10-04 | 深圳市靖洲科技有限公司 | Local obstacle avoidance path planning method for unmanned bicycle |
| CN109960261A (en) * | 2019-03-22 | 2019-07-02 | 北京理工大学 | A kind of dynamic barrier preventing collision method based on collision detection |
| CN113031592A (en) * | 2021-02-25 | 2021-06-25 | 杭州国辰机器人科技有限公司 | Robot path smoothing method and system based on fifth-order Bezier curve |
| WO2023029901A1 (en) * | 2021-09-01 | 2023-03-09 | 节卡机器人股份有限公司 | 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 |
Non-Patent Citations (1)
| Title |
|---|
| 丛岩峰;安向京;陈虹;喻再涛;: "基于滚动优化原理的类车机器人路径跟踪控制", 吉林大学学报(工学版), vol. 42, no. 1, 31 January 2012 (2012-01-31) * |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| Gao et al. | Gradient-based online safe trajectory generation for quadrotor flight in complex environments | |
| Mercy et al. | Spline-based motion planning for autonomous guided vehicles in a dynamic environment | |
| Li et al. | Real-time trajectory planning for autonomous urban driving: Framework, algorithms, and verifications | |
| Cherubini et al. | Autonomous visual navigation and laser-based moving obstacle avoidance | |
| Munir et al. | Autonomous vehicle: The architecture aspect of self driving car | |
| Li et al. | A practical trajectory planning framework for autonomous ground vehicles driving in urban environments | |
| Guo et al. | Obstacle avoidance with dynamic avoidance risk region for mobile robots in dynamic environments | |
| Shin et al. | Model predictive path planning for an autonomous ground vehicle in rough terrain | |
| 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 | |
| Azam et al. | N 2 C: neural network controller design using behavioral cloning | |
| Quang et al. | Design a nonlinear MPC controller for autonomous mobile robot navigation system based on ROS | |
| Francis et al. | Real-time path planning module for autonomous vehicles in cluttered environment using a 3D camera | |
| Frew et al. | Trajectory generation for constant velocity target motion estimation using monocular vision | |
| CN116263605B (en) | Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization | |
| Feng et al. | Implementation of dynamic obstacle avoidance on the CMU NavLab | |
| Pacis et al. | Enhancing functionality and autonomy in man-portable robots | |
| CN116520847A (en) | Path planning method, path planning device, mobile robot, electronic equipment and storage medium | |
| CN116263605A (en) | Mobile robot smooth and real-time collision avoidance method based on nonlinear optimization | |
| Björnberg | Shared control for vehicle teleoperation with a virtual environment interface | |
| 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 | |
| Dekkata et al. | Improved Model Predictive Control System Design and Implementation for Unmanned Ground Vehicles | |
| Sivarathri et al. | Kinematic modes of vision-based heterogeneous uav-agv system | |
| Febbo et al. | A Comprehensive Trajectory Planner for a Person-Following ATV |
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 |