在上一篇的Copter.cpp中运行的位置控制器的介绍,这篇相对较多较为复杂而且代码量大所以分段解释

在libraries\AC_AttitudeControl\AC_PosControl.cpp:下

首先是水平位置控制器

void AC_PosControl::run_xy_controller(float dt)

EKF选择

    AP::ahrs_navekf().getEkfControlLimits(ekfGndSpdLimit, ekfNavVelGainScaler);

这个估计应该是选择对应姿态和位置估计系统 (EKF), 在APM中有三种

官方的解释是一种 AP_NavEKF2 库中的一个 24 状态扩展卡尔曼滤波器，用于估计以下状态

态度（四元数）
速度（北、东、下）
位置（北、东、下）
陀螺偏置偏移 (X,Y,Z)
陀螺比例因子 (X,Y,Z)
Z 加速度偏差
地球磁场（北、东、下）
体磁场 (X,Y,Z)
风速（北，东）

2. 当前位置获取

 Vector3f curr_pos = _inav.get_position();

这里注意APM内写了特别多的重载函数需要仔细查询对应的调用是那个查询一下_inav就可以找到了最后跳转到libraries\AP_InertialNav\AP_InertialNav_NavEKF.cpp

const Vector3f &AP_InertialNav_NavEKF::get_position(void) const
{return _relpos_cm;
}

其中_relpos_cm 就是返回的当前位置而当前位置又是通过以下的来同样在libraries\AP_InertialNav\AP_InertialNav_NavEKF.cpp文件内

但是目前还没找到这个文件的在那里启动的目前感觉应该是在主函数loop里面调用的位置控制器的启动的为了确定我们在这个函数中添加了地面站消息打印 gcs().send_text(MAV_SEVERITY_CRITICAL, //地面站消息发送
"AP_InertialNav_NavEKF ok");

在自稳模式下也调用了这个即使是无GPS情况下

void AP_InertialNav_NavEKF::update(bool high_vibes)
{// get the NE position relative to the local earth frame originVector2f posNE;if (_ahrs_ekf.get_relative_position_NE_origin(posNE)) {_relpos_cm.x = posNE.x * 100; // convert from m to cm_relpos_cm.y = posNE.y * 100; // convert from m to cm}// get the D position relative to the local earth frame originfloat posD;if (_ahrs_ekf.get_relative_position_D_origin(posD)) {_relpos_cm.z = - posD * 100; // convert from m in NED to cm in NEU}// get the velocity relative to the local earth frameVector3f velNED;if (_ahrs_ekf.get_velocity_NED(velNED)) {// during high vibration events use vertical position changeif (high_vibes) {float rate_z;if (_ahrs_ekf.get_vert_pos_rate(rate_z)) {velNED.z = rate_z;}}_velocity_cm = velNED * 100; // convert to cm/s_velocity_cm.z = -_velocity_cm.z; // convert from NED to NEU}gcs().send_text(MAV_SEVERITY_CRITICAL,  //地面站消息发送"AP_InertialNav_NavEKF ok");
}

当前的位置被保存到了_relpos_cm中,具体这么获得的我们跳转到libraries\AP_AHRS\AP_AHRS_NavEKF.cpp文件中的get_relative_position_NE_origin()中,害真的是一套嵌一套我们接着看这个函数的作用是以米为单位写一个相对地面位置估计到原点

