PX4旋翼位置控制

最新推荐文章于 2024-03-24 11:55:59 发布

zhao23333

最新推荐文章于 2024-03-24 11:55:59 发布

阅读量1.1k

点赞数

分类专栏： PX4 文章标签： PX4 C++ Pixhawk

本文链接：https://blog.csdn.net/zhao23333/article/details/103280293

版权

PX4 专栏收录该内容

18 篇文章 8 订阅

订阅专栏

PX4旋翼位置控制

PX4 源码版本　v1.9.0
源码位置：mc_pos_control–multirotor：位置控制器

1. 概括

多悬翼的位置控制由内外环控制，外环P控制作用于位置差，产生期望速度，内环PID作用于速度差，产生期望推力

2. 符号和函数说明

2.1 符号说明

**_sp：期望值
**_dot：微分值
_param_mpc_xy_p：位置控制得到的Ｐ增益
_param_mpc_z_p：位置控制姿态比例增
_param_mpc_xy_vel_max：xy方向最大速度限制
param_mpc*_vel_p：*方向速度P增益
param_mpc*_vel_i：*方向速度i增益
param_mpc*_vel_d：*方向速度d增益

2.2 函数说明

constrain函数用于限定作用，限定规则：return (val < min_val) ? min_val : ((val > max_val) ? max_val : val);

3. 代码解析

3.1 外环位置控制(_positionController())

