标题：Fast-Tracker: A Robust Aerial System for Tracking Agile
Target in Cluttered Environments
作者：Zhichao Han*, Ruibin Zhang*, Neng Pan*, Chao Xu, and Fei Gao
来源：https://arxiv.org/pdf/2011.03968v1.pdf
代码：https://github.com/ZJU-FAST-Lab/Fast-tracker

文章目录

简介
- 系统框架组成
一、car_planner
- 参数
- 发布与订阅
- 目标点的回调rcvWaypointsCallback（）
- 模型可视化实现vis_carmodel（）
- 发布车辆位置信息pub_carstate（）
二、common_utils
三、tracking_utils
- 1. target_detection
- 2. target_prediction
- 3. grid_path_searcher
- 4. sfc
四、traj_server
五、plan_manage
- 1. EMERGENCY
- 2. RELOCATING
- 3. TRACKING
总结

简介

Fast-Tracker是浙江大学空中机器人实验室提出的一种系统化的解决方案，它可以无人飞行器 (UAV) 在混乱复杂的环境中自主安全地跟踪目标。

Fast-Tracker主要分为两个部分：目标运动预测和轨迹跟踪规划。

目标运动预测：利用目标历史的观测值信息，在考虑目标动态约束的情况下预测目标的未来运动轨迹。

轨迹跟踪规划器：传统的规划结构。前端采用考虑运动学的搜索方法，通过启发式的方法搜索出一条安全的跟踪轨迹。后端优化器将其优化为时空最优且无碰撞的轨迹。

系统框架组成

Fast-Planner开源的工具包主要是包含了五个文件夹，分别是car_planner、common_utils、plan_manage、tracking_utils、traj_server。

car_planner：模拟的是跟踪的目标，跟随着在地图中搜索的运动学上可行的路径移动。
common_utils：包含地图环境的生成、无人机模拟器、rviz插件等。
plan_manage：轨迹跟踪规划器。
tracking_utils：目标检测和运动预测、基于栅格地图的路径搜索。
traj_server：用作轨迹的可视化。

一、car_planner

car_planner包含了map_generator、car_planner和rviz节点，map_generator主要是用于随机生成地图，car_planner包括了汽车.dae模型的加载以及在地图中的路径规划，rviz则用于可视化。

终端上运行

roslaunch car_planner car.launch

节点连接图

参数

    // 车辆的初始位置nh_priv.param("car/init_x",current_state[0],-16.0);nh_priv.param("car/init_y",current_state[1],4.0);nh_priv.param("car/init_yaw",current_state[2],0.0);// 模型文件位置nh_priv.param("car/mesh_resource", mesh_resource, std::string("package://car_planner/param/car.dae"));// 车辆的大小nh_priv.param("car_search/car_l",car_l,0.6);nh_priv.param("car_search/car_w",car_w,0.4);nh_priv.param("car_search/car_h",car_h,0.3);

发布与订阅

    waypoints_sub= nh_priv.subscribe( "waypoints", 1, rcvWaypointsCallback );// 目标点的位置state_timer = nh_priv.createTimer(ros::Duration(0.05),pub_carstate);// 定时发布车辆的位置信息state_update_timer = nh_priv.createTimer(ros::Duration(0.01),state_update);// 定时器viscar_pub =  nh_priv.advertise<visualization_msgs::Marker>("/visualization/car_model", 100,true);// 模型的可视化state_pub = nh_priv.advertise<nav_msgs::Odometry>("car_state",1,true);// 模型的里程计信息

目标点的回调rcvWaypointsCallback（）

