参考文献:Minimum Snap Trajectory Generation and Control for Quadrotors
PX4中多旋翼无人机的控制分为姿态控制和位置控制两个部分。
大致解读如下
姿态控制 mc_att_control
![attc](https://i-blog.csdnimg.cn/blog_migrate/63acc142b9decc81c376af4f401b8581.png)
角度控制(外环 P)
/**
* Attitude controller.
* 姿态角控制 外环 P
* Input: 'vehicle_attitude_setpoint' topics (depending on mode)
* 输入: 姿态设定值'vehicle_attitude_setpoint'
* Output: '_rates_sp' vector, '_thrust_sp'
* 输出: 角速度设定值 '_rates_sp' 油门设定值 '_thrust_sp'
*/
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
由于偏航轴的响应比横滚俯仰慢,这里采用的是倾转分离的解耦合进行姿态角控制:
先进行一个倾斜(对齐Z轴),然后旋转(对齐X轴),这样以保证能够将当前机体坐标系迅速地旋转到目标坐标系,通过这种方式求解旋转向量(详见《视觉SLAM十四讲》)仅适用于小角度情形;
![small](https://i-blog.csdnimg.cn/blog_migrate/d2b0ea767c9ba37389f35bcad237e482.png)
对于旋转角度大于90度的情况(cos(z,Z)<0
)时,直接将目标姿态确定的四元数虚部(旋转瞬轴)作为现在旋转向量。
进而计算角速度设定值
_rates_sp = _params.att_p.emult(e_R); // P控制
- 1
由于偏航响应较慢,加入前馈控制
_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff; // 偏航前馈
- 1
其中主要是旋转向量e_R
的求取
/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
// 以最短距离移动推力向量
math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
/* axis and sin(angle) of desired rotation */
// 旋转向量(轴 角)
math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z); // 向量叉乘即误差
/* calculate angle error */
float e_R_z_sin = e_R.length(); // R_z 和 R_sp_z皆为单位向量, |x|=1
float e_R_z_cos = R_z * R_sp_z;
/* calculate weight for yaw control */
float yaw_w = R_sp(2, 2) * R_sp(2, 2);
/* calculate rotation matrix after roll/pitch only rotation */
math::Matrix<3, 3> R_rp;
if (e_R_z_sin > 0.0f) {
/* 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; // 旋转向量: 长度为 theta角大小
/* cross product matrix for e_R_axis */
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 = cos(theta) * I + (1 - cos(theta)) * n * n^T + sin(theta) * skewmatrix(n)
R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
} else {
/* zero roll/pitch rotation */
R_rp = R;
}
////////////// 对齐X轴
/* R_rp and R_sp has the same Z axis, calculate yaw error */
math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w; //tan = sin / cos
//////////////////////// 大角度旋转 ///////////////////////
if (e_R_z_cos < 0.0f) {
/* for large thrust vector rotations use another rotation method:
* calculate angle and axis for R -> R_sp rotation directly */
// 大的推力向量:直接计算 R->R_sp 的旋转向量(轴、角)
math::Quaternion q_error;
// 四元数组成为旋转轴 转过的角度
q_error.from_dcm(R.transposed() * R_sp);
// 四元数实部为 cos(theta) 虚部为 u_x*sin(theta) u_y*sin u_z*sin
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 */
// 使用基于Z轴的旋转和直接旋转的融合
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;
}
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
- 9
- 10
- 11
- 12
- 13
- 14
- 15
- 16
- 17
- 18
- 19
- 20
- 21
- 22
- 23
- 24
- 25
- 26
- 27
- 28
- 29
- 30
- 31
- 32
- 33
- 34
- 35
- 36
- 37
- 38
- 39
- 40
- 41
- 42
- 43
- 44
- 45
- 46
- 47
- 48
- 49
- 50
- 51
- 52
- 53
- 54
- 55
- 56
- 57
- 58
- 59
- 60
- 61
- 62
- 63
- 64
- 65
- 66
- 67
- 68
- 69
角速度控制(内环PID)
/*
* Attitude rates controller.
* 角速度控制环 内环 PID
* Input: '_rates_sp' vector, '_thrust_sp'
* 输入: 角速度设定值'_rates_sp' 推力设定值 '_thrust_sp'
* Output: '_att_control' vector
* 输出: 姿态控制量 '_att_control'
*/
- 1
- 2
- 3
- 4
- 5
- 6
- 7
- 8
角速度控制过程比较直观,需要注意的是一个系数tpa(Throttle PID Attenuation,油门PID衰减)
。
/* throttle pid attenuation factor */
float tpa = fmaxf(0.0f, fminf(1.0f, 1.0f - _params.tpa_slope * (fabsf(_v_rates_sp.thrust) - _params.tpa_breakpoint)));
// 衰减因子范围在 0 ~ 1 之间
- 1
- 2
- 3
在得到角速度误差后
/* angular rates error */
// 角速度误差
math::Vector<3> rates_err = _rates_sp - rates;
- 1
- 2
- 3
进行角速度PID控制(加前馈)
_att_control = _params.rate_p.emult(rates_err * tpa) + _params.rate_d.emult(_rates_prev - rates) / dt + _rates_int +
_params.rate_ff.emult(_rates_sp);
- 1
- 2
这里有几点需要注意的:
微分控制环节的误差量选取
标准的PID微分项D=Kd×(e(k)−e(k−1))
可以增强稳定性。
问:串级PID外环可以只用PI吗?
答:可以,因为内环相当于外环的D。
TPA的作用
TPA在此处使用是考虑到了抗积分饱和。
当系统存在一个方向的误差时,由于积分的累加作用会使控制量一直增大,可能会使控制量达到执行器的执行阈值,如果此时误差的方向发生改变,控制量会逐渐减小,控制量也会退出饱和区,执行器也会在阈值内执行;如果此时误差方向还是没有改变,控制量会继续增大但是执行器会一直保持在阈值,此时控制量就进入了饱和区。进入饱和区越深,退出饱和区时间就会越长,在饱和区时执行器会一直在阈值位置,如果误差发生反向,执行器不会立刻有反应,控制量会慢慢减小,等执行器推出饱和区才会有反应。这样就会使控制的动态响应变差,控制性能变差。
抗积分饱和算法
若 u(k-1)>Umax,只累加负的误差;若u(k-1)
位置控制mc_pos_control
位置控制同样采用的是串级PID控制。外环为位置P控制,内环为速度PID控制。
整体控制流程入下。
![mc_pos_ctrl](https://i-blog.csdnimg.cn/blog_migrate/cf3d268fb3d20456836c186e239c359e.png)
保方向饱和