基于防撞与避障的空间机动目标轨迹规划方法

技术领域

本发明涉及空间机动目标轨迹规划技术领域，尤其涉及基于防撞与避障的空间机动目标轨迹规划方法。

背景技术

在三维空间中可任意运动的无人驾驶飞行器(Unmanned Aerial Vehicle，UAV)或地面无人自动驾驶机动车(统称为UAV)在运动中能依靠传感器探测到其行进轨迹上的障碍物，UAV的机动性能与对手的跟踪性能是在相互竞争和相互促进的过程中不断向前发展的。早期由于UAV机动性能不强，一些简单的运动模型就能够很准确地描述其运动轨迹、速度等参数，并实现精确跟踪。近年来随着UAV机动能力的增强，一些新的目标跟踪理论与技术得到了发展。由探测器获得的信号对UAV的运动模型进行推测，并且选择建立在相应运动模型上的滤波算法，就能对UAV运动的状态向量进行精确估计和预测，从而实现准确跟踪的目的。

对于UAV探测信号直接采样的预处理工作，一般包括平滑、滤波等；对UAV的运动特性进行建模主要是用运动方程来描述目标的运动方式，运动模型准确与否，对随后UAV目标跟踪处理的精确度起着至关重要的作用。对于非机动目标，其运动特性可以用匀速、匀变速等线性模型来描述，在这种情况下UAV目标的机动性非常小，通常是作为噪声对待的；而随着UAV目标机动能力的增强，UAV目标的运动特性呈现出非线性特性，如果仍然按照线性运动模型来描述非线性运动的目标，就会产生很大的误差。对于大机动飞行目标的探测系统，要求具备机动性检测机制，用以感知目标机动性的变化情况，在此基础上建立合理的机动运动模型，并对运动参数进行不断的修正，以便使运动模型很好地符合实测情况，进而提高后续的目标跟踪准确度。

UAV目标信号探测过程中需要将混有噪声的量测数据经过滤波和预测运算，理想的情况是预先掌握了目标的运动模型，如果目标运动模型未知，则可根据目标状态的最优估计信息，实现对UAV运动目标的跟踪。在UAV目标做非机动运动的情况下，适用的滤波跟踪算法有α-β、α-β-γ、Kalman(卡尔曼)以及线性自回归滤波等线性滤波算法；对于UAV目标的非线性大机动性运动，如果再利用线性滤波器进行滤波处理将会使跟踪系统的性能变差，对于那些具有很大非线性特性的目标，甚至可能发生滤波发散的情况。为此，研究人员相继提出和改进了一些非线性滤波算法，如EKF(Expanded Kalman Filter，扩展卡尔曼滤波)、UKF(Unscented Kalman Filter，无迹卡尔曼滤波器)、PF(Particle Filter，粒子滤波)等。从滤波跟踪算法的发展历程可以看出，由于UAV目标机动性增强以及噪声环境的复杂性，滤波跟踪算法逐渐从线性算法、高斯分布噪声向非线性算法、非高斯分布噪声发展，以适应目标运动的非线性、非高斯噪声环境等情况。

截至目前，已有的研究工作主要是对于非机动性或机动性目标的探测、滤波、预测和跟踪的计算方法，而对于防撞和避障的定量化指标，尚未见到文献报道。

发明内容

针对上述存在的问题，本发明旨在提供一种基于防撞与避障的空间机动目标轨迹规划方法，利用该方法可以使UAV重新规划出运动参数及其轨迹，且重新规划出的运动参数和运动轨迹能够最大程度地防撞与避障，给出防撞和避障效果评价的定量化指标，填补了目前该指标方面的空白。

为了实现上述目的，本发明所采用的技术方案如下：

基于防撞与避障的空间机动目标轨迹规划方法，其特征在于，包括以下步骤，

S1：UAV在发现障碍物时，建立以障碍物为坐标原点的Oxyz三维空间正交坐标系，且在UAV和障碍物的移动过程中保持该坐标原点不变；

S2：根据步骤S1中建立的三维空间正交坐标系，建立三维空间UAV的运动轨迹模型；

S3：以障碍物对UAV的预测轨迹与UAV实际轨迹的偏差平方和的二次方根的积分作为UAV的防撞和避障指标参数，计算UAV防撞和避障指标参数；

