目录
1.摘要
本文提出了一种基于改进速度障碍法（VO）与球面斥力势场法相结合的多无人机三维避障策略，该策略旨在解决无人机在复杂动静态环境中的实时安全导航问题，通过优化路径选择机制减少航程损耗，并利用混合算法攻克了近距离避障失效的难题。
2.三维速度障碍避障策略
通过将无人机与障碍物抽象为质点 A A A 和 B B B，将传统的二维速度障碍法扩展至三维空间，并利用以 B B B 为中心的球体定义保护区 S p z S_{pz} Spz：
( x − x 0 ) 2 + ( y − y 0 ) 2 + ( z − z 0 ) 2 ≤ r p z 2 (x-x_0)^2+(y-y_0)^2+(z-z_0)^2\leq r_{pz}^2 (x−x0)2+(y−y0)2+(z−z0)2≤rpz2
通过从点 A A A向该球体引射线切线，在三维空间内构造出一个圆锥体及其内部区域，即碰撞锥 C C A , B ∘ CC_{A,B\circ} CCA,B∘
传统三维速度障碍避障策略的改进
三维速度障碍（VO）避障模型以无人机为原点建立坐标系，通过障碍物相对位置定义碰撞锥，并用几何关系得到其关键参数
r v o = r p z d A B 2 − r p z 2 d A B , d v o = d A B 2 − r p z 2 d A B , θ v o = arctan r v o d v o r_{vo} = \frac{r_{pz}\sqrt{d_{AB}^2 - r_{pz}^2}}{d_{AB}},\quad d_{vo} = \frac{d_{AB}^2 - r_{pz}^2}{d_{AB}},\quad \theta_{vo} = \arctan\frac{r_{vo}}{d_{vo}} rvo=dABrpzdAB2−rpz2 ,dvo=dABdAB2−rpz2,θvo=arctandvorvo
利用相对速度 v q ′ = v q − v o \mathbf{v}'q = \mathbf{v}q - \mathbf{v}o vq′=vq−vo，在旋转坐标系中通过条件
c v q x ′ > 0 , c v q y 2 + c v q z 2 c v q x ′ < r v o d v o cv'{qx} > 0,\quad \frac{\sqrt{cv{qy}^2 + cv{qz}^2}}{cv'{qx}} < \frac{r{vo}}{d_{vo}} cvqx′>0,cvqx′cvqy2+cvqz2 <dvorvo
判断是否进入碰撞锥，从而确定潜在碰撞。将碰撞锥沿障碍物速度平移得到速度障碍 VO（顶点为 v o \mathbf{v}o vo，表示所有危险速度集合），通过绕 x x x 轴旋转构造避障平面族，并利用交点参数
t g = z e l Φ z A Φ − z B Φ t_g = \frac{z^\Phi{el}}{z^\Phi_A - z^\Phi_B} tg=zAΦ−zBΦzelΦ
求得碰撞锥与平面的截面边界，截面形状由 θ e l Φ \theta^\Phi_{el} θelΦ 与 θ v o \theta_{vo} θvo 的关系决定。为降低计算量，引入目标点引导选择最优平面
ϕ t a r g e t = arctan P z P y , ϕ ∗ = arg min ( S v o ϕ ) , S v o ϕ = 1 2 ∣ ∑ ( x i y i + 1 − x i + 1 y i ) ∣ \phi_{target} = \arctan\frac{P_z}{P_y},\quad \phi^* = \arg\min(S_{vo\phi}),\quad S_{vo\phi} = \frac{1}{2}\left|\sum(x_i y_{i+1} - x_{i+1} y_i)\right| ϕtarget=arctanPyPz,ϕ∗=argmin(Svoϕ),Svoϕ=21 ∑(xiyi+1−xi+1yi)
基于势场与速度碰撞锥相结合的速度障碍避障策略
针对传统三维速度障碍法在靠近保护球 S p z S_{pz} Spz 时易出现奇异解的问题，引入球形势场 S p f S_{pf} Spf（半径 r p f > r p z r_{pf} > r_{pz} rpf>rpz）进行改进：当无人机进入该区域时，势场提供沿径向向外的排斥速度，使系统在扰动、延迟或高速障碍情况下仍能稳定避障。势场函数通过
Υ p f ( λ ) = ∫ λ r p f Θ p f ( x ) d x \Upsilon_{pf}(\lambda) = \int_{\lambda}^{r_{pf}} \Theta_{pf}(x)\,dx Υpf(λ)=∫λrpfΘpf(x)dx
构造，其中 Θ p f ( λ ) \Theta_{pf}(\lambda) Θpf(λ) 为光滑有界的排斥函数，结合 bump 函数保证连续性，最终得到径向速度
v p f = k p f Θ p f ( ∥ P B b ∥ σ ) P B b ∥ P B b ∥ v_{pf} = k_{pf}\, \Theta_{pf}(\|P_B^b\|_\sigma)\frac{P_B^b}{\|P_B^b\|} vpf=kpfΘpf(∥PBb∥σ)∥PBb∥PBb
根据与障碍距离 d A B d_{AB} dAB 的不同，最终速度采用分段形式，在保护区内仅使用势场速度（紧急避障），在势场区内叠加VO速度 + 势场速度实现平滑过渡，而在势场外仅采用 VO 策略
v d = { v p f , d A B ≤ r p z R ( − ϕ ) v v o Φ + v p f , r p z < d A B ≤ r p f R ( − ϕ ) v v o Φ , d A B > r p f v_d = \begin{cases} v_{pf}, & d_{AB} \leq r_{pz} \\ R(-\phi)v_{vo}^\Phi + v_{pf}, & r_{pz} < d_{AB} \leq r_{pf} \\ R(-\phi)v_{vo}^\Phi, & d_{AB} > r_{pf} \end{cases} vd=⎩ ⎨ ⎧vpf,R(−ϕ)vvoΦ+vpf,R(−ϕ)vvoΦ,dAB≤rpzrpz<dAB≤rpfdAB>rpf
3.结果展示
4.参考文献
1\] Xian B, Jiang P, Hu J, et al. A three-dimensional collision avoidance strategy for multiple unmanned aerial vehicles using velocity obstacle method\[J\]. Aerospace Science and Technology, 2025: 110793. ### 5.代码获取 xx ### 6.算法辅导·应用定制·读者交流 xx