文章目录

• 前言
• 一、位置控制
• 二、速度控制
• 三、加速度控制

前言

一个人可以走的更快，一群人才能走的更远，交流学习加qq:2096723956
更多保姆级PX4+ROS学习视频：https://b23.tv/ZeUDKqy
分享知识，传递正能量．如有疏漏或不当之处，恳请指出
代码版本：PX4 1.11.3
代码位置

一、位置控制

void PositionControl::_positionControl()
{

外环位置控制，只有比例控制

Vector3f vel_sp_position = (_pos_sp - _pos).emult(_gain_pos_p);

位置误差产生的期望速度和期望速度前馈叠加，作为总的期望速度，当总的期望速度和位置误差产生的期望速度都不是NAN时才会叠加，如果总的期望速度是NAN，则只将位置误差产生的期望速度作为总的期望速度．如果位置误差产生的期望速度为NAN或者位置误差产生的期望速度和总的期望速度都为NAN，则什么都不做．

ControlMath::addIfNotNanVector3f(_vel_sp, vel_sp_position);

如果位置误差产生的期望速度产生的期望速度为NAN，则将其置为零．

ControlMath::setZeroIfNanVector3f(vel_sp_position);

根据设置的水平最大速度和上升/下降最大速度对期望速度进行限幅

	_vel_sp.xy() = ControlMath::constrainXY(vel_sp_position.xy(), (_vel_sp - vel_sp_position).xy(), _lim_vel_horizontal);
	_vel_sp(2) = math::constrain(_vel_sp(2), -_constraints.speed_up, _constraints.speed_down);
}

二、速度控制

void PositionControl::_velocityControl(const float dt)
{

PID控制计算期望加速度

Vector3f vel_error = _vel_sp - _vel;
Vector3f acc_sp_velocity = vel_error.emult(_gain_vel_p) + _vel_int - _vel_dot.emult(_gain_vel_d);

速度误差产生的期望加速度和期望加速度前馈叠加，作为总的期望加速度，原理同位置控制

ControlMath::addIfNotNanVector3f(_acc_sp, acc_sp_velocity);

加速度控制

_accelerationControl();

垂直方向的误差速度误差限制，由于采用的是NED坐标系，即指向地面为正，而升力是向上的，所以是负的，如果升力大于 -_lim_thr_min，也就是升力的绝对值小于_lim_thr_min，并且垂直速度误差是正的（指向地面），则将垂直速度误差置为零，即不再使升力降低．如果升力小于 -_lim_thr_max，也就是升力的绝对值大于_lim_thr_max，并且垂直速度误差是负的（指向空中），则将垂直速度误差置为零，即不再使升力升高．

if ((_thr_sp(2) >= -_lim_thr_min && vel_error(2) >= 0.0f) ||
    (_thr_sp(2) <= -_lim_thr_max && vel_error(2) <= 0.0f)) {
	vel_error(2) = 0.f;
}

最大升力限幅

_thr_sp(2) = math::max(_thr_sp(2), -_lim_thr_max);

根据勾股定理，水平方向的最大推力的平方等于总的最大推力的平方减去Z方向的推力的平方．

const float thrust_max_squared = _lim_thr_max * _lim_thr_max;
const float thrust_z_squared = _thr_sp(2) * _thr_sp(2);
const float thrust_max_xy_squared = thrust_max_squared - thrust_z_squared;

开方求最大水平推力

float thrust_max_xy = 0;
if (thrust_max_xy_squared > 0) {
	thrust_max_xy = sqrtf(thrust_max_xy_squared);
}

水平方向推力限幅

const Vector2f thrust_sp_xy(_thr_sp);
const float thrust_sp_xy_norm = thrust_sp_xy.norm();
if (thrust_sp_xy_norm > thrust_max_xy) {
	_thr_sp.xy() = thrust_sp_xy / thrust_sp_xy_norm * thrust_max_xy;
}

抗积分饱和

const Vector2f acc_sp_xy_limited = Vector2f(_thr_sp) * (CONSTANTS_ONE_G / _hover_thrust);
const float arw_gain = 2.f / _gain_vel_p(0);
vel_error.xy() = Vector2f(vel_error) - (arw_gain * (Vector2f(_acc_sp) - acc_sp_xy_limited));

如果积分为NAN则置为零

ControlMath::setZeroIfNanVector3f(vel_error);

更新速度控制积分项

_vel_int += vel_error.emult(_gain_vel_i) * dt;

积分限幅

	_vel_int(2) = math::min(fabsf(_vel_int(2)), CONSTANTS_ONE_G) * sign(_vel_int(2));
}

三、加速度控制

void PositionControl::_accelerationControl()
{

假设垂直方向只有重力加速度，计算单位化的合力向量，该向量即为机体的Z轴向量，用于姿态生成

Vector3f body_z = Vector3f(-_acc_sp(0), -_acc_sp(1), CONSTANTS_ONE_G).normalized();

限制倾斜角度，即生成的期望姿态的最大倾斜角度

ControlMath::limitTilt(body_z, Vector3f(0, 0, 1), _constraints.tilt);

限制角度的程序如下，首先对机体Z轴向量和单位化的地理Z轴向量进行点乘，求得两个向量的余弦，再求反余弦得到倾斜角度，然后对角度进行限幅，

上面程序中body_unit对应下图的向量A，world_unit对应下图的向量D，(dot_product_unit * world_unit)对应下图的向量C，rejection对应下图的向量B，向量B与向量D垂直．利用向量D和单位化的向量B，通过最大倾斜角度angle可以构造出当倾斜角度达到最大时的推力向量最后赋值给body_unit，实现对最大倾斜角度的限制．

根据悬停油门计算合力大小

float collective_thrust = _acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust;

合力不仅用于悬停，还用于水平移动，因此需要将倾斜角度考虑进去．Vector3f(0, 0, 1).dot(body_z)通过向量点乘计算倾斜角度的余弦值．即合力的垂直分量始终为_acc_sp(2) * (_hover_thrust / CONSTANTS_ONE_G) - _hover_thrust，水平分量取决于倾斜角度．

collective_thrust /= (Vector3f(0, 0, 1).dot(body_z));

最小推力限制

collective_thrust = math::min(collective_thrust, -_lim_thr_min);

求合力

_thr_sp = body_z * collective_thrust;
}