目录
之前写了博客分析了一下旋翼姿态控制的基础知识:mc_att_control基础知识,这次就对照代码将整个旋翼姿态控制过程呈现一遍.先看一下整个程序的框图:
从图中可以看到,实际上整个控制分成内外两个环进行控制,外环的输入是位置控制产生的姿态角设定值 ,输出的是角速度设定值,内环根据输入的角速度设定值和陀螺仪测得的角速度值进行控制,产生最终给混控器的量.这个串级PID的控制流程如下图:
我们直接进入任务主进程函数task_mian(),首先是数据订阅:
/*
* do subscriptions
*/
_v_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));//飞机姿态
_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));//姿态设定值
_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
_v_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
_params_sub = orb_subscribe(ORB_ID(parameter_update));
_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
_motor_limits_sub = orb_subscribe(ORB_ID(multirotor_motor_limits));//电机转子的限制(判断是否饱和)
_battery_status_sub = orb_subscribe(ORB_ID(battery_status));
_gyro_count = math::min(orb_group_count(ORB_ID(sensor_gyro)), MAX_GYRO_COUNT);//需要订阅最多三组陀螺仪的数据
//orb_group_count():获取主题组的已发布实例的数量这里需要注意的是订阅了三组的陀螺仪的数据,然后选择偏差最小的陀螺仪.代码如下:
for (unsigned s = 0; s < _gyro_count; s++) {
_sensor_gyro_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
}
_sensor_correction_sub = orb_subscribe(ORB_ID(sensor_correction));
_sensor_bias_sub = orb_subscribe(ORB_ID(sensor_bias));
/* initialize parameters cache */
parameters_update();
/* wakeup source: gyro data from sensor selected by the sensor app *///所选择的陀螺仪数据变化,则唤醒进程
//陀螺仪的选择在sensor_correction.msg中有定义,选择误差小的陀螺仪数据我们看一下sensor_correction.msg:
uint8 selected_gyro_instance # gyro uORB topic instance for the voted sensor 选择的陀螺仪
uint8 selected_accel_instance # accelerometer uORB topic instance for the voted sensor
uint8 selected_baro_instance # barometric pressure uORB topic instance for the voted sensor后面就是轮询,开始我们的控制任务,这里我们主要分析外环和内环控制实现的两个函数:
control_attitude_rates(dt):通过选择数据最好的陀螺仪得到当前角速度信息,再和_rates_sp比较,通过PID+前馈控制,得到_att_control:即给混控器的四个u1,u2,u3,u4
内环的控制参考论文:High Performance Full Attitude Control of a Quadrotor on SO(3)
其主要思想是将整个旋转分成两部分:
vehicle_attitude_setpoint_poll();
_thrust_sp = _v_att_sp.thrust;
/* construct attitude setpoint rotation matrix */
math::Quaternion q_sp(_v_att_sp.q_d[0], _v_att_sp.q_d[1], _v_att_sp.q_d[2], _v_att_sp.q_d[3]);//期望姿态
math::Matrix<3, 3> R_sp = q_sp.to_dcm();//四元数转换成DCM矩阵(机体系到大地系(b-n))
/* get current rotation matrix from control state quaternions */
math::Quaternion q_att(_v_att.q[0], _v_att.q[1], _v_att.q[2], _v_att.q[3]);//当前姿态
math::Matrix<3, 3> R = q_att.to_dcm();//当前的DCM矩阵
/* all input data is ready, run controller itself */
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
/**
* @brief 先进行roll/pitch控制,再进行yaw控制,因为yaw的反应比它们慢
*控制的思路是:对齐当前的Z轴和期望的Z轴,然后绕Z轴旋转一个偏航角
*/
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));//当前的Z轴(左乘的时候只和坐标z相乘)
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));//期望的z轴
/* axis and sin(angle) of desired rotation (indexes: 0=pitch, 1=roll, 2=yaw).
* This is for roll/pitch only (tilt), e_R(2) is 0 */
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);//叉乘得到两个Z轴的误差,再转到机体系(最后的旋转轴是机体系Z轴)
/* calculate angle error */
/**
* @brief 这里的sin(tilt angle error)不直接用叉乘,而cos(tilt angle error)直接用点乘是因为叉乘得到的
* 是向量,这里注意DCM的单位正交性,所以||R_z||和||R_sp_z||都为1
*/
float e_R_z_sin = e_R.length(); // == sin(tilt angle error) length()函数求向量的模长 即|R_z|*|R_sp_z|sin(theta)
float e_R_z_cos = R_z * R_sp_z; // == cos(tilt angle error) == (R.transposed() * R_sp)(2, 2)我在代码中也做了很多注释,这段代码主要是取出当前姿态和期望姿态的Z轴,再利用点乘和叉乘得到Z轴间的差角 θ θ .我们利用轴角法得到误差,同时得到旋转轴和旋转角度,这样就可以得到罗德里格斯旋转了,得到对齐Z轴后的姿态角:
if (e_R_z_sin > 0.0f) {//角度在0-180度
/* get axis-angle representation *///轴角法表示
float e_R_z_angle = atan2f(e_R_z_sin, e_R_z_cos);//旋转角度
math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;//归一化,变为单位向量,为了求反对称矩阵(旋转轴)
e_R = e_R_z_axis * e_R_z_angle;//轴角法表示误差
/* cross product matrix for e_R_axis *///求反对称矩阵
// [0,-wz,wy;wz,0,-wx;-wy,wx,0]
math::Matrix<3, 3> e_R_cp;//反对乘矩阵
e_R_cp.zero();
e_R_cp(0, 1) = -e_R_z_axis(2);
e_R_cp(0, 2) = e_R_z_axis(1);
e_R_cp(1, 0) = e_R_z_axis(2);
e_R_cp(1, 2) = -e_R_z_axis(0);
e_R_cp(2, 0) = -e_R_z_axis(1);
e_R_cp(2, 1) = e_R_z_axis(0);
/* rotation matrix for roll/pitch only rotation */
//罗德里格斯旋转 这里是将R 进行了旋转,对齐了Z轴
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));//绕旋转轴旋转角度theta后,z轴对齐,只剩yaw了
} else {//如果误差不在0-180度,只进行yaw旋转
/* zero roll/pitch rotation */
R_rp = R;
}Z轴对齐后,我们就需要进行偏航的旋转,代码如下:
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));//期望的x轴
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));//旋转之后的x轴
/* calculate the weight for yaw control
* Make the weight depend on the tilt(倾斜) angle error: the higher the error of roll and/or pitch, the lower
* the weight that we use to control the yaw. This gives precedence to roll & pitch correction.
* The weight is 1 if there is no tilt error.
*/
/**
* 计算偏航控制的权重,tilt angle error越大,yaw权重就越小,这就是优先滚动和俯仰校正
* 这里用余弦表示是因为当tilt angle越大,余弦就越小,那么yaw_w就越小,也就是说pitch/roll的角度误差越大,对于偏航控制权重就越小
* 当没有pitch/roll偏差时,则可以进行完全的yaw控制了
*/
float yaw_w = e_R_z_cos * e_R_z_cos;//误差的平方(cos(tilt angel)) 点乘
/* calculate the angle between R_rp_x and R_sp_x (yaw angle error), and apply the yaw weight */
//计算yaw angle error
/**
yaw_w 就是对偏航转动的权重分量。如果 z 轴重合,yaw_w 就是 1,权重最大,也就是以
只有偏航转动,或者以其为主。随着 z 轴夹角增大,yaw 会两次方减小,降低偏航权重,使
得转动以俯仰滚转为主。
**/
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;//e_R(2):yaw e_R(0):pitch e_R(1):roll 轴角法 各轴的角度差
//这里%(叉乘之后)再*R_sp_z是因为叉乘得到向量,在点乘单位向量得到sin(theta),因为二者同方向
if (e_R_z_cos < 0.0f) {//角度在90度到270度
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
/**
* 大角度下直接算误差e_R_d
* 其中q_error.imag()得到误差角的一半
*/
math::Quaternion q_error;
q_error.from_dcm(R.transposed() * R_sp);
math::Vector<3> e_R_d = q_error(0) >= 0.0f ? q_error.imag() * 2.0f : -q_error.imag() * 2.0f;
/* use fusion of Z axis based rotation and direct rotation */
float direct_w = e_R_z_cos * e_R_z_cos * yaw_w;
e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;//互补滤波
}代码注释得很清楚,在yaw的控制中需要加入倾斜角误差决定的权重,最后内环的控制是一个P控制器加前馈控制,这里不用积分控制是因为积分控制有延迟,不利于这里的快速变化控制的实时性.控制器如下:
/**
外环是一个PF控制(比例前馈控制) 误差控制,也就是角度差的控制
**/
/* calculate angular rates setpoint */
_rates_sp = _params.att_p.emult(e_R);//att_p:P 增益 即att_p(0)*e_R(0) att_p(1)*e_R(1) att_p(2)*e_R(2)
/* Feed forward the yaw setpoint rate. We need to transform the yaw from world into body frame.
* The following is a simplification(简单化) of:
* R.transposed() * math::Vector<3>(0.f, 0.f, _v_att_sp.yaw_sp_move_rate * _params.yaw_ff)
*/
math::Vector<3> yaw_feedforward_rate(R(2, 0), R(2, 1), R(2, 2));//这里可以看上面的注释,相称之后只剩下R.transposed的最后一列(即R的最后一行)
yaw_feedforward_rate *= _v_att_sp.yaw_sp_move_rate * _params.yaw_ff;
//前馈等于自身乘以前馈系数
yaw_feedforward_rate(2) *= yaw_w;
_rates_sp += yaw_feedforward_rate;
外环控制相对比较简单,我将带有注释的代码放在这,主要是一个PID控制器加前馈控制:
/* reset integral if disarmed */
if (!_v_control_mode.flag_armed || !_vehicle_status.is_rotary_wing) {
_rates_int.zero();
}
// get the raw gyro data and correct for thermal errors
//获取原始陀螺仪数据并进行温度补偿
math::Vector<3> rates;
//从三个陀螺仪中选一个
if (_selected_gyro == 0) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_0[0]) * _sensor_correction.gyro_scale_0[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_0[1]) * _sensor_correction.gyro_scale_0[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_0[2]) * _sensor_correction.gyro_scale_0[2];
} else if (_selected_gyro == 1) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_1[0]) * _sensor_correction.gyro_scale_1[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_1[1]) * _sensor_correction.gyro_scale_1[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_1[2]) * _sensor_correction.gyro_scale_1[2];
} else if (_selected_gyro == 2) {
rates(0) = (_sensor_gyro.x - _sensor_correction.gyro_offset_2[0]) * _sensor_correction.gyro_scale_2[0];
rates(1) = (_sensor_gyro.y - _sensor_correction.gyro_offset_2[1]) * _sensor_correction.gyro_scale_2[1];
rates(2) = (_sensor_gyro.z - _sensor_correction.gyro_offset_2[2]) * _sensor_correction.gyro_scale_2[2];
} else {
rates(0) = _sensor_gyro.x;
rates(1) = _sensor_gyro.y;
rates(2) = _sensor_gyro.z;
}
// rotate corrected measurements from sensor to body frame
//将传感器测得的值旋转到机体坐标系
rates = _board_rotation * rates;//rates:陀螺仪测得的三轴角速率
// correct for in-run bias errors 纠正运行中偏差错误
rates(0) -= _sensor_bias.gyro_x_bias;
rates(1) -= _sensor_bias.gyro_y_bias;
rates(2) -= _sensor_bias.gyro_z_bias;
//note:1.6.4版本代码中没有对I项进行TPA操作,但是1.7.4中则进行了TPA操作
math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_p));//P增益衰减
//math::Vector<3> rates_i_scaled = _params.rate_i.emult(pid_attenuations(_params.tpa_breakpoint_i, _params.tpa_rate_i));
math::Vector<3> rates_d_scaled = _params.rate_d.emult(pid_attenuations(_params.tpa_breakpoint_d, _params.tpa_rate_d));//
/* angular rates error */
math::Vector<3> rates_err = _rates_sp - rates;
//PID加前馈控制
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int +
rates_d_scaled.emult(_rates_prev - rates) / dt +
_params.rate_ff.emult(_rates_sp);
//记录下期望角速度和当前角速度
_rates_sp_prev = _rates_sp;
_rates_prev = rates;
/* update integral only if motors are providing enough thrust to be effective */
//只有电机提供足够的推力才能生效，更新积分(保证开始姿态控制,怠速是不改变姿态的)
if (_thrust_sp > MIN_TAKEOFF_THRUST) {
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
// Check for positive control saturation 检测正向是否饱和
bool positive_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_pos) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_pos) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_pos);
// Check for negative control saturation 检测反向是否饱和
bool negative_saturation =
((i == AXIS_INDEX_ROLL) && _saturation_status.flags.roll_neg) ||
((i == AXIS_INDEX_PITCH) && _saturation_status.flags.pitch_neg) ||
((i == AXIS_INDEX_YAW) && _saturation_status.flags.yaw_neg);
// prevent further positive control saturation
if (positive_saturation) {
rates_err(i) = math::min(rates_err(i), 0.0f);
}
// prevent further negative control saturation
if (negative_saturation) {
rates_err(i) = math::max(rates_err(i), 0.0f);
}
// Perform the integration using a first order method and do not propaate the result if out of range or invalid
//使用一阶方法执行积分，如果超出范围或无效，则不要预测结果 实际上就是为了防止积分饱和
//当某一方向饱和时,rates_err(i)为0,不再进行积分分误差累计 一阶是指dt
float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
if (PX4_ISFINITE(rate_i) && rate_i > -_params.rate_int_lim(i) && rate_i < _params.rate_int_lim(i)) {
_rates_int(i) = rate_i;
}
}
}
/* explicitly limit the integrator state *///限制积分器的状态
for (int i = AXIS_INDEX_ROLL; i < AXIS_COUNT; i++) {
_rates_int(i) = math::constrain(_rates_int(i), -_params.rate_int_lim(i), _params.rate_int_lim(i));
}