目录
1.摘要
针对 Dubins 运动规划中采样随机性导致的路径曲率冗余问题，本文通过融合 Dubins 路径 RRT* 算法采样与碰撞检测机制，为保障路径优化过程中的安全性，引入了安全半径扩张策略。针对求解复杂性，本文提出了三种基本形式及直接求根法，设计了曲率约束路径优化算法（CCPOA）。通过整合改进Dubins-RRT*（MDR）与 CCPOA构建了一套完整运动规划框架，仿真结果验证了该算法在路径质量与规划效率上的优越性。
2.Dubins-RRT*算法
在平面坐标中引入航向角，将状态空间由 z = ( x , y ) ∈ R 2 z=(x,y)\in\mathbb{R}^2 z=(x,y)∈R2扩展：
q = ( x , y , θ ) ∈ R 2 × S q=(x,y,\theta)\in\mathbb{R}^2\times\mathbb{S} q=(x,y,θ)∈R2×S
利用 Dubins 路径的解析解实现满足车辆运动学约束的状态连接， D u b i n s S t e e r i n g ( q 1 , q 2 , r ) DubinsSteering(q_1,q_2,r) DubinsSteering(q1,q2,r)替代传统步进连接函数，并相应地将碰撞检测扩展为涵盖直线与圆弧段的 D u b i n s C o l l i s i o n F r e e ( ) DubinsCollisionFree() DubinsCollisionFree()。
根据曲率约束最短路径问题（CCSPP），本文通过将多点路径优化解构为三点 Dubins 问题 (3PDP)，即在已知两端点位姿及中间点位置的前提下，寻求最优中间点航向角 θ m \theta_m θm 以最小化总路径长度：
θ m = arg min θ ∈ [ 0 , 2 π ) ( C o [ ( z 1 , θ 1 ) , ( z m , θ ) ] + C o [ ( z m , θ ) , ( z 2 , θ 2 ) ] ) \theta_m = \arg\min_{\theta \in [0,2\pi)} \left( \mathrm{Co}[(z_1,\theta_1),(z_m,\theta)] + \mathrm{Co}[(z_m,\theta),(z_2,\theta_2)] \right) θm=argθ∈[0,2π)min(Co[(z1,θ1),(zm,θ)]+Co[(zm,θ),(z2,θ2)])
3PDP 最优解路径必属于特定路径集合 F F F 或其子串，且在最优条件下， θ m \theta_m θm 需满足如下等式约束：
cos ( Φ 1 − θ m ) cos ( α 1 / 2 ) = cos ( Φ 2 − θ m ) cos ( α 2 / 2 ) \frac{\cos(\Phi_1 - \theta_m)}{\cos(\alpha_1/2)} = \frac{\cos(\Phi_2 - \theta_m)}{\cos(\alpha_2/2)} cos(α1/2)cos(Φ1−θm)=cos(α2/2)cos(Φ2−θm)
其中， Φ i \Phi_i Φi 与 α i \alpha_i αi 分别对应路径段的方位角与弧度参数。
3.主要结果
具有角度生成规则的 Dubins-RRT * 算法
改进 Dubins-RRT *（MDR）算法通过平衡 RRT * 渐近最优性与 Dubins 路径的运动学约束，引入安全半径扩张机制与自适应角度生成规则。
在安全性保障方面，MDR 通过扩张安全半径 r s a f e r_\mathrm{safe} rsafe，将复杂的圆弧路径碰撞检测等效转化为基于直线段的几何检测，显著简化了计算过程。根据理论分析，当节点间距 d ≥ 4 r d\geq4r d≥4r时，任意 Dubins 路径点至连接段位移矢量的距离均满足
d ( z , z 1 z 2 ) ≤ 2 r d(z,z_1z_2)\leq2r d(z,z1z2)≤2r
在采样策略上，MDR采用了一种非随机角度生成方案，通过预设规则确定采样点的参考航向角 θ : \theta: θ:
θ = 1 2 ( a t a n 2 ( y d − y r a n d , x d − x r a n d ) + θ d ) \theta=\frac12(\mathrm{atan2}(y_d-y_{\mathrm{rand}},x_d-x_{\mathrm{rand}})+\theta_d) θ=21(atan2(yd−yrand,xd−xrand)+θd)
CCPO-3PDP
CCSPP可解构为多个关联的 3PDP实例，其最优航向角序列 Θ ∗ \Theta^* Θ∗需满足如下一致性约束：
θ i ⋆ = f ( z i − 1 , z i , z i + 1 , θ i − 1 ⋆ , θ i + 1 ⋆ ) , i = 1 , ... , n \theta_i^\star=f(z_{i-1},z_i,z_{i+1},\theta_{i-1}^\star,\theta_{i+1}^\star),\quad i=1,\ldots,n θi⋆=f(zi−1,zi,zi+1,θi−1⋆,θi+1⋆),i=1,...,n
为高效求解该非线性方程组，CCPOA 采用了基于 3PDP 算子的迭代优化策略。为加速收敛，本文利用路径段几何特征提出了启发式初始值预估公式：
θ m i = 1 2 [ κ ( z m i − 1 − z m i ) + κ ( z m i + 1 − z m i ) ] \theta_{mi}=\frac12[\kappa(z_{mi-1}-z_{mi})+\kappa(z_{mi+1}-z_{mi})] θmi=21[κ(zmi−1−zmi)+κ(zmi+1−zmi)]
4.结果展示
5.参考文献
Wang, Jianan, et al. "Dubins-RRT* motion planning algorithm considering curvature-constrained path optimization." Expert Systems with Applications 296 (2026): 128390.
6.代码获取
xx
7.算法辅导·应用定制·读者交流
xx