一个人可以走的更快,一群人才能走的更远,交流学习加qq:2096723956
更多保姆级PX4 ROS学习视频:https://b23.tv/ZeUDKqy
分享知识,传递正能量,如有疏漏或不当之处,恳请指出.
PX4固件版本:1.11.3
微信学习交流群:
PX4将混控器逻辑与实际姿态控制器分离,极大地提高了代码可重用性,不同的机型只需要定义不同的混控脚本,核心的代码可以通用(例如直升机的控制和多旋翼的控制是用的同一套控制算法).
混控器的作用是将速率控制器输出的所有三个轴(滚转、俯仰和偏航)的扭矩指令和标量推力值,转换为单独的电机推力指令或者舵机指令。最后,输出驱动器(例如UART、UAVCAN或PWM)将其缩放到执行器的输出值,例如PWM值。
脚本位置:
PX4使用控制输入组和输出组。不同的组中的值的含义是不同的,例如,用于核心飞行控制的姿态,或用于有效载荷的云台。输出组是一条物理总线,用于控制执行机构(电调),如飞控的fmu通道或者io通道或者UAVCAN通道(因为有些电调是支持UAVCAN协议的),如果uavcan已启用,主混控器将加载到设备/dev/uavcan/esc(uavcan),否则加载到/dev/pwm_output0(此设备将映射到带有I/O板的控制器上的IO驱动程序,以及不带I/O板的板上的FMU驱动程序),aux混控器文件加载到备/dev/pwm_output1中,该文件映射到带有I/O板的Pixhawk控制器上的FMU驱动程序.
每个控制输入组都有8个标准化的(-1… 1)命令端口,混控器就是定义这8个输入信号中的每一个如何映射到8个输出。由于有多个控制组(如飞行控制、有效载荷等)和多个输出组(总线),一个控制组可以向多个输出组发送命令。
不同的控制输入组的成员的含义如下:
Control Group #0 (Flight Control) 0: roll (-1..1) 1: pitch (-1..1) 2: yaw (-1..1) 3: throttle (0..1 normal range, -1..1 for variable pitch / thrust reversers) 4: flaps (-1..1) 5: spoilers (-1..1) 6: airbrakes (-1..1) 7: landing gear (-1..1) Control Group #1 (Flight Control VTOL/Alternate) 0: roll ALT (-1..1) 1: pitch ALT (-1..1) 2: yaw ALT (-1..1) 3: throttle ALT (0..1 normal range, -1..1 for variable pitch / thrust reversers) 4: reserved / aux0 5: reserved / aux1 6: reserved / aux2 7: reserved / aux3 #Control Group #2 (Gimbal) 0: gimbal roll 1: gimbal pitch 2: gimbal yaw 3: gimbal shutter 4: camera zoom 5: reserved 6: reserved 7: reserved (parachute, -1..1) #Control Group #3 (Manual Passthrough) 0: RC roll 1: RC pitch 2: RC yaw 3: RC throttle 4: RC mode switch (Passthrough of RC channel mapped by RC_MAP_FLAPS) 5: RC aux1 (Passthrough of RC channel mapped by RC_MAP_AUX1) 6: RC aux2 (Passthrough of RC channel mapped by RC_MAP_AUX2) 7: RC aux3 (Passthrough of RC channel mapped by RC_MAP_AUX3) Control Group #4 (Flight Control MC VIRTUAL) 0: roll ALT (-1..1) 1: pitch ALT (-1..1) 2: yaw ALT (-1..1) 3: throttle ALT (0..1 normal range, -1..1 for variable pitch / thrust reversers) 4: reserved / aux0 5: reserved / aux1 6: reserved / aux2 7: reserved / aux3 #Control Group #5 (Flight Control FW VIRTUAL) 0: roll ALT (-1..1) 1: pitch ALT (-1..1) 2: yaw ALT (-1..1) 3: throttle ALT (0..1 normal range, -1..1 for variable pitch / thrust reversers) 4: reserved / aux0 5: reserved / aux1 6: reserved / aux2 7: reserved / aux3 Control Group #6 (First Payload) 0: function 0 1: function 1 2: function 2 3: function 3 4: function 4 5: function 5 6: function 6 7: function 7
如果混控器是用在主通道输出的,则必须命名为XXXX.main.mix,如果是用于辅助通道输出的,则必须命名为XXXX.aux.mix.
多旋翼混控
R: <机身构型> <横滚缩放比例> <俯仰缩放比例> <偏航缩放比例> <怠速>
支持的多旋翼机身布局如下:
4x - X型四旋翼
4 - +型四旋翼
6x - X型六旋翼
6 - +型六旋翼
8x - X型八旋翼
8 - +型八旋翼
每个横滚、俯仰和偏航缩放比例值决定了横滚、俯仰和偏航控制相对于推力控制的比例。当计算作为浮点运算执行时,存储在定义文件中的值按10000的因数缩放,例如缩放系数0.5对应的值为5000。
横滚、俯仰和偏航输入范围为-1.0至1.0,而推力输入范围为0.0至1.0。每个执行器的输出在-1.0到1.0范围内。
怠速的范围从0.0到1.0。是指当所有控制输入为零时,电机旋转的速度。在执行器饱和的情况下,所有执行器值都会重新调整,以使饱和执行器输出限制在1.0。
累加混控
累加混控也成为简单混控(Simple),混控器将零或更多的控制输入合并到单个执行器输出中。输入被缩放,混控函数在应用输出缩放器之前对结果求和。格式如下:
M: <输入控制量个数>
O: <负缩放> <正缩放> <偏移值> <下限> <上限> <横向时间(可选)>
第一行M: <输入控制量个数>代表有个几输入来源,第二行O: 缩放值按10000的因数缩放;偏移值也按10000的因数偏移。<横向时间>用于执行器,如果执行器动作变化过快,可能会损坏执行器。例如,20000的<横向时间>值将限制执行器的变化率,使其从<下限>到<上限>至少需要2秒。
S: <控制输入组> <索引值> <负缩放> <正缩放> <偏移值> <下限> <上限>
S:表示具体的输入来源,与第一行M: <输入控制量个数>对应,有几个输入控制量,就有几个S:.如果<输入控制量个数>为零,混控器将输出一个受<下限>和<上限>约束的固定的<偏移值>。<控制输入组>值标识混控器将从中读取的控制组,<索引值>值标识该组内的偏移量。例如控制输入组0是载具姿态控制组,<索引值>0到3分别是横摇、俯仰、偏航和推力。
典型示例:
M: 2
O: 10000 10000 0 -10000 10000
S: 0 0 -6000 -6000 0 -10000 10000
S: 0 1 6500 6500 0 -10000 10000
有两个控制输入组,表示混控器将接收到两个输入
正负缩放均为1,偏移量为零,和输出范围-1到1。如果要反转PWM信号,必须更改输出比例的符号:O: -10000 -10000 0 -10000 10000
如果该行指定了默认的缩放比例O: 10000 10000 0 -10000 10000
,则可以(也应该)完全忽略该行.
第三行S:表示第一个控制输入:它接受控制组#0(飞行控制)和第一个输入(滚转)的输入。它缩放滚动控制输入*0.6,并恢复符号(-0.6以缩放单位变为-6000)。它不应用偏移(0)并输出到全量程(-1… 1)
直升机混控
倾斜盘的混控语句
H: <倾斜盘舵机数量,3或4个>
T: <油门推力曲线,当推力在 0%,25%, 50%,75%,100%时对应的油门值: 0%> <25%> <50%> <75%> <100%>
P: <总矩推力曲线,当推力在 0%,25%, 50%,75%,100%时对应的总矩值: 0%> <25%> <50%> <75%> <100%>
下面是每个倾斜盘舵机的混控
S: <舵机安装角度,以度为单位,0度为机头方向。从上面看,顺时钟方向为正> <舵机臂长度,10000等于1。如果所有舵机臂长度相同,则值应为10000。较大的臂长会减少伺服偏转量,较短的臂会增加伺服偏转量> <缩放值,伺服输出按缩放值/10000进行缩放> <偏移量,该值应介于-10000和 10000之间.控制量在缩放后,会加上偏移量> <控制量下限> <控制量上限>
舵机安装角度示意图如下:
直升机的尾桨可以用一个累加混控实现
混控器的输入是姿态控制产生的力矩,例如多旋翼经过内环角速率控制后输出的力矩如下:
1.抗下限饱和
在某些情况下,例如,在低推力和大横滚机动指令下,可能会出现一个电机指令变为负值的情况。这使得无人机无法执行这些指令(可逆电机除外),这称为执行器饱和。如下图:
多旋翼混控器将四个控制输入(滚动、俯仰、偏航、推力)组合成一组执行器输出,用于驱动电机速度控制器。代码位置:
unsigned
MultirotorMixer::mix(float *outputs, unsigned space)
{
如果最大输出通道数(通常为8)小于电机数,返回0.
if (space < _rotor_count) {
return 0;
}
从控制输入获取期望的roll,pitch,yaw力矩以及推力.roll,pitch,yaw分别取控制输入组0的第0,1,2个元素经过缩放系数缩放,再进行限幅,限制在-1到1之间.推力直接取控制输入组0的第3个元素,并限幅在0到1之间.
float roll = math::constrain(get_control(0, 0) * _roll_scale, -1.0f, 1.0f);
float pitch = math::constrain(get_control(0, 1) * _pitch_scale, -1.0f, 1.0f);
float yaw = math::constrain(get_control(0, 2) * _yaw_scale, -1.0f, 1.0f);
float thrust = math::constrain(get_control(0, 3), 0.0f, 1.0f);
电机饱和度清零
_saturation_status.value = 0;
电机饱和度成员变量的定义如下
switch (_airmode) { case Airmode::roll_pitch: mix_airmode_rp(roll, pitch, yaw, thrust, outputs); break; case Airmode::roll_pitch_yaw: mix_airmode_rpy(roll, pitch, yaw, thrust, outputs); break; case Airmode::disabled: default: // just in case: default to disabled mix_airmode_disabled(roll, pitch, yaw, thrust, outputs); break; }
上面一共有三种混控方式,对应不同的抗执行器饱和策略,roll_pitch和roll_pitch_yaw都属于使能Airmode的范畴,不同的是roll_pitch仅对横滚和俯仰进行抗执行器饱和处理,而偏航单独混控,roll_pitch_yaw则对横滚俯仰偏航均进行抗执行器饱和处理.disabled表示禁用Airmode.使能和禁用Airmode的原理前面已经叙述过.
应用以下推力模型,通过该推力模型,根据推力可以算出PWM值
thrust = (1 - _thrust_factor) * PWM _thrust_factor * PWM^2
移项并解一元二次方程算出PWM值如下
for (unsigned i = 0; i < _rotor_count; i ) {
if (_thrust_factor > 0.0f) {
outputs[i] = -(1.0f - _thrust_factor) / (2.0f * _thrust_factor) sqrtf((1.0f - _thrust_factor) *
(1.0f - _thrust_factor) / (4.0f * _thrust_factor * _thrust_factor) (outputs[i] < 0.0f ? 0.0f : outputs[i] /
_thrust_factor));
}
将PWM值缩放到怠速到1之间
outputs[i] = math::constrain(_idle_speed (outputs[i] * (1.0f - _idle_speed)), _idle_speed, 1.0f);
}
将最终的输出进行抗负向饱和以及变化速率限幅
定义标志位
for (unsigned i = 0; i < _rotor_count; i ) {
bool clipping_high = false;
bool clipping_low_roll_pitch = false;
bool clipping_low_yaw = false;
这里判断是否需要进行抗下限饱和,并对相关的标志位进行置位,具体的抗饱和操作根据设置在上面mix_airmode_rp
,mix_airmode_rpy
,mix_airmode_disabled
中进行。
if (outputs[i] < _idle_speed 0.01f) { if (_airmode == Airmode::disabled) { clipping_low_roll_pitch = true; clipping_low_yaw = true; } else if (_airmode == Airmode::roll_pitch) { clipping_low_yaw = 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_roll_pitch = true; clipping_low_yaw = true; } } _outputs_prev[i] = outputs[i];
更新状态标志位
update_saturation_status(i, clipping_high, clipping_low_roll_pitch, clipping_low_yaw);
}
将输出变化幅度的最大值置零,在每次进行混控前都要对该参数进行赋值,否则将不进行限幅
_delta_out_max = 0.0f;
返回
return _rotor_count;
}
文件位置
unsigned
HelicopterMixer::mix(float *outputs, unsigned space)
{
判断电机数和舵机数是否超过最大输出端口数,如果超过,则返回0。_mixer_info.control_count
是倾斜盘舵机数,加一是加上主螺旋桨的输出。
if (space < _mixer_info.control_count 1u) {
return 0;
}
根据设定的0%,25%, 50%,75%,100%油门/总矩曲线参数(在混控脚本中设置)计算当前期望推力下对应的油门/总矩值
获取期望推力值
float thrust_cmd = get_control(0, 3);
计算对应的推力区间系数
int idx = (thrust_cmd / 0.25f);
if (idx < 0) {
idx = 0;
} else if (idx > HELI_CURVES_NR_POINTS - 2) {
idx = HELI_CURVES_NR_POINTS - 2;
}
根据推力和油门-推力曲线参数计算油门
float tg = (_mixer_info.throttle_curve[idx 1] - _mixer_info.throttle_curve[idx]) / 0.25f;
float to = (_mixer_info.throttle_curve[idx]) - (tg * idx * 0.25f);
float throttle = constrain(2.0f * (tg * thrust_cmd to) - 1.0f, -1.0f, 1.0f);
根据推力和总矩-推力曲线参数计算油门计算总矩
float pg = (_mixer_info.pitch_curve[idx 1] - _mixer_info.pitch_curve[idx]) / 0.25f;
float po = (_mixer_info.pitch_curve[idx]) - (pg * idx * 0.25f);
float collective_pitch = constrain((pg * thrust_cmd po), -0.5f, 0.5f);
获取期望横滚/俯仰力矩
float roll_cmd = get_control(0, 0);
float pitch_cmd = get_control(0, 1);
将期望油门直接赋值给主旋翼电机
outputs[0] = throttle;
根据直升机动力学计算倾斜盘舵机输出,其中_mixer_info.servos[i].angle
为舵机安装角度,_mixer_info.servos[i].arm_length
为舵机臂长度,_mixer_info.servos[i].scale
为缩放因子,_mixer_info.servos[i].offset
为偏移量,都是从混控脚本里读取的。计算舵机输出也是根据安装角度和舵机臂长度长度来的。安装在0度和180度的舵机只能作用于俯仰角,安装于90度和270度的舵机只能作用于横滚角,此外舵机臂越长,所需的输出越大。
for (unsigned i = 0; i < _mixer_info.control_count; i ) {
outputs[i 1] = collective_pitch
cosf(_mixer_info.servos[i].angle) * pitch_cmd * _mixer_info.servos[i].arm_length
- sinf(_mixer_info.servos[i].angle) * roll_cmd * _mixer_info.servos[i].arm_length;
outputs[i 1] *= _mixer_info.servos[i].scale;
outputs[i 1] = _mixer_info.servos[i].offset;
outputs[i 1] = constrain(outputs[i 1], _mixer_info.servos[i].min_output, _mixer_info.servos[i].max_output);
}
返回
return _mixer_info.control_count 1;
}
unsigned
SimpleMixer::mix(float *outputs, unsigned space)
{
float sum = 0.0f;
if (_pinfo == nullptr) {
return 0;
}
如果可用的输出端口小于1,返回0
if (space < 1) {
return 0;
}
利用回调函数获取对应控制输入组中的成员的值input
。
for (unsigned i = 0; i < _pinfo->control_count; i ) {
float input = 0.0f;
_control_cb(_cb_handle,
_pinfo->controls[i].control_group,
_pinfo->controls[i].control_index,
input);
将input缩放并累加到sum
中
sum = scale(_pinfo->controls[i].scaler, input);
}
输出缩放,这里的缩放是对总的最终的输出作缩放,上面的缩放是对控制输入组中的成员进行缩放。
*outputs = scale(_pinfo->output_scaler, sum);
输出变化幅度限幅
if (_dt > FLT_EPSILON && _pinfo->slew_rate_rise_time > FLT_EPSILON) {
计算该周期内输出变化的最大幅度,因为变化范围在 [-1,1],所以要乘2
const float output_delta_max = 2.0f * _dt / _pinfo->slew_rate_rise_time;
限幅
float delta_out = *outputs - _output_prev;
if (delta_out > output_delta_max) {
*outputs = _output_prev output_delta_max;
} else if (delta_out < -output_delta_max) {
*outputs = _output_prev - output_delta_max;
}
}
将_dt置零,保证每次循环时都赋值_dt,否则将不进行输出变化幅度限值
_dt = 0.f;
返回
_output_prev = *outputs;
return 1;
}
代码位置
void output_limit_init(output_limit_t *limit)
{
limit->state = OUTPUT_LIMIT_STATE_INIT;
limit->time_armed = 0;
limit->ramp_up = true;
}
具体的计算PWM输出值
void output_limit_calc(const bool armed, const bool pre_armed, const unsigned num_channels, const uint16_t reverse_mask,
const uint16_t *disarmed_output, const uint16_t *min_output, const uint16_t *max_output,
const float *output, uint16_t *effective_output, output_limit_t *limit)
{
根据不同状态输出PWM,根据实际情况进行状态切换,将状态标志位limit->state
进行置位
共有以下四种状态
OUTPUT_LIMIT_STATE_INIT | 初始化状态 |
---|---|
OUTPUT_LIMIT_STATE_OFF | 锁定状态 |
OUTPUT_LIMIT_STATE_RAMP | 过渡状态 |
OUTPUT_LIMIT_STATE_ON | 开启状态 |
其中初始化状态和锁定状态下都将输出锁定值(一般为900us),过渡状态下PWM会从锁定值在一定时间内慢慢增加到最小值(例如1000us),过渡完成后,会进入开启状态,开启状态下可以在有效信号区间(如1000us至2000us)内直接进行输出。电调在上电后一般先给锁定值,然后电调会长叫一声,然后进入过渡状态,最后进入开启状态控制电机。如果电调上电后飞控直接输出有效信号或从锁定信号直接跳变到有效信号都会导致电调进入报警模式(急促“嘀嘀嘀”报警声) |
switch (limit->state) {
在初始状态下,经过INIT_TIME_US
后自动进入锁定状态
case OUTPUT_LIMIT_STATE_INIT: if (armed) { if (limit->time_armed == 0) { limit->time_armed = hrt_absolute_time(); } if (hrt_elapsed_time(&limit->time_armed) >= INIT_TIME_US) { limit->state = OUTPUT_LIMIT_STATE_OFF; } } break;
如果在锁定状态下解锁,则进入过渡状态,并把当前时间记为开始过渡时间
case OUTPUT_LIMIT_STATE_OFF: if (armed) { if (limit->ramp_up) { limit->state = OUTPUT_LIMIT_STATE_RAMP; } else { limit->state = OUTPUT_LIMIT_STATE_ON; } limit->time_armed = hrt_absolute_time(); } break;
如果在过渡状态下加锁,则进入锁定状态,否则在过渡完毕后进入开启状态
case OUTPUT_LIMIT_STATE_RAMP:
if (!armed) {
limit->state = OUTPUT_LIMIT_STATE_OFF;
} else if (hrt_elapsed_time(&limit->time_armed) >= RAMP_TIME_US) {
limit->state = OUTPUT_LIMIT_STATE_ON;
}
break;
在开启状态下如果加锁,则进入锁定状态
case OUTPUT_LIMIT_STATE_ON:
if (!armed) {
limit->state = OUTPUT_LIMIT_STATE_OFF;
}
break;
default:
break;
}
/* if the system is pre-armed, the limit state is temporarily on,
* as some outputs are valid and the non-valid outputs have been
* set to NaN. This is not stored in the state machine though,
* as the throttle channels need to go through the ramp at
* regular arming time.
*/
根据状态输出PWM
unsigned local_limit_state = limit->state;
if (pre_armed) {
local_limit_state = OUTPUT_LIMIT_STATE_ON;
}
unsigned progress;
锁定和初始状态下输出锁定值
switch (local_limit_state) {
case OUTPUT_LIMIT_STATE_OFF:
case OUTPUT_LIMIT_STATE_INIT:
for (unsigned i = 0; i < num_channels; i ) {
effective_output[i] = disarmed_output[i];
}
break;
过渡状态下根据过渡时间和最小输出值计算当前输出
case OUTPUT_LIMIT_STATE_RAMP: {
计算从过渡状态到现在的时间
hrt_abstime diff = hrt_elapsed_time(&limit->time_armed);
计算已经过去的时间与设定的过渡时间的比例,并乘以系数PROGRESS_INT_SCALING
progress = diff * PROGRESS_INT_SCALING / RAMP_TIME_US;
如果比例大于1,则置为1。
if (progress > PROGRESS_INT_SCALING) {
progress = PROGRESS_INT_SCALING;
}
不可用的输出置为锁定值
for (unsigned i = 0; i < num_channels; i ) {
float control_value = output[i];
if (!PX4_ISFINITE(control_value)) {
effective_output[i] = disarmed_output[i];
continue;
}
设置锁定值
uint16_t ramp_min_output;
if (disarmed_output[i] > 0) {
unsigned disarmed = disarmed_output[i];
如果锁定值大于最小输出值,则将最小输出值设为锁定值
if (disarmed > min_output[i]) {
disarmed = min_output[i];
}
计算最小输出和锁定置的差,并根据时间比例计算当前的过渡状态下的最小PWM值,这个最小PWM输出值就是在过渡时间内线性的从锁定值增加到最小值的值。
unsigned disarmed_min_diff = min_output[i] - disarmed;
ramp_min_output = disarmed (disarmed_min_diff * progress) / PROGRESS_INT_SCALING;
}
如果没有设置锁定值,直接输出最小值
else {
ramp_min_output = min_output[i];
}
如果反向标志位为1(通过参数设置),将输出反向
if (reverse_mask & (1 << i)) {
control_value = -1.0f * control_value;
}
根据当前控制量(-1到1)和最小PWM输出值以及最大PWM输出值计算最后的输出
effective_output[i] = control_value * (max_output[i] - ramp_min_output) / 2 (max_output[i] ramp_min_output) / 2;
防止无效的输出
if (effective_output[i] < ramp_min_output) {
effective_output[i] = ramp_min_output;
} else if (effective_output[i] > max_output[i]) {
effective_output[i] = max_output[i];
}
}
}
break;
在开启状态下,直接根据当前控制量control_value
(-1到1)和最小值min_output
以及最大值max_output
输出最终的PWM波,原理同过渡状态,只是最小值是固定的不随时间变化,输出的PWM范围在最小值到最大值之间。
case OUTPUT_LIMIT_STATE_ON: for (unsigned i = 0; i < num_channels; i ) { float control_value = output[i]; if (!PX4_ISFINITE(control_value)) { effective_output[i] = disarmed_output[i]; continue; } if (reverse_mask & (1 << i)) { control_value = -1.0f * control_value; } effective_output[i] = control_value * (max_output[i] - min_output[i]) / 2 (max_output[i] min_output[i]) / 2; if (effective_output[i] < min_output[i]) { effective_output[i] = min_output[i]; } else if (effective_output[i] > max_output[i]) { effective_output[i] = max_output[i]; } } break; default: break; } }
联系客服