削去旁支(细节请参考pixhawk mc_pos_control.cpp源码解读),位置控制主要流程如下
1.PID参数
param_get(_params_handles.xy_p, &v);
_params.pos_p(0) = v;
_params.pos_p(1) = v;
param_get(_params_handles.z_p, &v);
_params.pos_p(2) = v;
param_get(_params_handles.xy_vel_p, &v);
_params.vel_p(0) = v;
_params.vel_p(1) = v;
param_get(_params_handles.z_vel_p, &v);
_params.vel_p(2) = v;
param_get(_params_handles.xy_vel_i, &v);
_params.vel_i(0) = v;
_params.vel_i(1) = v;
param_get(_params_handles.z_vel_i, &v);
_params.vel_i(2) = v;
param_get(_params_handles.xy_vel_d, &v);
_params.vel_d(0) = v;
_params.vel_d(1) = v;
param_get(_params_handles.z_vel_d, &v);
_params.vel_d(2) = v;_pos(0) = _local_pos.x;
_pos(1) = _local_pos.y;
if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
_pos(2) = -_local_pos.dist_bottom;
} else {
_pos(2) = _local_pos.z;
}
_vel(0) = _local_pos.vx;
_vel(1) = _local_pos.vy;
if (_params.alt_mode == 1 && _local_pos.dist_bottom_valid) {
_vel(2) = -_local_pos.dist_bottom_rate;
} else {
_vel(2) = _local_pos.vz;
}3.控制流程(循环控制)
位置控制也是内外环P-PID控制，外环P是位置控制，内环PID是速度控制。但光流模式control_manual(dt)外环P项常为0，相当于只有内环速度控制；control_offboard(dt)不知道是什么模式，可选择双环或者单环控制；control_auto(dt)是GPS模式，一定是双环控制。
以上结论是通过以下过程推导出的
首先
_run_pos_control = true;
_run_alt_control = true;接着
control_manual(dt);里
_run_pos_control 、_run_alt_control可为false或者true(2016.07.19修改)
control_offboard(dt);里
_run_pos_control 、_run_alt_control可为false或者不改
control_auto(dt);里
_run_pos_control 、_run_alt_control没有改动
再接着外环处理
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}
以下摘取部分代码，帮助理解(注意以下代码是循环执行，_run_pos_control 、_run_alt_control 会不断的变为true，加上外环)(2016.07.19修改)
_run_pos_control = true;
_run_alt_control = true;
/* select control source */
if (_control_mode.flag_control_manual_enabled) {
/* manual control */
control_manual(dt);
_mode_auto = false;
} else if (_control_mode.flag_control_offboard_enabled) {
/* offboard control */
control_offboard(dt);
_mode_auto = false;
} else {
/* AUTO */
control_auto(dt);
}
……
if (_run_pos_control) {
_vel_sp(0) = (_pos_sp(0) - _pos(0)) * _params.pos_p(0);
_vel_sp(1) = (_pos_sp(1) - _pos(1)) * _params.pos_p(1);
}
……
if (_run_alt_control) {
_vel_sp(2) = (_pos_sp(2) - _pos(2)) * _params.pos_p(2);
}void
MulticopterPositionControl::control_manual(float dt)
{
math::Vector<3> req_vel_sp; // X,Y in local frame and Z in global (D), in [-1,1] normalized range
req_vel_sp.zero();
//每次都会运行
if (_control_mode.flag_control_altitude_enabled) {
/* set vertical velocity setpoint with throttle stick */
req_vel_sp(2) = -scale_control(_manual.z - 0.5f, 0.5f, _params.alt_ctl_dz, _params.alt_ctl_dy); // D
}
//每次都会运行
if (_control_mode.flag_control_position_enabled) {
/* set horizontal velocity setpoint with roll/pitch stick */
req_vel_sp(0) = _manual.x;
req_vel_sp(1) = _manual.y;
}
//每次都会运行
if (_control_mode.flag_control_altitude_enabled) {
/* reset alt setpoint to current altitude if needed */
reset_alt_sp();//将当前的位置赋给设定的期望位置
}
//每次都会运行
if (_control_mode.flag_control_position_enabled) {
/* reset position setpoint to current position if needed */
reset_pos_sp();//将当前的位置赋给设定的期望位置
}
/* limit velocity setpoint */
float req_vel_sp_norm = req_vel_sp.length();
if (req_vel_sp_norm > 1.0f) {
req_vel_sp /= req_vel_sp_norm;
}
/* _req_vel_sp scaled to 0..1, scale it to max speed and rotate around yaw */
math::Matrix<3, 3> R_yaw_sp;
R_yaw_sp.from_euler(0.0f, 0.0f, _att_sp.yaw_body);
math::Vector<3> req_vel_sp_scaled = R_yaw_sp * req_vel_sp.emult(
_params.vel_cruise); // in NED and scaled to actual velocity
/*
* assisted velocity mode: user controls velocity, but if velocity is small enough, position
* hold is activated for the corresponding axis
*/
//每次都会运行
/* horizontal axes */
if (_control_mode.flag_control_position_enabled) {
/* check for pos. hold */
if (fabsf(req_vel_sp(0)) < _params.hold_xy_dz && fabsf(req_vel_sp(1)) < _params.hold_xy_dz) {
if (!_pos_hold_engaged) {
if (_params.hold_max_xy < FLT_EPSILON || (fabsf(_vel(0)) < _params.hold_max_xy
&& fabsf(_vel(1)) < _params.hold_max_xy)) {
_pos_hold_engaged = true;//要外环，没拨动遥控，此时定点
} else {
_pos_hold_engaged = false;
}
}
} else {
_pos_hold_engaged = false;
}
/* set requested velocity setpoint */
if (!_pos_hold_engaged) {//不要外环，拨动遥控，此时飞行器会移动，只有速度环
_pos_sp(0) = _pos(0);
_pos_sp(1) = _pos(1);
_run_pos_control = false; /* request velocity setpoint to be used, instead of position setpoint */
_vel_sp(0) = req_vel_sp_scaled(0);
_vel_sp(1) = req_vel_sp_scaled(1);
}
}
/* vertical axis */
if (_control_mode.flag_control_altitude_enabled) {
/* check for pos. hold */
if (fabsf(req_vel_sp(2)) < FLT_EPSILON) {
if (!_alt_hold_engaged) {
if (_params.hold_max_z < FLT_EPSILON || fabsf(_vel(2)) < _params.hold_max_z) {
_alt_hold_engaged = true;//要外环，没拨动遥控，此时定点
} else {
_alt_hold_engaged = false;
}
}
} else {
_alt_hold_engaged = false;
}
/* set requested velocity setpoint */
if (!_alt_hold_engaged) {//不要外环，拨动遥控，此时飞行器会移动，只有速度环
_run_alt_control = false; /* request velocity setpoint to be used, instead of altitude setpoint */
_vel_sp(2) = req_vel_sp_scaled(2);
_pos_sp(2) = _pos(2);
}
}
}再经过一些处理就可以发布
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
if (_global_vel_sp_pub != nullptr) {
orb_publish(ORB_ID(vehicle_global_velocity_setpoint), _global_vel_sp_pub, &_global_vel_sp);
} else {
_global_vel_sp_pub = orb_advertise(ORB_ID(vehicle_global_velocity_setpoint), &_global_vel_sp);
}内环处理
math::Vector<3> vel_err = _vel_sp - _vel;D项在刚进循环的时候
_vel_err_d(0) = _vel_x_deriv.update(-_vel(0));
_vel_err_d(1) = _vel_y_deriv.update(-_vel(1));
_vel_err_d(2) = _vel_z_deriv.update(-_vel(2));I项在PID公式下面
thrust_int(0) += vel_err(0) * _params.vel_i(0) * dt;
thrust_int(1) += vel_err(1) * _params.vel_i(1) * dt;
thrust_int(2) += vel_err(2) * _params.vel_i(2) * dt;内环PID
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;此时就可保存在日志中，位置在后面些
/* save thrust setpoint for logging */
_local_pos_sp.acc_x = thrust_sp(0) * ONE_G;
_local_pos_sp.acc_y = thrust_sp(1) * ONE_G;
_local_pos_sp.acc_z = thrust_sp(2) * ONE_G;4.将内外环算出来的控制量转变为期望姿态， 用于后面的mc_att_control；定高用的_att_sp.thrust将作为油门输出
以下部分理论比较深，还没完全理解
body_z = -thrust_sp / thrust_abs;
body_x = y_C % body_z;
body_y = body_z % body_x;
/* fill rotation matrix */
for (int i = 0; i < 3; i++) {
R(i, 0) = body_x(i);
R(i, 1) = body_y(i);
R(i, 2) = body_z(i);
}
q_sp.from_dcm(R);
math::Vector<3> euler = R.to_euler();
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
_att_sp.thrust = thrust_abs;此时就可以发布
/* fill local position, velocity and thrust setpoint */
_local_pos_sp.timestamp = hrt_absolute_time();
_local_pos_sp.x = _pos_sp(0);
_local_pos_sp.y = _pos_sp(1);
_local_pos_sp.z = _pos_sp(2);
_local_pos_sp.yaw = _att_sp.yaw_body;//yaw方向处理方法和xyz方向不一样，还没理解
_local_pos_sp.vx = _vel_sp(0);
_local_pos_sp.vy = _vel_sp(1);
_local_pos_sp.vz = _vel_sp(2);其中_pos_sp(0); _pos_sp(1); _pos_sp(2);来自control_offboard(dt);或者control_auto(dt);
_local_pos_sp.vx比_global_vel_sp.vx多了一个过程的处理
if (!control_vel_enabled_prev && _control_mode.flag_control_velocity_enabled) {
// choose velocity xyz setpoint such that the resulting thrust setpoint has the direction
// given by the last attitude setpoint
_vel_sp(0) = _vel(0) + (-PX4_R(_att_sp.R_body, 0, 2) * _att_sp.thrust - thrust_int(0) - _vel_err_d(0) * _params.vel_d(0)) / _params.vel_p(0);
_vel_sp(1) = _vel(1) + (-PX4_R(_att_sp.R_body, 1, 2) * _att_sp.thrust - thrust_int(1) - _vel_err_d(1) * _params.vel_d(1)) / _params.vel_p(1);
_vel_sp(2) = _vel(2) + (-PX4_R(_att_sp.R_body, 2, 2) * _att_sp.thrust - thrust_int(2) - _vel_err_d(2) * _params.vel_d(2)) / _params.vel_p(2);
_vel_sp_prev(0) = _vel_sp(0);
_vel_sp_prev(1) = _vel_sp(1);
_vel_sp_prev(2) = _vel_sp(2);
control_vel_enabled_prev = true;
// compute updated velocity error
vel_err = _vel_sp - _vel;
}发布
if (_att_sp_pub != nullptr) {
orb_publish(_attitude_setpoint_id, _att_sp_pub, &_att_sp);
} else if (_attitude_setpoint_id) {
_att_sp_pub = orb_advertise(_attitude_setpoint_id, &_att_sp);
}您的鼓励将是笔者书写高质量文章的最大动力^_^!!
5万+
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
