EGO-Planner: a local path planning method without ESDF (Euclidean distance field) gradient

0 abstract

based ongradientThe planner is widely used in quadcopterslocal path planning,inEuclidean distance field (ESDF)for assessmentGradient magnitude and directionCrucial. However, there is a lot of redundancy in computing such a field, since the trajectory optimization process only coversESDFA very limited subspace of update range. In this paper, a method based onNo ESDFGradient planning framework significantly reduces computation time. The main improvements areThe collision term in the penalty function is formulated by comparing the collision trajectory with the collision-free guidance path. Only when the trajectory encounters a new obstacle will the generated obstacle information be stored, allowing the planner toExtract only necessary obstacle information. Then, if dynamic feasibility is violated, we wouldExtend time allotment，Introducing anisotropic curve fitting algorithm, adjusting the trajectory while maintaining the original shapehigher order derivatives。

1 Overview

1.1 Traditional methods

• Traditional methods generally use gradient-based planners, but rely on pre-built $ESDF$ plot to evaluate the magnitude and direction of gradients and use numerical optimization to generatelocal optimal solutions. However, just building$ESDF$ diagram is difficult,$ESDF$ calculations take up 70%$of\; the\; total\; time$$spent$ executing local planning.$70\%$ , so, build$ESDF\; becomesthe\; bottleneck\; of\; gradient-$ based planners.

1.2 How to build $ESDF$ ？

• The method can be divided into incremental full update and batch local calculation. However, neither of these two methods pays attention totrajectory itself. Therefore, too much computation is spent on computing ESDF ESDF which does not contribute to planning$ESDF$ value. In other words, currently based on$The\; ESDF$ method cannotServes trajectory optimization individually and directly. As shown in the figure below, for general autonomous navigation scenarios, the UAV is expected to avoid collisions locally, and the trajectory only covers a limited space within the ESDF update range. In practice, although some manual rules can determine a small ESDF range, they lack theoretical rationality and still lead to unnecessary calculations

---

• As shown in the figure, the trajectory during the optimization process only covers a very limited space of the ESDF update range.

• E S D F ESDF The updated space of$ES$$D$$F$ is the space surroundedpink

• But the space covered during trajectory optimization (as shown in the purple part in the picture above)

• So the trajectory only covers $The\; limited\; space\; of\; ESDF\; update\; range.\; In\; practice,although$ some manual rules can determine a small ESDF range, they lack theoretical rationality and still lead to unnecessary calculations$.$

---

1.3 $EGO-$Introduction to$Pl$$ann$$er$

• $EGO-Planner$ is anESDF-free ESDFGradient-based local path planning framework for$ES$$D$$F$
• $EGO-Planner\; consists\; ofa$ gradient-based spline optimizer and a post-refinement$procedure$

1.3.1 Method steps

• First optimize the trajectory using smoothness, collisions, and dynamic feasibility
• vs. precomputed $Unlike\; traditional\; methods\; of\; ESDF$ , collision costs are modeled by comparing trajectories within obstacles with guided collision-free trajectories.
• The force is then projected onto the collision trajectory and an estimated gradient is generated to wrap the trajectory outside the obstacle
• During the optimization process, the trajectory will bounce several times between nearby obstacles , eventually terminating in a safe area
• This way we only calculate gradients when necessary and avoid calculating $ESDF$
• 如果生成的轨迹违反动态限制，这通常是由不合理的时间分配引起的，则激活细化过程
• 在细化期间，当超出限制时重新分配轨迹时间。随着时间分配的扩大，生成了一种新的$B$样条，它在平衡可行性和拟合精度的同时，拟合了之前的动态不可行
• 为了提高鲁棒性，拟合精度采用各向异性建模，在轴向和径向上有不同的惩罚

2 相关工作

2.1 基于梯度的运动规划

• 基于梯度的运动规划是无人机局部轨迹生成的主流，这是一种无约束的非线性优化
• 许多框架利用其丰富的梯度信息，直接优化配置空间的轨迹
• 还有一种连续时间多项式的轨迹优化方法，但是，势函数积分导致了很大的计算代价
• 还有就是老朋友 $B-Spline$的参数化，利用其凸包特性
• 通过寻找无碰撞路径（通常可由采样的路径规划找出，比如$Kinodynamic$ $RRT\ast$），将无碰撞的初始路径作为前端，成功率显著提高
• 当初始无碰撞路径的生成考虑动力学约束的同时，性能会进一步提高
• 结合感知意识，鲁棒性提升明显
• $ESDF$ plays a vital role in assessing the distance to nearby obstacles in terms of gradient magnitude and direction.

