2026年IEEE TASE，未知环境中的无人机群动态轨迹规划

1.摘要

针对未知环境下多无人机群的动态轨迹规划难题，本文提出了分布式协同规划系统（DCPS），该系统由轨迹规划和协同规划两层组成：前者融合局部Kinodynamic-A*与高斯势场B样条（KGPB）算法，在满足运动动力学约束的前提下生成并优化单机轨迹；后者通过后向场邻域重规划（BFNP）方法，使无人机仅与近邻通信，并根据前方机的潜在碰撞区动态调整自身轨迹，大幅降低通信成本。

2.背景知识

动力学模型

假设机身与螺旋桨为均匀质量刚体，其六自由度运动由位置 $x , y , z$ T $x, y, z$ ^T $x,y,z$ T 和姿态角（滚转 ϕ \phi ϕ、俯仰 θ \theta θ、偏航 ψ \psi ψ）表征。由各转子产生的总推力 T = ∑ i = 1 4 F i T = \sum_{i=1}^{4} F_i T=∑i=14Fi 及三轴力矩 $M x , M y , M z$ T $M_x, M_y, M_z$ ^T $Mx,My,Mz$ T 驱动，其连续时间动力学方程为：

x ¨ = ( cos ⁡ ϕ sin ⁡ θ cos ⁡ ψ + sin ⁡ ϕ sin ⁡ ψ ) T / m y ¨ = ( cos ⁡ ϕ sin ⁡ θ sin ⁡ ψ − sin ⁡ ϕ cos ⁡ ψ ) T / m z ¨ = ( cos ⁡ ϕ cos ⁡ θ ) T / m − g ϕ ¨ = ( M x − ( J z z − J y y ) θ ˙ ψ ˙ ) / J x x θ ¨ = ( M y − ( J x x − J z z ) ψ ˙ ϕ ˙ ) / J y y ψ ¨ = ( M z − ( J y y − J x x ) ϕ ˙ θ ˙ ) / J z z \begin{aligned}&\ddot{x}=(\cos\phi\sin\theta\cos\psi+\sin\phi\sin\psi)T/m\\&\ddot{y}=(\cos\phi\sin\theta\sin\psi-\sin\phi\cos\psi)T/m\\&\ddot{z}=(\cos\phi\cos\theta)T/m-g\\&\ddot{\phi}=\left(M_{x}-\left(J_{zz}-J_{yy}\right)\dot{\theta}\dot{\psi}\right)/J_{xx}\\&\ddot{\theta}=\left(M_{y}-(J_{xx}-J_{zz})\dot{\psi}\dot{\phi}\right)/J_{yy}\\&\ddot{\psi}=\left(M_{z}-\left(J_{yy}-J_{xx}\right)\dot{\phi}\dot{\theta}\right)/J_{zz}\end{aligned} x¨=(cosϕsinθcosψ+sinϕsinψ)T/my¨=(cosϕsinθsinψ−sinϕcosψ)T/mz¨=(cosϕcosθ)T/m−gϕ¨=(Mx−(Jzz−Jyy)θ˙ψ˙)/Jxxθ¨=(My−(Jxx−Jzz)ψ˙ϕ˙)/Jyyψ¨=(Mz−(Jyy−Jxx)ϕ˙θ˙)/Jzz

其中 m m m 为质量， g g g 为重力加速度， J = diag $J x x , J y y , J z z$ J = \text{diag} $J_{xx}, J_{yy}, J_{zz}$ J=diag $Jxx,Jyy,Jzz$ 为转动惯量。三轴力矩由各桨叶推力差及臂长 R R R 决定： $M x M y M z$ = $R ( F 3 - F 1 ) R ( F 4 - F 2 ) M 1 - M 2 + M 3 - M 4$ \begin{bmatrix} M_x \\ M_y \\ M_z \end{bmatrix} = \begin{bmatrix} R(F_3 - F_1) \\ R(F_4 - F_2) \\ M_1 - M_2 + M_3 - M_4 \end{bmatrix} MxMyMz = R(F3−F1)R(F4−F2)M1−M2+M3−M4 将上述状态量与一阶导数整合为状态向量 X ( k ) X(k) X(k)，输入整合为 U ( k ) = $T , M x , M y , M z$ T U(k) = $T, M_x, M_y, M_z$ ^T U(k)= $T,Mx,My,Mz$ T，可写为离散状态空间形式： X ( k + 1 ) = f ( X ( k ) , U ( k ) ) X(k + 1) = f(X(k), U(k)) X(k+1)=f(X(k),U(k))

