trajectory planning and automatic obstacle avoidance
Post on 19-May-2022
8 Views
Preview:
TRANSCRIPT
To cite this article:LI Q L, LI B, SUN G H, et al. Trajectory planning and automatic obstacle avoidance algorithm for
unmanned surface vehicle based on exact penalty function[J/OL]. Chinese Journal of Ship Research, 2020,
16(1). http://www.ship-research.com/EN/Y2020/V16/I1/89.
DOI:10.19693/j.issn.1673-3185. 02209
Received:2020 - 11 - 30 Accepted:2020 - 12 - 08
Supported by:Fund of Science and Technology on Space Intelligent Control Laboratory in 2018 (KGJZDSYS-2018-03)
Authors:LI Qingliang, male, born in 1998, master degree candidate. Research interest: trajectory planning of intelligent agents.
E-mail: liqingliang@stu.scu.edu.cn
LI Bin, male, born in 1983, Ph.D., professor. Research interests: control problems in the aerospace field, autonomous
control of unmanned systems, secrete communication at the physical level, simultaneous information and power trans-
fer, and numerical optimization. E-mail: bin.li@scu.edu.cn
SUN Guohao, male, born in 1990, Ph.D., assistant professor. Research interests: signal processing of distributed net-
work radars and airborne/spaceborne radars, and intelligent sensing technology. E-mail: sghsjw2005@126.com
**Corresponding author:LI Bin, SUN Guohao
0 Introduction
In recent years, manless driving has received in-
creasing attention and has developed rapidly in the
fields of air vehicles, intelligent vehicles, and ships.
Intelligence is one of the development trends of fu-
ture ships [1]. Due to the low manufacturing cost,
short production cycle, strong environmental adapt-
ability, and low human cost of unmanned surface
vehicles (USV), they have good application pros-
pects in marine resource survey, channel measure-
ment, environment monitoring, water cleaning, and
military operation. How to quickly generate obsta-
cle avoidance routes for USVs in complex environ-
ments is one of the key technologies for their auton-
omous development [2].
Li et al. [3] summarized the primary development
paths of autonomous navigation of ships worldwide
in the context of Industry 4.0 and analyzed key tech-
nologies such as trajectory planning and intelligent
Trajectory planning and automaticobstacle avoidance algorithm for
unmanned surface vehicle based onexact penalty function
LI Qingliang1, LI Bin*2, SUN Guohao*2, CUI Xing3, MAO Xintao3
1 College of Electrical Engineering, Sichuan University, Chengdu 610065, China
2 School of Aeronautics and Astronautics, Sichuan University, Chengdu 610065, China
3 Beijing SunWise Space Technology Co., Ltd., Beijing 100194, China
Abstract: [Objectives] Currently, how to plan the safe and efficient movement trajectory of an unmanned surface ve-
hicle (USV) in local waters with multiple known obstacle positions is a research hotspot. [Methods] First, the obsta-
cle areas are treated with simple and effective circular and convex quadrilateral envelopes, and the obstacle avoid-
ance problem is transformed into the state inequality constraint of a time-optimal control problem. The time-optimal
control problem is then transformed into an optimal parameter selection problem by control parameterization and
time scale transformation. Finally, for multiple continuous state inequality constraints caused by multiple obstacles,
the exact penalty function method is used to append all state constraints to the cost function. The final form of the
problem is suitable for solving any effective optimization technique as a nonlinear optimization problem. [Results]
The numerical simulation results show that the planned trajectory successfully avoids the obstacles in the waters and
conforms to the motion characteristics of USVs. [Conclusions] The results of this study can provide valuable refer-
ences for the obstacle avoidance problem in USV trajectory planning.
Keywords: unmanned surface vehicles (USV); track planning; optimal control; exact penalty function; obstacle
avoidance
CLC number:U664.82
CHINESE JOURNAL OF SHIP RESEARCH,VOL.16,NO.1,FEB. 2021 23
downloaded from www.ship-research.com
CHINESE JOURNAL OF SHIP RESEARCH,VOL.16,NO.1,FEB. 2021
obstacle avoidance of USVs. Yu et al. [4] improved
the traditional A* algorithm by adding a cost func-
tion, which can be used to plan motion paths for un-
manned channel surveying ships. Song et al. [5] pro-
posed a smooth A* algorithm to reduce redundant
path points, which could provide more continuous
paths. Liu et al. [6] proposed an information-naviga-
tion-based path planning algorithm of USVs based
on the fast marching (FM) method, which had fast
computational speed and low computational com-
plexity. This algorithm covered two kinds of areas
(the navigation area and the obstacle avoidance ar-
ea) and could ensure that the planned trajectories
did not locate in any prohibited area. On the basis
of improving the existing line-of-sight (LOS) navi-
gation algorithm and the velocity obstacle (VO)
navigation algorithm, Cho et al. [7] proposed a way-
point tracking and obstacle avoidance algorithm,
namely that an additional control scheme was intro-
duced in the transition process of path points to im-
prove the stability of tracking control. In addition,
they improved the traditional VO algorithm, thus
solving the position uncertainty problem in the pres-
ence of obstacles, and verified the effectiveness of
the algorithm in the real sea environment. By com-
bining the bidirectional rapidly-exploring random
trees (Bi-RRT) with the velocity obstacle method,
Ouyang et al. [8] proposed an autonomous obstacle
avoidance algorithm for USVs based on the im-
proved Bi-RRT algorithm and used two parallel ex-
tended search trees to improve the real-time perfor-
mance of the algorithm. Influenced by artificial in-
telligence, many intelligent algorithms have been
used in the path planning of USVs, such as the parti-
cle swarm optimization[9], the ant colony algorithm[10],
reinforcement learning [11], and the artificial neural
network [12]. Shu [9] proposed a multi-objective hy-
brid particle swarm algorithm to optimize multiple
objectives such as the path length, path smoothness,
and safety of USVs. With the improved ant colony
algorithm, Qiu et al. [10] planned the shortest colli-
sion-free safety path for unmanned lifeboats. Shen[11] combined the A* parallel decision-making dy-
namic obstacle avoidance algorithm, with the deep
Q-learning-based intelligent obstacle avoidance al-
gorithm, so as to provide multi-layer obstacle avoid-
ance navigation for USVs. Wei [12] used the convolu-
tional neural network to recognize obstacles in near-
by waters, and built the collision risk index combin-
ing the fuzzy mathematics theory, thus proposing
the remote trajectory re-planning and short-range re-
active obstacle avoidance method.
The presence of obstacles poses a great challenge
for trajectory planning, and the processing method
of obstacles directly determines the complexity of
the trajectory optimization problem. The processing
for obstacles in the existing literature is generally
complex. Thus, this paper proposes to use circular
and convex quadrilateral envelopes to transform the
obstacle avoidance problem into an inequality con-
straint problem in the Cartesian coordinate system.
The exact penalty function is introduced to simplify
multiple enforced constraints brought by several ob-
stacles. As there are very few studies considering
the motion characteristics of USVs in trajectory
planning, this paper intends to solve the time-opti-
mal control problem by the control parameteriza-
tion method based on motion equations of USVs.
This paper solves the trajectory planning and auton-
omous obstacle avoidance problems of USVs com-
bining the control parameterization method and the
exact penalty function method and verifies the ef-
fectiveness of the proposed method by simulation.
1 Model of problem
1.1 Kinematic modeling of USVs
To simplify the problem, this paper views the
USV as a mass point, and only focuses on its trajec-
tory planning. Its kinematic coordinate system is
shown in Fig.1. In the figure, θ denotes the course
angle of the USV; V denotes the scalar quantity of
the resultant velocity vector of the USV; Vx and Vy
denote the velocity components of V along the x-ax-
is and the y-axis, respectively. Assuming that the co-
ordinates of the USV at the moment t are (x(t), y(t)),
the state quantity is X(t) = [x(t) y(t) θ(t) V(t)]T, and
the control quantity is u(t) = [γ(t) a(t)]T, the dynam-
ic equation of the USV (the symbol " · " at the top
of variables indicates the first-order derivative with
respect to time) is
(1)
where V(t) is the scalar quantity of the resultant ve-
locity vector of the USV at the moment t; θ(t) is the
course angle of the USV at the moment t; γ(t) is the
angular velocity of θ of the USV at the moment t;
a(t) is the acceleration of V of the USV at the mo-
ment t.
24
downloaded from www.ship-research.com
Fig.1 Kinematic coordinate system of USV
For simplification, Eq. (1) can be simply denoted
as the function f:
(2)
1.2 Modeling of obstacle avoidance area
of static obstacles
In the real navigation environment, the shapes of
obstacles are generally irregular. The modeling will
be very complicated and time-consuming if we con-
duct specific modeling for obstacles. For simplifica-
tion, this paper will use circular and convex quadri-
lateral envelopes to circle the obstacles. For irregu-
lar obstacles with a small length-width ratio, they
can be enveloped by circular envelopes, as shown
in Fig. 2 (a) and Fig. 2 (b). For elongated obstacles
with a too big length-width ratio, many navigable
areas will be wasted if we still use circular enve-
lopes (see Fig.2 (c)). It is not beneficial for trajecto-
ry planning. Thus, convex quadrilateral envelopes
are used to process elongated obstacles, as shown in
Fig.2 (d). The non-navigable areas consisting of en-
velopes of obstacles and standard prohibited areas
are hereafter referred to as prohibited areas (the ar-
eas where USVs are prohibited from navigation).
1.2.1 Modeling of circular envelopes
For irregular obstacles with a small length-width
ratio, circular envelopes can be used to reduce the
complexity of modeling. The longest line inside of
an obstacle is used as the diameter of the envelope
circle, and the center of this line is used as the cen-
ter of the envelope circle, as shown in Fig. 3. The
center of the circle is denoted by (xi, yi), and the ra-
dius is denoted by ri, where i indicates the number
of obstacles. To ensure the safety of USVs, the safe-
ty threshold ρ should be introduced, and its value is
three times the length of the USV hull. The enve-
lope circle with the safety threshold is used as the
prohibited area covering the obstacle, namely the
dashed circle in Fig.3, and its radius is Ri = ri +ρ. To
avoid obstacles, the position of the USV should sat-
isfy
(3)
Fig.3 Modeling of circular envelopes
1.2.2 Modeling of convex quadrilateral enve-
lopes
For irregular obstacles with a big length-width ra-
tio, many navigable areas will be wasted and the
navigation length of USVs will be increased if we
still use circular envelopes to establish models.
There even may be no navigable area. In this case,
it is better to use convex quadrilateral envelopes for
modeling. As shown in Fig.4, the convex quadrilat-
eral envelop is composed of four intersecting lines
( j indicates the number of obstacles).
The solid convex quadrilateral is the smallest enve-
lope of obstacles, and the dashed convex quadrilat-
eral is the envelope considering the safety threshold
ρ. This paper uses the linear equations in the slope-
intercept form to establish the model for
. It is assumed that areFig. 2 Envelopes of obstacle areas
(d) Elongated obstacles processedby quadrilateral envelopes
(a) Concave obstaclesprocessed by circularenvelopes
(b) Convex obstaclesprocessed by circularenvelopes
(c) Elongated obstaclesprocessed by circularenvelopes
LI Q L, et al. Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehiclebased on exact penalty function 25
downloaded from www.ship-research.com
CHINESE JOURNAL OF SHIP RESEARCH,VOL.16,NO.1,FEB. 2021
slopes of four sides, and are inter-
cepts of corresponding lines of four dashed sides on
the y-axis.
Fig. 4 Modeling of convex quadrilateral envelopes
To avoid the convex quadrilateral envelope, the
position of the USV should satisfy one of the fol-
lowing four inequalities
(4)
When the slope of a side does not exist, the in-
equality constraint of this side changes to the con-
straint for x(t). To simplify the constraint, we can
transform Equation (4) to
(5)
where is the function for getting the mini-
mum; z = 1, 2, 3, 4, which is the number of sides of
the convex quadrilateral envelope.
1.3 Modeling of the optimization prob-
lem
The objective of trajectory planning is to mini-
mize the total motion time of a USV with its known
initial state on the premise of considering con-
straints such as prohibited areas, the minimum/max-
imum driving speed, and the position of the termi-
nal point. The model of this problem (P1) is
(6)
where C0 is the constraint for control quantities; C1
is the constraint for kinematic equations of the
USV; C2 is the initial state of the USV; C3 and C4
are constraints for terminal positions; C5 is the con-
straint for the navigation speed of the USV; C6 and
C7 are respectively circular prohibited area and con-
vex quadrilateral prohibited area; T is the maximum
motion time of the USV; γmax is the maximum steer-
ing angular speed; amax is the maximum navigation
acceleration; X(0) is the USV state at the moment 0;
X0 is the initial state of the USV; xT and yT are coor-
dinates of the terminal position of the USV; Vmax is
the maximum navigation speed of the USV; Ni and
Nj are maximum numbers of obstacles in the circu-
lar envelope prohibited area and the convex quadri-
lateral envelope prohibited area, respectively. The
time domain [0, T] is a variable domain, and the
constraints C5-C7 all include the state constraints
of every moment. This will actually create numer-
ous constraints, thus posing a huge challenge to the
problem processing.
2 Optimal control algorithm basedon exact penalty function
2.1 Control parameterization
This paper uses time scale transformation and
control parameterization to process the above opti-
mization problem. As shown in Fig. 5, time scale
transformation can transfer the variable time do-
main t ∈ [0, T] to the fixed domain s ∈ [0, 1], where
s is the transferred time scale. It can be known from
References [13-14] that
(7)
where ψ is the time-scale transformation angle.
Fig.5 Time scale transformation
According to Eq.(2), Eq.(7) can be transformed to
(8)
To simplify the optimization process, this paper
26
downloaded from www.ship-research.com
divides the continuous control variables into K sec-
tions in s ∈ [0, 1], thus creating K + 1 nodes {0, s1,
s2, ..., sK}, where sK = 1. The results of sectional pa-
rameterization are shown in Fig.6, and the function
expression is given by [15]
(9)
where un(s) is the control variable, and n=1, 2,
which respectively denotes the course angular
speed and the navigation acceleration; σn,m indicates
the control variable that should be optimized, and
m = 1, 2, … , K, which denotes the number of con-
trol sections; is the value switching func-
tion.
Fig.6 Control parameterization
Here,
(10)
Thus, Eq. (6) can be re-written as the problem
model (P2):
(11)
where S0-S7 is the re-written forms of the con-
straints C0-C7; x(1), y(1) are position states at the
moment s = 1.
2.2 Constraints processing based on ex-
act penalty function
(P2) includes terminal equality constraints S3
and S4 as well as continuous state inequality con-
straints S5-S7, and this makes the problem process-
ing extremely difficult. Thus, this paper uses the ex-
act penalty function [13, 16] to process the constraints
S3-S7, so as to transform (P2) to an optimization
problem that only includes the control constraint
and the initial state constraint. The objective func-
tion J is given by
(12)
where
(13)
The Hamiltonian function is
(14)
where
(15)
The Lagrange multiplier vector λ(s) should satisfy
(16)
where ϵ indicates the penalty parameter; X(1) de-
notes the terminal state of the USV; α > 1, β > 2,
γ > 2, δ, Wi and Wj are all positive real numbers; ϵ-α
indicates the penalty weight; δϵβ denotes the penalty
factor; ϵγW indicates the relaxation factor, and W de-
notes the weight of the relaxation factor; κ indicates
the scaling factor.
For convex quadrilateral envelopes, when a USV
violates the constraint of a convex quadrilateral en-
velope, it definitely violates the constraints of four
sides of the convex quadrilateral envelope at the
same time. After four times of multiplications, its
penalty value will far bigger than that violating the
circular envelope. Thus, this paper uses the scaling
factor κ to scale the constraint of convex quadrilat-
eral envelopes.
At the early stage of optimization, there are many
constraint violations. To widen the penalty degrees
for constraints, we can introduce ϵ to increase the
penalty factor, so as to reduce the penalty weight
and increase the relaxation factor. As the optimiza-
tion proceeds, the constraint violations will gradual-
ly decrease. We can reduce the penalty factor by de-
creasing ϵ, so as to increase the penalty weight and
decrease the relaxation factor, eventually further re-
LI Q L, et al. Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehiclebased on exact penalty function 27
downloaded from www.ship-research.com
CHINESE JOURNAL OF SHIP RESEARCH,VOL.16,NO.1,FEB. 2021
ducing the number of constraint violations. When ϵ
tends to 0, the penalty weight will approximate to
infinity and the relaxation and penality factors will
tend to 0. At this point, the objective function J af-
ter optimization can be considered as fully satisfy-
ing the constraints.
It should be noted that ϵ can only approximate to
0 in the optimization process if δ is positive infinity
in theory. In practice, ϵ is considered small enough
when it is below a certain threshold, so δ can be set
as a large constant. For the relaxation factor weight
W, its specific value can be adjusted according to re-
laxation degrees of different penalty terms. It can be
accepted that the settings of other parameters only
satisfy the constraints required by theories [16].
Eq. (12) is a standard selection problem of opti-
mal parameters. The gradient equations of the objec-
tive function J are required to transform Eq.(12) to
a conventional non-linear optimization problem [15],
namely
(17)
With Eq.(17), Eq.(12) can be transformed to a se-
lection problem of optimal parameters without state
constraints, and it can be solved by any effective hi-
erarchical optimizer.
3 Numerical simulation
To verify the feasibility of the proposed algo-
rithm, we set that the initial state of the USV is
X(0) = [0, 0, 0.75, 5]T, the position of the terminal
point is (xT, yT) = (500, 500), and the maximum
speed is Vmax = 8 m/s. The related parameters of the
penalty function are α = 1.5, β = 3, γ = 3, δ = 108;
κ = 10-4; Wi = 0.3, where i = 1, 2, 3, 4, 5, 6; Wj =
0.3, where j = 1, 2. The other parameters are listed
in Table 1.
To verify the universality of the envelope model-
ing for prohibited areas, this paper sets circular and
convex quadrilateral envelopes with different radii
in the simulation. The simulation results by Matlab
are shown in Fig.7 (the areas filled by blue are actu-
ally prohibited areas, and the dashed envelopes are
prohibited areas considering the safety threshold).
According to the simulation results, the USV can to-
tally avoid the prohibited areas starting from the ini-
tial position. The total duration of the ideal trajecto-
ry is 100.6 s. It can be seen from Fig.7 that to mini-
mize the navigation time, the trajectory is tangential
to one prohibited area. In practice, the influences of
some uncertain factors may bring about the colli-
sion risk for USVs. However, this paper sets the
safety threshold, which can assure the safety dis-
tance between USVs and actually prohibited areas.
Fig.8 shows the curve of the USV position varying
with time.
Fig. 9 and Fig. 10 demonstrate the curves of the
speed and the control quantity of the USV varying
with time, respectively. The simulation results satis-
fy related constraints. It can be seen from Fig.9 that
to minimize the navigation time, the USV acceler-
ates from the initial state to the maximum naviga-
tion speed before 80 s and maintains around the
maximum speed. After 80 s, as the USV adjusts the
navigation direction with a big angle, it should
weigh and balance between the turning radius and
Term Value
Maximum acceleration
Maximum angular speed
Table 1 Simulation data
Safety threshold
Circular prohibited area 1/m
Circular prohibited area 2/m
Circular prohibited area 3/m
Circular prohibited area 4/m
Circular prohibited area 5/m
Convex quadrilateralprohibited area 1 /m
Convex quadrilateralprohibited area 2 /m
28
downloaded from www.ship-research.com
speed to minimize the navigation time. Although a
small turning radius can shorten the turning range,
the navigation speed is required to be reduced at the
same time. It can be seen from the simulation re-
sults that to minimize the navigation time, the rud-
der of this USV is hard over in the positive direc-
tion, and the navigation speed reduces, so the opti-
mal turning radius can be matched. It can be seen
that the simulation results verify the effectiveness
of the proposed algorithm.
4 Conclusion
This paper used circular and convex quadrilateral
envelopes to process irregular obstacles, and trans-
formed the problem of avoiding irregular obstacles
to inequality constraints under the Cartesian coordi-
nates, so as to establish the obstacle-avoidance tra-
jectory planning of USVs as a time-optimal control
problem including the continuous state inequality
constraint and the terminal constraint. By control
parameterization and time scale transformation, the
time-optimal control problem was further trans-
formed to a selection problem of optimal parame-
ters. Meanwhile, by the exact penalty function, the
continuous state inequality constraint and the termi-
nal constraint were established as the constraint pen-
alty function, which was then added to the objective
function. At last, a selection problem of optimal pa-
rameters without state constraints was established,
which can be solved by any effective hierarchical
optimization technology. Thus, the proposed algo-
rithm could effectively process the obstacle avoid-
ance problem in the USV trajectory planning.
References
[1] ZHANG SH K, LIU ZH J, ZHANG X K, et al. Devel-
opment and prospect of unmanned ship [J]. World
Shipping, 2015, 38 (9): 29-36 (in Chinese).
[2] ZHANG SH K, LIU ZH J, CAI Y, et al. Review on au-
tomatic routing technologies for unmanned vehicles
[J]. Navigation of China, 2019, 42 (3): 6-11 (in Chi-
nese).
[3] LI W H, ZHANG J Y, LIN S Y, et al. The development
path of maritime autonomous surface ships technology
[J]. Ship Engineering, 2019, 41 (7): 64-73 (in Chi-
nese).
Fig.7 Track map of USV
Starting pointEnd pointTrajectory
Fig.8 Diagram of USV position changing with time
Fig.9 Diagram of USV speed changing with time
Fig.10 Input control signal
Boundary value
γ(t)Boundary value
a(t)Boundary value
Ang
ular
spee
d/(r
ad·s
-1 )A
ccel
erat
ion/
(m·s
-2 )
LI Q L, et al. Trajectory planning and automatic obstacle avoidance algorithm for unmanned surface vehiclebased on exact penalty function 29
downloaded from www.ship-research.com
CHINESE JOURNAL OF SHIP RESEARCH,VOL.16,NO.1,FEB. 2021
[4] YU B X, CHU X M, LIU C G, et al. A path planning
method for unmanned waterway survey ships based on
improved A* algorithm [J]. Geomatics and Informa-
tion Science of Wuhan University, 2019, 44 (8): 1258-1264 (in Chinese).
[5] SONG R, LIU Y C, BUCKNALL R. Smoothed A* al-
gorithm for practical unmanned surface vehicle path
planning [J]. Applied Ocean Research, 2019, 83: 9-20.
[6] LIU Y C, BUCKNALL R. Path planning algorithm for
unmanned surface vehicle formations in a practical
maritime environment [J]. Ocean Engineering, 2015,
97: 126-144.
[7] CHO Y, HAN J, KIM J, et al. Experimental validation
of a velocity obstacle based collision avoidance algo-
rithm for unmanned surface vehicles [J]. IFAC-Papers
Online, 2019, 52 (21): 329-334.
[8] OUYANG Z L, WANG H D, WANG J Y, et al. Auto-
matic collision avoidance algorithm for unmanned sur-
face vessel based on improved Bi-RRT algorithm [J].
Chinese Journal of Ship Research, 2019, 14 (6): 8-14
(in Chinese).
[9] SHU Z Y. Global path planning of unmanned surface
vessel based on multi-objective hybrid particle swarm
algorithm [D]. Wuhan: Wuhan University of Technolo-
gy, 2017 (in Chinese).
[10] QIU CH, ZHOU H F, WANG R J, et al. Track plan-
ning of unmanned lifeboat based on improved ant colo-
ny algorithm [J]. Journal of Jimei University (Natural
Science), 2019, 24 (5): 358-363 (in Chinese).
[11] SHEN H Q. Collision avoidance navigation and con-
trol for unmanned marine vessels based on reinforce-
ment learning [D]. Dalian: Dalian Maritime Universi-
ty, 2018 (in Chinese).
[12] WEI X Y. Research on key technologies of autono-
mous local obstacle avoidance system for unmanned
surface vehicles [D]. Guangzhou: South China Univer-
sity of Technology, 2019 (in Chinese).
[13] LI B, YU C J, TEO K L, et al. An exact penalty func-
tion method for continuous inequality constrained opti-
mal control problem [J]. Journal of Optimization Theo-
ry and Applications, 2011, 151 (2): 260-291.
[14] LI B, XU C, TEO K L, et al. Time optimal Zermelo's
navigation problem with moving and fixed obstacles
[J]. Applied Mathematics and Computation, 2013, 224:
866-875.
[15] TEO K L, GOH C J, WONG K H. A unified computa-
tional approach to optimal control problems [M]. Es-
sex: Longman Scientific and Technical, 1991.
[16] YU C J, TEO K L, ZHANG L S, et al. A new exact
penalty function method for continuous inequality con-
strained optimization problems [J]. Journal of Industri-
al and Management Optimization, 2010, 6 (4): 895-910.
基于精确罚函数的无人艇航迹规划和自动避障算法
李清亮 1,李彬*2,孙国皓*2,崔星 3,毛新涛 3
1 四川大学 电气工程学院,四川 成都 610065
2 四川大学 空天科学与工程学院,四川 成都 610065
3 北京轩宇空间科技有限公司,北京 100194
摘 要:[目的目的]对于已知多个障碍物的局部水域,如何规划安全高效的无人艇(USV)运动航迹,是当前的研究
热点。[方法方法]首先,采用简洁有效的圆形包络面和凸四边形包络面处理障碍物区域,并将避障问题转化为时
间最优控制问题中的状态不等式约束;然后,利用控制参数化和时间尺度变换,将时间最优控制问题转化为最
优参数选择问题;最后,对于由多个障碍物带来的多个连续状态不等式约束,采用精确罚函数法将所有的状态
约束都附加到目标函数中,从而构建适用于任何有效优化技术予以求解的非线性优化问题。[结果结果]数值仿真
结果表明,该算法所规划的航迹能成功规避水域中的所有障碍物,同时符合无人艇的运动特性。[结论结论]研究
成果可为无人艇航迹规划的避障问题提供参考。
关键词:无人艇;航迹规划;最优控制;精确罚函数;障碍规避
30
downloaded from www.ship-research.com
top related