bool AP_AHRS_NavEKF::get_relative_position_NE_origin(Vector2f &posNE) const
{switch (active_EKF_type()) {case EKF_TYPE_NONE:return false;case EKF_TYPE2:default: {bool position_is_valid = EKF2.getPosNE(-1,posNE);return position_is_valid;}case EKF_TYPE3: {bool position_is_valid = EKF3.getPosNE(-1,posNE);return position_is_valid;}#if CONFIG_HAL_BOARD == HAL_BOARD_SITLcase EKF_TYPE_SITL: {Location loc;get_position(loc);posNE = get_home().get_distance_NE(loc);return true;}
#endif}
}

这里的active_EKF_type()就不细说了这个这个是判断当前用的是那个EKF来确认数据的选择

这个active_EKF_type()内容也很大改天深入研究一下

getPosNE(posNE) 返回 ekf 高度和垂直加速度的补充滤波器计算出的值单位为米

现在就弄清楚了我们的当前位置是这么来的了,下面是将单位转换成厘米

        _relpos_cm.x = posNE.x * 100; // convert from m to cm_relpos_cm.y = posNE.y * 100; // convert from m to cm

我们将高度写入地面站显示能成功输出

在这一步这里当前位置就已经能够成功获取和提取了

终于..

现在回到libraries\AC_AttitudeControl\AC_PosControl.cpp

为什么要获取当前位置是因为需要计算位置误差,

位置误差=期望位置-当前位置:

        _pos_error.x = _pos_target.x - curr_pos.x;_pos_error.y = _pos_target.y - curr_pos.y;

我们需要飞行到期望位置所以已经求得位置的误差所以只需要

当前位置+位置误差=期望位置

注意这里需要判断是否超出最大的约束 _leash

        if (limit_vector_length(_pos_error.x, _pos_error.y, _leash)) {_pos_target.x = curr_pos.x + _pos_error.x;_pos_target.y = curr_pos.y + _pos_error.y;}

期望速度=位置误差*Kp

 _vel_target = sqrt_controller(_pos_error, kP, _accel_cms);

我们来看看这个当前速度是这么来的:

Vector3f AC_PosControl::sqrt_controller(const Vector3f& error, float p, float second_ord_lim)
{if (second_ord_lim < 0.0f || is_zero(second_ord_lim) || is_zero(p)) {return Vector3f(error.x * p, error.y * p, error.z);}float linear_dist = second_ord_lim / sq(p);float error_length = norm(error.x, error.y);if (error_length > linear_dist) {float first_order_scale = safe_sqrt(2.0f * second_ord_lim * (error_length - (linear_dist * 0.5f))) / error_length;return Vector3f(error.x * first_order_scale, error.y * first_order_scale, error.z);} else {return Vector3f(error.x * p, error.y * p, error.z);}
}

加上我们的速度前馈

 _vel_target.x += _vel_desired.x;_vel_target.y += _vel_desired.y;

到这一部分我们的当前位置,位置误差,当前速度,期望速度都已经计算完成了,还差速度误差

获取当前速度

       _vehicle_horiz_vel.x = _inav.get_velocity().x;_vehicle_horiz_vel.y = _inav.get_velocity().y;

速度误差=期望速度-当前速度

    _vel_error.x = _vel_target.x - _vehicle_horiz_vel.x;_vel_error.y = _vel_target.y - _vehicle_horiz_vel.y;

调用PID控制器传入了速度误差

_pid_vel_xy.set_input(_vel_error);

void set_input(const Vector3f &input) { set_input(Vector2f(input.x, input.y)); }

然后调出pid:

_pid_vel_xy.set_input(_vel_error);vel_xy_p = _pid_vel_xy.get_p();// update i term if we have not hit the accel or throttle limits OR the i term will reduce// TODO: move limit handling into the PI and PID controllerif (!_limit.accel_xy && !_motors.limit.throttle_upper) {vel_xy_i = _pid_vel_xy.get_i();} else {vel_xy_i = _pid_vel_xy.get_i_shrink();}// get dvel_xy_d = _pid_vel_xy.get_d();

最后校准和互补数据传递给控制器

 //  加速度校准速度误差accel_target.x = (vel_xy_p.x + vel_xy_i.x + vel_xy_d.x) * ekfNavVelGainScaler;accel_target.y = (vel_xy_p.y + vel_xy_i.y + vel_xy_d.y) * ekfNavVelGainScaler;// reset accel to current desired acceleration//将加速度复位到所需的当前加速度if (_flags.reset_accel_to_lean_xy) {_accel_target_filter.reset(Vector2f(accel_target.x, accel_target.y));_flags.reset_accel_to_lean_xy = false;}// filter correction acceleration//滤波器校正加速度_accel_target_filter.set_cutoff_frequency(MIN(_accel_xy_filt_hz, 5.0f * ekfNavVelGainScaler));_accel_target_filter.apply(accel_target, dt);// pass the correction acceleration to the target acceleration output_accel_target.x = _accel_target_filter.get().x;_accel_target.y = _accel_target_filter.get().y;// Add feed forward into the target acceleration output//向目标加速度输出中加入前馈_accel_target.x += _accel_desired.x;_accel_target.y += _accel_desired.y;// the following section converts desired accelerations provided in lat/lon frame to roll/pitch angles// limit acceleration using maximum lean anglesfloat angle_max = MIN(_attitude_control.get_althold_lean_angle_max(), get_lean_angle_max_cd());float accel_max = MIN(GRAVITY_MSS * 100.0f * tanf(ToRad(angle_max * 0.01f)), POSCONTROL_ACCEL_XY_MAX);_limit.accel_xy = limit_vector_length(_accel_target.x, _accel_target.y, accel_max);// update angle targets that will be passed to stabilize controller//更新后的数据传递给控制器accel_to_lean_angles(_accel_target.x, _accel_target.y, _roll_target, _pitch_target);

_roll_target, _pitch_target 就是 get_roll() get_pitch() 所调用的位置数据

通过attitude_control->input_euler_angle_roll_pitch_euler_rate_yaw() 姿态控制实现位置控制

END

Apm飞控学习笔记-AC_PosControl位置控制-Cxm相关推荐

Apm飞控学习笔记之悬停loiter模式-Cxm
文章汇集 PX4/APM/飞控的学习笔记前言-Cxm_CHENxiaomingming的博客-CSDN博客_apm和px4哪个好前言时隔一段时间又开始琢磨APM飞控了,在上一篇中写了姿态控制,经过 ...
Apm飞控学习笔记-姿态控制-Cxm
目录 PX4/APM/飞控的学习笔记前言-Cxm_chen_taifu的博客-CSDN博客开始了开始了终于有时间可以学习飞控了此文章是用来当目录,我会持续更新我的学习之旅,希望能对各位有所帮助如果有 ...
Apm飞控学习笔记之添加我的设备或单片机串口通信-Cxm
目录 PX4/APM/飞控的学习笔记前言-Cxm_chen_taifu的博客-CSDN博客开始了开始了终于有时间可以学习飞控了此文章是用来当目录,我会持续更新我的学习之旅,希望能对各位有所帮助如果有 ...
Apm飞控学习笔记之添加我的飞行模式-Cxm
目录 PX4/APM/飞控的学习笔记前言-Cxm_chen_taifu的博客-CSDN博客开始了开始了终于有时间可以学习飞控了此文章是用来当目录,我会持续更新我的学习之旅,希望能对各位有所帮助如果有 ...
Apm飞控学习笔记之如何添加自己的功能-Cxm
目录: PX4/APM/飞控的学习笔记前言-Cxm_chen_taifu的博客-CSDN博客开始了开始了终于有时间可以学习飞控了此文章是用来当目录,我会持续更新我的学习之旅,希望能对各位有所帮助如果 ...
APM飞控学习笔记——自动模式下一分钟自动降落
提示:文章写完后,目录可以自动生成,如何生成可参考右边的帮助文档文章目录前言一.自动模式简介二.添加自动降落功能 1.飞控主循环调用逻辑 2.功能添加总结前言 APM是一款功能齐全的开源多 ...
Apm飞控学习笔记之-RC_Channel遥控器数据获取-Cxm
前言这一篇分析APM的遥控器数据获取 ArduCopter\Copter.cpp中rc_loop遥控器线程 const AP_Scheduler::Task Copter::scheduler_ta ...
第一篇——APM飞控学习笔记
关于什么是APM? 可能有很多玩航模或者是向我这样混迹于实验室的垃圾佬(bushi)看到过这个玩意--->>> 从狭义上来说,这个就是APM开源飞控. APM又叫ArduPilot- ...
基于tiva的匿名飞控学习笔记（1）
基于tiva的匿名飞控学习笔记(1) 开关状态任务遥控器数据处理任务数传数据交换延时存储任务开关状态任务匿名飞控的开关状态任务为函数Swtich_State_Task(u8 dT_ms),定 ...
APM飞控学习之路：1 无人机的分类与发展
"旧时王谢堂前燕,飞入寻常百姓家".无人机也像那堂前燕,从以前为军事所专属,负责侦查和战斗,飞入民用领域,在航拍.植保.快递.救灾.巡检.拍摄等行业大显身手,无人机+的应用遍地开花 ...

Apm飞控学习笔记-AC_PosControl位置控制-Cxm

首先是水平位置控制器

EKF选择

2. 当前位置获取

Apm飞控学习笔记-AC_PosControl位置控制-Cxm相关推荐

最新文章

热门文章