2.2 $ESDF$

• $ESDF\; haslong$ been used to construct objects from noisy sensor data. Some scholars have proposed an envelope algorithm that converts$ESDF$ ESDFThe time complexity of$ES$$D$$F$$O\left(n\right)$ ;$n$ is expressed as the number of voxels. This algorithm does not work with$Incremental\; construction\; of\; ESDF$ , while dynamic update fields are often required during quadcopter flight. To solve this problem,$Oleynikova$ and$Han$ proposed incremental$ESDFgeneration$ method,$Voxblox$$Voxblox$ 和 $FIESTA$ . Although these methods are very efficient in dynamic update situations, the resulting$ESDF$ almost always contains redundant information that$may\; not$$be$ used in the planning process at all. As shown above, the trajectory only sweeps the entireESDF ESDFA very limited subspace of the$ES$$D$$F\; update\; range.$Therefore, there is value in designing a smarter, more lightweight approach rather than maintaining the entire domain.

2.3 Collision avoidance

• The decision variable is BBControl point QQ$of\; B-$ spline curve$Q.$ _ Every$Q$ each independently has its own environment information. At the beginning, a BBthat satisfies the constraints but does not consider obstacles is generated.$B-$ spline curve$\Phi$ , next, for each collision segment detected in the iteration, use some algorithms (such as A*, RRT* and other global planning algorithms) to generate a collision-free path$\Gamma$ . For each control point Q i Q_iof the colliding line segment$Q_i$, will generate a positioning point pij p_{ij} on the obstacle surface.$p_{ij}$, and corresponds to a repulsive direction vector $v_{ij}$，$v_{ij}$with vector $Q_i$$p_{ij}$In the same direction, it should not be equal here, because this $v_{ij}$It is just a unit direction vector, just look at the red dotted and solid lines in the figure below. Each pair $p,v$ corresponds to a specific control point$Q_i$, this can also be seen from the figure, as shown in the figure below.

---

---

• for $i\in N+$ represents the index of the control point,$j\in N$ means { p , vp, v $p,Index\; of\; v$ } pair. Note that each { p , vp, v $p,v$ } pairs belong only to one specific control point. For the sake of brevity, the subscripts$ij$ without causing ambiguity. Then from$Q_i$to the $The\; obstacle\; distance\; of\; j$ obstacles is defined as

---

• $p,How\; is\; v$ generated?
• Trajectory through obstacles $\Phi$ is the control point$Q$ generates several$(p,v)$ to
• $p$ is a point on the obstacle surface ( pp was mentioned earlier$p$ is determined by the control point$Q$ generated),$v$ is from the control point$Q$ points toppunit vector of$p\; )$
• Perpendicular to the tangent vector $R_i$ （这个切向量，仔细看，就是之前用 $A\ast$ 生成的那条的不考虑碰撞的曲线的切线）的平面 $\Psi$ 与 $\Gamma$ （$\Gamma$是那一条无碰撞路径）相交，形成一条线 $l$，从中确定 $p,v$ 对。 距离场定义 $d_{ij}=(Q_i-p_{ij})\cdot v_{ij}$ 的切片可视化。颜色表示距离，箭头是等于 $v$ 的相同梯度。 $p$ 在平面上（障碍物表面上的点，如图 $c$ ）。
  

---

2.3.1 算法1：$Che\; ckAndAddObstacleInfo$

---

---

Pseudocode analysis:

1. $FindConsecutiveCollidingSegment\left(Q\right):$ Find the control point$QThe$ obstacle that collides with and determines whether it exists
2. $GetCollisionSegment\left(\right):$ Extract and control point$QColliding$ obstacles
3. $.push\_back\left(\right):$ Add obstacles to the$obstacle$$set$$S$ in
4. $PathSearch\left(\right):$ Generate a collision-free path for obstacles
5. $Find\_p\_v\_Pairs(Q,path):$ The matching$(p,v)$ to