S4：根据步骤S3中计算出来的UAV防撞和避障指标参数，对步骤S2中建立的UAV运动轨迹模型中的各参数进行调节，从而进行空间机动目标的轨迹规划。

进一步的，步骤S2的具体操作步骤包括，

S201：建立以t＝t

S202：当t

S203：根据步骤S202中简化后的非线性函数，可以得出对于三维空间的UAV运动轨迹模型为

S204：根据基于空间曲线拟合的微分多项式运动模型理论，步骤S203中的UAV运动轨迹模型可表示为Taylor级数展开的形式

进一步的，步骤S3的具体操作步骤包括，

S301：当障碍物对UAV观测的采样间隔为T，采用KF算法进行线性一步递推预测，UAV防撞和避障指标参数

S302：对步骤S301中的积分进行离散化，则

进一步的，步骤S4的具体操作步骤包括，

S401：令UAV的质心与在三维空间中距其质心最远的质点之间的距离为p米，障碍物的质心与在三维空间中距其质心最远的质点之间的距离为q米，运动过程中UAV的质心与障碍物的质心间的距离为d，则

S402：当d＞(p+q)时，被认定为防撞和避障成功，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥2T/Δt时终止递推运算；

S403：当d≤(p+q)时，被认定为防撞和避障失败，发生碰撞，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥2T/Δt时终止递推运算；

S404：计算

S405：调整步骤S204中UAV运动轨迹模型中各参数a

S406：根据求导公式(u±v)′＝u′±v′，uv＝u′v+uv′，

进一步的，步骤S3的具体操作步骤包括，

S301：当障碍物对UAV观测的采样间隔为T，采用EKF算法进行二阶非线性一步递推预测，UAV防撞和避障指标参数

S302：对步骤S301中的积分进行离散化，则

进一步的，步骤S4的具体操作步骤包括，

S401：令UAV的质心与在三维空间中距其质心最远的质点之间的距离为p米，障碍物的质心与在三维空间中距其质心最远的质点之间的距离为q米，运动过程中UAV的质心与障碍物的质心间的距离为d，则

S402：当d≤(p+q)时，被认定为防撞和避障成功，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥3T/Δt时终止递推运算；

S403：当d≤(p+q)时，被认定为防撞和避障失败，发生碰撞，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到m≥3T/Δt时终止递推运算；

S404：计算

S405：调整步骤S204中UAV运动轨迹模型中各参数a

S406：根据求导公式(u±v)′＝u′±v′，uv＝u′v+uv′，

进一步的，步骤S3的具体操作步骤包括，

S301：当障碍物对UAV观测的采样间隔为T，采用Jerk模型进行非线性一步递推预测，UAV防撞和避障指标参数

S302：对步骤S301中的积分进行离散化，则

进一步的，步骤S4的具体操作步骤包括，

S401：令UAV的质心与在三维空间中距其质心最远的质点之间的距离为p米，障碍物的质心与在三维空间中距其质心最远的质点之间的距离为q米，运动过程中UAV的质心与障碍物的质心间的距离为d，则

S402：当d＞(p+q)时，被认定为防撞和避障成功，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥4T/Δt时终止递推运算；

S403：当d≤(p+q)时，被认定为防撞和避障失败，发生碰撞，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥4T/Δt时终止递推运算；

S404：计算

S405：调整步骤S204中UAV运动轨迹模型中各参数a

S406：根据求导公式(u±v)′＝u′±v′，uv＝u′v+uv′，

本发明的有益效果是：

本发明中基于防撞与避障的空间机动目标轨迹规划方法可以在UAV头部的传感器发现障碍物后改变UAV的运动轨迹，规划出新的运动参数及轨迹，从而最大程度地防撞和避障；本发明中给出了UAV防撞和避障效果评价的定量化指标，填补了现有技术中在该指标方面的空白。

附图说明

图1为本发明实施例一中基于防撞与避障的空间机动目标轨迹规划方法的流程图；

图2为本发明仿真实验中的实验结果。

具体实施方式

为了使本领域的普通技术人员能更好的理解本发明的技术方案，下面结合附图和实施例对本发明的技术方案做进一步的描述。

实施例一：

如附图1所示，基于防撞与避障的空间机动目标轨迹规划方法，包括以下步骤，

