CN115903894A - 基于改进的aapf-irrt算法的无人机轨迹规划及跟踪控制方法 - Google Patents

基于改进的aapf-irrt算法的无人机轨迹规划及跟踪控制方法 Download PDF

Info

Publication number
CN115903894A
CN115903894A CN202211448781.1A CN202211448781A CN115903894A CN 115903894 A CN115903894 A CN 115903894A CN 202211448781 A CN202211448781 A CN 202211448781A CN 115903894 A CN115903894 A CN 115903894A
Authority
CN
China
Prior art keywords
algorithm
unmanned aerial
aerial vehicle
track
irrt
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Pending
Application number
CN202211448781.1A
Other languages
English (en)
Inventor
单梁
刘文倩
马强
李军
戚志东
颜建虎
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Nanjing University of Science and Technology
Original Assignee
Nanjing University of Science and Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Nanjing University of Science and Technology filed Critical Nanjing University of Science and Technology
Priority to CN202211448781.1A priority Critical patent/CN115903894A/zh
Publication of CN115903894A publication Critical patent/CN115903894A/zh
Pending legal-status Critical Current

Links

Images

Classifications

    • Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
    • Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
    • Y02T—CLIMATE CHANGE MITIGATION TECHNOLOGIES RELATED TO TRANSPORTATION
    • Y02T10/00—Road transport of goods or passengers
    • Y02T10/10—Internal combustion engine [ICE] based vehicles
    • Y02T10/40—Engine management systems

Landscapes

  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)

Abstract

本发明公开了一种基于改进的AAPF‑IRRT算法的无人机轨迹规划方法，包括：采用人工势场法与自适应步长方法对Informed RRT*算法进行改进，在Informed RRT*算法的基础上引入人工势场法指引随机采样点向目标点生成，引入自适应步长调整策略以加速随机树在无障碍环境下的扩展速度，得到改进的AAPF‑IRRT算法，并获得无人机飞行轨迹的离散坐标点；利用minimum snap算法结合闭式求解、飞行走廊、时间分配对步骤S02中的离散坐标点进行拟合，获得无人机的飞行轨迹。大大加快了快速搜索随机树算法向渐进最优路径收敛的速度；能够在使得无人机飞行轨迹光滑的前提下保证轨迹不与障碍物发生碰撞。

Description

