[发明专利]基于防撞与避障的空间机动目标轨迹规划方法

权利要求书

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

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

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

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

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

2.根据权利要求1所述的基于防撞与避障的空间机动目标轨迹规划方法，其特征在于，步骤S2的具体操作步骤包括，

S201：建立以t＝t₀为起始时间点的UAV运动轨迹非线性函数其中m＜n；式中，a_i和b_j均为常量，i＝0,1,…,n；j＝0,1,…,m；a_n≠0，b_m≠0；

S202：当t₀＝0时，步骤S201中的非线性函数可以简化为

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

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

3.根据权利要求2所述的基于防撞与避障的空间机动目标轨迹规划方法，其特征在于，步骤S3的具体操作步骤包括，

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

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

4.根据权利要求3所述的基于防撞与避障的空间机动目标轨迹规划方法，其特征在于，步骤S4的具体操作步骤包括，

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

S402：当d＞(p+q)时，被认定为防撞和避障成功，计算此时UAV防撞和避障指标参数的离散化表达式S₁₁，令S₁₁的初始值为0，对S₁₁进行递推运算，则

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

S403：当d≤(p+q)时，被认定为防撞和避障失败，发生碰撞，计算此时UAV防撞和避障指标参数的离散化表达式S₁₂，令S₁₂的初始值为0，对S₁₂进行递推运算，则

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

S404：计算Q₁的值越大，表示UAV防撞和避障成功的概率越高，如果Q₁＝100％则表示UAV与障碍物发生碰撞的概率为0；

S405：调整步骤S204中UAV运动轨迹模型中各参数a_xi、b_xj、a_yi、b_yj、a_zi、b_zj的值，使步骤S404中的Q₁大于设定的UAV防撞和避障成功的阈值，参数调整后的运动轨迹即为规划出来的目标轨迹；

S406：根据求导公式(u±v)′＝u′±v′，uv＝u′v+uv′，对步骤S204中UAV运动轨迹模型中的t求二阶导数，可得进而可得UAV的运动加速度对其进行离散可得a(n·Δt)，n＝1，2，…，2T/Δt，根据F(n·Δt)＝ma(n·Δt)，n＝1，2，…，2T/ΔT对UAV运动过程中的驱动力进行调整，从而实现UAV的轨迹规划。