PX4姿态控制源码分析_px4位姿估计源码

写在前面

源码版本：1.6.0rc1
源码位置：Firmware-1.6.0rc1\src\modules\mc_att_control\mc_att_control_main.cpp

PX4姿态控制源码分析

从功能函数执行开始task_main()

整体框架

1. 订阅数据（期望姿态、当前姿态、默认参数拷贝）
2. 外环处理
3. 内环处理
4. 把控制量进行发布

void MulticopterAttitudeControl::task_main()
{
	_v_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
	_v_rates_sp_sub = orb_subscribe(ORB_ID(vehicle_rates_setpoint));
	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
	_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));
	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
	_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);
	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));
	/* initialize parameters cache */
 	parameters_update();
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20

任务开始执行会订阅（subscribe）一些可能用到的数据，以备后面姿态控制使用。最后一行将同路径下参数文件（mc_att_control_params.c）中参数拷贝出来。

 /* wakeup source: gyro data from sensor selected by the sensor app */
 px4_pollfd_struct_t poll_fds = {};
 poll_fds.fd = _sensor_gyro_sub[_selected_gyro];
 poll_fds.events = POLLIN;
1
2
3
4

定义对_sensor_gyro_sub事件进行阻塞等待。

while (!_task_should_exit) {
		/* wait for up to 100ms for data */
		int pret = px4_poll(&poll_fds, 1, 100);

		/* timed out - periodic check for _task_should_exit */
		if (pret == 0) {
			continue;
		}

		/* this is undesirable but not much we can do - might want to flag unhappy status */
		if (pret < 0) {
			warn("mc att ctrl: poll error %d, %d", pret, errno);
			/* sleep a bit before next try */
			usleep(100000);
			continue;
		}

		perf_begin(_loop_perf);//空语句为了兼容ROS

		/* run controller on gyro changes */
		if (poll_fds.revents & POLLIN) {
			static uint64_t last_run = 0;
			float dt = (hrt_absolute_time() - last_run) / 1000000.0f;
			last_run = hrt_absolute_time();

			/* guard against too small (< 2ms) and too large (> 20ms) dt's */
			if (dt < 0.002f) {
				dt = 0.002f;

			} else if (dt > 0.02f) {
				dt = 0.02f;
			}
			/* copy gyro data */
			orb_copy(ORB_ID(sensor_gyro), _sensor_gyro_sub[_selected_gyro], &_sensor_gyro);
			
			/* check for updates in other topics */
			parameter_update_poll();
			vehicle_control_mode_poll();
			arming_status_poll();
			vehicle_manual_poll();
			vehicle_status_poll();
			vehicle_motor_limits_poll();
			battery_status_poll();
			control_state_poll();
			sensor_correction_poll();
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

对需要做阻塞等待的数据进行阻塞等待，通过高精度定时器获取积分时间dt，对其他数据进行检查更新。

以上就是在获取数据。

/* Check if we are in rattitude mode and the pilot is above the threshold on pitch
			 * or roll (yaw can rotate 360 in normal att control).  If both are true don't
			 * even bother running the attitude controllers */
		//判断是否在半自稳模式下 小舵量自稳 大舵量特技
			if (_v_control_mode.flag_control_rattitude_enabled) {
				//舵量判断（只有pitch roll）大舵量只控制角速度
				if (fabsf(_manual_control_sp.y) > _params.rattitude_thres ||
				    fabsf(_manual_control_sp.x) > _params.rattitude_thres) {
				    //如果是半自稳模式 将不再进行外环处理 直接进入内环角速度控制
					_v_control_mode.flag_control_attitude_enabled = false;
				}
			}
			if (_v_control_mode.flag_control_attitude_enabled) {

				if (_ts_opt_recovery == nullptr) {
					// the  tailsitter recovery instance has not been created, thus, the vehicle
					// is not a tailsitter, do normal attitude control
					control_attitude(dt);//姿态结算的第二步，外环处理

				} else {
					vehicle_attitude_setpoint_poll();
					_thrust_sp = _v_att_sp.thrust;
					math::Quaternion q(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
					math::Quaternion q_sp(&_v_att_sp.q_d[0]);
					_ts_opt_recovery->setAttGains(_params.att_p, _params.yaw_ff);
					_ts_opt_recovery->calcOptimalRates(q, q_sp, _v_att_sp.yaw_sp_move_rate, _rates_sp);

					/* limit rates */
					for (int i = 0; i < 3; i++) {
						_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i),_params.mc_rate_max(i));
					}
				}
				/* publish attitude rates setpoint */
				_v_rates_sp.roll = _rates_sp(0);
				_v_rates_sp.pitch = _rates_sp(1);
				_v_rates_sp.yaw = _rates_sp(2);
				_v_rates_sp.thrust = _thrust_sp;
				_v_rates_sp.timestamp = hrt_absolute_time();
				
				if (_v_rates_sp_pub != nullptr) {
					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

				} else if (_rates_sp_id) {
					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
				}
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

手动模式（manual）可以化为三种：
1. stabilize（遥感舵量转换出来的是角度控制指令，遥感转换出来的是角度数据）
2. acro（遥感舵量转换出来的是角速度指令，只控制角速度，特技飞行）
3. rattitude（半自稳模式，是上面两种模式的综合，舵量小为1. 舵量大为2. 小舵量自稳 大舵量特技）

如果使能了姿态控制，并且不需要垂直起降则进入最重要的外环控制（control_attitude(dt)），先不跟进他先把整体框架捋顺清晰。我们不要垂直起降，先不看else里的东西。再往后看是对期望角速度（_rates_sp）的限幅，对期望角速度、期望油门填充并发布，以便后面内环控制使用。

} else {
			/* attitude controller disabled, poll rates setpoint topic */
			if (_v_control_mode.flag_control_manual_enabled) {//如果是ACRO模式（特技飞行不控制角度）
				/* manual rates control - ACRO mode */
				_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x,
							    _manual_control_sp.r).emult(_params.acro_rate_max);
				_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);

				/* publish attitude rates setpoint */
				//角速度期望值来源于遥感舵量
				_v_rates_sp.roll = _rates_sp(0);
				_v_rates_sp.pitch = _rates_sp(1);
				_v_rates_sp.yaw = _rates_sp(2);
				_v_rates_sp.thrust = _thrust_sp;
				_v_rates_sp.timestamp = hrt_absolute_time();

				if (_v_rates_sp_pub != nullptr) {
					orb_publish(_rates_sp_id, _v_rates_sp_pub, &_v_rates_sp);

				} else if (_rates_sp_id) {
					_v_rates_sp_pub = orb_advertise(_rates_sp_id, &_v_rates_sp);
				}

			} else {
				/* attitude controller disabled, poll rates setpoint topic */
				vehicle_rates_setpoint_poll();
				_rates_sp(0) = _v_rates_sp.roll;
				_rates_sp(1) = _v_rates_sp.pitch;
				_rates_sp(2) = _v_rates_sp.yaw;
				_thrust_sp = _v_rates_sp.thrust;
			}
		}
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

