其他分享
首页 > 其他分享> > PX4从放弃到精通(八):位置控制代码解析

PX4从放弃到精通(八):位置控制代码解析

作者:互联网

文章目录

前言

一个人可以走的更快,一群人才能走的更远,交流学习加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;
}

标签:解析,max,sp,thr,PX4,xy,thrust,vel,控制代码
来源: https://blog.csdn.net/qq_38768959/article/details/122542603