上文写了PIXHAWK的姿态控制算法,该文主要对姿态控制输出到电机输出的通道分配环节进行深入解析。PX4的通道分配环节查看mixer_multirotor.cpp(对应多旋翼的通道分配)。
PX4的mixer策略:
/* 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]
*/
先融合横滚、俯仰、油门值,然后依据得到的结果对其横滚、俯仰进行比率钳位,最后融合航向值,对航向进行比率钳位,最后对整体做限幅和怠速处理。(从处理的顺序可以看出横滚/俯仰的响应优先级大于航向)
其中比率钳位的意思可看下图讲解:
这里思考一下,是比例钳位好?还是整体都减去四个电机的最大值越界的部分好呢?(有待后续进一步实验验证)
实际代码执行细节会相对更加丰富一些,接下来讲解一下代码结构。
首先从姿态控制器输出获得roll,pitch, yaw, thrust:
float roll = constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
float pitch = constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
float yaw = constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float thrust = constrain(get_control(0, 3), 0.0f, 1.0f);
然后在忽略yaw的情况下,计算融合后结果的最大最小值
/* perform initial mix pass yielding unbounded outputs, ignore yaw */
具体代码:
for (unsigned i = 0; i < _rotor_count; i++) {
float out = roll * _rotors[i].roll_scale +pitch * _rotors[i].pitch_scale +thrust;
out *= _rotors[i].out_scale;
/* calculate min and max output values */
if (out < min_out) {
min_out = out;
}
if (out > max_out) {
max_out = out;
}
outputs[i] = out;
}
其中_rotors[i].roll_scale,_rotors[i].pitch_scale等参数可见"mixer_multirotor.generated.h"中相关定义,如下:
const MultirotorMixer::Rotor _config_quad_x[] = {
{ -0.707107, 0.707107, 1.000000, 1.000000 },
//四行对应四个电机,第一行中的四个参数分别对应roll/pitch/yaw/throw的权重系数。
{ 0.707107, -0.707107, 1.000000, 1.000000 },
{ 0.707107, 0.707107, -1.000000, 1.000000 },
{ -0.707107, -0.707107, -1.000000, 1.000000 },
};
对计算得到的最大最小值进行判断输出是否会饱和(不在[0,1]区间内则饱和)对应代码见下文,若饱和则对相应寄存器状态进行处理,该处理结果将会作为角速度环是否积分的一个标志(if (_thrust_sp > MIN_TAKEOFF_THRUST && !_motor_limits.lower_limit && !_motor_limits.upper_limit )其中_motor_limits.lower_limit与_motor_limits.upper_limit 便是对应其标志)。
if (min_out < 0.0f) {
if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_LOWER_LIMIT;
}
}
if (max_out > 1.0f) {
if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_UPPER_LIMIT;
}
}
接着根据得到的最大最小值,进行了一系列的分类处理求得boost 和roll_pitch_scale(两变量对应下文解释)
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
其中具体分了一下5种情况:
if (min_out < 0.0f && max_out < 1.0f && -min_out <= 1.0f - max_out) {
float max_thrust_diff = thrust * thrust_increase_factor - thrust;
if (max_thrust_diff >= -min_out) {
boost = -min_out; //对应out [0,max-min]
} else {
boost = max_thrust_diff;
roll_pitch_scale = (thrust + boost) / (thrust - min_out);
}
} else if (max_out > 1.0f && min_out > 0.0f && min_out >= max_out - 1.0f) {
float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
if (max_thrust_diff >= max_out - 1.0f) {
boost = -(max_out - 1.0f); //对应out [1-max+min,1]
} else {
boost = -max_thrust_diff;
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust);
}
} else if (min_out < 0.0f && max_out < 1.0f && -min_out > 1.0f - max_out) {
float max_thrust_diff = thrust * thrust_increase_factor - thrust;
boost = constrain(-min_out - (1.0f - max_out) / 2.0f, 0.0f, max_thrust_diff);
roll_pitch_scale = (thrust + boost) / (thrust - min_out); //对应out [0,2max-1]
} else if (max_out > 1.0f && min_out > 0.0f && min_out < max_out - 1.0f) {
float max_thrust_diff = thrust - thrust_decrease_factor * thrust;
boost = constrain(-(max_out - 1.0f - min_out) / 2.0f, -max_thrust_diff, 0.0f);
roll_pitch_scale = (1 - (thrust + boost)) / (max_out - thrust); //对应out [2min,1]
} else if (min_out < 0.0f && max_out > 1.0f) {
boost = constrain(-(max_out - 1.0f + min_out) / 2.0f,
thrust_decrease_factor * thrust - thrust,
thrust_increase_factor * thrust - thrust);
roll_pitch_scale = (thrust + boost) / (thrust - min_out); //对应out [0,1]
}
即当outputs饱和时,会将roll_pitch_scale 缩小(<1),同时根据min_out 和max_out 来决定boost 的大小,其中使用的额相关参数:
// thrust boost parameters
float thrust_increase_factor = 1.5f;
float thrust_decrease_factor = 0.6f;
接着重新将thrust,boost, 变比例后的roll/pitch和yaw进行融合:
// mix again but now with thrust boost, scale roll/pitch and also add yaw
float out = (roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale)
* roll_pitch_scale + yaw * _rotors[i].yaw_scale + thrust + boost;
然后根据输出是否饱和,对航向进行比率钳位,使其输出在[0,1]范围内:
// scale yaw if it violates limits. inform about yaw limit reached
if (out < 0.0f) {
yaw = -((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale)
*roll_pitch_scale + thrust + boost) / _rotors[i].yaw_scale;
//yaw值变化,从而整个yaw作用内容进行比率钳位
if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
}
} else if (out > 1.0f) {
// allow to reduce thrust to get some yaw response
//(这里减小油门应该是为下一循环的yaw预留响应)
float thrust_reduction = fminf(0.15f, out - 1.0f);
thrust -= thrust_reduction;
yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale)
* roll_pitch_scale + thrust + boost)) / _rotors[i].yaw_scale;
//(感觉这里yaw变成0了)
if (status_reg != NULL) {
(*status_reg) |= PX4IO_P_STATUS_MIXER_YAW_LIMIT;
}
}
同时PX4IO_P_STATUS_MIXER_YAW_LIMIT也会对角速度环的积分进行影响。
最后加上比率钳位后的航向值,将最后输出限幅到[idle_speed,1]。
/* add yaw and scale outputs to range idle_speed...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;
outputs[i] = constrain(_idle_speed
+ (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
}
如此便得到了对应驱动电调的输入值,另外电调的PWM频率设定值需要快于角速度输出的频率或与其保持同步,推荐设置400Hz,这样响应跟得上计算输出值,响应更加及时。至此,PX4的总体控制框架总结完毕,PX4的精华当然不止于此,这只是冰山一角,学习PX4代码,建议先把操作系统(UCOS、FreeRTOS之类)学习一下,这样更加系统的了解整个程序的运行机制。
推荐扫码享购最速飞控学习视频:
欢迎加入欣飞鸽总动员一起交流讨论,qq群聊号码:879466897
更多无人机知识分享,请关注微信公众号:欣飞鸽
开心飞翔,灵动如鸽。愿与无人机爱好者,共创一片蓝天梦!
本文内容由网友自发贡献,版权归原作者所有,本站不承担相应法律责任。如您发现有涉嫌抄袭侵权的内容,请联系:hwhale#tublm.com(使用前将#替换为@)