S1：UAV在利用传感器发现障碍物时，建立以障碍物为坐标原点的Oxyz三维空间正交坐标系，且在UAV和障碍物的移动过程中保持该坐标原点不变；

S2：根据步骤S1中建立的三维空间正交坐标系，建立三维空间UAV的运动轨迹模型；

具体的，S201：建立以t＝t

S202：当t

S203：根据步骤S202中简化后的非线性函数，可以得出对于三维空间的UAV运动轨迹模型为

S204：根据基于空间曲线拟合的微分多项式运动模型理论，步骤S203中的UAV运动轨迹模型可表示为Taylor级数展开的形式

S3：以障碍物对UAV的预测轨迹与UAV实际轨迹的偏差平方和的二次方根的积分作为UAV的防撞和避障指标参数，计算UAV防撞和避障指标参数；其中，障碍物对UAV的预测轨迹即为障碍物对UAV运动轨迹进行采样后对UAV的运动轨迹进行预测得到的预测轨迹，UAV的实际轨迹即为UAV根据步骤S2中建立的运动轨迹模型进行运动得到的轨迹；

以障碍物对UAV的预测轨迹与UAV实际轨迹的偏差平方和的二次方根的积分作为UAV的防撞和避障指标参数是非常合理的，因为需要综合考虑算法的效率、可以利用的计算资源、性价比以及避免维数灾难等方面的问题，可以假设主动型障碍物可以使用当前较为先进的预测算法，进而能够比较精确地对UAV进行轨迹预测，这是进行准确主动碰撞的前提条件。

具体的，S301：当障碍物对UAV观测的采样间隔为T，采用KF算法进行线性一步递推预测，UAV防撞和避障指标参数

S302：对步骤S301中的积分进行离散化，则

S4：根据步骤S3中计算出来的UAV防撞和避障指标参数，进行空间机动目标的轨迹规划。

具体的，S401：令UAV的质心与在三维空间中距其质心最远的质点之间的距离为p米，障碍物的质心与在三维空间中距其质心最远的质点之间的距离为q米，运动过程中UAV的质心与障碍物的质心间的距离为d，则

S402：当d＞(p+q)时，被认定为防撞和避障成功，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥2T/Δt时终止递推运算；

S403：当d≤(p+q)时，被认定为防撞和避障失败，发生碰撞，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥2T/Δt时终止递推运算；

S404：计算

S405：调整步骤S204中UAV运动轨迹模型中各参数a

S406：根据求导公式(u±v)′＝u′±v′，uv＝u′v+uv′，

实施例二：

基于防撞与避障的空间机动目标轨迹规划方法，包括以下步骤，

S1：UAV在利用传感器发现障碍物时，建立以障碍物为坐标原点的Oxyz三维空间正交坐标系，且在UAV和障碍物的移动过程中保持该坐标原点不变；

S2：根据步骤S1中建立的三维空间正交坐标系，建立三维空间UAV的运动轨迹模型；

步骤S2的具体操作与实施例一中步骤S2的具体操作完全相同。

S3：以障碍物对UAV的预测轨迹与UAV实际轨迹的偏差平方和的二次方根的积分作为UAV的防撞和避障指标参数，计算UAV防撞和避障指标参数；其中，障碍物对UAV的预测轨迹即为障碍物对UAV运动轨迹进行采样后对UAV的运动轨迹进行预测得到的预测轨迹，UAV的实际轨迹即为UAV根据步骤S2中建立的运动轨迹模型进行运动得到的轨迹；

以障碍物对UAV的预测轨迹与UAV实际轨迹的偏差平方和的二次方根的积分作为UAV的防撞和避障指标参数是非常合理的，因为需要综合考虑算法的效率、可以利用的计算资源、性价比以及避免维数灾难等方面的问题，可以假设主动型障碍物可以使用当前较为先进的预测算法，进而能够比较精确地对UAV进行轨迹预测，这是进行准确主动碰撞的前提条件。

具体的，S301：当障碍物对UAV观测的采样间隔为T，采用EKF滤波算法进行二阶非线性一步递推预测，UAV防撞和避障指标参数

S302：对步骤S301中的积分进行离散化，则

S4：根据步骤S3中计算出来的UAV防撞和避障指标参数，进行空间机动目标的轨迹规划。

具体的，S401：令UAV的质心与在三维空间中距其质心最远的质点之间的距离为p米，障碍物的质心与在三维空间中距其质心最远的质点之间的距离为q米，运动过程中UAV的质心与障碍物的质心间的距离为d，则

