高飞实验室
src
global_planning.cpp
主函数，是一个 ROS节点，用于实现全局路径规划。
int main(int argc, char **argv)
{ //使用 ros::init 初始化 ROS 节点，并命名为 "global_planning_node"。接着，创建一个 ros::NodeHandle 对象 nh_，用于与 ROS 系统进行通信。
ros::init(argc, argv, "global_planning_node");
ros::NodeHandle nh_;
//创建一个 GlobalPlanner 类的对象 global_planner。GlobalPlanner 类负责处理全局路径规划。构造函数接收两个参数：一个 Config 对象，用于从 ROS 参数服务器获取配置参数；一个 ros::NodeHandle 对象，用于与 ROS 系统进行通信。
GlobalPlanner global_planner(Config(ros::NodeHandle("~")), nh_);
//创建一个 ros::Rate 对象 lr，用于控制循环的频率。这里的频率设置为 1000 Hz。
ros::Rate lr(1000);
while (ros::ok())//ROS 系统正常运行
{
global_planner.process();// 调用 global_planner.process() 方法，处理全局路径规划。
ros::spinOnce();// 调用 ros::spinOnce()，处理 ROS 事件队列，例如接收订阅的消息。
lr.sleep();// 调用 lr.sleep()，根据设置的频率（1000 Hz）暂停一段时间。
}
return 0;
}
GlobalPlanner 类
class GlobalPlanner
{
private://私有属性
Config config;//包含了一些参数，如最大速度、最大倾斜角度、最小推力等，用于规划路径
ros::NodeHandle nh;//用于与ROS系统进行通信
ros::Subscriber mapSub;//订阅地图
ros::Subscriber targetSub;//订阅目标点
bool mapInitialized;//表示地图是否已经初始化
voxel_map::VoxelMap voxelMap;//存储地图信息
Visualizer visualizer;//可视化路径规划结果
std::vector<Eigen::Vector3d> startGoal;//startGoal用于存储起点和终点
Trajectory<5> traj;//存储路径规划结果，5是pieces数
double trajStamp;//记录路径规划的时间戳。
public://公有属性
GlobalPlanner(const Config &conf,
ros::NodeHandle &nh_)
: config(conf),
nh(nh_),
mapInitialized(false),
visualizer(nh)
{ //根据config.mapBound值计算得出xyz 3个整数元素的三维向量，表示voxel Map的尺寸
const Eigen::Vector3i xyz((config.mapBound[1] - config.mapBound[0]) / config.voxelWidth,
(config.mapBound[3] - config.mapBound[2]) / config.voxelWidth,
(config.mapBound[5] - config.mapBound[4]) / config.voxelWidth);
//根据config.mapBound值计算voxel Map的偏移量
const Eigen::Vector3d offset(config.mapBound[0], config.mapBound[2], config.mapBound[4]);
//创建voxelMap对象
voxelMap = voxel_map::VoxelMap(xyz, offset, config.voxelWidth);
//创建mapSub订阅者，订阅话题config.mapTopic，将mapSub对象与mapSub函数绑定
mapSub = nh.subscribe(config.mapTopic, 1, &GlobalPlanner::mapCallBack, this,
ros::TransportHints().tcpNoDelay());
//创建targetSub订阅者，订阅话题config.targetTopic，将targetSub对象与targetSub函数绑定
targetSub = nh.subscribe(config.targetTopic, 1, &GlobalPlanner::targetCallBack, this,
ros::TransportHints().tcpNoDelay());
}
Config结构体：包含了一系列参数，这些参数通过ros::NodeHandle对象getParam()方法从ROS参数服务器中获取的
struct Config
{
std::string mapTopic;//地图话题
std::string targetTopic;//目标话题
double dilateRadius;//膨胀半径
double voxelWidth;//voxel宽度
std::vector<double> mapBound;//地图边界
double timeoutRRT;//超时时间
double maxVelMag;//最大速度
double maxBdrMag;//最大角速度
double maxTiltAngle;//最大倾斜角度
double minThrust;//最小推力
double maxThrust;//最大推力
double vehicleMass;//机身质量
double gravAcc;//重力加速度
double horizDrag;//水平阻力
double vertDrag;//垂直阻力
double parasDrag;//平面阻力
double speedEps;//速度平滑因子
double weightT;//时间权重
std::vector<double> chiVec;//位置速度角速度，倾斜角度和推力的惩罚权重
double smoothingEps;//平滑度
int integralIntervs;//积分间隔
double relCostTol;//优化算法的收敛标准，低于该值时终止优化过程
Config(const ros::NodeHandle &nh_priv)
{
nh_priv.getParam("MapTopic", mapTopic);
nh_priv.getParam("TargetTopic", targetTopic);
nh_priv.getParam("DilateRadius", dilateRadius);
nh_priv.getParam("VoxelWidth", voxelWidth);
nh_priv.getParam("MapBound", mapBound);
nh_priv.getParam("TimeoutRRT", timeoutRRT);
nh_priv.getParam("MaxVelMag", maxVelMag);
nh_priv.getParam("MaxBdrMag", maxBdrMag);
nh_priv.getParam("MaxTiltAngle", maxTiltAngle);
nh_priv.getParam("MinThrust", minThrust);
nh_priv.getParam("MaxThrust", maxThrust);
nh_priv.getParam("VehicleMass", vehicleMass);
nh_priv.getParam("GravAcc", gravAcc);
nh_priv.getParam("HorizDrag", horizDrag);
nh_priv.getParam("VertDrag", vertDrag);
nh_priv.getParam("ParasDrag", parasDrag);
nh_priv.getParam("SpeedEps", speedEps);
nh_priv.getParam("WeightT", weightT);
nh_priv.getParam("ChiVec", chiVec);
nh_priv.getParam("SmoothingEps", smoothingEps);
nh_priv.getParam("IntegralIntervs", integralIntervs);
nh_priv.getParam("RelCostTol", relCostTol);
}
};
process () 根据当前轨迹计算无人机的速度、推力、倾斜角和机体角速度，并将这些值发布到相应的主题上。同时，它还可视化无人机在轨迹上的当前位置。
inline void process()
{ //创建一个名为 physicalParams 的 Eigen::VectorXd 对象，用于存储物理参数，如无人机的质量、重力加速度、水平阻力、垂直阻力、寄生阻力和速度平滑因子。
Eigen::VectorXd physicalParams(6);
physicalParams(0) = config.vehicleMass;
physicalParams(1) = config.gravAcc;
physicalParams(2) = config.horizDrag;
physicalParams(3) = config.vertDrag;
physicalParams(4) = config.parasDrag;
physicalParams(5) = config.speedEps;
//创建一个 flatness::FlatnessMap 对象 flatmap，并使用 physicalParams 中的参数对其进行初始化。
flatness::FlatnessMap flatmap;
flatmap.reset(physicalParams(0), physicalParams(1), physicalParams(2),
physicalParams(3), physicalParams(4), physicalParams(5));
//检查轨迹是否包含至少一个片段。如果是，则计算当前时间与轨迹开始时间的差值 delta。
if (traj.getPieceNum() > 0)
{
const double delta = ros::Time::now().toSec() - trajStamp;
if (delta > 0.0 && delta < traj.getTotalDuration())//如果 delta 在轨迹总持续时间范围T内，执行以下操作：
{ //使用 flatmap.forward() 函数根据轨迹在 delta 点的速度 加速度 jerk来计算推力 (thr)、四元数 (quat) 和角速度 (omg)。
double thr;
Eigen::Vector4d quat;
Eigen::Vector3d omg;
flatmap.forward(traj.getVel(delta),
traj.getAcc(delta),
traj.getJer(delta),
0.0, 0.0,
thr, quat, omg);
//计算速度 (speed)、机体角速度大小 (bodyratemag) 和倾斜角 (tiltangle)。
double speed = traj.getVel(delta).norm();
double bodyratemag = omg.norm();
double tiltangle = acos(1.0 - 2.0 * (quat(1) * quat(1) + quat(2) * quat(2)));
//将这些值分别存储在 std_msgs::Float64 消息中，并通过相应的发布器发布。
std_msgs::Float64 speedMsg, thrMsg, tiltMsg, bdrMsg;
speedMsg.data = speed;
thrMsg.data = thr;
tiltMsg.data = tiltangle;
bdrMsg.data = bodyratemag;
// 使用 visualizer.visualizeSphere() 函数可视化当前位置的球体。
visualizer.speedPub.publish(speedMsg);
visualizer.thrPub.publish(thrMsg);
visualizer.tiltPub.publish(tiltMsg);
visualizer.bdrPub.publish(bdrMsg);
visualizer.visualizeSphere(traj.getPos(delta),
config.dilateRadius);
}
}
}
targetCallBack 是一个内联函数，也是一个回调函数，用于处理 geometry_msgs::PoseStamped 类型的消息。当接收到一个新的目标位置时，该函数会被调用。主要作用是处理新的目标位置消息，检查目标位置是否可行，并将可行的目标位置添加到 startGoal 向量中。然后调用 plan() 函数进行路径规划。
inline void targetCallBack(const geometry_msgs::PoseStamped::ConstPtr &msg)
{
if (mapInitialized)//首先，函数检查地图是否已初始化（mapInitialized）。如果地图尚未初始化，函数将直接返回，不执行任何操作。
{
if (startGoal.size() >= 2)//如果 startGoal 向量的大小大于等于2，那么清空 startGoal 向量。这是为了确保 startGoal 只包含起点和终点。
{
startGoal.clear();
}
//计算目标位置的z坐标（zGoal）。这是通过将输入消息中的 msg->pose.orientation.z 与地图边界和膨胀半径（config.dilateRadius）进行计算得到的。
const double zGoal = config.mapBound[4] + config.dilateRadius +
fabs(msg->pose.orientation.z) *
(config.mapBound[5] - config.mapBound[4] - 2 * config.dilateRadius);
// 创建一个 Eigen::Vector3d 类型的变量 goal，用于存储目标位置的x、y和z坐标。
const Eigen::Vector3d goal(msg->pose.position.x, msg->pose.position.y, zGoal);
if (voxelMap.query(goal) == 0)如果查询结果为0，表示目标位置可行
{
visualizer.visualizeStartGoal(goal, 0.5, startGoal.size());//可视化目标位置
startGoal.emplace_back(goal);//将目标位置添加到 startGoal 向量中
}
else
{
ROS_WARN("Infeasible Position Selected !!!\n");//目标位置不可行，发出警告信息。
}
plan();//调用 plan() 函数进行路径规划
}
return;
}
plan 函数，接收起点、终点、3D体素图进行路径规划。具体地，使用sfc_gen::planPath规划一条原始路径（我猜是那些前端采样或者搜索的算法），使用sfc_gen::convexCover生成一组凸多面体，使用sfc_gen::shortCut简化凸多面体，设置约束参数，使用gcopter::GCOPTER_PolytopeSFC对这些多面体进行优化，使用gcopter.setup设置路径规划的参数和约束条件，最后使用gcopter.optimize(traj, config.relCostTol)进行轨迹优化。
inline void plan()
{
if (startGoal.size() == 2)//当startGoal中包含两个元素时
{
std::vector<Eigen::Vector3d> route; //route变量 3x1 存储planPath生成的路径
//使用sfc_gen库(gcopter\include\gcopter\sfc_gen.hpp)中的planPath()函数规划路径
sfc_gen::planPath<voxel_map::VoxelMap>(startGoal[0],//路径起点
startGoal[1],//路径终点
voxelMap.getOrigin(),
voxelMap.getCorner(),
&voxelMap, 0.01,//voxelMap和分辨率参数
route);
std::vector<Eigen::MatrixX4d> hPolys; //convexCover()函数生成的凸多面体 4X4矩阵 多项式系数
std::vector<Eigen::Vector3d> pc;//getSurf()函数获得voxelMap的表面点
voxelMap.getSurf(pc);
//使用sfc_gen库中的convexCover()函数生成一组凸多面体，这些多面体是通过将路径投影到voxelMap上，然后围绕结果点构建一组凸包来生成的
sfc_gen::convexCover(route,//路径
pc,//voxelMap表面点
voxelMap.getOrigin(),//起点
voxelMap.getCorner(),//终点
7.0,//相邻点之间的最大距离
3.0,//路径和voxelMap表面点之间的最大间距
hPolys);
sfc_gen::shortCut(hPolys);// 对输入的一组凸多面体进行简化。函数内部使用一种启发式算法以减少多面体的数量并提高计算效率。简化后的多面体存储在 hPolys 中，可以用于后续的优化和轨迹规划。
if (route.size() > 1)
{
visualizer.visualizePolytope(hPolys);//可视化多面体
Eigen::Matrix3d iniState;//3X3矩阵
Eigen::Matrix3d finState;//3X3矩阵
//第一行为route的第一个元素，后面两行为零向量；
iniState << route.front(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero();
//第一行为route的最后一个元素，后面两行为零向量。
finState << route.back(), Eigen::Vector3d::Zero(), Eigen::Vector3d::Zero();
//使用GCOPTER库中的GCOPTER_PolytopeSFC类对这些多面体进行优化。优化过程包括设置各种约束参数，如magnitudeBounds（最大速度 | 最大角速度 | 最大倾斜角 | 最小推力 | 最大推力）、penaltyWeights（位置权重 | 速度权重 | 角速度权重 | 倾斜角权重 | 推力权重）和physicalParams（机身质量 | 重力加速度 | 水平阻力系数 | 垂直阻力系数 | 寄生阻力系数 | 速度平滑因子）。
gcopter::GCOPTER_PolytopeSFC gcopter;
// magnitudeBounds = [v_max, omg_max, theta_max, thrust_min, thrust_max]^T
// penaltyWeights = [pos_weight, vel_weight, omg_weight, theta_weight, thrust_weight]^T
// physicalParams = [vehicle_mass, gravitational_acceleration, horitonral_drag_coeff,
// vertical_drag_coeff, parasitic_drag_coeff, speed_smooth_factor]^T
// initialize some constraint parameters
//读取config文件 初始化约束参数
Eigen::VectorXd magnitudeBounds(5);
Eigen::VectorXd penaltyWeights(5);
Eigen::VectorXd physicalParams(6);
magnitudeBounds(0) = config.maxVelMag;
magnitudeBounds(1) = config.maxBdrMag;
magnitudeBounds(2) = config.maxTiltAngle;
magnitudeBounds(3) = config.minThrust;
magnitudeBounds(4) = config.maxThrust;
penaltyWeights(0) = (config.chiVec)[0];
penaltyWeights(1) = (config.chiVec)[1];
penaltyWeights(2) = (config.chiVec)[2];
penaltyWeights(3) = (config.chiVec)[3];
penaltyWeights(4) = (config.chiVec)[4];
physicalParams(0) = config.vehicleMass;
physicalParams(1) = config.gravAcc;
physicalParams(2) = config.horizDrag;
physicalParams(3) = config.vertDrag;
physicalParams(4) = config.parasDrag;
physicalParams(5) = config.speedEps;
const int quadratureRes = config.integralIntervs;//定义一个整型变量quadratureRes，并将其赋值为config.integralIntervs。用于数值积分的分割数，即将积分区间分割成多少个小区间进行数值积分。
traj.clear();//将traj清空以便后续使用
//调用gcopter中的setup函数，用于设置路径规划的参数和约束条件。如果setup函数返回false说明路径规划失败，直接返回
if (!gcopter.setup(config.weightT,//时间权重
iniState, finState,//3X3矩阵 起点和终点的位置速度加速度
hPolys, INFINITY,//4X4矩阵 多项式系数 | 时间上限
config.smoothingEps,//平滑度
quadratureRes,//积分分段数
magnitudeBounds,//约束条件
penaltyWeights,//惩罚权重
physicalParams))//物理参数
{
return;
}
if (std::isinf(gcopter.optimize(traj, config.relCostTol)))//使用GCOPTER_PolytopeSFC类的optimize（）函数执行优化。
{
return;
}
if (traj.getPieceNum() > 0)
{
trajStamp = ros::Time::now().toSec();
visualizer.visualize(traj, route);//如果优化成功，将使用visualizer对象的visualize（）函数对生成的轨迹进行可视化。visualize（）函数接收优化的轨迹和原始路径，并在RViz中对它们进行可视化。
}
}
}
}
mapCallBack 用于处理传感器消息中的点云数据 。该函数将遍历点云数据并将其添加到 voxelMap 中。在遍历过程中，如果点的任何坐标为 NaN 或 Inf，则该点将被忽略。一旦所有点都添加到 voxelMap 中，函数将对其进行膨胀处理，以便在规划路径时考虑无人机的尺寸。
inline void mapCallBack(const sensor_msgs::PointCloud2::ConstPtr &msg)
{
if (!mapInitialized)//如果地图尚未初始化
{
size_t cur = 0; //初始化当前索引为0
const size_t total = msg->data.size() / msg->point_step;//计算点云数据中的点数
float *fdata = (float *)(&msg->data[0]);//将点云数据转换成浮点数数组
for (size_t i = 0; i < total; i++)//对每个点执行以下操作
{
cur = msg->point_step / sizeof(float) * i;//计算当前点索引
//如果当前点的任何坐标为NaN或Inf，则跳过该点
if (std::isnan(fdata[cur + 0]) || std::isinf(fdata[cur + 0]) ||
std::isnan(fdata[cur + 1]) || std::isinf(fdata[cur + 1]) ||
std::isnan(fdata[cur + 2]) || std::isinf(fdata[cur + 2]))
{
continue;
}
//将该点添加到占据栅格地图中
voxelMap.setOccupied(Eigen::Vector3d(fdata[cur + 0],
fdata[cur + 1],
fdata[cur + 2]));
}
//将栅格地图进行膨胀操作，以便在路径规划中考虑无人机的尺寸
voxelMap.dilate(std::ceil(config.dilateRadius / voxelMap.getScale()));
mapInitialized = true;//表示地图已经初始化
}
}
include/gcopter
minco.hpp
BandedSystem 类
class BandedSystem
{ private:
int N;//带状系统矩阵大小
int lowerBw;//较低带宽
int upperBw;//较高带宽
// Compulsory nullptr initialization here
double *ptrData = nullptr;
create：函数采用三个整数参数n、p和q。创建一个具有较低带宽p、较高带宽q和大小n的带状系统矩阵，并将其初始化为全零。
inline void create(const int &n, const int &p, const int &q)
{
// In case of re-creating before destroying，以确保释放之前分配的任何内存。
destroy();
//成员变量， member variables
N = n;
lowerBw = p;
upperBw = q;
int actualSize = N * (lowerBw + upperBw + 1);//矩阵的实际大小计算为 N*（lowerBw+upperBw+1）
ptrData = new double[actualSize];//使用new为其分配内存
std::fill_n(ptrData, actualSize, 0.0);//使用std::fill_n将内存初始化为0
return;
}
destory：是一个成员函数，用于销毁 BandedSystem 类的对象，避免内存泄漏。
inline void destroy()
{
if (ptrData != nullptr)//如果 ptrData 不为 nullptr，则释放内存并将其设置为 nullptr。
{
delete[] ptrData;
ptrData = nullptr;
}
return;
}
reset：将矩阵中所有元素都重置为0
inline void reset(void)
{ //第一个参数是要填充的目标地址，第二个参数是要填充的元素个数，第三个参数是要填充的值=
std::fill_n(ptrData, N * (lowerBw + upperBw + 1), 0.0);
return;
}
实现了一个BandedSystem类，用于高效地解决带状线性方程组Ax=b
A是一个N*N带状矩阵，其下带宽为lowerBw，上带宽为upperBw。该类实现了create、destroy、reset等函数，用于创建、销毁和重置矩阵。此外，该类还实现了()运算符，用于获取矩阵中(i,j)位置的元素。具体实现方式是将矩阵存储在一个一维数组中，按照Matrix Computation中的建议进行存储。
// The band matrix is stored as suggested in "Matrix Computation"
inline const double &operator()(const int &i, const int &j) const
{
return ptrData[(i - j + upperBw) * N + j];
}
inline double &operator()(const int &i, const int &j)
{
return ptrData[(i - j + upperBw) * N + j];
}
factorizeLU ：成员函数，用于对带状矩阵进行LU分解。采用了标准的高斯消元法，但是由于矩阵是带状的，因此只需要对矩阵的带部分进行操作即可，从而提高了计算效率。需要注意的是，该函数没有采用部分主元高斯消元法，因此在某些情况下可能会出现数值不稳定的问题。
具体来说，该函数首先对矩阵进行了初等行变换，将矩阵变为一个上三角矩阵。在这个过程中，对于每一行，只需要对其下方的带部分进行操作即可。然后，函数再次对矩阵进行初等行变换，将矩阵变为一个下三角矩阵。在这个过程中，对于每一列，只需要对其上方的带部分进行操作即可。最终，矩阵被分解为一个下三角矩阵L和一个上三角矩阵U的乘积，即A=LU。
solve：求解带状线性方程组Ax=b，然后将x存储在b中。b的维度是N*m
该函数使用了高斯消元法，首先对矩阵进行了 LU 分解，然后通过前/后代法求解方程组。
具体来说，operator() 用于访问矩阵元素，lowerBw 和 upperBw 分别表示矩阵的下带宽和上带宽，b 是方程组右侧的向量，iM 表示矩阵第 j 列的最后一行。在第一个循环中，对于每一列 j，从第 j+1 行到第 iM 行，如果矩阵元素 operator()(i, j) 不为零，则将 b 的第 i 行减去 operator()(i, j) 乘以 b 的第 j 行。在第二个循环中，从最后一列开始，从第 iM 行到第 j-1 行，如果矩阵元素 operator()(i, j) 不为零，则将 b 的第 i 行减去 operator()(i, j) 乘以 b 的第 j 行。最后，将 b 的每一行除以对应的矩阵对角线元素 operator()(j, j)。
solveAdj：求解带状线性方程组ATx=b，然后将x存储在b中。b的维度是N*m
MINCO_S2NU类 , s=2 non-uniform time
class MINCO_S2NU
{
public:
MINCO_S2NU() = default;
~MINCO_S2NU() { A.destroy(); }//析构函数 调用A的destroy函数释放A的内存
private:
int N;//轨迹中线段的数量
Eigen::Matrix<double, 3, 2> headPV;//3*2矩阵 轨迹的初始位置 速度矩阵
Eigen::Matrix<double, 3, 2> tailPV;//3*2矩阵 轨迹的最终位置 速度矩阵
BandedSystem A;//带状矩阵
Eigen::MatrixX3d b;//3N+8矩阵 存储定义轨迹多项式函数的系统的矩阵
//长度为N的向量 存储轨迹的分段之间的时间间隔
Eigen::VectorXd T1;
Eigen::VectorXd T2;
Eigen::VectorXd T3;
setConditions设置轨迹的初始和最终位置状态，为后续的计算创建必要的对象和变量
inline void setConditions(const Eigen::Matrix<double, 3, 2> &headState,
const Eigen::Matrix<double, 3, 2> &tailState,
const int &pieceNum)
{
N = pieceNum;
headPV = headState;
tailPV = tailState;
A.create(4 * N, 4, 4);//尺寸为4*N的带状矩阵A 上下带宽为4
b.resize(4 * N, 3);//将b的大小调整为4N*3
T1.resize(N);
T2.resize(N);
T3.resize(N);
return;
}
setParameters：设置最小化轨迹优化问题的参数
inline void setParameters(const Eigen::Matrix3Xd &inPs, //3*N的矩阵 表示轨迹中的所有点
const Eigen::VectorXd &ts)//长度为N的向量 表示每个点的时间戳
{
T1 = ts;
T2 = T1.cwiseProduct(T1);
T3 = T2.cwiseProduct(T1);
A.reset();
b.setZero();
A(0, 0) = 1.0;
A(1, 1) = 1.0;
b.row(0) = headPV.col(0).transpose();
b.row(1) = headPV.col(1).transpose();
for (int i = 0; i < N - 1; i++)
{
A(4 * i + 2, 4 * i + 2) = 2.0;
A(4 * i + 2, 4 * i + 3) = 6.0 * T1(i);
A(4 * i + 2, 4 * i + 6) = -2.0;
A(4 * i + 3, 4 * i) = 1.0;
A(4 * i + 3, 4 * i + 1) = T1(i);
A(4 * i + 3, 4 * i + 2) = T2(i);
A(4 * i + 3, 4 * i + 3) = T3(i);
A(4 * i + 4, 4 * i) = 1.0;
A(4 * i + 4, 4 * i + 1) = T1(i);
A(4 * i + 4, 4 * i + 2) = T2(i);
A(4 * i + 4, 4 * i + 3) = T3(i);
A(4 * i + 4, 4 * i + 4) = -1.0;
A(4 * i + 5, 4 * i + 1) = 1.0;
A(4 * i + 5, 4 * i + 2) = 2.0 * T1(i);
A(4 * i + 5, 4 * i + 3) = 3.0 * T2(i);
A(4 * i + 5, 4 * i + 5) = -1.0;
b.row(4 * i + 3) = inPs.col(i).transpose();
}
A(4 * N - 2, 4 * N - 4) = 1.0;
A(4 * N - 2, 4 * N - 3) = T1(N - 1);
A(4 * N - 2, 4 * N - 2) = T2(N - 1);
A(4 * N - 2, 4 * N - 1) = T3(N - 1);
A(4 * N - 1, 4 * N - 3) = 1.0;
A(4 * N - 1, 4 * N - 2) = 2 * T1(N - 1);
A(4 * N - 1, 4 * N - 1) = 3 * T2(N - 1);
b.row(4 * N - 2) = tailPV.col(0).transpose();
b.row(4 * N - 1) = tailPV.col(1).transpose();
A.factorizeLU();
A.solve(b);
return;
}
getTrajectory：将维度为3的轨迹对象作为输入，并返回相同维度的轨迹
inline void getTrajectory(Trajectory<3> &traj) const
{
traj.clear();//清除轨迹对象
traj.reserve(N);//为N个轨迹点保留空间
for (int i = 0; i < N; i++)//在轨迹中的每个点进行迭代
{
traj.emplace_back(T1(i),//根据T1和b矩阵构造轨迹对象，T1是轨迹中每个点之间的时间间隔，b矩阵表示多项式函数的系数
b.block<4, 3>(4 * i, 0)
.transpose()
.rowwise()
.reverse());
}
return;
}
getEnergy：计算由多项式系数b和时间间隔T定义的轨迹的能量。
inline void getEnergy(double &energy) const
{
energy = 0.0;
for (int i = 0; i < N; i++)
{
energy += 4.0 * b.row(4 * i + 2).squaredNorm() * T1(i) +
12.0 * b.row(4 * i + 2).dot(b.row(4 * i + 3)) * T2(i) +
12.0 * b.row(4 * i + 3).squaredNorm() * T3(i);
}
return;
}
getCoeffs：b是一个Eigen::MatrixX3d类型的矩阵，它存储了方程组的系数矩阵。该函数直接返回了b的常量引用，因此可以在外部访问和使用这个系数矩阵，但不能通过它来修改b矩阵中的值。
inline const Eigen::MatrixX3d &getCoeffs(void) const
{
return b;
}
getEnergyPartialGradByCoeffs：计算能量代价函数关于多项式系数的偏导数 记作gdC
inline void getEnergyPartialGradByCoeffs(Eigen::MatrixX3d &gdC) const //接收一个Eigen::MatrixX3d的参数 gdc
{
gdC.resize(4 * N, 3);//将gdc大小设置为4N*3
for (int i = 0; i < N; i++)//遍历每个段
{//具体来说，对于每个段，函数首先计算该段的偏导数向量，其中每个元素都是一个3维向量。这些向量被存储在gdC的第4*i+3行中。计算偏导数向量的公式可以在函数内部找到，其中涉及到该段的系数向量b以及一些预先计算好的常数T1(i)、T2(i)、T3(i)等。接下来，函数计算该段的另一个偏导数向量，存储在gdC的第4*i+2行中。最后，函数将gdC的第4*i和第4*i+1行设置为零向量。
gdC.row(4 * i + 3) = 12.0 * b.row(4 * i + 2) * T2(i) +
24.0 * b.row(4 * i + 3) * T3(i);
gdC.row(4 * i + 2) = 8.0 * b.row(4 * i + 2) * T1(i) +
12.0 * b.row(4 * i + 3) * T2(i);
gdC.block<2, 3>(4 * i, 0).setZero();
}
return;
}
getEnergyPartialGradByTimes：计算能量代价函数关于时间的偏导数 记作gdT
inline void getEnergyPartialGradByTimes(Eigen::VectorXd &gdT) const
{
gdT.resize(N);
for (int i = 0; i < N; i++)
{
gdT(i) = 4.0 * b.row(4 * i + 2).squaredNorm() +
24.0 * b.row(4 * i + 2).dot(b.row(4 * i + 3)) * T1(i) +
36.0 * b.row(4 * i + 3).squaredNorm() * T2(i);
}
return;
}
propogateGrad：计算给定参数集的梯度。该函数不返回任何值，但会修改gradByPoints和gradByTimes参数，这两个参数分别表示相对于控制点和时间的梯度。
inline void propogateGrad(const Eigen::MatrixX3d &partialGradByCoeffs, //关于多项式系数的梯度
const Eigen::VectorXd &partialGradByTimes, //关于时间的梯度
Eigen::Matrix3Xd &gradByPoints, //关于控制点的梯度
Eigen::VectorXd &gradByTimes) //关于时间的梯度
{
gradByPoints.resize(3, N - 1);
gradByTimes.resize(N);
Eigen::MatrixX3d adjGrad = partialGradByCoeffs;//关于多项式系数的梯度的伴随梯度
A.solveAdj(adjGrad);//计算ATx=b的解
for (int i = 0; i < N - 1; i++)
{
gradByPoints.col(i) = adjGrad.row(4 * i + 3).transpose();
}
Eigen::Matrix<double, 4, 3> B1;
Eigen::Matrix<double, 2, 3> B2;
for (int i = 0; i < N - 1; i++)
{
// negative jerk
B1.row(0) = -6.0 * b.row(i * 4 + 3);
// negative velocity
B1.row(1) = -(b.row(i * 4 + 1) +
2.0 * T1(i) * b.row(i * 4 + 2) +
3.0 * T2(i) * b.row(i * 4 + 3));
B1.row(2) = B1.row(1);
// negative acceleration
B1.row(3) = -(2.0 * b.row(i * 4 + 2) +
6.0 * T1(i) * b.row(i * 4 + 3));
gradByTimes(i) = B1.cwiseProduct(adjGrad.block<4, 3>(4 * i + 2, 0)).sum();
}
// negative velocity
B2.row(0) = -(b.row(4 * N - 3) +
2.0 * T1(N - 1) * b.row(4 * N - 2) +
3.0 * T2(N - 1) * b.row(4 * N - 1));
// negative acceleration
B2.row(1) = -(2.0 * b.row(4 * N - 2) +
6.0 * T1(N - 1) * b.row(4 * N - 1));
gradByTimes(N - 1) = B2.cwiseProduct(adjGrad.block<2, 3>(4 * N - 2, 0)).sum();
gradByTimes += partialGradByTimes;
}
trajectory.hpp
Trajectory类
该类表示一个轨迹，其中包含了一系列的Piece对象，每个Piece对象都是一个维度为D的片段。
参数化构造函数Trajectory(const std::vector<double> &durs, const std::vector<typename Piece<D>::CoefficientMat> &cMats)，接受两个参数：durs和cMats，分别表示片段的持续时间和系数矩阵。该构造函数使用输入的持续时间和系数矩阵创建了一系列的Piece对象，并将它们添加到pieces容器中。
inline int getPieceNum() const，返回存储在pieces容器中的片段数量。
需要注意的是，代码中使用了typename关键字来指定Piece<D>::CoefficientMat是一个类型而不是一个成员变量。
总之，这段代码实现了一个轨迹类模板，用于管理和操作一系列维度为D的片段对象。
template <int D>
class Trajectory
{
private:
typedef std::vector<Piece<D>> Pieces;//`typedef`声明将`std::vector<Piece<D>>`命名为`Pieces`，用来表示存储片段的容器类型。
Pieces pieces;//定义了一个名为pieces的private变量，它的类型是Pieces，即std::vector<Piece<D>>
public:
Trajectory() = default;//默认构造函数，使用`default`关键字进行了默认实现。
//参数化构造函数，创建了一系列的`Piece`对象，并将它们添加到`pieces`容器中。
Trajectory(const std::vector<double> &durs,//片段的持续时间
const std::vector<typename Piece<D>::CoefficientMat> &cMats)//片段的系数矩阵
{
int N = std::min(durs.size(), cMats.size());
pieces.reserve(N);
for (int i = 0; i < N; i++)
{
pieces.emplace_back(durs[i], cMats[i]);
}
}
sfc_gen.hpp
planPath函数用于规划从起点到终点的路径，使用指定的映射对象来检查状态的有效性。它返回解决方案路径的成本，并用路径本身填充向量。
该函数接受几个参数，包括起始点s、目标点g、搜索空间lb和hb的上下限、指向映射对象mapPtr的指针、超时值以及对Eigen:：Vector3d对象p的向量的引用。
函数首先使用OMPL库中的RealVectorStateSpace类创建一个指向三维实向量状态空间的共享指针。然后，它使用作为参数提供的下限和上限来设置状态空间的边界。然后使用状态空间创建指向SpaceInformation对象的共享指针，并使用lambda函数设置状态有效性检查器，该函数通过查询相应位置的映射对象来检查给定状态是否有效。
接下来，使用OMPL中的ScopedState类设置开始状态和目标状态，并使用SpaceInformation对象以及开始状态和目的状态创建ProbemDefinition对象。优化目标是使用OMPL中的PathLengthOptimizationObjective类设置的。
然后使用SpaceInformation对象创建指向InformedRRTstar计划器的共享指针，并将ProbemDefinition对象设置为计划器的问题定义。然后设置计划器，并以超时值作为参数调用求解函数。如果找到了解决方案，函数将清除向量p，并用规划器返回的路径填充它。然后计算并返回解决方案路径的成本。
-
convexCover函数用于计算路径上的凸包 -
shortCut函数用于对凸包进行简化 -
这些函数都是通过调用OMPL库中的函数来实现的
1819
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
