Apollo感知系列干货分享.doc
《Apollo感知系列干货分享.doc》由会员分享,可在线阅读,更多相关《Apollo感知系列干货分享.doc(7页珍藏版)》请在三一文库上搜索。
1、Apollo感知系列干货分享任何一个系统的感知算法里,仅仅有* tracks_predict, const double predicted states std:vector for (int i = 0; i * tracks_predict, std:vector* new_objects, const std:vector for (size_t i = 0; i UpdateWithObject( 从上述的代码可以看到,更新过程有ObjectTrack:UpdateWithObject和ObjectTrack:UpdateWithoutObject两个函数完成,这两个函数间接调用ka
2、lman滤波器完成滤波更新,接下去我们简单地分析ObjectTrack:UpdateWithObject函数的流程。1234567891011121314151617181920212223/ file in apollo/modules/perception/obstacle/lidar/tracker/hm_tracker/object_track.cc void ObjectTrack:UpdateWithObject(TrackedObjectPtr* new_object, const double filter_-GetState( filter_-GetOnlineCovaria
3、nce( (*new_object)-anchor_point = belief_anchor_point_; (*new_object)-velocity = belief_velocity_; (*new_object)-velocity_uncertainty = belief_velocity_uncertainty_; belief_velocity_accelaration_ = (*new_object)-velocity - current_object_-velocity) / time_diff; / A.2 update track info . / B. smooth
4、object track / B.1 smooth velocity SmoothTrackVelocity(*new_object), time_diff); / B.2 smooth orientation SmoothTrackOrientation();从代码中也可以间接看出更新的过程A.1和A.2是更新KalmanFilter和TrackedObject状态信息,B是更新ObjectTrack状态,这里必须按顺序来更新!主要由KalmanFilter:UpdateWithObject函数完成,计算过成分下面几步:Step1. 计算更新评分ComputeUpdateQuality(ne
5、w_object, old_object)这个过程主要是计算更新力度,因为每个Object和对应的跟踪目标TrackedObject之间有一个关联系数association_score,这个系数衡量两个目标之间的相似度,所以这里需要增加对目标的更新力度参数。计算关联力度:update_quality_according_association_score = 1 - association_score / s_association_score_maximum_。默认s_association_score_maximum_= 1,关联越大(相似度越大),更新力度越大计算点云变化力度:updat
6、e_quality_according_point_num_change = 1 - |n1 - n2| / max(n1, n2)。点云变化越小,更新力度越大最终取两个值的较小值最为最终的更新力度。Step2.计算当前时刻的速度measured_velocity和加速度measured_acceleration(这两个变量相当于卡尔曼滤波中的观测变量$Z_t$)首先计算重心速度:measured_anchor_point_velocity = NewObject_barycenter(x,y,z) - OldObject_barycenter(x,y,z) / timediff。timedi
7、ff是两次计算的时间差,很简单地计算方式其次计算标定框(中心)速度:measured_bbox_center_velocity = NewObject_center(x,y,z) - OldObject_center(x,y,z) / timediff。这里的中心区别于上面的重心,重心是所有点云的平均值;重心是MinBox的中心值。还有一个需要注意的是,如果求出来的中心速度方向和重心方向相反,这时候有干扰,中心速度暂时置为0。然后计算标定框角点速度:A. 根据NewObject的点云计算bbox(这不是MinBox),并求出中心center,然后根据反向求出4个点。如果NewObject方向是
8、dir,那么首先对dir进行归一化得到dir_normal=dir/|dir|=(nx,ny,0),然后求他的正交方向dir_ortho=(-ny,nx,0),如果中心点坐标center,那么左上角的坐标就是: center+dir*size0*0.5+ortho_dir*size1*0.5。根据这个公式可以计算出其他三个点的坐标。B. 计算标定框bbox四个角点的速度:bbox_corner_velocity = (new_bbox_corner - old_bbox_corner) / time_diff)公式与上面的重心、中心计算方式一样。C. 计算4个角点的在主方向上的速度,去最小的点
9、最为标定框角点速度。只需要将B中的bbox_corner_velocity投影到主方向即可。最后在重心速度、重心速度、bbox角速度中选择速度增益最小的最后最终物体的增益。增益=当前速度-上时刻最优速度加速度measured_acceleration计算比较简单,采用最近3次的速度(v1,t1),(v2,t2),(v3,t3),然后加速度a=(v3-v1)_c = Eigen:Matrix3d:Identity(); / C_tEigen:Matrix3d mat_q = s_measurement_noise_ * Eigen:Matrix3d:Identity(); / R_tEigen:
10、Matrix3d mat_k = velocity_covariance_ * mat_c.transpose() * / H_t (mat_c * velocity_covariance_ * mat_c.transpose() + mat_q).inverse();从上面可知,代码和我们给出的结果是一致的。然后,由当前时刻的估算速度$X_t,t-1$、观测变量$Z_t$以及卡尔曼增益$H_t$,得到当前时刻的最优速度估计$X_t = X_t,t-1 + H_tZ_t - C_tX_t,t-1$,在apollo代码中计算了速度增益,也就是$X_t-X_t,t-1$:1234/ Compute
11、 posterior beliefEigen:Vector3d measured_velocity_d = measured_velocity.cast(); / Zv_tEigen:Vector3d priori_velocity = belief_velocity_ + belief_acceleration_gain_ * time_diff; / Xv_t,t-1Eigen:Vector3d velocity_gain = mat_k * (measured_velocity_d - mat_c * priori_velocity); / Gain = Xv_t - Xv_t,t-1然
12、后对速度增益进行平滑并且保存当前t时刻最优速度以及最优加速度。12345678910/ BreakdownComputeBreakdownThreshold();if (velocity_gain.norm() breakdown_threshold_) velocity_gain.normalize(); velocity_gain *= breakdown_threshold_;belief_anchor_point_ = measured_anchor_point_d;belief_velocity_ = priori_velocity + velocity_gain; / Xv_t =
- 配套讲稿:
如PPT文件的首页显示word图标,表示该PPT已包含配套word讲稿。双击word图标可打开word文档。
- 特殊限制:
部分文档作品中含有的国旗、国徽等图片,仅作为作品整体效果示例展示,禁止商用。设计者仅对作品中独创性部分享有著作权。
- 关 键 词:
- Apollo 感知 系列 干货 分享
链接地址:https://www.31doc.com/p-3249591.html