快有半年没更博客了。最近加班比较多，而且一直想研究研究ROS那块，但是学到的又不足以写出博客，也就一直没更新。
最近接了一个目标跟踪的二次开发的活，需求就是根据挂载摄像头传回来的与识别的目标的角度值，显示目标的跟踪功能。我看了下，类似与PX4现有的follow_me模式，所以找到了对应的代码学习了一遍。
该功能的代码在src/modules/navigator下面的follow_target.cpp和follow_target.h两个文件。和其它的飞行任务一样，FollowTarget类有on_inactive(),on_activation(),on_active()三个函数。由FollowTarget继承的MissionBlock类的代码可以知道，on_inactive()是在退出该任务时执行的函数，on_activation()是刚切换到该模式时执行的函数，on_active()就是该模式正常运行时执行的函数。
看完了代码，该部门代码主要的功能是计算与要跟随的目标的距离，然后发布目标点经纬度信息。在计算目标位置时，通过目标移动速度等得出一个目标偏差，即一个更加真实的目标位置信息。
void FollowTarget::on_inactive()
{
reset_target_validity();
}
void FollowTarget::on_activation()
{
_follow_offset = _param_nav_ft_dst.get() < 1.0F ? 1.0F : _param_nav_ft_dst.get();
//获取参数_responsiveness，用来修正当前的目标位置
_responsiveness = math::constrain((float) _param_nav_ft_rs.get(), .1F, 1.0F);
_follow_target_position = _param_nav_ft_fs.get();
if ((_follow_target_position > FOLLOW_FROM_LEFT) || (_follow_target_position < FOLLOW_FROM_RIGHT)) {
_follow_target_position = FOLLOW_FROM_BEHIND;
}
//也是一个修正矩阵，用来根据目标移动速度修正目标位置
_rot_matrix = Dcmf(_follow_position_matricies[_follow_target_position]);
}
void FollowTarget::on_active()
{
struct map_projection_reference_s target_ref;
follow_target_s target_motion_with_offset = {};//最终的经过修正的目标位置
uint64_t current_time = hrt_absolute_time();
bool radius_entered = false;
bool radius_exited = false;
bool updated = false;
float dt_ms = 0;
//判断目标位置是否更新
if (_follow_target_sub.updated()) {
updated = true;
follow_target_s target_motion;
_target_updates++;
// save last known motion topic for interpolation later
//将当前的目标位置赋值给前一目标位置
_previous_target_motion = _current_target_motion;
_follow_target_sub.copy(&target_motion);
if (_current_target_motion.timestamp == 0) {
_current_target_motion = target_motion;
}
//得到当前的目标位置
_current_target_motion = target_motion;
//通过初始化时的_responsiveness变量滤波计算得到当前的目标位置
_current_target_motion.lat = (_current_target_motion.lat * (double)_responsiveness) + target_motion.lat * (double)(
1 - _responsiveness);
_current_target_motion.lon = (_current_target_motion.lon * (double)_responsiveness) + target_motion.lon * (double)(
1 - _responsiveness);
} else if (((current_time - _current_target_motion.timestamp) / 1000) > TARGET_TIMEOUT_MS && target_velocity_valid()) {
reset_target_validity();
}
// update distance to target
if (target_position_valid()) {
// get distance to target
//这个两个函数计算出当前位置到目标位置的距离，并赋值给_target_distance变量
map_projection_init(&target_ref, _navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon, &_target_distance(0),
&_target_distance(1));
}
// update target velocity
if (target_velocity_valid() && updated) {
dt_ms = ((_current_target_motion.timestamp - _previous_target_motion.timestamp) / 1000);
// ignore a small dt
if (dt_ms > 10.0F) {
// get last gps known reference for target
//同样的函数，输入是_previous_target_motion和_current_target_motion也就得到了目标的移动距离
map_projection_init(&target_ref, _previous_target_motion.lat, _previous_target_motion.lon);
// calculate distance the target has moved
map_projection_project(&target_ref, _current_target_motion.lat, _current_target_motion.lon,
&(_target_position_delta(0)), &(_target_position_delta(1)));
// update the average velocity of the target based on the position
//如果直接采集到的目标数据里的速度值有效的话，直接采用，否则通过距离初一时间计算出速度值_est_target_vel
if (PX4_ISFINITE(_current_target_motion.vx) && PX4_ISFINITE(_current_target_motion.vy)) {
// No need to estimate target velocity if we can take it from the target
_est_target_vel(0) = _current_target_motion.vx;
_est_target_vel(1) = _current_target_motion.vy;
_est_target_vel(2) = 0.0f;
} else {
_est_target_vel = _target_position_delta / (dt_ms / 1000.0f);
}
// if the target is moving add an offset and rotation
//通过初始化的矩阵_rot_matrix和目标移动速度_est_target_vel修正，得到目标位置偏差_target_position_offset
if (_est_target_vel.length() > .5F) {
_target_position_offset = _rot_matrix * _est_target_vel.normalized() * _follow_offset;
}
// are we within the target acceptance radius?
// give a buffer to exit/enter the radius to give the velocity controller
// a chance to catch up
//判断是否超出了可跟随的范围
radius_exited = ((_target_position_offset + _target_distance).length() > (float) TARGET_ACCEPTANCE_RADIUS_M * 1.5f);
radius_entered = ((_target_position_offset + _target_distance).length() < (float) TARGET_ACCEPTANCE_RADIUS_M);
// to keep the velocity increase/decrease smooth
// calculate how many velocity increments/decrements
// it will take to reach the targets velocity
// with the given amount of steps also add a feed forward input that adjusts the
// velocity as the position gap increases since
// just traveling at the exact velocity of the target will not
// get any closer or farther from the target
//计算步进的速度值，有点像速度的微分
_step_vel = (_est_target_vel - _current_vel) + (_target_position_offset + _target_distance) * FF_K;
_step_vel /= (dt_ms / 1000.0F * (float) INTERPOLATION_PNTS);
_step_time_in_ms = (dt_ms / (float) INTERPOLATION_PNTS);
// if we are less than 1 meter from the target don't worry about trying to yaw
// lock the yaw until we are at a distance that makes sense
//判断距离目标位置的是否大于1
if ((_target_distance).length() > 1.0F) {
// yaw rate smoothing
// this really needs to control the yaw rate directly in the attitude pid controller
// but seems to work ok for now since the yaw rate cannot be controlled directly in auto mode
//输入当前位置和目标位置_current_target_motion计算yaw角
_yaw_angle = get_bearing_to_next_waypoint(_navigator->get_global_position()->lat,
_navigator->get_global_position()->lon,
_current_target_motion.lat,
_current_target_motion.lon);
_yaw_rate = wrap_pi((_yaw_angle - _navigator->get_local_position()->heading) / (dt_ms / 1000.0f));
} else {
_yaw_angle = _yaw_rate = NAN;
}
}
// warnx(" _step_vel x %3.6f y %3.6f cur vel %3.6f %3.6f tar vel %3.6f %3.6f dist = %3.6f (%3.6f) mode = %d yaw rate = %3.6f",
// (double) _step_vel(0),
// (double) _step_vel(1),
// (double) _current_vel(0),
// (double) _current_vel(1),
// (double) _est_target_vel(0),
// (double) _est_target_vel(1),
// (double) (_target_distance).length(),
// (double) (_target_position_offset + _target_distance).length(),
// _follow_target_state,
// (double) _yaw_rate);
}
if (target_position_valid()) {
// get the target position using the calculated offset
//通过目标位置和计算的位置偏差_target_position_offset修正目标位置信息
map_projection_init(&target_ref, _current_target_motion.lat, _current_target_motion.lon);
map_projection_reproject(&target_ref, _target_position_offset(0), _target_position_offset(1),
&target_motion_with_offset.lat, &target_motion_with_offset.lon);
}
// clamp yaw rate smoothing if we are with in
// 3 degrees of facing target
if (PX4_ISFINITE(_yaw_rate)) {
if (fabsf(fabsf(_yaw_angle) - fabsf(_navigator->get_local_position()->heading)) < math::radians(3.0F)) {
_yaw_rate = NAN;
}
}
// update state machine
//下面就是根据_follow_target_state各种不同的情况设置_mission_item的值，让navgator执行，输入都是target_motion_with_offset
switch (_follow_target_state) {
case TRACK_POSITION: {
if (radius_entered) {
_follow_target_state = TRACK_VELOCITY;
} else if (target_velocity_valid()) {
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle);
// keep the current velocity updated with the target velocity for when it's needed
_current_vel = _est_target_vel;
update_position_sp(true, true, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case TRACK_VELOCITY: {
if (radius_exited) {
_follow_target_state = TRACK_POSITION;
} else if (target_velocity_valid()) {
if ((float)(current_time - _last_update_time) / 1000.0f >= _step_time_in_ms) {
_current_vel += _step_vel;
_last_update_time = current_time;
}
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target_motion_with_offset, _yaw_angle);
update_position_sp(true, false, _yaw_rate);
} else {
_follow_target_state = SET_WAIT_FOR_TARGET_POSITION;
}
break;
}
case SET_WAIT_FOR_TARGET_POSITION: {
// Climb to the minimum altitude
// and wait until a position is received
follow_target_s target = {};
// for now set the target at the minimum height above the uav
target.lat = _navigator->get_global_position()->lat;
target.lon = _navigator->get_global_position()->lon;
target.alt = 0.0F;
set_follow_target_item(&_mission_item, _param_nav_min_ft_ht.get(), target, _yaw_angle);
update_position_sp(false, false, _yaw_rate);
_follow_target_state = WAIT_FOR_TARGET_POSITION;
}
/* FALLTHROUGH */
case WAIT_FOR_TARGET_POSITION: {
if (is_mission_item_reached() && target_velocity_valid()) {
_target_position_offset(0) = _follow_offset;
_follow_target_state = TRACK_POSITION;
}
break;
}
}
}总结了下代码的步骤：
1、判断_follow_target_sub是否更新，如果更新：
将将当前目标位置更新为前目标，即：_previous_target_motion = _current_target_motion；
然后用互补滤波，结合_current_target_motion和新的target_motion以及在初始化中获取的比例参数_responsiveness计算出新的_current_target_motion。（这里的_current_target_motion并不是最终的目标位置，还要融合目标速度计算出最后最确切的目标经纬度信息。）
2、判断位置是否有效，如果有效：
计算当前无人机的经纬度到目标点的距离，并赋值给_target_distance
3、判断速度是否有效，如果有效：
(1)计算出上一次目标位置(_previous_target_motion)和当前目标位置(_current_target_motion)的距离(_target_postion_delta)
(2)根据_target_postion_delta和时间间隔dt也就可以计算出目标移动的速度_est_target_vel
(3)根据目标速度以及各种配置的参数，计算目标移动位置的偏差_target_postion_offset
(4)根据目标位置偏差和当前的目标距离_target_distance，判断是否超出了跟随的半径范围
(5)计算出速度的步进值_step_vel
(6)当与目标距离大于1时，根据当前的位置(_navigation->get_global_postion)和当前的目标位置(_current_target_motion)计算出需要的yaw
4、根据目标偏差_target_position_offset计算目标经纬度target_motion_with_offset
5、接下来就是根据目标的经纬度设定需要的目标的位置。
所以要实现自己的跟随功能，主要是计算出与到目标位置之间的距离，然后进行各种转化得出目标位置的经纬度信息。
等我的跟随功能写好了，再贴出相应的代码。
2429
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