MPC控制

采用模型预测控制（MPC）实现无人机的目标轨迹跟踪。其优化目标函数通过加权调节状态误差、控制投入和终端误差，以平衡跟踪精度、控制能耗与时域收敛性：

J = ∑ l = 0 N − 1 ( ∥ X ( l ∣ k ) − X r e f ( l ∣ k ) ∥ Q 1 2 + ∥ U ( l ∣ k ) ∥ Q 2 2 ) + ∥ X ( N ∣ k ) − X r e f ( N ∣ k ) ∥ Q 3 2 \begin{aligned}J&=\sum_{l=0}^{N-1}\left(\|X(l|k)-X_{\mathrm{ref}}(l|k)\|{Q{1}}^{2}+\|U(l|k)\|{Q{2}}^{2}\right)\\&+\|X(N|k)-X_{\mathrm{ref}}(N|k)\|{Q{3}}^{2}\end{aligned} J=l=0∑N−1(∥X(l∣k)−Xref(l∣k)∥Q12+∥U(l∣k)∥Q22)+∥X(N∣k)−Xref(N∣k)∥Q32

3.分布式协同规划系统

KGPB方法

前端路径搜索与剪枝，针对传统Kinodynamic-A*计算量大的问题，引入相机视野信息构建局部目标点 P k l o c a l P_k^{local} Pklocal：

