Robust and Efficient Quadrotor Trajectory Generation for Fast Autonomous Flight
在本文中，提出了一种强大且高效的四旋翼运动规划系统，用于在 3 D 3D 3D 复杂环境中进行快速飞行。采用运动学动力学路径搜索方法在离散化控制空间中寻找安全、运动动力学可行且时间最短的初始轨迹。我们通过 B 样条 （ B − S p l i n e ） （B-Spline） （B−Spline）优化提高了轨迹的平滑度和间隙，该优化结合了来自欧几里德距离场 ( E D F ) (EDF) (EDF) 的梯度信息和有效利用 B B B 样条的凸包特性的动态约束。最后，通过将最终轨迹表示为非均匀 B B B 样条，采用迭代时间调整方法来保证动态可行和非保守的轨迹。
ros::init(argc, argv, "fast_planner_node");TopoReplanFSM topo_replan; KinoReplanFSM kino_replan;。#include <ros/ros.h>
#include <visualization_msgs/Marker.h>
#include <plan_manage/kino_replan_fsm.h>
#include <plan_manage/topo_replan_fsm.h>
#include <plan_manage/backward.hpp>
namespace backward {
backward::SignalHandling sh;
}
using namespace fast_planner;
int main(int argc, char** argv) {
ros::init(argc, argv, "fast_planner_node");
ros::NodeHandle nh("~");
int planner;
nh.param("planner_node/planner", planner, -1);
TopoReplanFSM topo_replan;
KinoReplanFSM kino_replan;
if (planner == 1) {
kino_replan.init(nh);
} else if (planner == 2) {
topo_replan.init(nh);
}
ros::Duration(1.0).sleep();
ros::spin();
return 0;
}
for (int i = 0; i < waypoint_num_; i++) {
nh.param("fsm/waypoint" + to_string(i) + "_x", waypoints_[i][0], -1.0);
nh.param("fsm/waypoint" + to_string(i) + "_y", waypoints_[i][1], -1.0);
nh.param("fsm/waypoint" + to_string(i) + "_z", waypoints_[i][2], -1.0);
}
exec_timer_ = nh.createTimer(ros::Duration(0.01), &KinoReplanFSM::execFSMCallback, this);
safety_timer_ = nh.createTimer(ros::Duration(0.05), &KinoReplanFSM::checkCollisionCallback, this);
if (exec_state_ == WAIT_TARGET)
changeFSMExecState(GEN_NEW_TRAJ, "TRIG");
else if (exec_state_ == EXEC_TRAJ)
changeFSMExecState(REPLAN_TRAJ, "TRIG");
使用基于梯度的数值优化来变形轨迹。红色和绿色曲线是初始路径和优化后的 B B B 样条。黄点代表 B B B 样条的控制点。由于忽略了距离信息，初始路径靠近障碍物，而 B B B 样条被基于梯度的优化推开。
B
B
B样条是一个分段多项式，由其次数
p
b
p_b
pb、一组
N
+
1
N+1
N+1 个控制点
{
Q
0
,
Q
1
,
⋯
,
Q
N
}
\left\{\mathbf{Q}_0, \mathbf{Q}_1, \cdots, \mathbf{Q}_N\right\}
{Q0,Q1,⋯,QN}和一个节点向量
[
t
0
,
t
1
,
⋯
,
t
M
]
\left[t_0, t_1, \cdots, t_M\right]
[t0,t1,⋯,tM]唯一确定，其中
Q
i
∈
R
3
,
t
m
∈
R
\mathrm{Q}_i \in \mathbb{R}^3, t_m \in \mathbb{R}
Qi∈R3,tm∈R和
M
=
N
+
p
b
+
1
M=N+p_b+1
M=N+pb+1。
B
B
B 样条轨迹由时间
t
t
t 参数化，其中
t
∈
[
t
p
b
,
t
M
−
p
b
]
t \in\left[t_{p_b}, t_{M-p_b}\right]
t∈[tpb,tM−pb]。对于均匀的
B
B
B 样条，每个节点跨度
Δ
t
m
=
t
m
+
1
−
t
m
\Delta t_m=t_{m+1}-t_m
Δtm=tm+1−tm 具有相同的值
Δ
t
\Delta t
Δt。为了评估 时间
t
∈
[
t
m
,
t
m
+
1
)
⊂
[
t
p
b
,
t
M
−
p
b
]
t \in\left[t_m, t_{m+1}\right) \subset\left[t_{p_b}, t_{M-p_b}\right]
t∈[tm,tm+1)⊂[tpb,tM−pb] 处的位置，我们首先对
t
t
t进行归一化，
s
(
t
)
=
(
t
−
t
m
)
/
Δ
t
s(t)=\left(t-t_m\right) / \Delta t
s(t)=(t−tm)/Δt，然后可以使用矩阵表示评估位置：
p
(
s
(
t
)
)
=
s
(
t
)
⊤
M
p
b
+
1
q
m
s
(
t
)
=
[
1
s
(
t
)
s
2
(
t
)
⋯
s
p
b
(
t
)
]
⊤
q
m
=
[
Q
m
−
p
b
Q
m
−
p
b
+
1
Q
m
−
p
b
+
2
⋯
Q
m
]
⊤
\begin{aligned} &\mathbf{p}(s(t))=\mathbf{s}(t)^{\top} \mathbf{M}_{p_b+1} \mathbf{q}_m \\ &\mathbf{s}(t)=\left[\begin{array}{lllll} 1 & s(t) & s^2(t) & \cdots & s^{p_b}(t) \end{array}\right]^{\top} \\ &\mathbf{q}_m=\left[\begin{array}{lllll} \mathbf{Q}_{m-p_b} & \mathbf{Q}_{m-p_b+1} & \mathbf{Q}_{m-p_b+2} & \cdots & \mathbf{Q}_m \end{array}\right]^{\top} \end{aligned}
p(s(t))=s(t)⊤Mpb+1qms(t)=[1s(t)s2(t)⋯spb(t)]⊤qm=[Qm−pbQm−pb+1Qm−pb+2⋯Qm]⊤
这里 M p b + 1 \mathbf{M}_{p_b+1} Mpb+1 是由 p b p_b pb 确定的常数矩阵。在我们的实现中， p b p_b pb 设置为 3 3 3。导数的评估完全相同，因为 B B B 样条的导数也是 B B B 样条。 B B B 样条的凸包特性对于设计优化公式至关重要，对于确保整个轨迹的动态可行性和安全性非常有用。
尽管在路径搜索和优化中限制了动力学可行性，但有时会得到不可行的轨迹。基本原因是梯度信息倾向于拉长整体轨迹，同时将其推离障碍物较远。因此，四旋翼飞行器必须更积极地飞行，以便在同一时间内行进更长的距离，如果原始运动已经接近物理极限，这不可避免地会导致过度激进的运动。
为了保证动态可行性，采用基于导数控制点与非均匀B样条的时间分配（节点跨度）之间的关系的时间调整方法。由于这些关系，可以通过调整相关的时间分配来改变预期的飞行轨迹攻击性（aggressiveness），因此可以在没有过度保守约束的情况下确保动态可行性。
利用 B B B样条的凸包特性，结合欧几里得距离场的梯度信息和动态约束，采用 B B B样条优化算法提高了轨迹的光滑性和间隙。最后，通过将最终轨迹表示为非均匀 B B B样条，采用迭代时间调整方法保证轨迹的动态可行性和非保守性。