• $Q_i$to the $The\; distance\; between\; j$ obstacles is as follows, you need to pay attention to the unit vector$v$ will not change again after the first generation, so$d_{ij}$The value of is divided into positive and negative, $d_{ij}=(Q_i-p_{ij})\cdot v_{ij}$
• Through the above formula, we can use it to determine the control point $Q_i$
• Because as I said before, vij v_{ij} among the old obstacles$v_{ij}$with vector $Q_i$$p_{ij}$in the same direction, then the vector $Q_i$$p_{ij}$That is $-(Q_i-p_{ij})$ , which is the same as$v_{ij}$in the same direction, while $(Q_i-p_{ij})$ must be the same as$v_{ij}$In reverse, if it is not a newly discovered obstacle, then $d_{ij}=(Q_i-p_{ij})\cdot v_{ij}$The calculation must be $d_{ij}<0$ (the two are in opposite directions), and fornewly discovered obstacles, it must be$d_{ij}=(Q_i-p_{ij})\cdot v_{ij}$, calculated as $d_{ij}>0$

In summary, in order to prevent the trajectory from being pulled out in front of the current obstacle, in order to avoid repeated { p , v } \{p, v\} before the trajectory escapes from the current obstacle in the first few iterations{ p,v } pair, { p , v } \{p,v\}is generated repeatedly during the iteration process$\{p,v\}$ Yes, the criterion for judging whether it is a new obstacle is: if the control point$Q_i$When in an obstacle, and for all obstacles jj currently obtained$j$ satisfies$d_{ij}>0$ , the obstacle is anewly discoveredobstacle, so only the obstacle information that affects the trajectory is calculated and the running time is reduced.

2.3.2 Summary of collision force estimation

In order to incorporate the necessary* environmental awareness into local planning, an objective function needs to be explicitly constructed ( designing a gradient-based trajectory optimizer ) to keep the trajectory clear of obstacles. $ESDFprovides$ this crucial collision information at the cost of$a$ heavy computational burden. In addition, as shown in Figure$2$ , due toESDF ESDFInsufficient error information returned by$ES$$D$$F$$The\; planner\; of\; ESDF$ can easily get stuck ina local minimumand cannot escape the obstacle. To avoid this, an additional front end is always needed to provide a collision-free initial trajectory. Since explicitly designed repulsions are quite effective for different tasks and environments, the above methodESDF ESDFimportant information forcollision avoidance.$ESDF$ 。

---

---

3 Gradient-based trajectory optimizer

3.1 Modeling