void PositionControl::_positionController()
{
	// P-position controller
    // 外环P控制，作用于位置差
	const Vector3f vel_sp_position = (_pos_sp - _pos).emult(Vector3f(_param_mpc_xy_p.get(), _param_mpc_xy_p.get(),
					 _param_mpc_z_p.get()));
	_vel_sp = vel_sp_position + _vel_sp;

	// Constrain horizontal velocity by prioritizing the velocity component along the
	// the desired position setpoint over the feed-forward term.
    // 这里的Vector2s是将向量前两个量拿出来了，然后有一个最大速度限制
	const Vector2f vel_sp_xy = ControlMath::constrainXY(Vector2f(vel_sp_position),
				   Vector2f(_vel_sp - vel_sp_position), _param_mpc_xy_vel_max.get());
	_vel_sp(0) = vel_sp_xy(0);
	_vel_sp(1) = vel_sp_xy(1);
	// Constrain velocity in z-direction.
	//　对z方向速度进行限制
	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

这个函数最终输出三个方向上面的期望速度_vel_sp(0),_vel_sp(1),_vel_sp(2).

3.2 内环速度控制(_velocityController(const float &dt))

首先是看一下下面这个注释

// Generate desired thrust setpoint.
// PID
// u_des = P(vel_err) + D(vel_err_dot) + I(vel_integral)
// Umin <= u_des <= Umax
//
// Anti-Windup:
// u_des = _thr_sp; r = _vel_sp; y = _vel
// u_des >= Umax and r - y >= 0 => Saturation = true
// u_des >= Umax and r - y <= 0 => Saturation = false
// u_des <= Umin and r - y <= 0 => Saturation = true
// u_des <= Umin and r - y >= 0 => Saturation = false
//
// Notes:
// - PID implementation is in NED-frame
// - control output in D-direction has priority over NE-direction
// - the equilibrium point for the PID is at hover-thrust
// - the maximum tilt cannot exceed 90 degrees. This means that it is
// not possible to have a desired thrust direction pointing in the positive
// D-direction (= downward)
// - the desired thrust in D-direction is limited by the thrust limits
// - the desired thrust in NE-direction is limited by the thrust excess after
// consideration of the desired thrust in D-direction. In addition, the thrust in
// NE-direction is also limited by the maximum tilt.

有以下几个重点：

采用PID控制输出期望推力
坐标系为NED坐标系
计算中包含悬停推力
D方向的推力控制优先于NE方向推力
Ｄ反向的推力不可能为正

因此我们先看D方向的推力控制；

	const Vector3f vel_err = _vel_sp - _vel;

	// Consider thrust in D-direction.
	// 根据速度误差计算D向的升力大小,thr_sp_d=P+I+D+thr_hover，注意方向D是向下的，所以加中立油门时会是负的
	float thrust_desired_D = _param_mpc_z_vel_p.get() * vel_err(2) +  _param_mpc_z_vel_d.get() * _vel_dot(2) + _thr_int(
					 2) - _param_mpc_thr_hover.get();

	// The Thrust limits are negated and swapped due to NED-frame.
	float uMax = -_param_mpc_thr_min.get();
	float uMin = -_param_mpc_thr_max.get();

	// make sure there's always enough thrust vector length to infer the attitude
	uMax = math::min(uMax, -10e-4f);

	// Apply Anti-Windup in D-direction.
	// 这里采取抗积分饱和，如果积分值超过最大限度或者最低限度则停止积分。
	bool stop_integral_D = (thrust_desired_D >= uMax && vel_err(2) >= 0.0f) ||
			       (thrust_desired_D <= uMin && vel_err(2) <= 0.0f);

	if (!stop_integral_D) {
		_thr_int(2) += vel_err(2) * _param_mpc_z_vel_i.get() * dt;

		// limit thrust integral
		_thr_int(2) = math::min(fabsf(_thr_int(2)), _param_mpc_thr_max.get()) * math::sign(_thr_int(2));
	}

	// Saturate thrust setpoint in D-direction.
	_thr_sp(2) = math::constrain(thrust_desired_D, uMin, uMax);

这里我有一个问题:为什么先计算期望推力再进行积分饱和算出积分项，这样在算推力的时候不就是前一个时刻的积分项吗？？
和D方向推力控制一样，再看NE方向上的控制。

if (PX4_ISFINITE(_thr_sp(0)) && PX4_ISFINITE(_thr_sp(1))) {
		//当NE方向的期望已经被赋值时，用最大倾斜角度限制倾斜方向即可。这个是由飞控上层规划的，这里不需要考虑，一般也不会跑这段代码。
		// Thrust set-point in NE-direction is already provided. Only
		// scaling by the maximum tilt is required.
		float thr_xy_max = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
		_thr_sp(0) *= thr_xy_max;
		_thr_sp(1) *= thr_xy_max;

	} else {
		// PID-velocity controller for NE-direction.
		Vector2f thrust_desired_NE;
		//算出期望推力
		thrust_desired_NE(0) = _param_mpc_xy_vel_p.get() * vel_err(0) + _param_mpc_xy_vel_d.get() * _vel_dot(0) + _thr_int(0);
		thrust_desired_NE(1) = _param_mpc_xy_vel_p.get() * vel_err(1) + _param_mpc_xy_vel_d.get() * _vel_dot(1) + _thr_int(1);

		// Get maximum allowed thrust in NE based on tilt and excess thrust.
		float thrust_max_NE_tilt = fabsf(_thr_sp(2)) * tanf(_constraints.tilt);
		float thrust_max_NE = sqrtf(_param_mpc_thr_max.get() * _param_mpc_thr_max.get() - _thr_sp(2) * _thr_sp(2));
		thrust_max_NE = math::min(thrust_max_NE_tilt, thrust_max_NE);

		// Saturate thrust in NE-direction.
		_thr_sp(0) = thrust_desired_NE(0);
		_thr_sp(1) = thrust_desired_NE(1);
		//推力限制，如果NE方推力模长大于最大模长，就按照最大推力限制等比例缩小推力
		if (thrust_desired_NE * thrust_desired_NE > thrust_max_NE * thrust_max_NE) {
			float mag = thrust_desired_NE.length();
			_thr_sp(0) = thrust_desired_NE(0) / mag * thrust_max_NE;
			_thr_sp(1) = thrust_desired_NE(1) / mag * thrust_max_NE;
		}

		// Use tracking Anti-Windup for NE-direction: during saturation, 
		//the integrator is used to unsaturate the output
		// see Anti-Reset Windup for PID controllers, L.Rundqwist, 1990
		float arw_gain = 2.f / _param_mpc_xy_vel_p.get();
		//积分抗饱和处理，当控制律输出没到极限时，_thr_sp和thrust_desired_NE是相等的，
		//所以正常积分，当输出达到极限后，在积分的同时就会向反方向减去一个超限的值，超得越多减得越多，
		//这可以起到抗饱和的作用，避免超限后积分还在持续增大。
		Vector2f vel_err_lim;
		vel_err_lim(0) = vel_err(0) - (thrust_desired_NE(0) - _thr_sp(0)) * arw_gain;
		vel_err_lim(1) = vel_err(1) - (thrust_desired_NE(1) - _thr_sp(1)) * arw_gain;

		// Update integral
		_thr_int(0) += _param_mpc_xy_vel_i.get() * vel_err_lim(0) * dt;
		_thr_int(1) += _param_mpc_xy_vel_i.get() * vel_err_lim(1) * dt;
	}
}