这一块是非正常的姿态控制外环，在使用acro模式下，因为不做角度控制，所以期望角速度来源于遥感舵量。

if (_v_control_mode.flag_control_rates_enabled) {
	control_attitude_rates(dt);
1
2

内环的计算（control_attitude_rates(dt)）得到控制量。后面就是对获取的控制量进行发布，这里不再粘贴了，下面进入最重要的环节，姿态结算的外环，内环函数。

/*******************************************************************************************************************************************/
先看外环 control_attitude(dt)，内环主要用到的就是解耦合控制和罗德里的旋转。

void MulticopterAttitudeControl::control_attitude(float dt)
{
	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();//R_sp:期望旋转矩阵
	
	/* get current rotation matrix from control state quaternions */
	//获取当前姿态
	math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);//当前四元数
	math::Matrix<3, 3> R = q_att.to_dcm();//R:当前旋转矩阵
1
2
3
4
5
6
7
8
9
10
11
12
13
14

以上就找到了期望姿态与当前姿态的矩阵表示形式。现在要做的就是如何由当前的姿态旋转到期望姿态。
R_sp = Ryaw Rrollpitch R ：先旋转roll pitch（对齐z轴） 再旋转yaw（对齐xy轴）， 因为roll pitch 要比yaw响应快。

/* 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));//R_z:当前姿态z轴
	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));//R_sp_z:期望姿态z轴
1
2
3

	/* axis and sin(angle) of desired rotation*/
	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);//e_R:机体坐标系下旋转轴向量
