时间最优快速三维避障路径规划方法发明专利.pdfVIP

(19)中华人民共和国国家知识产权局

(12)发明专利申请

(10)申请公布号CN109828600A

(43)申请公布日

2019.05.31

(21)申请号201910018101.4

(22)申请日2019.01.09

(71)申请人北京理工大学

地址100081北京市海淀区中关村南大街5

号

(72)发明人姜欢刘新福

(74)专利代理机构北京理工正阳知识产权代理

事务所(普通合伙)11639

代理人邬晓楠

(51)Int.Cl.

G05D1/10(2006.01)

权利要求书7页说明书16页附图8页

(54)发明名称

时间最优快速三维避障路径规划方法

(57)摘要

本发明公开的时间最优快速三维避障路径

规划方法，尤其涉及无人机避障路径规划方法，

属于无人机路径规划领域。本发明公开的时间最

优快速三维避障路径规划方法，通过考虑最大加

速度以及障碍约束条件，建立一个以各方向加速

度为控制量包含飞行时间和的无人最优控制模

型；然后将原非凸非线性的优化问题松弛为一个

二阶锥规划问题；最后通过迭代求解一系列二阶

锥规划问题得到原问题的解并得到速度方向的

最优变化策略，即通过协调飞行时间和飞行速度

方向以实现时间最优的避障路径规划，能够实现

避障路径和相关控制量的在线规划，且能够通过

A所优化的时间更优的飞行轨迹进一步提升无人

0机的任务反应能力。

0

6

8

2

8

9

0

1

N

C

CN109828600A权利要求书1/7页

1.时间最优快速三维避障路径规划方法，其特征在于：包括如下步骤，

步骤一：对无人机进行运动学建模并量纲归一化，建立三维无量纲运动方程；

步骤二：根据无人机避障飞行的具体要求建立速度和控制量的约束条件，给出障碍的

三维球和圆柱描述，选取时间最小作为优化目标，建立无人机避障路径时间最优控制问题

P0；

步骤三：把原无人机避障路径时间最优控制问题P0中的非线性动力学变换为线性动力

学，将原P0问题转化为问题P1；

时间自由最优控制问题能够通过增加一个参数转化为固定时间的最优控制问题；在原

P0问题中，初始时间是固定的，末端时间是自由的；将问题P0转化为具有固定初始时间和固

定结束时间的最优控制问题；

步骤四：通过凸松弛将问题P1中存在或者引入的非凸约束转化为凸约束，进而把P1问

题松弛为凸优化问题P2；

步骤五：在[t,t]上用(N+1)个离散点将问题P2离散形成二阶锥规划问题P3；所述(N+

0f

1)个离散点即{t,...,t}；

0N

步骤六：迭代求解步骤五得到的二阶锥规划问题P3，在每次迭代中，首先计算P3中的依

赖参数y(k)，然后再次求解P3问题，得到一个新的解，用于更新下一次迭代中的参数；重复

这个过程，直到当前的解与上一步的解一致，即实现通过协调飞行时间和飞行速度方向以

实现时间最优的避障路径规划，通过所优化的最优的避障路径飞行轨迹提升无人机执行任

务的反应能力。

2.如权利要求1所述的时间最优快速三维避障路径规划方法，其特征在于：步骤一具体

实现方法为，对无人机进行运动学建模，并量纲归一化，无人机三维避障的无量纲运动方程

表示为：

T

其中，[x，y，z]是无人机的空间位置，z是高度，x，y是水平面正交方向的坐标；Vc是无人

机速度，为已知量；ψ和φ分别为飞行路径角和航向角；在式(1)中，除了ψ和φ以外，距离