ＴＰ２４２．６ 单位代码： １０３３５

分类号：

密级： 公开 学 号： ￡型塑！多

洳专：ｊ、法
博士专业学位论文

⑧
中文论文题目： 多旋翼飞行器

遨应堂重塾塑型
Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ—
英文论文题目：一Ｋｉｎｏｄｙｎａｍｉｃ
ｆｏｒ Ｍｕｌｔｉｒｏｔｏｒ Ａｅｒｉａｌ Ｖｅｈｉｃｌｅｓ

申请人姓名： 叶鸿凯

指导教师： 高飞 特聘副研燧
合作导师： 许超 教授

学科（专业）： 电子信息

研究方向： 自主导航与运动规划

所在学院： 控制科学与工程学堕

论文递交日期二零二二年四月

万方数据
 多旋翼飞行器

动力学运动规划

⑧
论文作者签名：

指导教师签名：

论文评阅人ｌ： 隐名评阅

评阅人２： 隐名评阅

评阅人３： 隐名评阅

评阅人４： 隐名评阅

评阅人５： 隐名评阅

答辩委员会主席： 吴 俊 教 授 浙江大学

委员１：

委员２：

委员３：

委员４：

委员５：

答辩日期 ２０２２年０５月３０日

万方数据
 ｆｏｒ Ｍｕｉｔｉｒｏｔｏｒ Ａｅｒｉａｌ Ｖｅｈｉｃｌｅｓ

Ａｕｔｈｏｒ’Ｓ ｓｉｇｎａｔｕｒｅ：

Ｓｕｐｅｒｖｉｓｏｒ’Ｓ ｓｉｇｎａｔｕｒｅ： 红恳臣护

Ｅｘｔｅｒｎａｌ ｒｅｖｉｅｗｅｒｓ： 垒垦Ｑ望Ｙ塑Ｑ坚

△旦Ｑ望Ｙ坐Ｑ堕

垒旦Ｑ望Ｙ坐Ｑ坠璺

垒旦Ｑ望Ｙ塑Ｑ坚

垒旦Ｑ旦Ｙ坐Ｑ坠曼

Ｅｘａｍｉｎｉｎｇ Ｃｏｍｍｉｔｔｅｅ Ｃｈａｉｒｐｅｒｓｏｎ：

Ｐｒｏｆ．Ｊｕｎ Ｗｕ ＺＪＵ

Ｅｘａｍｉｎｉｎｇ Ｃｏｍｍｉｔｔｅｅ Ｍｅｍｂｅｒｓ：

垒墅Ｑ鱼ｉ堂皇旦！Ｑ￡兰垒璺必ｉ星墨旦里旦 旦鉴堕兰！
Ａｓｓｏｃｉａｔｅ Ｐｒｏｆ．Ｙｉ Ｚｈｏｕ ＨＮＵ

垒墅Ｑ旦ｉ堂皇旦！Ｑ￡．．№曼坠垒望ｇ 圣！旦

Ｄａｔｅ ｏｆ ｏｒａｌ ｄｅｆｅｎｃｅ： 丛型三Ｑ坐１ ２Ｑ呈呈

万方数据
 浙江大学研究生学位论文独创性声明

本人声明所呈交的学位论文是本人在导师指导下进行的研究工作及取得的研究成

果。除了文中特别加以标注和致谢的地方外，论文中不包含其他人已经发表或撰写过的

研究成果，也不包含为获得浙江大学或其他教育机构的学位或证书而使用过的材料。

与我一同工作的同志对本研究所做的任何贡献均已在论文中作了明确的说明并表示谢

意。

学位论文作者签名
１裼岫 签字日期：沙Ｖｙ年６月‘日

学位论文版权使用授权书

本学位论文作者完全了解浙江大学有权保留并向国家有关部门或机构送交本论

文的复印件和磁盘，允许论文被查阅和借阅。本人授权浙江大学可以将学位论文的全

部或部分内容编入有关数据库进行检索和传播，可以采用影印、缩印或扫描等复制手段

保存、汇编学位论文。

（保密的学位论文在解密后适用本授权书）

学位论文作者签名：

签字日期：沪１１年
１裼粕
占月 ６日
导师签名

签字日期：
稚高仉
年 月 曰

万方数据
 致谢

自１３年本科入学，我已然在求是园度过近九年时光，而今将要离开，许多往事依

旧历历在目，我想诚挚地感谢在这一段旅程中与我相识相知相助的人们。

我发自内心地感谢我的导师许超教授和高飞老师，你们的辛劳成就了现在的所有，

创建且维系着有爱的实验室大家庭。自大三的一个科研训练项目开始，许超老师便指导

我的科研。许老师勤勉尽责，对事物总是保持着极高的热忱。我们时常感叹于其知识渊

博且思维活跃。高飞老师则更加亦师亦友，从本科时的学长变为我硕博连读时期的导

师。我永远会记得的是高老师教会我要对问题本质进行不断思考，永远不要停止思考。

两位老师有着宽广又超前的视野，果敢而准确的决断，是我终身学习的榜样。能受两位

老师多年的言传身教，我实属幸运。

由衷感谢实验室的兄弟姐妹，从ＺＭＡＲＴ，到ＦＡＳＴＬＡＢ，从老和山脚，经甬江之

滨，到西塞山下，一起经历的跌宕愈之加深了我们感情。实验室于我一直是一个家，我

也将永远视这里为家。感谢朱疆成、邱炜、崔粲、王宏达几位师兄在我刚进入实验室时

对我的一切关照，你们带领我快速成长，完成从懵懂的本科生到研究生的转变，我会记

得那些饱含笑与泪的成都和北京之旅。感谢蔡声泽、叶长春、叶波、茹祥宇、翁一桢、

黄永斌、郭磊、朱均、万旭东、王瀚林几位师兄对我在科研和生活上的关照，我依旧回

味那些封存在已经消失的３０１、３０２，以及健身房和球场上的欢乐。令我感动和开心的是

毕业后大家也都还保持着联系，互相交流帮助。感谢汪哲培、张煜恰、熊坤、梁家铭几

位同期伙伴一路上的扶持，Ｐａｐｅｒ、比赛、项目，不论高峰还是低谷，大家一起经历了最

长的风风雨雨，你们会是一辈子最诚挚的朋友，我永远珍视，祝你们今后一切顺利。感

谢丁梓明、王立子、全伦、王钱浩、纪佳林、周鑫、杨天凯几位师弟，你们每个人都有着

非常鲜明的优点，而靠谱是共同点。和你们从３０４到宁波再到湖州，一起经历了ＦＡＳＴ

ＬＡＢ初期的建设，你们在我心里不仅是在科研领域能够独当一面的师弟。更是能够知

心交谈的朋友，和你们在一起的时光我非常快乐。感谢仲星光、殷隆基、贾振、侯嘉良、

温向勇、吴雨薇以及其他所有在ＦＡＳＴＬＡＢ相遇的朋友，有些情谊无法用相识的时间长
短衡量，一经熟识便知道会是永远的朋友。感谢所有师弟师妹，你们的天赋和成就比之

我绝对是有过之而无不及，与你们一起学习合作是我的荣幸。限于篇幅，对所有未具名

万方数据
 朋友的感谢暂且留于心中。

感谢我的朋友朱麒霈、叶林、孙健、孙聪，生活五味杂陈，深深感谢你们的支撑。

感谢我的家人。

如果这是Ｈａｒｒｙ Ｐｏｔｔｅｒ的魔法世界，那我希望习得提取记忆的咒语，有些能将之消

除，而有些能够永远存放。

生活塑之我独立而理性，而我固爱此间遇见的每一个人。

叶鸿凯

２０２２年６月１７日凌晨于浙大湖州研究院

万方数据
 摘要

近年来，小型多旋吴飞行器在航拍摄影、巡检监控、农业喷洒、物流配送等领域渐

渐崭露头角，随着其在各行业中的应用越来越广泛，对其自动化、自主化、智能化作业

的需求愈发强烈，开发飞行器自主导航系统成为研究热点。稳定的感知以及控制能够保

证自主导航系统的下限，而鲁棒且高效的运动规划则决定飞行作业任务完成情况的上

限。随着飞行作业的场景愈发偏向近地面的低空复杂环境，导航时对避障的要求越来越

高，考虑避障约束、飞行器的物理限制、以及作业任务需求的运动规划在整体自主导航

系统中扮演的角色越来越关键。

归纳飞行器自主导航中常见的三大类情形：一、全局规划。已知较大范围精确的静

态全局地图，在此情况下进行全局最优轨迹规划，导航执行此轨迹的过程中无需动态感

知与重规划；二、局部规划。无先验地图信患，只能实时动态感知自身周围的小范围环

境，此时导航过程中需要根据最新的精确地图信息不断进行重规划，每次重规划称作局

部轨迹规划；三、跟踪规划。已知粗略的大范围全局地图，可据此规划出全局轨迹，但

是由于全局地图不精确，甚至是过时的，此全局轨迹可能与实际的障碍物发生碰撞，因

而在导航跟踪全局参考轨迹的过程中仍然需要动态感知，获得精确的小范围地图，并且

进行实时重规划以避开实际的障碍物。本文分析多旋翼飞行器在上述情形下自主导航中

的运动规划侧重点并展开研究，主要内容如下：

１．对于全局规划情形，规划算法重点需要考虑是否具有全局最优性以及能否快速收

敛。本文使用在采样类动力学规划方法中深度结合局部优化的求解方法，提出时空形变

树算法，在探索解空间的过程中生长一棵在时间和空间层面同时发生形交的轨迹树，随

着树的生长自然地选择拓扑路径更好的轨迹，直至逼近全局最优解。树中一段轨迹边的

生长建模为求解末状态部分受约束的两点边值问题，使用约束松弛策略加速求解。本文

提出形变单元的概念，其仅包含树中的一个状态节点以及连接在其上的所有轨迹边，只

占整体轨迹树的一小部分。我们针对性地设计用形交单元表示的轨迹树整体质量性能指

标，在时间和空间层面优化不同形变单元的组合，花费尽量小的代价高效提升整体轨迹

树的质量，提升算法的收敛性。

２．对于局部规划情形，规划算法重点需要考虑飞行器的运动初始状态和求解的实时

Ｉｎ

万方数据
 性以应对实时变化的环境。本文使用采样类前端规划衔接高效后端提优的层级式规划框

架，每次重规划求解一条满足实时性且足够好的局部最优轨迹。本文根据环境快速构建

近似拓扑图，用其引导在状态空间的采样，极大提升了采样类动力学运动规划算法的求

解效率。针对轨迹提优，本文提出基于双层优化求解的轻量级后端算法，内层求解具备

闭式解形式，有效利用前端轨迹，花费极少计算代价提升了前端结果轨迹的光滑性和连

续性。之后将所提出的规划框架与算法应用于一架全自主四旋翼飞行器上进行未知环境

下的导航飞行测试，在未知的室外稠密树林、遮挡严重的室内房间等环境验证了算法的

有效性。

３．在上述层级规划的基础上，针对采样类规划在窄缝类空间中求解低效的问题，本

文提出在采样规划中结合双向搜索与快速求解的局部优化。快速局部优化专注于探索有

限范围内的解空间，其构建为一系列的无约束二次凸优化问题，每轮迭代具有闭式解。

针对前端初值信息没有被充分利用的问题，本文在后端提优时进一步将障碍物信患构建

进性能指标中，同时保有二次规划问题的结构，使得保证求解效率的同时成功率更高。

由此增强了采样类前端以及后端提优的效率与性能，更快地求解得到一个初始可行解，

并且提优的成功率更高，从而更好满足实时重规划的需求。之后本文进行了大量数值对

比测试，并在实物四旋翼飞行器上进行导航测试，在稠密障碍物、类迷宫等环境中验证

了所提出规划方法的有效性。

４．对于跟踪规划情形，除了考虑运动初始状态和求解实时性之外，规划算法还需要

考虑与参考轨迹的时空贴合以及连续重规划之间一致性。针对避障时规划结果不连贯，

导致飞行器运动路线随重规划频繁切换，产生类似于“摇摆犹豫”的现象，增加了碰撞

风险的问题，本文定义了轨迹问的７．．拓扑连贯关系，并提出考虑拓扑连贯性的采样类动

力学规划算法，增加了连续重规划之问的路径拓扑一致性，使得整体导航过程中避障时

轨迹更加顺滑，更加安全。考虑到时空上贴合参考轨迹与避开障碍物等具有冲突性质的

约束，本文提出自适应末端状态的重规划轨迹跟踪方法，使用轨迹类、罚函数等方法消

除约束，使用梯度下降法求解。线搜索时根据丁．拓扑连贯关系计算最大步长，保持优化

的结果轨迹与初值轨迹处于同一拓扑路径。与传统固定末状态的方法相比，所提出方法

在满足避障约束的同时大大缩小了实时位置跟踪误差。此外，本文在接近真实的仿真系

统中验证了所提出的轨迹跟踪算法的切实可靠性。

关键词：多旋翼飞行器；自主导航；运动规划；最优控制

ＩＶ

万方数据
 Ａｂｓｔｒａｃｔ

Ｉｎ ｒｅｃｅｎｔ ｙｅａｒｓ，ｓｍａｌｌ－ｓｉｚｅｄ Ｍｕｌｔｋｏｔｏｒ Ａｅｒｉａｌ Ｖｅｈｉｃｌｅｓ（ＭＡＶｓ）ｈａｖｅ ｂｅｅｎ ｕｓｅｄ ｉｎ ａｅｒｉａｌ

ｐｈｏｔｏｇｒａｐｈｙ，ｉｎｓｐｅｃｔｉｏｎ ａｎｄ ｍｏｎｉｔｏｒｉｎｇ，ａｇｒｉｃｕｌｔｕｒａｌ ｓｐｒａｙｉｎｇ，ｌｏｇｉｓｔｉｃｓ ａｎｄ ｄｅｌｉｖｅｒｙ，ａｎｄ ｍａｎｙ

ｏｔｈｅｒ ｆｉｅｌｄｓ．Ａｓ ｔｈｅｉｒ ａｐｐｌｉｃａｔｉｏｎｓ ｉｎ ｖａｒｉｏｕｓ ｆｉｅｌｄｓ ｂｅｃｏｍｅ ｂｒｏａｄｅｒ，ｔｈｅ ｄｅｍａｎｄｓ ｆｏｒ ｔｈｅｉｒ ａｌｌ－

ｔｏｍａｔｉｏｎ，ａｕｔｏｎｏｍｙ，ａｎｄ ｉｎｔｅｌｌｉｇｅｎｔ ｏｐｅｒａｔｉｏｎ ａｒｅ ｇｅｅｉｎｇ ｓｔｒｏｎｇｅｒ＇ａｎｄ ｄｅｖｅｌｏｐｉｎｇ ａｕｔｏｎｏｍｏｕｓ

ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｅｍｓ ｆｏｒ ＭＡＶｓ ｈａｓ ｂｅｃｏｍｅ ａ ｒｅｓｅａｒｃｈ ｈｏｔｓｐｏｔ．Ⅵｍｉｌｅ ｓｔａｂｌｅ ｐｅｒｃｅｐｔｉｏｎ ａｎｄ ｃｏｎ－

ｔｒｏｌ ｃａｎ ｅｎｓｕｒｅ ａ ｌｏｗｅｒ ｐｅｒｆｏｒｍａｎｃｅ ｂｏｕｎｄ ｏｆ ｔｈｅ ａｕｔｏｎｏｍｏｕｓ ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｅｍｓ，ｒｏｂｕｓｔ ａｎｄ

ｅｆｆｉｃｉｅｎｔ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｄｅｔｅｒｍｉｎｅｓ ｈｏｗ ｗｅｌｌ ｔｈｅ ｍｉｓｓｉｏｎｓ ｃａｌｌ ｂｅ ａｃｃｏｍｐｌｉｓｈｅｄ．Ａｓ ｔｈｅ ｏｐ－

ｅｒａｔｉｏｎ ｓｃｅｎａｒｉｏｓ ｉｎｃｌｉｎｅ ｔｏ ｔｈｅ ｌｏｗ－ａｌｔｉｔｕｄｅ ｎｅａｒ－ｇｒｏｕｎｄ ｃｏｍｐｌｅｘ ｅｎｖｉｒｏｎｍｅｎｔ，ｔｈｅ ｄｅｍａｎｄｓ

ｆｏｒ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｄｕｒｉｎｇ ｎａｖｉｇａｔｉｏｎ ｂｅｃｏｍｅ ｈｉｇｈｅｒ，ａｎｄ ｔｈｅ ｒｏｌｅ ｏｆ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｃｏｎ－

ｓｉｄｅｒｉｎｇ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ，ｐｈｙｓｉｃａｌ ｖｅｈｉｃｌｅ ｌｉｍｉｔａｔｉｏｎｓ，ａｎｄ ｔａｓｋ ｒｅｑｕｉｒｅｍｅｎｔｓ ｂｅｃｏｍｅｓ ｍｏｒｅ

ｃｒｉｔｉｃａｌ ｉｎ ｔｈｅ ｏｖｅｒａｌｌ ａｕｔｏｎｏｍｏｕｓ ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｅｍ．

Ｔｈｒｅｅ ｍａｉｎ ｃｏｍｍｏｎ ｓｉｔｕａｔｉｏｎｓ ｉｎ ＭＡＶ ａｕｔｏｎｏｍｏｕｓ ｎａｖｉｇａｔｉｏｎ ａｒｅ ｓｕｍｍａｒｉｚｅｄ：１．Ｇｌｏｂａｌ

ｐｌａｎｎｉｎｇ．Ｉｎ ｔｈｉｓ ｃａｓｅ，ａ ｌａｒｇｅ－ｓｃａｌｅ ａｎｄ ａｃｃｕｒａｔｅ ｓｔａｔｉｃ ｇｌｏｂａｌ ｍａｐ ｃａｎ ｂｅ ａｃｑｕｋｅｄ．Ｔｈｅ ｇｌｏｂａｌ

ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ｉｓ ｐｅｒｆｏｒｍｅｄ，ａｎｄ ｔｈｅ ｎａｖｉｇａｔｉｏｎ ｅｘｅｃｕｔｉｏｎ ｏｆ ｔｈｉｓ ｔｒａｊｅｃｔｏｒｙ ｉｓ ｐｅｒ－

ｆｏｒｍｅｄ ｗｉｔｈｏｕｔ ｏｎｌｉｎｅ ｓｅｎｓｉｎｇ ａｎｄ ｒｅｐｌａｎｎｉｎｇ；２．Ｌｏｃａｌ ｐｌａｎｎｉｎｇ．Ｔｈｅｒｅ ｉｓ ｎｏ ａ ｐｒｉｏｒｉ ｍａｐ

ｉｎｆｏｒｍａｔｉｏｎ，ａｎｄ ｏｎｌｙ ａ ｓｍａｌｌ ａｒｅａ ａｒｏｕｎｄ ｔｈｅ ＭＡＶ ｉｔｓｅｌｆ ｃａｎ ｂｅ ｓｅｎｓｅｄ ｄｙｎａｍｉｃａｌｌｙ ｉｎ ｒｅａｌ－

ｔｉｍｅ，ｓｕｃｈ ｔｈａｔ ｔｈｅ ｎａｖｉｇａｔｉｏｎ ｐｒｏｃｅｓｓ ｒｅｑｕｉｒｅｓ ｒｅｐｌａｎｎｉｎｇ ｃｏｎｔｉｎｕｏｕｓｌｙ ａｃｃｏｒｄｉｎｇ ｔｏ ｔｈｅ ｌａｔｅｓｔ

ａｃｃｕｒａｔｅ ｍａｐ ｉｎｆｏｒｍａｔｉｏｎ．Ｅａｃｈ ｒｃｐｌａｎｎｉｎｇ ｉｓ ｃａｌｌｅｄ ｌｏｃａｌ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ；３．Ｔｒａｃｋｉｎｇ

ｐｌａｎｎｉｎｇ．Ａ ｒｏｕｇｈ ｌａｒｇｅ－ｓｃａｌｅ ｇｌｏｂａｌ ｍａｐ ｉｓ ｋｎｏｗｎ，ａｎｄ ａ ｇｌｏｂａｌ ｔｒａｊｅｃｔｏｒｙ ｃａｌｌ ｂｅ ｐｌａｎｎｅｄ

ａｃｃｏｒｄｉｎｇｌｇ Ｈｏｗｅｖｅｒ，ｔｈｅ ｇｌｏｂａｌ ｍａｐ ｉｓ ｉｎａｃｃｕｒａｔｅ ｏｒ ｏｕｔｄａｔｅｄ，ａｎｄ ｔｈｅ ｇｌｏｂａｌ ｔｒａｊｅｃｔｏｒｙ ｍａｙ
ｃｏｌｌｉｄｅ ｗｉｔｈ ａｃｔｕａｌ ｏｂｓｔａｃｌｅｓ．Ｔｈｅｒｅｆｏｒｅ，ｉｎ ｔｒａｃｋｉｎｇ ｔｈｅ ｇｌｏｂａｌ ｒｅｆｅｒｅｎｃｅ ｔｒａｊｅｃｔｏｒｙ，ｔｈｅｒｅ ｉｓ ｓｔｉｌｌ

ａ ｎｅｅｄ ｆｏｒ ｄｙｎａｍｉｃ ｐｅｒｃｅｐｔｉｏｎ ｔｏ ｂｕｉｌｄ ａｃｃｕｒａｔｅ ａｒｏｕｎｄｉｎｇ ｍａｐｓ，ａｓ ｗｅｌｌ ＩｔＳ ｒｅａｌ－ｔｉｍｅ ｒｅｐｌａｎｎｉｎｇ

ｔｏ ａｖｏｉｄ ａｃｔｕａｌ ｏｂｓｔａｃｌｅｓ．Ｔｈｉｓ ｐａｐｅｒ ｓｔｕｄｉｅｓ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｏｆＭＡＹ ａｕｔｏｎｏｍｏｕｓ ｎａｖｉｇａｔｉｏｎ

ｉｎ ｔｈｅ ａｂｏｖｅ ｓｉｔｕａｔｉｏｎｓ，ｗｈｉｃｈ ｍａｉｎｌｙ ｉｎｃｌｕｄｅｓ：

１．Ｆｏｒ ｔｈｅ ｇｌｏｂａｌ ｐｌａｎｎｉｎｇ ｓｉｔｕａｔｉｏｎ，ｔｈｅ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍ ｓｈｏｕｌｄ ｆｏｃｕｓ ｏｎ ａｃｈｉｅｖｉｎｇ

ｇｌｏｂａｌ ｏｐｔｉｍａｌｉｔｙ ａｎｄ ｑｕｉｃｋ ｃｏｎｖｅｒｇｅｎｃｅ．Ｏｕｒ ｓｏｌｕｔｉｏｎ ｄｅｅｐｌｙ ｃｏｍｂｉｎｅｓ ｌｏｃａｌ ｏｐｔｉｍｉｚａｔｉｏｎ ｗｉｔｈ

万方数据
 ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｍｅｔｈｏｄｓ．Ｗｅ ｐｒｏｐｏｓｅ ａｎ ａｌｇｏｒｉｔｈｍ ｃａｌｌｅｄ ｓｐａｔｉｏ－ｔｅｍｐｏｒａｌ ｄｅｆｏｒｍａｂｌｅ ｔｒｅｅ ｔｈａｔ

ｇｒｏｗｓ ａ ｔｒａｊｅｃｔｏｒｙ ｔｒｅｅ ｔｏ ｅｘｐｌｏｒｅ ｔｈｅ ｓｏｌｕｔｉｏｎ ｓｐａｃｅ ａｎｄ ｍｅａｎｗｈｉｌｅ ｄｅｆｏｒｍｓ ｓｉｍｕｌｔａｎｅｏｕｓｌｙ
ｉｎ ｔｈｅ ｔｉｍｅ ａｎｄ ｓｐａｃｅ ｄｉｍｅｎｓｉｏｎｓ．Ｉｔ ｎａｔｕｒａｌｌｙ ｓｅｌｅｃｔｓ ａ ｔｒａｊｅｃｔｏｒｙ ｗｉｔｈ ｂｅｔｔｅｒ ｐａｔｈ ｔｏｐｏｌｏｇｙ

（ｈｏｍｏｔｏｐｙ ｃｌａｓｓ）ａｓ ｔｈｅ ｔｒｅｅ ｇｒｏｗｓ ｕｎｔｉｌ ａｐｐｒｏａｃｈｉｎｇ ｔｈｅ ｎｅａｒ ｇｌｏｂａｌ ｏｐｔｉｍａｌ．Ｔｈｅ ｇｒｏｗｔｈ ｏｆ ａ

ｔｒｅｅ ｅｄｇｅ ｉｓ ｍｏｄｅｌｅｄ ａｓ ｓｏｌｖｉｎｇ ａ ｔｗｏ—ｐｏｉｎｔ ｂｏｕｎｄａｒｙ ｖａｌｕｅ ｐｒｏｂｌｅｍ ｗｉｔｈ ａ ｐａｒｔｉａｌｌｙ ｃｏｎｓｔｒａｉｎｅｄ

ｅｎｄ ｓｔａｔｅ，ａｎｄ ａ ｃｏｎｓｔｒａｉｎｔ ｒｅｌａｘａｔｉｏｎ ｓｔｒａｔｅｇｙ ｉｓ ｕｓｅｄ ｔｏ ａｃｃｅｌｅｒａｔｅ ｔｈｅ ｓｏｌｖｉｎｇ ｐｒｏｃｅｓｓ．Ｗｅ

ｄｅｆｉｎｅ ｔｈｅ ｃｏｎｃｅｐｔ ｏｆ ａ ｄｅｆｏｒｍａｔｉｏｎ ｕｎｉｔ，ｗｈｉｃｈ ｏｎｌｙ ｃｏｎｔａｉｎｓ ｏｎｅ ｓｔａｔｅ ｎｏｄｅ ｉｎ ｔｈｅ ｔｒｅｅ ａｎｄ ａｌｌ

ｔｈｅ ｔｒａｊｅｃｔｏｒｙ ｅｄｇｅｓ ｃｏｎｎｅｃｔｅｄ ｔｏ ｉｔ，ａｃｃｏｕｎｔｉｎｇ ｆｏｒ ｏｎｌｙ ａ ｓｍａｌｌ ｐａｒｔ ｏｆ ｔｈｅ ｏｖｅｒａｌｌ ｔｒａｊｅｃｔｏｒｙ

ｔｒｅｅ．Ｗｅ ｄｅｓｉｇｎ ｏｂｊｅｃｔｉｖｅｓ ｅｘｐｒｅｓｓｉｎｇ ｔｈｅ ｏｖｅｒａｌｌ ｔｒｅｅ ｑｕａｌｉｔｙ ｂｙ ｍｅａｎｓ ｏｆ ｄｅｆｏｒｍａｔｉｏｎ ｕｎｉｔｓ，

ａｎｄ ｏｐｔｉｍｉｚｅ ｉｎ ｂｏｔｈ ｔｉｍｅ ａｎｄ ｓｐａｃｅ ｄｉｍｅｎｓｉｏｎｓ ｗｉｔｈ ｄｉｆｆｅｒｅｎｔ ｃｏｍｂｉｎａｔｉｏｎｓ ｏｆｔｈｅ ｕｎｉｔｓ．Ｉｎ ｔｈｉｓ

ｗａｙ，ｔｈｅ ｏｖｅｒａｌｌ ｔｒｅｅ ｑｕａｌｉｔｙ ｉｓ ｉｍｐｒｏｖｅｄ ｅｆｆｅｃｔｉｖｅｌｙ ａｔ ａ ｍｉｎｉｍｕｍ ｃｏｓｔ，ａｎｄ ｔｈｕｓ ｔｈｅ ｃｏｎｖｅｒｇｅｎｃｅ

ｉｓ ｉｍｐｒｏｖｅｄ．

２．Ｆｏｒ ｔｈｅ ｌｏｃａｌ ｐｌａｎｎｉｎｇ ｓｉｔｕａｔｉｏｎ，ｔｈｅ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍ ｓｈｏｕｌｄ ｃｏｎｓｉｄｅｒ ｔｈｅ ｖｅｈｉｃｌｅ ｍｏ—

ｔｉｏｎ．ｗｈｉｃｈ ｉｓ ｎｏｔ ｓｔａｔｉｏｎａｒｙ，ａｎｄ ｃｏｎｓｉｄｅｒ ｗｈｅｔｈｅｒ ｉｔ ｃａｎ ｂｅ ｓｏｌｖｅｄ ｉｎ ｒｅａｌ—ｔｉｍｅ ｔｏ ｃｏｐｅ谢ｍ ｔｈｅ

ｉｎｓｔａｎｔｌｙ ｃｈａｎｇｉｎｇ ｅｎｖｉｒｏｎｍｅｎｔ．Ｏｕｒ ｓｏｌｕｔｉｏｎ ａｄｏｐｔｓ ａ ｈｉｅｒａｒｃｈｉｃａｌ ｐｌａｎｎｉｎｇ ｆｒａｍｅｗｏｒｋ．Ｉｔ ｃｏｎ－

ｒａｉｎｓ ａ
ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｋｉｎｏｄｙｎａｍｉｅ ｆｒｏｎｔ－ｅｎｄ ｍｏｄｕｌｅ ａｎｄ ａ ｆｏｌｌｏｗｉｎｇ ｐｏｓｔ－ｐｒｏｃｅｓｓ ｂａｃｋ－ｅｎｄ

ｍｏｄｕｌｅ，ｗｈｉｃｈ ｌｏｏｋｓ ｆｏｒ ａ ｌｏｃａｌｌｙ ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｉｎ ｅａｃｈ ｒｅｐｌａｎｎｉｎｇ ｃｙｃｌｅ．Ｉｎ ｔｈｅ ｆｒｏｎｔ－ｅｎｄ，

ａｌｌ ａｐｐｒｏｘｉｍａｔｅ ｔｏｐｏｌｏｇｉｃａｌ ｇｒａｐｈ ｉｓ ｑｕｉｃｋｌｙ ｃｏｎｓｔｒｕｃｔｅｄ ａｃｃｏｒｄｉｎｇ ｔｏ ｔｈｅ ｅｎｖｉｒｏｎｍｅｎｔ ａｎｄ ｉｓ

ｕｓｅｄ ｔｏ ｇｕｉｄｅ ｔｈｅ ｓａｍｐｌｉｎｇ ｉｎ ｔｈｅ ｓｔａｔｅ ｓｐａｃｅ，ｗｈｉｃｈ ｓｉｇｎｉｆｉｃａｎｔｌｙ ｉｍｐｒｏｖｅｓ ｔｈｅ ｅｆｆｉｃｉｅｎｃｙ ｏｆ

ｔｈｅ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｋｉｎｏｄｙｎａｍｉｃ ｐｌａｎｎｉｎｇ．Ｉｎ ｔｈｅ ｂａｃｋ－ｅｎｄ，ｅｘｐｌｏｉｔｉｎｇ ｔｈｅ ｆｒｏｎｔ－ｅｎｄ ｌｅｇａｃｙ，

ｗｅ ｐｒｏｐｏｓｅ ａ
ｌｉｇｈｔｗｅｉｇｈｔ ｐｏｓｔ－ｐｒｏｃｅｓｓｉｎｇ ａｌｇｏｒｉｔｈｍ ｕｓｉｎｇ ｂｉ—ｌｅｖｅｌ ｏｐｔｉｍｉｚａｔｉｏｎ，ａｎｄ ｔｈｅ ｉｎｎｅｒ

ｌａｙｅｒ ｏｆｗｈｉｃｈ ｈａｓ ｃｌｏｓｅｄ－ｆｏｒｍ ｓｏｌｕｔｉｏｎｓ，ｉｍｐｒｏｖｉｎｇ ｔｈｅ ｓｍｏｏｔｈｎｅｓｓ ａｎｄ ｃｏｎｔｉｎｕｉｔｙ ｗｉｔｈ ｍｉｎｉ－

ｍａｌ ｃｏｍｐｕｔａｔｉｏｎ ｒｅｓｏｕｒｃｅｓ．Ｔｈｅ ｐｒｏｐｏｓｅｄ ｐｌａｎｎｉｎｇ ｆｒａｍｅｗｏｒｋ ａｎｄ ａｌｇｏｒｉｔｈｍ ａｌｅ ｔｈｅｎ ａｐｐｌｉｅｄ

ｔｏ ａ ｒｅａｌ ｑｕａｄｒｏｔｏｒ ｆｏｒ ａｕｔｏｎｏｍｏｕｓ ｎａｖｉｇａｔｉｏｎ ｔｅｓｔｓ，ｖｅｒｉｆｙｉｎｇ ｔｈｅ ａｌｇｏｆｉｔｈｒａ’Ｓ ｅｆｆｅｃｔｉｖｅｎｅｓｓ ｉｎ

ｏｂｓｃｕｒｅ ｅｎｖｉｒｏｎｍｅｎｔｓ ｓｕｃｈ ａｓ ｄｅｎｓｅ ｏｕｔｄｏｏｒ ｆｏｒｅｓｔｓ ａｎｄ ｈｅａｖｉｌｙ ｏｃｃｌｕｄｅｄ ｉｎｄｏｏｒ ｒｏｏｍｓ．

３．Ｂａｓｅｄ ｏｎ ｔｈｅ ａｂｏｖｅ—ｍｅｎｔｉｏｎｅｄ ｈｉｅｒａｒｃｈｉｃａｌ ｐｌａｎｎｉｎｇ，ｗｅ ｐｒｏｐｏｓｅ ｉｎｔｅｇｒａｔｉｎｇ ｆａｓｔ ｒｅ—

ｇｉｏｎａｌ ｏｐｔｉｍｉｚａｔｉｏｎ ａｎｄ ｂｉ—ｄｉｒｅｃｔｉｏｎａｌ ｓｅａｒｃｈ ｉｎ ｔｈｅ ｇｌｏｂａｌ ｓａｍｐｌｉｎｇ ｐｒｏｃｅｓｓ ｆｏｒ ｎａｒｒｏｗ ｓｐａｃｅ

ｐｌａｎｎｉｎｇ．Ｔｈｅ ｒｅｇｉｏｎａｌ ｏｐｔｉｍｉｚａｔｉｏｎ ｐｒｉｏｒｉｔｉｚｅｓ ｅｘｐｌｏｒｉｎｇ ｌｉｍｉｔｅｄ ｒｅｇｉｏｎａｌ ｓｐａｃｅｓ ａｎｄ ｉｓ ｓｏｌｖｅｄ

ｂｙ ａ ｓｅｑｕｅｎｃｅ ｏｆｕｎｃｏｎｓｔｒａｉｎｅｄ ｑｕａｄｒａｔｉｃ ｐｒｏｇｒａｍｍｉｎｇ ｗｉｔｈ ｃｌｏｓｅｄ－ｆｏｒｍ ｓｏｌｕｔｉｏｎｓ ｆｏｒ ｅａｃｈ ｉｔ－

万方数据
 ｅｒａｔｉｏｎ．Ｔｏ ｂｅＨｅｒ ｅｘｐｌｏｉｔ ｔｈｅ ｆｒｏｎｔ－ｅｎｄ ｌｅｇａｃｙ，ｗｅ ｆｕｒｔｈｅｒ ｉｎｃｏｒｐｏｒａｔｅ ｔｈｅ ｏｂｓｔａｃｌｅ ｉｎｆｏｒｍａｔｉｏｎ

ｉｎｔｏ ｔｈｅ ｏｂｊｅｃｔｉｖｅ ｄｅｓｉｇｎ ｏｆｔｈｅ ｂａｃｋ－ｅｎｄ ｏｐｔｉｍｉｚａｔｉｏｎ ｗｈｉｌｅ ｓｔｉｌｌ ｒｅｔａｉｎｉｎｇ ａ ｑｕａｄｒａｔｉｃ ｐｒｏｇｒａｍ－

ｍｉｎｇ ｓｔｒｕｃｔｕｒｅ ｓｕｃｈ ｔｈａｔ ｅｆｆｉｃｉｅｎｃｙ ｉｓ ｇｕａｒａｎｔｅｅｄ．Ａｓ ａ ｒｅｓｕｌｔ，ｔｈｅ ｏｖｅｒａｌｌ ｐｅｒｆｏｒｍａｎｃｅ ｏｆ ｔｈｅ

ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｆｏｎｔ－ｅｎｄ ａｎｄ ｔｈｅ ｂａｃｋ－ｅｎｄ ｏｐｔｉｍｉｚａｔｉｏｎ ｉｓ ｅｎｈａｎｃｅｄ ｉｎ ｔｈａｔ ａｎ ｉｎｉｔｉａｌ ｆｅａｓｉｂｌｅ

ｓｏｌｕｔｉｏｎ ｃａｎ ｂｅ ｏｂｔａｉｎｅｄ ｆａｓｔｅｒ，ａｎｄ ｔｈｅ ｓｎｃｃｅｓｓ ｒａｔｅ ｏｆ ｏｐｔｉｍｉｚａｔｉｏｎ ｉｓ ｈｉｇｈｅｒ，ｔｈｕｓ ｂｅｔｔｅｒ ｍｅｅｔ－

ｉｎｇ ｔｈｅ ｄｅｍａｎｄｓ ｏｆｒｅａｌ－ｔｉｍｅ ｒｅｐｌａｒｍｉｎｇ．Ｅｘｔｅｎｓｉｖｅ ｎｕｍｅｒｉｃａｌ ｃｏｍｐａｒｉｓｏｎ ｔｅｓｔｓ ａｎｄ ｎａｖｉｇａｔｉｏｎ

ｔｅｓｔｓ ｏｎ ａ ｒｅａｌ ｑｕａｄｒｏｔｏｒ ａｒｅ ｔｈｅｎ ｃｏｎｄｕｃｔｅｄ ｔｏ ｖｅｒｉｆｙ ｔｈｅ ｅｆｆｅｃｔｉｖｅｎｅｓｓ ｏｆ ｔｈｅ ｐｒｏｐｏｓｅｄ ｍｅｔｈｏｄ

ｉｎ ｄｅｎｓｅ ｏｂｓｔａｃｌｅ ａｎｄ ｍａｚｅｄｉｋｅ ｅｎｖｉｒｏｎｍｅｎｔｓ．

４．Ｉｎ ｔｈｅ ｃａｓｅ ｏｆ ｔｒａｃｋｉｎｇ ｐｌａｎｎｉｎｇ，ａｐａｒｔ ｆｒｏｍ ｃｏｎｓｉｄｅｒｉｎｇ ｔｈｅ ｖｅｈｉｃｌｅ ｍｏｔｉｏｎ ａｎｄ ｔｈｅ

ｒｅａｌ－ｔｉｍｅ ｃａｐａｂｉｌｉｔｙ，ｔｈｅ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍ ｆｕｒｔｈｅｒ ｎｅｅｄｓ ｔｏ ｒｅｓｐｅｃｔ ｔｈｅ ｓｐａｃｅ－ｔｉｍｅ ｆｉｔｎｅｓｓ ｗｉｔｈ

ｔｈｅ ｒｅｆｅｒｅｎｃｅ ｔｒａｊｅｃｔｏｒｙ ａｎｄ ｔｈｅ ｃｏｎｓｉｓｔｅｎｃｙ ｂｅｔｗｅｅｎ ｓｕｃｃｅｓｓｉｖｅ ｒｅｐｌａｎ ｃｙｃｌｅｓ．Ｔｏ ａｄｄｒｅｓｓ ｔｈｅ

ｐｒｏｂｌｅｍ ｏｆ ｉｎｃｏｈｅｒｅｎｔ ｐｌａｎｎｉｎｇ ｒｅｓｕｌｔｓ ｄｕｒｉｎｇ ｓｕｃｃｅｓｓｉｖｅ ｒｅｐｌａｎ ｃｙｃｌｅｓ ｗｈｅｎ ａｖｏｉｄｉｎｇ ｏｂｓｔａ－

ｃｌｅｓ，ｗｈｉｃｈ ｌｅａｄｓ ｔｏ ｆｒｅｑｕｅｎｔ ｐａｔｈ ｓｗｉｔｃｈｉｎｇｓ，ｍａｋｉｎｇ ｔｈｅ ｖｅｈｉｃｌｅ’＇ｈｅｓｉｔａｔｅ＇’ｔｏ ｄｏｄｇｅ ａｎｄ ｓｗｉｎｇ，

ａｎｄ ｉｎｃｒｅａｓｅ ｔｈｅ ｒｉｓｋ ｏｆ ｃｏｌｌｉｓｉｏｎ，ｔｈｉｓ ｐａｐｅｒ ｄｅｆｉｎｅｓ ｔｈｅ，－－ｔｏｐｏｌｏｇｙ－ｃｏｈｅｒｅｎｃｅ ｒｅｌａｔｉｏｎｓｈｉｐ ｂｅ—

ｔｗｅｅｎ Ｕ＇ａｊｅｃｔｏｒｉｅｓ ａｎｄ ｐｒｏｐｏｓｅｓ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｋｉｎｏｄｙｎａｍｉｃ ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍｓ ｔｈａｔ ｉｍｐｒｏｖｅ

ｒｅｐｌａｎ ｃｏｈｅｒｅｎｃｅ，ｐｒｏｄｕｃｉｎｇ ｓｍｏｏｔｈｅｒ ａｎｄ ｓａｆｅｒ ｔｒａｊｅｃｔｏｒｉｅｓ ｄｕｒｉｎｇ ｔｈｅ ｏｖｅｒａｌｌ ｎａｖｉｇａｔｉｏｎ ｐｒｏ—

ＣｅｓＳ．Ｃｏｎｓｉｄｅｒｉｎｇ ｔｈｅ ｃｏｎｆｌｉｃｔｉｎｇ ｃｏｎｓｔｒａｉｎｔｓ ｏｆ ｆｉｔｔｉｎｇ ｉｎ ｔｈｅ ｒｅｆｅｒｅｎｃｅ ｔｒａｊｅｃｔｏｒｙ ａｎｄ ａｖｏｉｄｉｎｇ

ｏｂｓｔａｃｌｅｓ，ｔｈｉｓ ｐａｐｅｒ ｐｒｏｐｏｓｅｓ ａ ｔｒａｊｅｃｔｏｒｙ ｏｐｔｉｍｉｚａｔｉｏｎ ｍｅｔｈｏｄ ｆｏｒ ｔｒａｃｋｉｎｇ ｒｅｐｌａｎｎｉｎｇ ｔｈａｔ ｃｏｎ－

ｓｔｒａｉｎｓ ｔｈｅ ｅｎｄ－ｓｔａｔｅ ｉｎ ｔｈｅ ｒｅｆｅｒｅｎｃｅ ｔｒａｊｅｃｔｏｒｙ ａｎｄ ｏｐｔｉｍｉｚｅｓ ｉｔ ａｄａｐｔｉｖｅｌｙ．Ｗｅ ｎｓｅ ｔｒａｊｅｃｔｏｒｙ

ｃｌａｓｓｅｓ，ｐｅｎａｌｔｙ ｆｕｎｃｆｉｏｎｓ，ａｎｄ ｏｔｈｅｒ ｍｅｔｈｏｄｓ ｔｏ ｅｌｉｍｉｎａｔｅ ｃｏｎｓｔｒａｉｎｔｓ ａｎｄ ｕｓｅ ｇｒａｄｉｅｎｔ ｄｅｓｃｅｎｔ ｔｏ

ｓｏｌｖｅ ｔｈｅ ｐｒｏｂｌｅｍ．Ｔｈｅ ｍａｘｉｍｕｍ ｓｔｅｐ ｓｉｚｅ ｉｓ ｃａｌｃｕｌａｔｅｄ ａｃｃｏｒｄｉｎｇ ｔｏ ｔｈｅ １＿－ｔｏｐｏｌｏｇｙ－ｃｏｈｅｒｅｎｃｅ
ｒｅｌａｔｉｏｎｓｈｉｐ ｄｕｒｉｎｇ ｔｈｅ ｌｉｎｅ ｓｅａｒｃｈ，ｋｅｅｐｉｎｇ ｔｈｅ ｒｅｓｕｌｔａｎｔ ｏｐｔｉｍｉｚｅｄ ｔｒａｊｅｃｔｏｒｙ ｉｎ ｔｈｅ ｓａｍｅ ｐａｔｈ

ｔｏｐｏｌｏｇｙ ａｓ ｔｈｅ ｉｎｉｔｉａｌ ｔｒａｊｅｃｔｏｒｙ．Ｃｏｍｐａｒｅｄ ｗｉｔｈ ｔｈｅ ｔｒａｄｉｔｉｏｎａｌ ｆＬｘｅｄ ｅｎｄ－ｓｔａｔｅ ｍｅｔｈｏｄ，ｔｈｅ ｐｒｏ－

ｐｏｓｅｄ ｍｅｔｈｏｄ ｇｒｅａｔｌｙ ｒｅｄｕｃｅｓ ｔｈｅ ｒｅａｌ－ｔｉｍｅ ｐｏｓｉｔｉｏｎ ｔｒａｃｋｉｎｇ ｅｒｒｏｒ ｗｈｉｌｅ ｓａｔｉｓｆｙｉｎｇ ｔｈｅ ｏｂｓｔａｃｌｅ

ａｖｏｉｄａｎｃｅ ｃｏｎｓｔｒａｉｎｔ．Ｔｈｅ ｐｅｒｆｏｒｍａｎｃｅ ｏｆｔｈｅ ｐｒｏｐｏｓｅｄ ｔｒａｊｅｃｔｏｒｙ ｔｒａｃｋｉｎｇ ａｌｇｏｒｉｔｈｍ ｉｓ ｖｅｄｆｉｅｄ

ｉｎ ａ ｒｅａｌｉｓｔｉｃ ｓｉｍｕｌａｔｅｄ ＭＡＶ ｎａｖｉｇａｔｉｏｎ ｓｙｓｔｅｍ．

Ｋｅｙｗｏｒｄｓ：Ｍｕｌｔｉｒｏｔｏｒｓ；Ａｕｔｏｎｏｍｏｕｓ Ｎａｖｉｇａｔｉｏｎ；Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ；Ｏｐｔｉｍａｌ Ｃｏｎｔｒｏｌ

ＶⅡ

万方数据
 ⅥⅡ

万方数据
 目录
致谢………………………………………………………………………………………………… Ｉ

摘要…………………………………………………………………………………………………ＩＩＩ

Ａｂｓｔｒａｃｔ．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．．１Ｉｉ‘

目录…………………………………………………………………………………………………Ⅸ
图目录………………………………………………………………………………………………ＸＩＩＩ

表目录………………………………………………………………………………………………ＸＶ

１绪论……………………………………………………………………………………………． １

１．１研究背景及意义………………………………………………………………………． １

１．２多旋翼飞行器动力学运动规划研究现状…………………………………………．． ３

１．２．１基于状态空间采样的方法…………………………………………………… ４

１．２．２基于图搜索的方法……………………………………………………………． ７

１．２．３基于优化的方法………………………………………………………………． ８

１．２．４混合式方法……………………………………………………………………．． ９

１．３本文主要研究内容及组织结构………………………………………………………１０

２多旋翼飞行器动力学运动规划问题基本构建…………………………………………．．１３

２．１引言………………………………………………………………………………………１３

２．２机体动力学模型约束…………………………………………………………………．１４

２．２．１非线性模型……………………………………………………………………一１５

２．２．２微分平坦及平坦后的线性模型………………………………………………１６

２．３过程约束………………………………………………………………………………．．１９

２．３．１系统状态／控制输入区间约束………………………………………………．．１９

２．３．２躲避障碍物约束………………………………………………………………．２０

２．４边值约束………………………………………………………………………………．．２２

２．４．１初始时刻约束…………………………………………………………………．．２２

２．４．２终止时刻约束…………………………………………………………………一２３

２．５性能指标………………………………………………………………………………．．２４

２．６本章小结………………………………………………………………………………．．２４

ＩＸ

万方数据
３结合采样与优化方法的全局最优动力学运动规划……………………………………．．２７

３．１引言………………………………………………………………………………………２７

３．２问题描述………………………………………………………………………………．．２９

３－３轨迹树的生长…………………………………………………………………………．．３０

３．３．１时空形变树算法………………………………………………………………．３０

３．３．２最优轨迹边求解………………………………………………………………．３５

３．３．３多项式轨迹边的两种表示……………………………………………………３８

３．４轨迹树的时空形变……………………………………………………………………．４０

３．４．１形变单元构造…………………………………………………………………．．４０

３．４．２优化性能指标设计……………………………………………………………．４１

３．４．３罚函数法构建形变单元的无约束优化……………………………………．４２

３．４．４梯度下降求解形变单元的优化………………………………………………４３

３．４．５形变模式………………………………………………………………………．．４５

３．５全局规划数值实验……………………………………………………………………．４６

３．５．１实验设置………………………………………………………………………．．４６

３．５．２形变模式比较…………………………………………………………………．．４６

３．５．３时空优化比较…………………………………………………………………．．４８

３．５．４与层级式规划方法比较………………………………………………………４９

３．６本章小结………………………………………………………………………………．．５２

４层次化实时局部动力学运动规划…………………………………………………………．５３

４．１引言………………………………………………………………………………………５３

４．２基于引导采样的前端动力学运动规划……………………………………………．．５４

４．２．１问题描述………………………………………………………………………．．５４

４．２．２基于引导采样的轨迹树生成算法…………………………………………．．５５

４．２．３基于近似拓扑引导的状态空间采样………………………………………．．５７

４．３轻量级高效后端轨迹提优…………………………………………………………．．５９
４．３．１问题描述………………………………………………………………………．．５９

４．３．２二次型优化目标构建…………………………………………………………．６１

４．３．３内层优化的闭式最优解构造…………………………………………………６３

万方数据
 ４．３．４双层优化求解流程……………………………………………………………．６４

４．４与相关算法的数值对比测试………………………………………………………．．６６

４．４．１采样策略对比测试……………………………………………………………．６６

４．４．２轨迹规划系统对比测试………………………………………………………６７

４．４．３后端提优算法对比测试………………………………………………………６８

４．４．４前后端集成对比测试…………………………………………………………．６９

４．５真机未知环境自主导航试验…………………………………………………………７０

４．５．１实验设定………………………………………………………………………．．７０

４．５．２路径点间导航…………………………………………………………………．．７１

４．５．３快速重规划测试……………………………………………………………７２
４．６本章小结………………………………………………………………………………．７３

５ 结合快速局部优化和双向搜索的实时规划………………………………………………７５

５．１引言………………………………………………………………………………………７５

５．２结合快速局部优化和双向搜索的采样规划前端…………………………………．７６

５．２．１双向轨迹树生长………………………………………………………………７６

５．２．２求解轨迹边的两点边值问题…………………………………………………７９

５．３快速局部优化…………………………………………………………………………．．８０

５．３．１考虑障碍物的二次型性能指标构建………………………………………．．８０

５．３．２问题构建及迭代求解过程……………………………………………………８１

５．３．３吸引点选取……………………………………………………………………．．８３

５．４后端轨迹提优…………………………………………………………………………．．８３

５．５数值与实物实验……………………………………………………………………８４
５．５．１实验设置………………………………………………………………………．．８４

５．５．２前端对比测试结果及分析……………………………………………………８５

５．５．３后端对比测试结果及分析……………………………………………………８６

５．５．４实物四旋翼飞行器飞行测试………………………………………………．．８８

５．６本章小结………………………………………………………………………………．８９

６ 自适应跟踪参考轨迹实时重规划…………………………………………………………９３

６．１引言………………………………………………………………………………………９３

万方数据
 ６．２基于采样的拓扑连贯性运动规划…………………………………………………．．９５

６．２．１ ７＿一拓扑连贯关系………………………………………………………………．．９６

６．２．２ ７．一拓扑连贯前端规划算法……………………………………………………．９７

６．３自适应跟踪参考轨迹的轨迹优化…………………………………………………．．１０３

６．３．１问题描述………………………………………………………………………．．１０３

６．３．２使用轨迹类处理等式约束……………………………………………………１０４

６．３．３使用罚函数处理不等式约束…………………………………………………１０５

６．３．４无约束优化问题及梯度推导…………………………………………………１０７

６１３．５求解过程………………………………………………………………………．．１０９

６．４仿真导航测试及结果分析……………………………………………………………１１２

６．５本章小结………………………………………………………………………………．．１１８

７总结与展望……………………………………………………………………………………１１９

７．１全文工作总结…………………………………………………………………………．．１１９

７．２将来工作展望…………………………………………………………………………．．１２０

参考文献…………………………………………………………………………………………．．１２１

ｌ咐勇ｔ…………．…．………………………………………………………………．…………．．．…．．．１２９

Ａ固定步长采样泛函积分的连续不可微性…………………………………………．．１２９

作者简历…………………………………………………………………………………………．．１３１

攻读学位期间科研成果…………………………………………………………………………．１３１

ＸＵ

万方数据
 图目录
图１．１ 多旋翼飞行器各类应用场景……………………………………………………． １

图１．２ 自主导航系统模块框架示意图…………………………………………………． ２

图２．１ 参考坐标系…………………………………………………………………………１５

图２．２不同分辨率下的占据栅格地图（图源自Ｈｏｍｔｍｇ等人【８４Ｉ工作）………．２１

图２．３ 欧氏距离场地图（图源自Ｈａｒｔ等人【８５】工作）………………………………２１

图２．４安全通道地图………………………………………………………………………２１

图３．１添加相同数量的状态节点后轨迹树的拓扑结构以及当前最优轨迹………２９

图３．２形变单元示意图…………………………………………………………………．．４０

图３．３形变模式意图………………………………………………………………………４５

图３．４测试环境以及某次结果轨迹……………………………………………………．４６

图３．５ 不同形变模式的轨迹代价收敛性比较示意图………………………………．．４７

图３．６ 不同优化方法的轨迹代价收敛性比较示意图………………………………．．５０

图３．７与层级式规划方法进行比较的测试环境以及递增时间预算时所得到

的结果轨迹…………………………………………………………………………５ｌ

图３．８基于不同时间预算的平均求解时间与轨迹质量对比………………………．５２

图４．１ 近似拓扑骨架构建的２维示意图………………………………………………５８

图４．２ 近似拓扑骨架构建与状态采样的３维示意图………………………………．．５９

图４．３ ２阶模型前端轨迹加速度不连续示意图………………………………………６０

图４．４路径拓扑惩罚项示意图…………………………………………………………．６ｌ

图４．５近似拓扑引导采样策略与均匀随机采样策略效率对比示意图……………６６

图４．６ 不同采样策略下轨迹树生长对比示意图………………………………………６７

图４．７测试环境以及不同后端优化方法产生的轨迹………………………………．．６８

图４．８不同起终点距离及不同障碍物密度下的集成测试对比结果………………７０

图４．９实物飞行所用四旋翼飞行器及机载设备………………………………………７ｌ

图４．１０室外测试场景以及实际飞行轨迹………………………………………………７２

图４．１１室外测试整体飞行速度曲线……………………………………………………．７２

图４．１２室内测试场景以及实际飞行轨迹………………………………………………７３

ｘＩＩＩ

万方数据
 图４．１３室内测试整体飞行速度曲线……………………………………………………．７３

图４．１４追踪移动目标飞行测试…………………………………………………………．７４

图４．１５重规划避障序列图………………………………………………………………．．７４

图５．１ 选择吸引点示意图………………………………………………………………．．８３

图５．２ 后端循环求解过程轨迹示意图…………………………………………………．８４

图５．３ 森林类型类２．５维测环境以及某次规划轨迹对比…………………………．．８６

图５．４洞穴类型３维测环境以及某次规划轨迹对比………………………………．．８６

图５．５ 窄缝类型３维测试环境以及某次规划轨迹对比……………………………．．８６

图５．６不同方法在不同环境下求解出第一条轨迹的平均用时（ｍｓ）……………８７

图５．７迷宫类型飞行测试环境…………………………………………………………．９０

图５．８ 实时重规划测试环境……………………………………………………………．．９ｌ

图６．１ 局部规划目标状态以及时域长度示意图………………………………………９４

图６．２连续重规划间的不连贯性示意图………………………………………………９５

图 ．３ ７－．拓扑连贯轨迹示意图…………………………………………………………．．９６

图 ．４ ７－．拓扑连贯生长树示意图………………………………………………………．．９７

图 ．５丁．拓扑连贯连接图示意图………………………………………………………．．１０２

图 ．６计算最大线搜索步长示意图……………………………………………………．１１２

图６．７是否考虑跟踪自适应性的测试环境与最终执行路线………………………．１１５

图６．８参考轨迹跟踪位置误差对比曲线………………………………………………１１５

图６．９重规划结果轨迹…………………………………………………………………．．１１７

图６．１０倾角、推力、角速率、速率曲线………………………………………………１１７

图６．１１连续重规划是否考虑拓扑连贯性对比测试环境……………………………．．１１８

图Ａ．１固定步长采样泛函积分示意图…………………………………………………．１２９

万方数据
 表目录
表３．１ 不同形变方法得到的平均轨迹时长和平均轨迹代价对比…………………．４９

表４．１ 前端动力学运动规划比较结果（平均值）…………………………………．．６７

表４．２后端数值对比结果………………………………………………………………．．６９

表５．１ 前端规划方法标记………………………………………………………………．８５

表５．２ 前端规划１０００次平均结果比较………………………………………………．．８５

表５．３ 后端优化对比结果第一部分……………………………………………………．８８

表５．４后端优化对比结果第二部分……………………………………………………．８９

表６．１ 轨迹优化参数符号数值表………………………………………………………．１１４

表６．２ 重规划求解时间表………………………………………………………………．．１１６

万方数据
万方数据
 浙江大学博士专业学位论文 １绪论

１绪论

１．１研究背景及意义

小型多旋翼飞行器（ＭｕｌｔｉｒｏｔｏｒＡｅｒｉａｌＶｅｈｉｃｌｅｓ，ＭＡＶｓ）是指以使用电机驱动的螺旋

桨作为飞行动力的非载人飞行器系统，相比其他固定翼或扑翼形式的无人飞行器，ＭＡＶｓ

有着更出色的机动灵活性、更简单的机械结构，以及更为低廉的成本，进而在物流配送、

精准农业、巡检监控、航拍摄影等领域有着广泛应用。

（ａ）农业喷洒 （ｂ）航拍摄影 （ｃ）物流配送

（ｄ）竞速飞行 （ｅ）高空喊话 （ｆ）巡检监控

图１．１多旋翼飞行器各类应用场景

随着应用领域不断丰富，飞行器的工作场景也逐渐从高空转向近地面的复杂环境，

因而对躲避障碍物的需求逐渐增强。面对障碍物环境复杂、先验地图缺失的场景，当前

飞行作业仍高度依赖熟练的人工操控和外部导航，使得任务的效率低下，以及受人为主

观影响较大。无人自主导航系统则能够利用愈发成熟的传感器，使用环境感知、状态估

计、运动规划、决策控制等相关领域的前沿算法来弥补这一缺陷，飞行器在复杂环境下

的自主无人导航在各项应用中显得愈发重要。

在多旋翼飞行器自主导航系统中，如图１．２所示，运动规划模块是承上启下的核心

之一。此模块求解飞行器在将来一段时间所期望处于的状态，也即轨迹，若为连续形式

万方数据
 浙江大学博士专业学位论文 ｌ 绪论

则是得到一个状态关于时间的连续函数，若是离散形式则是一段状态序列。此结果轨迹

输出给到所承接的控制模块，作为期望状态和当前状态一起计算出执行器接受的控制输

入以使得飞行器在将来一段时间跟随此期望的状态序列。运动规划模块的输入一般是由

状态估计模块输出的当前飞行器的状态，由地图构建模块所输出的当前所观测到的环境

信息，以及任务规划模块给出的目标状态。

对于已知且非动态的环境，运动规划只需进行一次，找到全局最优的轨迹，而对于

在未知环境下的导航，则需要在飞行器行进中实时感知当前环境，并且实时重规划，整

个流程一般以滚动优化（Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ）的框架进行，以一定频率触发规划或者按

需触发规划。稳定的感知以及控制能够保证自主导航系统的下限，而鲁棒且高效的运动

规划则决定飞行作业任务完成情况的上限，对于飞行作业的低空环境，障碍物高度非结

构化且难以预测，考虑避障约束、飞行器的物理限制、以及任务需求的运动规划在整体

自主导航系统中扮演的角色越来越关键。与动力学模型、物理限制等约束相比，躲避障

碍物的约束对轨迹求解的最优性有着更强的制约和影响。给定运动规划的起终点，复杂

的障碍物将可行空间分割成不同的路径拓扑类型（ＰａｔｈＴｏｐｏｌｏｇｙＣｌａｓｓ），或称同伦类型

（Ｈｏｍｏｔｏｐｙ Ｃｌａｓｓ），也将整体解空间划分成互相之间间隔较远的不同子区域。相同路径

拓扑类型下的轨迹很大程度上对应着解空间中同一子区域内的解，动力学模型、物理限

制等约束再决定各自子区域解空间下的最优解。

考虑到求解实时性的需求，许多运动规划方法Ｕ－５】使用层级式规划（Ｈｉｅｒａｒｃｈｉｃａｌ

Ｐｌａｎｎｉｎｇ）的框架，先基于最短路径规划在构型空间（Ｃｏｎｆｉｇｕｒａｔｉｏｎ Ｓｐａｃｅ）得到一条几

何路径，再对此几何路径进行时间参数化（Ｔｉｍｅ Ｐａｒａｍｅｔｅｒｉｚａｔｉｏｎ），或在此几何路径的

万方数据
 浙江大学博士专业学位论文 ｌ绪论

基础上进行轨迹规划，最终得到时间参数化的轨迹。几何路径规划解决寻找最终轨迹所

在的环境拓扑位置的问题，后续的时间参数化或轨迹规划再考虑系统的动力学等约束。

层级式规划的思路值得借鉴，但此类路径规划衔接上轨迹规划的做法存在几点问题：１．

不同层级规划的目标不一致，例如路径规划时追求最短路径，轨迹规划时考虑最短时

间、最小能量消耗等特定任务需求；２．在几何路径规划时未考虑任何飞行器的运动学和

动力学模型约束；３．在路径规划时未考虑飞行器的初始状态，例如在重规划时飞行器一

般具有初速度。上述问题造成的结果是。路径规划得到的路径没有反映出真正想要的轨

迹所应该在的环境拓扑位置，甚至在此结果路径的基础上进行轨迹规划可能无法得到满

足飞行器运动学和动力学约束的轨迹，或是规划出不期望的转弯或是掉头轨迹。

针对上述问题，动力学运动规划［６１（Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｉｎｇ）方法在状态空间（Ｓｔａｔｅ

Ｓｐａｃｅ）（或称相空间Ｐｈａｓｅ Ｓｐａｃｅ）进行一体式规划，整体考虑一个目标，求解时直接面

对所有约束且考虑系统的初始及末端状态，直接在全局解空间中求解最优的轨迹，得到

最终轨迹的位置处于合理的路径拓扑下且满足飞行器的运动学和动力学约束。此类方法

的研究或基于图搜索、或基于采样，或基于最优控制，各自有其优缺点，难以兼顾求解

实时性与最优性，尤其是对于空中作业的小型飞行器，其尺寸较小，载荷有限，受到传

感器精度和计算功率的约束。规划求解效率的问题更为突出，而实时性在动态环境中小

范围局部重规划时却是必要的，最优性则是对于较大范围的全局规划所需具备的。

本文综合考虑采样类与最优控制类动力学运动规划方法的优缺点，结合层级规划的

思想，针对复杂环境中多旋翼飞行器在不同任务情形下的大范围全局规划与小范围局部

规划问题，设计高效高质量的求解方法。本文的研究能够为飞行器自主导航规划的实时

求解与最优性求解提供设计思路，具有较大应用前景。

１．２多旋翼飞行器动力学运动规划研究现状

从解的最优性角度，动力学运动规划可分为最优动力学运动规划和可行动力学运动

规划。最优规划指在整体解空间中找到全局最优解或接近全局最优解，其主要方法的总

体思路与基于图搜索的和基于采样的最优路径规划方法相同，往往暗含贝尔曼最优性

原理川的思想，主要用于全局规划的离线求解。可行规划一般指在整体解空间中找到

高质量的可行解，或在解空间的某一子区域内找到最优解，即整体解空间的一个局部最

万方数据
 浙江大学博士专业学位论文 １绪论

优解，主要用于局部规划的实时求解。其主要方法同样包括图搜索类和采样类的可行路

径规划方法，另外还有基于优化的方法，以及混合方法，有时还衔接一个轨迹提优的步

骤。需要注意的是，各类方法并非绝对孤立，许多方法的思想有着内在联系，例如从求

解最优控制问题的角度，动态规划方法中的分治思想与采样类方法中拆分成子问题求解

的思想类似。本文将各种动力学运动规划方法在整体上分为三大类型：状态空间采样类

方法，图搜索类方法，以及混合式方法，以下对各类文献进行梳理与分析，主要针对飞

行器系统但不限于此。

１．２．１基于状态空问采样的方法

采样类方法的一大优点是其不显式构建可行状态空间的精确边界，而是通过在连

续的状态空间采样，得到离散的状态点，通过图型结构或树型结构（树型结构也可视作

特殊的图型结构）将其连接起来，以求高效地探索可行空间的连通性，因而主要用于

解决由障碍物约束带来的问题复杂度。此类方法一般具备概率完备性【８，９１（Ｐｒｏｂａｂｉｌｉｓｔｉｅ

Ｃｏｍｐｌｅｔｅｎｅｓｓ），即若一个问题存在解，则随着采样不断进行，找到一个可行解的概率趋

于１。许多针对构型空间的路径规划方法天然适应于扩展到状态空间的动力学运动规划，

不同之处在于采样的空间以及连接采样点的方式。连接两个状态点构成一段轨迹，此

过程也称作Ｓｔｅｅｒ，而这往往需要求解最优控制中的两点边值问题（ＴｗｏＰｏｉｎｔＢｏｕｎｄａｒｙ
Ｖａｌｕｅ Ｐｒｏｂｌｅｍ，ＴＰＢＶＰ），即对特定动力学系统求解给定初始条件与终止条件的运动方

程。当存在复杂约束、系统微分模型的非线性较强或目标函数不可徼时，求解ＴＰＢＶＰ变

得非常困难，而通过连接离散的状态来探索解空间的连通性需要大量求解ＴＰＢＶＰ，这

往往成为此类方法的效率瓶颈。

探索与开发（Ｅｘｐｌｏｒａｔｉｏｎ ａｎｄ Ｅｘｐｌｏｉｔａｔｉｏｎ）是采样类方法的两大核心。探索指连接

轨迹图使其遍布完整的解空间（针对多查询类（Ｍｕｌｔｉ Ｑｕｅｒｙ）问题），或生长轨迹树使

其生长至解空间中所有需要探索的部分（针对单查询类（Ｓｉｎｇｌｅ Ｑｕｅｒｙ）问题），开发指
如何利用采样出的解空间信息修正维护轨迹图和轨迹树的结构以更好地表征解空间。因

此，设计高效的非均匀采样方式来得到落在最优解的周围的状态样本，以及设计连通图

和轨迹树的构建方法成为两个主要研究方向。另外，对于动力学运动规划问题，由于两

个状态点之间的是一段轨迹，如何高效求解任意两个状态之间的连接轨迹也是一大方

向。

万方数据
 浙江大学博士专业学位论文 １ 绪论

关于图或树连接结构的构建方式，状态空间轨迹规划与构型空间路径规划都是根据

各自定义的距离代价度量（Ｍｅｔｒｉｃ）进行构建，不同之处在于对距离的度量方式的定义。

Ｋａｗａｋｉ等人［１０】提出的ＰＲＭ算法和ＬａＶａｌｌｅ等人【１１】提出的ＲＲＴ算法将采样出的节点和

图或树中与之最近的节点相连，此类方式能够快速探索整个空间，但没有利用图或树中

已有的结构信息。Ｋａｒａｍａｎ等人［１２１证明了ＰＲＭ与ＲＲＴ算法最终收敛到一个非最优解，

并在工作中提出了具有渐进最优性（Ａｓｙｍｐｔｏｔｉｃ Ｏｐｔｉｍａｌｉｔｙ）的ＰＲＭ＊、ＲＲＧ以及ＲＲＴ＋

算法，这些算法利用图或树的局部结构信息，连接节点的同时通过重连接操作修正图与

树的结构而达到局部最优状态，随着采样点的数量趋于无穷，得到最优解的概率趋近于

１。Ａｒｓｌａｎ等人【１３】进一步提出ＲＲＴ＃算法，对无希望的（Ｎｏｎｐｒｏｍｉｓｉｎｇ）节点不进行重连

接，以及使用循环重连接将由采样带来的信息传播到树的其他部分，从而解决了ＲＲＴ＊

算法中的过开发（ＯｖｅｒＥｘｐｌｏｉｔａｔｉｏｎ）与欠开发（ＵｎｄｅｒＥｘｐｌｏｉｔａｔｉｏｎ）问题，提高了算法

收敛到最优解的效率。上述方法都是增量式采样，Ｊａｎｓｏｎ等人【ｌ ４１提出ＦＭＴ＊算法，使用

批采样的方式得到一批次状态节点，根据直接动态规划［１５】方法连接批采样出的各节点，

可类比于图搜索中的Ｄｉｊｋｓｔｒａ算法【１６１。Ｇａｍｍｅｌｌ等人【”，１８］之后提出ＢＩＴ＊算法，使用多

次分批次采样出逐渐稠密的采样点，采取图搜索中增量式搜索的方法如ＬＰＡ＊算法【１９】

进行增量搜索，得到渐进最优的解。除了构建单棵树之外，同时构建双向的搜索树［２０－冽

也有助于快速探索以及解决目标点被障碍物围困住的情形。

关于采样分布问题的研究大致可分为两种，重要性采样与自适应采样【２３１。重要性

采样基于的是，对于某一特定任务，状态空间中各部分的重要性程度是不同的。例如在

可行解或是最优解的周围采样有助于提高算法的收敛性，然而这些解本身就是要求解

的量，是事先未知的。不过依据经验，我们可以估计最优解在解空间中的位置从而设

计采样状态的分布。一些工作［２４－２７】针对特定问题和场景设计特定的采样分布，对于路

径规划，Ｏｌｅｙｎｉｋｏｖａ等人【２６】和Ｂｌｏｅｈｌｉｇｅｒ等人【２４】在进行实时定位与建图（Ｓｉｍｕｌｔａｎｅｏｕｓ

ＬｏｃａｌｉｚａｔｉｏｎａｎｄＭａｐｐｉｎｇ，ＳＬＡＭ）的同时对可行空间的中轴路径骨架进行近似提取，类

似于Ｖｏｒｏｎｏｉ图的形式，之后在中轴骨架的附近采样位置点。对于动力学运动规划问题，

Ｙｅ等人［２７１根据速度方向一般会沿着中轴骨架路径的方向的特征进行采样。相较均匀采

样，这些方法找到第一个解所用的时间、质量和算法收敛率都有大幅提升。另一些工

作［２８－３３］利用机器学习能有效提取高维特征的特性，尝试开发通用的方法来获得采样的

概率分布而非只针对特定的场景和问题。这要求将地图抽象成用特定参数表征的形式，

万方数据
 浙江大学博士专业学位论文 １绪论

然而许多实际情况我们只能去检测一个采样的状态是否碰撞而无法知道地图的形式，如

何高效地表征抽象地图仍然是一个待解决的问题。且使用机器学习难以获得强的泛化

能力，较为繁重的网络推理也使其难以在自主导航中在线使用。需要指出的是，在规划

过程中状态空间中各部分的重要性程度并不是一成不变的，因此自适应采样相比重要

性采样更能够应对此类情况。Ｇａｍｍｅｌｌ等人【蚓提出Ｉｎｆｏｒｍｅｄ－ＲＲＴ＊，证明了当有一个初

始解之后，能够提升解质量的采样被约束在一个高维椭球内，因此能够进行直接采样

（Ｄｉｒｅｃｔ Ｓａｍｐｌｉｎｇ），其与均匀采样后拒绝采样（Ｓａｍｐｌｅ Ｒｅｊｅｃｔｉｏｎ）相比大大提升了采样

效率，尤其是在高维空间，其之后与Ｓｔｕｒｂ等人一起提出的ＢＩＴ，ｔＩＴ］、ＡＩＴ＊［３５１、ＥＩＴ＊［３６１

等算法也同样利用此特性进行自适应采样。然而这里得到的理论分布仅存在于最短路径

问题（Ｌ２－ｎｏｒｍ），若是处理最短时间或者最小能量等其他优化目标的问题，仍然无法直

接获得有效的状态空问采样分布，且其无法缩短找到第一个解的时问。关于状态空间的

自适应采样仍旧是一个研究难点。

关于Ｓｔｅｅｒ过程中ＴＰＢＶＰ求解问题的研究，可分为替代求解或是近似快速求解。

Ｂｏｅｕｆ等人【３７，３８１、Ｂｅｕｌ等人【３９，４０］以及Ｒａｕｓｃｈｅｒ等人１４１】利用最优控制中最短时间转移问

题Ｂａｎｇ—Ｂａｎｇ控制的结论，设计特定的控制输入模式来决定两个状态间转移的最短时间

轨迹，避免了求解ＴＰＢＶＰ，但此轨迹库引入了次优性且仅适用于时间最优的目标函数

和链式线性动力学模型。另外一些避免求解ＴＰＢＶＰ的方法在控制输入层面采样，根据

微分模型通过前向积分得到新状态点，例如Ｌｉ等人【４２】提出的ＳＳＴ＊算法。此类方法类

似于运动基元（Ｍｏｔｉｏｎ Ｐｒｉｍｉｔｉｖｅ）类方法，但基元通过在线构建且由随机性采样生成。

针对近似快速求解，Ｗｅｂｂ等人【４３】利用极小值原理，对线性幂零系统能够快速求得无约

束’ｒＰＢＶＰ的闭式解（Ｃｌｏｓｅｄ－ｆｏｒｍ Ｓｏｌｕｔｉｏｎ），之后再检查状态和控制输入的约束，整体

求解效率高于设计特定控制模式的方法，但是要求采样的状态较为合理。Ｚｈｅｎｇ等人【．町

在其基础上提出末状态部分受约束的ＴＰＢＶＰ闭式解求法，降低了采样所需的状态维度。

对于非线性系统，Ｐｅｒｅｚ等人【４５】提出ＬＱＲ－ＲＲＴ＊，在工作点将动力学模型线性化，使用

ＬＱＲ（Ｌｉｎｅａｒ Ｑｕａｄｒａｔｉｃ Ｒｅｇｕｌａｔｏｒ）近似求得解析解。

采样类方法因其对解空间的高效探索与开发而适合于状态空间维度较高的全局最

优规划。另外，针对具备渐进最优性的采样类算法，可以使其具备即时性质（Ａｎｙｔｉｍｅ

Ｐｒｏｐｅｒｔｙ），意思是其能够快速得到一个初始可行轨迹，之后在执行此轨迹的过程中。在

运算时间允许的情况下，继续提升后续未执行的部分的轨迹质量湖。或是得到一个初始

万方数据
 浙江大学博士专业学位论文 ｌ绪论

可行轨迹后使用高效后处理的方法对其提优，因而此类方法也适合有实时重规划需求的

自主导航情形。

１．２．２基于图搜索的方法

与基于采样方法将状态空间离散化不同，基于图搜索的动力学运动规划算法将控制

输入与状态转移的时间离散化，从某一初始状态出发，对系统施加某一常量控制输入。

维持一段时间，根据系统的微分模型前向积分，得到新的状态点，再对新的状态点进行

同样操作，由此生成层级式的符合模型约束的有向搜索图，各个离散的状态点构成图中

节点，连接节点的边即是一段常量控制的轨迹。对一个状态点应用不同控制输入得到的

轨迹边也称作运动基元（Ｍｏｔｉｏｎ Ｐｒｉｍｉｔｉｖｅ）。在此图上根据Ａ幸【４７】之类搜索策略得到分

段常量控制的最终轨迹。

Ｌｉｋｈａｃｈｅｖ等人【删和Ｚｈａｎｇ等人ｔ４９，５０３提前离线构造一个由层级式运动基元组成的

搜索空间，当机器人运动的时候，选取与当前运动状态最为接近的一个子搜索空间，在

线搜索出满足动力学约束的轨迹，并使用查表式启发函数加速搜索过程。Ｚｈａｎｇ等人的

后续工作［５１，５２］结合局部观测和先验地图，选择到达目标状态的概率最大的运动基元，避

免了繁重的在线图搜索。然而构造状态概率需要事先已知的地图，当环境完全未知且较

为稠密时无法在线实时规划。

此类方法是分辨率完备（Ｒｅｓｏｌｕｔｉｏｎ Ｃｏｍｐｌｅｔｅ）的，即在离散的分辨率足够精细的

情况下，一定能找到可行解。然而这种控制输入与采样时间的离散化需要对求解质量和

求解时间进行权衡，当离散程度较底时，能够较快得到一个可行解。但是质量不高，且

可能在此分辨率下无解，反之则有较高的求解成功率且得到的解质量较高，但求解时间

更长。另外更为致命的是，此类方法为指数计算复杂度，即平均求解时间按离散程度呈

指数增长，尤其是状态模型的维数较高时，求解时间难以估量，导致维数灾难（Ｃｕｒｓｅ ｏｆ

Ｄｉｍｃｎｓｉｏｎａｌｉｔｙ），从而难以用于有实时性求解需求的自主导航系统，或是只能用于短距

离局部规划。一些实际加速求解的做法包括在线构建搜索图、剪枝删去一些在解空间中

相邻的节点、设计合适的启发式函数（Ｈｅｕｒｉｓｔｉｃｓ）来更好地估计状态转移代价等等。

混合Ａ・【５３ｌ（ＨｙｂｒｉｄＡ幸）算法在线构建一棵搜索树，在拓展某一个节点时才对其构

建运动基元，并且将状态空间栅格化，以每个栅格只保留一个图节点的形式对树节点进

行剪枝，提升了求解速度但牺牲了解的最优性。针对多旋翼飞行器系统，Ｌｉｕ等人［５４，５５１

万方数据
 浙江大学博士专业学位论文 １ 绪论

根据飞行器模型的微分平坦特性【５６１，使用以加加速度为输入的三阶积分器来表示动力

学模型，从而得到有闭式解形式的启发式函数，但仍然没有维数灾难问题，无法实时使

用。Ｚｈｏｕ等人［５７１进一步牺牲了动力学模型精度，使用以加速度为输入的二阶积分模型

来降低状态的维度，缩短了求解时间能够满足实时局部重规划的需求，但是得到的轨迹

质量过低，需要繁重的后处理提优来提升轨迹质量。

大多数搜索类方法在特定离散的分辨率下保证了最优性。但得到这个最优解所需要

的时间是无法估计的，因此不适用于实时规划，剪枝删去相近节点的策略和ＡＲＡ＊［５８］之

类增加启发式函数的权重，先贪心找到一个次优解的方法都引入了较大的次优性，降低

了解的质量。

１．２．３基于优化的方法

不同于基于搜索和基于采样的方法增量式地探索整个解空间，基于优化的方法利用

数值优化技巧对动力学运动规划问题进行直接求解。虽然求解为线性复杂度，但是一方

面由于各类问题的约束常常造成解空间的结构强非凸，另一方面因为安全避障的约束难

以构建，使得优化过程的计算缓慢，且常常陷入局部最优解，难以得到全局的最优轨迹

或近似最优轨迹。对于求解局部最优解，基于梯度的优化则能够较好处理由系统模型约

束、任务约束等引入的问题复杂度，因此适合用于在有了轨迹初值后的后处理提优。

安全通道走廊（Ｓａｆｅ Ｃｏｒｒｉｄｏｒ）是一种表示可行工作空间的方式，对于飞行器来说，

安全飞行走廊由复杂三维空间中找到的一系列连通的可通行区域组成，通过将待优化轨

迹完全限制在安全走廊内来保证安全性。安全走廊通常为一系列具有凸性的单元，这些

凸单元提供了方便构建的碰撞约束以及相应的梯度，提升了轨迹优化的求解效率。由此

带来的一个问题是其限制了最终轨迹的环境拓扑位置，难以保证在全局解空间下的最

优性，可能需要构建许多条不同拓扑下的飞行通道来进行多次轨迹优化。另外，相比于

碰撞检测类方法，基于凸单元的安全走廊难以充分表征所有可行空间，可能导致解空间

过小的问题。Ｍｅｌｌｉｎｇｅｒ等人［５９】最早系统地指出，使用走廊约束来使飞行器轨迹被限定

在空间的一个区间内，但并未给出构造飞行走廊的方法。Ｄｅｉｔｓ等人【雏蚓使用一系列连

通凸多面体来表示可行空间，将限制多项式轨迹在多面体中的线性不等式约束描述为

一组多项式平方和（ＳｕｍｏｆＳｑｕａｒｅｓ）条件，并采用混合整数二阶锥规划（ＭｉｘｅｄＩｎｔｅｇｅｒ

Ｓｅｃｏｎｄ－Ｏｒｄｅｒ Ｃｏｎｉｃ Ｐｒｏｇｒａｍｍｉｎｇ）来求解轨迹各段在不同多面体中的分配问题。该方法

万方数据
 浙江大学博士专业学位论文 ｌ 绪论

可以得到固定段数分辨率下的最优解，然而仅适用于离线计算。Ｇａｏ等人【６３】同样采用凸

多面体描述可行区域。其使用Ｂ６ｚｉｅｒ曲线表征轨迹，将曲线的控制点限制在多面体内以

保证安全，利用时间最优路径重参数化【删（Ｔｉｍｅ．Ｏｐｔｉｍａｌ Ｐａｔｈ Ｐａｒａｍｅｔｅｒｉｚａｔｉｏｎ）来完成

轨迹的时间分配，在轨迹的空间域和时域进行交替优化。除了飞行走廊之外，一些工作

使用欧氏符号距离场（Ｅｕｃｌｉｄｅａｎ Ｓｉｇｎｅｄ Ｄｉｓｔａｎｃｅ Ｆｉｅｌｄ，ＥＳＤＦ）描述安全避障的约束，在

ＥＳＤＦ上进行线性插值以提供避障的梯度信患，优点是计算方便，但引入了非光滑函数。

Ｏｌｅｙｎｉｋｏｖａ等人【６５】对分段多项式轨迹在ＥＳＤＦ上做离散的路径积分，构建出与障碍物

发生碰撞的惩罚代价，Ｕｓｅｎｋｏ等人【删和Ｚｈｏｕ等人【５刀采用类似的约束描述但采用Ｂ样

条表征轨迹，将约束施加在样条曲线的离散控制点上。还有一些方法与基于采样类的方

法一样不显示构建可行工作空间的描述，仅使用碰撞检测，例如Ｒｉｃｈｔｅｒ等人【ｌ】使用二

次规划（ＱｕａｄｒａｔｉｃＰｒｏｇｒａｍｍｉｎｇ，ＱＰ）来优化分段多项式轨迹段间节点的高阶导数，将

中间路径点约束在以ＲＲＴ＊算法求得的节点上，推到得到闭式解形式的能量最优轨迹。

然而由于此类方案在优化求解时在性能指标以及约束的构建上均未考虑局部障碍物信

息，每次检查轨迹的碰撞后需要中途添加新的路径点，重新构建优化问题才能最终得到

可行解，其运算速率并不适合在线实时使用。

１．２．４混合式方法

一方面基于搜索和基于采样的方法重在高效地探索整个解空间，能够在全局强非凸

的解空间中搜索到最优或近似最优的轨迹，但是其探索解空间的效率经常因为狭窄通道

的存在而大大受限。另一方面基于优化的方法难以完全考虑整个解空间的约束而难以找

到全局的最优解，但是却能优先利用解空间中的局部信患因此能够较好地处理狭窄通道

等问题。所以，一个自然的发展趋势是结合各类方法的优势，取长补短。混合使用。

Ｃｈｏｕｄｈｕｒｙ等人【６７１提出ＲＡＢＩＴ＊算法，使用ＢＩＴ＊Ｂ＇ｑ作为其全局规划器，在全局探

索的过程中结合ＣＨＯＭＰ［ｒ键］进行局部优化，将一些本来与障碍物发生碰撞的路径边优

化至可行区域，从而加速全局探索的过程。然而其收敛效率高度依赖ＢＩＴ＊算法中根据

Ｌ２－Ｎｏｒｍ距离对采样区域进行的更新收缩，且ＣＨＯＭＰ需要事先构建障碍物距离场来提

供梯度使得轨迹收敛，繁重的局部优化可能反而使全局搜索的效率降低。Ｋｉｌｎ等人［６９，７０１

使用了同样的局部优化器，开发了稀疏的连通图来探索不同的轨迹拓扑空间，其障碍物

梯度由在探索的过程中执行的碰撞检测构建得到，虽然不需要提前或在线计算障碍物距

万方数据
 浙江大学博士专业学位论文 ｌ 绪论

离场，但这种方式得到的梯度可能非常不准确，有损于基于梯度优化的收敛效率。Ｈａｕｅｒ

等人【７１】在采样类方法中结合优化，在生长树的过程中重新部署某些节点的位置来提升

整体树的质量，其不事先构建环境的碰撞梯度信患，而是使用线搜索将节点在其他代

价梯度的方向上推离直到与障碍物发生碰撞。Ｓｈｉｒａｉ等人【７２】将局部优化集成到基于搜

索的ＬＷＡ＊方法【力】的规划框架中，提出一种满足动力学运动规划的通用框架，并使用

延迟优化（Ｌａｚｙ Ｏｐｔｉｍｉｚａｔｉｏｎ）来减少优化的负担，但是其使用的混合整数规划（Ｍｉｘｅｄ

ＩｎｔｅｇｅｒＰｒｏｇｒａｍｍｉｎｇ）依旧需要较长的计算时间。针对飞行器系统，Ｎａｔａｒａｊａｎ等人［７４】使

用加权Ａ木算法【７５，７６１（Ｗｅｉｇｈｔｅｄ Ａ宰）结合局部优化，在飞行器线性模型的低维空间进

行全局搜索，在非线性模型的高维空间进行局部优化，实现较为高效的全局姿态规划

（ＳＥ（３）Ｐｌａｎｎｉｎｇ）。

经验上来看，这种混合式做法的整体效率受集成局部优化的方式和局部优化的求解

效率影响较大。本文所提出的各类方法也采取混合式方法的思路，在整体上结合采样类

方法与局部优化求解各情形问题，深度探究结合方式与轻量级的局部优化算法设计。

１．３本文主要研究内容及组织结构

总体上，本文针对多旋翼飞行器的动力学运动规划算法及其在自主导航中的应用开

展研究。一般来说，不同的自主导航情形对动力学运动规划算法的要求及侧重点是不同

的，从感知信息的层面出发，根据己知地图的规模以及精确程度，本文归纳出以下三类

具有递进关系的多旋翼飞行器自主导航情形：

．情形一．全局规划：已知较大范围精确的静态全局地图，在此情况下进行全局最优

轨迹规划（通常轨迹为几十米至上百米），导航执行此轨迹的过程中无需动态感知

与重规划；

一情形二．局部规划：无先验地图信息，只能实时动态感知自身周围的小范围环境，

此时导航过程中需要根据最新的精确地图信患不断进行重规划，每次重规划称作

局部轨迹规划（通常轨迹为几米至十几米）；

一情形三．跟踪规划：已知粗略的大范围全局地图，可据此规划出全局轨迹，但是由
于全局地图不精确，甚至是过时的，此全局轨迹可能与实际的障碍物发生碰撞，因

万方数据
 浙江大学博士专业学位论文 １ 绪论

而在导航跟踪全局参考轨迹的过程中仍然需要动态感知，获得精确的小范围地图，

并且进行实时重规划以避开实际的障碍物。

多旋翼飞行器在上述情形中进行运动规划的共同要求是都需考虑其动力学及运动

学模型的约束，根据情形特点不同又需各自考虑不同的侧重点，本文考虑如下：

针对全局规划情形，已知大规模精确的静态全局地图，期望在完整解空间中找到最

优轨迹，则规划算法重点需要考虑是否具有全局最优性以及能否快速收敛，以期尽快找

到近似全局最优轨迹。此方面研究成果由本文的第３章介绍。

针对局部规划情形，无先验地图信息，精确的地图范围小且变化频率高，要求飞行

器在运动行进中快速进行重规划，则规划算法重点需要考虑飞行器的运动初始状态和求

解的实时性以应对实时变化的环境，以期高效地生成高质量的避障轨迹。此方面研究成

果由本文的第４章以及第５章介绍。

针对跟踪规划情形，大规模的地图较为粗略甚至过时，依此地图规划出的参考轨迹

可能与实际地图中的障碍物发生碰撞，同样要求飞行器在行进中进行重规划以避障。除

了考虑运动初始状态和求解实时性之外，规划算法还需要考虑与参考轨迹的时空贴合以

及连续重规划之间一致性，而同时避障的要求为此带来了挑战。此方面研究成果在本文

的第６章介绍。

全文总共包含７章节，具体内容安排如下：

第１章为绪论，介绍了本文课题所研究的背景及内容，详细梳理了不同类别的多旋

翼飞行器动力学运动规划方法并分析其特点，之后进一步讲述了本文所探究的不同情形

下的自主导航规划问题，指出其重点研究方向。

第２章从最优控制的角度描述多旋翼飞行器动力学运动规划问题的基本形式，从性

能指标构建、动力学模型精简、过程约束构建、边值约束构建几个方面对问题建模，此

章将作为建立不同情形不同任务下运动规划问题数学模型的基础。

第３章针对已知精确环境下的飞行器全局最优动力学运动规划问题，提出时空形变

轨迹树算法，使用在采样类动力学规划方法中深度结合局部优化的方法来求解。将树生

长时轨迹边的构建建模为末状态部分受约束的ＴＰＢＶＰ问题，使用约束松弛策略加速求

解。设计以最小优化单元表示的轨迹树整体质量性能指标，在时间和空间层面优化不同

单元的组合，花费尽量小的代价高效提升整体轨迹树的质量。

第４章针对未知环境导航中的局部实时重规划问题，提出使用采样类前端动力学运

万方数据
 浙江大学博士专业学位论文 １绪论

动规划衔接高效后端提优的层级式规划框架求解。构建近似拓扑图引导在状态空间的采

样来提升采样类动力学运动规划算法的求解效率。使用内层求解具备闭式解形式的双层

优化后端提优算法高效提升前端结果轨迹的光滑性和连续性。之后使用全自主四旋翼飞

行器在室内外复杂环境进行实时重规划的高速导航飞行测试。

第５章在上一章的基础上，面对障碍物窄缝等环境，增强了采样类前端以及后端提

优的效率与性能，更好地满足实时重规划的需求。提出在采样规划中结合双向搜索与快

速求解的局部优化，快速局部优化构建为一系列的无约束二次凸优化问题，每轮迭代具

有闭式解。在后端提优时将障碍物信息构建进性能指标中，同时保有二次规划问题的结

构，使得保证求解效率的同时成功率更高。之后在实物四旋翼飞行器上进行导航测试，

在稠密障碍物、类迷宫等环境中验证了所提出规划方法的有效性。

第６章探究未知环境下对全局参考轨迹的时空跟踪问题，定义了轨迹间的１－．拓扑连

贯关系，提出考虑拓扑连贯性的采样类动力学规划算法，增加连续重规划之间的路径拓

扑一致性，使得整体导航过程中避障时轨迹更加顺滑，更加安全。针对时空上贴合参考

轨迹与避开障碍物等具有冲突性质的约束提出自适应末端状态的重规划轨迹优化方法。

使用轨迹类、罚函数等方法消除约束，使用梯度下降法求解，并在线搜索时根据７Ｉ．拓扑

连贯关系计算最大步长，保持优化的结果轨迹与初值轨迹处于同一拓扑路径。之后在接

近真实的仿真导航系统中验证了所提出的轨迹跟踪算法。

第７章对全文工作进行总结，并提出对未来工作的展望。

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

２ 多旋翼飞行器动力学运动规划问题基本构建

２．１引言

多旋翼飞行器的动力学运动规划问题是指在其初始时刻状态与终止时刻状态之间

找到一条满足各项约束的轨迹。之所以称作动力学运动规划，是因为在规划时要考虑其

运动学与动力学模型，与常见构型空间的路径规划不同的是，各项约束大多涉及到时间

维度，因此最终结果是一条以时间参数化的状态轨迹，而轨迹中的构型状态可作为路

径。

此处以最优控制的形式对所考虑的规划问题进行整体统一的描述，针对不同情形具

体问题的数学模型则分别在各章中再详细介绍。基本数学模型描述如下：

ｍｉｎ歹：圣（￡，，ｘ＠，））＋厂。ｃｏ，ｘ＠），ｕ（￡））ｄｔ
．，￡０
（２－１ａ）

ｓ．ｔ．ｆ（ｔ，ｘ（ｔ），ｕ（ｔ））一文（ｔ）＝０， （２一ｌｂ）

ｃ（ｔｏ，ｔ，，ｘ（ｚｏ），ｘ（ｔ，）＇ｕ（ｔｏ），ｕ（￡川＝０， （２－ｌｅ）

９（ｔ，ｘ（ｔ），ｕ（ｔ））≤０，Ｖｔ∈［ｔｏ，ｔＡ， （２一ｌｄ）

其中ｘ（ｔ）和ｕ（ｔ）分别表示飞行器的状态向量和控制输入向量，（）＝ｄ（）／ｄｔ表示对时间

ｔ求导，ｔｏ和ｔ，分别表示规划的初始时刻与终止时刻。

上式表示了我们期望求解在容许范围内的控制输入ｕ（ｔ１，使得由终端代价圣和过

程代价ｃ组成的泛函性能指标歹沿着对应的状态轨线ｘ（ｔ）取得最小值。控制输入ｕ（ｔ）
和状态向量ｘ（ｔ）需满足式（２．１ｂ），式（２一ｌｅ）－与式（２—１ｄ）所描述的约束。

式（２．１ｂ）为模型约束，表示ｘ（ｔ）随时间的变化需满足系统的动力学模型，其通常以
连续形式的非线性或线性微分方程形式给出。

式（２．１ｃ）为边值约束，表示初始状态和终止状态需满足某些条件，其中初始状态一

般完全确定，终止状态则可能只有部分确定，而终止时刻ｔ，一般也不定，为待优化变

量。

式（２．１ｄ）为过程约束，其为泛函，表示ｘ（ｔ）和ｕ（ｔ）的变化需在某些容许范围内，作
用于整个连续的时间域ｔ∈【ｔｏ，ｔｊ】上。

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

相比于的一般运动规划问题，本文所研究的多旋翼飞行器平台及其在复杂环境下的

运动有以下特点：

１．载荷有限且能量效率不高，导致飞行时间短，因此希望规划出的轨迹节省能量。

２．飞行速度快且计算资源有限，在感知范围有限时要求实时求解，因此希望规划问

题的构造便于求解。

３．复杂环境为三维空间，障碍物密集且不规则，希望对环境的表征尽量简单又能够

尽量保留可行空间。

结合第１章所描述的不同情形，上述特点对于飞行器规划的性能指标了选取以及对

各项约束的构建与处理将产生影响。本章将对多旋翼飞行器规划的一般问题进行建模，

分析其受到的各项约束，其中第２．２节介绍本文构建的飞行器动力学模型约束，第２．３节

介绍本文考虑的过程约束，第２．４节介绍常见的边值约束如何构建，第２．５节介绍本文所

选取的性能指标，第２．６节总结本章内容。

２．２机体动力学模型约束

常见的多旋翼飞行器如双旋翼、四旋翼等一般为欠驱动系统，其构型空问的自由变

量属于ｓＥ（３１，有六个自由度，而独立控制变量的个数一般小于此。其较为完备的动力
学模型１５９】以电机转速作为控制输入。以三维空间的平移、旋转及其导数作为状态交量，

涉及到电机与螺旋桨的建模。该类型精确的非线性建模有助于设计飞行器姿态以及位置

控制的算法，然而从运动规划角度，遵循此非线性模型直接为状态变量或控制输入的每

一个维度规划轨迹则过于复杂。现今旋翼飞行器的导航框架一般将控制器与规划器解

耦，且利用其微分平坦【５６ｌ的性质在平坦空问进行轨迹规划，跟踪轨迹时再由平坦输出

恢复原本状态进行飞行器控制，本文所进行的规划与导航同样遵循此框架，即规划时的

模型约束使用平坦空间的动力学模型。

下面首先介绍一种满足实际应用需求的非线性模型，再介绍微分平坦以及如何使用

选定的平坦输出恢复该非线性模型的状态变量以及控制输入。

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

２．２．１非线性模型

此模型建立的思路是考虑实际中便于测量的状态量，例如位置与速度，以及考虑具

体应用应当约束的量，例如平稳飞行时姿态倾角不应过大，以及考虑便于设置允许取值

范围的一些物理限制，例如推力和机体角速度，而不考虑更复杂的电机模型与空气动力

学等特性，因其包含许多难以精确测量的参数。

我们首先引入如图２．１所示的参考坐标系（均为右手系）并作如下假设：

１．多旋翼为刚体，其机体坐标系原点与质心重合。

２．多旋翼运动过程中质量不变。

３．多旋翼只受重力与螺旋桨产生的拉力，其中重力沿世界坐标系磊。轴负方向，螺

旋桨拉力沿机体坐标系锄轴正方向。

．，．，１一’矿ｊ

Ｘ＂

考虑飞行器状态Ｘ＝＿［”ｐ，”Ｄ，Ｒ），其中”Ｐ＝（ｐ。，Ｐ。，仇）Ｔ∈Ｒ３为其质心相对于

世界坐标系Ｏ。ｚ。‰‰的平移矢量，”由＝（Ｖｘ，Ｖｙ，％）Ｔ∈Ｒ３为质心相于０。Ｘ。‰‰的

速度矢量，Ｒ∈ｓｏ（３）为以旋转矩阵表示的机体坐标系０６‰ＹｂＺｂ相对于世界坐标系的旋

转，由欧拉角经ｚ—ｘ—ｙ的转序得到。控制输入为１１＝｛，，６ｕ］．，其中ｆ∈Ｒ＞ｏ为所
有电机连带螺旋桨产生的合推力，６ｕ＝（Ｌｄｘ，％，心）Ｔ∈Ｒ３为机体角速度，则以牛顿一欧
拉方程描述的多旋翼飞行器动力学模型建立如下：

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

ｍ”苗＝ｍｇ＋Ｒｅａｌ （２—２ａ）

Ａ＝Ｒ【６ｕ】。， （２－２ｂ）

其中ｍ为飞行器质量，ｇ＝（０，０，一９）Ｔ为重力加速度向量， ｅ３＝（０，０，１）Ｔ为常数向量，

【６ｕ】×为６∽的反对称矩阵形式：

№：ｆ三－－。Ｗｚ兰 （２－３）

一岣蛾０

从运动规划实际应用的角度，此非线性模型相对精简，同时又较为精确地考虑了旋

翼飞行器的动力学特性。之后利用多旋翼非线性系统的微分平坦性质【５６】来进一步简化

模型约束。

２．２．２微分平坦及平坦后的线性模型

已有许多工作［５９・７７－７９１证明多旋翼系统具有微分平坦【５６１性质并在此基础上进行轨迹

规划与控制。具体来说，一个非线性系统具备微分平坦特性是指存在着平坦输出口，该

系统原本以非线性微分方程所描述的系统状态变量和控制输入能够由所选定的平坦输

出及其有限高阶导数｛盯，方，子，…｝直接解析计算得到，而无需对原本的非线性模型进
行积分。

在此我们选取多旋翼飞行器系统的平坦输出为仃＝‰，珊，乳，例Ｔ，其中前三项为

飞行器质心在世界坐标系下扛，Ｙ，ｚ）三轴的平移，妒为欧拉角中的偏航角。我们用，．Ｍ
表示变量ｒ从０到８阶关于时间导数的堆叠（ｒ，矿，．．．，ｒ（ｓ）），则微分平坦变换表示为从平

坦输出及其有限高阶导数到系统状态和控制输入的映射，记作

（Ｘ，ｕ）＝皿（盯Ｈ）． （２．４）

下面将给出式（２－２）下多旋翼飞行器动力学模型微分平坦变换皿的代数推导，即推

导如何由仃嘲解析地计算上小节所述非线性模型的状态向量ｘ＝｛”ｐ，”空，Ｒ）和控制
输入ｕ＝（，，６ｕ）。

１６

万方数据
 塑堑登博士专业譬垡论文 ２多旋翼飞行器动力学运动规划问题基本构建

”Ｐ与”西即分别为向量仃与寺中的前三项。

由定义可知旋转矩阵中三列的数值分别为机体坐标系的三轴单位向量在世界坐标

系下的表示，即Ｒ＝（Ｘｂ，Ｙｂ，铂），而直＝（瓯，Ｏｂ，磊），其中各列向量计算如下。
根据式（２－２ａ），锄与合推力向量同向，因此

Ｚｂ＝加ｒｉ５一ｇ），
（２－５）
毛＝伽（”§一ｇ）”Ｆ，
其中”酋与”夺分别为向量子与孑中的前三项，加：Ｒ３卜÷Ｒ３为将非零向量转化为同方
向单位向量的函数，先ｗ：Ｒ３卜÷缺３×３为其一阶导数，分别定义如下：

加（ｚ）＝ｚ／ＩＩ＝ｌｌ。，
Ｑ呦
帅）＝（一＝Ｔ扒＝／／ＩＩ圳：，
如图２．１所示，局部坐标系Ｏｂｘ。ｙｃＺｅ为一中间转换坐标系，其与机体坐标系原点相同，而

与世界坐标系在旋转上只有偏航角不同，则给定偏航角妒，可计算局部坐标系的Ｘ轴单

位向量及其对时间导数为

ｚｃ＝（ｃｏｓ妒，ｓｉｎ妒，ｏ）Ｔ，
（２－７）
圣。＝（一ｓｉｎ！ｂ，ｃｏｓ妒，０）。妒，

其中妒与西分别为向量盯与方中的第四项。而由欧拉角ｚ—ｘ—Ｙ的旋转顺序可知，
机体坐标系的Ｙ轴与Ｏｂｘ。孙平面垂直，由此可以确定机体坐标系的３，轴单位向量及其
对时间导数为

３，６＝’７■（锄×∞。），
（２－８）
Ｏｂ＝，ｉ＇Ⅳ（铂×ｚ。）（磊×ｚ。＋Ｚｂ×圣。）．

而机体坐标系的ｚ轴可由已求得的舶与Ｚｂ叉乘得到，计算如下

茁ｂ＝舶×铂，
（２－９）
氟＝雪６×孙＋铂×磊．

由此方法计算恢复飞行器姿态需满足两个条件：１ＹＴ）一ｇ不为零向量，即合推力不

为零；２．Ｚｂ×ｚ。不为零向量，即不允许姿态倾转９０。（相对于水平悬停时）。为了避开

第二类奇异点，我们采用Ｗａｔｔｅｒｓｏｎ等人【蛐ｌ基于Ｈｏｐｆ纤维化（ＨｏｐｆＦｉｂｒａｔｉｏｎ）的方法，

１７

万方数据
 浙江大学博士专业学位论文 ２ 多旋翼飞行器动力学运动规划问题基本构建

将飞行器姿态分解为偏航角引起的水平旋转，以及相对于水平面的倾斜角两部分，并且

使用四元数表示，分别称为偏航四元数和倾斜四元数。

给定偏航角砂，对应的偏航四元数为

蛳＝（ｃｏｓ（妒／２），０，０，ｓｉｎ（砂／２））。， （２－１０）

机体坐标系ｚ轴Ｚｂ如式（２—５），将机体ｚ轴从世界坐标系ｚ。旋转到锄的倾斜四元数为

栌而杀蓊（１＋Ｚｂ（３））《“２）＇锄（１Ｌ ０）Ｔ （２－１１）

可验证吼中不包含任何沿机体Ｚ轴自身的旋转分量，即偏航四元数和倾斜四元数互相

解耦，则飞行器整体的姿态四元数ｇ为

１ ＋

ｑ
２吼圆郇２万丽 １ Ｃ（

０《

删蜩悱删 胁托‰陋一一一一
ｌ Ｓ（一＿嘞㈤吖锄
”徊｜；㈦ ／）Ｓ几 砂．口叫现
㈨舭胆动

ｆ靠＋ｑ；一馥一谚 ２（ｑｌｑ２一ｑｏｑ３） ２（ｑｌｑａ＋ｑｏｑ２）＼

Ｒ＝冗忡ｔ（ｇ）＝ｌ ２（ｑｌｑ２＋ｑｏｑａ） 醯一ｑ｝＋谚一酲 Ｉ，
２（ｑ２ｑａ—９０９１） （２一１３）

＼２（ｑｌｑ３一ｑｏｑ２） ２（ｑ２ｑａ＋ｑｏｑｌ） 爵一ｑ｝一程＋酲／

ｆ磊（１）ｓｉｎ（妒）一２６（２）ｃｏｓ（妒）一磊（３）（ｚ６（１）ｓｉｎ（妒）一ｚｄ２）ｃ０８（妒））／（１＋锄（３））＼
２
Ｉ ｚｂ（１）ｃｏｓ（妒）＋ｚｂ（２）ｓｉｎ（妒）一之ｂ（３）（石６（１）ｃｏｓ（妒）＋ｚｂ（２）ｓｉｎ（妒））／（１＋２ｒ６（３））ｌ
＼ （Ｚｂ（２）２ｂ（１）一铂（１）庇（２））／（１＋铂（３））＋妒， ／

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

至此，通过式（２—５）至式（２一１５），我们能够由平坦输出及其至三阶的高阶导数｛盯，方，子，孑）
解析地计算恢复状态向量ｘ＝｛”ｐ，”由，Ｒ＞和控制输入Ｕ＝｛，，‰｝。
通过微分平坦，我们能够将模型约束简化为链式积分的线性系统

虫（￡）＝Ａｘ（ｔ）＋Ｂｕ（ｔ）， （２－１６）

根据不同应用考虑不同阶次，例如若为三阶线性系统，则以加加速度作为控制输入。使

用线性模型却依然能够考虑非线性项的约束，这为后续的优化计算带来极大便利。

２．３过程约束

过程约束是指在时间区间【ｔｏ，ｏ，】上对飞行器行为的限制，表示其无法到达状态空
间的某些区域，分为系统状态／控制输入区间约束，以及躲避障碍物的约束。

２．３．１系统状态，控制输入区间约束

考虑到实际物理系统的能力，例如速度、加速度无法到达无穷大，无法产生反方向

的推力等，系统状态及控制输入是有界的，其存在容许作用区间。考虑上小节所描述的

非线性以及线性的动力学模型，整体上飞行器在运动时会受到下列代数不等式约束：

ＩＩ郇（ｔ）ｌ Ｌ２＝ｌＩｆＤ（ｔ）１１２≤Ｖ。。， （２一１７ａ）

ｆｌａ（ｔ）ｌｌ。＝忡（圳：≤ａｍ。， （２—１７ｂ）

ｌＩＪ（ｔ）１１２＝０ａ（ｔ）Ｉ｜：≤Ｊ‘ｍａｘ， （２－１７ｅ）

１１６ｕ（ｔ）ＩＩ。≤Ｑ。ａｘ， （２－１７ｄ）

ＩＩ＆ｒｃｃｏｓ（ｅ３ｒａ（ｔ）ｅｚ）ｌＩ。钆。， （２—１７０

，ｍ｛。≤ｆ（ｔ）≤＾。，ｔ∈［ｔｏ，ｔｓ］， （２－１７０

其中ｖ。。，ａｍ。，Ｊ。。，Ｑ。ａｘ，Ｏｍ。和｛，ｍ洒‰。）分别为速度、加速地、加加速度、机体
角速率、姿态角和螺旋桨推力的区间幅值参数。一些参数的取值为飞行器的设计极限，

例如螺旋桨推力｛厶…‰，），而另一些参数则是根据具体任务情景确定的期望限制或
安全上下限，例如最大飞行速度ｖ。ａｘ和最大倾角０。。。式中的机体角速率６ｕ、旋转矩

阵Ｒ和螺旋桨推力值，由上一小节的微分平坦输出恢复得到。

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

注意到上述约束存在耦合关系，例如加速度与姿态角／推力之间存在关系，加加速

度与机体角速率之间存在联系，因此在规划时无需同时考虑。在不同的情形下，由于目

的不同，我们考虑使用不同的约束组合。例如在第４章探究实时局部规划时，考虑到有

限的感知距离，我们允许以较大姿态避开障碍物，且希望快速实时求解，因此我们使用

二阶线性模型，仅考虑速度与加速度的约束。在第３、５章我们使用三阶线性模型，则加

加速度约束需要被考虑。而在第６章探究跟随全局轨迹且希望飞行器保持姿态平稳，我

们使用确切的机体角速率约束、姿态角约束和推力约束来替代加速度和加加速度约束。

２３．２躲避障碍物约束

在所有任务情形中，飞行器在全时刻都需要避开障碍物。轨迹规划的避障依靠与地

图进行交互实现，在图１．２所示的模块化导航框架下，地图模块作为真实环境的抽象表

征，提供碰撞检测的接口，轨迹规划中对不同的地图类型有不同的处理方式。本文所探

究的任务情形多样，包含快速实时局部规划与全局最优规划，且复杂环境障碍物密集、

不规则，这对环境的表征提出两大要求：

１．能够以尽量小的计算花销快速查询某一构型是否与障碍物发生碰撞，这有利于快

速实时局部规划。

２．尽量接近真实环境，即地图的可行区域尽量不遗漏真实环境的可行空间，这对于

探究全局最优解至关重要。

根据上述要求，本文使用占据栅格地郾蜘（ＯｃｃｕｐａｎｃｙＧｒｉｄＭａｐ，ＯＧＭ）和欧式符
号距离场Ｉ翱（ＥｕｃｌｉｄｅａｎＳｉｇｎｅｄＤｉｓｔａｎｃｅＦｉｅｌｄ，ＥＳＤＦ）两种地图对环境进行表征。图２．２展

示了三维环境中不同分辨率下的占据栅格地图，图２．３展示了三维环境下的欧式符号距

离场地图。

两种地图都将环境以一定分辨率离散成空间栅格，每个栅格包含状态属性表示此栅

格是否与障碍物发生碰撞，碰撞检测时只需查询轨迹所经过或者覆盖的栅格是否碰撞。

在分辨率合适且感知准确的前提下，理论上真实环境的工作空间中无障碍物区域的任何

地方都为规划求解的可行空间。以安全通道为表征的地图形式为了计算效率一般表示成

连续相交的凸多面体或者球的组合，而真实环境中工作空间几乎都为非凸，尤其是复杂

的不规则环境，以凸组合形式的安全通道势必将一大部分真实可行区域排除在外，限制

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

图２．２不同分辨率下的占据栅格地图（图源自Ｈｏｒｎｕｎｇ等人ｌ“Ｉ工作）

图２．３欧氏距离场地图（图源自Ｈａｎ等人１８５Ｉ工作）

了全局最优解轨迹潜在的可行解空间，如图２．４所示。另外，占据栅格地图和欧氏距离场

地图的共同特点是查询效率高，查询复杂度为０（１）。

图２．４安全通道地图

碰撞检测的方式还与所研究对象构型空间状态的选取相关，本文所研究的多旋翼飞

行器体型相对较小，与地图的分辨率大小接近，因此将飞行器视作球体，则其构型状态

简化为三维空间中的位置

Ｐ＝（Ｐｘ，Ｐｕ，Ｐｚ）Ｔ∈酞３． （２—１８）

万方数据
 浙江大学博士专业学位论文 ２ 多旋翼飞行器动力学运动规划问题基本构建

如此一来，对空间障碍物按球体半径进行膨胀后即可构建出构型空间，碰撞检测只

需对构型空间中的点做检查。本文在进行实物测试时会将膨胀半径设置为稍大于机体半

径以考虑后续轨迹跟踪时的控制误差，保证安全性。

第４章和第５章使用占据栅格地图，构造完成后地图中每个栅格为三种状态：占用

（Ｏｃｃｕｐｉｅｄ），安全（Ｆｒｅｅ），或者未知（Ｕｎｋｎｏｗｎ），分别表示此栅格位置上有障碍物，没

有障碍物，或者还未观测到。避障需满足轨迹位置经过的栅格不为占用状态，轨迹避障

的约束描述为

ｐ（ｔ）隹ＯＧＭ。，ｔ∈［ｔｏ，ｔ１］， （２－１９）

其中ＯＧＭ赢表示被占据栅格所在位置的集合。

第３章和第６章使用欧氏距离场地图，其在占据栅格地图基础上构建，每个栅格额外

存储了一个数值，表示该栅格离与其最近的被占用栅格之间的距离。避障需满足轨迹位

置经过的栅格所示的距离大于一定阈值，轨迹避障的约束描述为

施丁（ｐ（ｔ））之ｒｔＤｌ，￡∈［ｔｏ，ｏ，】 （２—２０）

其中ｋ，ｚｓ７－：Ｒ３卜÷Ｒ表示计算一个三维位置点到其最近障碍物的距离的函数，ｎ川表示

安全距离阈值。

本文所使用的占据栅格地图构建算法为Ｅｌｆｅｓ等人【８２１提出的基于贝叶斯估计的概

率更新方法。欧氏距离场地图构建算法为Ｆｅｌｚｅｎｓｚｗａｌｂ等人【８６】提出的基于最小函数卷

积的方法。对于连续时间的避障约束，我们以一定时间分辨率对轨迹进行采样转化为离

散点的位置约束。

２．４边值约束

边值约束指飞行器运动起始时刻的状态ｘ（ｔｏ）和终止时刻的状态ｘ（ｔｓ）受特定条件
所限制，根据不同任务的不同情形所受约束不同，需结合实际情况具体确定。

２．４．１初始时刻约束

￡＝ｔｏ为规划的起始时刻，对于已知环境下的全局规划，例如第３章所探究的问题，

飞行器在起始时刻一般为静止状态，只需根据任务情形选定起始位置点即可。

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

对于未知环境下快速导航，例如第三、四、六章所探究的问题，需在行进中不停地

进行实时局部重规划，为了保证飞行器运动的连贯性，每次重规划起始状态的选择分为

以下两种情况：

１．当轨迹跟踪误差小于容许范围限度时，规划起始状态选取为当前所跟踪轨迹上某

个超前时刻的状态。令真实世界中当前时刻为ｔ。，则

ｘ（ｔｏ）＝ｘｔ，。。ｋ（ｔｏ＋ｚｘｔ）， （２－２１）

其中ｘ讹吐（ｔ）为当前所跟踪轨迹。此初始时刻约束假设了未来△￡时间内轨迹跟踪

较好，经过Ａｔ时间，飞行器的状态到了Ｘｔｒａｃｋ（ｔｏ＋Ａｔ）附近。

２．当轨迹跟踪误差超过容许的误差限度时，说明飞行器已经偏离了当前所跟踪的轨

迹，那么规划起始状态选取为以状态估计所得到的当前状态为初始值，再根据动

力学模型前向积分Ａｔ时间所得到的状态。即

ｘ（ｔｏ）＝血”。（ｘ。ｍ Ａｔ）， （２—２２）

其中Ｘ。“为状态估计所得到的飞行器当前状态，．也。为动力学方程。

上述约束要求每次轨迹重规划在△￡时间内结束，△ｔ的选取一般为数毫秒到数十毫

秒，根据具体规划任务和硬件计算性能条件确定。

２．４．２终止时刻约束

ｔ＝ｔ，为规划的起始时刻，对于全局规划，同起始时刻一样，飞行器在终止时刻一

般也为静止状态，只需根据任务情形选定终止位置点即可。此种起止均为静止状态的规

划被称作Ｒｅｓｔ．ｔｏ—Ｒｅｓｔ规划。

对于行进中不停进行着的实时局部重规划，每次重规划终止状态的选取根据任夯不

同有很大区别。此处列举一些例子，后续章节再结合具体情形具体分析。例如第４章的

实物试验中，终止状态由所跟随的移动目标确定，或是在交互中由用户给定，为状态空

间中一个确定的点：

ｘ（ｔｓ）＝ｘｇｏａｌ． （２－２３）

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

再例如第６章所探究的跟踪参考轨迹的情形，要求重规划终止状态约束在参考轨迹上，

此时终止状态约束为状态空间中的一条曲线：

ｘ（ｔ１）∈ｘ９捌， （２－２４）

其中Ｘ州为满足约束的目标状态点的集合。

２．５性能指标

对于飞行器规划，常见的性能指标有最短时间【８＂１１，最小能量【５９，９２］等等。式（２．１ａ）所

表示的综合性能指标了由积分指标Ｊ：￡（ｔ，ｘ（ｔ），ｕ（ｔ））出和终端指标圣（￡，，ｘ（ｔ，））两部
分构成。其中，积分指标用于描述任务对某些指标在时域过程中的积累优化需求，例如

若希望运动过程尽快完成，则可将积分指标设置为￡＝１，若希望飞行器在运动过程中

的机动尽量平缓，则可将积分指标设置为使累积控制输入最小Ｃ＝ｕ（ｔ）Ｔｕ（ｔ）。终端指

标用于描述任务对终止时刻状态的优化需求，对于飞行器的运动规划此指标较少出现，

对于终止时刻状态的限制一般在约束中构建而非在优化目标上构建。

本文所探究的飞行器复杂环境下自主导航，结合第２．１节中所描述的平台及其运动

特点，规划中的共同性能目标为期望以尽量平滑（节省能量）的轨迹尽快地完成任务。

因此，本文所探究的大多数情形下将以最小能量和最短时间的加权和为性能指标：

了＝石飞＋－ｕ１州）Ｔｕ ｔ））出柏咱）＋石飞ｕ∽Ｔｕ＠）出， （２－２５）

其中ｐ为时间项的权重。

２．６本章小结

本章针对飞行器运动规划的一般问题从最优控制的角度给出了数学模型的基本构

建。本章的主要贡献体现在三个方面：首先，构建了一个精简且实用的非线性动力学模

型，并依据微分平坦性质将此非线性的模型约束用链式线性的模型约束替代，而在过程

约束中需考虑的状态同样可由平坦输出及其高阶导数解析地计算恢复得到；其次，分析

了所探究问题的碰撞检测需求，选定了适合的地图表征形式，并给出了两种地图下避障

约束的描述；最后，阐述了在不同情形的飞行器导航问题中，尤其是未知环境下边值约

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

束该如何构建。本章所建立的问题基本模块将用于建立飞行器在不同情形不同任务下的

运动规划模型，具体将在后续章节介绍。

万方数据
 浙江大学博士专业学位论文 ２多旋翼飞行器动力学运动规划问题基本构建

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

３结合采样与优化方法的全局最优动力学运动规划

３．１引言

第２章从最优控制的角度构建了多旋翼飞行器运动规划的基本问题，本章主要探究

情形一．全局规划，也即飞行器在已知精确复杂环境下的全局最优运动规划问题。

式（２一１）所表示的最优控制问题的求解方法可分为直接法（Ｄｉｒｅｃｔ Ｍｅｔｈｏｄ）、间接

法（ＩｎｄｉｒｅｃｔＭｅｔｈｏｄ）和动态规划法（Ｄｙｎａｍｉｃ Ｐｒｏｇｒａｍｍｉｎｇ，ＤＰ）三类【９３】。直接法将控

制输入和状态变量在时间上离散化，将问题转化为离散的非线性规划问题（Ｎｏｎｌｉｎｅａｒ

Ｐｒｏｇｒａｍｍｉｎｇ，ＮＬＰ），再求解原问题离散形式的数值最优解。直接法形式较为统一，能

处理较为复杂的非线性约束，对不同问题的适用范围广，因而被广泛使用，但也存在两

个缺点［９４】：一、难以保证转化后的ＮＬＰ问题为原最优控制问题的最优解；二、初值的

选取对求解效率影响很大，初值选取不好容易导致陷入局部最优解。障碍物作为一类特

殊的强非线性约束，对初值选取有着很大影响。一些工作【ｚ，６３，９５彻首先求解最短路径规
划问题（以路径长度作为性能指标）得到一条无碰撞的几何路径，接着以此为初值在附

近的安全空间求解满足动力学约束的轨迹（性能指标发生变化）。由于障碍物约束的存

在，优化求解得到的轨迹一般很难跳出由该几何路径确定的特定路径拓扑类（Ｈｏｍｏｔｏｐｙ

Ｃｌａｓｓ）。因此，以此种层级式的规划方式，我ｔｆ］能够得到在特定路径拓扑类下的局部最

优解。然而，全局最优轨迹却可能不落在此特定路径拓扑类下，因为路径的长度和轨迹

的代价没有必然联系。若期望求取全局最优解，则需找到属于不同路径拓扑的路径分别

作为初值进行求解后再互相比较。层级式的规划方式由于前后性能指标的不一致从而不

适用于求取全局最优轨迹。

相比于直接法的离散化形式求解，间接法则通常利用取得极值的必要条件，并结

合充分性条件或问题的实际物理意义来得到解析形式的最优解【９８】。间接法包括变分法

（Ｃａｌｃｕｌｕｓ ｏｆＶａｒｉａｔｉｏｎ）和庞特里亚金极大值原理（Ｍａｘｉｍｕｍ Ｐｒｉｎｃｉｐｌｅ）１９９１。古典变分法

要求控制变量不受限制，是一个开集，且哈密顿函数对控制变量的导数存在，而实际中

控制输入一般为有界闭集。极大值原理为对交分法的改进，其允许控制变量受约束，并

且不要求哈密顿函数对控制变量连续可微。间接法通常涉及到啥密顿函数两点边值问题

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

（ＴｗｏＰｏｉｎｔＢｏｕｎｄａｒｙＶａｌｕｅＰｒｏｂｌｅｍ，ＴＰＢＶＰ）的求解，当存在复杂的非线性约束例如障

碍物约束时求解是很困难的。

动态规划法将原最优控制问题拆分为若干个阶段，根据贝尔曼最优性准则求取到每

个阶段的累积最优控制序列，直至求解完整的原最优控制问题。其中心思想是将原规模

较大的问题转换为多个规模较小、更易求解的子问题，分而治之∞】。在机器人的运动

规划领域，一些最优性采样类运动规划方法（Ｏｐｔｉｍａｌ Ｓａｍｐｌｉｎｇ－ｂａｓｅｄ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ，

ＯＳＢＭＰ）便是应用了动态规划法中分治的思想和贝尔曼最优性准则。在本质上，此类

方法通过增量式【１２１或者批次式【１４】采出的状态样本将连续的状态空间离散化，再以树结

构【ｌｌ】或者图结构【ｌｏｌ的形式依据最优性准则连接这些状态样本来探索解空间的连通性，

连接任意两个状态之间的运动轨迹求解即为求解一个ＴＰＢＶＰ。通过在采样时引入随机

性，其能够高效地探索完整的解空间从而得到接近全局最优的解。

结合本章探究的问题，我们提出一种结合采样和优化的方法来求解全局最优轨迹。

不同于层级式的方法，本章方法将原较大规模的问题通过采样分解为求解一系列子问

题，每个子问题具有和原问题相同的性能指标和约束，但规模更小，更容易求解。通过

不断求解分布于完整解空问中的子问题，我们在状态空间生长一棵轨迹树来不断探索具

有不同局部极小解的子空间，在算法的运行过程中自然地跳出局部最优解，确定更好的

路径拓扑，而无需提前确定路径初值。另外，由于ＯＳＢＭＰ需要大量求解ＴＰＢＶＰ，而近

邻状态间的轨迹更容易满足约束，尤其是避障约束，因此我们使用约束松弛，即在求解

ＴＰＢＶＰ时暂时性地忽略过程约束，仅考虑模型约束和边值约束，使用极小值原理求取

子问题的解析解，直到必要时再检查求解结果是否违反约束，以此增加求解效率。

虽然ＳＢＭＰ适用于求解全局最优问题，但是单纯基于采样的方法【４３，１００，１０ｌ】收敛缓

慢，因其难以采到最优解周围的状态样本。充分采样有助于轨迹树的良好生长然而需花

费许多计算时间。为了提升收敛性，我们期望轨迹树中每一条边的生长都以尽量小的代

价为探索解空间做出最大贡献，每一个被添加的节点都在状态空间中合适的位置。另

外，对涉及到系统动力学模型的轨迹规划来说，时间这一维度至关重要。因此，我们提

出在添加一个状态节点到轨迹树中之后在空间和时间两个层面对轨迹树进行形变优化，

即，进行时空优化来保持轨迹树良好生长，如图３．１所示。优化轨迹树的难点在于保持优

化效率的同时设计实际的优化目标。对轨迹树中每一个状态节点和每一条轨迹边都进行

形变优化过于低效，我们引入形变单元的概念，其包含一个状态节点以及所有连接此节

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

（ａ）结合时空形变优化 （ｂ）不结合形变优化

图３．１添加相同数量的状态节点后轨迹树的拓扑结构以及当前最优轨迹

点的轨迹边。一个形变单元只占完整轨迹树的－４，部分，然而对其进行形变优化能够提

升以该单元所包含节点为根节点的子树的质量。以此方式，我们以较低的代价高效利用

了每个状态样本带来的解空间中新的信崽，从而加速了求解全局轨迹算法的收敛性。

在本章后续部分，第３．２节建立了具体问题的数学模型，提出使用结合采样与优化

的方式求解；第３．３节描述了轨迹树在状态空间的生长过程，将轨迹边的求解描述为终

端时刻自由、终端状态受约束的最优控制问题并松弛约束求解；第３．４节提出了以形变

单元的形式对轨迹树进行形变提优，该约束优化问题以罚函数法求解。第３．５节进行了

比较数值实验，验证了本章所提出方法的高效性；第３．６节进行总结。

３．２问题描述

本章我们选取３阶线性积分模型，以加加速度作为控制输入，偏航角另外考虑，则

多旋翼飞行器的线性模型为

文（ｔ）＝ｎｘ（ｔ）＋Ｂｕ（ｔ），
ｌ 》

），ｕｃｔ，＝Ｆｃ幻，ｐｃｔ，＝
ｆ Ｐ

…瓢沙
Ｉ × ３

ｘ（ｔ）＝ｌ由 Ｂ ＝ ，

Ｉ 眺Ｉ ３
／，ｆ－Ｉ 、＼

ｆＦ ．１
）０ ＿
１

其中Ｉ。为礼维单位方阵，Ｏｉ。Ｊ为ｔ行Ｊ列的零矩阵。

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

选取权衡时间一能量最优的性能指标，全局规划的具体数学模型为线性二次最短时

间（Ｌｉｎｅａｒ Ｑｕａｄｒａｔｉｃ Ｍｉｎｉｍｕｍ Ｔｉｍｅ，ＬＱＭＴ）问题【１０２１：

。。曷｝岛，了＝／ｏ巧（ｐ＋三ｕ（ｔ）Ｔｕ（ｔ））出 陋乏ａ

ｓ．ｔ．文（ｔ）＝Ａｘ（ｔ）＋Ｂｕ（ｔ）， ∞
捌 ｂ

ｘ（ｏ）＝Ｘｉ。砒ｘ（Ｔｊ）＝Ｘ９。￡， 吣乞Ｃ

ｌＩｖ（ｔ）ｌｌ。＝ｌＩｆ，（ｔ）１１２≤Ｖ。。， 叫
也 ｄ

ｌｌａ（ｔ）［Ｉ：＝ＩＩｉＢ（ｔ）ｌｌ。≤ａ。。， 陋乏ｅ

ＩＩｊ（ｔ）ｔｌ。＝ＩＩｒｉ（ｔ）ｌｌ。≤Ｊ。。， ㈣
他 蟹

ｆ倪ｍ－（ｐ（ｔ））≥７＂ｔ。ｆ，Ｖｔ∈【０，Ｔｓ】 陇之．ｇ

表示求取最优控制ｕ＋（ｚ）和最优到达时间乃，使得飞行器从初始时刻状态Ｋ础到达目

标状态Ｘｇｏａｌ，期间使得运动尽量平缓且耗时尽量短，满足速度、加速度，以及加加速度
在某些幅值内，同时避开所有障碍物。

我们采取采样与优化相结合的方法来求解上述全局规划问题，具体方法分为轨迹树

的生长和轨迹树的形变两个部分。

３．３轨迹树的生长

３．３．１时空形变树算法

轨迹树生长过程算法的主体框架与ＲＲＴ＃算法类似，包括采样、找父节点、重连接

等操作，输入规划起始状态Ｋ。ｉｔ，规划目标状态南。一，障碍物环境Ｅ，以及形变模式￡’

最终求解得到近似全局最优轨迹矿。

我们首先定义所需的数据结构。给定现有的轨迹树结构，记为丁，树上每一个状态

节点竹∈丁包含以下信息：

・‰：节点礼对应的状态；

・陬：树丁中节点礼的父节点；

・ｅ。：从节点‰到住的轨迹边，也表示为（砩。，‰）；

１０

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

・已：轨迹边ｅ。的时长；

・Ｇ：树丁中节点ｎ的直接子节点；

・夕。：从树７＿的根节点至节点ｎ所经过的轨迹边所构成的轨迹的累积代价值（ｃｏｓｔ－

ｔｏ－ｃｏｍｅ ｖａｌｕｅ，ｇ－ｖａｌｕｅ），代价计算如式（３－２ａ）。

轨迹树生长的具体步骤如算法１所示，在不混淆的情况下，我们以节点ｎ对应的状

态‰代表该节点。

算法１：时空形变树算法
Ｉｎｐｕｔ：障碍物环境Ｅ，起始状态ｘ／础，目标状态ｂ。２，形变模式Ｃ

Ｏｕｔｐｕｔ：轨迹矿

Ｉ树中状态点Ｖ÷－（ｘ砒），树中轨迹边￡÷－０，轨迹树丁卜（Ｖ，￡），
２ ｗｈｉｌｅ终止条件未达到ｄｏ

３ Ｘ。。＋－－Ｓａｍｐｌｉｎｇ（Ｅ）；

４ 疋。。。＋－－ＮｅａｒＮｅｉｇｈｂｏｒＳｅａｒｃｈ（７－，ｘ。ｅ１｜Ｊ，ｒ）；
５
ｉｆ砩＋＿ＣｈｏｏｓｅＰａｒｅｎ∥‰。‰ｅ”蟛ｔｈｅｎ
６ Ｖ÷－＋．【ｘ。。。），￡＋－＋（ｘｐ，Ｘｎｅｗ），丁÷－（Ｖ，￡）；
７ ｉｆ ＴｒｙＣｏｎｎｅｃｔｉｎｇＧｏａｌ（ｘｎｅ¨ｘ９。∥ｔｈｅｎ
８ ｌ找到一个可行解；
ｌ—

９ ｉｆ已经找到一个可行解ｔｈｅｎ

ｍ ＲｅｗｉｒｅｌｎＣａｓｃａｄｅ（，ｋ。，ｘｎｅ。）；
ｎ ｌ甜＋－－ＳｅｌｅｃｔＤｅｆｏｒｍａｔｉｏｎＵｎｉｔｓ（ｘｐ，￡）；
：ｊ ＤｅｆｏｒｍｌｎＯｒｄｅｒ（Ｌ０；

１３矿＋－ＲｅｔｒｉｅｖｅＴｒａｊｅｃｔｏｒｙ（Ｔ，ｘ９砌）；

以下详细介绍算法１中所涉及各函数的具体内容。

１．初始化（第１行）：将初始状态Ｋｎｌ。添加到轨迹树７－中作为树的根节点，此时树

中只有一个节点，没有边。

２．采样（第３行）：在三维工作空间均匀随机采样９维状态空间的前三维，即采样位

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

置状态，得到新状态点ｘ。。。速度状态和加速度状态在采样时设为不定，将在后续求解

ＴＰＢＶＰ时自动计算，之后在第３．３．２节详述。

３．近邻搜索（第４行）：我们以固定的欧氏距离半径ｒ在轨迹树的状态节点中搜索出

距离ｘ。。小于ｒ的节点，该过程可描述为

ＮｅａｒＮｅｉｇｈｂｏｒＳｅａｒｃｈ（Ｔ，ｘ，ｒ）：＝｛ｕ∈ｙ｜ｌＩ口一ｘＩｌ２≤ｒ）． （３－３）

在Ｗｅｂｂ等人【４３】的工作中，在给定一固定代价值了和一状态节点ｘ时，其计算出了能

够以小于代价歹到达此节点的后向可达集，以及从此节点出发，能够以小于代价歹到

达的前向可达集，将状态范围在这两种可达集内的状态节点作为近邻节点，分别进行后

续的选择父节点和重连接操作。然而此计算开销大，我们使用欧氏距离代替真正的代价

距离来简化计算。Ｌｉ等人｛ｍ０３１和Ｚｈｅｎｇ等人［４ｑ的工作在近邻搜索时同样使用欧氏距离，

其指出了前（后）向可达集实际为以Ｘ为中心的次黎曼球（ｅ－ｒａｄｉｕｓｓｕｂ－Ｒｉｅｍａｎｎｉａｎｂａｌｌ），

而在一些条件下该次黎曼球内部总是存在一个特定大小的欧式空间高维球（Ｅｕｃｌｉｄｅａｎ

ｈｙｐ即－ｂａｌｌ），因此在近邻搜索时可以使用欧氏距离近似替代真正的代价距离，在后续的

找父节点和重连接时再使用代价距离。使用大于一定值的固定的距离半径进行紧邻搜索

依然能保证渐进最优性【１２１。

４．选择父节点（第５、６行）：从近邻节点的集合‰。中，选择一个“有潜力的
（ｐｒｏｍｉｓｉｎｇ）”节点作为）【ｔｌ。的父节点，使得轨迹边（昂，翰。）满足约束，并且经由ｘｐ
到达）【’１。的累计代价值最小，该过程为动态规划最优性准则的体现，可描述为

ｃ幻。ｓｅＰｎｒｅ佗‘（ｚ，ｘ，Ｅ）：５ ｇｘｖ＋歹（】ｃｐ，ｘ）
ｘ，ｒａＥｉｎｚ

ｓ．ｔ．ＣｈｅｃｋＦｅａｓｉｂｌｅ（ｘｐ，ｘ，Ｅ）＝＝Ｔｒｕｅ，（３－４）

夕ｘ，＋Ｊ（砩，ｘ）＋Ｈ（ｘ，舄。１）＜盛，，删

其中ＣｈｅｃｋＦｅａｓｉｂｌｅ函数检查轨迹边是否满足约束，歹函数表示计算轨迹边的代价，Ｈ

函数表示计算可接受的（ａｄｍｉｓｓｉｂｌｅ）启发式函数对轨迹边的代价进行估计，此估计作为

真实代价的下界，虼ｒｒｅｎｔ为当前已获得的最优解的代价，若当前无可行解，则虻。硎＝
＋ｏｏ。若找到满足条件的父节点，则将节点）ｃ，｜。和轨迹边（ｘｐ，ｘ）添加到轨迹树丁中。“有

潜力的（ｐｒｏｍｉｓｉｎｇ）”节点意思为可能存在经由该节点到目标状态的轨迹，其代价小于

当前已有解代价。若此节点为ｎｏｎ－ｐｒｏｍｉｓｉｎｇ，则无需将其添加至树中而耗费计算资源。

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

算法２：选择父节点
Ｉｎｐｕｔ：障碍物环境Ｅ，潜在父节点集合疋，待连接节点）ｃ，ｌ。。

Ｏｕｔｐｕｔ：父节点～
１
Ｘｐ＋－ｎｕｌｌ，Ｑ÷－０；

／・对“有潜力的”父节点进行排序 ●ｆ

３
ｌ豇ｔ，【。。。÷－９ｘ。＋了（Ｋ，ｘ。。）；
４
ｉｆ孔渤。。＋日（ｘ。。，誓争埘）＜唬，—ｔ ｔｈｅｎ

ｓ
Ｌ Ｑ÷－＋＠，氨‰Ａ

７ ｘ嘲。÷－Ｑ．ｔ叩（）；

。。９
Ｌ ｘ№ｐ≯＋－－小
关于了函数和日函数的计算将在第３．３．２节和第３．３．３节详述。实际实现如算法２所示，我

们先对荪。口ｒ中的节点毪进行排序，不考虑障碍物，计算其中每个节点状态转移到）ｃ，ｌ。。

的代价，再加上）ｃ‘的累计代价值甄，得到从轨迹树根节点经由ｘ／到ｘ一的轨迹累积代
价的一个估计蟊；，。。。，若）ｃ‘“有潜力”，则将元组（】ｃ‘，豇，‰。）存进优先队列Ｑ中，按
氟，ｘ～以升序排列。在之后的循环中每次取出Ｑ中轧渤。。最小的节点（算法２第７行），
此时再考虑障碍物，对轨迹边（】ｃｔ，ｘ。。）进行碰撞检测，若无碰撞，即找到符合要求的
父节点，退出循环。先对潜在的父节点进行排序相当于延迟了碰撞检测，不需要对所有

潜在的父节点轨迹边做碰撞检测。由于碰撞检测操作往往比估计代价要花费更多计算资

源，此种做法往往效率更高。

５．尝试连接目标点（第７行）：由于目标状态为状态空问中的一个点，当向树中添加

一个新节点后，若此新节点距离目标点小于一定距离，则我们尝试以此节点状态和目标

点状态作为边值条件求解ＴＰＢＶＰ，若求解结果满足约束，则找到了一条满足约束的轨

迹，即式（３．２）的一个可行解。

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

算法３：序列重连接
Ｉｎｐｕｔ：障碍物环境Ｅ，轨迹树丁，新添加的节点ｈ。。
１ Ｑ ４－＋ｘｎｅ＂；

２ ｗｈｉｌｅ Ｑ非空ｄｏ

ｘｉ＋．Ｑ．ｚ印（）；

疋。。４－ＮｅａｒＮｅｉｇｈｂｏｒＳｅａｒｃｈ（Ｔ，ｘ，ｒ）；
ｆｏｒ Ｘｃ∈，％。，ｄｏ

＾ｘ涔。÷－５ｂｃ；＋了（ｘｉ，Ｘ。）；

ｉｆ＾ｘ淳。＜ｇｘ。八＾ｘ泓。＋Ｈ（ｘ。，Ｘｇｏａｌ）＜夕乞ｒｒ。。ｔ ｔｈｅｎ
ｉｆ ＣｈｅｃｋＦｅａｓｉｂｌｅ（ｘｉ，ｘｃ，到ｔｈｅｎ

Ｉ改ｘ。的父节点为强；

Ｌ Ｌ睁＋ｘｃ；

６．序列重连接（第１０行）：新添加的状态节点ｘ。。和轨迹边带来了新的信息，则可

利用这些信息重新构建轨迹树的拓扑结构。即，若经由轧。到达其近邻节点集合以。。，
中的某些节点ｘ而使得这些节点的累积代价值减小，且新构建的轨迹边（ｈ。。，Ｘ）满足
约束，则将这些节点的父节点改变为ｈ。。。如算法３所示，我们采取ＲＲＴ＃［１３】算法中的

重连接策略，不止对新添加状态节点）【ｒ｜。的近邻节点进行重连接，若某个节点ｘ被重

连接了，则对ｘ近邻节点也尝试重连接，即，将新信息充分传播到树中的其他区域。同

样，我们只对“有潜力的（ｐｒｏｍｉｓｉｎｇ）”节点进行重连接操作。实际操作时，我们将重连

接后的节点放入优先队列Ｑ，按节点的累计代价值ｇ—ｖａｌｕｅ以升序排列，之后每次从

优先队列中取出ｇ—ｖａｌｕｅ最小的节点，对其近邻节点尝试重连接，若某节点被重连接

了，则将其放入队列，如此迭代进行，直至队列为空，即树的结构不再需要发生改变。

７．时空形变优化（第１１、１２行）：在添加一个新的节点后，我们对轨迹树进行形变优

化以充分利用局部解空间信息。我们首先在树中选取形变单元（在第３．４．１节介绍），再

按顺序对这些形变单元进行优化，时空形变优化的过程将在第３．４节详述。

８．获取当前最优轨迹（第１３行）：在算法运行的过程中，所有“有潜力的（ｐｒｏｍｉｓｉｎｇ）”

节点保存其当前最优的父节点，若找到解，则可从终止节点根据父结点指针不断回溯至

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

根节点，途中经过的轨迹边即构成当前最优轨迹。

９．终止条件（第２行）：终止条件可设为算法运行时间超出预设求解时间，或是轨迹

树中节点的个数超出预设个数等等，可根据不同需求不同设置。

３．３．２最优轨迹边术解

在算法１每一轮迭代的ＣｈｏｏｓｅＰａｒｅｎｔ函数、ＴｒｙＣｏｎｎｅｃｔｉｎｇＧｏａｌ函数和ＲｅｗｉｒｅＩｎＣａｓ－

ｃａｄｅ函数中，我们都需要求解两个邻近状态点之间的一条最优轨迹边，此过程作为整个

全局轨迹求解的一个基础部分会被大量调用，因而需要尽可能快的求解。在求解某一条

轨迹边的时候，作为从原问题分解出的小规模同类问题。性能指标与全局优化问题相同，

对于各类约束，我们则将过程约束暂时忽略，只考虑边值约束和模型约束，根据极大值

原理推导得到解析形式的解。即给定始末状态，我们便能够解析地得到连接此始末状态

以多项式形式表示的一段轨迹，其最优系数和最优时长的计算过程只涉及多项式求根和

约束检验等代数计算，而不涉及优化迭代。对于边值约束，对于ＣｈｏｏｓｅＰａｒｅｎｔ函数，起始

时刻状态ｘ（０）为近邻搜索出节点的状态，完全给定；终止时刻状态ｘ（Ｔ）为新采样出的
节点状态，只有三维的位置Ｐ给定，其余自由，即部分受约束。对于ＴｒｙＣｏｎｎｅｃｔｉｎｇＧｏａｌ

函数和ＲｅｗｋｅＩｎＣａｓｃａｄｅ函数，起始时刻状态和终止时刻状态均为树中已有节点的状态

或目标状态ｘ。砌，采样状态中自由的部分已在ＣｈｏｏｓｅＰａｒｅｎｔ函数中计算得到，因此完全

给定。据此，求解轨迹边的问题描述为终端时刻自由，终端状态受约束的最优控制问题：

州Ｉｉ氓ｌｉｎＴ，球（０）＇ｘ（剐＝ｆ（ｐ＋烈１矿ｕ＠）出（３－５ａ）
ｓ＋ｔ． 文（￡）＝Ａｘ（ｔ）＋Ｂｕ（ｔ）， （３－５ｂ）

ｘ（Ｏ）＝Ｘｏ， （３－５ｃ）
ｒ

Ｇ【ｘ（Ｔ），卅：｛《Ｔ）一ｘ１
２ ０’ 话终止状态完全给定’
（３．５ｄ）
ｌ
ｘ（Ｔ）出。１，２，３一Ｐ＝０，ｉｆ终止状态部分给定，

其中状态变量、控制输入等与式（３．２）相同。ｄｉｒａｌ，２，３表示某一向量的第１，２，３维，Ｘｏ统一

表示起始时刻状态，ｘ。统一表示终止时刻状态，Ｐ表示采样出的三维位置状态。

依据极大值原理的具体求解过程如下：

３５

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

引入协态向量函数ｘ（ｔ１∈Ｒ９，并作哈密顿函数如下：

日（￡，ｘ（ｔ），ｕ（ｔ），入（￡））＝ｐ＋互１ ｕ（ｔ）Ｔｕ（ｔ）＋入（ｔ）Ｔ（Ａｘ（￡）＋Ｂｕ（ｔ）），（３－６）

则协态方程为：

却）－＿器一Ａ＇ｒｘ（班 （３－７）

由Ａ矩阵的结构可知协态向量函数ｘ（ｔ）满足其前三维为常数，中间三维为关于时间ｔ

的一次多项式，后三维为关于时问ｔ的二次多项武。

因在最优轨线ｘ’（ｔ）和最优控制输入ｕ‘（≠）上哈密顿函数取极小值，而此时暂时假

设ｕ（ｔ）约束不受限制，是开集，则做控制方程如下：

型塑磐掣“（ｔ）＋矿洲：０，（３－８）
一ｒ川…ｒ７’
ａ旷ｆｔ）

由Ｂ矩阵的结构可发现最优控制ｕ’（ｔ）各维度上的解分别为最优协态向量函数”（ｔ）后
三维对应维度上的相反数：

ｕ’（ｔ）＝一入象。７．８’９（ｔ）， （３－９）

即为关于时间ｔ的二次多项式。

再由３阶线性模型式（３—５ｂ）可知最优轨线ｘｌ（￡）的位置维度每一轴各自为关于时间ｔ
的五次多项式，以ｚ轴为例，表示如下：

ｘ＋（ｔ）出。１’４＇７＝

㈢＝（萤｛ｉ）
其中ｃｘ＝（ａ。，ｂｚ，ｃｘ，如，ｅ。，厶）Ｔ为未知的ｚ轴多项式系数向量，卢（ｔ）＝（１，ｔ，ｔ２，ｔ３，ｔ４，ｔ５）Ｔ
（３－１０）

为时间基底向量。则茁轴的最优控制为：

矿（￡）ｄ‘ｍ。＝露（ｔ）＝％Ｔ声（ｔ）， （３－ｎ）

对Ｙ轴和ｚ轴同理。

则最优协态向量函数可写作

”（￡）＝一ｐ‘５’（ｔ）Ｔ Ｃｚ Ｃｙ ｃ：】， 卢㈣（￡）Ｔ ｂ勺ｃ。］， 卢（３’（ｔ）Ｔ Ｃｘ Ｃｙ

Ｃｚ】）Ｔ∈Ｒ９，（３－１２）
３６

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

其中ｐ（。’（ｚ）表示ｐ（ｚ）对ｔ的ｓ阶导数，例如ｐ（３’（ｔ）＝声（ｔ）。
式（３－５ａ）的性能指标可写作

７（ｘ（。），ｘ（Ｔ））＝了（ｃ＊，Ｔ）＝／Ｔ（ｐ十五１ｕ（ｔ）Ｔｕ（ｚ））出
＝ｐＴ＋互１。。丢：，ｃｊ ＺＴ声＠）声（ｔ）Ｔｄｚ ｃｔ，‘３—１３’

其中Ｃｋ为南轴的多项式系数向量。

下面将分别讨论末端状态完全给定与部分给定的情形：

情形１．末蜡状态完全给定

对于终止状态完全给定的情况，由边界条件ｘ（ｏ）＝）【０∈Ｒ９和ｘ（Ｔ）＝Ｘ１∈Ｒ９可
确定１８维齐次线性方程组

㈤ｘ（Ｏ）／／＝㈤Ｘｌ ｆ３—１４）

当Ｔ＞０时线性无关，可将所有多项式系数Ｃｋ用Ｔ表示，再带入式（３—１３）将其转换为关

于Ｔ的标量函数歹（ｃ％（Ｔ），Ｔ）＝了口），求取最优到达时间Ｔ转换为标量函数求极值问
题：

Ｔ＋＝ａｒｇｒａｉｎＪ（Ｔ、
（３－ １５）
一
ｓ．ｔ．Ｔ＋＞７１ｍ…歹（ｒ）＞０

其中露；。为可行到达时间下界的一个估计，可根据位移距离、速度、加速度约束幅值

等计算。实际求解上式时，我们计算方程警＝０的根，此例中为求解一元六次方程，
从所有根与露伽之中选取使代价Ｊ（Ｔ）最小的满足条件的作为最优到达时间Ｔ＋。求得
Ｐ之后，即可恢复得到最优多项式系数。

情形２．束靖状态部分给定
对于终止状态只有位置给定的情况，除了已知的边界条件外，还需联立横截条件。

根据式（３．５ｄ）的终止状态约束，可写横截条件为

入（Ｔ）＝—ＯＧ［盯ｘ（Ｔ），Ｔ］Ｔ＿Ｕ＝（ｖ１，ｖｚ，ｖａ，０，０，０，０，０，０）Ｔ∈Ｒ９，（３一１６）
其中ｕ＝（ｕ。，忱，ｕ３）Ｔ∈酞３为终端等式约束相关的拉格朗日乘子向量。最优协态方程

式（３—１２）满足”（Ｔ）＝Ａ（Ｔ），联合已知的边界条件ｘ（Ｏ）＝Ｘｏ∈Ｒ９和Ｘｄｉ。１．２，３（Ｔ）＝Ｐ∈

３７

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

（ｘ。篓；Ｔ，）＝（ｉ，］， ｃ３－，７，

３．３．３多项式轨迹遗的两种表示

当系统为ｓ阶线性积分模型时，最优轨迹表不为２ｓ一１次多呗式。对｛ｚ，Ｙ，ｚ，中仕

意一维，省略维度下角标，我们记多项式边界条件ｄ＝ＩｘＴ（ｔ）Ｉ仁０，ｘＴ（ｔ）Ｉｔ：Ｔ］Ｔ∈Ｒ２８，多

项式系数向量ｃ∈Ｒ加，则给定时长Ｔ，一段多项式可由多项式系数向量和时长｛ｃ，Ｔ】－，

与多项式边界条件向量和时长｛ｄ，Ｔ）两种形式表示。
由ｃ至ｄ的转换可由边界条件的定义得到，如下：

ｄ＝Ａｚ（Ｔ）ｃ，堋＝∞Ｇ扮 ｐ㈣

其中的各个块矩阵Ｅ，Ｆ，Ｇ为Ｒ“５维的方阵，其第ｉ行Ｊ列的元素构建如下：

Ｅ巧２｛：一。！：ｆｆ；ｉ≠＝Ｊｊ，， 。。∞

Ｆ巧ｃＴ，。｛：『一１”／。一。卜ｐ一‘：ｆｆｉｉ＞＜Ｊｊ，， ｃ３－２。，

Ｇ文Ｔ）＝譬黜・Ｔ一卅．（３－２１）

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

由ｄ至ｃ的转换可通过对矩阵Ａ，（Ｔ）进行整体求逆［１】或者更高效的分块求逆［１０４，１０５】

得到，然而矩阵求逆的计算复杂度高，后续第３．４．４节的约束优化求解需要大量使用此两

种表示形式的转换，我们希望尽快求解。

通过观察矩阵Ａｆ（Ｔ），发现其为Ｃｏｎｆｌｕｅｎｔ Ｖａｎｄｅｒｍｏｎｄｅ Ｍａｔｒｉｘ形式，由Ｗａｎｇ等

人【１０６】的工作指出，｛ｃ，Ｔ】．与｛ｄ，Ｔ）之间是双射关系（Ｂｉｊｅｃｔｉｏｎ），即存在解析形式的转
换，如下：

ｃ＝Ａｂ（Ｔ）ｄ，钆㈤２㈦品）’ ｐ２２，

其中的各个块矩阵Ｕ，Ｖ，Ｗ同样为Ｒｓ。８维的方阵，根据ＳｃｈａｐｐｅＵｅ等人【１０７】的工
作，其第ｉ行Ｊ列的元素构建如下：

吣Ｐ叫！ｉ州ｌ （３—２３）

ｉｉ（Ｔ）＝竺诗高等掣，
ｌ＝Ｊｊ，，

Ｖ）１‘ｋ－ｊ¨－知（０ｉ（‘）１一（力‘州＝；Ｅ （３－２４）

ｗ羽）－笥与杀祷斜・
ｗ…∑：＝州旬力（８吉１）（２８—等‘１） （３－２５）

因此一条轨迹边可由ｃ，Ｔ）或｛ｄ，Ｔ）两种形式表示。此两种可互相解析转换的多
项式轨迹边表示形式为后续的轨迹树约束形交优化提供了极大便利。

当轨迹时长给定时，例如以第３．３．２节所述方式求得，一条轨迹边的代价可经由｛ｃ，Ｔ）
计算为

ｆｌ（ｃ，Ｔ）＝ＰＴ＋ｆｏＴ三ｃＴ卢㈤∽ｐ㈤（￡）Ｔｃ ｄｔ＝∥＋互１ｃＴＱ（丁）ｃ， （３－２６）

或经由｛ｄ，Ｔ）计算为

ｆｆ（ｄ，Ｔ）＝了（ｘ＠）ｌｔ＝０，Ｘ（帖Ｔ，Ｔ）＝ｐＴ＋ｚ１－ｄＴＭ（Ｔ）ｄ，
。－２７’
ｄ：Ｊｌｘ（ｔ）ｌｔ：ＴＩ
ｘ＠）Ｉｔ－。Ｉ，Ｍ（丁）：Ａｊ（Ｔ）Ｑ（Ｔ）Ａ。（Ｔ）．
此代价可作为在完整约束解空间中的两个状态点之间最优转移代价的一个估计，

第３．３．１节中的启发式函数日由此计算。

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

３．４轨迹树的时空形变

在轨迹树不断生长以探索解空间信息的过程中，当一个新的状态节点和新的轨迹边

被添加到轨迹树上时，此轨迹树随即采集了解空间的一些新信息。然而，由于采样的不

充分，一些状态节点的累积代价值（ｃｏｓｔ－ｔｏ—ｃｏｍｅ ｖａｌｕｅ）也许并不能很好地估计解空间

中该处代价的实际值，表现为树没有很好的生长，尤其在时间这一维度。而对轨迹树中

的某些轨迹边进行形变能够提升轨迹树的质量。

３．４．１形交单元构造

我们首先定义形变单元的概念，并将其作为轨迹树形变的最小单位。

如果我们在状态空间“重新放置”树上其中一个节点，即改变其状态，并保持其他

所有节点的状态不变，同时保持节点间的连接结构不变，那么只有与该被“重新放置”

的节点相连接的轨迹边会受到影响。因此，我们定义一个形变单元为某个集合，该集的

元素包含一个状态节点、该节点与其父节点之间的边、和该节点与其直接子节点之间的

边。在一个形变单元内，时空形变通过优化该节点状态和这些相关轨迹边的时长进行，

与此同时轨迹树上其他所有节点的状态和其他所有轨迹边的时长保持不变。我们记一个

形变单元中的元素集合如下：

｛Ｋ，Ｔ。）＝｛ｘ。，兀，正，ｌｌ∈厶）， （３－２８）

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

其中礼为被“重新放置”的节点，厶表示节点札所有直接子节点的集合，Ｔ。＝｛死，Ｚ，Ｉｉ Ｅ

厶｝包含与之相连接的所有轨迹边的时长。值得注意点是，我们设置节点的状态为优化

变量而非轨迹边的多项式参数，由此带来两大优势：

１．隐式地消除了节点上的连续性约束。

２．优化变量的个数大大减少。

一个形交单元的例子如图３．２所示，该例中的形变单元含有三条轨迹边。

３．４．２优化性能指标设计

优化形变单元时我们需要设计一个合理的性能指标，其需遵循一些准则：在形变过

后，轨迹树的整体质量应当有所提升，从而根据此树将有着更高的可能性找到更好的甚

至是最优的全局轨迹。另外，该指标的计算应当尽量简单。根据Ｈａｕｅｒ等人【７ｌ】所揭示

的，通过在状态空间生长轨迹树对解空间进行一定程度离散的覆盖，则轨迹树中所有节

点的代价值（ｃｏｓｔ—ｔｏ—ｃｏｍｅｖａｌｕｅ，ｇ－ｖａｌｕｅ）之和能够在某种程度上作为对此解空间的最

优值函数的一个估计。同样考虑到优化一个形变单元会且仅会改变该单元所包含节点以

及该节点所有后继节点的代价值，即影响以该节点为根的子树，而整个树中的其他轨迹

分支却不受影响。我们因此设计优化性能指标如下：

肌＋∑仇，（３－２９）
ｉ∈Ｄ“

其中口。表示节点ｎ所有后继节点的集合。另外可选择性地，我们可以给上式中每一个

节点赋上权重Ｗ∈【０，１】，以表示其构成最终的最优轨迹的可能性，彰显其重要程度。我

们能够设计某些启发式函数（ｈｅｕｒｉｓｔｉｃｓ），例如第３．３．３节所描述的来估计节点到规划终

点的代价值（ｃｏｓｔ－ｔｏ—ｇｏｖａｌｕｅ），并以此估计值作为权重。如此设计性能指标表示我们期

望优化以该形变单元所包含的节点为根的子轨迹树。

根据树结构的性质，我们有

鼽＝ｃ（砩。，）【７Ｉ）＋绑。， （３－３０）

其中ｃ（麓，ｘＪ）表示连接节点ｉ与Ｊ的边的代价，可由式（３－２７）解析地得到，其作为考虑所

有约束时两个节点之间状态转移所耗费真正代价值的一个估计。通过式（３．３０），性能指

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

标的计算通过计算轨迹树中每一条边的代价值ｃ（ｘｉ，玛）的加权和来完成。每条边（Ｘｉ，ｘｊ）
的权重计算如下：以整体树的起始节点为起点，以优化单元所代表子树的每一个节点为

终点，每一个起终点组合都有一条轨迹，对每一条轨迹，计数某轨迹边被经过的次数，

即为该边的权重。注意，本段落中边的权重非上一段落所描述的节点的权重。边的权重

由优化开始时当前树的结构确定，对于某树形结构，各边的权重是确定的。而节点的权

重则通过启发式函数估计，是可选项，也可不估计而都设为１。

因此，性能指标转化为：

如＋∑ｇｉ＝∑∑嘞ｃ（毪，巧）
｛∈Ｄｎ ‘∈７；Ｊ∈Ｑ

＝厶ｃ（‰，ｘ。）＋∑哦ｃ（ｘ。，墨）＋ｃ （３．３１）
ｌ∈矗

＝∑喀ｃ（酝，毪）＋Ｃ，
ＺＪ ’’…’７
｛∈｛ｎ）Ｕ ｃ，ｌ

其中兀表示以节点ｎ为根节点的子树，厶等于１＋ｎｂ ｄｅｓ（ｎ），ｎｂ＿ｄｅｓ（ｎ）为节点ｎ所

有后继节点的个数，Ｃ为一个常量，表示形变单元中所包含的边以外的、树中的其他边

的代价和。

至此，所设计的性能指标仅与某一选定的形交单元相关，却能够在某种程度上呈现

某一子轨迹树的质量，进而作为一部分解空间的最优值函数的估计。

３．４．３罚函数法杓建形交单元的无约束优化

在第３．３节轨迹树的生长过程中，我们求解轨迹边时对待障碍物和运动学等约束以

先暂时忽略后检查的方式实现了对完整解空间的快速探索。而在本节形变优化的过程

中，我们更专注于形变单元附近的局部解空间，利用已在树中收集的解空间信息来提升

轨迹树的质量。此时，障碍物和运动学等约束应当直接处理而非用后检查的方式。

记９（ｐＭ（ｔ））∈时＋１ １ ０为式（３－２ｄ）至式（３．２９）等泛函形式的不等式约束，我们构

建罚函数以软约束的形式将其转移至性能指标中从而构建成无约束优化问题。由于泛

函形式的不等式约束实际包含无穷多个不等式约束而难以直接处理，我们采取Ｔｅｏ等

人ＢｏＢ，１０９］的方法，对构建出的罚函数在固定个数的离散的时刻点进行采样，对违背约束

的部分进行积分，以有限维度的约束积分逼近，则由罚函数转化成的性能代价指标杓建

为其采样点的加权和。另外，注意到这些泛函约束的计算在每一条轨迹边之问相互独

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

立，即，９＠Ｈ（ｔ）），￡∈【０，正ｌ仅涉及到ｃｔ以及正。
因此。对于某一多项式系数以及时长为｛ｃｔ，正）的轨迹边，其罚函数转化成的性能

（３・３２）
乃（ｃｔ忍，乜）２卺∑ｊｆｆｉｏ畸ｚＴｍ凹眵（ｃｔ，正，￡），ｏＪ，
其中ｋｉ是采样点的个数，爿∈Ｒ茹１为权重向量，表示泛函约束向量９中每一维的惩罚
因子，（ｗｏ，Ｗ１，…，呲；一－，Ｗｋ，）＝（１／２，１，…，１，ｌ／２）为梯形求积法则中的系数，ｍａｘ［－，・】
为对向量每一维度分别取较大值的操作，ｔ＝歹／‰．互为采样时间间隔。

对式（３－３１）中的每一条边构建如式（３－３２）所示的罚函数，则针对某一形变单元＜ｘ。，Ｔ。）

ｔ瓠∈｛磊“以（讯聃讹忍觑）），
。‘３３’

铲｛黑鲫鼍
其中ｄ｜ｌ等于１＋ｎｂ＿ｄｅｓ（几），ｎ６＿ｄｅｓ∞）为节点礼所有后继节点的个数。

３．４．４梯度下降求解形交单元的优化

对形变单元中的一条轨迹边＜ｑ，正】．，ｉ∈｛ｎ＞Ｕ厶，我们根据链式法则推导式（３．３３）中

解耦的性能指标相对于优化变量｛ｈ，Ｔ。）的梯度，如下；

筹＝鼍象＝Ｑｃ耻ｔ，甏＝筹关象， ｃ３圳，

甏＝ｐ＋尹１配ｋ，甏＝髻＋警豢毛，（３－３５）

等＝鼍姜啪ｍ吡ｂ卜吲，０］， （３－３６）

堕：』［Ａｏｌ（硝Ａ｛ｌ（硝．Ｔ，删＝ｎ （３－３７）
‰Ｉ陋（正）Ｔ Ａ№）Ｔ］。，班∈厶，

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

ｂＯＧｈ＿２／３（ｈ。），（ｔ酱）ｐＣｈ）（ｔ），ｖ草ＯＧ＿ｈ ＝啊２Ｂ矿（ｈ＋鬻１）（ｔ）Ｔｃｉｐ（ｈ∽挑｛１）２渤’叫。，
鼍一眦）酱，警一科ｃｔ鬻，
ｐⅢ’

万方数据
 ＮＯ—ＤＥ
浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

訾．

图３．３形变模式意图

３．４．５形变模式

对一个形变单元进行优化能够更充分地探索其附近的局部解空间而保持轨迹树的

其他部分不变。对多个形变单元进行优化有助于提升树的整体质量但会带来更多的计算

开销，权衡计算负担和期望的优化程度，我们提出４种不同的形变模式，各种模式以不

同形变单元组合的形式对轨迹树的不同部分进行形变优化。如图３．３所示，在一个新的

状态节点礼ｅ叫被加入到轨迹树中之后，我们记其父节点为ｎ，树的形变优化按以下４种

模式中的一种进行：

节点模式（ＰａｔｔｅｒｎＮＯＤＥ）：仅优化一个形变单元，该单元包含节点几和与之相连
接的边，如图３．３左上角蓝色的树边所示。

树干模式（Ｐａｔｔｅｒｎ ＴＲＵＮＫ）：优化多个形变单元。从节点ｎ根据父节点指针不断

往前回溯至起始节点的一个直接子节点，按顺序记录下所经过的每个节点，再以相反顺

序优化这些节点所在的形变单元，即从该直接子节点优化到节点ｎ，如图３．３右上角橙色

的树边所示。

树枝模式（Ｐａｔｔｅｒｎ ＢＲＡＮＣＨ）：优化多个形变单元。从节点礼开始，对其后继节

点进行广度优先搜索，按顺序记录下所经过的每个节点，再以此顺序优化这些节点所在

的形变单元，即优化以节点ｎ为根节点的子树中所包含的所有形变单元，如图３．３左下

角红色的树边所示。

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

全树模式（Ｐａｔｔｅｒｎ ＴＲＥＥ）：优化多个

形变单元。从整树的根节点ｓｔａｒｔ开始，对

其后继节点进行广度优先搜索，按顺序记

录下所经过的每个节点，再以此顺序优化

这些节点所在的形变单元，即优化完整树

中所包含的所有形变单元，如图３．３右下角

绿色的树边所示。

按不同模式进行形变优化的能效将在

下一节进行讨论。

３．５全局规划数值实验

３．５．１实验设置

在以下的数值试验中，我们设置ｓ＝

３，即使用加加速度作为输入的三阶积分

模型来表示我们的多旋翼飞行器动力学模

型。式（３．５ａ）性能指标中时间项的权重设置

为Ｐ＝１００。速度、加速度、加加速度的

幅值限制分别设置为５ｍ／ｓ，７ｍ／ｓ２，以及

１５ｍ／ｓ３。地图事先已知并且为静态，对于碰
撞检测，我们使用０．１ｍ分辨率的占据栅格

地图，将障碍物膨胀０．２ｍ后以相同分辨率

构建欧氏距离场地图以提供距离梯度信息。

３．５．２形交模式比较
图３．４测试环境以及某次结果轨迹
本实验使用一台含英特尔ｉ７．６７００处理

器，主频为３．４ ＧＨｚ的计算机进行。

本实验对比测试了按不同形变模式进

行规划，以及不使用时空形交而仅仅使用

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

单纯采样方法进行运动规划（记作ｗ／ｏ）的效果。除此之外，我们还与一种基于搜索的

全局动力学运动规划求解方法进行比较。对于搜索类的方法，我们使用了动力学Ａ星

算法［５５１（ｋｉｎｏｄｙｎａｍｉｃＡ木）的启发项增强版本（ｗｅｉｇｈｔｅｄＡ木【７６１）来加速搜索过程，并比

较了３种不同增强启发项的权重，分别为１．７、２－３、２．８。由于所提出的算法具有渐进收

敛性，我们给每种算法设置３秒钟的运行时长，每种算法运行１００次。

测试环境以及某次运行的结果轨迹如图３．４ａ所示。其中，淡蓝色轨迹指示以节点模

式（ＰａＲｅｍＮＯＤＥ）进行形变的结果，橙色轨迹指示以树干模式（ＰａＲｅｍＴＲＬ脚（）进行
形变的结果，红色轨迹指示以树枝模式（ＰａｔｔｅｒｎＢＩ乙心ＣＨ）进行形变的结果，绿色轨迹
指示以全树模式（ＰａｔｔｅｒｎⅡ也Ｅ）进行形变的结果，紫色轨迹指示不进行时空形变的结

果，深蓝色轨迹指示动力学Ａ星算法的结果。可以发现相比于不进行形变（紫色）和搜

索类方法（深蓝色），本章所提出的时空形变树方法产生更顺滑的轨迹，尤其是以树枝

模式（ＰａＲｅｍ ＢＲＡＮＣＨ）进行形变（红色）。

惯ｏＵ

ｃｏ；ｎ｜ｏ们铝∞

１１ｍｅ ｆｉｒｉｓ）

图３．５不同形变模式的轨迹代价收敛性豳磬呖寻意图

最优轨迹的代价随时间变化的收敛过程如图３．５所示，其中短划线表示１００次实验

的标准差，不加启发项增强的搜索方法花费数十秒求解因而没有在图中显示。所提出基

于采样的时空形变全局规划方法能够快速找到第一个解，并且在之后轨迹代价快速收

敛。相比于不进行形交（ｗ／ｏ，紫色菱形标记），按任意一种模式进行形变的收敛效率均

４７

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

极大提高。在４中模式中，树枝模式（ＰａｔｔｅｒｎＢＲＡＮＣＨ，红色粗十字标记）稍稍胜过全

树模式（Ｐａｔｔｅｒｎ ＴＲＥＥ，绿色粗叉形标记）而完全优于节点模式（Ｐａｔｔｅｍ ＮＯＤＥ，淡蓝色

圆点标记）和树干模式（ＰａｔｔｅｒｎＴＲＵＮＫ，橙色倒三角标记）。这是因为树中新加入的状

态节点所带来的新信息而产生的潜在提升主要作用于以其父节点为根的子树，而树的其

他部分因此新节点而受影响的可能性较小。因此，以树枝模式（ＰａｔｔｅｒｎＢＲＡＮＣＨ）进行

形变优化不失为对计算代价和提升树质量之间的一个较好的权衡。对于不同启发权重的

搜索类的方法（深蓝色细叉形标记），权重越大则越快找到解，然而其轨迹的代价也越

高，质量越低，且所耗时均大于所提出的方法而求得全局轨迹的质量又低于所提出的方

法。

３．５．３时空优化比较

本实验使用一台含英特尔ｉ７—１０７５０Ｈ处理器，主频为２．６ ＧＨｚ的计算机。

第３．４节提出的时空形变优化能够方便地与快速探索随机树（ＲＲＴ）一族的采样类运

动规划算法相结合，本实验将其融合至三种相似的ＲＲＴ族动力学运动规划算法中，即

原始ＲＲＴ、ＲＲＴ＊和ＲＲＴ＃，分别记作ｋＲＲＴ、ｋＲＲＴ＊和ｋＲＲＴ＃。对于三种算法，我们

将本章所提出的时空形变优化与仅空间形变优化和不形变优化进行比较，分别记作加后

缀．ＳＴ、一Ｓ和不加后缀。仅空间形变时我们不优化轨迹边的时长，仅优化树中节点的状

态。对于进行形变的方法，我们采取树枝模式（ＰａｔｔｅｍＢＲＡＮｃＨ）。我们给每种算法设

置７秒钟的运行时长，每种算法运行１００次。

测试环境以及某次运行的结果轨迹如图３．４ｂ所示。其中，淡蓝色轨迹指示ｋＲＲＴ算

法不进行形变优化的规划结果，深蓝色轨迹指示ｋＲＲＴ算法进行时空形交的规划结果，

黄色轨迹指示ｋＲＲＴ＊算法不进行形变优化的规划结果，绿色轨迹指示ｋＲＲＴ＊算法进行

时空形交的规划结果，橙色轨迹指示ｋＲＲＴ＃算法不进行形交优化的规划结果，红色轨

迹指示ｋＲＲＴ＃算法进行时空形变的规划结果。分别将淡蓝色轨迹与深蓝色轨迹、黄色

轨迹与绿色轨迹、橙色轨迹与红色轨迹进行比较，可以发现经过相同的计算时间，进行

时空形变优化的方法能得到更加平滑的轨迹。

表３．１中记录了１００次运行后不同形变方法经过相同计算时间后得到的轨迹时长和

轨迹代价的平均值和标准差。对比仅空间形变优化，可以发现本章所提出的时空形变不

仅最终得到的平均轨迹代价更小，平均轨迹时长也更短，说明了在时间维度进行形变的

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

表３．１不同形变方法得到的平均轨迹时长和平均轨迹代价对比

有效性。

最优轨迹的代价随时间变化的收敛过程如图３．６所示，对每一种ＲＲＴ族动力学运动

规划算法，将同一种颜色和标记下的实线（时空形变）与点划线（仅空间形变）和点线

（不形变）进行比较，可以发现进行时空形变优化的收敛最快。结合形变后三种ＲＲＴ族

方法的收敛趋势一致，这进一步说明了所提出的时空优化方法的泛用性。

３．５．４与层级式规划方法比较

本实验使用一台含英特尔ｉ７．６７００处理器，主频为３．４ ＧＨｚ的计算机进行。

为了测试所提出的全局轨迹规划方法其搜寻更优路径拓扑的能力（以更小轨迹代

价表示），我们与两种层级式规划的轨迹规划方法进行比较：Ｂｒｙ等人【２】的方法和Ｗａｎｇ

等人［９７】的方法。本方法与被比较的方法都使用相同的时间．能量最优的性能指标，并且

求解轨迹都表示为分段多项式。被比较的两种方法的求解都需要提供一条先导的几何路

径。在Ｂｒｙ等人［２３的方法中，该几何路径隐式地包含了附近无障碍区域的信息。通过将

待优化多项式轨迹的中间路径点的位置固定在几何路径的拐点上，该待优轨迹某种程度

上就被限制在了几何路径周围的安全区域，其余优化变量如轨迹中间路径点的高阶导数

和每段轨迹的时长则通过梯度下降优化得到。在Ｗａｎｇ等人［９７１的方法中，该几何路径用

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

≮ｏＵ

ｃｏ茹ｎｌｏ∽苗∞

图３．６不同优化方法的轨迹代价收敛性比翱ｇｉ；意图

于引导生成一条安全飞行走廊（Ｓａｆｅ Ｆｌｉｇｈｔ Ｃｏｒｒｉｄｏｒ，ＳＦＣ），接着将待优化的轨迹严格

约束在安全飞行走廊中从而保证安全。其方法的优化变量为多项式轨迹中间路径点以及

各段时长。就我们所知，Ｗａｎｇ等人【９７】的轨迹生成方法为基于飞行走廊类方法中效率最

高的。

我们使用ＲＲＴ＊算法生成所需的几何路径，使用Ｌｉｕ等人【９６】的方法生成安全飞行走

廊。我们不断增加ＲＲＴ＊算法的时间预算，分别为２ｍｓ，５ｍｓ，ｌＯｍｓ，３０ｍｓ，５０ｍｓ，和

１００ｍｓ，对应生成６条总长度递减的几何路径，将其用作Ｂｒｙ等人［２１方法和Ｗａｎｇ等人【９７】

方法的先导几何路径。基于此６条几何路径，两种方法分别对应生成６条轨迹。接着，

我们运行本章所提出的时空形变全局轨迹规划算法，在ｌＯｍｓ，５０ｍｓ，１５０ｍｓ，２００ｍｓ，

２５０ｍｓ，和３００ｍｓ ５个递增的时刻分别记录当前所得到的最优轨迹，同样得到５条轨迹。

三种算法分别运行１００次，测试环境以及某次运行的结果轨迹如图３．７所示。红色

轨迹指示本章所提出算法的规划结果，绿色轨迹指示Ｂｒｙ等人［２】方法的规划结果，蓝色

轨迹指示Ｗａｎｇ等人【９７】方法的规划结果，淡黄色透明凸多面体指示安全飞行走廊。

图３．８展示了三种方法１００次规划的平均求解结果，图中６组数据分别对应６种不

同时间预算下求解出的轨迹。图３．８ａ展示了层级式方法求得的轨迹平均代价（绿色和蓝

色折线）随着其先导几何路径长度（灰色矩形块）的变化趋势，我们发现其轨迹质量并

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

（ａ）２ｍｓ时间运行ＲＲＴ＊，１０ｍｓ运行本章算法 （ｂ）５ｍｓ时间运行ＲＲＴ＋，５０ｒａｓ运行本章算法

（ｃ）ｌＯｒｅｓ时间运行ＲＲＴ＊，１５０ｍｓ运行本章算法（ｄ）３０ｍｓ时间运行ＲＲＴ＊，２００ｍｓ运行本章算法

（ｅ）５０ｍｓ时间运行ＲＲＴ＊，２５０ｍｓ运行本章算法（Ｄ ｌＯＯｍｓ时间运行ＲＲＴ。，３００ｍｓ运行本章算法

图３．７与层级式规划方法进行比较的测试环境以及递增时间预算时所得到的结果轨迹

不是随着先导轨迹质量提升而提升，有时基于长度较短的先导路径得到的轨迹反而代价

更高，这是因为其层级式的方案在不同层级的优化性能指标不一致，这不利于全局轨迹

规划。而本章所提出的方法在完整的基于采样的规划过程中有着一致的性能指标，且具

有渐进最优性，随着时间预算增加，轨迹代价不断下降（红色折线）。图３．８展示了三种

方法求解时每个模块的平均耗时。在每一组条形图中，左边条为Ｗａｎｇ等人【９７】方法的耗

时，其由ＲＲＴ＊几何路径规划（黄色块）、安全飞行走廊构建（蓝色块）和轨迹优化（灰

色条）三部分组成；中间条为Ｂｒｙ等人【２】方法的耗时。其由ＲＲＴ＊几何路径规划（黄色

块）和轨迹优化（灰色条）两部分组成；右边条为本章所提出方法的耗时，只需轨迹优

化（灰色条）一部分。可以发现本章所提出方法的总体耗时在各组中均最短。分别对比

图３．８ｂ和图３．８ａｅｅ对应的组别，可以发现本章所提出方法以最少的耗时得到代价最小的

轨迹。

万方数据
 浙江大学博士专业学位论文 ３结合采样与优化方法的全局最优动力学运动规划

Ｉ…“Ｒ…Ｒ译鳞蓟专复………………ｉｉ溺蔺喾石猫溺爱蕲簖
—．－８吖等入方法孰迹代价—●一本章所提方法孰迹代价

盎２２
￡
剧１７
磊

１２
刀卫．『＃器荫沭溉一ｒｎ一
７ ”拍弱Ｍ妁及扎
ｉ｜ ｜｜

（ａ）基于不同先导几何路径求得的平均轨迹代价

（ｂ）总体以及各过程平均耗时

图３．８基于不同时间预算的平均求解时间与轨迹质量对比

３．６本章小结

本章针对已知精确环境下的飞行器全局最优动力学运动规划问题，整体上使用在采

样类动力学规划方法中深度结合局部优化的方法来求解。我们总结主要贡献如下：１．我

们提出时空形变树算法，在探索解空间的过程中生长一棵在时间和空间层面同时发生形

变的轨迹树，随着树的生长自然地选择拓扑路径更好的轨迹，直至逼近全局最优解。２．

树中一段轨迹边的生长建模为求解末状态部分受约束的ＴＰＢＶＰ问题，使用约束松弛策

略加速求解。３．我们提出形变单元的概念，其仅包含树中的一个状态节点以及连接在其

上的所有轨迹边，只占整体轨迹树的一小部分，我们之后针对性的设计用形变单元表示

轨迹树整体质量的性能指标，在时间和空间层面优化不同形变单元的组合，如此便能够

花费尽量小的代价高效提升整体轨迹树的质量，提升算法的收敛性。

链接１中给出了本章内容动画形式的介绍。

ｈｔｔｐｓ：／／ｗｗｗ．ｊｉａｎｇｕｏｙｕｎ．ｃｏｍ／ｐ／ＤＴＶａｅｆｏＱＩＮ６ｕＣｈｊＴｊｂｋＥＩＡＡ

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

４层次化实时局部动力学运动规划

４．１引言

在上一章中，我们探究了飞行器在已知复杂环境下的全局最优轨迹规划问题，期望

在复杂的障碍物环境中尽可能快地找到接近最优的轨迹，然而由于障碍物环境的拓扑复

杂性，即使对飞行器的非线性动力学模型做了简化，探究完整的解空间求解最优轨迹或

是接近最优轨迹也往往需要花费较高的计算代价，无法实时进行。

在本章中，我们探究情形二．局部规划，也即飞行器在未知环境下的导航，重点研

究局部实时运动规划问题。对于未知环境下的高速飞行，要求在行进中极短时间内完成

轨迹的重规划以应对实时改变的、不可预知的环境，避开障碍物。对于一段完整的导航

过程，随着飞行器在环境中的位置发生改变，其周围环境不断变化，重规划不断进行，

每次飞行器也只是执行重规划出轨迹的－－ｄ＇部分，求解最优轨迹的必要性不如全局规划

时强烈。因此，在局部实时运动规划问题中，对最优解的需求转变为快速求解一个足够

好的可行解。另外，于较为廉价的飞行平台，例如一些常见的消费级旋翼飞行器来说，

其搭载的计算平台一般较低级，计算资源有限，如此则难以快速生成无碰撞的高质量轨

迹，生成轨迹的实时性与高质量难以同时保证。

对于上述难题，我们提出了一个系统性的轨迹规划方案来填补这一缺陷。我们的方

案采取了层次化的规划框架，分为两层，称之为前端和后端。

前端动力学运动规划基于简化的飞行器动力学模型，考虑飞行器的初末状态，得到

满足运动学约束的、无碰撞的轨迹。对于高维度状态空间下的动力学运动规划，基于采

样类的方法可以通过设计智能采样策略来提升整体规划效率而具备巨大潜力。可以设想

一下下述情景：飞行器沿着长走廊高速飞行，则速度方向朝向走廊墙面的状态样本大概

率没有价值，而那些速度方向沿着走廊的状态样本则有较大可能落在最优轨迹附近。另

外较为重要的是，基于采样类的算法天然具有即时性（Ａｎｙｔｉｍｅ Ｐｒｏｐｅｒｔｙ），意味着我们

能够快速得到一个可行的轨迹，而在执行此轨迹的过程中再去寻找更优的轨迹［４叼。在未

知环境中快速飞行对规划的实时性要求高，即时性显得尤为重要。因此，我们的前端动

力学运动规划使用基于采样类方法并且根据环境的障碍物分布进行状态空间采样。在此

万方数据
 浙江大学博士专业学位论文４层次化实时局部动力学运动规划

前端中，我们先快速构建一个拓扑连接图来近似表征可行工作空间的连接性，再依据此

拓扑连接图在高维状态空间采样以引导轨迹树的生长，最终高效得到较高质量的轨迹。

由于考虑了初始状态和动力学约束，相比于单纯的路径规划结果，此轨迹会位于一个相

对合理的路径拓扑中，且其在时间维度的规划结果会较为合理。

有了前端较为高质量的轨迹初值，后端则较为轻量，基于前端得到的轨迹，后端充

分地利用已有轨迹所探求到的解空间信患进行高效地提优。我们设计了一个轻量级的基

于优化的后端来进一步提升轨迹的一些关键性能指标，即平滑性和连续性，同时保证其

安全无碰撞以及运动学可行。为了保障后端求解的高效性，我们整合那些已在前端获得

的信息到优化目标当中，并且构建为一系列具有闭式解（Ｃｌｏｓｅｄ．ｆｏｒｍＳｏｌｕｔｉｏｎ）的二次

规划（Ｑｕａｄｒａｔｉｃ Ｐｒｏｇｒａｍｍｉｎｇ，ＱＰ）问题。

每一次重规划可作为独立的问题求解，因此本章后续描述均针对某一次重规划。

本章节内容安排如下：在第４．２节中，我们描述了基于近似拓扑引导采样的前端动

力学运动规划算法，得到高质量的初值轨迹，给到第４．３节中描述的后端进行高效轨迹

提优。之后在第４．４节进行与相关算法的数值对比测试，在第４．５节使用真实四旋翼飞行

器在未知环境下进行自主导航试验。最后在第４．６节进行总结。

４．２基于引导采样的前靖动力学运动规划

４．２．１问题描述

前端规划主要考虑初始状态、动力学约束和避障，相较于上一章的全局规划，初始

状态一般非静止，考虑到实时求解需求，本章对飞行器的模型进一步简化，使用２阶

线性模型，以加速度作为控制输入。前端规划问题的数学建模如式（４．１）所示。整体与

第３．２节全局规划类似，但减去了加加速度的约束，为２阶线性模型，且碰撞检测使用占

据栅格图而非梯度距离场地图。规划初始时刻状态的选取如第２．４．１节所述，规划终止时

刻状态根据实际需求决定，在第４．５节描述。对于前端求轨迹初值，我们使用单一采样类

的方法进行求解，期间不进行形变优化等操作。

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

。嚣Ⅳ＝／ｏ巧（ｐ＋扣Ｔｕ㈣）出 （４—１ａ）

ｓ．ｔ． 文（￡）＝Ａｘ（ｔ）＋Ｂｕ（ｔ）， （４－ｌｂ）

ｘ（ｏ）＝Ｘｉｎｌｔ，ｘ（乃）＝ｂ。ｉ， （４－ｉｔ）

０口（￡）１１２＝ｌＩｂ（ｔ）１１２≤Ｖ。。， （４－Ｉｄ）

ＩＩｏ（ｔ）１１２＝Ｉｌｉｊ（ｔ）１１２≤ａｍ。， （４－］０

ｐ（ｔ）Ｉ≠ＯＧ‰，Ｖｔ∈【０，乃】， （４－Ｉｆ）

ｘｃｚ，＝（；：：），ｕ。，＝酋＠，，ｐｃ”＝仨差；），Ａ＝（三：！； 黔Ｂ啦）， （４－１９）

４．２．２基于引导采样的轨迹树生成算法

算法流程如算法４所示，主体与算法ｌ类似，遵循ＲＲＴ＊算法［１２１的框架。其中初始化

（第１行）、近邻搜索（第４行）、选择父节点（第５、６行）、尝试连接目标点（第７行）、重

连接（第１０行）、获取当前最优轨迹（第ｌｌ行）、终止条件（第２行）等操作与算法ｌ、算

法２、算法３相同或类似，以下主要描述不同之处，各记号沿用第３．３．１节的定义。

１．在采样状态节点（第３行）时，状态样本为６维，根据第４．２．３节所描述的使用近似

拓扑引导采样。对于状态样本的前三维位置，我们首先随机选择近似拓扑图的一条边，

再随机采样出该边上的一个位置点，之后以该位置点为中心，在垂直该边的平面上按二

维高斯分布采样，得到位置点。对于后三维速度，其方向以该边的方向为中心，按高斯

分布采出偏离此中心方向一定夹角的向量，其大小以最大允许速度幅值为界均匀随机采

样。

２．在选择父节点（第５、６行）、尝试连接目标点（第７行）、重连接（第１０行）时，由

于速度状态也由采样确定而非未知，连接两个状态点的无约束轨迹即为求解终端时刻自

由、始末状态完全已知的两点边值问题（ＴＰＢＶＰ）。

３．在重连接时（第ｌＯ行），权衡求解实时性和轨迹最优性，我们只对新添加状态节

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

算法４：引导采样动力学ＲＲＴ＊算法
Ｉｎｐｕｔ：障碍物环境Ｅ，起始状态ｘ俐ｔ，目标状态ＸｇｏｏＪ

Ｏｕｔｐｕｔ：轨迹丌．

－树中状态点Ｖ÷－｛ｘｍ“】．，树中轨迹边￡卜０，轨迹树７－＋－Ⅳ，￡），
２ ｗｈｉｌｅ终止条件未达到ｄｏ

３ ｘ＂＂＋一ＴｏｐｏＧｕｉｄｅｄＳａｍｐｌｉｎｇ（Ｅ）；

４ 荪。。＋－ＮｅａｒＮｅｉｇｈｂｏｒＳｅａｒｃｈ（Ｔ，）【ｔｌ。。，ｒ）；
Ｓ
ｉｆ砩÷－ＣｈｏｏｓｅＰａｒｅｎｔ（Ｘ．ｅⅢ，‰ｍ助ｔｈｅｎ
‘
Ｖ÷－＋｛ｘ。。），￡÷－＋（昂，ｘ～），丁÷＿（Ｖ，￡）；
７ ｉｆ ＴｒｙＣｏｎｎｅｃｔｉｎｇＧｏａｌ（ｘｍ＂）啕∞￡Ｊ ｔｈｅｎ
８
Ｌ找到一个可行解；
， ｉｆ已经找到一个可行解ｔｈｅｎ

ｍ
Ｌ Ｒｅｗｉｒｅ（Ｘ，～，‰）；
１１矿÷－ＲｅｔｒｉｅｖｅＴｒａｊｅｃｔｏｒｙ（Ｔ，‰１）；

点的近邻节点进行重连接，而非如算法３的循环重连接。

对于上述ＴＰＢＶＰ的求解，我们同样采样松弛约束的方法，先暂时忽略过程约束，按

极大值原理求解，获得轨迹后再检查约束。哈密顿函数，协态方程，以及控制方程的构

建分别与式（３－６），式（３－７），以及式（３．８）相同。此时协态向量函数为６维，ｘ（ｔ）∈Ｒｅ，由

Ｂ矩阵的结构可发现最优控制ｕ＋（ｔ）各维度上的解分别为最优协态向量函数”（ｚ）后三
维对应维度上的相反数：

Ｕ‘Ｏ）＝一入盏。４．５＇６（ｔ），（４－２）

即为关于时间￡的一次多项式。以ｚ轴为例，最优控制为：

ｕ＋（ｔ）跏。１＝ｎ：（ｔ）＝％Ｔ声（ｔ）， （４－３）

其中ｃ。＝（％，ｋ，白，也）Ｔ为未知的ｚ轴多项武系数向量，卢（ｚ）＝（１，ｔ，ｔ２，ｔ３）Ｔ为时间基

底向量。再由２阶线性模型式（４－１）可知最优轨线ｒ（ｔ）的位置维度每一轴各自为关于时

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

间ｔ的三次多项式，以ｚ轴为例，表示如下：

ｘ＋ｃｔ，凼。，，ｔ＝＠善；）＝（耋；｛：；） ｃ４・４，

对剪轴和Ｚ轴同理。式（４－１）的性能指标可写作

‰耻／ｏＴ（％１ｕ∽Ｔｕ（ｚ））出＝矿＋－１。；吕：，《伽咖舳％（４．５）
其中ｃｋ为七轴的多项式系数向量。

由于终止状态完全给定，由边界条件ｘ（Ｏ）＝Ｘｏ∈Ｒ６和ｘ（Ｔ）＝Ｘｌ∈Ⅱ妒可确定１２
维齐次线性方程组

㈥＝㈡Ｘｌ
当Ｔ＞０时线性无关，可将所有多项式系数ｃｋ用Ｔ表示，再带入式（３．１３）将其转换为关
㈨，

于Ｔ的标量函数了（ｃ七（Ｔ），Ｔ）＝歹（Ｔ），求取最优到达时间Ｔ＋的方法与第３．３．２节的情形

１相同，不同点为此时计算方程笃笋＝０的根时为求解一元二次方程。求得Ｔ・之后，
即可恢复得到最优多项式系数。

至此，给出完整边界条件，我们在考虑２阶模型约束和边值约束的情况下计算出以

多项式形式描述的最优轨迹边｛《（）ｃｏ，ｘ－），Ｔ‘（ｘｏ，ｘ・））。之后再检查求解时被忽略的速
度、加速度、障碍物等过程约束是否满足。若满足，则得到一条轨迹边，若不满足，则

・重新采样计算。

４．２．３基于近似拓扑引导的状态空问采样

许多前人的工作展示了改变采样分布对算法收敛率的提升作用。在实际环境中，考

虑到最优轨迹的速度一般沿着可行路径生长的方向，Ｂｌｏｃｈｌｉｇｅｒ等人【２４１和Ｏｌｅｙｎｉｋｏｖａ等

人【矧在环境中提取可行路径的中轴骨架，再在骨架周围采样。但是精确的提取中轴骨

架是一个较为耗费计算资源的事情，尤其是当环境中障碍物拓扑变得复杂，规划空间变

大时，骨架路径无法实时提取。而按照概率进行采样其实不需要非常精确且无碰撞的可

行骨架路径，只需提取出简要的拓扑路径即可，因此我们采用近似拓扑图引导采样。

近似拓扑图的生成如图４．Ｉ所示。首先根据上一章的ＴＰＢＶＰ解析求解一条连接规划

的起始和终止状态的轨迹，此时忽略各类约束，得到一条无约束轨迹（红色曲线）。之后

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

图４．１近似拓扑骨架构建的２维示意图

沿着这条轨迹，检查其和环境障碍物的碰撞，所使用的环境表示为占据栅格图，因此可

以得到穿进和穿出障碍物的点，记为只。和‰。，连接每一组只。和‰。得到横穿线段
（蓝色虚线段）。接着从每一组横穿线段的中点出发，在垂直横穿线段方向和平行水平面

方向做直线追踪（桔黄色虚线段），直到找到没有被障碍物占据的点，作为图顶点，记

为Ｍ。规划起始位置点和规划终止位置点也是图的顶点，按顺序连接规划起始位置点、

Ｋ、和规划终止位置点即构成近似拓扑图的有向边（橘黄色实箭头线段）。不像其他一

些在无障碍无区域精确得到拓扑图的方法，本文的方法使用了极少的计算代价得到了部

分的拓扑信息，作为精确度的牺牲，近似拓扑图的边可能会穿过障碍物，然而我们按概

率在图的边的周围采样状态的位置（一个采样点的状态由三维的位置和速度），尽量避

免了这一部分的影响。至于采样状态的速度向量，其方向以一定的高斯分布偏离其最近

图边的方向（绿色扇形区域），其大小以最大允许速度幅值为界均匀随机采样。３维环境

中的近似拓扑图与引导采样如图４．２所示，绿点表示随机采样点的位置状态，红线段表

示速度状态，橙色线段为近似拓扑图的边，红色曲线为无约束轨迹，深蓝色曲线为前端

规划结果，浅蓝色曲线为后端规划结果。与Ｒ６ｓｍａｎｎ等人【４】工作类似，此种近似拓扑图

的构建适用与非凸障碍物较少的环境，在实际自主导航时，由于感知距离一般有限，精

确地图范围较小，且我们把未知区域当做可行区域，因此按近似拓扑图引导采样可行且

高效，将在第４．４．１节进行实验验证。

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

图４．２近似拓扑骨架构建与状态采样的３维示意图

４．３轻量级商效后端轨迹提优

如上节所述，前端规划得到的轨迹满足了初末状态约束、运动学及动力学模型约束

和避障约束，然而由于即时性，其采样可能不够充分，存在提优空间。在本节，我们展

示如何结合前端轨迹的路径拓扑信息来高效地提升轨迹的平滑性和连续性。

探究前端的轨迹，我们发现首先由于在规划时考虑了初末状态、动力学模型以及避

障的约束，其几何位置以及处于一个相对合理的几何路径拓扑中。如一些工作【５３’９６】所

已经展示的，从这条初始轨迹出发，在其邻近的解空间中能找到一个质量更优的解。其

次，此前端轨迹基于加速度作为输入的线性模型，由上节可知，在三维空间中的任一维

度，其由分段三次多项式表征，具有整体Ｃｏ以及Ｃ１连续性但仅为每段之内Ｇ２连续，

即加速度在每相连的两段轨迹之间存在突变。此突变会造成飞行器期望姿态的突变和抖

动，有害于飞行器的控制。因此我们希望在后端优化中尽量减小这一突变，使得提优后

的轨迹更加平滑与连续。如图４．３所示，蓝色细线为前端轨迹的加速度示意，橙色细线为

经过后端优化后的加速度示意。

４．３．１问题描述

对每一维度，考虑共含ｍ段，每段为礼次的多项式轨迹，第ｉ段表示为ｐｉ（ｔ）＝

ｃ丁ｔ，ｔ∈【０，列，其中ｑ＝［Ｃｏ，ｃ１，Ｃ２，…，％］Ｔ∈Ｒ叶１为其第ｉ段的系数，ｔ＝【１，ｔ，ｔ２，．．．，卅Ｔ
为时间基底，正为该段时长。我们的目标为以前端轨迹得到的多项式系数作为初值，保

持每段时长不变，尽量高效的找到更优的多项式系数。

万方数据
浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

基于前述观察分析，我们令后端优化的目标函数Ｊ由三部分组成，将后端优化问题

构建如下：

ｔ。，是器ｋ）Ｊ＝九以＋Ａ＾厶＋Ａｃ以
（４—７）
ｓ～ｔ ｘ（￡）∈疋，一，ｕ（ｔ）∈“，ｒｅｅ，

Ａ。∈Ⅱ≈＋，Ａ＾∈Ⅱ≈＋，Ａ。∈Ⅱ＆＋，Ａ。＋Ａ＾＋Ａ。＝１，

其中ｅＴ＝［ｃｊ，ｃｊ，…，ｃ黝为ｍ段多项式的系数向量，以为整体平滑性的代价，以为惩
罚与前端轨迹路径处于不同集合路径拓扑的代价，以为惩罚段间加速度不连续性的代

价，九＞０，ｈ＞０，Ａ。＞０为各惩罚项的相应权重。优化变量为｛ｃ，九，ｈ，Ａ。）。
此构建中，Ｊｈ项非常关键，如图４．４ａ所示，相比于前人工作【２，５５，５７】对障碍物碰撞约

束的考量，我们通过在优化目标中加上此项，处理优化轨迹时就从将其推离障碍物转换

成了将其吸引至无碰撞的可行解，这避免了需要耗费巨大计算量的距离场（ＥＳＤＦ）计

算。另外，此项将潜在的解空间范围极大地缩小于初始可行解的周围，如图４．４ｂ所示，

对于障碍物约束造成的强非凸优化，不同路径拓扑下的解可理解为分布于解空间中的不

同极小值周围，图４．４ａ中不同路径拓扑下的三种不同颜色的路径对应图４．４ｂ解空间中三

个不同“坑”中的点。

对于式（４．７）所描述问题的求解，由于实时导航对求解效率的需求，我们不求获得最

优的．［ｃ，九，Ａ＾，Ａ。）以使得优化目标最小化，转而期望尽快求得一个尽量好可行解。我
们构建一个双层优化（Ｂｉ．１ｅｖｅｌ Ｏｐｔｉｍｉｚａｉｏｎ）求解方法，内层求解优化变量ｃ，外层求解

｛九，ｈ，入。）。我们知道，一个无约束的二次优化问题存在闭式解（Ｃｌｏｓｅｄ－ｆｏｒｍ Ｓｏｌｕｔｉｏｎ）。
那么对于式（４－７），当优化目标是二次形时，若给定各项系数｛九，ｈ，九）且暂时忽略不
等式约束，则多项式的系数向量ｃ能够闭式求解。以下将叙述如何构建优化目标为二次

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

型，如何推导双层优化内层对Ｃ的最优闭式解，以及如何使用双层优化来求解一个可行

且较优的｛Ａ。，Ａｈ，Ａ。）权重组合。

（ａ）障碍物考虑方法示意图 （ｂ）解空间中不同路径拓扑的解示意图

图４．４路径拓扑惩罚项示意图

４．３．２二次型优化目标构建

１．平滑惩罚代价以构建为待优化轨迹中每段多项式高阶导数的积分和：

五＝∑ｆ彬（ｔ）ｍ
七∈｛ｚ，掣，＝）。”

＝∑∑ｆ嘲（￡）】２ｄｔ
ｋ∈｛ｏ，Ｙ，：）ｉ＝１。”

＝∑∑ＣＴ啦／ｔ。）（㈣Ｔ舭啦
“
（４．８）
ｋｅ｛ｚ，Ｙ，ｚ）ｉ＝１

＝∑∑ｃ纛Ｑ。Ｊ Ｃｉ，ｔ

＝Ｆ ＣＴＱ＾

其中Ｔ＝丑＋乃＋…＋％为轨迹总时长，互为各段时长，ｔ（Ｊ）＝ｄＪ［１，ｔ，ｔ２，…，ｔ“】Ｔ／ｄ∥
为ｔ＝【１，ｔ，ｔ２，…，ｔ叮７的Ｊ次导数向量。Ｑ叫＝后‘ｔｕ’（ｔ（Ｊ’）Ｔｄｔ为佗＋１维的对称方阵，
以及Ｑ。为（ｎ＋１）×ｍ维的块对角矩阵。由于前端轨迹基于加速度输入模型，为分段三

次多项式，则此处选取礼＝３，Ｊ＝２。

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

２．路径拓扑惩罚代价＾构建为待优化轨迹与前端轨迹之间路径距离的积分和：

以＝∑／眺（￡）一菇（ｚ）】２ｄｔ
” ，ｔ‘

＝∑∑厂慨，七（ｔ）一ｍ（ｔ）】２出
Ｊ ０
奄∈｛￡，簟，ｚ）ｔ＝＝１

＝∑∑（ｃ讪一ｃ扩／’ｔｔＴ班（ｃ讪一ｃ靠）（４－９）

＝∑∑（ｃ啦一《。）Ｔ吼，ｔ（ｃ讪一ｃ瓢）
ｋＥ｛￥，ｐ，：）ｉ＝ｌ

＝∑（ｃ—ｃ４）ＴＱ＾（ｃ—ｃ‘），
屉∈｛霉，鼽＝）

其中矿（￡）为前端轨迹的位置。ｃ＋为其ｍ段系数。Ｑ“＝／ｏ‘ｔｔＴｄｔ为ｎ＋ｌ维的对称方
阵，以及ｑｈ为何＋１）Ｘ ｍ维块对角矩阵。

此项将迫使待优化的轨迹在位置上靠近前端无碰撞的轨迹，因此更可能在同一个路

径拓扑下的无障碍物区域找到可行解。

３．连续性惩罚代价以使得待优化的轨迹整体趋近于俨连续，其构建如下：

＝∑∑‰ｔ（２）Ｉ目；一略坫ｔ（２）Ｉｔ－ｏ】２（４－１０）
ｋｅ｛ｘ，ｙ声｝ｉ＝－１

＝∑ｃＴｑｃｃ，
ｋ６｛ｘ，Ⅻ声）

其中热，ｋ（ｈ）为在ｋ维第砂段末尾的加速度，藏＋，，％（ｏ）为第Ｏ＋１）地段起始的加速度。
Ｑｃ为对称带状矩阵。

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

将上述惩罚项整合在一起得到整体的二次型优化目标如下：

．ｒａｉｎ．Ｊ＝九五＋ｈ＾＋ｋ五
｛ｃ，Ａ，，Ａｈ，ｋ）

＝∑【九ｃＴＱｃ＋ｈ（ｃ—ｃ＋）ＴＱ＾ｃ—ｃ＋）＋Ａ。ｃＺＱ。ｃ】
七∈｛ｚ，掣，：）

＝
∑［ｃＴ（Ａ，Ｑ＋ｈＱｈ＋Ａ。Ｑ。）ｃ一２ＡｈｃＴＱｈｃ’＋Ａｈ（Ｃ’）ＴＱｈｃ’】（４－１ １）
ｋ∈｛￡，掣，ｚ）

ｓ．ｔ．Ａｃ＝ｄ，ｘ（ｔ）∈疋，”８，ｕ（ｔ）∈“，”８，

Ａ８∈Ｒ＋，Ａ＾∈Ｒ＋，Ａｃ∈ⅡＥ＋，Ａｓ－Ｉ－Ａａ＋Ａｃ＝１，

其中Ａｃ＝ｄ为每段的边界约束，即始末状态约束以及段间高阶导数的连续性约束，此

处为位置和速度连续。具体构建可参考Ｂｒｙ等人［２１文献。每一维的代价函数相互独立可

解耦分别求解。

４．３．３内层优化的闭式最优解构造

内层优化时只优化多项式系数ｃ，将权重系数｛九，ｈ，ｋ）当做常量。
如Ｂｒｙ等人在【２】中所述，分段多项式轨迹除了由每段的各项系数表征之外，还可由

每段的边界高阶导数表征，转换如下：

ｃ：Ｋ Ｈ，Ｋ：Ａ－１ｃ，（４－１２）
ｌ山Ｉ
其中矩阵Ｋ将多项式系数向量ｃ映设至边界导数向量［ｄＪ《１Ｔ，ｄ／为边界导数向量中
已知的部分，也为边界导数向量中未知的部分，即真正的参与求解的优化变量，・Ｃ矩阵

的构成可见Ｂｒｙ等人的工作【２】。

以ｄｐ为优化变量，式（４．１１）中的等式约束可被消去，内层优化构建为无约束的二次

规划问题，描述如下：

Ⅱ嘻ｎ
Ｊ＝匠］ＴＲ巴］一２入ｈ匕］。ｚ＋入＾ｃｃ＋，ＴＱｈｃ＋，。４．。３，
Ｒ＝ＫＴ（九Ｑ＋ＡｈＱ＾＋入。Ｑ。）Ｋ， ｚ＝ＫＴＱ＾Ｃ’．

略去ｔ，中不影响求解最优性的常数项，ｔ，对ｄ口的雅各比矩阵计算如下：

甏＝２％ｄ，＋２％ｄｐ一２Ａ母 （４．１４）

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

其中毗。和Ｚ。为Ｒ和Ｚ中对应维数的块矩阵。令式（４．１４）等于０，可解析求得优化变
量ｄ。如下：

ｄｐ＝啄（入ｈＺｐ—Ｉｋｌｄｌ）． （４－１５）

得到ｄ。之后，将其带入式（４—１２）可求得ｃ。

至此，若给定一组权重系数｛九，ｈ，Ａ。】．，我们便能够解析地求得基于此权重比的待
优化轨迹的最优系数，此为一轮内层优化求解。

４．３．４双层优化求解流程

由于在求解过程中略去了不等式约束，即没有考虑轨迹是否与障碍物碰撞与高阶

导数是否超限，在求解之后我们需要检查这些约束，若约束满足，则得到了问题４．１３的

最优解，也即问题４一ＩＩ在基于特定权重数值｛九，Ａ＾，入。】．组合下的一个可行解。通过调整
各项权重系数的比例，我们能够调整待优化轨迹的平滑性、连续性以及与障碍物的距离

等，调整各项权重比例的过程即为双层优化的外层优化过程。

调整各项权重的思路为：由于｛九，Ａ，Ｉ，ｋ）之和约束为常量，我们九占比尽量大以
获得更加平滑的轨迹，同样希望Ａ。占比尽量大以获得段间更加连续的轨迹，但此两项

增加造成的ｈ占比减小会使得待优化轨迹越远离初始的安全轨迹，更有可能发生碰撞。

因此，我们首先确定Ａ＾的占比，在可行的范围内得到一个尽量小的值，从而留给其他

两项更大的调节裕度，接着，将其固定，我们再确定Ａ。的占比，在可行的范围内得到

一个尽量大的值以使段间加速度尽可能连续，从而迫近整体Ｃ２连续。

双层优化流程如算法５所示，我们记ｎ＝ｈ／（Ｘ。＋ｈ＋九）表示路径拓扑惩罚项代

价占整体的比重，记ｒｃ＝九／（九＋Ａ。）表示惩罚段间不连续项代价占其与平滑项代价
之和的比重。令％的初始值ｒ。．伽ｎ接近于１以期望待优化轨迹在位置上更加贴合初始

的安全轨迹，具有更大可能性保持安全。令ｒｃ的初始值ｒｃ加ｉｔ接近于０。在第一个循环

中，每次迭代时ｒ。保持不变而不断减小ｎ，此过程中优化的轨迹会愈发偏离初始轨迹，

直到求得的轨迹发生障碍物，经过第一轮循环我们得到了尽可能小的路径拓惩罚扑项

占比，留下了尽量大的裕度来使轨迹更加平滑和段问连续。接着进行第二轮循环，保持

上一轮得到的％保持不变而不断增加ｒｃ，此过程中优化的轨迹的段间会愈发连续，直

到求得的轨迹不满足约束，经过第二轮循环我们即得到了更加平滑与连续的轨迹。在每

个循环内层的ＣｌｏｓｅｄＦｏｒｍＳｏｌｖｅ０函数即为使用式（４－１５）－与式（４－１２）解析求得轨迹系数。

万方数据
 浙江大学博士专业学位论文４层次化实时局部动力学运动规划

ＣｈｅｃｋＦｅａｓｉｂｌｅ０函数检查内层优化得到的临时轨迹是否发生碰撞或其他高阶导是否违

反约束。我们的动力学前端提供了较好的每段多项式的时间分配，因此即使优化中时间

分配不变，高阶导的约束仍然容易满足。

由此，经过双层优化求解，我们对前端轨迹进行了高效地提优，在其解空间周围找

到了更加平滑与连续的轨迹，同时满足安全性。

算法５：双层优化算法
Ｉｎｐｕｔ：障碍物环境Ｅ，初始轨迹死。ｎ

Ｏｕｔｐｕｔ：优化轨迹７ｒ

ｌ
ｒｈ÷－ｒｈ．ｉｎｉｔ，ｒｅ÷－ｒｃ，ｉｎｉｔ，７ｆｌｅａ÷－７ｒｉｎｉｔ

＾第一轮外层循环，确定ｒ＾ ・／

２ ｗｈｉｌｅ ｒｈ＞０ ｄｏ

３ 几肌ｐ＋－ＣｉｏｓｅｄＦｏｒｍＳｏｌｖｅ（ｒｈ，ｒｃ）；
４ ｉｆ ＣｈｅｃｋＦｅａｓｉｂｌｅ（丌ｔｅｍｐ，Ｅ）ｙａｉｔｓ ｔｈｅｎ

Ｌ：。
５

肛第二轮外层循环，确定ｒｃ

９ ｗｈｉｌｅ ｒｓ＞０ ｄｏ

ｍ 丌ｔｅｔ印＋－ＣｌｏｓｅｄＦｏｒｍＳｏｌｖｅ（ｒｈ，ｒｃ）；
ｎ ｉｆ ＣｈｅｃｋＦｅａｓｉｂｌｅ（丌ｔｅ，，Ｉｐ，Ｅ）ｆａｉｌｓ ｔｈｅｎ

Ｌ：晶
ｎ

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

４．４与相关算法的数值对比测试

所有的数值仿真测试在一台搭载２．２ＧＨｚ英特尔ｉ７—４７０２ＨＱ处理器、１６Ｇ内存的计

算机中进行。

４．４．１采样策略对比测试

我们将所提出的近似拓扑引导采样策略与典型的均匀采样策略进行对比。所测试

的模拟环境如图４．６所示。采样规划时间限制为１０秒，并且以所提出的采样方法所得

结果的轨迹代价作为基准，两种策略的最优轨迹代价比值随着求解时间的变化趋势如

图４．５所示。

观察图４．５中呈下降趋势的两条蓝色曲线，可以发现若使用所提出的近似拓扑引导

采样策略，几毫秒内便找到初始可行解，之后轨迹代价迅速下降。与之对比，若是在状

态空间均匀采样，则得到初始可行解需花费数百毫秒，且之后轨迹代价缓慢收敛。观察

呈上升趋势的黄色曲线，可以发现给定同样的时间，采取近似拓扑引导采样时由于更加

合理的状态分布，轨迹树中节点的数量更多。从图４．６中可以对比看到引导采样经过３ｍｓ

的时间预算所得的轨迹树（绿色曲线）生长地更顺滑，而均匀采样经过ｌＯＯｍｓ的时间所

生长的轨迹树依旧非常杂乱，引导采样所得轨迹（橙色）也相较均匀采样所得轨迹（蓝

色）更合理。

１．２５
Ｆｉ一勿疏雨矿ｔ酉莉画君碌薜了……ｊ ３ ５

｝＊轨迹代价（拓扑引导采样） ３ 趁
１．２ ｝—ｅ－树中节点数量（均匀随机采样） 丹
Ｉ蔓—趣睦蔓樾量』堑 ２ ５ 碰
孙

屡
１．１５
｛¨二≥／ ３００

２００
２ 潜
呻

萎ｕ １００

０
１ Ｓ ＾
立
￡ １ 宝
３
１．０５
０ ５

１啊 ———————－———＿Ｊ ０

０．００ ４．００ ６．００ ８．００ １０。００

时间（Ｓ）

图４．５近似拓扑射导采样策略与均匀随机采样策略效率对比示意图

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

（ａ）近似拓扑引导此采样 （ｂ）均匀随机采样

图４．６不同采样策略下轨迹树生长对比示意图

４．４．２轨迹规划系统对比测试

对于轨迹规划系统的数值测试，我们从三个方面与其他当前最为先进的相关算法进

行对比：前端动力学运动规划、后端轨迹优化和前后端集成测试。速度与加速度约束分

别设置为５ｍ／ｓ和６ｍ／ｓ２。
１）前端动力学运动规划对比

对于前端，我们将所提出方法找到的第一个解与Ｚｈｏｕ等人【５７】基于图搜索的方法结

果进行比较。其将加速度控制输入和时间离散化，基于相同的二阶线性模型前向积分得

到图节点，以求解两点边值问题设计启发式函数进行图搜索。我们进行１０００次规划，随

机选取间隔１０—１５ｍ的起终点，求解的均值记录在表４．１中。从表中可以看到，花费近

似的计算时间，我们的方法得到的轨迹平均代价更小，长度更短，段数更少，并且成功

率更高。我们的方法使用了两点边值问题的求解结果，轨迹每段内的控制输入线性变化

而非像Ｚｈｏｕ等人【５７】方法中的为常量，因此整体更加平滑。我们的方法仅仅平均多花费

了３．９３％的计算时间（仍控制在５ｍｓ以内，满足实时求解），极大提升了轨迹的质量，

为后端优化提供了更好的初始轨迹从而大大降低了后端所需要的求解时间。

表４．１前端动力学运动规划比较结果（平均值）

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

４．４．３后靖提优算法对比测试

对于后端优化，我们将所提出方法与Ｚｈｏｕ等人的方法［５７１、Ｒｉｃｈｔｅｒ等人的方法【２ｌ、

Ｍｅｌｌｉｎｇｅｒ等人的方法【５９】以及Ｔｏｒｄｅｓｉｌｌａｓ等人的方法【９５】进行对比。其优化目标中都包含

使控制输入的积分极小化。Ｚｈｏｕ等人【５７】采用Ｂ样条来表征轨迹，优化样条上的控制点，

其额外使用梯度距离场（ＥｓＤＦ）来将控制点推离障碍物以保证安全，求解则使用开源

的非线性优化库ＮＬｏｐｔ［１１４１。Ｒｉｃｈｔｅｒ等人【２１使用分段多项式表征轨迹，其同样通过无约

束二次规划（ＱＰ）来优化中间路径点的高阶导数，每段轨迹的时长则通过梯度下降进

行优化调整，求解使用ＮＬｏｐｔ。Ｍｅｌｌｉｎｇｅｒ等人［５９】同样使用分段多项式表征轨迹，其构

建约束二次规划问题来优化多项式系数，保持轨迹总时长固定不变，使用梯度下降优化

每段的时间分配，求解使用ＮＬｏｐｔ。Ｔｏｒｄｅｓｉｌｌａｓ等人【９５】使用贝塞尔曲线（Ｂ６ｚｉｅｒＣｕｒｖｅ）

表征轨迹，额外构建无障碍物的安全走廊，使用混合整数规划（ＭＩＱＰ）将轨迹严格约

束在内以保证安全，求解则使用商用的Ｇｕｒｏｂｉ优化器［１１５】。

图４．７测试环境以及不同后靖优化方法产生的轨迹

为了公平对比，除了Ｔｏｒｄｅｓｉｌｌａｓ等人使用Ｉｎｆｏｒｍｅｄ－ＲＲＴ＊算法得到初值以获得安全

走廊以外，对其他所比较的方法，我们使用同一条前端轨迹作为初值，由所提出的前端

规划得到。对于Ｒｉｃｈｔｅｒ等人和Ｍｅｌｌｉｎｇｅｒ等人的基于优化分段多项式的方法，我们比较

了以梯形公式求初始时间分配和以前端轨迹给出的时间分配，分别加上后缀记作Ｔ和

Ｋ。当优化过程中某一段中出现碰撞时，我们从初始轨迹的相应段上取出中间时刻的路

径点，添加至待优化轨迹中，即增加多项式轨迹段数，重新进行一次优化。此过程不断

进行直到整条轨迹都无碰撞。每一轮优化求解的停止条件设置为５ ｍｓ求解时间。测试

环境和轨迹求解结果如图４．７所示。

万方数据
 浙江大学博士专业学位论文４层次化实时局部动力学运动规划

数值对比结果如表４．２中所列，可以发现本章所提出方法花费更短的时间产生代价

更小更加平滑的轨迹，这是由于其他的方法均将问题构建为强约束的非凸规划，使用通

用的非线性优化求解器，利用梯度下降法迭代求解，收敛缓慢，计算量大。而本章提出

的后端优化则将问题构建为无约束的序列二次优化，使用双层优化求解，利用了凸性使

得内层求解都有闭式解而无需计算梯度信息进行数值迭代。Ｚｈｏｕ等人的方法使用梯度

距离场检查轨迹是否碰撞，需要额外的计算时间，本例约为５０ ｆｆｌ，８。Ｔｏｒｄｅｓｉｌｌａｓ等人的方

法建立安全走廊，同样需要额外的计算时间，本例约为１００ｆｒ？，８。Ｒｉｃｈｔｅｒ等人和Ｍｅｌｌｉｎｇｅｒ

等人的方法在优化中不考虑障碍物信息因此需要多次迭代来生成一条无碰撞的轨迹。我

们的方法则将高质量的前端轨迹整合进优化目标中，从而缓和了上述问题带来的影响。

另外，我们注意到本章提出的后端优化方法虽然没有优化轨迹时问，但求得的轨迹时长

依然小于使用梯形时ｆ，１分配作为初值的Ｒｉｃｈｔｅｒ等人和Ｍｅｌｌｉｎｇｅｒ等人的方法，并且和与

使用前端轨迹时间作为初值的结果差距甚徽。Ｔｏｒｄｅｓｉｌｌａｓ等人方法的求解耗费数百毫秒，

完全无法满足高速飞行中实时求解的需求。

４．４．４前后靖集成对比测试

对于集成对比测试，我们与Ｚｈｏｕ等人的方法【５７１进行比较。在４０ｍ×４０ｍ×３ｍ

的环境中，我们在不同距离的起终点（１０—１５ ｍ和２０—２５ ｍ）和不同障碍物密度

（１００，２００，３００个障碍物）的情况下分别对比，数值结果如图４．８所示。从图４．８ｂ中可以着

６９

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

出，作为一个整体规划系统，我们的系统方案在每一个场景中均产生代价更小即更加平

滑的轨迹。从图４．８ａ中我们发现，在规划距离较近时（１０—１５ ｍ），我们的方法具备优

势。对于现实世界的应用，一个轻量级飞行器的精确感知距离往往小于１０ ｍ，甚至小

于５ ｍ，因此局部规划的范围往往较短，由下节实物飞行测试可以验证。

融 蛇

Ｏ Ｄ玷

ｎ毗
—ｏ厘茁琏锚
Ｏ Ｄ：宝

（ａ）求解时间及占比（ｍｓ）
雹
４０
毡
、一一

￥ ３０

￡
隶
鼷
嘲
器
鞘

４．５真机未知环境自主导航试验

我们在室内和室外的未知复杂环境下分别进行自主导航测试，相关实验视频可在链

接１中找到。

４．５．１实验设定

我们所使用的测试平台为一架自组装的四旋翼飞行器，如图４．９所示，其使用大疆

Ｎ３飞行控制器２，搭载一个前视的ＲｅａｌＳｅｎｓｅ Ｄ４３５ｉ双目深度相机３用于环境感知和自身

‘ｈｔｔｐｓ：／／ｗｗｗ．ｊｉａｎｇｕｏｙｕｎ．ｃｏｍ／ｐ／Ｄｃ＿ｗｇｆＵＱＩＮ６ｕＣｈｉ７ｊ７ｋＥＩＡＡ

２ｈｔｔｐｓ：／／ｗｗｗ．ｄｊｉ．ｃｏｍ／ｃｎ／ｎ３
３ｈｔｔｐｓ：／／ｗｗｗ．ｉｎｔｅｌｒｅａｌｓｅｎｓｃ．ｃｏｒｎ／ｄｅｐｔｈ－ｃａｍｅｒａ－ｄ４３５ｉ／

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

状态估计，相机的视场角为６０。。由于远处深度估计不准确，我们只取３ｍ内的深度估

计值，即精确的感知范围只有３ ｍ。碰撞检测则基于占据栅格图【¨郇，其通过融合自身位

姿所对应的深度图得到。未知区域被视作安全以实现快速飞行。每当观测到新的障碍物

或者设置新的目标点时进行触发式重规划。所有的计算在一台ＣＰＵ版大疆妙算２机载

电脑４上在线进行。

Ｏｉｌ Ｍａｎｉｆｏｌｄ２一Ｃ
机载计算机

ＤＩＩＮ３飞行接制器

Ｉｎｔｅｌ Ｒｅａｌｓｅｎｓｅ Ｄ４３５ｉ

双目深度相机

图４．９实物飞行所用四旋翼飞行器及机载设备

４．５．２路径点闻导航

室外和室内测试的终点分别设置在约５０ ｍ外和１５ ｍ外，令飞行器飞至终点再返

回起点，每当观测到新的障碍物时进行重规划。

室外测试的场景如图４．１０ａ所示，为事先未知的密集树林，其中四旋翼飞行器由红框

标出。最终执行轨迹如图４．１０ｂ，整体飞行的速度曲线如图４．１１，在实时感知下基本保持

了３ｍ／ｓ以上的飞行速度。
室内测试的场景如图４．１２ａ所示，障碍物尺寸相比室外时的树木更加巨大，对观测造

成更多的遮挡，对规划的实时性造成更大挑战。整体飞行的速度曲线如图４．１３，最终执

行轨迹如图４．１２ｂ，其中蓝色轨迹为回程轨迹，其相比去程的红色轨迹更加平滑，这是因

为回程时部分环境已在去程时观测过并记录下来，使得规划能够利用的环境信息更为丰

富。

４ｈｔｔｐｓ：／／ｗｗｗ．ｄｊｉ．ｃｏｍ／ｃｒｄｍａｎｉｆｏｌｄ－２

７１

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

（ａ）室外飞行测试场景

（ｂ）室外测试实际飞行轨迹

图４．１０室外测试场景以及实际飞行轨迹

ｏ
ｉ ３
一
越２
蝴
１

０
３０ 时间（Ｓ）

图４．１１室外测试整体飞行速度曲线

４．５．３快速重规划测试

为了进一步测试所提出规划算法的快速重规划性能，我们在室外飞行中不断改变目

标点让飞行器去追踪，当目标点改变或者新观测到障碍物时都进行重规划。在第一个测

试中，我们让飞行器追踪一个不断移动的二维码目标，如图４．１４所示，其中追逐目标由

淡蓝色框标出，四旋翼飞行器由红框标出。在第二个测试中，我们在飞行器飞行的过程

中任意随机地改变目标点。图４．１５展示了飞行器在躲避障碍物时按时间顺序观测到的地

图以及第一人称视角和第三人称视角图，左列为占据栅格地图，中间为第三人称视角，

右列为第一人称视角，序列时间间隔为０．２５ ｓ。

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

（ａ）飞行测试场景 （ｂ）实际飞行轨迹

图４．１２室内测试场景以及实际飞行轨迹

童３
Ｅ
一
型２
锻
１

图４．１３室内测试整体飞行速度曲线

４．６本章小结

本章主要探究未知环境导航中的局部实时重规划问题，使用采样类前端规划衔接高

效后端提优的层级式规划框架，每次重规划求解一条保证实时性且足够好的局部最优轨

迹。我们总结本章主要贡献如下：１．前端规划采用基于引导采样的动力学ＲＲＴ＊算法，

根据当前环境快速构建近似拓扑图，用其引导在状态空间的采样，极大提升了采样类动

力学运动规划算法的求解效率。并且由于考虑了飞行器的初始运动状态，我们得到一条

位于合理路径拓扑下且满足动力学、避障等约束的初值轨迹。２．提出基于双层优化求解

的轻量级后端提优算法，内层求解具备闭式解形式，有效利用前端轨迹，花费极少计算

代价提升了前端结果轨迹的光滑性和连续性。３．将所提出的规划框架与算法应用于一架

全自主四旋翼飞行器上进行导航飞行测试，在未知的室外稠密树林、遮挡严重的室内房

间等环境保持３ｍ／ｓ的高速飞行。

万方数据
 浙江大学博士专业学位论文 ４层次化实时局部动力学运动规划

图４．１４追踪移动目标飞行测试

图４．１５重规划避障序列图 ．一㈣霹…一 蒸一 霪一爹

７４

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

５结合快速局部优化和双向搜索的实时规划

５．１引言

在上一章中，我们探究了飞行器在未知环境下的导航问题，重点研究了局部实时运

动规划，我们使用层级式的前端一后端方案，其中前端使用单纯采样类动力学运动规划

方法，后端构建求解双层优化问题。

对于使用单纯采样类方法的前端，当环境中存在如窄缝等难以通过的空间时，因采

样空间的维度高、边界广，往往需要大量采样才能联通窄缝两端的空间，导致其需要花

费较长时间才能得到一条可行轨迹，从而有时无法满足行进中重规划的实时求解需求。

局部优化（ＲｅｇｉｏｎａｌＯｐｔｉｍｉｚａｔｉｏｎ，ＲＯ）则利用局部小范围空Ｍ的信患而专注于探索某

些限定范围的解空间，有助于探索难以靠单纯采样联通的区域。因此，在全局采样探索

的过程中结合局部优化己成为应对这类场景的主流方案［６７，６９，７０，１１７１，－ｌ ８１。从以往经验上看，

此类方案的总体效率取决于局部优化部分的求解效率。对于此，大多数工作构建非线性

优化问题并使用梯度下降类方法求解，而精确的梯度信息往往不难么容易计算得到，尤

其对于复杂的障碍物环境。本章考虑将ＲＯ构建为一系列迭代求解的无约束二次优化问

题，每次迭代都有闭式解，从而避免了计算繁杂的梯度信息，并且保证ＲＯ部分的求解

效率。另外，单向生长的轨迹树只有恰好连接上目标点时才算得到一条可行轨迹，当目

标状态点被障碍物“困住”时难以被连接到从而降低了求解成功率。同时构建另一棵由

目标点反向生长的轨迹树，尝试连接两棵树中的多个状态节点能够增加找到可行解的概

率。

对于后端提优，上一章的双层优化方法构建闭式求解的内层，虽然求解效率较高，

但在构建问题的过程中没有考虑周围的障碍物信患，在有限的安全空间内难以保证得到

无碰撞的可行解。本章对于后端提优问题的构建在一定程度上考虑了初始轨迹周围的障

碍物信息，增加了求解轨迹的安全性，而获取这些信患仅涉及迭代过程中对临时轨迹进

行碰撞检测，是原本就需要执行的操作，没有增加额外计算。同时我们保留了具有闭式

解的优点，保证了求解效率。

本章在上一章内容之上进行更深一步的探究，所提方法更好地满足飞行器自主导航

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

实时求解的需求。提出的改进主要在两方面：１．针对窄缝等难以靠单纯采样方法求解的

环境，我们在采样类的前端规划中结合双向搜索和快速求解的局部优化，极大减小了找

到第一个可行解所需的时间且提高了成功率。２．针对前端初值轨迹没有被充分利用的问

题，汲取前端快速局部优化中的思想，在后端提优问题的构造中考虑初值轨迹周围的障

碍物信息，同时保留有闭式解的二次型建模，使提优问题的求解更为高效且更容易满足

无碰撞障碍物约束。

本章内容安排如下：第５．２节描述了结合快速局部优化和双向搜索的采样规划前端

算法的具体流程，包括如何生长连接两棵双向轨迹树，以及如何求解轨迹边，其中所用

到的快速局部优化问题的构建与求解过程在第５．３节叙述。第５．４节描述了后端提优算法，

重点讲述了与上一章相关方面的不同点。之后第５．５节在数值仿真上将本章方法与相关

方法进行对比，验证了高效性，并在实物四旋翼飞行器上进行导航测试。最后在第５．６节

进行总结。

５．２结合快速局部优化和双向搜索的采样规划前靖

对于旋翼飞行器类漂移系统【ｌｏｏ］（Ｄｒｉｆｔ Ｓｙｓｔｅｍ），除了初始速度之外，初始加速度

对重规划初始状态的影响同样较大，因此我们此时考虑３阶线性模型，则与第３章一样，

以加加速度作为控制输入ｕ，状态向量Ｘ为９维向量。前端规划的问题描述与式（３－２）相

同，此处不再赘述。

５．２．１双向轨迹树生长

结合快速局部优化和双向搜索的采样规划流程如算法６、算法７所示。我们同时构建

两棵向对方生长的轨迹树，以重规划问题的初始状态和目标状态作为根节点，分别称作

正向树和反向树，以下介绍其中所涉及各函数的具体内容。

１．拓扑引导采样（算法６第４行）：按第４．２．３节所描述的使用近似拓扑引导采样。只

在３维的位置维度和３维的速度维度采样，加速度维度在采样时设为不定，将在后续连

接正向树求解ＴＰＢＶＰ时自动计算，之后在第５．２．２节详述。

２．近邻搜索（算法６第５、ｌｌ行）：同样，我们以固定的欧氏距离半径ｒ在不同轨迹

树的状态节点中分别搜索出距离‰。小于ｒ的节点。

７６

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

算法６：结合局部优化的丞鱼堡室挝蔓鎏
Ｉｎｐｕｔ：障碍物环境Ｅ，起始状态ｘ愀，目标状态Ｘｇｏ。ｄ
Ｏｕｔｐｕｔ：轨迹矿

１正向树中状态节点ｈ÷－（ｘ州），正向树中轨迹边白÷－０，正向轨迹树
乃＋＿（ｈ，白），
２反向树中状态节点Ｖｂ÷－｛ｘｔ砸。＞，反向树中轨迹边矗÷－仍，反向轨迹树

兀÷－（Ⅵ，邑），
３ ｗｈｉｌｅ终止条件未达到ｄｏ

● ｘ。。。＋一ＴｏｐｏＧｕｉｄｅｄＳａｍｐｌｉｎｇ（Ｅ）；

‘
ｉｆ｛砩，０）÷－ＣｈｏｏｓｅＰａｒｅｎｔ（Ｘ．一，‰一ｔｈｅｎ
７
Ｉ¨÷－＋｛）ｃｔｌ。），白÷－＋昂，乃十－（ｈ，白）；
８ ｌ连接上正向树；
９

ｎ ‰，÷－ＮｅａｒＮｅｉｇｈｂｏｒＳｅａｒｃｈ（７；，ｘ。。，ｒ）；
：： ｉｆ｛）ｃｐ，昂】．÷－ＣｈｏｏｓｅＰａｒｅｎ吖％。。Ｋ—ｔｈｅｎ
¨ ＩⅥ÷－＋｛ｈ。。｝，磊＋－＋昂，兀÷－（Ⅵ，磊）；
¨

：２

：宝

ｎ ｉｆ连接上正向树八连接上反向树ｔｈｅｎ

Ｌ戛蔗咖嘶尻‰）．
坞 Ｉ ｉｆ ｘｍ＂．ｇｖａｌｕｅ＿＿］ｏｒｗａｒｄ＋ｘｎｅｔｔ，．ｇ＿ｖａｌｕｅ＿ｂａｃｋｗａｒｄ＜７ｒ＊．ＣＯＳｔ ｔｈｅｎ

ｌ
坶

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

算法７：选择父节点
Ｉｎｐｕｔ：障碍物环境Ｅ，潜在父节点集合疋，待连接节点ｘ

Ｏｕｔｐｕｔ：父节点）（ｐ，连接到父节点的轨迹边昂
１砩÷－ｎｕｌｌ，ｍｉｎ—ｃｏｓｔ＋Ｉ ｏｏ；

２ ｆｏｒｘｍｉｎ饥名ｄｏ

３ ｉｆ父节点在正向树中ｔｈｅｎ

●
Ｌ￡÷－（‰。，ｘ）；
５ ｉｆ父节点在反向树中ｔｈｅｎ

‘
Ｌ￡÷－ｘ，ｘｍ‘ｎ）；
７ ｉｆ ｃｏｌｌｉｓｉｏｎｓｅ－ＣｈｅｃｋＦｅａｓｉｂｌｅ（￡，Ｅ）人ｘ硝ｎ．ｇ＿ｖａｌｕｅ＋￡．ｃｏｓｔ＜ｍｉｎ—ｃｏｓｔ ｔｈｅｎ
８
ｂ÷．ｘ厕ｎ，ｍｉｎ—ｃｏｓｔ÷－ｘｍ讯．ｇ＿ｖａｌｕｅ＋￡．ｃｏｓｔ，昂÷－￡；

¨ ｌ￡÷－ＲｅｇｉｏｎａｉＯｐｔｉｍｉｚｅ（ｏｅ，Ｅ）；
ｎ ｉｆ局部优化成功八ｘ。沁ｇ＿ｖａｌｕｅ＋￡．ｃｏｓｔ＜ｒａｉｎ—ｃｏｓｔ ｔｈｅｎ

ｎ
ｌ Ｌ ｂ＋－ｘ戚。，ｍｔ礼－印武＋－）ｃ，，咖．＂ｄｚⅡｅ＋￡．ｃｏｓｔ，ｏ＋－￡；

３．选择父节点（算法６第６、１２行）：此步骤如算法７所示，从潜在父节点的集合麓。，
中寻找最合适的父节点。当连接正向树时，树中节点作为求解ＴＰＢＶ＇Ｐ的初始状态，采
样节点作为终止状态，采样节点中不定的加速度维度通过求解ＴＰＢＶＰ确定。当连接反

向树时，采样节点作为初始状态，树中节点作为终止状态，若之前已连接上正向树，则

采样节点的加速度维度由之前的ＴＰＢＶ＇Ｐ求得，否则此时令加速度状态置零。若求得的

轨迹满足约束且累积轨迹代价为所有潜在父节点中最小的，则选其为父节点。若此轨迹

与障碍物发生碰撞且碰撞范围小于一定闽值，我们不是如上章简单的丢弃此轨迹，而是

对其进行局部优化，具体在第５．３节介绍，若局部优化成功且累积轨迹代价最小，则选其

为父节点。

４．重连接（算法６第ｌＯ、１６行）：当新节点被添加到正向轨迹树或反向轨迹树中后，

我们对新添加状态节点的近邻节点进行重连接。对于正向树的重连接，新节点作为求解

ＴＰＢＶＰ的初始状态，树中节点作为终止状态；对于反向树的重连接，树中节点作为初

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

始状态，新节点作为终止状态。

５＋获得当前最优解（算法６第１９行）：若某一节点同时连接上正向轨迹树与反向轨迹

树，则说明找到一条连接重规划初始状态与目标状态的可行轨迹。若该节点在两棵树中

的累积代价之和小于当前已有最优轨迹的代价，则说明找到了一条更优的轨迹，此时按

该节点在两棵树中的父指针分别回溯至两棵树的根节点即得到该轨迹。

５．２．２求解轨迹边的两点边值问题

Ｇ【ｘｃＴ，，卅＝（二｛：；：主ｔ３二：）＝。， ｃ５－－，

（５．２）
入（砷＝—ＯＧｌ［ｘ聂（Ｔ万）丁，Ｔ一］ｖｕ＝（口ｌ，忱，地，Ｕ４，地，‰，。，。，。）Ｔ∈时，

斟㈢， 仔３，

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

５．３快速局部优化

局部优化在生长轨迹树的每一轮迭代中可能会被频繁调用，因此我们希望尽可能快

的求解。通过分析求解１１ＰＢＶＰ得到的发生碰撞的轨迹边‰ｌ池，其有着较为合理的轨
迹时长，且我们已对其进行碰撞检测，知晓何处与障碍物发生碰撞，希望以较小代价将

碰撞的部分形变至周围的安全空间。上一章后端优化部分的二次型构建因有闭式解而求

解效率较高，在局部优化时我们同样利用此种构建形式。遵循轨迹边的多项式参数化形

式，我们将如ｌ洫接轨迹时长均匀分割成Ｊ段５次多项式（算法８第２行），给优化引入
更多的自由度。

５．３．１考虑障碍物的二次型性能指标构建

对于｛ｚ，Ｙ，２】Ｉ任一维度，二次型性能指标由三项组成。
１．平滑惩罚代价以构建为待优化轨迹中每段多项式加加速度的积分和：

以＝ｆ帅）１２出 ‘５４’

：妻ｃｊ Ｚ霸ｔｃｓ，ｃｔｔｓ，，Ｔｄｔ ｃ；：ｃＴＱ，ｃ，

其中Ｔ＝丑＋乃＋…＋乃轨迹边的总时长，正为第｛段的时长，ｃＴ＝【ｃ丁，霹，…，ｃ习
为待优化的Ｊ段多项式的总系数向量。

２．蓐径拓扑惩罚代价＾构建为待优化轨迹边与之前碰撞的轨迹边‰址上位置点
ｆ（ｔ）之间路径距离的积分和，与式（４．９）相同：

＾＝／由（ｔ）一矿（ｔ）】２ｄｔ
‘５＿５’

：妾ｃｃ。一ｅ，Ｔ／正ｔｔＴｄｚ ｃｃ；一《，：ｃｃ—ｃ・，ＴＱ＾ｃｃ—ｃ・，．

其中ｃ汀＝【ｃ’丁，矿ｊ，…，ｃ譬］为原轨迹边分割后的Ｊ段多项式的总系数向量。此项将待
优化轨迹边在位置上靠近原轨迹边，从而使得大部分不碰撞的部分保留在原安全区域附

近。

３．障碍物碰擅惩罚代价五的构建与上一项类似，将上式的ｆ（ｔ）换成常量位置点
‰（ｔ），提供“拉力”对待优化轨迹进行形交（关于“拉力”的描述在第５．３．３介绍）。此项

８０

万方数据
 塑望盔兰坚主童些兰垡堡塞 ！丝鱼堡垄星塑垡垡塑翌塑垫室塑塞堕壑型

较为关键，因其隐式地提供了障碍物信息并将待优化轨迹的碰撞部分吸引至安全区域，

构建如下：

以＝∑／一囟（￡）一ｐ印（ｔ）】２疵
ａｐＥＡＰｓ。ｂ，４ｐ

＝∑∑／…眈（ｔ）咱，印（ｔ）】２ｄｔ
叩∈＾Ｐ。ｉ５Ｌ“咖９ ． （５—６）

＝∑∑（ｃｉ—ｃ罗）Ｔ／（４‘“”ｔｔＴｄ￡（ｃ；一ｃ，）
＝＞．二（ｃ—ｃ叩）ＴＱ。，印（ｃ—ｃ印），
函ｐｓ
其中ＡＰｓ为吸引点（ＡｔｔｒａｃｔｉｎｇＰｏｉｎｔｓ，ＡＰｓ）的集合，ｐ。ｐ（ｔ）为其中一个吸引点的位置，

为常量，（￡。。印．ｔ。，印）互［ｏ，卅为该吸引点所影响的总体轨迹时间，Ｌ为该吸引点所影响

的轨迹段的集合，（ｔ“仰一ｔ。，印ｐ）∈【ｏ，列为对应受影响轨迹段ｉ上受“拉力”影响的时
间段。

整体性能指标如。构建为上３项指标的加权和，如下：

如ｏ＝ｋ以＋ｈ工十九以＝ｃＴＱｃ＋ｃＴＭ＋Ｎ， （５—７ａ）

Ｑ＝Ａ。Ｑ。＋Ａ．Ｑ，＋Ａ。∑Ｑ。，印， （５－７ｂ）
ａｐＥＡＰｓ

Ｍ＝一２（ＡｈＱ，ｃ＋＋Ａ。∑Ｑｃ，ａｐｃａ／＇）， ’
（５—７ｃ）
ａｐ∈ＡＰｓ

Ｎ＝Ａｈ（ｃ＋）ＴＱ，ｃ‘＋Ａ。∑（ｃ印）ＴＱ。，印ｃ印， （５—７ｄ）
ａｐ∈ＡＰｓ

其中九，ｈ，Ａ。为对应项的权重。我们发现，添加障碍物惩罚后，性能指标依旧为二次

型，且当轨迹时长非负、“拉力”影响时间段非负、以及权重项非负时，二次项的系数

矩阵Ｑ始终为正定。

５．３．２ 问题杓建及迭代求解过程

局部优化的迭代求解过程如算法８所示，我们将局部优化构建为一系列二次优化问

题，循环求解，每一轮循环问题建模为

ｒａｉｎＪＲｏ
。
（５・８）
ｓ．ｔ．Ａｃ＝ｄ，Ｖｔ∈［ｏ，卅，ｘ（ｔ）∈ｚ，”。。，ｕ（ｔ）∈ｕｆ’８８，

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

算法８：局部优化／轨迹提优
Ｉｎｐｕｔ：障碍物环境Ｅ，初始轨迹７ｒ洲ｔ或轨迹边占

Ｏｕｔｐｕｔ：优化轨迹７ｒ，是否成功标志ｒｅｓｕｌｔ
ｌ ｉｆ输入轨迹边ｔｈｅｎ

２
Ｌ‰ｔ ｔ－－ＳｐＨｔＥｄｇｅ（￡）；
３ ｗｈｉｌｅ ｉｔｅｒａｔｉｏｎ ｎｌｇｍ＜ｍａｘ礼ｔ‘ｍ ｄｏ

４ Ａｄ｜ｕｓｔＯｂ｜ｅｃｔｉｖｅ０；

Ｓ
丌咖巾＋－ＣｌｏｓｅｄＦｏｒｍＳｏｉｖｅ０；
６ ｆｆ ｃｏｌｌｉｓｉｏｎｓ＋－－ＣｈｅｃｋＦｅａｓｉｂｌｅＯｈｅ哪助ｔｈｅｎ

Ｌ苫州朋训沣讹ｅ；
７

０
Ｌ ＡｄｊｕｓｔＯｂ｜ｅｃｔｉｖｅ（ｃｏｌｌｉｓｉｏｎｓ）；

¨ｒｅｓｕｌｔ÷一ｆａｌｓｅ；

其中Ａｃ＝ｄ为每段的边界约束，即始末状态约束以及段间高阶导数的连续性约束，此

处为位置，速度，加速度连续。

与上一章不同的是，我们不优化权重系数，只优化多项式系数，当每段轨迹时长

已知、所有吸引点都确定、权重给定时，可根据第４．３．３节所描述的方法以闭式解求出

式（５．８）问题的无约束最优轨迹（算法８第５行）。之后我们对求得的轨迹进行碰撞检测，

若无碰撞，则得到一条安全轨迹，循环结束（算法８第６、７、８行）；若碰撞，则根据碰

撞信息搜索选取新的吸引点ＡＰ，在原有吸引点的基础上，增量式添加新的吸引点，重

新构建障碍物碰撞惩罚代价五，此时得到新的与式（５．８）结构相同的二次优化问题（算
法８第１０行），继续闭式求解，如此循环直至求解出安全轨迹或循环次数至上限。关于速

度、加速度、加加速度的幅值约束，若求解得的轨迹违背了这些约束，我们则对应增加

违背约束对应轨迹段的时长，之后重新闭式求解。

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

５．３－３吸引点选取

吸引点很大程度上引导了待优化轨迹形变的方向，其选取较为重要，下面描述局部

优化中如何选取吸引点。如图５．１ａ所示，对于碰撞轨迹边乏０ｆｆ池（黄色曲线）中的碰撞

部分，我们记碰撞起始与碰撞结束的点分别为Ｐ１与Ｐ２，记碰撞中间点为只。接着我们

使用路径搜索的方法（例如Ａ星算法）得到一条从Ｐ１到Ｐ２的安全路径（绿色折线），
．．．．．．．．．．．．．．．Ｊ
记此路径的中间点为Ｐｍ。作向量只《并延长一定距离（红色箭头），此时便得到针对
该碰撞部分的吸引点Ｒ，该吸引点只影响轨迹边碰撞部分附近的轨迹（蓝色虚线所指

示范围）。由于我们只对相对较短的碰撞轨迹进行局部优化，找安全路径的搜索范围较

小，因此搜索过程能够在非常短的时间内完成。Ｚｈｏｕ等人【１１９】的工作使用了类似的局部

搜索策略来构造与障碍物距离梯度信息，得到的梯度结果较为粗糙、不精确，可能损害

其梯度优化的收敛。本章对求解问题的二次型构造避免了计算精确的障碍物梯度，而是

把障碍物的信息融合进性能指标中，在每次循环的过程中改变优化指标。

▲
、
豳
豳
Ｌ

圈
＼ Ｌ 嗣
吣～ 隧 豳
～ ｋ穰

黼 ＿＿ 目＿
、
１●

∞翻
湛
隧
蕊 潮 －簿 一
ｒ
翳
Ｈ嘲

豳 黼豳
’

、 ■’ 瀚豳巴 ≯
——

露 豳隧
黼 豳圈躐 隧 缀隧
飞 —一 隔『
豳 叫旷
Ｉ

曩 疆圈
翱豳 嬲
№一
Ｅ ●
ａ

ｔ＇－前端轨迹
，—、芋影响区域
，”。菜…・次迭代的中｜１１ｌ】轨迹

（ａ）局部优化的情况 （ｂ）后端提优的情况

图５．１选择吸引点示意图

５．４后端轨迹提优

在后端提优中，我们使用与局部优化相同的问题构造方式，性能指标构造与式（５．４）至

式（５．７）类似，求解过程与算法８类似，不再赘述，各记号沿用第３．３．１节的定义，下面主

要描述与局部优化求解时的不同之处：

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

１．不再需要算法８第２行所示的分割轨迹，因为此时待优化轨迹由前端规划得到，已

经是分段多项式，有较高的优化自由度。

２．选择吸引点时不再需要搜索安全路径，因为此时前端给出的初值轨迹已经是安全

无碰撞的了。如图５．１ｂ所示，当某一次循环的结果轨迹（黄色虚曲线）与障碍物发生碰

撞时，我们记碰撞部分的中间点为Ｐｃ，之后在初值轨迹（橙色实曲线）上找到对应时刻

的位置点，记为只，作向量只耳并延长一定距离（红色箭头），此时便得到针对该碰撞

部分的吸引点尸口，该吸引点只影响待优化轨迹上点耳附近的轨迹（蓝色虚线所指示范

围）。

后端循环迭代过程求解轨迹的示意如图５．２所示，为了更好的观察，我们将障碍物

设为半透明，橙色轨迹表示前端轨迹初值，红点表示吸引点，在迭代的过程中不断增加，

对应的蓝色线段表示某一吸引点在优化轨迹上所作用的范围。随着迭代进行，优化轨迹

由浅绿色变为深绿色，最终的深绿色轨迹相较初值轨迹更为顺滑同时与障碍物保持了一

定距离。通过充分利用前端轨迹所提供的时间和空间的信息，且考虑轨迹周围障碍物的

信息，同时保留迭代闭式求解的结构，后端提优相较上一章更为高效且成功率更高。

５．５数值与实物实验

５．５．１实验设置

在仿真与实物飞行中，我们设置飞行器运动状态高阶导数的幅值上限为５ｍ／ｓ的

速度，７ｍ／ｓ２的加速度，以及１５ｍ／ｓ３的加加速度。性能指标中时间项的权重设置为

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

Ｐ＝１００。所有的仿真数值比较经由一台含３．４ＧＨｚＩｎｔｅｌ ｉ７－６７００处理器的计算机完成，实

物飞行测试所使用的自组装四旋翼飞行器与上一章（第４．５节）相同，机载计算机的处理

器为１．８ＧＨｚＩｎｔｅｌｉ７—８５５０Ｕ。对于碰撞检测，我们事先使用０．１ｍ分辨率的占据栅格地图

构建，障碍物所在栅格被膨胀０．２ｍ。

５．５．２前蟠对比测试结果及分析

为了验证在采样前端中结合双向搜索（ＢＳ）和局部优化（Ｒｏ）的效果，我们对是

否结合ＢＳ、ＲＯ二者之一或者都结合进行控制变量的消融实验（Ａｂｌａｔｉｏｎ Ｓｔｕｄｙ），不同

方法的标记如表５．１所示。为了排减小样类方法偶然性的影响，每种方法在３种类型的

环境中各自运行１０００次。３种环境分别为如图５．３所示的森林类型类２．５维环境（记作

Ｅ），图５．４所示的洞穴类３维环境（记作Ｃ．），以及图５．５所示的含有许多窄缝的２．５维环

境（记作ＷＩ）。森林、洞穴、窄缝三种环境分别给定０．１５ｓ，１．０ｓ，２．５ｓ的求解时间预算。

某一次规划的结果轨迹也展示在３个环境图中，其中橙色为结合ＢＳ与ＲＯ方法所求得

的第一条轨迹，红色为此方法用完时间预算后求得的最终轨迹，绿色为结合ＲＯ的最终

轨迹，黄色为结合ＢＳ的最终轨迹，蓝色为不结合ＢＳ或者ＲＯ的基准方法的最终轨迹。

表５．１前端规翊方法标记

Ｂａｓｅｌｉｎｅ 单纯采样规划，都不结合 ＢＳ．Ｒｏ 结合双向搜索以及局部优化

ＢＳ 仅结合双向搜索 ＲＯ 仅结合局部优化

表５．２前蛸规划１０００次平均结果比较

图５．６展示了不同方法在３种环境下求解出第一条轨迹的平均用时，可以发现，相

较于基准方法，不论是结合双向搜索或是结合局部优化，求解得到第一个解的时间都大

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

大缩短，尤其是对于窄缝的环境。表５．２展示了每种方法的最终轨迹的平均代价以及求

解成功率，可以发现经过相同的求解时间，相较于基准方法，不论是结合ＢＳ或是ＲＯ，

在不同环境中求解得到的轨迹质量都有所提升，求解成功率也都大大增加。总体来说，

在较大尺度的复杂环境下，本章提出的前端规划方法能够在数十毫秒内快速得到一条可

行轨迹，并且在给定的时间预算内提升轨迹质量，因此适合有实时求解需求的应用。

图５．３森林类型类２．５维测环境以及某次规划轨迹对比

图５．４洞穴类型３维测环境以及某次规划轨迹对比

图５．５窄缝类型３维测试环境以及某次规划轨迹对比

５．５．３后端对比测试结果及分析

不同后端提优方法对前端结果的利用方式不同，利用程度不一，我们将本章的后端

提优方式与以下三种学界前沿方式进行比较。

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

８∞
８１９．３
７ ∞
６ ∞
５ ∞
４ ∞
２３５．５
３∞

２ ∞
－１９０．９
１∞

ｏ
ｔ陪
（ａ）森林环境 （ｂ）洞穴环境 （ｃ）窄缝环境

图５．６不同方法在不同环境下求解出第一条轨迹的平均用时（ｍｓ）

方法１：第４章所提出的后端提优方法，其构建路径拓扑惩罚代价（第４－３．２节）使待

优化的轨迹靠近前端轨迹周围的安全区域，使用双层优化求解多项式轨迹系数参数以及

各代价项的权重，但是在构建优化问题时没有考虑周围的障碍物信息。

方法２：Ｌｉｕ等人【５５】的轨迹提优方法，其将分段多项式轨迹的中间路径点固定在前

端轨迹每一段的中间路径点上，优化路径点的其他自由状态，若发生碰撞则增加轨迹树

固定点的个数，在构建优化问题时同样没有考虑周围的障碍物信息。

方法３：Ｚｈｏｕ等人［５７】的方法，其在前端轨迹上采样出控制点，用Ｂ样条重新参数

化轨迹，使用距离场将轨迹推向无障碍物的区域。

从方法上看，方法１，方法２，以及本章的提优方法在每一轮迭代求解时均有闭式

解，而方法３构造软约束的非凸优化问题，使用通用优化求解器ＮＬｏｐｔ［１１４１求解。测试

环境与第５．５．２节相同，每种提优方法使用同样的前端初值运行３００次，平均结果记录在

表５．３与表５．４中。

从表中可以发现，本章的后端提优方法在求解时间、轨迹长度、轨迹平滑程度（加

加速度积分）、以及求解成功率上都胜过其他方法，尤其是轨迹平滑程度有显著提升。

我们同样发现在窄缝环境中，本章方法相较方法２在求解成功率上有所下降，迭代次数

上也有所增加，这是因为在含有较多仅恰好允许飞行器通过的窄缝环境中，类似于方法

２在优化时将一些路径点固定在安全区域的做法能够提供更强的约束以使得待优化的轨

迹待在安全的区域，从而更容易求解成功，但从数据中可以发现相应的轨迹平滑程度就

下降了。此处的成功率仅针对后端优化，对于完整的前端．后端求解框架，若后端求解

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

表５．３后蝤优化对比结果第—部分

失败。则前端已满足约束的轨迹依旧可以被执行。

与其他方法相比，本章方法由于以下两点原因而在整体上有着较好的表现。

１．额外利用了前端轨迹附近的碰撞信息，将原前端轨迹中与障碍物相聚较近的部分

推离障碍物。在迭代过程中检测到的周围障碍物提供了更丰富的周围环境信患。将其构

造成碰撞惩罚代价整合至优化性能指标中而同时保有闭适求解结构，相比于方法１，保

证求解效率的同时增加了成功率。

２．无需固定中间路径点，保留了更多的优化自由度，因而相比于方法２能得到加加

速度积分更小更顺滑的轨迹。

方法３一定程度上代表了其他构造非凸优化利用梯度信患求解的方法，收相比于构

造为凸问题享有闭式解的方法，方法３的优化求解需要更多的时间收敛，且当障碍物梯

度信息不准确时（例如树丛等杂乱环境）容易求解失败。

总结来说，当已有一个较高质量的前端轨迹初值时，本章所提出的后端方法作为一

个有效的轨迹提优手段，能够仅花费极少计算代价使得前端轨迹更加顺滑更适合飞行器

执行。

５．５．４实物四旋翼飞行善飞行测试

我们在两个挑战性的环境中进行实物飞行测试，相关实验视频可在链接１中找到。

第一个测试环境如图５．７ａ所示，为类似迷宫类型的狭窄环境，规划的起点与终点都

被障碍物“困住”。给定相同的ｌＯＯｍｓ前端规划时间预算，规划得到的轨迹如图５．７ｂ所

示，其中蓝色轨迹为本章前端方法得到的轨迹，红色轨迹为在前端轨迹的基础上使用本

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

表５．４后螭优化对比结果第二部分

章后端提优得到的轨迹，黑色轨迹为按第４章前端方法得到的轨迹经过第４章后端方法提

优后得到的轨迹。对比红色轨迹与黑色轨迹，可以发现本章方法进一步改进了上一章的

方法。得到更顺滑的轨迹。

在第二个测试中，我们进一步测试在复杂环境下的实时规划求解能力。测试环境如

图５．８ａ所示，复杂的障碍物将环境分出许多不同的路径拓扑，且中间只有两个圆形门洞

可供通行。在飞行器不问断飞行的过程中不断随机改交其目标点，带有初始速度与加速

度的情况下进行重规划。图５．８ｂ中枚红色箭头示意了当次重规划的初始速度，对比红色

轨迹（本章方法）与黑色轨迹（上一章方法），可以发现本章的方法规划出的轨迹顺着

当前初速度行进，处于更合理的路径拓扑中，而对比的轨迹出现了不期望的调头。

５．６本章小结

本章主要探究实时重规划问题，同样使用前端．后端的层级式规划框架，在上一章

的基础上增强了采样类前端以及后端提优的效率与性能，更快地求解得到一个初始可行

解，并且提优的成功率更高，从而更好满足实时重规划的需求。我们总结主要贡献如下：

１．针对采样类动力学轨迹规划在窄缝类空问中求解效率低的问题，提出在采样规划中结

合快速求解的局部优化以及结合双向搜索的方法。双向搜索利于求解目标点“被困住”

的情形，快速局部优化专注于探索有限范围内的解空间，其构建为一系列的无约束二次

凸优化问题，每轮迭代具有闭式解。２．针对前端初值信患没有被充分利用的问题，我们

在后端提优时将障碍物信患构建进性能指标中，同时保有二次规划问题的结构，使得保

证求解效率的同时成功率更高。３．进行了大量数值对比测试，并在实物四旋翼飞行器上

进行导航测试，在稠密障碍物、类迷宫等环境中验证了所提出规划方法的有效性。

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

（ａ）四旋翼飞行轨迹组合图

（ｂ）规划轨迹示意图

图５．７迷宫类型飞行测试环境

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

（ａ）四旋翼飞行轨迹组合图

（ｂ）规划轨迹示意图

图５．８实时重规划测试环境

９１

万方数据
 浙江大学博士专业学位论文 ５结合快速局部优化和双向搜索的实时规划

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

６ 自适应跟踪参考轨迹实时重规划

６．１引言

上述章节分别探究了飞行器在大范围地图中的全局最优轨迹规划，以及相对小规模

的局部实时规划问题。本章主要探究情形三．跟踪规划，即已知大范围地图信息，但是
精度较为粗糙，甚至是过时的，由此地图规划出全局参考轨迹，在跟踪此参考轨迹的过

程中进行相对小规模的局部实时规划。

此情形下的导航在某种程度上可以认为是参考轨迹的跟踪问题，与一般意义下的全

状态轨迹跟踪不同的是，我们只希望在时间与空间上跟踪此轨迹的位置。当面对系统动

力学可行性约束、安全避障约束、以及一些特定任务要求等诸多约束时，其难点在于如

何以及在导航系统的哪个环节考虑约束，以及如何处理与参考轨迹之间出现较大偏差的

情况。

许多相关工作在控制器层面考虑跟踪参考轨迹：基于模型预测控制（Ｍｏｄｅｌ Ｐｒｅｄｉｃ—

ｆｉｖｅＣｏｎｔｒｏｌ，ＭＰＣ）类的方法［１２０，１２１１构建非线性优化问题求解离散化的控制输入，其形式

较为通用，但对避障约束以及大跟踪误差的处理能力有限。Ｒｏｄｒｉｇｕｅｚ—Ｓｅｄａ等人【ｍ，１２３】根

据跟踪轨迹以及飞行器互相避让的指标针对性地设计特定的专家控制律（Ｃｏｎｔｒｏｌ Ｌａｗ），

虽然计算简便，且能够在基于某些假设的前提下理论上保证稳定性与收敛性，但是实际

情况往往容易违背这些假设，导致控制率无法应用。另一些基于强化学习的方法［１２４，１２５］

通过大量模拟训练得到输入输出模型，将当前的观测输入直接映射得到一定离散分辨率

下的最优的控制信号，然而现实系统与抽象模型之间有不可忽视的建模误差，得到的模

型往往无法满足实际物理系统的应用需求。

由于上述缺点，近年来考虑跟踪参考轨迹的工作［１掘１２８】趋向于在导航系统中增加局

部规划模块，在此局部规划中考虑避障等各类约束相比在控制器中考虑更加灵活，以滚

动时域（Ｒｅｃｅｄｉｎｇ Ｈｏｒｉｚｏｎ）的方式不断进行局部重规划，控制器只考虑跟踪局部规划

得到的轨迹，而全局参考轨迹则提供局部目标等信息。针对面向跟踪参考轨迹的局部重

规划，我们分析出以下几点需求：１．最基本的，局部规划的轨迹需满足系统动力学模型、

状态／控制输入幅值约束和安全避障等所有约束；２．需要鲁棒且灵活地跟踪参考轨迹。在

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

无障碍物时在时空维度尽可能跟踪参考轨迹，即贴合参考轨迹上相应时刻的位置，而在

避障或受扰动时能够自适应地调整全局参考轨迹上的跟踪范围，且之后快速回到全局轨

迹上。

考虑上述需求，在求解时有以下两个问题需要解决：

１．如何确定局部规划的目标状态，以及如何确定的局部规划的时域长度，也即局部

轨迹的时长。若将目标状态设定为全局参考轨迹上的状态，由于希望跟上参考轨迹，则

目标状态与时域长度之间存在约束关系，规划的时域长度即确定了参考轨迹上的目标

状态。如图６．１所示，灰色曲线为全局参考轨迹，黄色点指代当前ｔ＝ｔｏ时刻系统的状

态，黑色点为ｔ＝ｔｏ时刻对应参考轨迹上的期望状态，若选取Ｔ，１时长之后参考轨迹上

的状态（红色箭头）作为目标状态，由于其具有非零速度，又与障碍物靠的太近，则满

足动力学等约束的轨迹势必与障碍物发生碰撞（红色曲线），而选取乃ｚ时长之后的目

标状态（蓝色箭头）则存在可行解（蓝色曲线）。针对局部目标状态的选择，Ｏｌｅｙａｉｋｏｖａ

等人【６５１选取全局参考轨迹上距离当前位置一定范围之外的点，并且在局部规划时固定

此目标状态。Ｌｉｕ等人【１２６１根据当前跟踪误差基于一个模糊模型（ＦｕｚｚｙＭｏｄｅｌ）确定出

规划的时域长度。Ｓｅｎｂａ§ｌａｒ等人［１２８】选取全局参考轨迹上距离当前时刻一定时长之后的

点，并且在局部规划时固定此目标状态。本章提出的方法将规划的时域长度设为优化变

量，目标状态则约束在全局参考轨迹上，时域长度决定了目标状态在全局参考轨迹上的

位置，在优化的过程中自适应地确定最优的目标状态与时域长度，与根据经验确定的方

法相比更加灵活，对参考轨迹的时空跟踪更加精确。

图６．１局部规划目标状态以及时域长度示意图

２．如何增加连续重规划之间的连贯性。如图６．２所示，黄色、绿色、蓝色、红色曲线

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

依次为连续重规划轨迹，任意前后两次重规划都分别选择了不同的方向来绕过障碍物，

造成实际执行的路线（黑色点）产生“摇摆”并且一直在“靠近”障碍物而没有尽早地

绕开，虽然前三次重规划的结果都避开了障碍物，但是第四次的重规划（红色虚线）却

没有足够的空间来规划出无碰撞轨迹。若在连续的重规划之间考虑连贯性，即尽可能选

择同一方向饶障，则可以尽早的远离障碍物，更加安全。针对重规划连贯性，Ｋｎｅｐｐｅｒ

等人［１２９・ｔａｌｌ在重规划时使用运动基元（ＭｏｔｉｏｎＰｒｉｍｉｔｉｖｅｓ），在选择执行的轨迹基元时考

虑不同基元之间的拓扑相似性，类似于反应式避障。Ｃｈｅｎ等””２】提出局部认知地图的
概念，使用特定的地图表征区分轨迹间的拓扑关系，但只能用于二维规划。本章中定义

与地图的表征形式无关的轨迹间丁．拓扑连贯关系，并且设计基于采样的轨迹规划方法，

针对性地规划出与之前重规划结果之间具备连贯关系的轨迹，使得在完整导航过程中所

执行路线的一致性更强，更加顺滑，更加安全。

本章后续内容安排为在第６．２节给出７－一拓扑连贯关系的定义，并且介绍考虑拓扑连

贯的、基于采样的动力学运动规划方法；在第６．３节提出自适应优化末端状态跟踪参考

轨迹的轨迹规划方法，介绍如何处理各项约束，以及如何在优化过程中保持拓扑连贯；

在第６．４节将所提方法应用于接近真实的四旋翼仿真导航系统，进行对比测试；在６．５节

进行小结。

６．２基于采样的拓扑连贯性运动规划

路径拓扑等效性（ＨｏｍｏｔｏｐｙＥｑｕｉｖａｌｅｎｃｅ）的概念在许多工作中被提出过［１３３。３７１，这

些工作大多关于如何判断首尾相连的路径是否属于其定义下相同的路径拓扑，以及关于

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

如何找到属于不同路径拓扑的路径。对于本章所探究的重规划一致性问题，我们针对性

地提出关于时间参数化轨迹的７－一拓扑连贯的概念。我们首先给出轨迹拓扑连贯性的定

义，再介绍考虑拓扑连贯的、基于采样的动力学运动规划方法。

６．２．１丁．拓扑连贯关系

定义１．（７－一拓扑连贯关系）给定障碍物环境Ｅ，以及两条以时间参数化的轨迹７／＂１（ｔ），ｔ∈

［ｏ，丑】，Ｉｒ２（ｔ），ｔ∈【０，乃】，其中五，Ｔ２分别为其轨迹时长，若满足ＩＩｐ。，（ｔ）一ｐ。：（ｔ）１１２＜￡，
即两条轨迹起始时刻位置之间的距离在一定范围内，且对所有的ｔ∈［０，ｍｉｎ（Ｔ，正，疋）］，

线段Ｐ。。（ｔ）ｐ。。（￡）不与环境障碍物发生碰撞，则称轨迹７１＂１（ｔ）与丌２（ｚ）互为７－一拓扑连贯关

系，记作ｎｃ（７ｌ－１（ｔ），丌２（ｔ），７．）。

对于检测某一时间段范围内不同轨迹上任意两点的连线段是否与环境障碍物发生

碰撞，我们采取类似Ｚｈｏｕ等人【１３７，１３８１均匀可视性形变（Ｕｎｉｆｏｒｍ Ｖｉｓｉｂｉｌｉｔｙ Ｄｅｆｏｒｍａｔｉｏｎ）

的做法，在待检测的轨迹上根据时间递增采样位置点，检测相同时刻所对应轨迹位置点

的连线路径是否与环境障碍物发生碰撞。此种做法为Ｊａｉｌｌｅｔ等人【１３４】所提出的可视性形

变（ＶｉｓｉｂｉｌｉｔｙＤｅｆｏｒｍａｔｉｏｎ）的简化，在只引入小范围误差的情况下极大减少了检测所需

的计算量。

吼（ｔ） ‰（ｔ）

Ⅻ渺
图６．３下．拓扑连贯轨迹示意图

与大多数路径拓扑等效性定义不同的是，上述拓扑连贯关系的定义不需要轨迹具

有相同的起始位置点与终止位置点，我们要求起始位置接近，对终止位置则没有要求。

图６－３的例子展示了三条轨迹的拓扑连贯关系，图中三条轨迹起始位置接近，终止位置相

距较大，可将黄色轨迹～（ｔ）视为前一次重规划的结果，蓝色轨迹丌６（ｔ）和绿色轨迹丌６（￡）

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

为此次重规划可能的轨迹。我们对每条轨迹以０．１ｓ的时间分辨率采样出位置点，以直线

连接对应时刻的位置，判断与障碍物的碰撞，得到７１＂？／（ｔ）与丌口（￡）互为０．３ｓ一拓扑连贯关

系，７１＂ｙ（ｔ）与仉（ｔ）互为０．９ｓ．拓扑连贯关系。
根据７－一拓扑连贯的定义，则在重规划时考虑连贯性的做法即为使得本次规划轨迹

与前一次重规划的结果互为丁一拓扑连贯关系，可形象地表述为在ｔ∈『０，７＿１的时间内，本

次重规划轨迹与上一次规划结果具有相同的“前进方向”，从相同的方向绕过障碍物。ｒ

为可调节的参数，当取７．＝０时即表示重规划时不考虑连贯性，每次重规划相互独立，７Ｉ

越大则表示重规划时对连贯性的需求越强。

６．２．２ ７．．拓扑连贯前靖规划算法

我们给出两种考虑７Ｉ一拓扑连贯的轨迹规划算法：

１．给定一条轨迹７ｊ－ｔ（ｔ）以及拓扑连贯参数７．，ｐｍ（ｏ）与规划的起始位置接近，要求规
划出的轨迹与ｍ（ｔ）互为７－一拓扑连贯关系，称此算法为丁一拓扑连贯生长树。
２．没有给定前置轨迹，给定拓扑连贯参数７－，要求规划出尽量多条起终点相同的轨

迹，其互相不为７－一拓扑连贯关系，称此算法为丁．拓扑连贯连接图。

６．２．２．１ ７－一拓扑连贯生长树

ｔ）

算法主体流程与算法１类似，遵循ＲＲＴ＊算、法【１２】的框架，不再赘述，以下描述不同

之处，各记号沿用第３．３．１节的定义。在采样时，除了均匀采样之外，还增加采样给定轨

迹上的状态点以及其附近状态点。在选择父节点时，如算法９所示，当检查连至某一潜

万方数据
 浙江大学博士专业学位论文 ６自适应跟踪参考轨迹实时重规划

算法９：选择父节点
Ｉｎｐｕｔ：障碍物环境Ｅ，潜在父节点集合疋，待连接节点ｘ。。。，轨迹丌ｌ，拓扑连

贯参数７－

Ｏｕｔｐｕｔ：父节点）（ｐ

Ｉ砩÷－ｎｕｌｌ，Ｑ÷－Ｄ；

２ ｆｏｒ Ｘｉ∈疋ｄｏ

３
ｌ＾ｘ硝…÷－９ｘ，＋７（Ⅺ，Ｘｎｅｗ）；
４ ｉｆ豇尚。。＋Ｈ（ｘ。㈣Ｘｇｏａｌ）＜瓯，，。ｔ ｔｈｅｎ

ｓ
ｌ Ｌ Ｑ÷－十（Ｋ氨‰Ａ
６ ｗｈｉｌｅ Ｑ非空ｄｏ

ｘ。ｉ。＋－Ｑ．ｔｏｐ０；
ｉｆ ＣｈｅｃｋＦｅａｓｉｂｌｅ（ｘｍｉ．，ｘ。。＂，Ｅ）ｔｈｅｎ

‰Ⅲ卜ＲｅｔｒｉｅｖｅＴｒａｊｅｃｔｏｒｙ（Ｔ，ｘｍｉ。）＋（Ｘ。㈨Ｘ。。）；
／・判断是否与给定的轨迹＂ｆｉｔ互为ｒ一拓扑连贯 年｛

ｉｆ ７￡Ｃ（７ｎ，７ｒｈ。。，７Ｉ）ｔｈｅｎ

昂÷－Ｘｍｉｎ；

ｂｒｅａｋ；

在父节点ｘ。缸的轨迹边（ｘ。饥，ｘ一）为无碰撞之后（第８行），从ｘ。ｉｎ根据父节点指针
回溯至根节点再加上轨迹边（ｘ，ｎ溉巍。。），得到轨迹７ｒｎ。（第９行），再判断丌竹。。是否与
给定的轨迹为７．．拓扑连贯关系（第ｌＯ行），若是，才将ｘｍｉ。认定为符合条件的父节点。

在重连接时，如算法１０所示，当检查连至某一潜在子节点）【ｃ的轨迹边（ｈ。，）【ｃ）为无碰

撞之后（第４行），从ｘ。。。根据父节点指针回溯至根节点再加上轨迹边（ｈ。。，Ｘｃ），得到
轨迹７ｒ。。。（第５行），再判断７ｒ。。。是否与给定的轨迹为下．拓扑连贯关系（第６行），若是，

才将ｘ。的父节点改为ｘ。。。。在尝试连接目标点时，同样需做上述判断。

图６．４展示了１－－拓扑连贯生长树算法的一个例子。红色曲线为前置轨迹＿ｒｔ（ｔ），给定

下，轨迹树７．的生长如绿色曲线所示，在ｔ＝【０，７－］的时间内轨迹树与丌Ｉ（￡）（黑点部分）
具有“相同方向”，而在ｔ＞丁之后，轨迹树的生长没有限制。蓝色虚线为不符合与７ｒ，（ｔ１

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

算法１０：重连接
Ｉｎｐｕｔ：障碍物环境Ｅ，轨迹树丁，潜在子节点集合疋，新添加的节点，ｘ。。，

轨迹死，拓扑连贯参数丁

１ ｆｏｒｘｃ∈疋ｄｏ

＾ｘ。…。。÷＿级…＋歹（】（ｔｌ。。，）（ｃ）；

ｉｆ盈…１ｘｃ＜鲰。八参‰…＆＋Ｈ（ｘ。，Ｘｇｏａ！）＜唬～。ｔ ｔｈｅｎ
ｉｆ ＣｈｅｃｋＦｅａｓｉｂｌｅ（ｘ。。，）ｃｃ，助ｔｈｅｎ

“ｅｔＥｌ÷－ＲｅｔｒｉｅｖｅＴｒａｊｅｃｔｏｒｙ（Ｔ，％。。）＋（ｘｎ㈨Ｘｃ）；
／・判断是否与给定的轨遗丌ｌ互为Ｐ拓扑连贯

ｉｆ ７－ｔＣ（Ｔｒｌ，７ｒ。。。，７－）ｔｈｅｎ

Ｌ ｋ：女Ｘｃ的父节点为）ｃ，ｌ。；

成７－．拓扑连贯关系的生长方向，因此不能加入轨迹树中。

６．２．２．２丁．拓扑连贯连接图

在我们考虑的动力学运动规划中，轨迹连接图为有向图，其构建过程主体框架如算

法１１所示，输入规划起始状态Ｘ涮。，规划目标状态ｘ。。ｚ，障碍物环境Ｅ，以及拓扑连贯

参数丁，最终求解得到互相不为丁一拓扑连贯的轨迹集合Ⅱ。

我们首先定义所需的数据结构。轨迹连接图９由有向轨迹边的集合￡以及顶点的

集合１，构成，记为９∽，ｙ）。

图中每一段轨迹边ｅ∈￡包含以下信息：

・％。“：边的起始顶点；

・仇。舭边的末尾顶点；

・（Ｘｈ。ａｄ，ｘｔａｉｌ）：连接起始顶点状态与末尾顶点状态的单段多项式轨迹，其中Ｘｈ。。ｄ为
起始顶点对应的状态，Ｘ缸订为末尾顶点对应的状态；

在不混淆的情况下，我们以（Ｘｈｅ。ｄ，ｘ涮ｚ）表示一段轨迹边。

轨迹７ｒ由多段依次收尾相连的轨迹边构成，函数Ｃｏｓｔ（Ｉｔ）表示此轨迹的代价。

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

图中每一个顶点甜∈Ｖ包含以下信息：

・Ｘｈ。ａｄ：顶点对应的状态向量；

・Ⅱ：连接起始状态对应顶点至ｕ的轨迹的集合，其中轨迹互相不为丁．拓扑连贯；

在不混淆的情况下，我们以顶点对应的状态ｘ代表该顶点。

下面描述拓扑连贯连接图生成算法，如算法１ｌ所示，在每一轮迭代中，主要步骤如

下：

１．拓扑引导采样（第３行）：与算法６中的相同，只在３维的位置维度和３维的速度

维度采样，加速度维度在采样时设为不定，将在后续生成可行轨迹边求解ＴＰＢＶＰ时自

动计算，求解ＴＰＢＶＰ的方法与第５．２．２节相同，记此采样点为ｘ。。。

２．近邻搜索（第４行）：同样，我们以固定的欧氏距离半径ｒ在图顶点中搜索出距离

）【，ｌ。小于ｒ的顶点作为其近邻顶点。

３．生成可行轨迹边（第５、６、７、８、９行）：对每一个近邻顶点ｘ。和求解以其为初

始时刻状态，ｘ。。为终止时刻状态的ＴＰＢＶＰ，得到单段多项式轨迹，若此轨迹满足约

束，则将其构建轨迹边，添加到图轨迹边的集合中。

４．添加顶点：若近邻顶点中找到至少一段满足约束的轨迹边（第ｌＯ行），则将ｘ。。

添加到顶点集合中（第１ｌ行）。此时，顶点ｘ。。中还未含有任何轨迹，稀。．Ⅱ为空集。

之后，对每一个新添加轨迹边（ｘ州。，ｘ。。。）的起始顶点）‰。（第１２行），将其含有的所
有轨迹】ｃ，，咖．１１分别与边（‰。，）ｃ，ｌ。）连成新轨迹７ｒｍ机（第１３、１４行），对每一条新轨迹
而眈，使用算法１２更新顶点‰。所含的轨迹集合ｘ。。．ＩＩ。

５．更新顶点所合轨迹（第１５、１９第行）：如算法１２所示，对某一顶点‰，给出一条

连接起始顶点与）ｃｃ的轨迹丌０，在顶点ｘｃ的轨迹集合ｘｃ．Ⅱ中寻找与之互为７－．拓扑连贯

的轨迹。若没有找到，则将丌。添加到Ｋ．Ⅱ中；若找到一条丁．拓扑连贯的轨迹丌ｃ，且７ｒ０

的轨迹代价低于丌ｃ，则将Ｘ。．ＩＩ中的丌ｃ替换为砌。

６．尝试连接目标顶点（第１６行）：当向图中添加一个新顶点ｘ。。后，若此新顶点距

离目标顶点小于一定距离，则我们尝试以此顶点状态和目标顶点状态作为边值条件求解

ＴＰＢＶＰ，若求解结果满足约束，则按算法１２更新目标顶点所含轨迹集合。

７．终止条件（第２行）：终止条件可设为算法运行时间超出预设求解时间，轨迹图中

边的个数超出预设个数，或是轨迹图中节点的个数超出预设个数等等，可根据不同需求

ｌ∞

万方数据
 浙江大学博士专业学位论文 ６自适应跟踪参考轨迹实时重规划

算法ｌｌ：拓扑连贯连接图算法
Ｉｎｐｕｔ：障碍物环境Ｅ，起始状态ｘ州￡，目标状态ｘ９Ｄｎｌ，拓扑连贯参数７．

Ｏｕｔｐｕｔ：轨迹集合Ⅱ

１图中状态点ｙ＋－｛ｘ｛碱），图中轨迹边￡÷－Ｏ，连接图９＋－（１，，￡），
２ ｗｈｉｌｅ终止条件未达到ｄｏ

Ｉ｝

Ｉ ｉｆＬＣ＝ｈｅｃｋＦ发ｅａｓｉｂｌｅ（ｘ一：，‰柚）ｔｈ钮
７

Ｌ＝三盎兰∽；
Ｈ

：２

“ ｉｆ ＴｒｙＣｏｎｎｅｃｔｉｎｇＧｏａｌ（ｘｎｅｔ‘，，ｘ口∞Ｊ ｔｈｅｎ

抄
Ｉ Ｌ：＝裂㈡；
２０
ＩＩ÷－ｘ９刊．Ⅱ；

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

算法１２：更新顶点所含轨迹ＵｐｄａｔｅＮｏｄｅＴｒａｊ（ｘ。，７１０，ｒ）
Ｉｎｐｕｔ：待更新顶点Ｘ。，待比较轨迹丌ｏ，拓扑连贯参数７－
１ ｆｏｒ ７ｒｃ ｉｎ ｘｃ．１１ ｄｏ

悟手州ｈｅｎ
ｅｌｓｅ

ｘ。．Ⅱ÷－＋丌。；
Ｌ

进行不同设置。

８．最后当终止条件满足后，算法停止迭代，目标顶点的轨迹集合Ｘ９０ａ１．Ⅱ即为所求
的轨迹集合（第２０行）。

图６．５展示了７－一拓扑连贯连接图算法的一个例子。实曲线为构建出的连接图，对于

其中一个顶点ｈ，从初始顶点ｘ溉：到）【ｔＩ的轨迹共有５条，非拓扑等效的轨迹有黄色与

蓝色３条，但是互相不为７．一拓扑连贯关系的轨迹只有蓝色所示的２条，因此ｘ。．Ⅱ含有
２条轨迹。

ｔ＝

ｔ＝Ｔ

万方数据
 浙江大学博士专业学位论文 ６自适应跟踪参考轨迹实时重规划

６．３ 自适应跟踪参考轨迹的轨迹优化

６．３．１问题描述

记所跟踪的参考轨迹为Ｘ，。，（ｔ），在ｔ＝０时刻开始跟踪。对于跟踪参考轨迹时的每

一次重规划，我们记规划开始时刻为ｔｏ，对应参考轨迹上的状态为ｘ，。，（ｔｏ）。在第２章规
划问题基本建模的基础上，轨迹跟踪重规划所对应优化问题的具体数学描述如下：

。。积与，了＝Ｚ巧（三ｕ（ｔ）Ｔｕ（ｔ））ａｔ＋ｐ，（Ｔｓ一霉。，）２ （６—１ａ）

ｓ．ｔ．文（ｔ）＝Ａｘ（ｔ）＋Ｂｕ（ｔ）， （６—１”

ｘ（Ｏ）＝ｘ眺，ｘ（乃）＝ｘ，。ｉ（ｔｏ＋乃）， （６－１ｃ）

ｌＩｖ（ｔ）ｌｌ：＝ＩＩｐ（ｔ）ｌｌ：≤Ｖ。。， （６—１ｄ）

ＩＩａｌ＇ＣＣＯＳ（ｅｊ冗。。缸（口０））ｅ。）０。≤曰。。， （６－１ｅ）

１１６ｕ（ｔ）Ｉｌ。≤Ｑ。ａｘ， （６—１０

厶ｉ。≤ｆ（ｔ）５，ｋ。， （６－ｌｇ）

ｆｖｚｓＴ（Ｐ（ｔ））≥ｒｔＤｌ，耽∈【ｏ，乃】， （６－１ｈ）

饨ｃ（ｐｉ。ｎ（ｔ），ｐ（￡），７．）． （６一ｌｉ）

式（６一ｌａ）优化目标中除了最小控制量之外，还增加了一项惩罚轨迹时长 （乃）与期

望跟踪时长（耳。，）的差值，其中Ｐｒ为此项的权重。

式（６．１ｂ）为飞行器动力学模型等式约束，采取三阶链式线性系统。

式（６．１ｃ）为边值等式约束，其中重规划时规划起始时刻状态的确定如第２．４．１节所述，

规划终止时刻状态约束在全局参考轨迹上，由轨迹时长，也即规划时域长度确定，此项

末状态约束的构建确保了在单条局部轨迹执行完成后，飞行器恰好在对应时刻接入全局

参考轨迹，到达了该刻所全局参考轨迹所规划的期望状态。将轨迹时长作为优化变量，

自适应地确定了最优的目标状态与时域长度，即自适应地跟踪参考轨迹。

式（６．１ｄ）至式（６．１ｈ）为表示系统所受物理限制的过程不等式约束，除了速度幅值约束

（式（６．１ｄ））之外，在许多情形下希望飞行器保持姿态平稳，加速度幅值约束和加加速度

幅值约束在一定程度上能表示飞行的平稳性，但是不够确切，在此我们精确地限制飞

行过程中的的姿态角幅值小于最大允许倾角‰。（式（６—１ｅ））、机体角速率向量模的幅

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

值小于最大允许角速率Ｑ。。。（式（６．１０）、以及推力幅值在螺旋桨所能产生的最小推力

五。ｉ。以及最大推力厶。。之间（式（６一ｌｇ））。轨迹姿态角、机体角速率、以及推力的计算
由第２．２．２节给出的微分平坦变化ｍ得到。对于避障约束（式（６—１ｈ）），我们使用梯度距离

场，希望优化轨迹在空间位置上与障碍物保持最小距离ｒ。ｏｌ。

式（６．１ｉ）为拓扑保持约束，其中ｐ（ｔ）为待优化轨迹的位置，Ｐ洲。（￡）为给定的表示期
望路径拓扑的轨迹位置，我们希望优化出的轨迹与之互为７Ｉ．拓扑连贯关系。

对于上述问题的求解，我们使用轨迹类的优化变量形式消除各类等式约束，尤其是

末状态约束，使用罚函数法处理不等式约束，最终转换为非光滑的无约束优化问题，整

体上采取Ｌｅｗｉｓ等人［１３９］提出的处理非光滑优化的拟牛顿法求解，在线搜索时额外考虑

拓扑连贯性约束。以下详细描述约束处理方法与求解过程。

６．３．２使用轨迹类处理等式约束

Ｗａｎｇ Ａ Ｅ９７］提出ＭＩＮＣＯ（ＭＩＮｉｍｕｍＣｏｎｔｒｏｌ Ｏｐｔｉｍｉｚａｔｉｏｎ）轨迹类，其实际为分

段多项式，一条ｓ阶的ＭＩＮＣＯ轨迹实际为一条２ｓ一１阶的多项式样条轨迹，定义如下：

‰…２∽：［。，Ｔ】一叫ｃ＝Ｍ（ｑ，Ｔ）， ㈣
ｑ∈Ｒ”（Ｍ．１），Ｔ∈Ｒ笺｝，
其中ｐ（ｔ）表示一条ｍ维Ｍ段的多项式轨迹位置，ｃ＝（ｃ丁…．，ｃ］［ｆ）Ｔ∈Ｒ２Ｍ“…为
其整体多项式系数向量，ｑ＝（ｑ１，．．．，ｑｍ一１）为多项式段间路径点构成的矩阵，Ｔ＝

（噩…．，％）Ｔ为每段多项式时长构成的向量。
ＭＩＮＣＯ轨迹类的核心为线性复杂度的平滑映射

ｃ＝Ａ４（ｑ，Ｔ）， （６－３）

其将段间路径点ｑ，以及每段时长Ｔ的组合转化为多项式系数ｃ。给定起始与终止约束，

其使用多项式作为基底，ＭＩＮＣＯ轨迹类解析地求解了如下最小控制量的优化问题：

ｒａ础ｉｎ，ｏ＇７幽．。：Ｊｏｎ《昝置（知Ｔｕ（ｔ））出（６－４ａ）
础）ｚ
２
Ｌ抄）‘ｕ（。））出
ｓ．ｚ．ｕ（￡）＝ｐＯ）（￡），耽∈［０，乃】 （６４ｂ）

ｐ卜１】（ｏ）＝－ｏ，ｐ卜１１（巧）＝ｐ，，（６－４ｃ）

ｐ（正）＝吼，Ｉ≤ｉ＜Ｍ， （６－４ｄ）

万方数据
 浙江大学博士专业学位论文 ６自适应跟踪参考轨迹实时重规划

其中，－ｏ为给定的由０至ｓ一１阶的起始时刻位置导数，Ｆｒ为给定的由０至ｓ—ｌ阶的

终止时刻位置导数。

任一以Ｃ＝Ｍ（ｑ，Ｔ）为系数矩阵的多项式样条自身即是严格经过路径点ｑ的唯一
最小控制量轨迹。另外，给定可微函数Ｆ（ｃ，Ｔ）＝Ｆ（Ｍ（ｑ，Ｔ），Ｔ）＝Ｈ（ｑ，Ｔ），只需提供
ＯＦ／Ｏｃ以及ＯＦ／ＯＴ，ＭＩＮＣＯ轨迹类内部可实现以线性复杂度计算出其对｛ｑ，Ｔ）的导
数ＯＨ／Ｏｑ以及ｃｇＨ／ｃＴｒ。

据此，我们使用ＭＩＮＣＯ轨迹类的参数化系数｛ｑ，Ｔ）作为优化变量，自然地消去了
线性动力学模型、起始状态、末端状态的等式约束。

６．３．３使用罚函数处理不等武约束

６．３．３．１外点罚函数处理过程约束

对于式（６．１ｄ）至式（６一ｌｈ）所示的５个泛函类型过程不等式约束，我们首先将其表示为

方便运算的形式，记作如下：

９（ｃ，Ｔ，ｔ）＝９（ｘ（ｚ），ｕ（ｔ），ｔ）

忪（圳；一ｖｍ。２

ａｌ＇ｃｃｏｓ（ｅｊ冗舭ｔ（ｑ（ｔ））ｅ３）一％。 （６－５）
１１６ｕ（ｔ）瞻一ｆ２ｌｎ。２ ５ ０，Ⅵ∈【０，Ｔｆ】．

（，（ｔ）一厶）２一露

ｒｔｄ—ｆｖｚｓ－ｒ（ｐ（ｔ）１

其中厶＝（，竹。＋，ｍ讯）１２，，ｒ＝（，ｍ。一厶ｔ。）／２由推力幅值计算。由平坦输出ｘ（ｔ）以
及ｕ（ｔ）计算倾角、机体角速率、推力等物理参数的方法由第２．２．２节的微分平坦变化霍
给出。之后，我们借鉴Ｔｅｏ等人【１０８，１０９］方法的思想，对约束函数式（６．５）在离散的时刻点

进行采样，对违背约束的部分进行积分，即使用采样点的加权和来构建罚函数。考虑到

优化过程中轨迹时长发生变化，为了保证罚函数采样转化的精度，我们对分段多项式中

的每一段分别采取固定时问间隔采样，则这些泛函约束的计算在每一段轨迹边之间相互

独立，即９（ｃ‘，正，ｔ），ｔ∈【０，正１仅涉及到ｃｉ以及正。

对于某一多项式系数以及时长为｛ｃ‘，正】．的轨迹边，对式（６．５）中的其中一个维度０，

万方数据
 浙江大学博士专业学位论文 ６自适应跟踪参考轨迹实时重规划

我们记在ｔ时刻对泛函采样得到的函数值为

亿（ｃｉ，正，ｔ）＝ｉｎ８２（［ｇｏ（Ｃｉ，正，￡），０］ （６－６）

令采样间隔为６，则对于第ｉ∈｛１…．，Ｍ）段轨迹边，维度Ｏ所对应的约束转化成的罚
函数代价指标计算如下：

工（ｃ。，互，６）＝九０∑屿Ｒ（ｃｔ，互，Ｊ６）＋
ｊ２０
（６－７）

；（ｒ，－ｈａ）［Ｐｏ（ｃｉｔ删圳％啪）］），ｎ＝阱
其中ｋ为泛函约束向量乡中第Ｏ维度的惩罚因子，６为采样时间间隔，礼＝ｌ正／副为采
样点个数，（Ｗ０，Ｗ１，…，哟一１，％）＝（１／２，１，…，１，１／２）为梯形求积法则中的系数。虽然
整数礼随着正的变化而改变，但是此种固定时间间隔采样得到的代价指标函数关于变

量正是连续的，证明见附录Ａ，满足非光滑优化的拟牛顿、法【１３９１求解的前提要求。

６．３．３．２内点罚函数考虑拓扑连贯性

对于式（６－１ｉ）所示的拓扑连贯约束，由于待优化轨迹的位置关于优化变量是连续变

化的，若我们保证拟牛顿法中每一轮线搜索的结果轨迹都与上一轮迭代的结果互为７Ｉ．拓

扑连贯关系，则最终得到的轨迹与给定的初值轨迹保有７－．拓扑连贯关系。对此，我们分

别计算每一轮线搜索的最大步长（在第６．３．５．１节详述），使得在此最大步长内的变化都

满足下．拓扑连贯。

考虑到给定的初值轨迹为无碰撞轨迹，我们构建对数障碍函数（ＬｏｇａｒｉｔｈｍｉｃＢａｒｒｉｅｒ

Ｆｕｎｃｔｉｏｎ），使用内点罚函数法将拓扑连贯约束转移到代价指标上。对于某一多项式系数

以及时长为｛ｃｉ，正｝的轨迹边，我们记在ｔ时刻采样得到的对数障碍函数值为

№剐）＝－崦（型岛笋型）， 洚８，

其中，／孤ｓｒ（）为计算某一位置点与地图中障碍物最近距离的函数，由梯度距离场地图

获得，ｄｔ。ｆ为障碍函数开始作用的距离容许参数。

令６为采样间隔，下为拓扑连贯参数，我们首先找到ｔ＝丁时刻对应的轨迹段，记

为叫，则对于第ｔ∈｛ｌ，…，伽）段轨迹边，拓扑连贯约束转化成的罚函数代价指标计算

万方数据
 浙江大学博士专业学位论文 ６自适应跟踪参考轨迹实时重规划

玩（ｃｔ，互，６）＝Ａ＾（６∑岣魄（ｃｔ，互，Ｊ６）＋三（Ｔ一礼Ｊ）［玩（ｃｔ，乃，礼６）＋Ｂｈ（ｃｔ，正，可）］）
１＝０

ｎ＝Ｉ吾Ｉ，Ｔ－－＜卜墨‰鬻』’卅－ｌ
（６－９）

其中，Ａ＾为此项的惩罚因子，礼为采样点个数。

此代价项的含义为，在优化轨迹的位置连续变化时，当其与障碍物的最近距离小于

也０２时，此项代价就变得接近无穷大，保证了优化时不发生拓扑跳变，从而保证与初始

轨迹７－．拓扑连贯。

６．３．４无约束优化问题及梯度推导

至此，通过使用轨迹类表示优化变量消除等式约束，以及使用罚函数消除不等式约

束、式（６—１）的约束优化问题转化为了无约束优化问题，如下：

．ｍｉｎ』、ｆｏ。Ｔ…Ｍ一（知Ｔｕ（ｚ，）疵
Ｍ ５ Ｍ 、
（６－１０ａ）

＋肼（乃一霉。，）２＋∑玩（ｃｔ，正，ｄ）＋∑∑玩（ｃｔ，正，６）｝．
ｉ＝１ ｏ＝１ ｉ＝１ Ｊ
（６—１０ｂ）

我们接下来根据链式法则推导式（６－１０）中各项对｛ｃ，Ｔ】－的导数，之后由ＭＩＮＣＯ轨
迹类求得其对优化变量｛ｑ，Ｔ）的导数。
１．对于式（６－ｌＯａ）的控制量代价项，

Ｑ酬ｃ砜
？甜Ｍ（知～，ｄｒ＝》Ｍ 睁Ⅲ

警－Ｑｃ州（玑等＝扣酬（啦
２．对于轨迹时长相关的代价项西＝Ｐｒ（乃一乃。，）２，

警嘞（乃也，）． （６－１２）

３．对于拓扑连贯项巩，我们只需计算采样点处罚函数值大于０时的梯度，则其相

万方数据
 浙江大学博士专业学位论文 ６自适应跟踪参考轨迹实时重规划

鼍砘（６喜屿掣＋互１ ｃ～∞［掣＋掣］），
对Ｃｔ的导数为

篓砘Ｇ１ｒＬＢ螂∥Ｍ（啪，吵咖“…一１． （６－１４）

篆一ｎ（狮ｃｔ＇咖叶Ｂｈ（ｃｉ’”啪吲１Ｉ．…¨小肛叫，ｗ”’

鼍砘（６壹ｊ＝Ｏ岣掣＋互１ ｃ正删［掣＋掣］），
甏咄（三瓯（啪删＋纰忍正）】），
（６－１５）

其中藏（ｃｉ，正，ｔ）ｌａｃｉ对于式（６－５）的各维度不同，下面分别推导各维度对｛ｐ，ｔ，，ｇ，ｕ，ｎ
的导数，而｛ｐ，ｔ，，ｑ，ｕ，．厂】．＝皿ｐ【３１），则｛ｐ，御，口，ｕ，，）对平坦输出的导数可根据第２．２．２节
的微分平坦变化关系式皿经自动微分１求得。最终（３０（ｃｉ，正，ｔ）／Ｏｃｉ可由链式法则推得。

嬲：２毗ｔ），（６－１６）ｔ
Ｉ）针对式（６－５）中第一维速度幅值约束，

一。”。’
粕（ｑ，）

Ｆ丽丽－２
２）针对第二维倾角幅值约束，根据式（２－１３）中冗哗ｔ（ｑ）的形式，可推导得

鬻２ 忻钆…Ｔ，
ｌ石而２了ｉ：彳ｉ写盂：夏ｉ丽。铷’一吼＇一啦’船’＇ ｃ纠７）
‘６。１７’

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

缫：２ｕ（拱￡），（６－１８）Ｉ
３）针对弟二维机体角逮翠梗的幅僵约采，

～ｒ川Ｖ’
舢（世，ｔ）
４）针对第四维推力幅值约束，

紫筹＝２（肼㈡一，ｍ）， （６－１９）

５）针对第五维障碍物约束，

篙掣ｔ一蒜ｔ．
ａｐ（ｃ｛，） 卸（ｑ，）‘
睁２。，
…一’

至此，我们给出了式（６－ｌｏ）无约束优化问题的性能指标对优化变量｛ｑ，Ｔ）的梯度表
示形式．之后可用描席下降法求解。

６．３．５求解过程

对式（６．１０）的问题我们采用Ｌｅｗｉｓ等人１１３９１提出的处理非光滑优化的拟牛顿法求解，

其本质为Ｌ－ＢＦＧＳ算法，此处给出其主要步骤的伪代码流程如算法１３所示。关于Ｌ．ＢＦＧＳ

算法的详细信息可参见Ｎｏｅｅｄａｌ的文章【１４０ｌ，本文不再介绍，下面我们描述考虑拓扑连

贯性的线搜索最大步长计算方法，以及线搜索方法。

算法１３：拟牛顿法流程
Ｉｎｐｕｔ：轨迹初值墨。／ｔ

Ｏｕｔｐｕｔ：优化结果ｒ

１ ｗｈｉｌｅ停止条件未满足ｄ０
ｌ

２
ｌ计算线搜索最大步长；
ｌ
３ ｌ线搜索；
ｌ
４ ｌ更新Ｈｅｓｓｉａｎ信息；

６．３．５．１考虑拓扑连贯性的最大线搜索步长计算

对于某一个线搜索子问题，我们记以下３个条件并给出１个命题。

条件１：步长对应的轨迹与线搜索初值轨迹‰。互为丁一拓扑连贯关系。

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

条件２：Ａｒｍｉｊｏ准则［１４１１。

条件３：Ｗｏｌｆｅ准则【１４２１。

命题１．根据式（６－１０）中的性能指标，对于某一次线搜索，若线搜索的初值轨迹不为局部

极小，并且其与障碍物的最近距离大于障碍函数开始作用的距离容许参数ｄｔｏｌ，则满足

条件Ｊ且不满足条件２的步长一定存在。

证明．初值轨迹ｘ。娃与障碍物的最近距离大于障碍函数开始作用的距离容许参数也ｄ，

对数障碍惩罚项为０，由于线搜索时随着步长连续变化轨迹连续形变，则在此拓扑内的

轨迹均与Ｋ喇。互为丁．拓扑连贯关系，当步长增长至某一定值，轨迹形变至与障碍物充

分接近时，对数障碍惩罚项接近无穷大，此时轨迹不满足Ａｒｍｉｊｏ准则，此步长满足条

件１且不满足条件２。 口

据此，考虑拓扑连贯性的步长计算即为寻找一个最大线搜索步长，使得此步长下的

轨迹满足与线搜索初值轨迹Ｘ州。互为丁一拓扑连贯关系，但不满足Ａｒｍｉｊｏ准则［１４１１。

算法流程如算法１４所示。给定某一次线搜索的初始轨迹ｘ。ｉｔ＇梯度反方向ｄ，以及

拓扑连贯参数丁，将最大步长ｍａｘ 初始化为１，步长下界
ｐ ｅｔｓ．，为化 始
ｌｏｗｅｒ 初
ｂｏｕｎｄ ０

上界ｕｐｐｅｒ ｂｏｕｎｄ初始化为无穷大（第ｌ行）。在每一轮迭代中，我们计算当前步长下的

轨迹ｘｔ。。（第３行），若此轨迹不满足与初值轨迹Ｘ衲托互为７－一拓扑连贯关系（第４行），

则说明当前步长过大，将ｕｐｐｅｒ ｄｎｕｏｂ＿贯连扑拓一－７足满若；）行第（长步前当为置设 ５

关系，但不满足Ａｒｍｉｊｏ准则（第６行），则说明当前步长下的轨迹经初值轨迹连续形变

够充分接近障碍物但没有改变拓扑，此时找到了一个符合条件的最大步长，退出迭代

（第４行）；若满足７－．拓扑连贯关系，且满足Ａｒｍｉｊｏ准则（第８行），则说明当前步长过小，

将ｌｏｗｅｒ ｂｏｕｎｄ设置为当前步长（第９行）。之后，若初始步长下的轨迹已不满足７－．拓扑

连贯关系（第１０行），则更新当前步长为ｌｏｗｅｒ ｂｏｕｎｄ与ｕｐｐｅｒ ｂｏｕｎｄ的中值（第１１０），

否则更新为初始步长的２．１倍（第１３行），继续下一次迭代，直到找到符合条件的步长

或达到迭代次数限制（第２行）。

由此，我们找到满足条件１且不满足条件２的最大步长，此最大步长将作为之后非

精确线搜索的步长上界。图６．６展示了计算最大线搜索步长的示意图。在此次线搜索中，

初始轨迹为７ｒｉ。ｎ（ｔ），颜色渐淡的轨迹表示在梯度反方向上随步长增加的轨迹连续形变

过程。经过步长ｓｔｅｐ２得到轨迹几卸２（ｔ），其与障碍物的最近距离小于障碍函数开始作

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

算法１４：计算线搜索最大步长
Ｉｎｐｕｔ：轨迹初值ｘ捌。，梯度反方向ｄ，拓扑连贯参数７－

Ｏｕｔｐｕｔ：最大步长ｍａｘ＿ｓｔｅｐ
１
ｍａｘ—ｓｔｅｐ÷＿１，步长下界ｌｏｗｅｒ—ｂｏｕｎｄ＋－－Ｏ，步长上界ｕｐｐｅｒ＿ｂｏｕｎｄ÷－＋ｏ。；
２ ｗｈｉｌｅ不到最大迭代次数限制ｄｏ

３
ｘｔ啪ｐ÷一Ｘｉｎｎ＋ｄ率ｍａｘ＿ｓｔｅｐ；
４ ｉｆ ＴｏｐｏＣｏｎｓｉｓｔｅｎｃｙ（ｘｉ嘶ｔ，ｘｔｅ＂ｔｐ，砂ｆａｉｌｓ ｔｈｅｎ

５
Ｌ啪ｅｒ－６。ｕｎｄ＋－ｍａｘ＿ｓｔｅｐ；
６ ｅｌｓｅ ｉｆ
Ａｒｍｉｊｏ伍佣ｔ，ｘ￡。一知ｉ／ｓ ｔｈｅｎ
７
Ｌ ｂｒｅａｋ；

８ ｅｌｓｅ

９
Ｌ ｆ～ｒ—ｂｏｕｎｄ‘－ｍ。ｕ姚
ｍ ｉｆ ｕｐｐｅｒ＿ｂｏｕｎｄ＜＋ｏｏ ｔｈｅｎ

ｎ
Ｌ ｍ。ｔ耵ｅｐ＋－（１０ｗｅ＿。札礼ｄ＋卿ｅｒ＿ｂｏｕ几ｄ）／２；
ｕ ｅｌｓｅ

ｎ
Ｌ ｍ吼就ｅｐ＋－２“ｍｎｚｊ卸；

用的容许距离（红圈所示处），轨迹代价趋于无穷大，但仍与７ｒｉ砒（ｚ）互为７＿－拓扑连贯关

系，此步长符合条件。

６．３．５．２连续非光滑函数的非精确线搜索

在此线搜索中，我们以上节求得的最大步长为上界，寻找一个满足条件２且满足条

件３的步长。Ｌｅｗｉｓ等人［１３９］的工作证明了当式（６．１０）中的性能指标函数连续且有下确界

时，此步长一定存在。

算法流程如算法１５所示，给定某一次线搜索的初始轨迹均。加梯度反方向ｄ，以及

最大步长ｍａｘ ｓｔｅｐ，将当前步长ｓｔｅｐ＿ｓｉｚｅ初始化为１，步长下界１０１ｌＪｅｒ—ｂｏｕｎｄ初始化

为Ｏ，上界ｕｐｐｅｒ ｄｎ初始化为
ｕｏｂ＿当算计们我，中代ｍａｘ
迭轮一每在ｓｔｅｐ
。）行１第（

前步长下的轨迹Ｘｔ。。（第３行），若此轨迹不满足Ａｒｒｏｙｏ准则（第４行），则说明当前步

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

长过大，将ｕｐｐｅＫ ｂｏｕｎｄ设置为当前步长（第５行）；若满足满足Ａｒｍｉｊｏ准则，但不满

足Ｗｏｌｆｅ准则（第７行），则说明当前步长下的轨迹代价下降不够充分，将ｌｏｗｅｒ ｂｏｕｎｄ

设置为当前步长（第８行）；若满足Ａｒｍｉｊｏ准则，且满足Ｗｏｌｆｅ准则（第９行），则此时

找到了一个符合条件的最大步长，退出迭代（第４行）。之后，若初始步长下的轨迹已

不满足Ａｒｍｉｊｏ准则（第１１行），则更新当前步长为ｌｏｗｅｒ ｂｏｕｎｄ与ｕｐｐｅｒ ｂｏｕｎｄ的中值

（第１２行），否则更新为初始步长的２．１倍（第１４行），继续下一次迭代，直到找到符合条

件的步长或达到迭代次数限制（第２行）。

６．４仿真导航测试及结果分析

我们将所提出的自适应跟踪参考轨迹规划算法应用于高度仿真的四旋翼飞行器，在

如图６．７所示的约１００ｍ×１００ｍ×４ｍ规模的大型地下车库场景进行轨迹跟踪的模拟导

航飞行测试，相关仿真导航视频可在链接２中找到。飞行器上搭载３２线模拟激光雷达，

垂直视场角为９００，水平视场角为３６００，角度分辨率０．３５０，我们融合当前的激光点云观

测信息与里程计信息实时更新局部占据栅格地图，局部建图的范围为３２ｍ ｘ ３２ｍ×８ｍ，

分辨率０．２５ｍ，以飞行器为中心。之后将此局部占据栅格地图的占据部分在水平方向膨

胀２个分辨率单位，再转换为梯度距离场地图，局部地图更新频率为１０Ｈｚ。全局参考

轨迹最高速度为６ｍ／ｓ，如图６．７深蓝色曲线所示，可以看到参考轨迹在许多地方与障碍
物发生碰撞，我们进行实时建图与重规划，跟踪参考轨迹上对应时刻的位置，同时避开

障碍物，且满足飞行器的各个物理性能限制。实验的传感器模拟、建图、规划、控制等

２ｈｔｔｐｓ：／／ｗｗｗ．ｊｉａｎｇｕｏｙｕｎ．ｃｏｍ／ｐ／ＤＶａＷｒＦＹＱＩＮ６ｕＣｈｊＨｊ７ｋＥＩＡＡ

万方数据
 浙江大学博士专业学位论文 ６自适应跟踪参考轨迹实时重规划

算法１５：连续非光滑函数的非精确线搜索
Ｉｎｐｕｔ：轨迹初值Ｘ概ｔ，梯度反方向ｄ，最大步长ｍａｘ＿ｓｔｅｐ

Ｏｕｔｐｕｔ：合适步长ｓｔｅｐ＿ｓｉｚｅ
１ ｓｔｅｐ＿ｓｉｚｅ＋．１，步长下界ｌｏｗｅｒ—ｂｏｕｎｄ＋＿０，步长上界

ｕｐｐｅｒ＿ｂｏｕｎｄ＋＿ｍａｘ＿ｓｔｅｐ，ｂｒａｃｋｅｔ＋＿Ｆａｌｓｅ；

２ ｗｈｉｌｅ不到最大迭代次数限制ｄｏ

３ ｘｔｅ竹甲七一ｘｉｎｎ＋ｄ木ｓｔｅｐ＿ｓｉｚｅ；

僻ｕｐｐｅ胁ｒ＿ｂｏ—ｕｎｄ＋＝ｓ－吣溉；
５

８
Ｌ ｌ删ｅｒ—ｂｏｕ孔ｄ÷－毗”魄；
９ ｅｌｓｅ

０
Ｌ ｂｒｅａｋ；

ｌ ｉｆ ｂｒａｃｋｅｔ＝＝ｍｅ ｔｈｅｎ
２
Ｌ ｓｔｅｐ．ｓ协，－（１０ｗｅｒ＿６㈣ｄ＋唧ｅｒ＿ｂｏｕ礼ｄ）／２；
３ ｅｌｓｅ

４
Ｌ ｓｔｅ疋毗ｅ÷Ｉ ２．１年ｓｔｅｐ＿毗ｅ；

所有模块均在一台含英特尔ｉ７．１０７５０Ｈ处理器，主频为２．６ ＧＨｚ的计算机上进行，规划

算法采用的各个参数数值见表６．１，另外拟牛顿法迭代的收敛判断条件设置为整体性能

代价的变化率小于ｌｅ一４。

每次重规划初始时刻状态的选取如第２．４．１节所述。对于终止时刻状态，我们首先选

取全局参考轨迹上在当前时刻往后耳。，的状态点作为前端规划的终止时刻状态，若此

状态点的位置在障碍物内，则向后顺延直至找到不碰撞的状态点。若之前一次重规划的

结果轨迹与参考轨迹之间不为丁．拓扑连贯关系，则说明当前需要考虑避障，此时使用

７－．拓扑连贯生长树算法，得到一条与上一次重规划的结果轨迹互为７＿．拓扑连贯的初值轨

迹，否则使用７－．拓扑连贯连接图算法，得到一条或多条初始轨迹，我们选取代价最小的

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

一条，初始轨迹求解的时间阈度设定为１５ｍｓ。之后，将此初值轨迹给到自适应跟踪轨

迹优化算法，自适应地优化约束在参考轨迹上的终止时刻状态，得到满足所有约束的局

部重规划轨迹，给到控制器跟踪此局部轨迹。

我们首先验证所提出的自适应重规划算法在跟踪参考轨迹上的性能提升，如图６．７所

示，红色箭头组成的路线为使用所提出的自适应跟踪算法完整跟踪参考轨迹后飞行器所

实际经过的路线，灰色箭头组成的路线则由将每次重规划的末状态固定的方法得到，没

有考虑自适应性，深蓝色曲线为全局参考轨迹，从细节图中我们可以发现本章所提方法

在避开障碍物的前提下与参考轨迹贴合地更好。在细节图６．７ａ中，使用自适位跟踪在安

全避障的同时紧贴参考轨迹，而固定末状态的方法则选择了另一方向绕障。图６．８展示

了两种方法在相应时刻与参考轨迹的位置跟踪误差曲线，可以看出本章自适应调节跟踪

末状态点的方法有着明显更小的跟踪误差。表６．２中记录了重规划中轨迹优化的求解耗

时占比，自适应跟踪过程总共进行了７９７次重规划，固定末端点跟踪的方式总共进行了

８４９次重规划。与固定末端点的优化方式相比，自适应跟踪的确增加了少量求解耗时，

但也可以保有１０Ｈｚ的重规划频率，且求解成功率更高。

图６．９展示了连续８个时刻的重规划结果轨迹，分别对应图６．７ｃ中８个箭头所指位置，

此时飞行器从两根立柱中间穿过，接着避开参考轨迹上所经过的障碍物。红色曲线为重

规划结果轨迹，红色箭头为使用本章自适应跟踪方法的飞行器当前位置与速度指向，灰

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

（ａ）路线细节（ａ）处示意图 （ｂ）路线细节（ｂ）处示意图

（ｃ）最终执行路线对比

图６．７是否考虑跟踪自适应性的测试环境与最终执行路线

图６．８参考轨迹跟踪位置误差对比曲线

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

表６．２重规翅ｌ：求解时间表

色箭头为使用固定末状态跟踪方法的飞行器当前位置与速度指向，深蓝色轨迹为全局参

考轨迹，深蓝色箭头为当前时刻所对应的参考轨迹上的位置与速度指向，可以对比发现

本章自适应跟踪方法的实时跟踪误差小得多。

图６．１０展示了完整的跟踪过程中飞行器的实时推力、机体角速率、与水平面倾角、

以及速率曲线，可以发现整个过程中各项物理指标均在所设置的幅值约束之内。

在与上述相同的规划参数设置下，在如图６．１１所示的环境中，我们与不考虑连续重

规划连贯性进行参考轨迹跟踪进行对比。红色箭头组成的路线为使用所提出的拓扑连贯

自适应跟踪算法完整跟踪参考轨迹后飞行器所实际经过的路线，灰色箭头组成的路线则

不考虑拓扑连贯性，深蓝色曲线为全局参考轨迹。从细节图６．１１ａ中，我们看到灰色路线

的方法一开始选择了左侧饶障，之后又交到右侧饶障，犹豫不定，导致最终执行的路线

十分扭曲且与障碍物距离过近，而考虑了连续重规划拓扑连续的红色路线则有着较好的

避障一致性，最终执行的路线十分顺滑。

万方数据
 浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

（ａ）箭头１处重规划结果 （ｂ）箭头２处重规划结果

（ｃ）箭头３处重规划结果 （ｄ）箭头４处重规划结果

（ｅ）箭头５处重规划结果 （Ｏ箭头６处重规划结果

（ｇ）箭头７处重规划结果 （ｈ）箭头８处重规划结果

图６．９重规划结果轨迹

艏
∞

弘
∞
蓬磐掣攀掣磐碧磐妻塑些
瞧三三二二二Ｉ～二～ …。二立二曼一－二…曼～毫一曼曼二ｌ…：曼兰二量二：三

柏
坫

＂
５

回 ２０
３０时间ｌｓ）４０

图６．１０倾角、推力、角速率、速率曲线

１１７

万方数据
浙江大学博士专业学位论文 ６ 自适应跟踪参考轨迹实时重规划

、。。、。鬻、罗冀同一嗣一 ，。 ＿瞄Ｌ
々。／‘７ Ｉ；
Ｙ『一
一 一Ｊ●
羹囊羞，、ｊ。；纛囊。７ｊ ７雾毒一。 一冀曩，■
纂攀摹簿一ｉ攀。强誓磐。一“麓。更ｊ 一叠１。Ｉ ７’矽哆舅参１誊…
《 。
ｉ、

（ａ）轨迹细节（ａ）示意图 （ｂ）轨迹细节（ｂ）示意图

（ｃ）最终执行轨迹对比

图６．１１连续重规划是否考虑拓扑连贯性对比测试环境

６．５本章小结

本章探究了未知环境下对全局参考轨迹的时空跟踪问题。我们总结主要贡献如下：

１．考虑到连续重规划间轨迹的路径拓扑不一致，避障时出现“摇摆犹豫”的现象，导致

飞行器与障碍物过分接近，增加了碰撞的风险，我们定义了轨迹间的７－．拓扑连贯关系，

并提出考虑拓扑连贯性的采样类动力学规划算法，增加了连续重规划之间的路径拓扑一

致性，使得整体导航过程中避障时轨迹更加顺滑，更加安全。２．考虑到时空上贴合参考

轨迹与避开障碍物等具有冲突性质的约束，我们提出自适应末端状态的重规划轨迹优

化方法，使用轨迹类、罚函数等方法消除约束，使用梯度下降法求解。在线搜索时根据

７－一拓扑连贯关系计算最大步长，保持优化的结果轨迹与初值轨迹处于同一拓扑路径。与

传统固定末状态的方法相比，所提出方法在满足避障约束的同时大大缩小了实时位置跟

踪误差。３．在接近真实的仿真系统中验证了所提出的轨迹跟踪算法。

万方数据
 浙江大学博士专业学位论文 ７总结与展望

７总结与展望

７．１全文工作总结

本文以多旋翼飞行器在复杂环境下的自主导航为背景，主要针对其在全局规划、局

部规划、跟踪规划三大类情形下的运动规划问题，详细分析了侧重点及难点，并提出相

应的解决方案，全文工作总结如下：

１．对于已知精确环境下导航的全局规划情形，我们重点考虑了算法的全局最优性和

快速收敛性。我们提出时空形变轨迹树算法，整体上使用在采样类动力学规划方法中深

度结合局部优化的方法来求解。我们将树生长时轨迹边的构建建模为末状态部分受约束

的两点边值问题，使用约束松弛策略加速求解，从而能够快速探索全局解空间。我们设

计以最小优化单元表示的轨迹树整体质量性能指标，在时间和空间层面优化不同单元的

组合，花费尽量小的代价高效提升整体轨迹树的质量，从而极大加速了算法的收敛性。

２．对于未知环境导航中的局部实时重规划情形，我们重点考虑了飞行器的运动初始

状态以及算法的实时求解性，使用采样类前端动力学运动规划衔接高效后端提优的层级

式规划框架求解。我们构建近似拓扑图引导在状态空闻的采样，提升了采样类动力学运

动规划算法的求解效率，之后提出内层求解具备闭式解形式的双层优化后端提优算法，

高效提升了前端结果轨迹的光滑性和连续性，最后使用全自主四旋翼飞行器在室内外复

杂环境进行了实时重规划的高速导航飞行测试。

３．在层级式规划的基础上，针对在障碍物窄缝等环境中求解低效的问题，我们首先

提出在采样规划中结合双向搜索与快速求解的局部优化的方法，将快速局部优化构建为

一系列的无约束二次凸优化问题，每轮迭代具有闭式解，接着我们在后端提优时将障碍

物信息构建进性能指标中，同时保有二次规划问题的结构，使得保证求解效率的同时成

功率更，由此增强了采样类前端以及后端提优的效率与性能，更好地满足实时重规划的

需求。最后在实物四旋翼飞行器上进行导航测试，在稠密障碍物、类迷宫等环境中验证

了所提出规划方法的有效性。

４．对于未知环境下对全局参考轨迹的时空跟踪规划情形，针对避障时规划结果不连

贯的问题，我们定义了轨迹间的丁．拓扑连贯关系，提出考虑拓扑连贯性的采样类动力学

１１９

万方数据
 浙江大学博士专业学位论文 ７总结与展望

规划算法，增加了连续重规划之间的路径拓扑一致性，使得整体导航过程中避障时轨迹

更加顺滑，更加安全。面对时空上贴合参考轨迹与避障等具有冲突性质的约束，我们提

出自适应末端状态的重规划轨迹优化方法，在安全避障的前提下减小了实时跟踪误差，

并在线搜索时根据丁一拓扑连贯关系计算最大步长，保持优化的结果轨迹与初值轨迹处

于同一拓扑路径。最后在接近真实的仿真导航系统中验证了所提出的轨迹跟踪算法。

７．２将来工作展望

对于多旋翼飞行器自主导航系统，针对运动规划层面，我们认为还可在多个方面展

开研究，包括但不限于以下几点：

１．从系统可靠性的角度，考虑到感知与控制均有误差，可以对考虑不确定性的运动

规划算法开展研究。本文所研究内容需要大致可靠的上游感知和较为稳定的底层控制支

持，然而真实工况中存在感知高度不准确以及部分控制失效的情况，开发考虑感知不确

定性以及运动不确定性的规划算法能够对极端情况下的飞行作业提供一些安全保证。

２．从学科发展趋势的角度，随着大数据时代的到来，各类数据的获取越来越便捷，
硬件计算能力不断提高，可以对数据驱动的运动规划算法开展研究。本文所研究的规划

算法基于控制理论、优化理论等思想，偏向传统学院派的方式，虽然可解释性较强，但

存在调参繁杂、难以考虑周全等缺点，而基于数据的开发能够利用算力提高开发效率，

发现一些单纯靠人工设计难以考虑到的情况，例如使用端到端强化学习的导航规划方法

在限定场景中能够取得好的效果，或是利用以往知识（即数据）对特定情形下的启发式

函数进行精确估计。

３．从应用发展趋势的角度，许多场景对人机协同作业，或是多机协同作业的需求越

来越大，可以对协同规划算法开展研究。本文所研究的算法针对单机规划，能使单智能

体具有高度自主及智能性，对于人在回路或是多智能体的规划研究，则需从人机交互、

行为预测、信患通信、博弈等角度进行思考。

万方数据
 浙江大学博士专业学位论文 参考文献

参考 文献
【ｌ】 Ｒｉｃｈｔｅｒ Ｃ，Ｂｒｙ Ａ，Ｒｏｙ Ｎ．Ｐｏｌｙｎｏｍｉａｌ Ｔｒａｊｅｃｔｏｒｙ Ｐｌａｎｎｉｎｇ ｆｏｒ Ａｇｇｒｅｓｓｉｖｅ Ｑｕａｄｒｏｔｏｒ Ｆｌｉｇｈｔ ｉｎ Ｄｅｎｓｅ
Ｉｎｄｏｏｒ Ｅｎｖｉｒｏｎｍｅｎｔｓ【Ｃ］ｌｌＰｒｏｃ．ｏｆｔｈｅ Ｉｎｔｌ．Ｓｙｍ．ｏｆＲｏｂｏｔ．Ｒｅｓｅａｒｃｈ．２０１３：６４９－６６６．

【２】 Ｂｒｙ Ａ，Ｒｉｃｈｔｅｒ Ｃ，Ｂａｃｈｒａｃｈ Ａ，ｅｔ ａ１．Ａｇｇｒｅｓｓｉｖｅ ｆｌｉｇｈｔ ｏｆｆｉｘｅｄ－ｗｉｎｇ ａｎｄ ｑｕａｄｒｏｔｏｒ ａｉｒｃｒａＲ ｉｎ ｄｅｎｓｅ
ｉｎｄｏｏｒ ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｊ］．Ｉｎｔｌ．Ｊ．Ｒｏｂｏｔ．Ｒｅｓｅａｒｃｈ（ＩＪＲＲ），２０１５，３４（７）：９６９－１００２．

【３】 Ｗａｎｇ Ｚ，Ｚｈｏｕ Ｘ，Ｘｕ Ｃ，ｅｔ ａ１．Ａｌｔｅｒｎａｔｉｎｇ Ｍｉｎｉｍｉｚａｔｉｏｎ Ｂａｓｅｄ Ｔｒａｊｅｃｔｏｒｙ Ｇｅｎｅｒａｔｉｏｎ ｆｏｒ Ｑｕａｄｒｏｔｏｒ
Ａｇｇｒｅｓｓｉｖｅ Ｆｌｉｇｈｔ［Ｊ］．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，２０２０，５（３）：４８３６－４８４３．
【４】 Ｒｔｓｍａｎｎ Ｃ，Ｈｏｆｆｉｎａｎｎ Ｆ，Ｂｅｒｔｒａｍ Ｔ．Ｉｎｔｅｇｒａｔｅｄ ｏｎｌｉｎｅ ｔｒａｊｅｃｔｏｒｙ ｐｌａｎｎｉｎｇ ａｎｄ ｏｐｔｉｍｉｚａｔｉｏｎ ｉｎ ｄｉｓ－
ｔｉｎｃｆｉｖｅｔｏｐｏｌｏｇｉｅｓ［Ｊ］．Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｎｏｍｏｕｓ Ｓｙｓｔｅｍｓ，２０１７，８８：１４２－１５３．ＤＯＩ：ｈｔｔｐｓ：／／ｄｏｉ．ｏｒｇ
／１０．１０１６／ｊ．ｒｏｂｏｔ．２０１６．１ １．００７．
【５】 Ｐｈａｍ Ｑ Ｃ．Ａ Ｇｅｎｅｒａｌ，Ｆａｓｔ，ａｎｄ Ｒｏｂｕｓｔ Ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｏｆｔｈｅ Ｔｉｍｅ－Ｏｐｔｉｍａｌ Ｐａｔｈ Ｐａｒａｍｅｔｅｒｉｚａｔｉｏｎ

Ａｌｇｏｒｉｔｈｍ［Ｊ］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ，２０１４，３０（６）：１５３３－１５４０．ＤＯＩ：１０．１ １０９／ＴＲＯ．２０１４．２

３５１１１３．

０毋 Ｄｏｎａｌｄ Ｂ，Ｘａｖｉｅｒ Ｐ，Ｃａｎｎｙ Ｊ，ｅｔ ａ１．Ｋｉｎｏｄｙｎａｍｉｃ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇｐ］．Ｊｏｕｒｎａｌ ｏｆｔｈｅ ＡＣＭ（ＪＡＣＭ），

１９９３，４０（５）：１０４８－１０６６．
０刀 Ｂｅｌｌｍａｎ Ｒ．Ｏｎ ｔｈｅ Ｔｈｅｏｒｙ ｏｆＤｙｎａｍｉｃ Ｐｒｏｇｒａｍｍｉｎｇ［Ｉ］．Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆｔｈｅ Ｎａｔｉｏｎａｌ Ａｃａｄｅｍｙ ｏｆ
Ｓｃｉｅｎｃｅｓ ｏｆｔｈｅ Ｕｎｉｔｅｄ Ｓｔａｔｅｓ ｏｆＡｍｅｒｉｃａ，１９５２，３８（８）：７１６－７１９．

●观 ＳｖｅｓｔｋａＰ．Ｏｎｐｒｏｂａｂｆｌｉｓｔｉｃｃｏｍｐｌｅｔｅｎｅｓｓａｎｄｅｘｐｅｃｔｅｄｃｏｍｐｌｅｘｉｔｙｆｏｒｐｒｏｂａｂｉｌｉｓｔｉｃｐａｔｈｐｌａｎｎｉｎｇ叨．，
１９９６．
ＬａｖａＵｅ Ｓ Ｍ．Ｐｌａｎｎｉｎｇ ａｌｇｏｒｉｔｈｍｓ［Ｍ］．Ｃａｍｂｒｉｄｇｅ ｔｕｆｆ．ｐｒｅｓｓ．２００６．
●ｎ 观∞ ＫａｖｒａｋｉＬ，Ｓｖｅｓ＆ａＰ，ＬａｔｏｍｂｅＪＣ，ｅｔａｌ．Ｐｒｏｂａｂｉｌｉｓｔｉｃｍａｄｍａｐｓｆｏｒｐａｔｈｐｌａｎｎｉｎｇｉｎｈｉｇｈ－ｄｉｍｅｎｓｉｏｎａｌ
ｃｏｎｆｉｇｕｒａｔｉｏｎ ｓｐａｃｅｓ［Ｊ］．ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓｏｎＲｏｂｏｔｉｃｓａｎｄＡｕｔｏｍａｔｉｏｎ，１９９６，１２（４）：５６６—５８０．ＤＯＩ：
ｌＯ．１１０９／７０．５０８４３９．
ＬａＶａｌｌｅ Ｓ．Ｒａｐｉｄｌｙ－ｅｘｐｌｏｒｉｎｇ ｒａｎｄｏｍ ｔｒｅｅｓ：Ａ ｎｅｗ ｔｏｏｌ ｆｏｒ ｐａｔｈ
ｐｌａｎｎｍｇ阴．，１ ９９８．
口ｎ Ｕ习 ＫａｘａｍａｎＳ，ＦｒａｚｚｏｌｉＥ．Ｓａｍｐｌｉｎｇ－ｂａｓｅｄａｌｇｏｒｉｔｈｍｓｆｏｒｏｐｔｉｍａｌｍｏｔｉｏｎｐｌａｎｎｉｎｇ［Ｊ］．１１１ｅＩｎｔｅｒｎａｔｉｏｎａｌ
Ｊｏｕｍａｌ ｏｆＲｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ，２０１ ｌ，３０：８４６．８９４．
ｎ∞ Ａｒｓｌａｎ Ｏ，Ｔｓｉｏｔｒａｓ Ｐ．Ｕｓｅ ｏｆ ｒｅｌａｘａｔｉｏｎ ｍｅｔｈｏｄｓ ｉｎ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｆｌｇｏｆｉｔｈｎ岱ｆｏｒ ｏｐｔｉｍａｌ ｍｏｔｉｏｎ
ｐｌａｎｎｉｎｇ［Ｃ］／／２０１３ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ０１１ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ．２０１３：２４２１－２４２８．
ＤＯＩ：１０．１】０９／Ｉ（、ＲＡ．２０ｌ ３．６６３０９０６．

［１４］ Ｊａｎ．ｓｏｎ Ｌ，Ｓｃｈｍｃｒｌｉｎｇ Ｅ，Ｃｌａｒｋ Ａ，ｅｔ ａ１．Ｆａｓｔ ｍａｒｃｈｉｎｇ竹∞：Ａ ｆａｓｔ ｍａｒｃｈｉｎｇ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｍｅｔｈｏｄ
ｆｏｒ ｏｐｔｉｍａｌ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｉｎ ｍａｎｙ ｄｉｍｅｎｓｉｏｎｓ［Ｉ］．Ｔｈｅ Ｉｎｔｏｒｎａｔｉｏｎａｌ ＪｏｎｍａＩ
ｏｆＲｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ．
２０１５，３４（７）：８８３－９２１．ＤＯＩ：１０．１１７７，０２７８３６４９１５５７７９５８．
［１５］ Ｓｎｉｅｄｏｖｉｃｈ Ｍ．ＤｉｊｋｓＵ＇ａ’Ｓ ａｌｇｏｒｉｔｈｍ ｒｅｖｉｓｉｔｅｄ：ｔｈｅ ｄｙｎａｍｉｃ ｐｒｏｇｒａｍｍｉｎｇ ｃｏｎｎｅｘｉｏｎ［Ｊ］．Ｃｏｎｔｒｏｌ ａｎｄ
Ｃｙｂｅｒｎｅｔｉｃｓ。２００６。３５：５９９－６２０．
［１６１ Ｄｉｊｋｓ廿ａ Ｅ Ｗ，ｃｔ ａ１．Ａ ｎｏｔｅ ｏｎ ｔｗｏ ｐｒｏｂｌｅｍｓ ｉｎ ｃｏｎｎｅｘｉｏｎ ｗｉｔｈ ｇｒａｐｈｓ［Ｊ］．Ｎｕｍｃｒｉｓｃｈｅ ｍａｔｈｅｍａｔｉｋ，
１９５９，ｌ（１）：２６９－２７１．
【１７１ Ｃｒａｍｍｅｌｌ Ｊ Ｄ，Ｓｒｉｎｉｖａｓａ Ｓ Ｓ，Ｂａｒｆｏｏｔ Ｔ Ｄ．Ｂａｔｃｈ Ｉｎｆｏｒｍｅｄ Ｔｒｅｅｓ唧Ｔ＋）：Ｓａｍｐｌｉｎｇ－ｂａｓｅｄ
ｏｐｔｉｍａｌ
ｐｌａｎｎｉｎｇ ｖｉａ ｔｈｅ ｈｅｕｒｉｓｔｉｃａｌｌｙ ｇｕｉｄｅｄ ｓｅａｒｃｈ ｏｆ ｉｍｐｌｉｃｉｔ ｒａｎｄｏｍ ｇｅｏｍｅｔｒｉｃ ｇｒａｐｈｓ［Ｃ］／／２０１５ ＩＥＥＥ
Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ）．２０１５：３０６７．３０７４．ＤＯＩ：ｌＯ．１ １０９ＨＣ
ＲＡ．２０１５．７１３９６２０．

［１ ８】Ｇ－ａｍｍｅｌｌ Ｊ Ｄ，Ｂａｒｆｏｏｔ Ｔ Ｄ，Ｓｒｉｎｉｖａｓａ Ｓ Ｓ．Ｂａｔｃｈ Ｉｎｆｏｒｍｅｄ Ｔｒｅｅｓ∞ＩＴ＋）：Ｉｎｆｏｒｍｅｄ ａｓｙｍｐｔｏｔｉｃａｌｌｙ

ｏｐｔｉｍａｌ ａｎｙｔｉｍｅ ｓｅａｒｃｈ［Ｊ］．Ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆ Ｒｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ，２０２０，３９（５）：５４３・５６７．
ＤＯＩ：１０．１１７７／０２７８３６４９１９８９０３９６．
［１９］ Ｋｏｅｎｉｇ Ｓ，Ｌｉｋｈａｃｈｅｖ Ｍ，Ｆｕｒｃｙ Ｄ．Ｌｉ危Ｉｏｎｇ ｐｌａｎｎｉｎｇ Ａ＋阴．Ａｒｔｉｆｉｃｉａｌ Ｉｎｔｅｌｌｉｇｅｎｃｅ，２００４，１５５（１－２）：
９３．１４６．

［２０】 ＫｕｆｆｎｅｒＪ，ＬａＶａｌｌｅＳ．ＲＲＴ－ｃｏｎｎｅｃｔ：Ａｎｅｆｆｉｃｉｅｎｔａｐｐｒｏａｃｈｔｏｓｉｎｇｌｅ－ｑｕｅｒｙｐａｔｈｐｌａｎｎｉｎｇ［Ｃ］／／Ｐｒｏｃｅｅｄｉｎｇｓ
２０００ ＩＣＲＡ．Ｍｉｌｌｅｎｎｉｕｍ Ｃｏｎｆｅｒｅｎｃｅ．ＩＥＥＥ Ｉｎｔｅｍａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ．

Ｓｙｍｐｏｓｉａ Ｐｒｏｃｅｅｄｉｎｇｓ化ａｔ．Ｎｏ．Ｈ３７０６５）：ｖ０１．２．２０００：９９５－１ ００１ ｖ０１．２．ＤＯＩ：１０．１ １０９／ＲＯＢＯＴ

．２０００．８４４７３０．

１２１

万方数据
 浙江大学博士专业学位论文 参考文献

ｆ２１１ Ｃｈｅｎｇ Ａ，Ｓａｘｅｎａ Ｄ Ｍ，Ｌｉｋｈａｃｈｅｖ Ｍ．Ｂｉｄｉｒｅｃｔｉｏｎａｌ Ｈｅｕｒｉｓｔｉｃ Ｓｅａｒｃｈ ｆｏｒ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ ｗｉｔｈ ａｌｌ
Ｅｘｔｅｎｄ Ｏｐｅｒａｔｏｒ［Ｃ］／／２０１９ ＩＥＥ朗ｔＳＪ
Ｉｎｔｅｍａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ．
ＩＲＯＳ ２０１９，Ｍａｃａｕ，ＳＡＲ，Ｃｈｉｎａ，Ｎｏｖｅｍｂｅｒ ３－８，２０１９．ＩＥＥＥ。２０１９：７４２５—７４３０．ＤＯＩ：ｌＯ．１ １０９／ＩＲｏ
ｓ４０８９７．２０１９．８９６７７９６．

［２２】 日ｅｍｌ＇ｎ Ｓ，Ｏｂｅｒｌ｛ｉｎｄｅｒ Ｊ，Ｈｃｒｍａｎｎ Ａ，ｅｔ ａ１．ｍＨ口．Ｃｏｎｎｅｃｔ：Ｆａｓｔｅｒ，ａｓｙｍｐｔｏｔｉｃａｌｌｙ ｏｐｔｉｍａｌ ｍｏｔｉｏｎ

ｐｌａｎｎｉｎｇ［Ｃ］／／２０１５ ＩＥＥＥ Ｉｎｔｅｍａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ｂｉｏｍｉｍｅｔｉｃｓ（ＲＯＢｌ０１．２０１５：
１６７０—１６７７．ＤＯＩ：１０．１１０９／ＲＯＢＩｏ．２０１５．７４１９０１２．

［２３】 陈瑛．航天器交会与接近操作采样运动规划与控制［Ｄ］．哈尔滨工业大学，２０２０．
［２４】 Ｂｌｏｃｈｌｉｇｅｒ Ｆ，Ｆｅｈｒ Ｍ，Ｄｙｒａｃｚｙｋ Ｍ，ｃｔ ａ１．Ｔｏｐｏｍａｐ：Ｔｏｐｏｌｏｇｉｃａｉ Ｍａｐｐｉｎｇ ａｎｄ Ｎａｖｉｇａｔｉｏｎ Ｂａｓｅｄ ｏｎ
ＶｉｓｕａｌＳＬＡＭＭａｐｓ［Ｃ］／／２０１８ＩＥＥＥＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｆｅｎｃｅｏｎＲｏｂｏｔｉｃｓａｎｄＡｕｔｏｍａｔｉｏｎ（ＩＣＲＡ）．
２０１８：３８１８－３８２５．ＤＯｌ：１０．１１０９／ＩＣＲＡ．２０１８．８４６０６４１．

［２５】 Ｃｏｖｅｒ Ｈ，Ｃｈｏｕｄｈｕｒｙ Ｓ，Ｓｃｈｃｒｃｒ Ｓ，ｅｔ ａ１．Ｓｐａｒｓｅ Ｔａｎｇｅｎｔｉａｌ Ｎｅｔｗｏｒｋ（ＳＰＡＲＴＡＮ）：Ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ

ｆｏｒ ｍｉｃｒｏ ａｅｒｉａｌ ｖｅｈｉｅｌｅｓ［Ｃ］／／Ｐｒｏｃ．ｏｆ也ｅ ＩＥＥＥ Ｉｎｔｌ．Ｃｏｎ￡ｏｎ Ｒｏｂｏｔ．ａｎｄ Ａｕｔｏｍ．２０１３：２８２０．２８２５．

【２６】 ＯｌｅＴｎｉｋｏｖａ Ｈ，Ｔａｙｌｏｒ Ｚ，Ｓｉｅｇｗａｒｔ Ｒ，ｅｔ ａ１．Ｓｐａｒｓｅ ３Ｄ Ｔｏｐｏｌｏｇｉｃａｌ Ｇｒａｐｈｓ ｆｏｒ Ｍｉｃｒｏ－Ａｅｒｉａｌ Ｖｅｈｉｃｌｅ
Ｐｌａｎｎｉｎｇ［Ｃｌ／／Ｐｒｏｃ．ｏｆｔｈｅ ＩＥ］卧ＳＪ
Ｉｎｔｌ．Ｃｏｎｆ ｏｎ Ｉｎｔｅｌｌ．Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔ．２０１８：１．９．
ｆ２７１ Ｙｅ Ｈ。Ｚｈｏｕ Ｘ。Ｗ缸ｇ Ｚ，ｅｔ ａ１．ＴＧＫＩＰｌａｎｎｅｒ：Ａｎ Ｅ伍ｃｉｅｎｔ Ｔｏｐｏｌｏｇｙ Ｇｕｉｄｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｅｒ
ｆｏｒ Ａｕｔｏｎｏｍｏｕｓ Ｑｕａｄｒｏｔｏｒｓ［／］．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，２０２１，６（２）：４９４－５０１．ＤＯＩ：
ｌＯ．１１０９几ＲＡ．２０２０．３０４７７９８．
【２８】 Ｚｕｃｋｅｒ Ｍ，Ｋｕｆｆａｌｅｒ Ｊ，Ｂａｇｎｅｌｌ Ｊ Ａ．Ａｄａｐｔｉｖｅ ｗｏｒｋｓｐａｃｅ ｂｉａｓｉｎｇ ｆｏｒ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｐｌａｎｎｅｒｓ［Ｃ］／／
２００８ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ．２００８：３７５７．３７６２．

『２９］ Ａｒｓｌａｎ Ｏ。Ｔｓｉｏｔｒａｓ Ｐ．Ｍａｃｈｉｎｅ ｌｅａｒｎｉｎｇ ｇｕｉｄｅｄ ｅｘｐｌｏｒａｔｉｏｎ ｆｏｒ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ａ１．

ｇｏｄｔｈｍｓ［Ｃ］／／２０１５皿醐ｔＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ（ｍｏｓ）．

２０１ ５：２６４６．２６５２．

［３０】 Ｚｈａｎｇ Ｃ，Ｈｕｈ Ｊ，Ｌｅｅ Ｄ Ｄ．Ｌｅａｒｎｉｎｇ Ｉｍｐｌｉｃｉｔ Ｓａｍｐｌｉｎｇ Ｄｉｓｔｒｉｂｕｔｉｏｎｓ ｆｏｒ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ［Ｃ］／／２０１８

ｍＥＢＩＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ（ＩＲｏＳ）．２０１８：３６５４—３６６１．

ＤＯＩ：１０．１１０９／ＩＲＯＳ．２０１８．８５９４０２８．
ｆ３ １１ Ｉｃｈｔｅｒ Ｂ，Ｓｅｈｍｅｒｌｉｎｇ Ｅ。Ｌｅｅ Ｔ Ｗ Ｅ，ｅｔ ａ１．Ｌｅａｒｎｅｄ Ｃｒｉｔｉｃａｌ Ｐｒｏｂａｂｉｌｉｓｔｉｃ Ｒｏａｄｍａｐｓ ｆｏｒ Ｒｏｂｏｔｉｃ Ｍｏ．
ｔｉｏｎ Ｐｌａｎｎｉｎｇ［Ｃ］／／２０２０ ｍ髓Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ）．２０２０：
９５３５．９５４１．ＤＯＩ：１０．１１０９／ＩＣＲＡ４０９４５．２０２０．９１９７１０６．
【３２】 Ｃｈｅｎｇ＆Ｓｈａｎｋａｒ】（＇Ｂｕｒｄｉｃｋ Ｊ Ｗ．Ｌｅａｒｎｉｎｇ ａｎ Ｏｐｔｉｍａｌ Ｓａｍｐｌｉｎｇ Ｄｉｓｔｒｉｂｕｔｉｏｎ ｆｏｒ Ｅ伍ｃｉｅｎｔ Ｍｏｒｉｏｎ
Ｐｌａｎｎｉｎｇ［Ｃ］／／２０２０ⅢＥ剐ＲｓＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ（ＩＲＯＳｌ．
２０２０：’７４８５．７４９２．ＤＯＩ：１０．１１０９／ＩＲＯ￥４５７４３．２０２０．９３４１２４５．

【３３】 Ｑｕｒｅｓｈｉ Ａ Ｈ，Ｍｉａｏ Ｙ，Ｓｉｍｅｏｎｏｖ Ａ，ｃｔ ａ１．Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ Ｎｅｔｗｏｒｋｓ：Ｂｒｉｄｇｉｎｇ ｔｈｅ Ｇａｐ Ｂｅｔｗｅｅｎ
Ｌｅａｒｎｉｎｇ．Ｂａｓｅｄ ａｎｄ Ｃｌａｓｓｉｃａｌ Ｍｏｒｉｏｎ Ｐｌａｎｎｅｒｓ［／］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ０ｎ Ｒｏｂｏｔｉｃｓ。２０２ｌ，３７ｎ１：４８．
６６．ＤＯＩ：ｌＯ．１１０９厂１Ｒ０．２０２０．３００６７１６．

【３４】 Ｇａｍｍｅｌｌ Ｊ Ｄ，Ｓｒｉｎｉｖａｓａ Ｓ Ｓ，Ｂａｒｆｏｏｔ Ｔ Ｄ．Ｉｎｆｏｒｍｅｄ ＩＵ盯＋：Ｏｐｔｉｍａｌ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ

ｐａｔｈ ｐｌａｎｎｉｎｇ
ｆｏｃｕｓｅｄ ｖｉａ ｄｉｒｅｃｔ ｏｆ ａｎ ａｄｍｉｓｓｉｂｌｅ ｅｌｌｉｐｓｏｉｄａｌ ｈｅｕｒｉｓｔｉｃ［Ｃ］／／Ｐｒｏｅ．ｏｆ ｔｈｅ ＩＥＥＥ『ＲＳＪ Ｉｎｔｌ．
ｓａｍｐｌｉｎｇ
Ｃｏｎｅ ｏｎ Ｉｎｔｅｌｌ．Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔ．Ｃｈｉｃａｇｏ，ＩＬ，２０１４：２９９７．３００４．
『３５１ Ｓｔｒｕｂ Ｍ，Ｇａｍｍｅｌｌ Ｊ．Ａｄａｐｔｉｖｅｌｙ Ｉｎｆｏｒｍｅｄ １ｈｅｓ（ＡＩＴ＋）：Ｆａｓｔ Ａｓｙｍｐｔｏｔｉｃａｌｌｙ Ｏｐｔｉｍａｌ Ｐａｔｈ Ｐｌａｎｎｉｎｇ
ｔｈｒｏｕｇｈ Ａｄａｐｔｉｖｅ ＨｅｕｒｉｓｔｉｃｓｒＣ］／Ｉ．２０２０：３１９１．３１９８．ＤＯＩ：１０．１ １０９／ＩＣＲＡ４０９４５．２０２０．９１９７３３８．
【３６】 ＳｔｒｕｂＭＰ，ＧａｍｍｅｌｌＩＤ．刖陌。锄ｄＥｌＴ‘：Ａｓｙｍｍｅｔｒｉｃｂｉｄｉｒｅｃｔｉｏｎａｌｓａｍｐｌｉｎｇ－ｂａｓｅｄｐａｔｈｐｌａｎｎｉｎｇ［／］．
Ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆＲｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ（ＩＪＲＲ），２０２２．

［３７１ Ｂｏｅｕｆ Ａ，Ｃｏｒｔｔｓ Ｊ，Ａｌａｍｉ Ｋ ｃｔ ａ１．Ｐｌａｎｎｉｎｇ ａｇｉｌｅ ｍｏｔｉｏｎｓ ｆｏｒ ｑｕａｄｒｏｔｏｒｓ ｉｎ ｃｏｎｓｔｒａｉｎｅｄ ｅｎｖｉｒｏｎ＝

ｍｅｎｔｓ［Ｃ］／／２０１４ⅡｍＤＲＳＪＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｃｎｃｅｏｎＩｎｔｅｌｌｉｇｅｎｔＲｏｂｏｔｓａｎｄＳｙｓｔｅｍｓ．２０１４：２１８．
２２３．ＤＯＩ：１０．１１０９／ＩＲＯＳ．２０１４．６９４２５“．
『３８］ Ｂｏｅｕｆ Ａ，Ｃｏｒｔｔｓ Ｊ’Ａｌａｍｉ Ｒ，ｅｔ ａ１．Ｅｎｈａｎｃｉｎｇ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｋｉｎｏｄｙｎａｍｉｃ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｆｏｒ
ｑｕａｄｒｏｔｏｒｓ［Ｃ］／／２０１５ＩＥＥＥ／ＲＳＪＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｅｎｃｅｏｎＩｎｔｅｌｌｉｇｅｎｔＲｏｂｏｔｓａｎｄＳｙｓｔｅｍｓｔＩＲＯＳ）．
２０１５：２４４７．２４５２．ＤＯＩ：１０．１ １０９，ＩＲＯＳ．２０１５．７３５３７０９．

『３９］ Ｂｅｕｌ Ｍ，Ｂｅｈｎｋｅ Ｓ．Ｆａｓｔ伽ｌ ｓｔａｔｅ ｔｒａｊｅｃｔｏｒｙ ｇｅｎｅｒａｔｉｏｎ ｆｏｒ ｍｕｌｔｉｒｏｔｏｒｓ［Ｃ］／／２０１７ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎ．
ｆｅｒｅｎｃｅ ｏｎ Ｕｎｍａｎｎｅｄ Ａｉｒｃｒａｆｔ Ｓｙｓｔｅｍｓ ＣＩＣＵＡＳ）．２０１７：４０８－４１６．ＤｏＩ：１０．１１０９／ＩＣＵＡＳ．２０１７．７９９１
３０４．

１２２

万方数据
 浙江大学博士专业学位论文 参考文献

『４０１ ＢｅｕｌＭ，ＢｅｈｎｋｅＳ．ＦａｓｔＴｉｍｅ－ｏｐｔｉｍａｌＡｖｏｉｄａｎｃｅｏｆＭｏｖｉｎｇＯｂｓｔａｃｌｅｓｆｏｒＨｉｇｈ－ＳｐｅｅｄＭＡＶ Ｆｌｉ曲ｔ［ｃ］／／

２０１９ ＩＥＥＥ瓜ＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏ协ａｎｄ Ｓｙｓｔｅｍｓ（ＩＲＯＳ）．２０１９：７２４０－
７２４７．ＤｏＩ：１０．１１０９／ＩＲＯ￥４０８９７．２０１９．８９６８１０３．
ｆ４１１ Ｒａｎｓｃｈｅｒ Ｆ，Ｓａｗｏｄｎｙ Ｏ．Ｅｆｆｉｃｉｅｎｔ Ｏｎｌｉｎｅ Ｔｒａｊｅｃｔｏｒｙ Ｐｌａｎｎｉｎｇ ｆｏｒ Ｉｎｔｅｇｒａｔｏｒ Ｃｈａｉｎ Ｄｙｎａｍｉｃｓ ｕｓｉｎｇ
Ｐｏｌｙｎｏｍｉａｌ Ｅｌｉｍｉｎａｔｉｏｎ们．ＡｒＸｉｖ ｐｒｅｐｒｉｎｔ ａｒＸｉｖ：２０１２．０７０２９，２０２０．
ｆ４２１ Ｌｉ Ｙ。Ｌｉｔｔｌｅｆｉｅｌｄ ｚ’Ｂｅｋｒｉｓ Ｋ Ｅ．Ｓｐａｒｓｅ ｍｅｔｈｏｄｓ ｆｏｒ ｅｆｆｉｃｉｅｎｔ ａｓｙｍｐｔｏｔｉｃａｌｌｙ ｏｐｔｉｍａｌ ｂｎｏｄｙｎａｍｉｃ

ｐｌａｎｎｉｎｇ［ＧＩ／／ＡＩｇｏｆｉｔｈｍｉｃ ｆｏｕｎｄａｔｉｏｎｓ ｏｆｒｏｂｏｔｉｃｓ ＸＩ．Ｓｐｒｉｎｇｅｒ，２０１５：２６３－２８２．

『４３１ Ｗ曲ｂ Ｄ Ｊ．ｖａｎ ｄｅｎ Ｂｅｒｇ Ｊ．Ｋｍｏｄｙｎａｍｉｃ ＲＲＴｏ：Ａｓｙｍｐｔｏｔｉｃａｌｌｙ ｏｐｔｉｍａｌ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｆｏｒ ｒｏｂｏｔｓ
ｗｉｔｈ ｌｉｎｅａｒ ｄｙｎａｍｉｃｓ［Ｃ］／／ｅｒｏｃ．ｏｆ ｔｈｅ正ＥＥ Ｉｎｔｌ．Ｃｏｎｆ．ｏｎ Ｒｏｂｏｔ．ａｎｄ Ａｕｔｏｍ．２０１３：５０５４－５０６１．
ＤＯＩ：ｌＯ．１ １０９，ｌＣＲＡ．２０１３．６６３１２９９．

｜“】 Ｚｈｅｎｇ Ｄ，Ｔｓｉｏｔｒａｓ Ｐ．Ａｃｃｅｌｅｒａｔｉｎｇ Ｋｉｎｏｄｙｎａｍｉｃ砒盯ｏ Ｔｈｒｏｕｇｈ Ｄｉｍｅｎｓｉｏｎａｌｉｔｙ Ｒｅｄｕｃｔｉｏｎ［Ｃ］／／

２０２ｌ ＩＥＥＥ，ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ（ｔｏｏＳｌ．２０２１：３６７４—
３６８０．ＤＯＩ：１０．１１０９，瓜Ｏ￥５１ １６８．２０２１．９６３６７５４．
『４５１ Ｐｅｒｅｚ Ａ，Ｐｌａｔｔ Ｒ，Ｋｏｎｉｄａｒｉｓ Ｇ，ｅｔ ａ１．ＬＱＲ－ＲＲＴ＊：Ｏｐｔｉｍａｌ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｍｏｔｉｏｎ ｐｌａｒｍｉｎｇ ｗｉｍ
ａｕｔｏｍａｔｉｃａｌｌｙ ｄｅｒｉｖｅｄ ｅｘｔｅｎｓｉｏｎ ｈｅｕｒｉｓｔｉｃｓ［Ｃ］／／２０１２耻Ｅ
Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ
ａｎｄ Ａｕｔｏｍａｔｉｏｎ．２０１２：２５３７－２５４２．ＤＯＩ：１０．１ １０９／ＩＣＲＡ．２０１２．６２２５１７７．
『４６］ Ｋａｒａｍａｎ Ｓ．Ｗｍｔｅｒ Ｍ氏Ｐｅｒｅｚ Ａ。ｅｔ ａ１．Ａｎｙｔｉｍｅ ｍｏｔｉｏｎ ｐｌａｎｎｉｎｇ ｕｓｉｎｇ ｔｈｅ ｍ盯［Ｃ］／／Ｐｒｏｃ．ｏｆ ｔｈｅ
ⅢＥＥ Ｉｎｔｌ．Ｃｏｎｆ．ｏｎ Ｒｏｂｏｔ．ａｎｄ Ａｕｔｏｍ．２０１ ｌ：１４７８－１４８３．
ｆ４７１ Ｈａｒｔ Ｐ Ｅ，Ｎｉｌｓｓｏｎ Ｎ Ｊ。ＲａｐｈａｅＩ Ｂ．Ａ ｆｏｒｍａｌ ｂａｓｉｓ ｆｏｒ ｔｈｅ ｈｅｕｒｉｓｔｉｃ ｄｅｔｅｒｍｉｎａｔｉｏｎ ｏｆ ｍｉｎｉｍｍｎ ｃｏｓｔ

ｐａｔｈｓ［Ｊ］．ＩＥＥＥ ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｓｙｓｔｅｍｓ Ｓｃｉｅｎｃｅ ａｎｄ Ｃｙｂｅｒｎｅｔｉｃｓ，１９６８，４（２）：ｌＯｏ－１０７．

『４８１ Ｌｉｋｈａｃｈｅｖ Ｍ。Ｆｅｒｇｕｓｏｎ Ｄ．Ｐｌａｎｎｉｎｇ ｌｏｎｇ ｄｙｎａｍｉｃａｌｌｙ ｆｅａｓｉｂｌｅ ｍａｎｅｕｖ睨＇ｓ ｆｏｒ ａｕｔｏｎｏｍｏｕｓ ｖｅｈｉ－

ｃｌｅｓ［Ｊ］．Ｔｈｃ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆＲｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ，２００９，２８（８）：９３３－９４５．

『４９１ Ｚｈａｎｇ Ｊ，Ｃｈａｄｈａ Ｒ Ｇ。Ｖｅｌｉｖｅｌａ Ｖ，ｅｔ ａ１．Ｐ－ＣＡＰ：Ｐｒｅ－Ｃｏｍｐｕｔｅｄ Ａｌｔｅｒｎａｔｉｖｅ Ｐａｔｈｓ ｔｏ Ｅｎａｂｌｅ Ａｇｇｒｅｓ－
ｓｉｖｅ Ａｅｒｉａｌ Ｍａｎｅｕｖｅｒｓ ｉｎ Ｃｌｕｔｔｅｒｅｄ Ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｃ］／／２０１８ ＩＥＥＥ瓜ＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ
Ｉｎｔｅｉｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ ＯＲＯＳ）．２０１８：８４５６．８４６３．ＤＯＩ：ｌＯ．１１０９假ＯＳ．２０１８．８５９３８２６．
【５０］ Ｚｈａｎｇ Ｊ，ｃｈａｄｈａ Ｋ、，ｃｌｉｖｅｌａ ｖ，ｅｔ ａ１．ＰＣＡＬ：Ｐｒｅ－ｃｏｍｐｕｔｅｄ Ａｌｔｅｒｎａｔｉｖｅ Ｌａｎｅｓ ｆｏｒ Ａｇｇｒｅｓｓｉｖｅ Ａｅｒｉａｌ
Ｃｏｌｌｉｓｉｏｎ Ａｖｏｉｄａｎｃｅ［Ｃ］／／．２０１９．

ｆ５１］ Ｚｈａｎｇ Ｊ，Ｈｕ Ｃ，Ｃｈａｄｈａ Ｒ Ｇ，ｅｔ ａ１．Ｍａｘｉｍｕｍ Ｌｉｋｅｌｉｈｏｏｄ Ｐａｔｈ Ｐｌａｎｎｉｎｇ ｆｏｒ Ｆａｓｔ Ａｅｒｉａｌ Ｍａｎｅｕｖｅｒｓ
ａｎｄ Ｃｏｌｌｉｓｉｏｎ Ａｖｏｉｄａｎｃａ［Ｃ］／／２０１９Ⅱ匹ｌ汰ＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ

Ｓｙｓｔｅｍｓ ０Ｒｏｓ）．２０１９：２８０５－２８１２．ＤＯＩ：１０．１１０９／琢ＤＳ４０８９７．２０１９．８９６７８２８．
『５２１ Ｚｈａｎｇ Ｊ。Ｈｕ Ｃ，Ｃｈａｄｈａ＆ｅｔ ａ１．Ｆａｌｃｏ：Ｆａｓｔ ｌｉｋｅｌｉｈｏｏｄ－ｂａｓｅｄ ｃｏｌｌｉｓｉｏｎ ａｖｏｉｄａｎｃｅ ｗｉｔｈ ｅｘｔｅｎｓｉｏｎ ｔｏ
ｈｕｍａｎ．ｇｕｉｄｅｄ ｎａｖｉｇａｔｉｏｎ［Ｊ］．Ｊｏｕｒｎａｌ ｏｆＦｉｅｌｄ Ｒｏｂｏｔｉｃｓ，２０２０．３７．ＤＯＩ：ｌＯ．１００２／ｒｏｂ．２１９５２．
『５３１ Ｄｏｌｇｏｖ Ｄ．Ｔｈｎｍ Ｓ。Ｍｏｎｔｅｍｅｆｌｏ Ｍ。ｅｔ ａ１．Ｐａｔｈ ｐｌａｎｎｉｎｇ ｆｏｒ ａｕｔｏｎｏｍｏｕｓ ｖｅｈｉｃｌｅｓ ｉｎ ｕｎｋＩＩｏｗｎ ｓｅｍｉ－
ｓｔｒｕｃｔｕｒｅｄｅｎｖｉｒｏｎｍｅｎｔｓ［Ｊ］．ＴｈｅＩｎｔｅｒｎａｔｉｏｎａｌ ＪｏｕｒｎａｌｏｆＲｏｂｏｔｉｃｓＲｅｓｅａｒｃｈ，２０１０，２９（５）：４８５－５０１．
【５４】Ｌｉｕ Ｓ，Ｍｏｈｔａ Ｋ，Ａｔａｎａｓｏｖ Ｎ，ｅｔ ａ１．Ｓｅａｒｃｈ－Ｂａｓｅｄ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ ｆｏｒ Ａｇｇｒｅｓｓｉｖｅ Ｆｌｉｇｈｔ ｉｎ ＳＥ（３）【Ｊ】．
ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，２０１８，３（３１：２４３９－２４４６．Ｄ０ｌ：１０．１ １０９，Ｉ．ＲＡ．２０１８．２７９５６５４，

『５５１ Ｌｉｕ Ｓ．Ａｔａｎａｓｏｖ Ｎ。Ｍｏｈｔａ Ｋ，ｅｔ ａ１．Ｓｅａｒｃｈ－ｂａｓｅｄ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ ｆｏｒ Ｑｕａｄｒｏｔｏｒｓ璐ｉｎｇ Ｌｉｎｅａｒ

Ｑｕａｄｒａｔｉｃ Ｍｉｎｉｍｕｍ Ｔｉｍｅ Ｃｏｎｔｒｏｌ［Ｃ］／／Ｐｒｏｃ．ｏｆｔｈｅ １日；Ｅ／ＲＳＪ Ｉｎｆｌ．Ｃｏｎｆ’ｏｎ Ｉｎｔｅｌｌ．Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔ．

２０１７：２８７２．２８７９．

【５６］ＦＵ哐ＳＳ Ｍ，Ｌ色ＶＤＩＥ Ｊ，ＭＡＲ：兀Ｎ Ｐ，ｅｔ ａ１．Ｆｌａｔｎｅｓｓ ａｎｄ ｄｅｆｅｃｔ ｏｆｎｏｎ－ｌｉｎｅａｒ ｓｙｓｔｅｍｓ：ｉｎｔｒｏｄｕｃｔｏｒｙ

ｔｈｅｏｒｙ ａｎｄ ｅｘａｍｐｌｅｓｆＪ］．Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆＣｏｎｔｒｏｌ。１９９５。６１（６）：１３２７．１３６１．ＤＯｌ：１０．１０８刚∞
２０７１７９５０８９２１９５９．
『５７１ Ｚｈｏｕ Ｂ，Ｇａｏ Ｆ．Ｗａｎｇ Ｌ，ｅｔ ａ１．Ｒｏｂｕｓｔ ａｎｄ ｅ施ｅｉｅｎｔ ｑｕａｄｒｏｔｏｒ ｔｒａｊｅｃｔｏｒｙ ｇｅｎｅｒａｔｉｏｎ ｆｏｒ ｆａｓｔ ａｌｌ－

ｔｏｎｏｍｏｕｓ ｆｌｉｇｈｔ［Ｊ】．皿Ｅ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，２０１９，４（４）：３５２９－３５３６．

『５８１ Ｌｉｋｈａｃｈｅｖ Ｍ。Ｇｏｒｄｏｎ Ｇ Ｊ。Ｔｈｒｕｎ Ｓ．ＡＲＡ’：Ａｎｙｔｉｍｅ Ａ’ｗｉｔｈ ｐｒｏｖａｂｌｅ ｂｏｕｎｄｓ ｏｎ ｓｕｂ－ｏｐｔｉｍａｌｉｔｙ［Ｃ］／／

Ａｄｖａｎｃｅｓ ｉｎ ＮｅｕｒａＩ Ｉｎｆｏｒｍａｔｉｏｎ Ｐｒｏｃｅｓｓｉｎｇ Ｓｙｓｔｅｍｓ．２∞４：７６７－７７４．
ｆ５９１ Ｍｅｌｌｉｎｇｅｒ Ｄ，Ｋｕｍａｒ Ｖ．Ｍｉｎｉｍｕｍ ｓｎａｐ ｔｒａｊｅｃｔｏｒｙ ｇｅｎｅｒａｔｉｏｎ ａｎｄ ｃｏｎＵ＇ｏｌ ｆｏｒ ｑｕａｄｒｏｔｏｒｓ［Ｃ］／／Ｐｒｏｃ．
ｏｆｔｈｅＩＥＥＥＩｎｔｉ．Ｃｏｎｆ．ｏｎＲｏｂｏｔ．ａｎｄＡｕｔｏｍ．Ｓｈａｎｇｈａｉ．Ｃｈｉｎａ，２０ｌｌ：２５２０．２５２５．
ｆ６０１ Ｄｅｉｔｓ Ｒ，Ｔｅｄｒａｋｅ Ｒ．Ｃｏｍｐｕｔｉｎｇ ｌａｒｇｅ ｃｏｎｖｅｘ ｒｅｇｉｏｎｓ ｏｆ ｏｂｓｔａｃｌｅ－ｆｒｅｅ ｓｐａｃｅ ｔｈｒｏｕｇｈ ｓｅｍｉｄｅｆｉｎｉｔｅ

ｐｒｏｇｒａｍｍｉｎｇ［Ｇ］／／Ａｌｇｏｒｉｔｈｍｉｃ Ｆｏｕｎｄａｔｉｏｎｓ ｏｆＲｏｂｏｔｉｃｓ ＸＩ：ｖ０１．１０７．Ｓｐｒｉｎｇｅｒ，２０１５：１０９－１２４．

［６１１ Ｄｅｉｔｓ ｋ Ｔｅｄｒａｋｅ Ｒ．Ｅｆｆｉｃｉｅｎｔ Ｍｉｘｅｄ－Ｉｎｔｅｇｅｒ Ｐｌａｎｎｉｎｇ ｆｏｒ ＵＡＶｓ ｉｎ Ｃｌｕｔｔｅｒｅｄ Ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｃ］／／
Ｐｒｏｅ．ｏｆｔｈｅⅡ呢Ｅ Ｉｎｔｌ．Ｃｏｎｆ．ｏｎ Ｒｏｂｏｔ．ａｎｄ Ａｕｔｏｍ．Ｓｅａｔｔｌｅ。Ｗａｓｈｉｎｇｔｏｎ，ＵＳＡ，２０１５：４２４９．

１２３

万方数据
 浙江大学博士专业学位论文 参考文献

［６２】Ｌａｎｄｒｙ Ｂ，ｌ：）ｅｉｔｓ Ｒ，Ｆｌｏｒｅｎｃｅ Ｐ Ｒ，ｅｔ ａ１．Ａｇｇｒｅｓｓｉｖｅ ｑｕａｄｒｏｔｏｒ ｆｌｉｇｈｔ ｔｈｒｏｕｇｈ ｃｌｕｔｔｅｒｅｄ ｅｎｖｉｒｏｎｍｅｎｔｓ
ｕｓｉｎｇ ｍｉｘｅｄ ｉｎｔｅｇｅｒ ｐｒｏｇｒａｍｍｉｎｇ［Ｃ］／／２０１ ６ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｆｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕ－
ｔｏｍａｔｉｏｎ（ＩＣＲＡ）．２０１６：１４６９．１４７５．ＤＯＩ：１０．１１０９／！ＣＲＡ．２０１６．７４８７２８２．
［６３】 ＧａＧ Ｆ，Ｗａｎｇ Ｌ，Ｚｈｏｕ Ｂ，ｅｔ ａ１．Ｔｅａｃｈ－Ｒｅｐｅａｔ－Ｒｅｐｌａｎ：Ａ Ｃｏｍｐｌｅｔｅ ａｎｄ Ｒｏｂｕｓｔ Ｓｙｓｔｅｍ ｆｏｒ Ａｇｇｒｅｓｓｉｖｅ
Ｈｉｇｈｔ ｉｎ Ｃｏｍｐｌｅｘ Ｅｎｖｉｒｏｎｍｅｎｔｓ叨．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ。２０２０，３６（５）：１５２６．１５４５．ＤＯＩ：
ｌＯ．１１供Ｉ门『ｌ的．２０２０．２９９３２１５．
ｆ６４１ Ｖｅｒｓｃｈｅｕｒｅ Ｄ，Ｄｅｍｅｎｌｅｎａｅｒｅ Ｂ，Ｓｗｅｖｅｒｓ Ｊ，ｅｔ ａ１．Ｔｉｍｅ－ｏｐｔｉｍａｌ ｐａｔｈ ｔｒａｃｋｉｎｇ ｆｏｒ ｒｏｂｏｔｓ：Ａ ｃｏｎｖｅｘ

ｏｐｔｉｍｉｚａｔｉｏｎ ａｐｐｒｏａｃｈ［Ｊ］．ＩＥ髓Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｕｔｏｍａｔｉｃ Ｃｏｎｔｒ０１．２００９．５４（Ｉ∞：２３１８－２３２７．

『６５１ Ｏｌｅｙｎｉｋｏｖａ Ｈ，Ｂｕｒｒｉ Ｍ，Ｔａｙｌｏｒ Ｚ，ｅｔ ａ１．Ｃｏｎｔｉｎｕｏｕｓ－ｔｉｍｅ ｔｒａｊｅｃｔｏｒｙ ｏｐｔｉｍｉｚａｔｉｏｎ ｆｏｒ ｏｎｌｉｎｅ ＵＡｖ
ｌＥＥ日ＲＳＪ Ｉｎｔｌ．Ｃｏｎｆ．ｏｎ Ｉｎｔｅｌｌ．Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔ．Ｄａｅｊｅｏｎ．Ｋｏｒｅａ，２０１６：
ｒｅｐｌａｎｎｉｎｇ［Ｃ］／ＩＰｒｏｃ．ｏｆｔｈｅ
５３３２－５３３９．

【６６】Ｕｓｅｎｋｏ Ｖ，ｖｏｎ Ｓｔｕｍｂｅｒｇ Ｌ，Ｐａｎｇｅｒｃｉｃ Ａ，ｅｔ ａ１．Ｒｅａｌ－ｔｉｍｅ ｔｒａｊｅｃｔｏｒｙ ｒｅｐｌａｎｎｉｎｇ ｆｏｒ ＭＡＶｓ ｕｓｉｎｇ
ｕｎｉｆｏｒｍ Ｂ－ｓｐｌｉｎｅｓ ａｎｄ ａ ３Ｄ ｃｆｆｃｕｌａｒ ｂｕｆｆｅｒ［Ｃ］／／Ｐｒｏｃ．ｏｆｔｈｅ ＩＥＥＥ』ＲＳＪ Ｉｎｔｉ．Ｃｏｎｅ ｏｎ Ｉｎｔｅｌｌ．Ｒｏｂｏｔｓ
ａｎｄ Ｓｙｓｔ．２０１７：２１５－２２２．
『６７１ Ｃｈｏｕｄｈｕｒｙ Ｓ。Ｇａｍｒｎｅｌｌ Ｊ Ｄ。Ｂａｒｆｏｏｔ Ｔ Ｄ。ｅｔ ａ１．Ｒｅｇｉｏｎａｌｌｙ ａｃｃｅｌｅｒａｔｅｄ ｂａｔｃｈ ｉｎｆｏｒｍｅｄ ｔｒｅｅｓ（ＲＡ．
Ｂｎ’＋、：Ａ ｆｒａｍｅｗｏｒｋ ｔｏ ｉｎｔｅｇｒａｔｅ ｌｏｃａｌ ｉｎｆｏｒｍａｔｉｏｎ ｉｎｔｏ ｏｐｔｉｍａｌ ｐａｔｈ ｐｌａｎｎｉｎｇ［Ｃ］／／２０１６髓Ｅ
Ｉｎ－
ｔｅｍａｔｉｏ岫ｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ）．２０１６：４２０７—忆１４．ＤＯＩ：ｌＯ．１ １０９，ＩＣＲ
Ａ．２０１６．７４８７６１５．
『６８１ Ｚｕｃｋｅｒ Ｍ，Ｒａ廿ｉｆｒＮ，Ｄｒａｇａｎ Ａ Ｄ。ｅｔ ａ１．Ｃｈｏｍｐ：Ｃｏｖａｒｉａｎｔ ｈａｍｉｌｔｏｎｉａｎ ｏｐｔｉｍｉｚａｔｉｏｎ ｆｏｒ ｍｏｔｉｏｎ
ｐｌａｕｎｉｎｇ［ｊ］．ＴｈｅＩｎｔｅｒｎａｔｉｏｎａｌ ＪｏｕｒｎａｌｏｆＲｏｂｏｔｉｃｓＲｅｓｅａｒｃｈ，２０１３，３２（９．１０）：１１６４．１１９３．
【６９］ＫｉｍＤ，ＫｗｏｎＹ。ＹｏｏｎＳ．ＤａｎｃｉｎｇＰＲＭ＊：ＳｉｍｕｌｔａｎｅｏｕｓＰｌａｎｎｉｎｇｏｆＳａｍｐｌｉｎｇａｎｄＯｐｔｉｍｉｚａｔｉｏｎｗｉｔｈ
Ｃｏｎｆｉｇｕｒａｔｉｏｎ Ｆｒｅｅ Ｓｐａｃｅ Ａｐｐｍｘｉｍａｔｉｏｎ［Ｃ］／／２０１ ８ ＩＥＥＥ Ｉｎｔｅｍａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ

Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ）．２０１８：７０７１－７０７８．ＤＯＩ：１０．１１０９／ＩＣＲＡ．２０１８．８４６３１８ｌ，
『７０］ Ｋｉｍ Ｄ＇Ｋａｎｇ Ｍ，Ｙｏｏｎ Ｓ．Ｖｏｌｕｍｅｔｒｉｃ Ｔｒｅｅ＊：Ａｄａｐｔｉｖｅ Ｓｐａｒｓｅ Ｇｒａｐｈ ｆｏｒ Ｅｆｆｅｃｔｉｖｅ Ｅｘｐｌｏｒａｔｉｏｎ ｏｆ

ＨｏｍｏｔｏｐｙＣｌａｓｓｅｓ［Ｃ］／／２０１９压Ｅ日ＲＳＪＩｎｔｅｒｎａｔｉｏｎａｌＣｏｎｆｅｒｅｎｃｅｏｎＩｎｔｅｌｌｉｇｅｎｔＲｏｂｏｔｓａｎｄＳｙｓｔｅｍｓ
ｇＲＯＳ）．２０１９：１４９６．１５０３．ＤＯＩ：１０．１１０９／ＩＲｏＳ４０８９７．２０１９．８９６７７２８．
【７１］Ｈａｕｅｒ Ｆ，Ｔｓｉｏｔｒａｓ Ｐ．Ｄｅｆｏｒｍａｂｌｅ Ｒａｐｉｄｌｙ－Ｅｘｐｌｏｒｉｎｇ Ｒａｎｄｏｍ Ｔｒｅｅｓ［Ｃ］／／Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆ Ｒｏｂｏｔｉｃｓ：
Ｓｃｉｅｎｃｅ ａｎｄ Ｓｙｓｔｅｍｓ．Ｃａｍｂｒｉｄｇｅ，Ｍａｓｓａｃｈｕｓｅｔｔｓ，２０１７．ＤＯＩ：１０．１５６０７瓜ＳＳ．２０１７．ⅫＩ．００８．

１７２］ Ｓｈｉｒａｉ Ｙ，Ｌｉｎ Ｘ，Ｍｅｈｔａ Ａ，ｅｔ ａ１．ＬＴｏ：Ｌａｚｙ Ｔｒａｊｅｃｔｏｒｙ Ｏｐｔｉｍｉｚａｔｉｏｎ ｗｉｔｈ Ｇｒａｐｈ－Ｓｅａｒｃｈ Ｐｌａｎｎｉｎｇ ｆｏｒ

Ｈｉｇｈ ＤＯＦ Ｒｏｂｏｔｓ ｉｎ Ｃｌｕｔｔｅｒｅｄ Ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｃ］／／２０２１ Ｉ眦Ｉｎｔｅｒｎａｔｉｏｎａｌ

Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ
ａｎｄ Ａｕｔｏｍａｔｉｏｎ ｇｆＲＡ）．２０２１：７５３３－７５３９．ＤＯＩ：１０．１１０９忸ＣＲＡ４８５０６．２０２１．９５６１５０２．
【７３］Ｃｏｈｅｎ Ｂ，ｐｈｉｌｌｉｐｓ Ｍ，Ｌｉｋｈａｃｈｅｖ Ｍ．Ｐｌａｎｎｉｎｇ Ｓｉｎｇｌｅ－ａｒｍ Ｍａｎｉｐｕｌａｔｉｏｎｓ ｗｉｔｈ ｎ－Ａｎｎ Ｒｏｂｏｔｓ［Ｃ］／／
Ｐｒｏｃｅｅｄｉｎｇｓ ｏｆＲｏｂｏｔｉｃｓ：Ｓｃｉｅｎｃｅ ａｎｄ Ｓｙｓｔｅｍｓ（ＲＳＳ’１４）．２０１４．

【７４】Ｎａｔａｒａｊａｎ凡Ｃｈｏｓｅｔ Ｈ，Ｌｉｋｈａｃｈｅｖ Ｍ．Ｉｎｔｅｒｌｅａｖｉｎｇ ｇｒａｐｈ ｓｅａｒｃｈ ａｎｄ ｔｒａｊｅｃｔｏｒｙ ｏｐ血ｎｉｚａｔｉｏｎ ｆｏｒ ａｇ－
ｇｒｅ路ｉｖｅ ｑｕａｄｒｏｔｏｒ ｎｉｇｈｔｆｆ］．髓Ｅ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，２０２１，６（３）：５３５７－５３６４．
『７５１ Ｐｏｈｌ Ｉ．Ｆｉｒｓｔ ｒｅｓｕｌｔｓ ｏｎ ｔｈｅ ｅｆｆｅｃｔ ｏｆ ｅｌ＇ｒｏｔ ｉｎ ｈｅｕｒｉｓｔｉｃ ｓｅａｒｃｈｆＪｌ．Ｍａｃｈｉｎｅ Ｉｎｔｅｌｌｉｇｅｎｃｅ。１９７０。５：２１９－
２３６．

【７６１ Ｒｆｉｄｉｇｅｒ Ｅ，ＲｏｌｆＤ．Ｗｅｉｇｈｔｅｄ Ａ＋ｓｅａｒｃｈ—ｒｅｌｙｉｎｇ ｖｉｅｗ ａｎｄ ａｐｐｌｉｃａｔｉｏｎ［Ｊ］．Ａｒｔｉｆｉｃｉａｌ Ｉｎｔｅｌｌｉｇｅｎｃｅ，

２００９．１７３（１４）：１３１０－１３４２．ＤＯＩ：ｈｔｔｐｓ：／／ｄｏｉ．ｏｒｇ／１０．１０１６／ｊ．ａｒｔｉｎＬ２００９．０６．００４．
『７７１ ＭｕｅＨｅｒ Ｍ ｗ，Ｈｅｌｍ Ｍ，Ｄ’Ａｎｄｒｅａ Ｒ．Ａ ｃｏｍｐｕｔａｔｉｏｎａｌｌｙ ｅ伍ｃｉｅｎｔ ｍｏｔｉｏｎ ｐｒｉｍｉｔｉｖｅ ｆｏｒ ｑｕａｄｒｏｃｏｐｔｅｒ
ｔｒａｊｅｃｔｏｒｙ ｇｅｎｅｒａｔｉｏｎ［Ｊ］．ｍ髓Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ，２０１５。３ｌ（６）：１２９４．１３１０．
『７８１ Ｆｅｎ＇ｉｎ Ｊ，Ｌｅｉｓｈｍａｎ＆Ｂｅａｒｄ氏ｅｔ ａ１．Ｄｉｆｆｅｒｅｎｔｉａｌ ｆｌａｔｎｅｓｓ ｂａｓｅｄ ｃｏｎｔｒｏＩ ｏｆａ ｒｏｔｏｒｃｒａｆｌ ｆｏｒ ａｇｇｒｅｓｓｉｖｅ

ｍａｎｅｎｖｅｒｓ［Ｃ］／／２０１ｌ啦Ｅ／ＲｓＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ．２０１１：

２６８８－２６９３．ＤＯＩ：ｌＯ．１１０９／ⅡｔＯＳ．２０１ １．６０９５０９８．

『７９１ Ｆａｅｓｓｌｅｒ Ｍ。Ｆｒａｎｃｈｉ Ａ。Ｓｃａｒａｍｕｚｚａ Ｄ．Ｄｉｆｆｅｒｅｎｔｉａｌ Ｆｌａｔｎｅｓｓ ｏｆＱｕａｄｒｏｔｏｒ Ｄｙｎａｍｉｃｓ Ｓｕｂｊｅｃｔ ｔｏ Ｒｏｔｏｒ

Ｄｒａｇ ｆｏｒ Ａｃｃｕｒａｔｅ Ｔｒａｃｋｉｎｇ ｏｆＨｉｇｈ－Ｓｐｅｅｄ Ｔｒａｊｅｃｔｏｒｉｅｓ［Ｊ］．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，
２０１８，３∞：６２晰２６．ＤＯＩ：ｌＯ．１１０９／Ｉ凰Ａ．２０１７．”７６３５３．
【８０１ ＷａｔｔｅｒｓｏｎＭ，ＫｕｍａｒＶ．Ｃｏｎｔｒｏｌｏｆｑｕａｄｒｏｔｏｒｓｕｓｉｎｇｔｈｅｈｏｐｆｆｉｂｒａｔｉｏｎｏｎ ｓｏ（３）【ｑ／／ＲｏｂｏｔｉｃｓＲｅ－
ｓｅａｒｃｈ．Ｓｐｒｉｎｇｅｒ，２０２０：１９９－２１５．
［８１】 Ｖｉｎｃｅ Ｊ．Ｑｕａｔｅｒｎｉｏｎｓ ｆｏｒ Ｃｏｍｐｕｔｅｒ Ｇｒａｐｈｉｃｓ［Ｍ］．Ｓｐｒｉｎｇｅｒ，２０１ １．

ＩＳ２】 Ｅｌｆｅｓ Ａ．Ｕｓｉｎｇ ｏｃｃｕｐａｎｃｙ ｇｒｉｄｓ ｆｏｒ ｍｏｂｉｌｅ ｒｏｂｏｔ ｐｅｒｃｅｐｔｉｏｎ ａｎｄ ｎａｖｉｇａｔｉｏｎ［Ｊ］．Ｃｏｍｐｕｔｅｒ，１９８９，
２２（６）：４６５７．

１２４

万方数据
 浙江大学博士专业学位论文 参考文献

［８３］ Ｏｌｅｙｎｉｋｏｖａ Ｈ。Ｍｉｌｌａｎｅ Ａ，Ｔａｙｌｏｒ Ｚ。ｅｔ ａ１．Ｓｉｇｎｅｄ Ｄｉｓｔａｎｃｅ Ｆｉｅｌｄｓ：Ａ Ｎａｔｕｒａｌ Ｒｅｐｒｅｓｅｎｔａｔｉｏｎ ｆｏｒ Ｂｏｔｈ
Ｍａｐｐｉｎｇ ａｎｄ Ｐｌａｒｍｉｎｇ［Ｃ］／／ＲＳＳ Ｗｏｒｋｓｈｏｐ：Ｇｅｏｍｅｔｒｙ ａｎｄ Ｂｅｙｏｎｄ・Ｒｅｐｒｅｓｅｎｔａｔｉｏｎｓ，Ｐｈｙｓｉｃｓ，ａｎｄ

Ｓｃｅｎｅ Ｕｎｄｅｒｓｔａｎｄｉｎｇ ｆｏｒ Ｒｏｂｏｔｉｃｓ．２０ ｌ ６．

［８４】 Ｄ．Ｂｅｒｇ Ｍ，Ｍａｒｃ Ｖ Ｋ，Ｍａｒｋ Ｏ，ｅｔ ａ１．Ｃｏｍｐｕｔａｔｉｏｎａｌ ｇｅｏｍｅｔｒｙ［Ｍ］．Ｓｐｒｉｎｇｅｒ，２０００．

［８５】 Ｈａｌｌ Ｌ，Ｇａｏ Ｆ，Ｚｈｏｕ Ｂ，ｅｔ ａ１．ＦＩＥＳＴＡ：Ｆａｓｔ Ｉｎｃｒｅｍｅｎｔａｌ Ｅｕｃｌｉｄｅａｎ Ｄｉｓｔａｎｃｅ Ｆｉｅｌｄｓ ｆｏｒ Ｏｎｌｉｎｅ Ｍｏｔｉｏｎ

Ｐｌａｎｎｉｎｇ ｏｆＡｅｒｉａｌ Ｒｏｂｏｔｓ［Ｃ］／／２０１９ ＩＥＥＥ／ＲＳＪ ＩｎｔｅｍａｆｉｏｎａＩ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ

Ｓｙｓｔｅｍｓ（Ⅱｌ－ｏＳｌ．２０１９：４４２３．４４３０．ＤＯＩ：１０．１１０９偶０￥４０８９７．２０１９．８９６８１９９．
Ｆｅｌｚｅｎｓｚｗａｌｂ Ｐ Ｆ，Ｈｕｔｔｅｎｉｏｃｈｅｒ Ｄ Ｐ．Ｄｉｓｔａｎｃｅ Ｔｒａｎｓｆｏｒｍｓ ｏｆ Ｓａｍｐｌｅｄ Ｆｕｎｃｔｉｏｎｓ［Ｊ／ｏＬｌ．Ｔｈｅｏｒｙ ｏｆ

Ｃｏｍｐｕｔｉｎｇ，２０１２，８（１９）：４１５．４２８．ｈａｐ：／／ｗｗｗ．ｔｈｅｏｒｙｏｆｃｏｍｐｕｔｉｎｇ．ｏｒｇ／ａｒｔｉｃｌｅｓ／ｖ００８ａ０１９．ＤＯＩ：
１０．４０８６／ｔｏｅ．２０１２．ｖ００８ａ０１９．

【８７］ Ｂｏｅｕｆ Ａ，Ｃｏｒｔｅｓ Ｊ，Ａｌａｍｉ＆ｅｔ ａ１．Ｐｌａｎｎｉｎｇ ａｇｉｌｅ ｍｏｔｉｏｎｓ ｆｏｒ ｑｕａｄｒｏｔｏｒｓ ｉｎ ｃｏｎｓｔｒａｉｎｅｄ ｅｎｖｉｒｏｎ－

ｍｅｎｔｓ［Ｃ］／／Ｐｒｏｃ．ｏｆｔｈｅ ＩＥＥ］傩Ｊ Ｉｎｔｌ．Ｃｏｎｆ．ｏｎ Ｉｎｔｅｌｌ．ＲｏｂｏＢ ａｎｄ Ｓｙｓｔ．Ｃｈｉｃａｇｏ．ＩＬ，２０１４：２１８－２２３．

［８８］ Ｂｅｕｌ Ｍ．Ｂｅｈｎｋｅ Ｓ．Ａｎａｌｙｔｉｃａｌ ｔｉｍｅ．ｏｐｔｉｍａｌ ｔｒａｊｅｃｔｏｒｙ ｇｅｎｅｒａｔｉｏｎ ａｎｄ ｃｏｎｔｒｏｌ ｆｏｒ ｍｕｌｔｉｒｏｔｏｒｓ［Ｃ］／／
２０１６ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｕｎｍａｎｎｅｄ Ａｉｒｃｒａｆｔ Ｓｙｓｔｅｍｓ（ＩＣＵＡＳｌ．２０１６：８７．９６．ＤＯＩ：１０．１ｌＯ
９／ＩＣＵＡＳ．２０１６．７５０２５３２．

［８９］ Ｂｅｕｌ Ｍ．Ｂｅｈｎｋｅ Ｓ．Ｆａｓｔ Ｔｉｍｅ．ｏｐｔｉｍａＩ Ａｖｏｉｄａｎｃｅ ｏ胝ｖｉｎｇ Ｏｂｓｔａｃｌｅｓ ｆｏｒ Ｈｉｇｈ－Ｓｐｅｅｄ Ｍ怂，Ｆｌｉｇｈｔ［Ｃ］／／
２０１９ ＩＥＥＥ，ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｅｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ（ｍｏｓ）．２０１９：７２４０－
７２４７．ＤＯＩ：１０．１１０９／ｍＯ￥４０８９７．２０１９．８９６８１０３．
【９０］ Ｂｅｒｓｃｈｅｉｄ Ｌ．Ｋｒ６９ｅｔ Ｔ．１ｅｒｋ－ｌｉｍｉｔｅｄ Ｒｅａｌ－ｔｉｍｅ Ｔｒａｊｅｃｔｏｒｙ Ｇｅｎｅｒａｔｉｏｎ ｗｉｔｈ Ａｒｂｉｔｒａｒｙ Ｔａｒｇｅｔ Ｓｔａｔｅｓ［Ｊ］．
ＡｒＸｉｖ．２０２ｌ。ａｂｅ２１０５．０４８３０．
【９１】 Ｈａｓｃｈｋｅ Ｒ，Ｗｅｉｔｎａｕｅｒ Ｅ，Ｒｉｔｔｅｒ Ｈ．Ｏｎ－ｌｉｎｅ ｐｌａｎｎｉｎｇ ｏｆｒｉｍｅ—ｏｐｔｉｍａｌ，ｉｅｒｋ－ｌｉｍｉｍｄ ｔｒａｊｅｃｔｏｒｉｅｓ［Ｃ］／／
２００８ ＩＥＥＥ瓜ＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ．２００８：３２４８－３２５３．ＤＯＩ：
１０．１１０９／ＩＲＯＳ．２００８．４６５０９２４．

［９２］ Ｗ＾ＮＧ Ｙ Ｓ，ＳＵＮ Ｌ，ＺＨＯＵ Ｌ，ｅｔ ａ１．Ｏｎｌｉｎｅ Ｍｉｎｉｍｕｍ．ａｃｃｅｌｅｒａｔｉｏｎ Ｔｒａｊｅｃｔｏｒｙ Ｐｌａｎｎｉｎｇ ｗｉｔｈ ｔｈｅ
Ｋｉｎｅｍａｔｉｃ Ｃｏｎｓｔｒａｉｎｔｓ［Ｊ］．Ａｃｔａ Ａｕｔｏｍａｔｉｃａ Ｓｉｎｉｃａ，２０１４．４０（ｎ：１３２８．１３３８．ＤＯＩ：ｈｔｔｐｓ：／／ｄｏｉ．ｏｒｇ／１０

．１０１６／Ｓ１８件１０２９（１４１６００１４－８．
［９３］ Ｖｉｎｔｅｒ Ｒ．Ｏｐｔｉｍａｌ ｃｏｎ缸，０１．Ｒｅｐｒｉｎｔ ｏｆｔｈｅ ２０００ ｅｄ［Ｍ１．２０１０．ＤＯＩ：１０．１００７／９７８．０．８１７６．８０８６．２．
【９４］ 吴冰．自主泊车的最优轨迹规划与跟踪控制研究【Ｄ】．合肥工业大学，２０２１．
［９５】 Ｔｏｒｄｅｓｉｌｌａｓ Ｊ．Ｌｏｐｅｚ Ｂ Ｔ．Ｈｏｗ Ｊ Ｐ．ＦＡＳＴＥＲ：Ｆａｓｔ ａｎｄ Ｓａｆｅ Ｔｒａｊｅｃｔｏｒｙ Ｐｌａｎｎｅｒ ｆｏｒ Ｆｌｉｇｈｔｓ ｉｎ Ｕｎｋｎｏｗｎ

Ｅｎｖｋｏｎｍｅｎｍ［Ｃ］／／Ｐｒｏｃ．ｏｆｔｈｅⅡＩＥ吣Ｊ Ｉｎｔｌ，Ｃｏａｆ，Ｏｌｌ Ｉｎｔｅｌｌ．Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔ．２０１９．

［９６】 Ｌｉｕ Ｓ，Ｗａｔｔｅｒｓｏｎ Ｍ．Ｍｏｈｔａ Ｋ，ｃｔ ａ１．Ｐｌａｎｎｉｎｇ Ｄｙｎａｍｉｃａｌｌｙ Ｆｅａｓｉｂｌｅ Ｔｒａｊｅｃｔｏｒｉｅｓ ｆｏｒ Ｑｕａｄｒｏｔｏｒｓ ｕｓｉｎｇ
Ｓａｆｅ Ｆｌｉｇｈｔ Ｃｏｒｒｉｄｏｒｓ ｉｎ ３－Ｄ Ｃｏｍｐｌｅｘ Ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｊ］．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ（ＲＡ－
Ｌ１．２０１７：１６８８－１６９５．
【９７］ Ｗ抽ｇ Ｚ，Ｚｈｏｕ Ｘ，Ｘｕ Ｃ，ｅｔ ａ１．Ｇｅｏｍｅｔｒｉｃａｌｌｙ ｃｏｎｓｔｒａｉｎｅｄ ｔｒａｊｅｃｔｏｒｙ ｏｐｔｉｍｉｚａｔｉｏｎ ｆｏｒ ｍｕｌｔｉｃｏｐｔｅｒｓ［Ｊ］．
ＡｒＸｉｖ ｐｒｃｐｒｍｔ ａｒＸｉｖ：２１０３．００１９０。２０２１．

［９８】 李柏．复杂约束下自动驾驶车辆运动规划的计算最优控制方法研究【Ｄ】．浙江大学，２０１８．
【９９】 张洪钺，王青．最优控制理论与应用ＩＭ】．２００６．
【１００】 Ｓｃｈｍｅｒｌｉｎｇ Ｅ，Ｊａｎｓｏｎ Ｌ．Ｐａｖｏｎｅ Ｍ．Ｏｐｔｉｍａｌ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｍｏｔｉｏｎ ｐｌａｒｍｉｎｇ ｕｎｄｅｒ ｄｉｆｆｅｒｅｎｔｉａｌ ｃｏｎ－

ｓｔｒａｉｎｔｓ：Ｔｈｅｄｒｉｆｔｃａｓｅｗｉｔｈｌｉｎｅａｒａｆｆｍｅｄｙｎａｍｉｃｓ［Ｃ］／／２０１５５４ｔｈＩＥＥＥＣｏｎｆｅｒｅｎｃｅｏｎＤｅｃｉｓｉｏｎａｎｄ
Ｃｏｎｔｒｏｌ（ＣＤＣｌ．２０１５：２５７４．２５８１．ＤｏＩ：１０．１１０９／ＣＤＣ．２０１５．７４０２６０４．
【１０１】 Ｙｅ Ｈ，Ｚｈｏｕ Ｘ，Ｗａｎｇ Ｚ．ｅｔ ａ１．ＴＧＫ－Ｐｌａｎｎｅｒ：Ａｎ Ｅｍｅｉｅｕｔ Ｔｏｐｏｌｏｇｙ Ｇｕｉｄｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｅｒ
ｆｏｒ Ａｕｔｏｎｏｍｏｕｓ Ｑｕａｄｒｏｔｏｒｓ［Ｊ］．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，２０２ｌ，６（２）：４９４—５０１．ＤＯＩ：
１０．１ １０９，Ｉ．Ｒ Ａ．２０２０．３０４７７９８．

【１０２】 Ｖｅｒｒｉｅｓｔ Ｅ．Ｌｅｗｉｓ Ｆ．Ｏｎ ｔｈｅ ｌｉｎｅａｒ ｑｕａｄｒａｔｉｃ ｍｉｎｉｍｎｍ－ｔｉｍｅ ｐｒｏｂｌｅｍ［Ｊ］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ａｕｔｏ－
ｍａｒｉｅ Ｃｏｎｔｒｏｌ，ｌ ９９１，３６（７）：８５９－８６３．ＤＯＩ：ｌ Ｏ．１ｌ ０９／９．８５０６６．

【１０３】 Ｌｉ Ｙ，Ｌｉｔｔｌｅｆｉｅｌｄ Ｚ，Ｂｄｃｒｉｓ Ｋ Ｅ．Ａｓｙｍｐｔｏｔｉｃａｌｌｙ ｏｐｔｉｍａｌ ｓａｍｐｌｉｎｇ－ｂａｓｅｄ ｋｉｎｏｄｙｎａｍｉｃ ｐｌａｒｍｉｎｇ［Ｊ／ＯＬ］

Ｔｈｅ Ｉｎｔｅｍａｔｉｏｎａｌ ｌｏｕｍａｌ ｏｆＲｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ，２０１６，３５（５）：５２８．５６４．ｈｔｔｐｓ：／／ｄｏｉ．ｏｒｇ／１０．１ １７７／０２７８
３６４９１５６１４３８６．ＤＯＩ：１０．１１７７／０２７８３６４９１５６１４３８６．

［１０４］ Ｂｕｒｋｅ Ｄ。Ｃｈａｐｍａｎ Ａ，Ｓｈａｍｅｓ Ｉ．Ｇｅｎｅｒａｔｉｎｇ Ｍｉｎｉｍｕｍ．Ｓｎａｐ Ｑｕａｄｒｏｔｏｒ Ｔｒａｊｅｃｔｏｒｉｅｓ Ｒｅａｌｌｙ Ｆａｓｔ［Ｃ］／／
２０２０ ＩＥＥＥ／ＲＳＪ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｅｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ ０ＲＯＳｌ．２０２０：１４８７．
１４９２．ＤＯＩ：１０．１ １０９／ⅡｔＯ￥４５７４３．２０２０．９３４１７９４．

１２５

万方数据
 浙江大学博士专业学位论文 参考文献

［１０５】 Ｂｕｒｒｉ Ｍ。Ｏｌｅｙｎｉｋｏｖａ Ｈ，Ａｃｈｔｅｌｉｋ Ｍ，ｅｔ ａ１．Ｒｅａｌ—ｔｉｍｅ ｖｉｓｕａｌ－ｉｎｅｒｔｉａｌ ｍａｐｐｉｎｇ，ｒｅ．１０ｃａｌｉｚａｔｉｏｎ ａｎｄ

ｐｌａｎｎｉｎｇ ｏｎｂｏａｒｄ Ｍ ＡＶｓ ｉａ ｕｎｋｎｏｗｎ ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｃ］／／Ｐｒｏｅ．ｏｆｔｈｅ ＩＥＥＥ／ＲＳＪ Ｉｎｔｌ．Ｃｏｎｆ．ｏｎ Ｉｎｔｅｉｌ．
Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔ．Ｈａｍｂｕｒｇ，Ｇｅｒｍａｎｙ。２０１ ５．

【１０６】 Ｗａｎｇ Ｚ，Ｙｅ Ｈ，ｘｕ Ｃ，ｅｔ ａ１．Ｇｅｎｅｒａｔｉｎｇ Ｌａｒｇｅ・Ｓｃａｌｅ Ｔｒａｊｅｃｔｏｒｉｅｓ Ｅｆｆｉｃｉｅｎｔｌｙ ｕｓｉｎｇ Ｄｏｕｂｌｅ Ｄｅｓｅｒｉｐ－
ｔｉｏｍ ｏｆＰｏｌｙｎｏｍｉａｌｓ［Ｃ］／／２０２１ ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ（ＩＣＲＡ）．
２０２１：７４３６．７４４２．ＤＯＩ：１０．１１０９／ＩＣＲＡ４８５０６．２０２１．９５６１５８５．

【１０７】 ＳｃｈａｐｐｅｌｌｅＲ．ＴｈｅｉｎｖｅｒｓｅｏｆｔｈｅｃｏｎｆｌｕｅｎｔＶａｎｄｅｒｍｏｎｄｅｍａｔｒｉｘ［Ｊ］．ＩＥＥＥＴｒａｎｓａｃｔｉｏｎｓｏｎＡｕｔｏｍａｔｉｃ
Ｃｏｎｔｒｏｌ，１９７２，１７（５）：７２４．７２５．ＤＯＩ：１０．１１０９几～Ｃ．１９７２．１１００１２９．
【１０８】 Ｔｅｏ Ｋ，Ｒｅｈｂｏｃｋ Ｖ。Ｊｅｎｎｉｎｇｓ Ｌ．Ａ ｎｅｗ ｃｏｍｐｕｔａｔｉｏｎａｌ ａｌｇｏｒｉｔｈｍ ｆｏｒ ｆｕｎｃｆｉｏｎａＩ ｉｎｅｑｕａｌｉｔｙ ｃｏｎｓｔｒａｉｎｅｄ

ｏｐｔｉｍｉｚａｔｉｏｎ ｐｒｏｂｌｅｍｓ［Ｊ］．Ａｕｔｏｍａｔｉｃａ，１９９３，２９（３）：７８９－７９２．ＤＯＩ：ｈｔｔｐｓ：／／ｄｏｉ．ｏｒｇ／１０．１０１６／０００５－１０
９８（９３）９００７６－６，
【１０９】 Ｔｏｏ Ｋ，Ｙａｎｇ Ｘ，Ｊｅｒｍｉｎｇｓ Ｌ．Ｃｏｍｐｕｔａｔｉｏｎａｌ Ｄｉｓｅｒｅｔｉｚａｔｉｏｎ Ａｌｇｏｄｔｈｍｓ ｆｏｒ Ｆｕｎｃｔｉｏｎａｌ Ｉｎｅｑｕａｌｉｔｙ Ｃｏｎ—
ｓｔｒａｉｎｅｄ Ｏｐｔｉｍｉｚａｆｉｏｎ［Ｊ】．Ａｎｎａｌｓ ｏｆＯｐｅｒａｔｉｏｎｓ Ｒｅｓｅａｒｃｈ，２０００，９８（１－４）：２１５．２３４．

【１１０】 Ｏｌｅｙｎｉｋｏｖａ Ｈ，Ｔａｙｌｏｒ Ｚ，Ｆｅｈｒ Ｍ，ｅｔ ａ１．Ｖｏｘｂｌｏｘ：Ｉｎｃｒｅｍｅｎｔａｌ ３Ｄ Ｅｕｃｌｉｄｅａｎ Ｓｉｇｎｅｄ Ｄｉｓｔａｎｃｅ Ｆｉｅｌｄｓ
ｆｏｒ ｏｎ－ｂｏａｒｄＭ戳ｐｌａｒｍｉｎｇ［Ｃ］／／２０１７ ＩＥＥＥ／ＲＳＩ Ｉｎｔｅｍａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ ＩｎｔｅｌｌｉｇｅｎｔＲｏｂｏｔｓ ａｎｄ

Ｓｙｓｔｅｍｓ（ｔｏｏＳｌ．２０１７：１３６６．１３７３．ＤＯＩ：１０．１１０呱ｏＳ．２０１７．８２０２３１５．
Ｃｈａｐｒａ Ｓ。Ｃａｎａｌｅ Ｒ．Ｎｕｍｅｆｉｃａｌ Ｍｅｔｈｏｄｓ ｆｏｒ Ｅｎｇｉｎｅｅｒｓ［Ｍ］．ＭｃＧｒａｗ Ｈｉｌｌ，２０１４．
ＦｌｅｔｃｈｅｒＲ ｆ．Ｐｒａｃｔｉｃａｌ ｍｅｔｈｏｄｓ ｏｆｏｐｔｉｍｉｚａｆｉｏｎ［Ｍ］．Ｗｉｌｅｙ，１９８７．
ⅢⅢⅢ Ｕ刁习 Ｈａａｒａｌａ Ｎ。Ｍｉｅｔｔｉｎｅｎ Ｋ Ｍｉｉｌｃｅｌ５ Ｍ．Ｇｌｏｂａｌｌｙ ｃｏｎｖｅｒｇｅｎｔ ｌｉｍｉｔｅｄ ｍｅｍｏｒｙ ｂｕｎｄｌｅ ｍｅｔｈｏｄ ｆｏｒ ｌａｒｇｅ．
ｓｃａｌｅ ｎｏｎｓｍｏｏｔｈ ｏｐｔｉｍｉｚａｔｉｏｎ［Ｊ］．Ｍａｔｈｅｍａｔｉｃａｌ Ｐｒｏｇｒａｍｍｉｎｇ，２００７，１０９：１８１－２０５．

Ⅲｑ Ｊｏｈｎｓｏｎ Ｓ Ｇ．１１１ｅ ＮＬｏｐｔ ｎｏｎｌｉｎｅａｒ－ｏｐｔｉｍｉｚａｔｉｏｎ ｐａｃｋａｇｅ［Ｃ／ＯＬ］／／．ｈｔｔｐ：／／ｇｉｔｈｕｂ．ｃｏｍ／ｓｔｅｖｅｎｇｉ／ｎｌｏ

ｐｔ．

Ⅲ习 Ｇｕｒｏｂｉ Ｏｐｔｉｍｉｚａｔｉｏｎ，ＬＬＣ．Ｇｕｒｏｂｉ Ｏｐｔｉｍｉｚｅｒ Ｒｅｆｅｒｅｎｃｅ Ｍａｎｕａｌ［ＥＢ／ＯＵ．２０２２．ｈｔｔｐｓ：／／ｗｗｗ．ｇｕｒｏ

ｂｉ．ｔｏｍ．
Ｔｈｎｍ Ｓ．Ｂｕｒｇａｒｄ Ｗ，Ｆｏｘ Ｄ．Ｐｒｏｂａｂｉｌｉｓｔｉｃ Ｒｏｂｏｔｉｃｓ［Ｍ］．２００５．
ⅢⅢ ｑ刀 Ｓｔｏｎｅｍａｎ Ｓ．Ｌａｍｐａｒｉｅｌｌｏ Ｒ．Ｅｍｂｅｄｄｉｎｇ ｎｏｎｌｉｎｅａｒ ｏｐｔｉｍｉｚａｔｉｏｎ ｉｎ ＲＲＴ’ｆｏｒ ｏｐｔｉｍａｌ ｋｉｎｏｄｙｎａｍｉｃ

ｐｌａｒｍｉｎｇ［Ｃ］／／５３ｒｄ疆ＥＥ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｄｅｃｉｓｉｏｎ ａｎｄ Ｃｏｎｔｒ０１．２０１４：３７３７－３７４４．Ｄ０１：１０．Ｉ １０９／ＣＤ

Ｃ．２０１４．７０３９９７１．

［１１８】 Ｃ，Ａｌｔｅｒｏｖｉ乜Ｒ．Ｉｎｔｅｒｌｅａｖｉｎｇ Ｏｐｔｉｍｉｚａｔｉｏｎ ｗｉｔｈ Ｓａｍｐｌｉｎｇ－Ｂａｓｅｄ Ｍｏｔｉｏｎ Ｐｌａｎｎｉｎｇ

Ｋｕｎｔｚ Ａ，Ｂｏｗｅｎ
（ＩＯＳｍ删：Ｃｏｍｂｉｎｉｎｇ Ｌｏｃａｌ Ｏｐｔｉｍｉｚａｔｉ伽、＾ｒｉｔｈ Ｇｌｏｂａｌ Ｅｘｐｌｏｒａｔｉｏｎ［Ｊ／ＯＬ］．，２０１６ａｒＸｉｖ：１６０７．０６３７
４．ｈｔｔｐ：／／ａｒｘｉｖ．ｏｒｇ／ａｂｓ／１ ６０７．０６３７４．
［１１９】 Ｚｈｏｕ Ｘ，Ｗａｎｇ Ｚ，Ｙｊ Ｈ，ｃｔ ａ１．ＥＧＯ．Ｐｌａｎｎｅｒ：Ａｎ ＥＳＤＦ－Ｆｒｅｅ Ｇｒａｄｉｅｎｔ－Ｂａｓｅｄ Ｌｏｃａｌ Ｐｌａｎｎｅｒ ｆｏｒ
Ｑｕａｄｒｏｔｏｒｓ［Ｊ］．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，２０２１，６（２】：４７８－４８５．ＤＯＩ：１０．１１０９／ＬＲＡ
．２０２０．３０４７７２８．

［１２０１ Ｂａｔｋｏｖｉｃ Ｌ Ａｌｉ Ｍ，Ｆａｌｃｏｎｅ Ｐ。ｅｔ ａ１．Ｓａｆｅ Ｔｒａｊｅｃｔｏｒｙ Ｔｒａｃｋｉｎｇ ｉｎ Ｕｎｃｅｒｔａｉｎ Ｅｎｖｉｒｏｎｍｅｎｔｓ［ＥＢ／ＯＬ］．
ａｒＸｉｖ．２０２０．ｈｔｔｐｓ：／／ａｒｘｉｖ．ｏｒｇ／ａｂｓ／２００１．１１６０２．

［１２１】 Ｆｅｂｂｏ Ｈ，Ｌｉｎ Ｊ，Ｊａｙａｋｕｍａｒ Ｐ，ｅｔ ａ１．Ｍｏｖｉｎｇ ｏｂｓｔａｃｌｅ ａｖｏｉｄａｎｃｅ ｆｏｒ ｌａｒｇｅ，ｈｉｇｈ—ｓｐｅｅｄ ａｕｔｏｎｏｍｏｕｓ
ｇｒｏｕｎｄ ｖｅｈｉｃｌｅｓ［Ｃ］／／２０１７ Ａｍｅｒｉｃａｎ Ｃｏｎｔｒｏｌ Ｃｏｎｆｅｒｅｎｃｅ ｆＡｃｃ）．２０１７：５５６８．５５７３．ＤＯＩ：１０．２３９１９
／ＡＣＣ．２０１７．７９６３８２１．

Ｒｏｄｒｉｇｕｅｚ．Ｓｅｄａ Ｅ Ｊ．Ｄｅｃｅｎｔｒａｌｉｚｅｄ ｔｒａｊｅｃｔｏｒｙ ｔｒａｃｋｉｎｇ ｗｉｔｈ ｃｏｌｌｉＳｉｏｎ ａｖｏｉｄａｎｃｅ ｃｏｎｔｒｏｌ ｆｏｒ ｔｅａｍｓ ｏｆ
ｕｎｍａｎｎｅｄ ｖｅｈｉｃｌｅｓ ｗｉｔｈ ｃｏｎｓｔａｎｔ ｓｐｅｅｄ［Ｃ］／／２０１４ Ａｍｅｒｉｃａｎ Ｃｏｎｔｒｏｌ Ｃｏｎｆｅｒｅｎｃｅ．２０１４：１２１６．１２２３．
ＤＯＩ：１０．１１０９／ＡＣＣ．２０１４．６８５９１８４．

［１２３】 Ｒｏｄｒｉｇｕｅｚ．Ｓｅｄａ Ｅ Ｊ．Ｄａｗｋｉｎｓ Ｊ Ｊ．Ｄｃｃｅｎｔｒａｌｉｚｅｄ Ｃｏｏｐｅｒａｔｉｖｅ ＣｏＵｉｓｉｏｎ Ａｖｏｉｄａｎｃｅ Ｃｏｎｔｒｏｌ ｆｏｒ Ｕｎ－
ｍａｎｎｅｄ Ｒｏｔｏｒｃｒａｆｔ ｗｉｔｈ Ｂｏｕｎｄｅｄ Ａｃｃｅｌｅｒａｔｉｏｎ［Ｊ］．Ｊｏｕｒｎａｌ ｏｆ Ｇｕｉｄａｎｃｅ，Ｃｏｎｔｒｏｌ，ａｎｄ Ｄｙｎａｍｉｃｓ，
２０１ ８，４１（１１１：２４４５．２４５４．ＤＯＩ：ｌ Ｏ．２５ １４／１．Ｇ００３４３０．

［１２４】 ＳｔｅｒＯｉｄ Ｓ，Ｎ敏ｅｌｉ Ｔ。Ａｌｏｎｓｏ－Ｍｏｍ Ｊ，ｅｔ ａ１．Ｓａｍｐｌｅ

Ｅ伍ｃｉｅｎｔ Ｌｅａｒｎｉｎｇ ｏｆＰａｔｈ Ｆｏｌｌｏｗｉｎｇ ａｎｄ Ｏｂｓｔａｃｌｅ
Ａｖｏｉｄａｎｃｅ Ｂｅｈａｖｉｏｒ ｆｏｒ Ｑｕａｄｒｏｔｏｒｓ［Ｊ］．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ，２０１ ８，３（４）：３８５２・
３８５９．ＤＯＩ：１０．１ １０９／Ｉ，ＲＡ．２０１８．２８５６９２２．

［１２５】 Ｚｈａｎｇ Ｔ，Ｋａｈｎ Ｇ．Ｌｅｖｉｎｅ Ｓ。ｅｔ ａ１．Ｌｅａｒｎｉｎｇ ｄｅｅｐ ｃｏｎｔｒｏｌ ｐｏｌｉｃｉｅｓ ｆｏｒ ａｕｔｏｎｏｍｏｕｓ ａｅｒｉａｌ ｖｅｈｉｃｌｅｓ谢ｍ
ｈ口Ｃ－ｇｕｉｄｅｄ ｐｏｌｉｃｙ ｓｅａｒｃｈ［Ｃ］／／２０１６观Ｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ
（ＩＣＲＡ）．２０１６：５２８．５３５．ＤＯＩ：１０．１１０９／ＩＣＲＡ．２０１６．７４８７１７５．

１２６

万方数据
 浙江大学博士专业学位论文 参考文献

【１２６］ Ｌｉｕ Ｘ，Ｚｈａｎｇ Ｍ，Ｒｏｇｅｒｓ Ｅ．Ｔｒａｊｅｃｔｏｒｙ Ｔｒａｃｋｉｎｇ Ｃｏｎｔｒｏｌ ｆｏｒ Ａｕｔｏｎｏｍｏｕｓ Ｕｎｄｅｒｗａｔｅｒ Ｖ曲ｉｃｌｅｓ Ｂａｓｅｄ
ｏｎ Ｆｕｚｚｙ Ｒｃ－Ｐｌａｎｎｉｎｇ ｏｆａ Ｌｏｃａｌ Ｄｅｓｉｒｅｄ ＴｒａｊｅｃｔｏｒｙｒＪｌ．１ＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ｖ曲ｉｃｕｌａｒ Ｔｅｃｈｎｏｌｏｇｙ，

２０１９。６８（１２）：１１６５７－１１６６７．ＤＯＩ：１０．１ １０９厂ＴＶＴ．２０１９．２９４８１５３．
【１２７］ Ｓｕｂｏｓｉｔｓ Ｊ ｋ Ｇｅｒｄｅｓ Ｊ Ｃ．Ｆｒｏｍ ｔｈｅ Ｒａｃｅｔｒａｃｋ ｔｏ ｔｈｅ Ｒｏａｄ：Ｒｅａｌ－Ｔｉｍｅ Ｔｒａｉｅｅｔｏｒｙ Ｒｅｐｌａｎｎｉｎｇ ｆｏｒ
Ａｕｔｏｎｏｍｏｕｓ Ｄｒｉｖｉｎｇ［Ｊ］．ⅢＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ ＩｎｔｅｌｌｉｇｅｎｔⅥ出ｉｃｌｅｓ，２０１９，４（２）：３０９—３２０．ＤＯＩ：１０．１
１０９／ＴＩＶ．２０１９．２９０４３９０，

［１２８］ 争ｅｎｂａ§ｌａｒ Ｂ，Ｈ抽ｉｇ ｗ，Ａｙａｎｉａｎ Ｎ．ＲＬｓｓ：Ｒｅａｌ－ｔｉｍｅ Ｍｕｌｔｉ・Ｒｏｂｏｔ Ｔｒａｊｅｃｔｏｒｙ Ｒｅｐｌａｎｎｉｎｇ ｕｓｉｎｇ Ｌｉｎ・
ｅａｒＳｐａｔｉａｌＳｅｐａｒａｔｉｏｎｓ［Ｊ［．ＡｒＸｉｖｅ－ｐｒｉｎｔｓ，２０２１．ａｒＸｉｖ：２１０３．０７５８８：ａｒＸｉｖ：２１０３．０７５８８ａｒＸｉｖ：２１０３．０
７５８８ Ｉｔｓ．Ｒ０］．

【１２９］ Ｋｎｅｐｐｅｒ Ｒ，Ｓｒｉｎｉｖａｓａ Ｓ，Ｍａｓｏｎ Ｍ．Ａｎ Ｅｑｕｉｖａｌｅｎｃｅ Ｒｅｌａｔｉｏｎ ｆｏｒ Ｌｏｃａｌ Ｐａｔｈ Ｓｅｔｓ［Ｃｌ／／：ｖ０１．６８．２０１ Ｏ：
１９－３５．ＤＯＩ：１０．１００７／９７８—３－６４２—１７４５２一Ｏ ２．

【１３０］ Ｋｎｅｐｐｅｒ Ｒ Ａ，Ｍａｓｏｎ Ｍ Ｔ．Ｉｍｐｒｏｖｅｄ ｈｉｅｒａｒｃｈｉｃａｌ ｐｌａｎｎｅｒ ｐｅｒｆｏｒｍａｎｃｅ ｕｓｉｎｇ ｌｏｃａｌ ｐａｔｈ ｅｑｕｉｖａ－
ｌｅｎｃｅ［Ｃ］／／２０１ １ ｌＥＥＥ，ＲＳＪ Ｉｎｔｅｍａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ．２０１ １：３８５６．
３８６１．ＤＯＩ：１０．１１０９／ＩＲＯＳ．２０１１．６０９５１２６．

【１３１１ Ｋｎｅｐｐｅｒ Ｒ Ａ，Ｓｒｉｎｉｖａｓａ Ｓ Ｓ，Ｍａｓｏｎ Ｍ Ｔ．Ｔｏｗａｒｄ ａ ｄｅｅｐｅｒ ｕｎｄｅｒｓｔａｎｄｉｎｇ ｏｆ ｍｏｔｉｏｎ ａｌｔｅｒｎａｔｉｖｅｓ

ｖｉａ ａｌｌ ｅｑｕｉｖａｌｅｎｃｅ ｒｅｌａｔｉｏｎ ｏｎ Ｉｏｃａｌ ｐａｔｈｓ［／］．Ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｍａｌ ｏｆＲｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ。２０１２．
３ｌ（２）：１６７－１８６．ＤＯＩ：１０．１１７７，０２７８３６４９１１４３０４１８．
【１３２］ Ｃｈｅｎ Ｑ，Ｌｕ Ｙ，Ｗａｎｇ Ｙ，ｅｔ ａ１．Ｆｒｏｍ ｔｏｐｏｌｏｇｉｃａｌ ｍａｐ ｔｏ ｌｏｃａｌ ｃｏｇｎｉｔｉｖｅ ｍａｐ：ａ ｎｅｗ ｏｐｐｏｒｔｕｎｉｔｙ ｏｆ
ｌｏｃａｌ ｐａｔｈ ｐｌａｎｎｉｎｇ［Ｊ］．Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｅｒｖｉｃｅ Ｒｏｂｏｔｉｃｓ，２０２１，１４（２）：２８５—３０１．ＤＯＩ：１０．１００７／ｓｌ １３７０—０２
１－００３５２－ｚ．

［１３３］ Ｂｈａｔｔａｃｈａｒｙａ Ｓ，Ｌｉｌｄａａｃｈｅｖ Ｍ，Ｋｕｍａｒ Ｖ Ｒ，Ｔｏｐｏｌｏｇｉｃａｌ ｃｏｎｓｔｒａｉｎｔｓ ｉｎ ｓｅａｒｃｈ．ｂａｓｅｄ ｒｏｂｏｔ ｐａｔｈ ｐｌａｎ．
ｎｉｎｇ［Ｊ］．Ａｕｔｏｎｏｍｏｕｓ Ｒｏｂｏｔｓ，２０１２，３３：２７３—２９０．
［１３４］ Ｊａｉｌｌｅｔ Ｌ．Ｓｉｍｅｏｎ Ｔ．Ｐａｔｈ Ｄｅｆｏｒｍａｔｉｏｎ Ｒｏａｄｍａｐｓ：Ｃｏｍｐａｃｔ Ｇｒａｐｈｓ ｗｉｔｈ Ｕｓｅｆｕｌ Ｃｙｃｌｅｓ ｆｏｒ Ｍｏｔｉｏｎ

Ｐｌａｎｎｉｎｇ；叨．Ｔｈｅ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｊｏｕｒｎａｌ ｏｆＲｏｂｏｔｉｃｓ Ｒｅｓｅａｒｃｈ，２００８，２７（１１．１２）：１１７５．１１８８．ＤＯＩ：１０

．１１７７／０２７８３６４９０８０９８４１１．

【１３５］ ０ｕｉｓｐｅ Ａ Ｈ，Ｋｕｎｚ Ｔ，Ｓｔｉｌｍａｎ Ｍ．Ｇｅｎｅｒａｔｉｏｎ ｏｆｄｉｖｅｒｓｅ ｐａｔｈｓ ｉｎ ３Ｄ ｅｎｖｉｒｏｎｍｅｎｔｓ［Ｃ［／／２０ ｌ ３ ＩＥＥＥ／ＲＳＪ

Ｉｎｔｅｍａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ ＩｎｔｅｌｌｉｇｅｎｔＲｏｂｏｒｓ ａｎｄ Ｓｙｓｔｅｍｓ．２０１３：５９９４．５９９９．ＤＯＩ：１０。１１０９／ⅡｌＯＳ
．２０１３．６６９７２２６．

［１３６］Ｙｉ Ｄ，Ｇｏｏｄｒｉｃｈ Ｍ Ａ，Ｓｅｐｐｉ Ｋ Ｄ．Ｈｏｍｏｔｏｐｙ—ａｗａｒｅ ＲＲＴ＋：Ｔｏｗａｒｄ ｈｕｍａｎ—ｒｏｂｏｔ ｔｏｐｏｌｏｇｉｃａｌ ｐａｔｈ－

ｐｌａｎｎｉｎｇ［Ｃ［／／２０１６ １ ｌｔｈ ＡＣＭ／ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ＯＨ Ｈｕｍａｎ－Ｒｏｂｏｔ Ｉｎｔｅｒａｃｔｉｏｎ（Ｉ－ｍ０．
２０１６：２７９．２８６．ＤＯＩ：１０．１１０９棚．２０１６．７４５１７６３．
【１３７］ Ｚｈｏｕ Ｂ，Ｐａｎ Ｊ，Ｇａｏ Ｆ，ｅｔ ａ１．ＲＡＰＴＯＲ：Ｒｏｂｕｓｔ ａｎｄ Ｐｅｒｃｅｐｔｉｏｎ－Ａｗａｒｅ Ｔｒａｊｅｃｔｏｒｙ Ｒｅｐｌａｏｎｉｎｇ ｆｏｒ

Ｑｕａｄｒｏｔｏｒ Ｆａｓｔ Ｆｌｉｇｈｔ［Ｊ］．ＩＥＥＥ Ｔｒａｎｓａｃｔｉｏｎｓ ｏｎ Ｒｏｂｏｔｉｃｓ，２０２１，３７（６）：１９９２－２００９．ＤＯＩ：１０．１ １０９／Ｔ

ＲＯ．２０２１．３０７１５２７．

［１３８］ Ｚｂｏｕ Ｂ，Ｇａｏ Ｆ，Ｐａｎ Ｊ，ｅｔ ａ１．Ｒｏｂｕｓｔ Ｒｅａｌ－ｔｉｍｅ ＵＡＶ Ｒｅｐｌａｎｎｉｎｇ Ｕｓｉｎｇ Ｇｕｉｄｅｄ Ｇｒａｄｉｅｎｔ－ｂａｓｅｄ Ｏｐｒｉ．
ｍｉｚａｔｉｏｎ ａｎｄ Ｔｏｐｏｌｏｇｉｃａｌ Ｐａｔｈｓ［Ｃ［／／２０２０ ＩＥＥＥ Ｉｎｔｅｍａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａ－

ｔｉｏｎ（ＩＣＲＡ）．２０２０：１２０８—１２１４．ＤＯＩ：１０．１ １０９／ＩＣＲＡ４０９４５．２０２０．９１ ９６９９６．

【１３９］ Ｌｅｗｉｓ Ａ Ｓ，Ｏｖｃｒｔｏｎ Ｍ Ｌ．Ｎｏｎｓｍｏｏｔｈ ｏｐｔｉｍｉｚａｔｉｏｎ ｖｉａ ｑｕａｓｉ－Ｎｅｗｔｏｎ ｍｅｔｈｏｄｓ『Ｊ１．Ｍａｔｈｅｍａｔｉｃａｌ Ｐｒｏ．

ｇｒａｍｍｉｎｇ，２０１３，１４１（１）：１３５－１６３．
【１４０］ Ｎｏｃｅｄａｌ Ｊ．Ｕｐｄａｔｉｎｇ ｑｕａｓｉ－Ｎｅｗｔｏｎ ｍａｔｒｉｃｅｓ ｗｉｔｈ ｌｉｍｉｔｅｄ ｓｔｏｒａｇｅ［Ｊ［．Ｍａｔｈｅｍａｔｉｃｓ ｏｆ ｃｏｍｐｕｔａｔｉｏｎ，

１９８０．３５（１５１１：７７３．７８２．
［１４１］ ～－ｍｉｊｏ Ｌ．Ｍｉｎｉｍｉｚａｆｉｏｎ ｏｆｆｕｎｃｆｉＯｎｓ ｈａｖｉｎｇ Ｌｉｐｓｃｈｉｔｚ ｃｏｎｔｉｎｕｏｕｓ ｆｉｒｓｔ ｐａｒｔｉａｌ ｄｅｒｉｖａｔｉｖｅｓ［Ｊ］．Ｐａｃｉｆｉｃ
Ｊｏｕｒｎａｌ ｏｆＭａｔｈｅｍａｒｉｅｓ，１９６６，ｌ ６ｆ１１：１－３．

【１４２］ Ｗｂｌｆｅ Ｐ．Ｃｏｎｖｅｒｇｅｎｃｅ Ｃｏｎｄｉｔｉｏｎｓ ｆｏｒ Ａｓｃｅｎｔ Ｍｅｔｈｏｄｓ［Ｊ］．ＳＬ州Ｒｅｖｉｅｗ，１９６９，１１ｆ２）：２２６．２３５．

万方数据
 浙江大学博士专业学位论文 参考文献

１２８

万方数据
 浙江大学博士专业学位论文 附录

附录

Ａ 固定步长采样泛函积分的连续不可徼性

给定一段多项式轨迹ｐ（￡），ｔ∈【０，卅，其系数向量为ｃ。记任一以其为参数的泛函为
Ｇ（ｃ，Ｔ，ｔ），按固定时间Ｉ＇ａ－Ｊ隔６取ｔ在其上采样，使用梯形积分计算出轨迹的一个代价值，

如图Ａ．１所示，记作如下：

歹（ｃ，Ｔ，６）＝６∑ｗｊＧ（ｃ，Ｔ，Ｊ６）
Ｊ２０
（Ａ一１）

＋；（Ｔ—ｎ占）【毋（ｃ，Ｔ，ｎ占）＋毋（ｃ，Ｔ，Ｔ）］，礼＝ｌ了Ｔ
其中ｎ＝【叫副为采样点个数，（ＣｄＯ，ｕ１，…，％一，，％）＝（１／２，ｌ，・一，１，１／２）为梯形求积
法则中的系数。

－Ⅵｖ’～Ｖ 、

．八
司
Ｉ
＼厂√
Ｕ ／ｌ

简絮吵罔６日６硝６两６吲６
Ｉ Ｉ
７
Ｉ 莩 。一Ｉｌ
ｔ＝－８５ ｔ

图Ａ．１固定步长采样泛函积分牙｝意图

有如下命题成立：

命题２．若泛函９（ｃ，Ｔ，ｔ）关于ｃ和Ｔ连续，则函数Ｓ（ｃ，Ｔ，６）关于ｃ和Ｔ连续。

证明．将Ｔ视作常量，当Ｔ不变时，采样点个数ｎ不变，若泛函ａ（ｃ，Ｔ，ｔ）关于ｃ的变

化连续，则显然函数了（ｃ，Ｔ，Ｊ）关于ｃ连续。

将Ｃ视作常量，当Ｔ变化时，采样点个数几可能变化，取莱一

Ｔ＝丑＝ｋ８＋６一Ｅ，ｋ∈Ｎ＋， （Ａ一２）

１２９

万方数据
 浙江大学博士专业学位论文 附录

其中ｅ为正无穷小量，此时ｎ＝后，函数了（ｃ，Ｔ，ｄ）计算为

１
况ｃｃ，正，６，＝６
一ｋ－１：９
ｃｃ，正，Ｊ∞＋互１９
ｃｃ，丑，七∞］。Ａ一３，
＋三（６一ｅ）【夕（ｃ，矗，忌６）＋９（ｃ，ｒｌ，ｋｃｆ＋５一ｅ）】．
取另一

Ｔ＝Ｔ２＝凫６＋６， （Ａ一４）

此时ｎ＝七十１，函数Ｊ（ｃ，正，Ｊ）计算为

孙剐＝６防删＋圭ｊ＝ｌ绯忍弼垮１㈦驸川）］ （Ａ－５）

当ｅ趋近于０时，即在采样点个数礼发生变化的时刻点处，计算历（ｃ，丑，巧）与

况（ｃ，死，６）的极限值，若二者相等，则说明函数歹（ｃ，Ｔ，６）关于Ｔ连续。
由于ｔｉｍ。－＋ｏ眵（ｃ，乃，ｔ）一乡（ｃ，正，￡）】＝０，可计算

ｌ。ｉ．＋ｍ。［历（ｃ，丑，６）一况（ｃ，马，６）］
吖）ｆ

２≮滁麓Ｇ¨（ｃ，Ｔｈ，ｋ５＋町６Ｔ
＝０
＋言Ｅ［夕（ｃ， ，七ｄ）＋夕（ｃ， Ｔ
，七ｄ＋ｄ—Ｅ）］｝
．㈣

得证。

万方数据
 浙江大学博士专业学位论文 攻读学位期间科研成果

作者简历

叶鸿凯，男，浙江云和人，２０１３年９月至２０１７年６月就读于浙江大学控制科学与

工程学院，学习自动化（控制）专业，获得工学学士学位。２０１７年９月保研进入浙江大

学控制科学与工程学院攻读硕士学位，师从许超教授，并于２０２０年３月转为硕博连读，

师从高飞特聘副研究员、许超教授。攻读研究生学位期间，致力于无人系统自主导航，

尤其是空中机器人运动规划方面的研究。

万方数据
 浙江大学博士专业学位论文 攻读学位期间科研成果

１３２

万方数据
 浙江大学博士专业学位论文 攻读学位期间科研成果

攻读学位期间科研成果
１．ＴＧＫ－Ｐｌａｎｎｅｒ：Ａｎ Ｅｆｆｉｃｉｅｎｔ Ｔｏｐｏｌｏｇｙ Ｇｕｉｄｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｅｒ ｆｏｒ Ａｕｔｏｎｏｍｏｕｓ

Ｑｕａｄｒｏｔｏｒｓ．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ．２０２１，６（２）：４９４－５０１．（ＳＣＩ，第一

作者）

２．Ｌｅａｒｎｉｎｇ—ｂａｓｅｄ ３Ｄ Ｏｃｃｕｐａｎｃｙ Ｐｒｅｄｉｃｔｉｏｎ ｆｏｒ Ａｕｔｏｎｏｍｏｕｓ Ｎａｖｉｇａｔｉｏｎ ｉｎ Ｏｃｃｌｕｄｅｄ Ｅｎ—

ｖｉｒｏｎｍｅｎｔｓ．ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ．２０２１，

４５０９－４５１６．（ＥＩ，共同第一作者）

３．Ｇｅｎｅｒａｔｉｎｇ ｌａｒｇｅ—ｓｃａｌｅ ｔｒａｊｅｃｔｏｒｉｅｓ ｅｆｆｉｃｉｅｎｔｌｙ ｕｓｉｎｇ ｄｏｕｂｌｅ ｄｅｓｃｒｉｐｔｉｏｎｓ ｏｆｐｏｌｙｎｏｍｉａｌｓ．

ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｒｏｂｏｔｉｃｓ ａｎｄ Ａｕｔｏｍａｔｉｏｎ．２０２ １，７４３６－７４４２．（ＥＩ，第

二作者１

４．ＥＧＯ－Ｐｌａｎｎｅｒ：Ａｎ ＥＳＤＦ—ｆｒｅｅ ｇｒａｄｉｅｎｔ－ｂａｓｅｄ ｌｏｃａｌ ｐｌａｎｎｅｒ ｆｏｒ ｑｕａｄｒｏｔｏｒｓ．ＩＥＥＥ Ｒｏｂｏｔｉｃｓ

ａｎｄ Ａｕｔｏｍａｔｉｏｎ Ｌｅｔｔｅｒｓ．２０２０，６（２）：４７８－４８５．（ＳＣＩ，第三作者）

５．Ａｎ Ｅｘｐｅｒｉｍｅｎｔ Ｉｍｐｌｅｍｅｎｔａｔｉｏｎ ｏｆＯｕｔｄｏｏｒ Ｆｏｒｍａｔｉｏｎ Ｆｌｉｇｈｔ．ＩＥＥＥ Ａｎｎｕａｌ Ｉｎｔｅｒｎａｔｉｏｎａｌ

Ｃｏｎｆｅｒｅｎｃｅ ｏｎ ＣＹＢＥＲ Ｔｅｃｈｎｏｌｏｇｙ ｉｎ Ａｕｔｏｍａｔｉｏｎ，Ｃｏｎｔｒｏｌ，ａｎｄ Ｉｎｔｅｌｌｉｇｅｎｔ Ｓｙｓｔｅｍｓ．

２０１８，９３—９８．（Ｅｌ，第一作者）

６．Ｓｐａｔｉｏ—ｔｅｍｐｏｒａｌ Ｄｅｆｏｒｍａｂｌｅ Ｔｒｅｅｓ ｆｏｒ Ｓａｍｐｌｉｎｇ－ｂａｓｅｄ Ｍｕｌｔｉｒｏｔｏｒ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎ—

ｎｉｎｇ．ＩＥＥＥ Ｉｎｔｅｒｎｍｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ ａｎｄ Ｓｙｓｔｅｍｓ．２０２２，审稿中

（ＥＩ，第一作者）

７．Ｅｆｆｉｃｉｅｎｔ Ｓａｍｐｌｉｎｇ－ｂａｓｅｄ Ｋｉｎｏｄｙｎａｍｉｃ Ｐｌａｎｎｉｎｇ ｗｉｔｈ Ｒｅｇｉｏｎａｌ Ｏｐｔｉｍｉｚａｔｉｏｎ ａｎｄ Ｂｉｄｉ—

ｒｅｃｔｉｏｎａｌ Ｓｅａｒｃｈ ｆｏｒ Ｍｕｌｔｉｒｏｔｏｒｓ．ＩＥＥＥ Ｉｎｔｅｒｎａｔｉｏｎａｌ Ｃｏｎｆｅｒｅｎｃｅ ｏｎ Ｉｎｔｅｌｌｉｇｅｎｔ Ｒｏｂｏｔｓ

ａｎｄ Ｓｙｓｔｅｍｓ．２０２２，审稿中（Ｅｌ，第一作者）

８．Ｆｌｅｘｉｂｌｅ Ｌｏｃａｌ Ｒｅｐｌａｎｎｉｎｇ ｆｏｒ Ｍｕｌｔｉｒｏｔｏｒｓ Ｔｒａｊｅｃｔｏｒｙ Ｆｏｌｌｏｗｉｎｇ．撰写中（第一作者）

９．一种考虑视场角限制以及运动不确定性的结合记忆的旋翼飞行器高速飞行避障方

法：ＣＮ １０９６３４３０１ Ｂ ２０２１．０１．２６（授权发明专利，第一作者）

１３３

万方数据
 浙江大学博士专业学位论文 攻读学位期间科研成果

１０．一种四旋翼飞行器室外编队灯光表演系统及控制方法：ＣＮ １０８１５３３２７８２０２０．０５．１９

（授权发明专利，第二作者）

１１．２０１７国际空中机器人大赛ＩＡＲＣ第一名

１２．２０１８国际空中机器人大赛ＩＡＲＣ第一名，第七代任务总冠军（第一完成人）

１３．ＩＣＲＡ ２０１８ＤＪＩＲｏｂｏＭａｓｔｅｒ国际人工智能挑战赛决赛圈（第一完成人）

万方数据