S402：当d＞(p+q)时，被认定为防撞和避障成功，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥3T/Δt时终止递推运算；

S403：当d≤(p+q)时，被认定为防撞和避障失败，发生碰撞，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥3T/Δt时终止递推运算；

S404：计算

S405：调整步骤S204中UAV运动轨迹模型中各参数a

S406：根据求导公式(u±v)′＝u′±v′，uv＝u′v+uv′，

实施例三：

基于防撞与避障的空间机动目标轨迹规划方法，包括以下步骤，

S1：UAV在利用传感器发现障碍物时，建立以障碍物为坐标原点的Oxyz三维空间正交坐标系，且在UAV和障碍物的移动过程中保持该坐标原点不变；

S2：根据步骤S1中建立的三维空间正交坐标系，建立三维空间UAV的运动轨迹模型；

步骤S2的具体操作与实施例一中步骤S2的具体操作完全相同。

S3：以障碍物对UAV的预测轨迹与UAV实际轨迹的偏差平方和的二次方根的积分作为UAV的防撞和避障指标参数，计算UAV防撞和避障指标参数；其中，障碍物对UAV的预测轨迹即为障碍物对UAV运动轨迹进行采样后对UAV的运动轨迹进行预测得到的预测轨迹，UAV的实际轨迹即为UAV根据步骤S2中建立的运动轨迹模型进行运动得到的轨迹；

以障碍物对UAV的预测轨迹与UAV实际轨迹的偏差平方和的二次方根的积分作为UAV的防撞和避障指标参数是非常合理的，因为需要综合考虑算法的效率、可以利用的计算资源、性价比以及避免维数灾难等方面的问题，可以假设主动型障碍物可以使用当前较为先进的预测算法，进而能够比较精确地对UAV进行轨迹预测，这是进行准确主动碰撞的前提条件。

具体的，S301：当障碍物对UAV观测的采样间隔为T，采用Jerk模型进行非线性一步递推预测，UAV防撞和避障指标参数

S302：对步骤S301中的积分进行离散化，则

S4：根据步骤S3中计算出来的UAV防撞和避障指标参数，进行空间机动目标的轨迹规划。

具体的，S401：令UAV的质心与在三维空间中距其质心最远的质点之间的距离为p米，障碍物的质心与在三维空间中距其质心最远的质点之间的距离为q米，运动过程中UAV的质心与障碍物的质心间的距离为d，则

S402：当d＞(p+q)时，被认定为防撞和避障成功，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥4T/Δt时终止递推运算；

S403：当d≤(p+q)时，被认定为防撞和避障失败，发生碰撞，计算此时UAV防撞和避障指标参数的离散化表达式S

令n的初始值为0，每进行一次递推运算后，n＝n+1，直到n≥4T/Δt时终止递推运算；

S404：计算

S405：调整步骤S204中UAV运动轨迹模型中各参数a

S406：根据求导公式(u±v)′＝u′±v′，uv＝u′v+uv′，

仿真实验：

在该仿真实验中，利用障碍物对UAV运动轨迹的5个观测点，计算出经过这5个采样点的UAV可能的轨迹，结合本发明中实施例二和实施例三中采用EKF算法进行二阶非线性一步递推预测和Jerk模型进行非线性一步递推预测两种方法对空间机动目标的轨迹进行规划，结果如附图2所示。

从附图2中可以看出，5个小圆圈为障碍物观测UAV所得到的5个离散采样点，如果它按照实施例二中采用EKF算法进行二阶非线性一步递推预测，就得到离散采样点之间的插值并拟合为图2中虚线构成的轨迹曲线；如果它按照实施例三中Jerk模型进行非线性一步递推预测，就得到离散采样点之间的插值并拟合为图2中小圆点构成的轨迹曲线；而空间机动目标UAV规划的实际运动轨迹为图中实线曲线，表现出很强的机动性。从附图2中可以看出，UAV规划的实际运动轨迹与障碍物所能够预测到的UAV轨迹偏差很大，也就是说障碍物与其发生碰撞的可能性很小。

以上显示和描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种变化和改进，这些变化和改进都落入要求保护的本发明范围内。本发明要求保护范围由所附的权利要求书及其等效物界定。