1
2

用叉乘找到两个z轴之间的误差，因为两个z轴是基于导航坐标系的，但是飞机的旋转是基于机体系的，所以要把他们的叉乘旋转到机体坐标系上。叉乘的结果是一个向量，这个向量是垂直于两个z轴所在的平面的。所以叉乘结果的向量是可以作为旋转轴的。（我们要先对齐z轴）

/* calculate angle error */
	//两z轴误差角度正弦e_R = |a||b|sinθ  
	float e_R_z_sin = e_R.length();
	//两z轴误差角度余弦cos = |a||b|cosθ    
	float e_R_z_cos = R_z * R_sp_z;
1
2
3
4
5

向量e_R是叉乘得到的所以其大小就是 a b sinθ ，点乘得到 a b cosθ。

	/* 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;//定义roll/pitch旋转矩阵
	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);//得到对齐z轴需要的旋转角度
		//因为e_R_z_sin = e_R.length()，所以，e_R/e_R_z_sin是对e_R进行归一化作为旋转轴
		math::Vector<3> e_R_z_axis = e_R / e_R_z_sin;
		
		e_R = e_R_z_axis * e_R_z_angle;//航偏误差（轴角法）
1
2
3
4
5
6
7
8
9
10
11
12
13
14

yaw_w，R_sp(2,2)表示两个 z 轴 之间夹角的余弦，当两个 z 轴的夹角越大时对偏航角度的影响也越大。

e_R用旋转轴乘以旋转角，就表示两个旋转轴的误差。

/* 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);
1
2
3
4
5
6
7
8
9

用旋转轴构造旋转轴叉积矩阵

	/* rotation matrix for roll/pitch only rotation
	 * 由当前旋转矩阵R、(rx ryrz)、θ经过罗德里格变换得
	 * 到只重合z轴所需的旋转矩阵R_rp                   */ 
	R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
	
} else {//对应上面if (e_R_z_sin > 0.0f)
	/* zero roll/pitch rotation */
	R_rp = R;
}
1
2
3
4
5
6
7
8
9

利用罗德里的旋转公式求出R_rp：对齐z轴的中间过渡姿态。

/* 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;
1
2
3
4

这时候，对齐z轴之后R_sp_x与R_rp_x只差一个偏航角了。

(R_rp_x % R_sp_x) * R_sp_z 这是求 sin。
R_rp_x % R_sp_x求出来是个向量，大小是sin但是还有方向呢，方向就是R_sp_z。 * R_sp_z 是把前面的向量转换为数值。可以算算这个点乘，单位向量，同方向 cos=1，最后剩下的就是仅仅是我们想要的 sin 数值了。

R_rp_x * R_sp_x 求 cos。 atan2f （sin，cos）求偏航角。

最后乘以yaw_w，yaw_w 在先旋转 roll pitch 对齐 z 轴的过程中，对偏航的影响。float yaw_w = R_sp(2, 2) * R_sp(2, 2)。yaw 权重随着 z 轴夹角增大而二次方减小，所以如 果 z 轴夹角越大，就会更加偏向于先通过转动将 z 轴夹角减小。在 z 轴夹角减小后，倾向于偏航转动使 x 轴重合。

如两个 z 轴重合 yaw_w=1，e_R(2) 就是算出来的，相当于转动过程对偏航没 有影响，这种情况确实没有影响。如果如两个 z 轴 90 度 yaw_w=0 其实 e_R(2) 偏航=0， 就是这种情况下根本不用偏航了。 yaw_w 就是对偏航转动的权重分量。如果 z 轴重合，yaw_w 就是 1，权重最大，也就是以 只有偏航转动，或者以其为主。随着 z 轴夹角增大，yaw 会两次方减小，降低偏航权重，使得转动以俯仰滚转为主。

这是小角度的姿态控制过程，小角度指在 90 度以内。如果大角度 90 度以外呢？

if (e_R_z_cos < 0.0f) {//大于 90 度，飞机立起来了，大角度姿态控制
		/* for large thrust vector rotations use another rotation method:
		 * calculate angle and axis for R -> R_sp rotation directly */
		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 
		  *使用基于Z轴旋转和直接旋转的融合*/
		//R_sp(2,2)其实就是就是 XY 两个轴需要旋转的角度的 cos，
		//XY 需要转的越多，那么这个权重就越小，意思就是当 XY 转角较大的时候，yaw 的控制就适当减弱一下。
		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