P k l o c a l = { P k C v i s i o n = True P k c + λ v d C v i s i o n = False P_k^{local} = \begin{cases} P_k & C_{vision} = \text{True} \\ P_k^c + \lambda v_d & C_{vision} = \text{False} \end{cases} Pklocal={PkPkc+λvdCvision=TrueCvision=False

利用RDP算法通过阈值 δ \delta δ 剔除冗余点，精简轨迹：

∥ q × ( P i − P 1 ) ∥ ∥ q ∥ > δ \frac{\|q \times (P_i - P_1)\|}{\|q\|} > \delta ∥q∥∥q×(Pi−P1)∥>δ

后端轨迹优化，利用B样条平滑生成轨迹后，建立3D障碍物的高斯势场：

G ( x , y , z ) = A exp ⁡ ( − ∥ C x y z − C c e n ∥ 2 2 σ 2 ) G(x, y, z) = A \exp\left(-\frac{\|C_{xyz} - C_{cen}\|^2}{2\sigma^2}\right) G(x,y,z)=Aexp(−2σ2∥Cxyz−Ccen∥2)

沿负梯度方向确定安全边界。若轨迹点落入场势，则计算其均值中心 C ˉ a v e \bar{C}_{ave} Cˉave 并沿外向安全距离 Δ D \Delta D ΔD 偏移至安全区，形成兼顾动态可行性与平滑度的柔性避障轨迹：

C n e w = ( x ˉ a v e , y ˉ a v e , z ˉ a v e ) + Δ D C_{new} = (\bar{x}{ave}, \bar{y}{ave}, \bar{z}_{ave}) + \Delta D Cnew=(xˉave,yˉave,zˉave)+ΔD

BFNP方法

后向场邻域重规划(BFNP)方法通过仅与最近邻无人机通信，使集群通信负荷随规模线性增长，避免了全通信的计算冗余。

依据两机欧氏距离 d ( k ) = ∥ H b a c k ( k ) − H f r o n t ( k ) ∥ d(k)=\|H_{back}(k)-H_{front}(k)\| d(k)=∥Hback(k)−Hfront(k)∥，在前机周围划分两级分级避障检测区低风险速度调节区( $d(k)\不改变原轨迹，仅调整后机加速度以拉开安全离：$

a b a c k ( k ) = − k v k 2 ( d ( k ) − d s a f e ) H b a c k ( k ) − H f r o n t ( k ) ∥ H b a c k ( k ) − H f r o n t ( k ) ∥ a_{back}(k)=-\frac{k_v}{k_2(d(k)-d_{safe})}\frac{H_{back}(k)-H_{front}(k)}{\|H_{back}(k)-H_{front}(k)\|} aback(k)=−k2(d(k)−dsafe)kv∥Hback(k)−Hfront(k)∥Hback(k)−Hfront(k)

高风险位置调节区( $d(k \当碰撞紧迫时触发轨迹重规划。按比例调整后机高度：$

h b a c k ( k + 1 ) = h b a c k ( k ) − k h ( d ( k ) − d s a f e ) h_{back}(k+1)=h_{back}(k)-k_h(d(k)-d_{safe}) hback(k+1)=hback(k)−kh(d(k)−dsafe)

随后融合障碍物高度 h a b s h_{abs} habs、障碍物安全距离 d a b s d_{abs} dabs 及最低飞行高度 h m i n h_{min} hmin 约束，计算出最优重规划高度并调用KGPB算法重建安全轨迹：

h b a c k replan ( k + 1 ) = max ⁡ ( h o b s + d o b s , h m i n , h b a c k ( k + 1 ) ) h_{back}^{\textit{replan}}(k+1)=\max(h_{obs}+d_{obs},h_{min},h_{back}(k+1)) hbackreplan(k+1)=max(hobs+dobs,hmin,hback(k+1))

基于分布式协同规划系统轨迹规划

任务一：未知环境下的集群协同自由遍历 输入局部目标点 P k l o c a l P_k^{local} Pklocal，通过分布式协同规划系统（DCPS）直接输出各无人机的控制量 U i U_i Ui，用以验证系统自由遍历飞行的有效性。

任务二：未知环境下的集群协同编队遍历 无人机无需设定固定位置，可根据自身状态弹性调整编队排序。重排后的第 i i i 架无人机目标位置 F i k F_i^k Fik 结合了编队中心与相对偏移量 D i f o r m D_i^{form} Diform：

F i k = 1 n ∑ i = 1 n P i c + D i f o r m F_i^k = \frac{1}{n} \sum_{i=1}^n P_i^c + D_i^{form} Fik=n1i=1∑nPic+Diform

最前方的无人机飞向最远的目标点，最后方的无人机飞向最近的目标点，重新映射后的目标点记为 P i f o r m P_i^{form} Piform。

任务三：未知环境下追踪动态目标的协同编队 集群围绕动态目标形成特定编队，第 i i i 架无人机的编队目标点 H f o r m i H_{form}^i Hformi 依据距离阈值 ϵ \epsilon ϵ 切换：

H f o r m i = { H t a r g e t + D i f o r m if ∥ P i c − H f o r m i ∥ ≤ ϵ H c f o r m + D i f o r m otherwise H_{form}^i = \begin{cases} H_{target} + D_i^{form} & \text{if } \|P_i^c - H_{form}^i\| \leq \epsilon \\ H_c^{form} + D_i^{form} & \text{otherwise} \end{cases} Hformi={Htarget+DiformHcform+Diformif ∥Pic−Hformi∥≤ϵotherwise

若在误差 ϵ \epsilon ϵ 内，则以目标点 H t a r g e t H_{target} Htarget 为基准编队；否则以集群当前几何中心 H c f o r m H_c^{form} Hcform 为基准聚合，确保安全逼近。

4.论文结果

5.参考文献

Yang F, Lu Q, Zhang B, et al. Dynamic Trajectory Planning for a Group of Unmanned Aerial Vehicles in Unknown Environments $J$ . IEEE Transactions on Automation Science and Engineering, 2025.