void rcvWaypointsCallback(const geometry_msgs::PoseStamped& msg)
{  Eigen::Quaterniond tq; tq.x() = msg.pose.orientation.x;tq.y() = msg.pose.orientation.y;tq.z() = msg.pose.orientation.z;tq.w() = msg.pose.orientation.w;Eigen::Matrix3d trot(tq);double tyaw  = atan2(trot.col(0)[1],trot.col(0)[0]);target_pt<<msg.pose.position.x,msg.pose.position.y,tyaw;ROS_INFO_STREAM("TARGET="<<target_pt);ROS_INFO("[node] receive the planning target");// 通过ompl实现kinosearch->reset();kinosearch->car_search(current_state,target_pt);kinosearch->visualize(0.1);update_flag = true;update_time = ros::Time::now().toSec();
}

模型可视化实现vis_carmodel（）

void vis_carmodel(Eigen::Vector3d cur_state){Eigen::Vector3d pos;Eigen::Matrix3d rota_M;double x = cur_state[0],y=cur_state[1],z=0;double yaw = cur_state[2];rota_M  <<  cos(yaw),-sin(yaw),0,sin(yaw),cos(yaw),0,0       ,0       ,1; pos = Eigen::Vector3d(x,y,z)+rota_M*(Eigen::Vector3d(car_l/2,0,0));visualization_msgs::Marker WpMarker;WpMarker.id = 0;WpMarker.header.stamp = ros::Time::now();WpMarker.header.frame_id = "world";WpMarker.action = visualization_msgs::Marker::ADD;WpMarker.type = visualization_msgs::Marker::MESH_RESOURCE;WpMarker.ns = "car_mesh";WpMarker.mesh_use_embedded_materials = true;WpMarker.color.r = 0.0;WpMarker.color.g = 0.0;WpMarker.color.b = 0.0;WpMarker.color.a = 0.0;WpMarker.scale.x = car_l/4.5;WpMarker.scale.y = car_l/4.5;WpMarker.scale.z = car_l/4.5;Eigen::Matrix3d rot_c,rot_f;rot_c << 0,-1,0, 1,0,0,0,0,1;rot_f = rota_M*rot_c;Eigen::Quaterniond q(rot_f);WpMarker.pose.orientation.w = q.w();WpMarker.pose.orientation.x = q.x();WpMarker.pose.orientation.y = q.y();WpMarker.pose.orientation.z = q.z();WpMarker.pose.position.x = pos[0];WpMarker.pose.position.y = pos[1];WpMarker.pose.position.z = pos[2];WpMarker.mesh_resource = mesh_resource;viscar_pub.publish(WpMarker);
}

发布车辆位置信息pub_carstate（）

void pub_carstate(const ros::TimerEvent& event){// state_pubnav_msgs::Odometry car_odom;car_odom.header.frame_id = "world";car_odom.header.stamp  =ros::Time::now();car_odom.pose.pose.position.x = current_state[0];car_odom.pose.pose.position.y = current_state[1];car_odom.pose.pose.position.z = 0;Eigen::Matrix3d rota_M;double yaw = current_state[2];rota_M  <<  cos(yaw),-sin(yaw),0,sin(yaw),cos(yaw),0,0       ,0       ,1;Eigen::Quaterniond q(rota_M); car_odom.pose.pose.orientation.x = q.x();car_odom.pose.pose.orientation.y = q.y();car_odom.pose.pose.orientation.z = q.z();car_odom.pose.pose.orientation.w = q.w();state_pub.publish(car_odom);
}

car_search.cpp是带运动学约束的A的实现，为的是在地图中找出最优的路径，其中实现的函数较多在这不一一展开，源码中加入了tie breaker，这是对A算法改进，为的是相同F值情况下打破了路径对称性，提高路径的搜索效率。

二、common_utils

common_utils包含了map_generator、plan_env、quadrotor_msgs、rviz_plugins和uav_simulator。主要包括随机生成地图、局部地图生成、无人机的模拟以及rviz显示插件等。

三、tracking_utils

tracking_utils包含grid_path_searcher、sfc、target_detection和target_prediction。

1. target_detection

target_detection该节点是接收地图信息、车辆的姿态信息和无人机的位姿信息，判断目标位置是否在障碍物外，再发布/target话题给/tracking_fsm_node节点。

