Ardupilot如何实现避障代码分析
目录
-
-
- 目录
- 摘要
-
- 1.注册避障模块实现初始化
- 2.更新避障数据
- 3.避障控制
摘要
本节主要记录自己学ardupilot的避障代码过程，首先看下怎么注册避障传感器，然后怎么获取避障数据，最后怎么实现避障控制的过程。
主要围绕以下几点进行学习：
- 怎么初始化避障模块？
- 怎么获取避障距离数据？
- 避障原理理解
- 避障调用程序
- 怎么实现避障控制
1.注册避障模块实现初始化
(1)避障初始化函数执行顺序
void Copter::setup()
{
cliSerial = hal.console;
//加载初始化参数表到 in var_info[]s
AP_Param::setup_sketch_defaults();
// 初始化无人机的结构布局
StorageManager::set_layout_copter();
//注册传感器
init_ardupilot();
// 初始化 main loop 任务
scheduler.init(&scheduler_tasks[0], ARRAY_SIZE(scheduler_tasks));
// 初始化 main loop 任务
perf_info_reset();
fast_loopTimer = AP_HAL::micros();
}
——————————————————————————————————————
void Copter::init_ardupilot()
{
init_proximity();//避障函数初始化
}
——————————————————————————————————————
void Copter::init_proximity(void)
{
#if PROXIMITY_ENABLED == ENABLED
g2.proximity.init();//测距传感器初始化
g2.proximity.set_rangefinder(&rangefinder);//设置近距传感器
#endif
}
——————————————————————————————————————
(2)避障初始化函数执行顺序
------------------------------------------------------------
首先看下 g2.proximity.init()函数;
------------------------------------------------------------
void AP_Proximity::init(void)
{
if (num_instances != 0) //没有传感器就直接返回
{
//初始化号召等待2s-------- init called a 2nd time?
return;
}
for (uint8_t i=0; i//传感器识别
if (drivers[i] != nullptr) //我们为这个实例加载了一个驱动程序，所以它必须存在（尽管它可能不健康）
{
// we loaded a driver for this instance, so it must be present (although it may not be healthy)
num_instances = i+1;
}
//初始化状态标志-----initialise status
state[i].status = Proximity_NotConnected;
}
}
------------------------------------------------------------
继续看下识别我们用的哪种传感器 detect_instance(i);
------------------------------------------------------------
void AP_Proximity::detect_instance(uint8_t instance)
{
uint8_t type = _type[instance];
if (type == Proximity_Type_SF40C) //激光测距模块
{
if (AP_Proximity_LightWareSF40C::detect(serial_manager))
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_LightWareSF40C(*this, state[instance], serial_manager);
return;
}
}
if (type == Proximity_Type_MAV)
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_MAV(*this, state[instance]);
return;
}
if (type == Proximity_Type_TRTOWER)
{
if (AP_Proximity_TeraRangerTower::detect(serial_manager))
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_TeraRangerTower(*this, state[instance], serial_manager);
return;
}
}
if (type == Proximity_Type_RangeFinder) //测距仪器
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);
return;
}
#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
if (type == Proximity_Type_SITL)
{
state[instance].instance = instance;
drivers[instance] = new AP_Proximity_SITL(*this, state[instance]); //创建实例
return;
}
#endif
}
///////////////////////////////////////////////
可以看到这里主要是选择哪种类型的避障模块
/////////////////////////////////////////////// 1》这里看下支持的驱动类型：
2》假如我们使用的是Proximity_Type_RangeFinder，则要创建对象
drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);
3》用到的参数
// table of user settable parameters
const AP_Param::GroupInfo AP_Proximity::var_info[] = {
// 0 is reserved for possible addition of an ENABLED parameter
// @Param: _TYPE
// @DisplayName: Proximity type
// @Description: What type of proximity sensor is connected
// @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder
// @User: Standard
AP_GROUPINFO("_TYPE", 1, AP_Proximity, _type[0], 0),
// @Param: _ORIENT
// @DisplayName: Proximity sensor orientation
// @Description: Proximity sensor orientation
// @Values: 0:Default,1:Upside Down
// @User: Standard
AP_GROUPINFO("_ORIENT", 2, AP_Proximity, _orientation[0], 0),
// @Param: _YAW_CORR
// @DisplayName: Proximity sensor yaw correction
// @Description: Proximity sensor yaw correction
// @Units: degrees
// @Range: -180 180
// @User: Standard
AP_GROUPINFO("_YAW_CORR", 3, AP_Proximity, _yaw_correction[0], PROXIMITY_YAW_CORRECTION_DEFAULT),
// @Param: _IGN_ANG1
// @DisplayName: Proximity sensor ignore angle 1
// @Description: Proximity sensor ignore angle 1
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG1", 4, AP_Proximity, _ignore_angle_deg[0], 0),
// @Param: _IGN_WID1
// @DisplayName: Proximity sensor ignore width 1
// @Description: Proximity sensor ignore width 1
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID1", 5, AP_Proximity, _ignore_width_deg[0], 0),
// @Param: _IGN_ANG2
// @DisplayName: Proximity sensor ignore angle 2
// @Description: Proximity sensor ignore angle 2
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG2", 6, AP_Proximity, _ignore_angle_deg[1], 0),
// @Param: _IGN_WID2
// @DisplayName: Proximity sensor ignore width 2
// @Description: Proximity sensor ignore width 2
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID2", 7, AP_Proximity, _ignore_width_deg[1], 0),
// @Param: _IGN_ANG3
// @DisplayName: Proximity sensor ignore angle 3
// @Description: Proximity sensor ignore angle 3
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG3", 8, AP_Proximity, _ignore_angle_deg[2], 0),
// @Param: _IGN_WID3
// @DisplayName: Proximity sensor ignore width 3
// @Description: Proximity sensor ignore width 3
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID3", 9, AP_Proximity, _ignore_width_deg[2], 0),
// @Param: _IGN_ANG4
// @DisplayName: Proximity sensor ignore angle 4
// @Description: Proximity sensor ignore angle 4
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG4", 10, AP_Proximity, _ignore_angle_deg[3], 0),
// @Param: _IGN_WID4
// @DisplayName: Proximity sensor ignore width 4
// @Description: Proximity sensor ignore width 4
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID4", 11, AP_Proximity, _ignore_width_deg[3], 0),
// @Param: _IGN_ANG5
// @DisplayName: Proximity sensor ignore angle 5
// @Description: Proximity sensor ignore angle 5
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG5", 12, AP_Proximity, _ignore_angle_deg[4], 0),
// @Param: _IGN_WID5
// @DisplayName: Proximity sensor ignore width 5
// @Description: Proximity sensor ignore width 5
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID5", 13, AP_Proximity, _ignore_width_deg[4], 0),
// @Param: _IGN_ANG6
// @DisplayName: Proximity sensor ignore angle 6
// @Description: Proximity sensor ignore angle 6
// @Units: degrees
// @Range: 0 360
// @User: Standard
AP_GROUPINFO("_IGN_ANG6", 14, AP_Proximity, _ignore_angle_deg[5], 0),
// @Param: _IGN_WID6
// @DisplayName: Proximity sensor ignore width 6
// @Description: Proximity sensor ignore width 6
// @Units: degrees
// @Range: 0 45
// @User: Standard
AP_GROUPINFO("_IGN_WID6", 15, AP_Proximity, _ignore_width_deg[5], 0),
#if PROXIMITY_MAX_INSTANCES > 1
// @Param: 2_TYPE
// @DisplayName: Second Proximity type
// @Description: What type of proximity sensor is connected
// @Values: 0:None,1:LightWareSF40C,2:MAVLink,3:TeraRangerTower,4:RangeFinder
// @User: Advanced
AP_GROUPINFO("2_TYPE", 16, AP_Proximity, _type[1], 0),
// @Param: 2_ORIENT
// @DisplayName: Second Proximity sensor orientation
// @Description: Second Proximity sensor orientation
// @Values: 0:Default,1:Upside Down
// @User: Standard
AP_GROUPINFO("2_ORIENT", 17, AP_Proximity, _orientation[1], 0),
// @Param: 2_YAW_CORR
// @DisplayName: Second Proximity sensor yaw correction
// @Description: Second Proximity sensor yaw correction
// @Units: degrees
// @Range: -180 180
// @User: Standard
AP_GROUPINFO("2_YAW_CORR", 18, AP_Proximity, _yaw_correction[1], PROXIMITY_YAW_CORRECTION_DEFAULT),
#endif
AP_GROUPEND
};5》在missionplanner中显示参数，
------------------------------------------------------------
其次看下 void set_rangefinder(const RangeFinder *rangefinder) 函数;
------------------------------------------------------------
// RangeFinder rangefinder {serial_manager, ROTATION_PITCH_270}; //测距使用
void set_rangefinder(const RangeFinder *rangefinder)
{
_rangefinder = rangefinder;
}
2.更新避障数据
SCHED_TASK(update_proximity, 100, 50), //更新近距离传感器，避障使用1》分析代码：void Copter::update_proximity(void)
void Copter::update_proximity(void)
{
#if PROXIMITY_ENABLED == ENABLED
g2.proximity.update();
#endif
}2》分析代码：g2.proximity.update();
void AP_Proximity::update(void)
{
for (uint8_t i=0; iif (drivers[i] != nullptr)
{
if (_type[i] == Proximity_Type_None)
{
//允许用户在运行时禁用接近传感器----- allow user to disable a proximity sensor at runtime
state[i].status = Proximity_NotConnected;
continue;
}
drivers[i]->update();//更新数据
}
}
//编制主实例-第一传感器返回良好数据---------work out primary instance - first sensor returning good data
for (int8_t i=num_instances-1; i>=0; i--)
{
if (drivers[i] != nullptr && (state[i].status == Proximity_Good))
{
primary_instance = i;
}
}
} 3》分析代码：drivers[i]->update();//更新数据
//这里由于 virtual void update() = 0;是纯虚函数，我们以AP_Proximity_RangeFinder为例进行讲解
void AP_Proximity_RangeFinder::update(void)
{
//如果没有测距仪目标立即退出-------------exit immediately if no rangefinder object
const RangeFinder *rngfnd = frontend.get_rangefinder();
if (rngfnd == nullptr)
{
set_status(AP_Proximity::Proximity_NoData);
return;
}
//查看所有测距仪----------------------look through all rangefinders
for (uint8_t i=0; inum_sensors(); i++)
{
if (rngfnd->has_data(i))
{
//检查水平测距仪------------check for horizontal range finders
if (rngfnd->get_orientation(i) <= ROTATION_YAW_315)
{
uint8_t sector = (uint8_t)rngfnd->get_orientation(i); //获取方向
_angle[sector] = sector * 45; //获取角度
_distance[sector] = rngfnd->distance_cm(i) / 100.0f; //获取距离
_distance_min = rngfnd->min_distance_cm(i) / 100.0f; //最小距离
_distance_max = rngfnd->max_distance_cm(i) / 100.0f; //最大距离
_distance_valid[sector] = (_distance[sector] >= _distance_min) && (_distance[sector] <= _distance_max); //是否在限制的范围
_last_update_ms = AP_HAL::millis(); //获取上次时间
update_boundary_for_sector(sector);//这里是更新扇区的介绍，后面会重点讲解
}
//检查向上测距仪----------check upward facing range finder
if (rngfnd->get_orientation(i) == ROTATION_PITCH_90)
{
_distance_upward = rngfnd->distance_cm(i) / 100.0f;
_last_upward_update_ms = AP_HAL::millis();
}
}
}
//检查超时并设置健康状态--------- check for timeout and set health status
if ((_last_update_ms == 0) || (AP_HAL::millis() - _last_update_ms > PROXIMITY_RANGEFIDER_TIMEOUT_MS))
{
set_status(AP_Proximity::Proximity_NoData);
} else
{
set_status(AP_Proximity::Proximity_Good);
}
}
备注：这里要注意的是：这些值应该传到避障控制函数中，怎么传过去的。我们记住这个函数：update_boundary_for_sector(sector)
3.避障控制
@调用避障的地方主要是高度控制和悬停控制处
@主要来自AC_Avoidance库：
1》定点代码避障分析
(1)wp_nav->update_loiter(ekfGndSpdLimit, ekfNavVelGainScaler);
(2)calc_loiter_desired_velocity(dt,ekfGndSpdLimit); //获取目标速度，作为反馈信息_vel_desired.x，_vel_desired.y
在（2）中找到下面这个函数
if (_avoid != nullptr)
{
_avoid->adjust_velocity(_pos_control.get_pos_xy_kP(),_loiter_accel_cmss, desired_vel);
}（3）分析代码adjust_velocity（）
void AC_Avoid::adjust_velocity(float kP, float accel_cmss, Vector2f &desired_vel)
{
//没有使能立即退出---exit immediately if disabled
if (_enabled == AC_AVOID_DISABLED)
{
return;
}
//限制加速度------limit acceleration
float accel_cmss_limited = MIN(accel_cmss, AC_AVOID_ACCEL_CMSS_MAX);
if ((_enabled & AC_AVOID_STOP_AT_FENCE) > 0) //这个是围栏设置，可以先不看
{
adjust_velocity_circle_fence(kP, accel_cmss_limited, desired_vel);
adjust_velocity_polygon_fence(kP, accel_cmss_limited, desired_vel);
}
if ((_enabled & AC_AVOID_USE_PROXIMITY_SENSOR) > 0 && _proximity_enabled) //采用近距传感器，重点看这里
{
adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel); //根据近距离传感器调节速度
}
}（4）分析代码adjust_velocity_proximity(kP, accel_cmss_limited, desired_vel)
void AC_Avoid::adjust_velocity_proximity(float kP, float accel_cmss, Vector2f &desired_vel)
{
// exit immediately if proximity sensor is not present
if (_proximity.get_status() != AP_Proximity::Proximity_Good)
{
return;
}
//如果没有目标速度，立即退出-------exit immediately if no desired velocity
if (desired_vel.is_zero())
{
return;
}
//从近距传感器获取边界-----------get boundary from proximity sensor
uint16_t num_points;
const Vector2f *boundary = _proximity.get_boundary_points(num_points);//计算边界点
adjust_velocity_polygon(kP, accel_cmss, desired_vel, boundary, num_points, false, _margin); //通过避障点计算所需要的目标速度。
}——————————————————————————————————————————————————————————————————代码分析：如何计算避障边界点？
const Vector2f *boundary = _proximity.get_boundary_points(num_points);//获取避障边界点，由于 const AP_Proximity& _proximity;
const Vector2f* AP_Proximity::get_boundary_points(uint16_t& num_points) const
{
return get_boundary_points(primary_instance, num_points);
}const Vector2f* AP_Proximity::get_boundary_points(uint8_t instance, uint16_t& num_points) const
{
if ((drivers[instance] == nullptr) || (_type[instance] == Proximity_Type_None))
{
num_points = 0;
return nullptr;
}
//从后端获取边界点-------------get boundary from backend
return drivers[instance]->get_boundary_points(num_points); //
}因此最终获取边界点的函数是：drivers[instance]->get_boundary_points(num_points);
const Vector2f* AP_Proximity_Backend::get_boundary_points(uint16_t& num_points) const
{
//高水平状态检查----------------------------------high-level status check
if (state.status != AP_Proximity::Proximity_Good)
{
num_points = 0;
return nullptr;
}
//检查至少一个扇区是否有有效数据，如果没有，退出--------check at least one sector has valid data, if not, exit
bool some_valid = false;
for (uint8_t i=0; i<_num_sectors; i++)
{
if (_distance_valid[i])
{
some_valid = true;
break;
}
}
if (!some_valid)
{
num_points = 0;
return nullptr;
}
// return boundary points
num_points = _num_sectors;
return _boundary_point;
}
最终就是获取_boundary_point
这个值怎么计算，我们全局查找，可以看到哪里有调用：
update_boundary_for_sector(sector); //更新边界区域,这个函数我们突然想起是来自void AP_Proximity_RangeFinder::update(void)也就是更新避障数据的函数，对应第二部分内容，到此可以看出更新数据与数据被使用m已经联系起来。
我们只关心这个函数：update_boundary_for_sector(sector); //更新边界区域
void AP_Proximity_Backend::update_boundary_for_sector(uint8_t sector)
{
//区域合理检查-----sanity check
if (sector >= _num_sectors) //大于8个扇区就直接返回
{
return;
}
//查找相邻扇区（顺时针）------- find adjacent sector (clockwise)
uint8_t next_sector = sector + 1;
if (next_sector >= _num_sectors)//最多8个扇区，超过立即设置下一个扇区是0
{
next_sector = 0;
}
//边界点位于两个扇区之间的界线上，在两个扇区中发现的距离较短。
//boundary point lies on the line between the two sectors at the shorter distance found in the two sectors
float shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT; //没有扇区默认就是100m
//判断有哪些扇区是有数据的
if (_distance_valid[sector] && _distance_valid[next_sector]) //邻居扇区比较，获取最小距离
{
shortest_distance = MIN(_distance[sector], _distance[next_sector]);
} else if (_distance_valid[sector]) //这是只有一个扇区的地方
{
shortest_distance = _distance[sector]; //我们只使用这个扇区，所以如果正前方避障时，只有第一扇区采集避障最短距离
} else if (_distance_valid[next_sector])
{
shortest_distance = _distance[next_sector];
}
if (shortest_distance < PROXIMITY_BOUNDARY_DIST_MIN) //扇区的最小距离不能小于0.6m
{
shortest_distance = PROXIMITY_BOUNDARY_DIST_MIN;
}
_boundary_point[sector] = _sector_edge_vector[sector] * shortest_distance; //向量用于每个扇区的右边缘，用于加速边界的计算
// if the next sector (clockwise) has an invalid distance, set boundary to create a cup like boundary
//如果下一扇区（顺时针）有无效距离，设置边界以创建杯形边界。
if (!_distance_valid[next_sector])
{
_boundary_point[next_sector] = _sector_edge_vector[next_sector] * shortest_distance;
}
//重复扇区和先前扇区之间的边缘-------repeat for edge between sector and previous sector
uint8_t prev_sector = (sector == 0) ? _num_sectors-1 : sector-1;
shortest_distance = PROXIMITY_BOUNDARY_DIST_DEFAULT;
if (_distance_valid[prev_sector] && _distance_valid[sector])
{
shortest_distance = MIN(_distance[prev_sector], _distance[sector]);
} else if (_distance_valid[prev_sector])
{
shortest_distance = _distance[prev_sector];
} else if (_distance_valid[sector])
{
shortest_distance = _distance[sector];
}
_boundary_point[prev_sector] = _sector_edge_vector[prev_sector] * shortest_distance;
// if the sector counter-clockwise from the previous sector has an invalid distance, set boundary to create a cup like boundary
//如果扇区逆时针从先前扇区起到无效的距离，则设置边界以创建杯形边界。
uint8_t prev_sector_ccw = (prev_sector == 0) ? _num_sectors-1 : prev_sector-1;
if (!_distance_valid[prev_sector_ccw])
{
_boundary_point[prev_sector_ccw] = _sector_edge_vector[prev_sector_ccw] * shortest_distance; //计算边界点
}
}到这里计算得到了边界点数据：_boundary_point[]
——————————————————————————————————————————————————————————————————
我们看下其中的一部分代码
_boundary_point[next_sector] = _sector_edge_vector[next_sector] * shortest_distance;
查找可以看出_sector_edge_vector这个矢量是一个余弦值，用来计算最短距离的
被这里调用：
init_boundary();
AP_Proximity_Backend::AP_Proximity_Backend(AP_Proximity &_frontend, AP_Proximity::Proximity_State &_state) :
frontend(_frontend),
state(_state)
{
//用于建立边界围栏的扇形边缘矢量初始化------ initialise sector edge vector used for building the boundary fence
init_boundary();
}是一个构造函数，只学创建对象就会被调用，还记得前面创建的对象吗？（drivers[instance] = new AP_Proximity_RangeFinder(*this, state[instance]);）所以这里就直接初始化了
我们分析下这个代码：
void AP_Proximity_Backend::init_boundary()
{
for (uint8_t sector=0; sector < _num_sectors; sector++)
{
float angle_rad = radians((float)_sector_middle_deg[sector]+(float)_sector_width_deg[sector]/2.0f); //转换成弧度25度
_sector_edge_vector[sector].x = cosf(angle_rad) * 100.0f; //扇形边界矢量的右边界，用来计算最短距离，这个第一扇区的距离=边界×cos25
_sector_edge_vector[sector].y = sinf(angle_rad) * 100.0f;//
_boundary_point[sector] = _sector_edge_vector[sector] * PROXIMITY_BOUNDARY_DIST_DEFAULT; //这里是默认边界点，第一区域的计算：结果是_sector_edge_vector[sector].x=100m*cos(25),_sector_edge_vector[sector].y=100m*sin(25)
}
}图中的八个箭头就是我们每个区域需要计算的值。