混控函数在MIX函数
MultirotorMixer::mix(float *outputs, unsigned space)
查看注释
/* Summary of mixing strategy:
1) mix roll, pitch and thrust without yaw.
2) if some outputs violate range [0,1] then try to shift all outputs to minimize violation ->
increase or decrease total thrust (boost). The total increase or decrease of thrust is limited
(max_thrust_diff). If after the shift some outputs still violate the bounds then scale roll & pitch.
In case there is violation at the lower and upper bound then try to shift such that violation is equal
on both sides.
3) mix in yaw and scale if it leads to limit violation.
4) scale all outputs to range [idle_speed,1]*/
可以知道混控具体分为四步
1.简单混控无边界,无边界输出,不混合偏航
for (unsigned i = 0; i < _rotor_count; i++) {
float out = roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale +
thrust * _rotors[i].thrust_scale;
// 其中rotor[i]这个矩阵就是机型的混控模型矩阵,是由程序编译时生成的mixer_multirotor_normalized.generated.里面可以看到不同机型混控矩阵
/* 计算最大最小值 */
if (out < min_out) {
min_out = out;
}
if (out > max_out) {
max_out = out;
}
outputs[i] = out;
}
2.如果某些输出超出范围[0,1]，则尝试移动所有输出以将超出最大最小值的增加或减少总推力（增压）。推力的总增加或减少是有限的。
最大最小之差小于1，可以通过偏移解决。加油油门增加幅度大于|min|，可以通过增加油门的方式解决，boost = -min_out;如果油门增加幅度太小，需要把roll和pitch的比列缩放后再计算
float boost = 0.0f; // value added to demanded thrust (can also be negative)所需推力的附加值（也可以是负值）
float roll_pitch_scale = 1.0f; // scale for demanded roll and pitch所需滚动和俯仰的比例
float delta_out_max = max_out - min_out; // distance between the two extrema两个极端之间的距离
if (delta_out_max <= 1.0f) {
if (min_out < 0.0f) {
boost = -min_out;
} else if (max_out > 1.0f) {
boost = -(max_out - 1.0f);
}
} else {
roll_pitch_scale = 1.0f / (delta_out_max);
boost = 1.0f - ((max_out - thrust) * roll_pitch_scale + thrust);
//这个boost就是将输出区间缩放到[0,1]的偏移量,可以带入下面的输出公式中验证出来,这是个很牛逼的归一化公式
}
3.第三步,加入比例缩放和偏航,使输出范围限制在[0,1]
//再次混合，但现在加入推力增益，缩放滚动/俯仰，也增加偏航。
for (unsigned i = 0; i < _rotor_count; i++) {
float out = (roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
yaw * _rotors[i].yaw_scale +
(thrust + boost) * _rotors[i].thrust_scale;
//如果偏航超出限制，则进行缩放。减小下溢出
if (out < 0.0f) {
if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
yaw = 0.0f;
} else {
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
}
} else if (out > 1.0f) {
//允许减小推力以获得一些偏航响应
float prop_reduction = fminf(0.15f, out - 1.0f);
//保持请求的最大减少量
thrust_reduction = fmaxf(thrust_reduction, prop_reduction);
if (fabsf(_rotors[i].yaw_scale) <= FLT_EPSILON) {
yaw = 0.0f;
} else {
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
roll_pitch_scale + (thrust - thrust_reduction) + boost)) / _rotors[i].yaw_scale;
}
}
}
第四步 融合怠速处理
//应用总推力减小，一个支柱的最大值
thrust -= thrust_reduction;
//将偏航和缩放输出添加到范围空闲速度…1
for (unsigned i = 0; i < _rotor_count; i++) {
outputs[i] = (roll * _rotors[i].roll_scale +
pitch * _rotors[i].pitch_scale) * roll_pitch_scale +
yaw * _rotors[i].yaw_scale +
(thrust + boost) * _rotors[i].thrust_scale;
outputs[i] = math::constrain(_idle_speed + (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
}
最后输出时候进行了一下滤波检查
/* slew rate limiting and saturation checking 转换速率限制和饱和检查*/
for (unsigned i = 0; i < _rotor_count; i++) {
bool clipping_high = false;
bool clipping_low = false;
// check for saturation against static limits根据静态极限检查饱和度
if (outputs[i] > 0.99f) {
clipping_high = true;
} else if (outputs[i] < _idle_speed + 0.01f) {
clipping_low = true;
}
根据转换速率限制检查饱和度
if (_delta_out_max > 0.0f) {
float delta_out = outputs[i] - _outputs_prev[i];
if (delta_out > _delta_out_max) {
outputs[i] = _outputs_prev[i] + _delta_out_max;
clipping_high = true;
} else if (delta_out < -_delta_out_max) {
outputs[i] = _outputs_prev[i] - _delta_out_max;
clipping_low = true;
}
}
_outputs_prev[i] = outputs[i];
}
算法就这些,很清晰,几个简单的公式就可以将控制平稳输出,适用于多种机型,是很牛比的,在研究代码期间我也领悟了很多,二次开发一定要多查资料,搞清算法程序框架
谢谢大佬的文章,让我收益良多https://blog.csdn.net/qq_27016651/article/details/81123204