基于改进的AAPF-IRRT算法的无人机轨迹规划及跟踪控制方法
技术领域
本发明属于轨迹规划和自动控制技术领域，本发明涉及一种基于改进的AAPF-IRRT算法的无人机轨迹规划及跟踪控制方法。
背景技术
随着机器人技术的发展，各种各样以无人系统技术为基础的智能设备(无人机、无人车等)被广泛应用于轨道交通、物流运输、巡查勘探、应急救灾等多个领域。为完成物资配送任务，无人机须在有大量复杂障碍物的环境中完成路径规划。而在进行喷洒消毒的过程中，无人机保持在固定高度飞行，在进行路径规划时可以将三维路径规划问题降至二维以简化问题的复杂度。此外由于无人机续航时间的限制，如何为无人机在二维和三维环境中以更快的速度规划出一条长度更短、轨迹更优的路径依旧是当前研究的热门方向。最终需要构建合适的飞行控制器，对无人机进行控制以实现对规划轨迹的精确跟踪。
无人机航迹规划分为前端的路径规划和后端航迹平滑两个部分。现有的路径规划算法分为基于搜索的路径规划(Dijkstra、A*)、基于概率的路径规划(概率路图法PRM、快速扩展随机树RRT)、基于势场的算法(人工势场法)以及基于智能算法(神经网络、遗传算法、蚁群算法)的路径规划。但是这些算法在理论上都存在一定的不足。例如人工势场法存在局部最小值的问题，A*算法应用在较大空间或高维空间中，运算量会非常大，而智能算法则有泛化性差、参数调节困难的缺点。快速扩展随机树算法无需进行栅格地图的建立，运算量小，适合较大空间和高维空间中的路径规划。但该算法也存在由于采样的随机性使得算法每次规划出的路径不一致，无法得到最优路径，路径规划速度缓慢的问题。
在进行完路径规划后，会生成一系列的离散空间点，这样生成的轨迹无法适应于无人机运动。主要表现为：1.无人机的速度和加速度不能立刻突变；2.无人机不能停下来再进行转向；3.消耗能量。需要对生成的这些离散空间点进行轨迹规划，即在路径规划的基础上加入时间序列信息，对无人机执行任务时的速度与加速度进行约束，常见的轨迹规划方法为多项式插值的方式。为了使得能耗最低且轨迹光滑，一种有效的规划方式是minmumsnap算法，但是缺点有二：其一是缺乏一种有效的轨迹时间分配策略，导致生成的轨迹容易出现“打结”现象，降低轨迹质量；其二是生成的轨迹有可能偏离原轨迹导致碰到障碍物。
例如申请号2019102367291公开了一种基于A*/minimum_snap算法的无人机轨迹控制方法，采用A*算法获得无人机运动轨迹的离散坐标点；利用minimum_snap算法对轨迹的离散坐标点进行拟合，获得规划的运动轨迹。A*算法应用在较大空间或高维空间中，运算量会非常大；minmum snap算法缺乏一种有效的轨迹时间分配策略，导致生成的轨迹容易出现“打结”现象，降低轨迹质量；其二是生成的轨迹有可能偏离原轨迹导致碰到障碍物。
此外传统的PID控制器在对轨迹进行跟踪时易出现超调、积分饱和、抗干扰能力差的问题，而现阶段控制理论虽取得了较大的发展，但很少被应用到工业控制中。
发明内容
本发明的目的在于提供一种基于改进的AAPF-IRRT算法的无人机轨迹规划及跟踪控制方法，采用人工势场法指引采样点向目标点移动以及自适应步长扩展策略，并最终提出AAPF-IRRT路径规划算法，大大加快了RRT树扩展并收敛到渐进最优的速度；使用时间分配、飞行走廊约束、闭式求解、轨迹指引为minimum snap算法提供了较好的约束，避免minimum snap算法生成的多项式轨迹存在“打结”现象以及生成轨迹大大偏离前端AAPF-IRRT算法规划出来的路径；最后在姿态控制方面采用抗干扰能力强、易于调参、稳定性强的ADRC控制器进行姿态环的控制，使得无人机能够更快更稳地跟踪生成的轨迹。
实现本发明目的的技术解决方案为：
一种基于改进的AAPF-IRRT算法的无人机轨迹规划方法，包括以下步骤：
S01：采用人工势场法与自适应步长方法对Informed RRT*算法进行改进，在Informed RRT*算法的基础上引入人工势场法指引随机采样点向目标点生成，引入自适应步长调整策略以加速随机树在无障碍环境下的扩展速度，得到改进的AAPF-IRRT算法；
S02：采用改进的AAPF-IRRT算法获得无人机飞行轨迹的离散坐标点；
S03：利用minimum snap算法结合闭式求解、飞行走廊、时间分配对步骤S02中的离散坐标点进行拟合，获得无人机的飞行轨迹。
优选的技术方案中，所述步骤S01中随机采样点的生成方法包括：
S11：在自由空间Xfree中生成随机点xrand，并令xprand＝xrand；
S12：计算xprand与障碍物的最近距离dmin，当dmin>Dobs且xprand≠xgoal时，Dobs为临界距离，执行步骤S13，否则返回xprand；
S13：计算xprand与目标点xgoal之间的吸引力势能Uatt：
Figure BDA0003950578860000041
其中，Ka表示吸引力势场比例因子，参数
Figure BDA0003950578860000042
是定义的xprand与xgoal的临界范围，d(xprand,xgoal)为随机点xprand与目标点xgoal之间的距离；
S14：对吸引力势能Uatt求负梯度得到随机点xprand与目标点xgoal之间的吸引力
Figure BDA0003950578860000043
Figure BDA0003950578860000044
S15：使xprand沿着
Figure BDA0003950578860000045
的方向以步长η移动：
Figure BDA0003950578860000046
循环执行步骤S12～S15。
优选的技术方案中，所述步骤S11中的自由空间采样，在寻找到第一条从起始点到目标点的可达路径后，进行椭圆/椭球采样域的限制，采样空间也由自由空间Xfree变为椭圆/椭球空间Xellipse，所述椭圆 /椭球空间的生成方法包括：
S111：得到第一条可达路径的路径长度Cbest并将其作为椭圆的长轴长，将起点xstart和目标点xgoal作为椭圆的焦点；
S112：计算起点和目标点之间的距离为cmin，cmin＝||xgoal-xstart||2，并计算起点与目标点的中心xcenter，xcenter＝(xstart+xgoal)/2；
S113：计算超椭球体横轴a1与单位矩阵l1的外积M＝a1×l1，a1＝(xgoal -xstart)/cmin，l1＝[1,0,0]；
S114：对M进行奇异值分解：U∑VT≡M，U和V是两组正交单位向量，∑为进行奇异值分解后的对角矩阵；
S115：计算从椭球坐标系到世界坐标系的旋转矩阵 C＝Udiag{1,...,1,det(U)det(V)}VT，det(U)与det(V)分别为U和V的行列式，VT为矩阵V的转置，diag表示对角矩阵；
S116：从n维单位球中均匀分布的样本转换到超椭球中均匀分布的样本所需的变换矩阵L：
Figure BDA0003950578860000051
S117：得到从n维单位球转换到世界坐标系下的随机采样点xrand， xrand＝CLxball+xcenter，式中xball为在单位球中的采样点。
优选的技术方案中，所述步骤S01中改进的AAPF-IRRT算法采用自适应步长策略扩展新节点xnew，扩展新节点的方法包括：
S011：找到随机树中距离xprand最近的临近点xnearest，沿着xnearest与xprand的连线方向扩展步长step，生成xnew：
Figure BDA0003950578860000061
S012：若xnew与xnearest的连线未经过障碍物且步长未达到步长上限stepmax，则以倍数ks扩展步长step，step＝step*ks，并以该步长重新扩展xnew，否则直接返回xnew；
S013：循环执行步骤S012。
优选的技术方案中，所述步骤S03中离散点拟合方法包括：
S31：对无人机航点每个维度单独求解其轨迹，以下的求解步骤均为单个维度；
S32：将轨迹P(t)按照时间段Tn-1～Tn(n＝1,...,n-1)分割为n-1段，第 n段轨迹记为Pn(t)(n＝1,...,n-1)，并用五次多项式表示，式中i为多项式中每一项的次数，pn,i(n＝1,...,n-1)为第n段轨迹相应项次数对应的系数：
Figure BDA0003950578860000062
S33：求n-1段轨迹每段Pn(t)(n＝1,...,n-1)的最小加加加速度
Figure BDA0003950578860000071
Figure BDA0003950578860000072
为第n段轨迹Pn(t)的四阶倒数：
Figure BDA0003950578860000073
S34：求使得
Figure BDA0003950578860000074
最小的凸二次规划表达式Jn(T)；
Figure BDA0003950578860000075
式中T为第n段轨迹从Tn-1～Tn的时间，l与i均为多项式中每一项的次数，
Figure BDA0003950578860000076
为定义符号，即
Figure BDA0003950578860000077
Figure BDA0003950578860000078
S35：对每个离散点已知的位置p、速度v、加速度a构造等式约束，以T0时刻的离散点为例，位置约束：
Figure BDA0003950578860000079
式中 p0为T0时刻对应点的位置；速度约束：
Figure BDA00039505788600000710
式中P1'(T0)为P1(T0)在T0时刻的导数，v0为T0时刻对应点的速度；加速度约束：
Figure BDA0003950578860000081
式中P1"(T0)为P1(T0)在 T0时刻的导数；
S36：根据相邻轨迹段之间的位置、速度、加速度连续构建等式约束，即第n段和第n-1段轨迹的各阶导数相等：
Figure BDA0003950578860000082
式中
Figure BDA0003950578860000083
和
Figure BDA0003950578860000084
分别为两个相邻段轨迹Pn-1(t)和Pn(t)在相应时间Tn-1和Tn处的k阶导，k＝0，1，2，分别表示了位置、速度和加速度，l与i均为多项式中每一项的次数，
Figure BDA0003950578860000085
Figure BDA0003950578860000086
优选的技术方案中，所述步骤S03中minimum snap算法对二次正定型进行闭式求解，其步骤为：
S031：将多项式轨迹转化为带有具体物理含义的速度、加速度，即当前点位置的各阶微分记为dn，构造矩阵Mn将pn映射为pn的微分 dn，即Mn·pn＝dn，然后将目标函数化为如下形式：
Figure BDA0003950578860000091
式中
Figure BDA0003950578860000092
S032：使用选择矩阵C将dn分解为自由变量dP和约束变量dF，自由变量是指实际求解过程中只知道当前点的位置而不知道其速度、加速度、加加速度的变量，而约束变量是知道其位置、速度、加速度等约束的量：
Figure BDA0003950578860000093
S033：重新带入dn到二次规划表达式Jn(T)中，得到如下形式：
Figure BDA0003950578860000094
式中将矩阵
Figure BDA0003950578860000095
定义为矩阵R，RFF、RFP、RPF、RPP分别为矩阵R的左上角、右上角、左下角、右下角的分块矩阵；
S034：求得使目标函数最小的自由变量的表达式
Figure BDA0003950578860000096
Figure BDA0003950578860000097
优选的技术方案中，所述步骤S03中minimum snap算法的时间分配为梯形时间分配，在一段轨迹中无人机以最大加速度加速到最大速度后匀速、再以最大加速度减速到目标点速度，时间分配准则按照如下函数分配各段轨迹时间：
Figure BDA0003950578860000101
式中，Sj代表第j段轨迹两端点之间的直线距离，vm代表无人机的最大速度，am代表无人机的最大加速度，T(Sj)代表输入量为Sj、输出为轨迹的分配时间。
优选的技术方案中，所述步骤S03中minimum snap算法的飞行走廊约束的方法包括：
S131：在两个离散路径点的连线之间采样多个中间点q，对每个中间点均施加立方体走廊约束；
S132：将所有中间点受到的立方体约束表示为不等式约束的形式，加入到二次规划问题的约束中，对每个中间点q，设置立方体的边长为r，以第n段轨迹为例，对每个中间点增加两个不等式约束：
Figure BDA0003950578860000102
Figure BDA0003950578860000103
式中tq为在中间点q的时刻，Pn(tq)可以根据tq计算得出，
Figure BDA0003950578860000104
本发明还公开了一种基于改进的AAPF-IRRT算法的无人机轨迹跟踪控制方法，采用上述的无人机轨迹规划方法得到无人机的飞行轨迹；
使用自适应控制方法对无人机进行控制以跟踪生成的飞行轨迹，其步骤为：
S41：将无人机的控制分为姿态环和位置环，使用自抗扰控制器对姿态环进行控制；所述自抗扰控制器包括TD跟踪微分器、NLSEF 非线性状态反馈、及ESO扩张观测器；
S42：对TD跟踪微分器、NLSEF非线性状态反馈及ESO扩张观测器进行参数调节，使得无人机真实轨迹跟踪期望轨迹；
S43：对位置环采用PID控制器进行控制，并对比例、积分、微分三个环节参数进行调节跟踪性能。
本发明又公开了一种计算机存储介质，其上存储有计算机程序，所述计算机程序被执行时实现上述的基于改进的AAPF-IRRT算法的无人机轨迹跟踪控制方法。
本发明与现有技术相比，其显著优点为：
(1)本发明改进的AAPF-IRRT算法的无人机路径规划方法，采用融合人工势场和自适应步长的Informed RRT*算法，可以有效地指引树的生成，大大加快随机树收敛的速度，从而为无人机在更短的时间内规划出一条路径更优的航迹点。
(2)原算法Informed RRT*使用椭圆采样域的方式对采样点进行了限制，加快了树收敛的速度，但这是在找到第一条可达路径之后，然而在复杂环境中找打第一条路径的时间往往很长。因此本发明的改进的AAPF-IRRT路径规划算法引入目标引力场，由此来引导随机树向目标方向生长，解决了算法随机盲目、缺乏导向性的问题，因此能够很大程度上缩短原算法的搜索时间。
(3)考虑到输入全局地图的复杂度，原算法在进行随机树的扩展时采用的使固定步长step，这种方式使得随机树的扩展十分缓慢，本发明加入自适应步长扩展策略，使得在无障碍环境中扩展步长可以进行自适应调节，直到扩展到步长上限stepmax为止，该方法大大提高了随机树扩展的速度，使得算法收敛更快。
(4)本发明中改进的AAPF-IRRT算法能够为无人机规划出一条从起点到终点有效的离散点，但该路径比较曲折并不光滑，无人机沿着这样的轨迹行驶会导致大量的能量损失，因此需要对前端生成的路径点进行平滑处理，使得处理之后各个节点之间的过渡更加光滑。基于minimum snap算法的轨迹优化是为了实现无人机的加加加速度变化率最小，但该算法易存在节点打结、平滑轨迹偏离原来轨迹的现象。本发明通过加入梯形时间分配、飞行走廊约束以及轨迹指引的方式为 minimum snap算法加入更多的约束，使得规划出的飞行轨迹更加平滑且不会碰到障碍物。
(5)本发明针对AAPF-IRRT和minimum snap算法生成的轨迹进行跟踪时采用的是ADRC和PID混合的控制方式，姿态环采用ADRC 进行控制，位置环采用PID进行控制，主要是为了提高控制器抗干扰的能力。
附图说明
图1是本实施例基于改进的AAPF-IRRT算法的无人机轨迹规划及跟踪控制方法整体流程图；
图2是本实施例IRRT*节点扩展示意图；
图3是本实施例算法椭圆采样域示意图；
图4是本实施例AAPF-IRRT算法流程图；
图5是本实施例的IRRT*算法规划的初始路径；
图6是本实施例的APF-IRRT*算法规划的初始路径；
图7是本实施例IRRT*算法规划的最终路径；
图8是本实施例的APF-IRRT*方法规划出的最终路径；
图9是本实施例加自适应步长后的IRRT*算法规划的初始路径；
图10是本实施例加自适应步长后的IRRT*算法规划的最终路径；
图11是本实施例AAPF-IRRT算法规划的初始路径；
图12是本实施例AAPF-IRRT算法规划的最终路径；
图13是本实施例后端航迹平滑minimum snap算法的实施流程图；
图14是原始minimum snap算法在三维中规划出的平滑轨迹；
图15是二维俯视x-y平面下minimum snap算法存在打结现象；
图16是加入时间分配、飞行走廊约束后的三维轨迹图；
图17是二维俯视x-y平面下加入时间分配、飞行走廊约束后的轨迹图；
图18是位置外环PID控制框图；
图19是姿态内环自适应控制框图；
图20是四旋翼无人机整体控制流程图。
具体实施方式
本发明的原理是：针对RRT算法进行了一系列改进，使其快速收敛到渐进最优，找到一条路径更短、轨迹更优的轨迹以适合无人机飞行。结合时间分配、飞行走廊约束、闭式求解、轨迹指引方法，将无人机的轨迹规划问题转化为数学上的二次规划问题进行求解，生成一条能耗最低、轨迹光滑的轨迹。选用以PID为主体的自抗扰控制器ADRC对无人机的姿态环进行跟踪控制，以实现对外界抗干扰的功能。
实施例1：
一种基于改进的AAPF-IRRT算法的无人机轨迹规划方法，包括以下步骤：
S01：采用人工势场法与自适应步长方法对Informed RRT*算法进行改进，在Informed RRT*算法的基础上引入人工势场法指引随机采样点向目标点生成，引入自适应步长调整策略以加速随机树在无障碍环境下的扩展速度，得到改进的AAPF-IRRT算法；
S02：采用改进的AAPF-IRRT算法获得无人机飞行轨迹的离散坐标点；
S03：利用minimum snap算法结合闭式求解、飞行走廊、时间分配对步骤S02中的离散坐标点进行拟合，获得无人机的飞行轨迹。
另一实施例，一种基于改进的AAPF-IRRT算法的无人机轨迹跟踪控制方法，采用上述的无人机轨迹规划方法得到无人机的飞行轨迹；
使用自适应控制方法对无人机进行控制以跟踪生成的飞行轨迹，其步骤为：
S41：将无人机的控制分为姿态环和位置环，使用自抗扰控制器对姿态环进行控制；所述自抗扰控制器包括TD跟踪微分器、NLSEF 非线性状态反馈、及ESO扩张观测器；
S42：对TD跟踪微分器、NLSEF非线性状态反馈及ESO扩张观测器进行参数调节，使得无人机真实轨迹跟踪期望轨迹；
S43：对位置环采用PID控制器进行控制，并对比例、积分、微分三个环节参数进行调节跟踪性能。
又一实施例中，一种计算机存储介质，其上存储有计算机程序，所述计算机程序被执行时实现上述的基于改进的AAPF-IRRT算法的无人机轨迹规划及跟踪控制方法。
下面以一较佳的实施例为例进行详细说明基于改进的 AAPF-IRRT算法的无人机轨迹规划及跟踪控制方法的流程：
本实施例的基于改进的AAPF-IRRT/minimum snap算法的无人轨迹规划及跟踪控制方法，尤其适用于四旋翼，具体的实施流程见图1，具体实施方式详述如下：
步骤1，采用人工势场法与自适应步长方法对Informed RRT*算法进行改进：
首先在Informed RRT*算法的基础上引入人工势场法指引随机采样点向目标点生成，这样的改进使得随机树的扩展更具有目的性；其次引入自适应步长调整策略以加速随机树在无障碍环境下的扩展速度；
步骤2，采用改进的AAPF-IRRT算法获得无人机飞行轨迹的离散坐标点：
首先引入膨胀半径对障碍物进行膨胀，膨胀半径为无人机的质心到螺旋桨的长度，这可使得无人机在真实环境中飞行时能够避开特别狭窄的区域，规划出的路径点相对安全；然后采用改进的AAPF-IRRT 算法得到一条从初始点到目标点的无碰撞路径，该路径是空间中一系列离散的点，路径点个数为n；
步骤3，利用minimum snap算法结合闭式求解、飞行走廊、时间分配对步骤2中的离散坐标点进行拟合，获得适合无人机运动动力学的飞行轨迹：
首先根据步骤2生成的离散点将轨迹分为n-1段，将每段轨迹使用一个多项式函数表示，对每个维度分别使用minimum snap的方法进行闭式求解；接着控制无人机的加加加速度(snap)最小以使得损耗能量最小，通过将多项式轨迹四阶导数平方的积分值作为代价函数的方式，将该问题转化为二次正定型的凸优化问题；其次通过各段轨迹连接点的位置、速度、加速度相等，以及无人机速度、加速度的上限对代价函数进行约束；最后为了使得拟合后的轨迹与原轨迹更贴合以避免无人机碰到障碍物，加入飞行走廊进行约束，再经过时间分配最终得到一条适合无人机动力学的飞行轨迹；
步骤4，使用自适应控制(Active Disturbance Rejection Control, ADRC)方法对四旋翼无人机进行控制以跟踪步骤3中生成的飞行轨迹：
针对四旋翼无人机滚转、俯仰、偏航角之间强耦合以及在空中飞行时极易受到风扰的特质，首先对其内环姿态环采用ADRC方法进行控制，接着对外环位置环采用PID方法进行控制。
一较佳的实施，步骤1，采用人工势场法与自适应步长方法对 Informed RRT*(IRRT*)算法进行改进，改进后的整体算法流程如图 4所示：
步骤1中改进的AAPF-IRRT算法融合人工势场法于IRRT*算法，改进算法随机采样点的生成步骤为：
步骤1.1、在自由空间Xfree中生成随机点xrand，并令xprand＝xrand；
步骤1.2、计算xprand与障碍物的最近距离dmin，当dmin>Dobs(临界距离)且xprand≠xgoal时，进行步骤1.3，否则返回xprand；
步骤1.3、计算xprand与目标点xgoal之间的吸引力势能Uatt，Ka表示吸引力势场比例因子，用来调节吸引势能的大小，参数
Figure BDA0003950578860000171
是定义的xprand与xgoal的临界范围，d(xprand,xgoal)为随机点xprand与目标点xgoal之间的距离；
Figure BDA0003950578860000172
步骤1.4、对吸引力势能求负梯度得到随机点xprand与目标点xgoal之间的吸引力
Figure BDA0003950578860000173
Figure BDA0003950578860000174
步骤1.5、使xprand沿着
Figure BDA0003950578860000181
的方向以步长η移动；
Figure BDA0003950578860000182
步骤1.6、循环步骤1.2～1.5。
如图5～图8分别为原算法IRRT*、人工势场法指引采样后 APF-IRRT*算法生成的初始轨迹和最终轨迹，可以看出改进后的算法由于规划出的初始路径较短，采样域可以迅速限制在更小的椭圆域中，加速树的收敛速度，使得规划出的轨迹更优。
如图2所示，一较佳的实施，步骤1.1中的自由空间采样，在寻找到第一条从起始点到目标点的可达路径后，会进行椭圆/椭球采样域的限制，采样空间也由自由空间Xfree变为椭圆/椭球空间Xellipse。
具体的，如图3所示，椭圆/椭球空间的生成步骤为：
步骤2.1、得到第一条可达路径的路径长度Cbest并将其作为椭圆的长轴长，将起点xstart和终点xgoal作为椭圆的焦点；
步骤2.2、计算起点和终点之间的距离为cmin，cmin＝||xgoal-xstart||2，计算起点与终点的中心xcenter，xcenter＝(xstart+xgoal)/2；
步骤2.3、计算超椭球体横轴a1与单位矩阵l1的外积，a1＝(xgoal- xstart)/cmin，M＝a1×l1；
步骤2.4、对M进行奇异值分解，U∑VT≡M，U和V是两组正交单位向量，∑为进行奇异值分解后的对角矩阵；
步骤2.5、计算从椭球坐标系到世界坐标系的旋转矩 C＝Udiag{1,...,1,det(U)det(V)}VT，det(U)与det(V)分别为U和V的行列式，VT为矩阵V的转置，diag为单位矩阵；
步骤2.6、从n维单位球中均匀分布的样本转换到超椭球中均匀分布的样本所需的变换矩阵L：
Figure BDA0003950578860000191
步骤2.7、得到从n维单位球转换到世界坐标系下的随机采样点 xrand，xrand＝CLxball+xcenter，式中xball为在单位球中的采样点。
具体的，步骤1中改进的AAPF-IRRT算法采用自适应步长策略扩展新节点xnew，改进算法扩展新节点的生成步骤为：
步骤3.1、找到随机树中距离xprand最近的临近点xnearest,沿着xnearest与xprand的连线方向扩展步长step，生成xnew；
Figure BDA0003950578860000192
步骤3.2、若xnew与xnearest的连线未经过障碍物且步长未达到步长上限stepmax，则以倍数ks扩展步长step，step＝step*ks，并以该步长重新扩展xnew，否则直接返回xnew；
步骤3.3、循环步骤3.2。
如图9～图12分别为使用IRRT*算法、APF-IRRT*加了自适应步长后的AIRRT*和AAPF-IRRT算法进行的路径规划初始路径和最终路径的对比，可以看到增加了自适应步长调整策略的算法能够在更少的迭代次数下找到第一条路经，从而更快地锁定椭圆采样域，而 AAPF-IRRT算法因为有人工势场法向目标点的采样指引，能够在最短的迭代次数下找到初始路径，从而给路径优化至渐进最优提供更多的时间，路径质量更好。
步骤3中minimum snap算法对步骤2中生成的离散点进行拟合，其具体算法流程如图13所示，其步骤为：
步骤4.1、实际中无人机航点往往是二维或三维的(具体是二维还是三维要根据无人机的飞行任务判断其高度信息是否可以直接忽略)，通常对每个维度单独求解其轨迹，以下的求解步骤均为单个维度；
步骤4.2、将n-1段轨迹均表示为多项式的形式，由于需要求最小加加加速度即轨迹的四阶倒数，因此对每段轨迹均选择用五次多项式进行表示，式中i为多项式中每一项的次数，pn,i(n＝1,...,n-1)为第n段轨迹相应项次数对应的系数；
Figure BDA0003950578860000201
步骤4.3、求n-1段轨迹每段Pn(t)(n＝1,...,n-1)的最小加加加速度
Figure BDA0003950578860000202
为第n段轨迹Pn(t)的四阶倒数；
Figure BDA0003950578860000203
步骤4.4、求使得
Figure BDA0003950578860000204
最小的凸二次规划表达式Jn(T)；
Figure BDA0003950578860000211
式中T为第n段轨迹从Tn-1～Tn的时间，l与i均为多项式中每一项的次数，
Figure BDA0003950578860000212
为定义符号，即
Figure BDA0003950578860000213
Figure BDA0003950578860000214
步骤4.5、对每个离散点已知的位置p、速度v、加速度a构造等式约束，以T0时刻的离散点为例，
位置约束：
Figure BDA0003950578860000215
式中p0为T0时刻对应点的位置；速度约束：
Figure BDA0003950578860000216
式中 P1'(T0)为P1(T0)在T0时刻的导数，v0为T0时刻对应点的速度；加速度约束：
Figure BDA0003950578860000217
式中P1"(T0)为 P1(T0)在T0时刻的导数；
步骤4.6、根据相邻轨迹段之间的位置、速度、加速度连续构建等式约束，即第n段和第n-1段轨迹的各阶导数相等：
Figure BDA0003950578860000221
式中，
Figure BDA0003950578860000222
和
Figure BDA0003950578860000223
分别为两个相邻段轨迹Pn-1(t)和Pn(t)在相应时间Tn-1和Tn处的k阶导，k＝0，1，2，分别表示了位置、速度和加速度，l与i均为多项式中每一项的次数，
Figure BDA0003950578860000224
Figure BDA0003950578860000225
步骤3中minimum snap算法中对二次正定型进行闭式求解，其步骤为：
步骤5.1、由于直接对多项式轨迹进行优化求解会带来数值不稳定性，需要将其转化为带有具体物理含义的速度、加速度等，即当前点位置的各阶微分。构造矩阵Mn将pn映射到dn，即Mnpn＝dn，然后将目标函数化为如下形式；
Figure BDA0003950578860000226
式中
Figure BDA0003950578860000231
步骤5.2、使用选择矩阵C将dn分为自由变量dP和约束变量dF；
Figure BDA0003950578860000232
步骤5.3、重新带入dn到目标函数中，得到如下形式；
Figure BDA0003950578860000233
式中将矩阵
Figure BDA0003950578860000234
定义为矩阵R，RFF、RFP、RPF、RPP分别为矩阵R的左上角、右上角、左下角、右下角的分块矩阵；
步骤5.4、求得使目标函数最小的自由变量的表达式
Figure BDA0003950578860000235
Figure BDA0003950578860000236
步骤3中minimum snap算法中的时间分配为梯形时间分配，在一段轨迹中无人机以最大加速度加速到最大速度后匀速、再以最大加速度减速到目标点速度，时间分配准则按照如下函数分配各段轨迹时间：
Figure BDA0003950578860000241
式中，Sj代表第j段轨迹两端点之间的直线距离，vm代表无人机的最大速度，am代表无人机的最大加速度，T(Sj)代表输入量为Sj、输出为轨迹的分配时间。
步骤3中minimum snap算法中的飞行走廊约束其目的在于解决算法规划出的轨迹与原始轨迹差别较大，如图14所示，且存在打结现象的问题，如图15所示，其步骤为：
步骤6.1、在两个离散路径点的连线之间多采样一些中间点k，对每个中间点均施加立方体约束，如图16所示；
步骤6.2、将所有中间点受到的立方体约束表示为不等式约束的形式，加入到二次规划问题的约束中。对每个中间点i，设置立方体的边长为r，增加两个不等式约束。
Figure BDA0003950578860000242
Figure BDA0003950578860000243
式中tq为在中间点q的时刻，Pn(tq)可以根据tq计算得出，
Figure BDA0003950578860000244
从图17可以看出，经过了飞行走廊约束、时间分配后，原来算法 minimum snap的打结现象消失。
步骤4中使用自适应控制方法对四旋翼无人机进行控制以跟踪步骤3中生成的飞行轨迹，整体控制框图如图20所示，其步骤为：
步骤7.1、将无人机的控制分为姿态环和位置环，针对姿态环的强耦合、易受干扰的问题，选用无需解耦、鲁棒性强的ADRC自抗扰控制器自适应控制方法对姿态环进行控制；
步骤7.2、如图19所示，ADRC由TD跟踪微分器，NLSEF非线性状态反馈，ESO扩张观测器组成，需要对这三个模块进行参数调节，使得无人机真实轨迹能够以更快，更准确的方式跟踪期望轨迹；
步骤7.3、由于姿态环本身就确定了无人机的水平位置，因此在高度换上采用PID的方式进行控制，如图18所示，并对比例、积分、微分三个环节参数进行调节使得跟踪性能更快更稳。
自适应控制方法能够使得无人机跟踪生成的飞行轨迹并具有抗外界扰动的性质。
上述实施例为本发明优选地实施方式，但本发明的实施方式并不受上述实施例的限制，其他的任何未背离本发明的精神实质与原理下所作的改变、修饰、替代、组合、简化，均应为等效的置换方式，都包含在本发明的保护范围之内。