e_R_z_cos < 0.0f 大于 90 度，飞机立起来了，大角度姿态控制。现在大角度下直接求现在姿态和期望姿态之间的差值，并用四元数进行表示。
q_error(0)就是 cosθ/2，q_error(0)姿态偏差在 90-180 度
q_error.imag() * 2.0f 求姿态角度差
e_R_d 最终算出来就是现在姿态和期望姿态间的直接差值，不加修饰。

float direct_w = e_R_z_cos * e_R_z_cos * yaw_w; //yaw_w = R_sp(2,2)*R_sp(2,2)。
R_sp(2,2)其实就是就是 XY 两个轴需要旋转的角度的 cos，XY 需要转的越多，那么这个权重就越小，意思就是当 XY 转角较大的时候，yaw 的控制就适当减弱一下。

这种平方的权重，当误差很大的时候，如果>1 平方就会更大，补偿更快。比如 飞机遇到风的时候要求快速的补偿误差。当误差很小的手，如<1，平方会衰减的 更快，就是当误差越小的时候我们需要弥补的越少，甚至可以不用弥补了免得浪 费资源。就如同偏航一样，我们不希望飞机机头转的太快。

0>e_R_z_cos>-1，代表两个 z 轴之间的偏差，在 90-180 度之间。随着 z 轴之间的角度变大，e_R_z_cos * e_R_z_cos 为正也变大， e_R = e_R * (1.0f - direct_w) + e_R_d * direct_w;最终计算的姿态误差中 e_R_d 大角度计算的这个偏差所占比重 direct_w = e_R_z_cos * e_R_z_cos * yaw_w 也变大。

就是两个 z 轴之间角度越大，大角度姿态控制下，最终姿态偏差越相信 e_R_d。

大角度的情况下，直接就算角度，直接控制迅速点，不讲究稳定先修正把角度拉 过去，不需要体轴那么算了，所以求的直接是角度差，先求 q_error，这个就是两个旋转矩阵之间差的四元数，这个四元数的意义本身就是旋转，然后四元数的 虚部，imag 部分，代表的 0.5*旋转角度，这里可以看看四元数定义，然后就不管什么体轴地轴，先往 R_sp 的方向转那么多角度再说。最后算出大角度下的 姿态误差，产生期望角速度。

_rates_sp = _params.att_p.emult(e_R);
1

最终作用外环计算产生期望的角速度，即角度有速度肯定要有角速度去弥补。而 且误差越大期望弥补的角速度越大。

	/* limit rates */
	for (int i = 0; i < 3; i++) {
		if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
		    !_v_control_mode.flag_control_manual_enabled) {
			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.auto_rate_max(i), _params.auto_rate_max(i));

		} else {
			_rates_sp(i) = math::constrain(_rates_sp(i), -_params.mc_rate_max(i), _params.mc_rate_max(i));
		}
	}

	/* feed forward yaw setpoint rate */
	//前馈的作用会加速偏航的响应
	//因为飞机能提供的偏航的力矩比较小
	_rates_sp(2) += _v_att_sp.yaw_sp_move_rate * yaw_w * _params.yaw_ff;
	
	/* weather-vane mode, dampen yaw rate */
	if ((_v_control_mode.flag_control_velocity_enabled || _v_control_mode.flag_control_auto_enabled) &&
	    _v_att_sp.disable_mc_yaw_control == true && !_v_control_mode.flag_control_manual_enabled) {
		float wv_yaw_rate_max = _params.auto_rate_max(2) * _params.vtol_wv_yaw_rate_scale;
		_rates_sp(2) = math::constrain(_rates_sp(2), -wv_yaw_rate_max, wv_yaw_rate_max);
		// prevent integrator winding up in weathervane mode
		_rates_int(2) = 0.0f;
	}
}
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