2. target_prediction

target_prediction实现的是目标的运动轨迹预测。

按照论文所述，目标运动轨迹预测使用的是贝塞尔曲线。

其中每个 $bnib^i_n$ 是一个n阶Bernstein多项式，[ $c_0$ ， $c_1$ ，…， $c_n$ ]是贝塞尔曲线的控制点集合。

实现的过程：将观测到的处于世界坐标下目标的位置和时间戳加入到一个长度为L的FIFO队列。该队列表示为 $Q_{target}$ = [ $q_1$ ， $q_2$ ，…， $q_L$ ]，其中 $q_i$ = { $p_{t_i}$ ， $t_i$ }。 $Q_{target}$ 包含的时间范围为[ $t_1$ ， $t_L$ ]，其中 $t_L$ 表示的是当前时间。当获得新的目标观测值时，拟合过去的观测值来生成新的目标预测轨迹 $B^(t)\hat{B}(t)$ 。预测时间范围为（ $t_L$ ， $t_p$ ]，在该段时间内预测目标运动，所提出的目标运动预测方法如图所示。

过去的观测结果可信度会随着时间的流逝而降低，观测值越靠前，在损失函数中所占权重就越低。我们选择添加权重 $w_{t_i}$ 来区分不同时间的置信度。使用双曲线 $t a n$ $h (x)$ 计算权重：

当 $t_i$ 与当前时间 $t_L$ 之间的时间差增大时，其函数值会迅速减小，从而可以有效地区分不同观测值的置信度。

整体损失函数设计如下：

损失函数包含两部分，第一部分是为了尽可能减少目标轨迹和观测值之间的距离残差，第二部分是添加了二阶正交项以避免过度拟合。 $w_p$ 代表权重值。

为了确保目标预测轨迹的动力学可行性，需要将预测速度和加速度约束在[- $v_{mp}$ ， $v_{mp}$ ]和[- $a_{mp}$ ， $a_{mp}$ ]中。由于贝塞尔曲线´的凸包性和hodograph性，我们只需要在x，y，z之外的一维µ上对每个控制点 $c_i$ 施加以下约束：
其中，n是贝塞尔曲线的阶数，st是时间比例因子。
贝塞尔曲线二范数正定，所以目标预测问题是带约束的QP问题。源码中使用OOQP来构建和解决所提出带约束的贝塞尔回归问题。

源码中判断是否可以求解出预测轨迹