• This article uses uniform $B-$ spline curve$\Phi$ represents the trajectory, and its order is$p_b$, uniform $Each\; node\; of\; the\; B$ -spline curve has the same time interval{ t 1 , t 2 , . . . , t M } \{t_1, t_2, ... , t_M\}{ t1​,t2​,...,tM​} , the time interval between each node and its parent node is the same, which is:${△}_t=t_{m+1}-t_m$
• $The\; first\; property\; of\; the\; B$ -spline curve, that is, the convex hull property, shows that a certain section of the curve is only related to$p_b+1$ continuous control point ($\mathrm{Q}\in {\mathbb{R}}^3$ ) is related, and the curve is contained within the convex hull formed by these points, such as$B$ -spline$(t_i,t_{i+1})$ 内的跨度位于由 { Q i − p b , Q i − p b + 1 , . . . , Q i } \{Q_i - p_b, Q_{i - p_b + 1}, ... , Q_i\} { Qi​−pb​,Qi−pb​+1​,...,Qi​} 形成的凸包内
• $B$ 样条曲线的第二个性质表面，$B$ 样条曲线的 $k$ 阶导数仍然是 $p_b-k$ 阶 $B$ 样条曲线，由于时间间隔 ${△}_t$ 是相同的，轨迹 $\Phi$ 的一阶（$V_i$）、二阶（$A_i$）、三阶（$J_i$）导数的控制点分别为：$${\mathbf{V}}_i=\frac{{\mathbf{Q}}_{i+1}-Q_i}{\Delta t},{\mathbf{A}}_i=\frac{V_{i+1}-V_i}{\Delta t},{\mathbf{J}}_i=\frac{A_{i+1}-A_i}{\Delta t}$$
• In fact, the general framework is $Fast-Planner\text{'}sframework$ ,$HKUST$ is still awesome
• Then according to $UAV$ 的微分平坦特性，需要降低要规划的变量，优化问题被重新定义为：$$\underset{\mathrm{Q}}{\min }J={\lambda }_s{J}_s+{\lambda }_c{J}_c+{\lambda }_d{J}_d$$

微分平坦特性，在做轨迹规划的过程中不可能对 $UAV$ $12$ 维的全维度空间进行规划，这是非常复杂的，但可以找到一个平坦输出的空间，这个空间只有四个维度的变量，位置$x,y,z$和偏航角 $\Psi$，剩下所有的状态都能用这四个变量和其有限阶导数的代数组合所表示，因此在做无人机轨迹规划的时候只需要对这四个变量进行规划就可以了。

• 是不是很熟悉？这其实就是在贝塞尔曲线那里的三个惩罚项函数
• $J_s$is the smooth term penalty, $J_c$is the collision term penalty, $J_d$is the dynamic feasible item penalty, $\lambda$ isthe weight of the penalty term

3.1.1 Smooth term penalty

• In $B-SPLine$ trajectory optimization was proposed, you can check out my previous$Blog$ , has been mentioned. The smoothness penalty is formulated asthe time integralover the squared derivative ofacceleration, jerk, etc.. Thanks toBB$The\; convex\; hull\; property\; of\; B$ -spline curves, as long as the trajectory Φ Φis minimized$The\; sum\; of\; squares\; of\; the\; second-order\; and\; third-order\; control\; points\; of\; \Phi$ can effectively reduce the sum of squares of acceleration and jerk (similar to$minimum$ $snap$ , this thing is really classic, used everywhere), the formula is as follows:$$J_s=\sum _{i=1}^{N_c-2}{\Vert A_i\Vert }_2^2+\sum _{i=1}^{N_c-3}{\Vert J_i\Vert }_2^2$$

3.1.2 Collision penalty

• The collision penalty moves the control point away from the obstacle by taking a safety gap and penalizing the control point $d_{ij}<s_f$to achieve. In order to further optimize, a quadratic continuously differentiable penalty function is constructed , and as $d_{ij}$decreases to suppress its slope, thus obtaining the piecewise function: $$j_c(i,j)=\{\begin{array}{ll}0,& c_{ij}\le 0\\ c_{ij}^3,& 0\le c_{ij}\le s_f\\ 3s_f{c}_{ij}^2-3s_f^2{c}_{ij}+s_f^3,& c_{ij}>s_f\end{array}$$
  $$c_{ij}=s_f-d_{ij}$$
• Summing the penalties for all control points gives the total collision term penalty: $$J_c=\sum _{i=1}^{N_c}{j}_c\left(Q_i\right)$$
• Compared with the traditional trilinear interpolation method to find the gradient of the collision term, the quadratic continuously differentiable penalty function pair $Q_i$ 的导数来得到梯度：$$\frac{\partial j_c}{\partial Q_i}=\sum _{i=1}^{N_c}\sum _{j=1}^{N_p}\{\begin{array}{ll}0,& c_{ij}\le 0\\ -3c_{ij}^2,& 0\le c_{ij}\le s_f\\ -6s_f{c}_{ij}+3s_f^2,& c_{ij}>s_f\end{array}$$

3.1.3 Feasible item penalty

• Its feasibility is ensured by limiting the high-order derivatives of the trajectory in each dimension . Due to the nature of the convex hull, constraining the derivatives of the control points is sufficient to constrain the entire $B$ -splines. $F\left(\right)$ is for each dimension (velocity accelerationJ erk JerkPenalty function constructed from higher-order derivatives of$J$$er$$k$$$J_d=\sum _{i=1}^{N_c-1}{oh}_{v}F\left(V_i\right)+\sum _{i=1}^{N_c-2}{oh}_{a}F\left(A_i\right)+\sum _{i=1}^{N_c-3}{oh}_{j}F\left(J_i\right)$$
  $$F(C)=\sum _{r=x,y,z}f\left(c_r\right)$$
  $$f\left(c_r\right)=\{\begin{array}{ll}{a}_1{c}_r^2+b_1{c}_r+c_1,& c_r<-c_j\\ {\left(-\lambda c_m-c_r\right)}^3,& -c_j<c_r<-\lambda c_m\\ 0,& -\lambda c_m\le c_r\le \lambda c_m\\ {\left(c_r-\lambda c_m\right)}^3,& \lambda c_m<c_r<c_j\\ a_2{c}_r^2+b_2{c}_r+c_2,& c_r>c_j\end{array}$$

It can be seen that the penalty functions applied in problem modeling are all polynomial sums, which is beneficial to reducing the complexity of solving optimization problems. (Lightweight algorithm)

3.2 Optimization solution (numerical optimization)

• Ratio: $$\underset{\mathrm{Q}}{\min }J=l_s{J}_s+l_c{J}_c+l_d{J}_d$$will continue to change as new obstacles are added, which requires the solver to restart and solve quickly, and the objective function mainly consists of quadratic terms, so $Hessian($ Hessian matrix) information can speed up convergence. But get the exact$Hessian$ consumes a lot of computer resources. So use the quasi-Newton method ($quasi-Newton$ $methods$ ) Approximately calculate Hessian Hessianfrom gradient information$Hessian$
• B arzilai − B orwein Barzilai- Borwein$Barzilai\_\_-Borwein$ $method$、$truncated$ $Newton$ $method$和$L-BFGS$ m e t h o d h methodh After$m$$e$$t$$h$$o$$d$$h$$L-BFGS$ performs best, balancing restart loss and inverse$The\; accuracy\; of\; the$$Hessian$ estimate. $L-BFGS$ evaluates the approximate Hessianfrom the previous objective function$Hessian$ , but requires a series of iterations to reach a relatively accurate estimate

4 Time redistribution and trajectory further optimization

• Exact time allocation before optimization is unreasonable because no information about the final trajectory is known at that time, so an additional time reallocation procedure is crucial to ensure dynamic feasibility
• Previously, the trajectory was parameterized as non-uniform $B$ -splines that iteratively lengthen a subset of the node spans when some segments exceed the derivative limit
• However, a node spans $△t_n$Can affect multiple control points, causing discontinuities in higher-order derivatives of the previous trajectory when adjusting node spans near the start state
• So the safe trajectory Φ s Φ_s obtained from the previous section${Phi}_s$, after reasonable time reallocation, regenerate a $B-$ spline curve${Phi}_f$, and then use the anisotropic curve fitting method ( $an$ $anisotropic$ $curve$ $fitting$ $method$）使 ${Phi}_f$While maintaining shape with ${Phi}_s$almost the same derivative shape at the same time (since ${Phi}_s$are safe trajectories, so the shapes must be more or less the same), and can freely optimize their control points to satisfy the constraints of higher-order derivatives

4.1 Specific steps

• First like $Fast-As\; Planner\; did,\; calculate\; the\; limit\; exceedance\; rate\; (the\; maximum\; proportion\; exceeding\; thelimit$ , subscript$m$ 表示限制的最大值）：$$r_e=\max \left\{\left|{\mathbf{V}}_{i,r}/v_m\right|,\sqrt{|{\mathbf{A}}_{j,r}/a_m|},\sqrt[3]{|{\mathbf{J}}_{k,r}/j_m|},1\right\}$$
• $r_e$Show that relative to ${Phi}_s$In other words, ${Phi}_f$How much time needs to be allocated
• $V_i$，$A_j$and $J_k$are respectively with $△$ The first, second and third orders of$t\; are\; inversely\; proportional.\; The\; speed\; and\; its\; derivatives\; can\; be\; reduced\; through\; the\; inverse\; relationship\; with\; time.$
• Obtained ${Phi}_f$The new time interval is: $$\Delta t^{\prime }=r_e\Delta t$$
• By solving a closed- form least squares problem as follows, the initial generation time span is Δ t ′ \Delta t^{\prime} under the constraints of velocity and its derivatives$\Delta t^{\prime }$ ’s trajectory${Phi}_f$, while maintaining and ${Phi}_s$The same shape and number of control points, and then recalculate the smoothness term penalty and feasibility term penalty to get a new objective function: $$\underset{\mathbf{Q}}{\min }{J}^{\prime }=l_s{J}_s+l_d{J}_d+l_f{J}_f$$
• $l_f$is the weight of the fitness ( fitting ) term, $J_f$is defined as from ${Phi}_f(\alpha T\prime )$ to${Phi}_s\left(\alpha T\right)$ Integral of anisotropic displacement ($a\in [0,1]$ ), where$T$ and$T\prime$ is the trajectory${Phi}_s$和${Phi}_f$the duration of
• Since the fitted curve ${Phi}_s$It is already collision-free ( just said ${Phi}_s$is a safe trajectory ), so for the two curves, use the axial displacement da d_a with low weight $d_a$To relax the smooth adjustment restrictions , use a high-weighted radial displacement $d_r$to prevent collisions , as shown in the figure below,

---

---

• Use a spherical metric (it seems to have been mentioned in one of the teacher's articles, I forgot the details) to make the displacement on the surface of the same sphere produce the same penalty. (About radial displacement and axial displacement should be understood in the specific algorithm,At present, I think that the axial displacement is the tangent direction of the point, and the radial direction is the perpendicular direction of the tangent line. The calculation formulas for both are as follows:)
  $$\begin{array}{rl}{d}_a& =\left(F_f-{Phi}_s\right)\cdot \frac{{\dot{Phi}}_s}{\Vert {\dot{Phi}}_s\Vert }\\ d_r& =\Vert \left(F_f-{Phi}_s\right)\times \frac{{\dot{Phi}}_s}{\Vert {\dot{Phi}}_s\Vert }\Vert \end{array}$$

• Used to measure ${Phi}_f(\alpha T\prime )$ penalty size ellipsoid is an ellipsoid with${Phi}_f(\alpha T\prime )$ is the center of the ellipse, and its semi-major axis length is$a$ . Its semi-minor axis length is$b$ . Then the axial displacement$d_a$and radial displacement $d_r$ 为：
  $$\begin{array}{rl}& d_a\left(\alpha T{\_}^{\prime }\right)=\left(F_f\left(\alpha T{\_}^{\prime }\right)-{Phi}_s\left(\alpha T\right)\right)\cdot \frac{\dot{{Phi}_s}(\alpha T}{\Vert \dot{{Phi}_s}\left(\alpha T\right)\Vert }\\ & d_r\left(\alpha T{\_}^{\prime }\right)=\Vert \left(F_f\left(\alpha T{\_}^{\prime }\right)-{Phi}_s\left(\alpha T\right)\right)\times \frac{\dot{{Phi}_s}(\alpha T}{\Vert \dot{{Phi}_s}\left(\alpha T\right)\Vert }\Vert \end{array}$$

• Then the fitting term can be expressed as:
  $$J_f={\int }_0^1\left[\frac{d_a{\left(\alpha T{\_}^{\prime }\right)}^2}{a^2}+\frac{d_r{\left(\alpha T{\_}^{\prime }\right)}^2}{b^2}\right]da$$

• Among them $a$ and$b$ are the semi-major axis and semi-minor axis of the ellipse respectively, and the semi-minor axis bbcorresponding to the radial displacement$b$ Increase the penalty weight of radial displacement to prevent collisions

• The fitting term of the above formula is discretized into a limited number of points ${Phi}_f(\alpha \Delta t\prime )$和${Phi}_s\left(aDt\right)$

• Among them, $\mathrm{K}\in \mathbb{R}$，$0\le k\le [T/△t]$

5 Experimental results

5.1 Algorithm framework

---

---

• Code analysis
• $FindInit(Q,G):$ Generate a BBthat satisfies terminal constraints but does not consider obstacles.$B-$ spline curve$The\; control\; point\; corresponding\; to\; \Phi$ corresponds to the initialization step.
• $IsCollisionFree(E,Q):$ Determine whether the control point is collision-free in the environment. If there is a collision,$false$ , output$true$
• $Che\; ckAndAddObstacleInfo(E,Q):$ Detect the obstacles where the control points are located and add obstacle information ({ p, v} \{p, v\}{ p,v } pair and distance field)
• $EvaluatePenalty\left(Q\right):$ Construct corresponding penalty terms for control points based on problem modeling$J$ and the corresponding gradient (this penalty term is the three penalties, the smooth term penalty, the collision term penalty, and the feasible term penalty)
• $OneStepOptimize(J,G):$ Solve the minimization problem of the penalty term, that is,the optimization solution, thereby obtaining the control point position that satisfies the minimization of the penalty term, that is, completing the first step of trajectory optimization
• $IsFeasible\left(Q\right):$ Judgment is determined by control point$Whether\; the\; trajectory\; determined\; by\; Q$ is feasible (mainly whether the speed and its multi-order derivatives exceed the limiting maximum value)
• $ReAllocateTime\left(Q\right):$ By reassigning the control point$The\; time\; reduction\; speed\; of\; the\; trajectory\; determined\; by\; Q$ and its multi-order derivatives make it meet various speed constraints.
• $CurveFittingOptimize\left(Q\right):$ Replace the collision term in the previous penalty with a curve fitting term to solve the penalty term optimization problem. On the premise of satisfying the new time interval, it can fit the trajectory composed of the old control points to obtain a new trajectory, and achieve constrained feasibility on the premise of inheriting the collision-free characteristics of the old trajectory.

5.2 Implementation details

• Set the order of the B-spline curve $p_b=3$ , the number of control points$N_c=About\; 25$ , depending on the planning and expected distance (about$7m$ ) and the initial distance between adjacent points (approximately$0.3m$ ) decided. These parameters are empirically derived by balancing the complexity and degrees of freedom of the problem

• Because according to $According\; to\; the\; properties\; of\; B$ -spline curves, a control point only affects the surrounding trajectory, so the time complexity of the algorithm is$O(N_c)$

• $L-The\; complexity\; of\; BFGS$ is also linear over the same relative tolerance ($The$ $complexity$ $of$ $L-BFGS$ $is$ $also$ $linear$ $on$ $the$ $same$ $relative$ $tolerance$)

• In collision-free path search, we use $A$ star ($A\ast$ ) algorithm performs trajectory optimization, and the trajectory it generates$\Gamma$ often sticks to obstacles. Therefore, we can directly useAATrajectory Γ Γgenerated by$A\; star\; algorithm$Select the anchor point on$\Gamma$$anchor$ $point$）$p$ without having to search the surface of the obstacle (here is the real explanation of the use of$The\; role\; of\; the\; A$ -star algorithm). For Figure$3$ vector R i R_idefined in$b$$R_i$, through uniform $The\; properties\; of\; b$ -spline parameterization can be derived:
  $$R_i=\frac{{\mathbf{Q}}_{i+1}-Q_{i-1}}{2\Delta t}$$

• Here $R_i$It is determined in the figure below that { p , v } \{p,v\}{ p,v } The key to right

---

---

After reading this, let’s look at Figure 3 in the paper $3:$
Step 1: Generate a$B-$ spline curve$\Phi$ , relying onAATrajectory Γ Γgenerated by$A\; star\; algorithm$$\Gamma Step$
2: According to the above$R_i$Formula via $The\; control\; points\; of\; the\; B$ -spline curve calculate the vector$R_i$, and then make a perpendicular to $R_i$The plane $\Psi$ , planeΨ$\Psi$ and rely onAATrajectory Γ Γgenerated by$A\; star\; algorithm$$\Gamma$ intersects at the anchor point ($anchor$ $point$）$p$，连接对应的定位点（$anchor$ $point$）$p$ 与控制点 $Q$，才得到直线 $l$，而向量 $v$ 是向量 $l$ 对应的由起点控制点 $Q$ 到终点定位点 $p$ 对应的单位向量。( $v$ 可能是生成以后不会再变化的)。到这里才生成了 { p , v } \{p,v\} { p,v} 对
$Q：$关于 { p , v } \{p,v\} { p,v } In the process, why is the anchor point$p$ locatesAAIs the trajectory generated by the$A-\; star\; algorithm\; close\; to\; the\; obstacle\; rather\; than\; directly\; positioned\; on\; the\; surface\; of\; the\; obstacle?$
$A:$ Neck$The\; trajectory\; generated\; by\; the\; A*$ algorithm must be a safe and collision-free trajectory. Secondly, if it is directly positioned on the surface of the obstacle, because this is a simulation, errors will inevitably occur, making the results inconsistent with the results obtained in the real world. Furthermore, use A*A$The\; computing\; power\; of\; trajectory\; positioning\; generated\; by\; the\; A\ast$ algorithm is lowerthan that of positioning directly on the obstacle surface, so whether it is to leave a stable margin or to reduce the calculation power, you still choose to use$A*$ algorithm for positioning.

---

---

6 Summary

This method still has some flaws, namely $The\; A*$ algorithm searches for local minima introduced and the uniform time redistribution introduces conservative trajectories. Therefore, we will work on performing topology planning to escape local minima and reformulate the problem to generate near-optimal trajectories. The planner is designed for static environments and does not need to deal with slow moving obstacles (below$0.5m/s$ ). In the future, we will study dynamic environment navigation through moving object detection and topology planning.