Claims (10)

1.一种基于改进的AAPF-IRRT算法的无人机轨迹规划方法，其特征在于，包括以下步骤：
S01：采用人工势场法与自适应步长方法对Informed RRT*算法进行改进，在InformedRRT*算法的基础上引入人工势场法指引随机采样点向目标点生成，引入自适应步长调整策略以加速随机树在无障碍环境下的扩展速度，得到改进的AAPF-IRRT算法；
S02：采用改进的AAPF-IRRT算法获得无人机飞行轨迹的离散坐标点；
S03：利用minimum snap算法结合闭式求解、飞行走廊、时间分配对步骤S02中的离散坐标点进行拟合，获得无人机的飞行轨迹。
2.根据权利要求1所述的基于改进的AAPF-IRRT算法的无人机轨迹规划方法，其特征在于，所述步骤S01中随机采样点的生成方法包括：
S11：在自由空间Xfree中生成随机点xrand，并令xprand＝xrand；
S12：计算xprand与障碍物的最近距离dmin，当dmin>Dobs且xprand≠xgoal时，Dobs为临界距离，执行步骤S13，否则返回xprand；
S13：计算xprand与目标点xgoal之间的吸引力势能Uatt：
Figure FDA0003950578850000011
其中，Ka表示吸引力势场比例因子，参数
Figure FDA0003950578850000012
是定义的xprand与xgoal的临界范围，d(xprand,xgoal)为随机点xprand与目标点xgoal之间的距离；
S14：对吸引力势能Uatt求负梯度得到随机点xprand与目标点xgoal之间的吸引力
Figure FDA0003950578850000021
Figure FDA0003950578850000022
S15：使xprand沿着
Figure FDA0003950578850000023
的方向以步长η移动：
Figure FDA0003950578850000024
循环执行步骤S12～S15。
3.根据权利要求2所述的基于改进的AAPF-IRRT算法的无人机轨迹规划方法，其特征在于，所述步骤S11中的自由空间采样，在寻找到第一条从起始点到目标点的可达路径后，进行椭圆/椭球采样域的限制，采样空间也由自由空间Xfree变为椭圆/椭球空间Xellipse，所述椭圆/椭球空间的生成方法包括：
S111：得到第一条可达路径的路径长度Cbest后将其作为椭圆的长轴长，将起点xstart和目标点xgoal作为椭圆的焦点；
S112：计算起点和目标点之间的距离为cmin，cmin＝||xgoal-xstart||2，并计算起点与目标点的中心xcenter，xcenter＝(xstart+xgoal)/2；
S113：计算超椭球体横轴a1与单位矩阵l1的外积M＝a1×l1，a1＝(xgoal-xstart)/cmin，l1＝[1,0,0]；
S114：对外积M进行奇异值分解：U∑VT≡M，U和V是两组正交单位向量，∑为进行奇异值分解后的对角矩阵；
S115：计算从椭球坐标系到世界坐标系的旋转矩阵C＝Udiag{1,...,1,det(U)det(V)}VT，det(U)与det(V)分别为U和V的行列式，diag表示对角矩阵，VT为矩阵V的转置；
S116：从n维单位球中均匀分布的样本转换到超椭球中均匀分布的样本所需的变换矩阵L：
Figure FDA0003950578850000031
S117：得到从n维单位球转换到世界坐标系下的随机采样点xrand，xrand＝CLxball+xcenter，式中xball为在单位球中的采样点。
4.根据权利要求1所述的基于改进的AAPF-IRRT算法的无人机轨迹规划方法，其特征在于，所述步骤S01中改进的AAPF-IRRT算法采用自适应步长策略扩展新节点xnew，扩展新节点的方法包括：
S011：找到随机树中距离xprand最近的临近点xnearest，沿着xnearest与xprand的连线方向扩展步长step，生成xnew：
Figure FDA0003950578850000032
S012：若xnew与xnearest的连线未经过障碍物且步长未达到步长上限stepmax，则以倍数ks扩展步长step，step＝step*ks，并以该步长重新扩展xnew，否则直接返回xnew；
S013：循环执行步骤S012。
5.根据权利要求1所述的基于改进的AAPF-IRRT算法的无人机轨迹规划方法，其特征在于，所述步骤S03中离散点拟合方法包括：
S31：对无人机航点每个维度单独求解其轨迹，以下的求解步骤均为单个维度；
S32：将轨迹P(t)按照时间段Tn-1～Tn分割为n-1段，第n段轨迹记为Pn(t)，并用五次多项式表示，式中i为多项式中每一项的次数，pn,i为第n段轨迹相应项次数对应的系数：
Figure FDA0003950578850000041
S33：求n-1段轨迹每段轨迹Pn(t)的最小加加加速度
Figure FDA0003950578850000042
为第n段轨迹Pn(t)的四阶倒数；
Figure FDA0003950578850000043
S34：求使得
Figure FDA0003950578850000044
最小的凸二次规划表达式Jn(T)；
Figure FDA0003950578850000045
式中T为第n段轨迹从Tn-1～Tn的时间，l与i均为多项式中每一项的次数，
Figure FDA0003950578850000051
为定义符号，即
Figure FDA0003950578850000052
Figure FDA0003950578850000053
S35：对每个离散点已知的位置p、速度v、加速度a构造等式约束，以T0时刻的离散点为例，位置约束：
Figure FDA0003950578850000054
式中p0为T0时刻对应点的位置；速度约束：
Figure FDA0003950578850000055
式中P1'(T0)为P1(T0)在T0时刻的一阶导数，v0为T0时刻对应点的速度；加速度约束：
Figure FDA0003950578850000056
式中P1"(T0)为P1(T0)在T0时刻的二阶导数；
S36：根据相邻轨迹段之间的位置、速度、加速度连续构建等式约束，即第n段和第n-1段轨迹的各阶导数相等：
Figure FDA0003950578850000057
式中，
Figure FDA0003950578850000061
和
Figure FDA0003950578850000062
分别为两个相邻段轨迹Pn-1(t)和Pn(t)在相应时间Tn-1和Tn处的k阶导，k＝0，1，2，分别表示了位置、速度和加速度，l与i均为多项式中每一项的次数，
Figure FDA0003950578850000063
Figure FDA0003950578850000064
6.根据权利要求5所述的基于改进的AAPF-IRRT算法的无人机轨迹规划方法，其特征在于，所述步骤S03中minimum snap算法对二次正定型进行闭式求解，其步骤为：
S031：将多项式轨迹转化为带有具体物理含义的速度、加速度，即当前点位置的各阶微分记为dn，构造矩阵Mn将pn映射为pn的微分dn，即Mn·pn＝dn，然后将目标函数化为如下形式：
Figure FDA0003950578850000065
式中，
Figure FDA0003950578850000066
S032：使用选择矩阵C将dn分解为自由变量dP和约束变量dF：
Figure FDA0003950578850000067
S033：重新带入dn到二次规划表达式Jn(T)中，得到如下形式：
Figure FDA0003950578850000071
式中，将矩阵
Figure FDA0003950578850000072
定义为矩阵R，RFF、RFP、RPF、RPP分别为矩阵R的左上角、右上角、左下角、右下角的分块矩阵；
S034：求得使目标函数最小的自由变量的表达式
Figure FDA0003950578850000073
Figure FDA0003950578850000074
7.根据权利要求1所述的基于改进的AAPF-IRRT算法的无人机轨迹规划方法，其特征在于，所述步骤S03中minimum snap算法的时间分配为梯形时间分配，在一段轨迹中无人机以最大加速度加速到最大速度后匀速、再以最大加速度减速到目标点速度，时间分配准则按照如下函数分配各段轨迹时间：
Figure FDA0003950578850000075
式中，Sj代表第j段轨迹两端点之间的直线距离，vm代表无人机的最大速度，am代表无人机的最大加速度，T(Sj)代表输入量为Sj、输出为轨迹的分配时间。
8.根据权利要求1所述的基于改进的AAPF-IRRT算法的无人机轨迹规划方法，其特征在于，所述步骤S03中minimum snap算法的飞行走廊约束的方法包括：
S131：在两个离散路径点的连线之间采样多个中间点q，对每个中间点均施加立方体走廊约束；
S132：将所有中间点受到的立方体约束表示为不等式约束的形式，加入到二次规划问题的约束中，对每个中间点q，设置立方体的边长为r，以第n段轨迹为例，对每个中间点增加两个不等式约束：
Figure FDA0003950578850000081
式中，tq为中间点q所对应的时刻，Pn(tq)可以根据tq计算得出，
Figure 1
9.一种基于改进的AAPF-IRRT算法的无人机轨迹跟踪控制方法，其特征在于，采用权利要求1-8任一项所述的无人机轨迹规划方法得到无人机的飞行轨迹；
使用自适应控制方法对无人机进行控制以跟踪生成的飞行轨迹，其步骤为：
S41：将无人机的控制分为姿态环和位置环，使用自抗扰控制器对姿态环进行控制；所述自抗扰控制器包括TD跟踪微分器、NLSEF非线性状态反馈、及ESO扩张观测器；
S42：对TD跟踪微分器、NLSEF非线性状态反馈及ESO扩张观测器进行参数调节，使得无人机真实轨迹跟踪期望轨迹；
S43：对位置环采用PID控制器进行控制，并对比例、积分、微分三个环节参数进行调节跟踪性能。
10.一种计算机存储介质，其上存储有计算机程序，其特征在于，所述计算机程序被执行时实现权利要求9所述的基于改进的AAPF-IRRT算法的无人机轨迹跟踪控制方法。
CN202211448781.1A 2022-11-18 2022-11-18 基于改进的aapf-irrt算法的无人机轨迹规划及跟踪控制方法 Pending CN115903894A (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202211448781.1A CN115903894A (zh) 2022-11-18 2022-11-18 基于改进的aapf-irrt算法的无人机轨迹规划及跟踪控制方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202211448781.1A CN115903894A (zh) 2022-11-18 2022-11-18 基于改进的aapf-irrt算法的无人机轨迹规划及跟踪控制方法

Publications (1)

Publication Number Publication Date
CN115903894A true CN115903894A (zh) 2023-04-04

Family

ID=86474070

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202211448781.1A Pending CN115903894A (zh) 2022-11-18 2022-11-18 基于改进的aapf-irrt算法的无人机轨迹规划及跟踪控制方法

Country Status (1)

Country Link
CN (1) CN115903894A (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117631686A (zh) * 2023-12-07 2024-03-01 浙江大学 面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法
CN117631686B (zh) * 2023-12-07 2024-06-07 浙江大学 面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN117631686A (zh) * 2023-12-07 2024-03-01 浙江大学 面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法
CN117631686B (zh) * 2023-12-07 2024-06-07 浙江大学 面向多旋翼无人机的路径优化方法以及轨迹跟踪控制方法

Similar Documents

Publication Publication Date Title
Qiu et al. A multi-objective pigeon-inspired optimization approach to UAV distributed flocking among obstacles
US11727812B2 (en) Airplane flight path planning method and device based on the pigeon-inspired optimization
CN110806756B (zh) 基于ddpg的无人机自主引导控制方法
Alonso-Mora et al. Shared control of autonomous vehicles based on velocity space optimization
Wang et al. Composite Astar and B-spline algorithm for path planning of autonomous underwater vehicle
Tutuko et al. Route optimization of non-holonomic leader-follower control using dynamic particle swarm optimization
Yang et al. Three-dimensional UAV cooperative path planning based on the MP-CGWO algorithm
Soleymani et al. Behavior-based acceleration commanded formation flight control
Farhood Neural network based control system for robots group operating in 2-d uncertain environment
Wen et al. A collision forecast and coordination algorithm in configuration control of missile autonomous formation
CN114661066A (zh) 一种基于强化学习的无人机集群智能避障方法
Patrikar et al. Real-time motion planning of curvature continuous trajectories for urban UAV operations in wind
Zhang et al. Collision Avoidance of Fixed-Wing UAVs in Dynamic Environments Based on Spline-RRT and Velocity Obstacle
Park et al. Formation reconfiguration control with collision avoidance of nonholonomic mobile robots
CN115903894A (zh) 基于改进的aapf-irrt算法的无人机轨迹规划及跟踪控制方法
Deng et al. Multi-robot dynamic formation path planning with improved polyclonal artificial immune algorithm
CN114200936B (zh) 基于最优控制及宽度学习的agv实时路径规划方法
He et al. Reinforcement learning based multi-robot formation control under separation bearing orientation scheme
Kanjanawanishkul Coordinated path following for mobile robots using a virtual structure strategy with model predictive control
Choi et al. Collision avoidance of unmanned aerial vehicles using fuzzy inference system-aided enhanced potential field
Liu et al. A strategy of multi-UAV cooperative path planning based on CCPSO
Guo et al. Online path planning for UAV navigation based on quantum particle swarm optimization
Jiang et al. Fast three-dimensional path planning with obstacle avoidance constraints
Xie et al. A distributed multi-agent formation control method based on deep Q learning
Letizia et al. A new recursive framework for trajectory generation of UAVs

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination