10.1中介绍了目标位置点的计算逻辑,知道下一时刻的目标位置后,飞控需要根据当前位置进行计算,依次得到期望速度,期望拉力矢量,期望姿态。至此就完成了多旋翼的位置控制。
1、期望速度计算
上篇计算得到期望位置之后,根据飞机当前位置,可知道位置差矢量。在位置速度串级PID中,位置环只采用了比例,速度环采用了PID。![在这里插入图片描述](https://img-blog.csdnimg.cn/2020042617233336.png)
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);
}
得到期望速度之后,主要进行了以下三步操作,用来规范真正的期望速度:
#限制最大水平速度
确保水平速度矢量和大小在限制范围内,对水平两个速度进行等比例缩小;
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) +
_vel_sp(1) * _vel_sp(1));
if (vel_norm_xy > _params.vel_max(0)) {
_vel_sp(0) = _vel_sp(0) * _params.vel_max(0) / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _params.vel_max(1) / vel_norm_xy;
}
#限制最大垂直速度
确保垂直速度在限制范围内,对垂向速度进行限制。
if (_vel_sp(2) < -1.0f * _params.vel_max_up) {
_vel_sp(2) = -1.0f * _params.vel_max_up;
}
if (_vel_sp(2) > _params.vel_max_down) {
_vel_sp(2) = _params.vel_max_down;
}
#限制期望速度变化量(加速度)
对当前和之前的期望速度做差,限制其变化量,即限制加速度值。
math::Vector<2> acc_hor;
acc_hor(0) = (_vel_sp(0) - _vel_sp_prev(0)) / dt;
acc_hor(1) = (_vel_sp(1) - _vel_sp_prev(1)) / dt;
if ((acc_hor.length() > _params.acc_hor_max) & !_reset_pos_sp) {
acc_hor.normalize();
acc_hor *= _params.acc_hor_max;
math::Vector<2> vel_sp_hor_prev(_vel_sp_prev(0), _vel_sp_prev(1));
math::Vector<2> vel_sp_hor = acc_hor * dt + vel_sp_hor_prev;
_vel_sp(0) = vel_sp_hor(0);
_vel_sp(1) = vel_sp_hor(1);
}
float acc_v = (_vel_sp(2) - _vel_sp_prev(2)) / dt;
if ((fabsf(acc_v) > 2 * _params.acc_hor_max) & !_reset_alt_sp) {
acc_v /= fabsf(acc_v);
_vel_sp(2) = acc_v * 2 * _params.acc_hor_max * dt + _vel_sp_prev(2);
}
_vel_sp_prev = _vel_sp;
_global_vel_sp.vx = _vel_sp(0);
_global_vel_sp.vy = _vel_sp(1);
_global_vel_sp.vz = _vel_sp(2);
2.期望拉力矢量计算
拉力矢量由期望速度和当前速度的差进行PID计算得到。
thrust_sp = vel_err.emult(_params.vel_p) + _vel_err_d.emult(_params.vel_d) + thrust_int;
得到期望拉力矢量之后,又需要进行以下三种操作得到最终的实际期望拉力矢量:
#确保升力大于最小值
确保拉力矢量的幅值不能小于最小拉力值,不至于飞机掉下去。
if (-thrust_sp(2) < thr_min) {
thrust_sp(2) = -thr_min;
saturation_z = true;
}
#保证飞机不超过最大倾斜角
限制最大倾斜角。利用Z向的拉力值乘以最大倾斜角的正切,得到最大水平速度。并根据实际的水平和速度的幅值等比例缩小水平两个速度。
if (thr_min >= 0.0f && tilt_max < M_PI_F / 2 - 0.05f) {
float thrust_sp_xy_len = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
if (thrust_sp_xy_len > 0.01f) {
float thrust_xy_max = -thrust_sp(2) * tanf(tilt_max);
if (thrust_sp_xy_len > thrust_xy_max) {
float k = thrust_xy_max / thrust_sp_xy_len;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
}
}
#限制最大推力
如果拉力矢量的幅值大于最大拉力,那么让水平期望拉力为零,Z向拉力为拉力。
if (-thrust_sp(2) > thr_max) {
thrust_sp(0) = 0.0f;
thrust_sp(1) = 0.0f;
thrust_sp(2) = -thr_max;
saturation_xy = true;
saturation_z = true;
} else {
float thrust_xy_max = sqrtf(thr_max * thr_max - thrust_sp(2) * thrust_sp(2));
float thrust_xy_abs = math::Vector<2>(thrust_sp(0), thrust_sp(1)).length();
float k = thrust_xy_max / thrust_xy_abs;
thrust_sp(0) *= k;
thrust_sp(1) *= k;
saturation_xy = true;
}
3.期望姿态计算
PIX中的期望姿态由期望拉力矢量和期望航向来计算。期望航向已经在路径规划时得到。
期望拉力矢量其实是指定了机体的期望Z轴,期望航向是指定了机体的期望Z轴。
即,将拉力矢量归一化,就是往姿态矩阵 的第三列。
math::Vector<3> body_x;
math::Vector<3> body_y;
math::Vector<3> body_z;
if (thrust_abs > SIGMA) {
body_z = -thrust_sp / thrust_abs;
} else {
body_z.zero();
body_z(2) = 1.0f;
}
将期望航向角通过三角函数变换在于姿态矩阵R_sp 的第三列叉乘就能得R_sp 的第一列。
math::Vector<3> y_C(-sinf(_att_sp.yaw_body), cosf(_att_sp.yaw_body), 0.0f);
if (fabsf(body_z(2)) > SIGMA) {
body_x = y_C % body_z;
if (body_z(2) < 0.0f) {
body_x = -body_x;
}
body_x.normalize();
再通过叉乘R_sp 的第三列和第一列,就能得到完整的姿态矩阵。
body_y = body_z % body_x;
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);
}
至此,就得到了期望的姿态矩阵,可将R转换为四元数,或者直接求解水平期望姿态角。
matrix::Quatf q_sp = R;
memcpy(&_att_sp.q_d[0], q_sp.data(), sizeof(_att_sp.q_d));
_att_sp.q_d_valid = true;
matrix::Eulerf euler = R;
_att_sp.roll_body = euler(0);
_att_sp.pitch_body = euler(1);
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)