对期望角速度做一个限幅，再加入前馈控制，前馈的作用有两个，

1. 让控制更加跟手，在你打航向的时候，输入误差会增大很多，这样 输出到下一环的控制量是变大了，所以控制会更快
2. 增加抗风性

风吹飞机一般都是小角度，飞机自稳的时候一般杆量也是 0，这些情况下，都能增加下一环的控制输入，让控制更快。四旋翼航向控制优先级是最弱的，所以增加一些前馈，对航向控制有很大帮助，而 roll/pitch 不加是因为基本上串级 PID 都够了。

/*******************************************************************************************************************************************/
再看姿态内环的处理control_attitude_rates(dt)

void
MulticopterAttitudeControl::control_attitude_rates(float dt)
{
	/* reset integral if disarmed */
	if (!_armed.armed || !_vehicle_status.is_rotary_wing) {
		_rates_int.zero();
	}

	/* get transformation matrix from sensor/board to body frame */
	//得到传感器/板到机体系的变换矩阵
	get_rot_matrix((enum Rotation)_params.board_rotation, &_board_rotation);

	/* fine tune the rotation */
	//微调旋转
	math::Matrix<3, 3> board_rotation_offset;
	board_rotation_offset.from_euler(M_DEG_TO_RAD_F * _params.board_offset[0],
					 M_DEG_TO_RAD_F * _params.board_offset[1],
					 M_DEG_TO_RAD_F * _params.board_offset[2]);
	_board_rotation = board_rotation_offset * _board_rotation;

	// get the raw gyro data and correct for thermal errors
	//获取原始陀螺数据并校正热误差
	math::Vector<3> rates(_sensor_gyro.x * _sensor_correction.gyro_scale[0] + _sensor_correction.gyro_offset[0],
			      _sensor_gyro.y * _sensor_correction.gyro_scale[1] + _sensor_correction.gyro_offset[1],
			      _sensor_gyro.z * _sensor_correction.gyro_scale[2] + _sensor_correction.gyro_offset[2]);

	// rotate corrected measurements from sensor to body frame
	//旋转修正测量从传感器到机体系
	rates = _board_rotation * rates;

	// correct for in-run bias errors
	rates(0) -= _ctrl_state.roll_rate_bias;
	rates(1) -= _ctrl_state.pitch_rate_bias;
	rates(2) -= _ctrl_state.yaw_rate_bias;

	math::Vector<3> rates_p_scaled = _params.rate_p.emult(pid_attenuations(_params.tpa_breakpoint_p, _params.tpa_rate_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));

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

上面这些都是在计算当前角速度，tpa 的功能类似于一个简单的经验型的非线性 PID，就是根据油门大小调节 P 项的输出，让 控制更加符合心理预期。

/* angular rates error */
	math::Vector<3> rates_err = _rates_sp - rates;

	_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;
1
2
3
4
5
6
7
8
9
10

内环的PID计算，并加上了前馈控制使控制更平滑一些。

	/* 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     使用一阶方法执行积分，如果超出范围或无效，则不要传播结果
			//积分项
			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));
	}
}//end control_attitude_rates(dt)

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

PID 计算过程中最麻烦的是积分项的处理，防止进入饱和区，进去饱和区越深反向退出的时 候越慢，会导致飞机外在反映的迟钝。抗积分饱和，当前输出没有达到饱和时，才把本次的误差积分项累加到积分环节中。

最终内环的输出量就是_att_control这个变量，到这里内环就结束了，跳出内环对就是数据进行判断、填充、发布，姿态控制也就结束了。

总结：整体框架

1. 订阅数据（期望姿态、当前姿态、默认参数拷贝）
2. 外环处理（ control_attitude(dt)）
3. 内环处理（control_attitude_rates(dt)）
4. 把控制量（_att_control）进行发布

以上是个人对PX4姿态控制的源码理解，如有不正确的地方感谢提出批评指正，欢迎一起讨论学习QQ：1103706199。