// 输入最大速度、加速度以及存储一定数量的目标行走过的路径点
int Bezierpredict::TrackingGeneration(const double max_vel,const double max_acc,vector<Eigen::Vector4d> predict_list_complete){ros::Time time_1 = ros::Time::now();segs = 1;vector<double>  time_intervals;vector<double>  total_time_intervals;vector<Vector3d> predict_list;vector<double> predict_list_time;vector<double> history_weight_list; history_time_total = 0;int init_flag = 0;for(int i = 0; i < _MAX_SEG; i++){predict_list.push_back(predict_list_complete[i].head(3));if(!init_flag){history_time_init = predict_list_complete[i][3];init_flag = 1;}history_time_total = predict_list_complete[i][3] - history_time_init;predict_list_time.push_back(history_time_total);//ROS_INFO_STREAM("pre_time: " << predict_list_time[i]);}//ROS_INFO_STREAM("tanh weight: " );for(int i = 0; i < _MAX_SEG; i++){double tanh_input = predict_list_time[_MAX_SEG - 1] - predict_list_time[i];if(!tanh_input){history_weight_list.push_back(1);}else{tanh_input = 1.0 / tanh_input;history_weight_list.push_back(tanh(1.2 * tanh_input));//ROS_INFO_STREAM("round: " <<i << " " << tanh(6 * tanh_input));}}for(int i=0;i<segs;i++){//time_intervals.push_back(time(i));time_intervals.push_back(history_time_total); //0.1秒total_time_intervals.push_back(time_intervals[i] + (_TIME_INTERVAL* _PREDICT_SEG));}Vector3d end_p = predict_list[_MAX_SEG - 1];int constrain_flag = 0;// double time_diff = (ros::Time::now().toSec() - history_time_init)- predict_list_time[_MAX_SEG-1];// //ROS_INFO_STREAM("time diff: " << time_diff);// if(time_diff >= 0.25){//     constrain_flag = 1;// }int vars_number = traj_order+1;//6int all_vars_number = 3*vars_number;//XYZ 18int nx = segs*3*vars_number;double c[nx];double  xupp[nx];    char   ixupp[nx];double  xlow[nx];char   ixlow[nx];for(int i=0;i<nx;i++){c[i] = 0.0;xlow[i] = 0.0;ixlow[i] = 0;xupp[i] = 0.0;ixupp[i] = 0;}int my = 3;                             double b[3];                            b[0] = end_p[0];   //end_pb[1] = end_p[1];b[2] = end_p[2];int nnzA = vars_number * 3;             int irowA[nnzA];                       int jcolA[nnzA];double dA[nnzA];int nn_idx = 0;int row_idx = 0;double curr_t = time_intervals[0] ;double total_t =  total_time_intervals[0];for(int i = 0; i < 3; i++){for(int j = 0; j < vars_number; j++){dA[nn_idx] = C_[j] * pow(curr_t/total_t, j) * pow((1 - curr_t/total_t), (traj_order - j));irowA[nn_idx]  = row_idx;jcolA[nn_idx]  = i * vars_number + j;nn_idx ++;}row_idx ++;}int high_order_con_num = 3*(vars_number-1)*segs+3*(vars_number-2)*segs;const int mz  = high_order_con_num;char iclow[mz];char icupp[mz];double clow[mz];double cupp[mz];int m_idx = 0;for(int i = 0; i < 3*(vars_number-1)*segs; i++){iclow[m_idx] = 1;icupp[m_idx] = 1;clow[m_idx]  = -max_vel;cupp[m_idx]  = max_vel;m_idx++;}for(int i=0;i<3*(vars_number-2)*segs;i++){iclow[m_idx]=1;icupp[m_idx]=1;clow[m_idx]=-max_acc;cupp[m_idx]= max_acc;m_idx++;}int nnzC =segs * (vars_number-1)*2 *3 + segs * (vars_number-2)*3 *3;int irowC[nnzC];int jcolC[nnzC];double dC[nnzC];nn_idx  = 0;row_idx = 0;//velocityfor(int k = 0; k < segs ; k ++ ){   for(int i = 0; i < 3; i++){  // for x, y, z loopfor(int j = 0;j < traj_order;j++){dC[nn_idx]     = -1.0 * traj_order/total_time_intervals[k];dC[nn_idx + 1] =  1.0 * traj_order/total_time_intervals[k];irowC[nn_idx]     = row_idx;irowC[nn_idx + 1] = row_idx;jcolC[nn_idx]     = k * all_vars_number + i * vars_number+j;    jcolC[nn_idx + 1] = k * all_vars_number + i * vars_number + j + 1;    row_idx ++;nn_idx += 2;}}}//accelerationfor(int k = 0; k < segs ; k ++ ){   double scale_k = pow(total_time_intervals[k],2);for(int i = 0; i < 3; i++){ for(int j = 0; j < traj_order - 1; j++){    dC[nn_idx]     =  1.0 * traj_order * (traj_order - 1) / scale_k;dC[nn_idx + 1] = -2.0 * traj_order * (traj_order - 1) / scale_k;dC[nn_idx + 2] =  1.0 * traj_order * (traj_order - 1) / scale_k;irowC[nn_idx]     = row_idx;irowC[nn_idx + 1] = row_idx;irowC[nn_idx + 2] = row_idx;jcolC[nn_idx]     = k * all_vars_number + i * vars_number + j;    jcolC[nn_idx + 1] = k * all_vars_number + i * vars_number + j + 1;    jcolC[nn_idx + 2] = k * all_vars_number + i * vars_number + j + 2;    row_idx ++;nn_idx += 3;}}}_Q = MatrixXd::Zero(vars_number * segs * 3, vars_number * segs * 3);_M = MatrixXd::Zero(vars_number * segs * 3, vars_number * segs * 3);for(int i=0; i<segs; i++){for(int j = 0; j < 3; j++){// calculate Matrix Q_Q.block(i*all_vars_number+j*vars_number, i*all_vars_number+j*vars_number, vars_number, vars_number) = getQ(vars_number, total_time_intervals,i);// calculate Matrix M_M.block(i*all_vars_number+j*vars_number, i*all_vars_number+j*vars_number, vars_number, vars_number) = getM(vars_number, total_time_intervals, i);}}MatrixXd Ct = MatrixXd::Zero(1,segs*3*(traj_order+1));MatrixXd distance_Q = MatrixXd::Zero(vars_number * segs * 3, vars_number * segs * 3); double t_base = 0;for(int i= 0;i<segs;i++){if(i>=1)t_base  += time_intervals[i-1]; for(double j = 0;j<_MAX_SEG;j+=1){Ct.block(0,i*3*(traj_order+1),1,3*(traj_order+1)) += getCt(predict_list_time[j], predict_list[j]) * history_weight_list[j];distance_Q.block(all_vars_number*i,all_vars_number*i,all_vars_number,all_vars_number)+= getdistance_Q(predict_list_time[j]) * history_weight_list[j];        }}//ROS_INFO_STREAM("asdf" << Ct);Ct =  Ct*_M;/**///MatrixXd snapQ = _Q;//double sim_weight = 0.0;for(int i = 0; i < nx; i++)c[i] = Ct(0,i);_Q = 2 * (Lambda_ACC * _MAX_SEG * _Q  + distance_Q);//ROS_INFO_STREAM("asdf" << _Q);MatrixXd M_QM;M_QM = MatrixXd::Zero(_M.rows(),_M.cols());//ROS_INFO_STREAM  ("M:rows"<<_M.rows()<<"col"<<_M.cols()<<"Q:rows"<<_Q.rows()<<"col:"<<_Q.cols());M_QM = _M.transpose()*_Q*_M;const int nnzQ = 3 * segs * (traj_order + 1) * (traj_order + 2) / 2; //n(n+1)/2int    irowQ[nnzQ]; int    jcolQ[nnzQ];double    dQ[nnzQ];int sub_shift = 0;int Q_idx = 0;for(int k = 0; k < segs; k ++){for(int p = 0; p < 3; p ++ )for( int i = 0; i < vars_number; i ++ )for( int j = 0; j < vars_number; j ++ )if( i >= j ){irowQ[Q_idx] = sub_shift + p * vars_number + i;   jcolQ[Q_idx] = sub_shift + p * vars_number + j;  dQ[Q_idx] = M_QM(sub_shift + p * vars_number + i,sub_shift + p * vars_number + j);//dQ[Q_idx] = 1;Q_idx ++ ;}sub_shift += all_vars_number;}//my=0;//nnzA=0;QpGenSparseMa27 * qp;QpGenData * prob;QpGenVars      * vars;QpGenResiduals * resid;GondzioSolver  * s;if(constrain_flag){int my = 0;               double *b = 0;          int nnzA = 0;             int *irowA = 0;           int *jcolA = 0;double *dA = 0;int nn_idx  = 0;int row_idx = 0;qp = new QpGenSparseMa27( nx, my, mz, nnzQ, nnzA, nnzC );//cout<<"irowQ: "<<irowQ[nnzQ-1]<<"jcolQ: "<<jcolQ[nnzQ-1];//cout<<"nx: "<<nx<<" my: "<<my<<" mz: "<<mz<<" nnzQ: "<<nnzQ<<" nnzA: "<<nnzA<<" nnzC: "<<nnzC<<" size: "<<M_QM.cols()<<" "<<M_QM.rows();prob = (QpGenData * ) qp->copyDataFromSparseTriple(c,      irowQ,  nnzQ,   jcolQ,  dQ,xlow,   ixlow,  xupp,   ixupp,irowA,  nnzA,   jcolA,  dA,     b,irowC,  nnzC,   jcolC,  dC,clow,   iclow,  cupp,   icupp );vars  = (QpGenVars *) qp->makeVariables( prob );resid = (QpGenResiduals *) qp->makeResiduals( prob );s     = new GondzioSolver( qp, prob );}else{qp = new QpGenSparseMa27( nx, my, mz, nnzQ, nnzA, nnzC );//cout<<"irowQ: "<<irowQ[nnzQ-1]<<"jcolQ: "<<jcolQ[nnzQ-1];//cout<<"nx: "<<nx<<" my: "<<my<<" mz: "<<mz<<" nnzQ: "<<nnzQ<<" nnzA: "<<nnzA<<" nnzC: "<<nnzC<<" size: "<<M_QM.cols()<<" "<<M_QM.rows();prob = (QpGenData * ) qp->copyDataFromSparseTriple(c,      irowQ,  nnzQ,   jcolQ,  dQ,xlow,   ixlow,  xupp,   ixupp,irowA,  nnzA,   jcolA,  dA,     b,irowC,  nnzC,   jcolC,  dC,clow,   iclow,  cupp,   icupp );vars  = (QpGenVars *) qp->makeVariables( prob );resid = (QpGenResiduals *) qp->makeResiduals( prob );s     = new GondzioSolver( qp, prob );}// Turn Off/On the print of the solving process//s->monitorSelf();int ierr = s->solve(prob, vars, resid);if( ierr == 0 ) {double d_var[nx];vars->x->copyIntoArray(d_var);// cout<<"d_var="<<d_var;// int temp_count=0;// for(int kk=0;kk<nx;kk++)// {//     cout<<"d_var="<<d_var[kk];//     temp_count++;//     if(temp_count%10==0)//         cout<<"    count="<<temp_count<<endl;// }// cout.precision(4);// cout << "Solution: \n";// vars->x->writefToStream(cout, "x[%{index}] = %{value}");vars->x->copyIntoArray(d_var);PolyCoeff = MatrixXd::Zero(segs, all_vars_number);PolyTime  = VectorXd::Zero(segs);obj = 0.0;int var_shift = 0;MatrixXd Q_o(vars_number,vars_number);//    int s1d1CtrlP_num = traj_order + 1;//    int s1CtrlP_num   = 3 * s1d1CtrlP_num;//int min_order_l = floor(minimize_order);//int min_order_u = ceil (minimize_order);for(int i = 0; i < segs; i++ ){   PolyTime(i) = total_time_intervals[i];for(int j = 0; j < all_vars_number; j++){PolyCoeff(i , j) = d_var[j + var_shift];// cout<<"coeff in is  "<<PolyCoeff(i , j)<<"i="<<i<<"  j="<<j<<endl;}var_shift += all_vars_number;     } MatrixXd flat_poly = MatrixXd::Zero(1,segs*all_vars_number);for(int i=0;i<segs;i++){flat_poly.block(0,i*all_vars_number,1,all_vars_number) = PolyCoeff.block(i,0,1,all_vars_number);}/*double sum_time;for(int i=0;i<time_intervals.size();i++)sum_time+=time_intervals[i];ROS_INFO_STREAM("CT: "<<Ct*flat_poly.transpose());ROS_INFO_STREAM("distance: "<<flat_poly* distance_Q*flat_poly.transpose());ROS_INFO_STREAM("snap Q: "<<flat_poly*snapQ*flat_poly.transpose());ROS_INFO_STREAM("time : "<<sum_time);*/} else if( ierr == 3)ROS_ERROR("Front Bezier Predict: The program is provably infeasible, check the formulation");else if (ierr == 4)ROS_ERROR("Front Bezier Predict: The program is very slow in convergence, may have numerical issue");elseROS_ERROR("Front Bezier Predict: Solver numerical error");ros::Time time_2 = ros::Time::now();// ROS_INFO_STREAM("Bezier time consumed:" << (time_2 - time_1).toSec()*1000<<" ms");return ierr;
}

3. grid_path_searcher

该节点是基于混合A *算法的运动学搜索方法，通过离散化控制输入来扩展生成节点（运动原语），可以在体素网格图中搜索出安全且动力学可行的轨迹。方法包含邻节点的扩展和启发式函数设计，算法框架如下：

具体的原理解析请参考[论文解读]Fast-Tracker：在复杂环境中敏捷跟踪目标的鲁棒性空中系统

4. sfc

该节点的作用是生成飞行走廊。关于飞行走廊的原理可以参考[运动规划算法]基于飞行走廊的轨迹优化

四、traj_server

主要是一些数据的可视化。

五、plan_manage

Fast-tracker整个运动规划的具体实现，包含了目标运动预测、状态切换的切换等。

订阅与发布

   detect_sub = nh.subscribe("target",1,&plan_manage::tg_list_cb,this);odom_sub = nh.subscribe("odom",1,&plan_manage::odom_cb,this);
//    kino_search_vispub = nh.advertise<visualization_msgs::Marker>("visualization/vis_hybridAstar_traj", 1);traj_vispub = nh.advertise<visualization_msgs::Marker>("visualization/vis_smooth_traj", 1);pretraj_vispub = nh.advertise<visualization_msgs::Marker>("visualization/vis_pre_traj", 1);
//    cor_vispub = nh.advertise<visualization_msgs::Marker>("visualization/vis_corridor",1);TrackTrajPub =  nh.advertise<quadrotor_msgs::PolynomialTrajectory>("trajectory", 50);replan_timer = nh.createTimer(ros::Duration(1/replan_frequency), &plan_manage::fsm_timer_cb, this);

plan_manage包含了三种状态的切换分别为EMERGENCY（紧急情况）、RELOCATING（重定位）和TRACKING（追踪）。

1. EMERGENCY

EMERGENCY：无人机追踪过程中发生碰撞的危险情况时采取的紧急制动措施。relocate_init、init_flag全置为false，无人机的速度和加速度全置为0。

2. RELOCATING

RELOCATING：无人机追踪过程中目标丢失（接收目标话题的时间超时）时采取主动探索策略重新定位目标。在多数情况下，无人机可以快速到达目标最后的观测位置，并沿着目标预测轨迹来重新定位目标，具体步骤如下：
1）首先，通过路径搜索算法来获得起点（无人机的当前位置）与目的地（最后的目标观测值）之间的最短安全路径 $p_{obs}$ 。然后，离散化最后一段的目标预测轨迹。若预测轨迹会撞上障碍物，则利用局部路径搜索算法来生成无碰撞路径 $p_{pre}$ 。将 $p_{obs}$ 和 $p_{pre}$ 进行连接为完整的重定位路径 $p_r$ ；
2）基于 $p_r$ 生成飞行走廊；
3）在飞行走廊内生成时空最优且无碰撞的轨迹作为最终的重新定位轨迹；
4）在重新定位过程中，如果沿着重新定位轨迹观察到新的障碍物，则会生成一条新的安全轨迹。

3. TRACKING

正常情况下对目标进行跟踪。前端的路径搜索采用的是混合A *算法，后端轨迹优化采取的时空轨迹优化生成分段多项式轨迹，参考论文Generating large-scale trajectories efficiently usingdouble descriptions of polynomials，实现源码large_scale_traj_optimizer。

总结

Fast-tracker整体流程总结。

第一步：目标的运动预测。采用贝塞尔曲线来描述目标运动的预测轨迹，维护包含空间位姿和时间、长度为L的FIFO队列，当获得新的目标观测值时，会拟合过去的观测值来生成新的目标预测轨迹。

第二步：轨迹的跟踪规划。前端的路径搜索采用的是混合A *算法，后端轨迹优化采取的时空轨迹优化生成分段多项式轨迹。

第三步：重定位。无人机追踪过程中丢失目标时，通过路径搜索获取当前位置与目标最后观测位置之间的最短路径，然后离散化目标预测轨迹，若预测轨迹与障碍物发生碰撞，则通过局部路径规划生成无碰撞的路径，拼接两段路径，并基于拼接好的路径生成飞行走廊，在飞行走廊内生成时空最优且无碰撞的轨迹作为最终的重定位轨迹，重定位过程中，若沿着重定位轨迹观察到新的障碍物，则会生成一条新的安全轨迹。

[运动规划算法]Fast-tracker分析相关推荐

[运动规划算法]基于飞行走廊的轨迹优化
文章目录简介一.介绍二.原理飞行走廊生成三.项目演示参考简介 Btraj是一个在线的四旋翼无人机的运动规划框架,主要用于未知环境中的自主导航.基于机载状态估计和环境感知,采用基于快速行进 ...
[运动规划算法]基于似然场的快速避障算法
文章目录一.简介二.原理 1. 问题描述 2. 概率模型 3. 局部概率 4. 全局概率 5. 方法实现三.项目演示参考一.简介这是一种在复杂环境中实现快速自主飞行的规划方法.通常,在复杂 ...
Vrep中支持的运动规划算法
以下为vrep支持的运动规划算法(以后遇到一种记录一种): BiTRRT BITstar BKPIECE1 CForest EST FMT KPIECE1 LazyPRM LazyPRMstar L ...
rrt算法流程图_基于RRT的运动规划算法综述
基于 RRT 的运动规划算法综述 1. 介绍在过去的十多年中, 机器人的运动规划问题已经收到了大量的关注, 因为机器人开始成为现代工业和日常生活的重要组成部分. 最早的运动规划的问题只是考虑如何移 ...
【运动规划算法项目实战】如何实现Dubins曲线和Reeds-Shepp曲线（附ROS C++代码）
文章目录前言一.Dubins曲线二.Reeds-Shepp曲线三.应用场景四.代码实现 4.1 Dubins曲线实现 4.2 Reeds-Shepp曲线实现 4.3 RVIZ显示五.总结 ...
[运动规划算法]Dubins曲线和Reeds-Shepp曲线
简介 Dubins曲线是在满足曲率约束和规定的始端和末端的切线方向的条件下,连接两个二维平面(即X-Y平面)的最短路径,并假设车辆行驶的道路只能向前行进.如果车辆也可以在反向行驶,则路径为Reeds– ...
【运动规划算法项目实战】如何使用分离轴定理算法实现碰撞检测（附ROS C++代码）
文章目录前言一.分离轴定理简介二. 碰撞检测流程三.代码实现 3.1 计算物体的顶点坐标 3.2 计算出物体的所有边 3.3 检测两个三维物体是否发生碰撞 3.4 完整代码 3.5 RVIZ显 ...
【动态规划】机器人路径规划——算法设计与分析慕课作业
题目内容: 一个机器人只能向下和向右移动,每次只能移动一步,设计一个算法求机器人从(1,1)到(m,n)有多少条路径. 输入格式: 以空格分开m,n 输出格式: 路径条数输入样例: 4 5 输出样例 ...
【运动规划算法项目实战】路径规划中常用的插值方法（附ROS C++代码）
文章目录简介一.线性插值代码实现二.三次样条插值三.B样条插值四.贝塞尔曲线插值总结简介常见用于处理路径平滑的插值算法主要包括线性插值.三次样条插值.B样条插值和贝塞尔曲线插值等,下 ...

[运动规划算法]Fast-tracker分析