遍尝后宫佳丽美,开源飞控PX4姿态控制代码解析


遍尝后宫佳丽美
遍尝后宫佳丽美

原标题:开源飞控PX4姿态控制代码解析

引言

上一讲PX4姿态控制算法详解我们对PX4姿态控制的算法进行了详细解析,还没看过的朋友先打开第三条内容仔细阅读后再来继续浏览吧。本讲内容主要是摘取PX4中关于姿态控制律实现的代码进行逐行分析,让各位朋友在代码实践中感受控制算法的魅力。姿态控制器的代码在文件Firmwaresrcmodulesmc_att_controlAttitudeControlAttitudeControl.cpp的update函数中,我们先把代码贴上来,然后再进行逐行解读:

matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward) { // ensure input quaternions are exactly normalized because acosf(1.00001) == NaN q.normalize; qd.normalize;

// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitchconstVector3f e_z = q.dcm_z;constVector3f e_z_d = qd.dcm_z;Quatf qd_red(e_z, e_z_d);

if(fabsf(qd_red(1)) > (1.f- 1e-5f) || fabsf(qd_red(2)) > (1.f- 1e-5f)) {// In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,// full attitude control anyways generates no yaw input and directly takes the combination of// roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.qd_red = qd;

} else{// transform rotation from current to desired thrust vector into a world frame reduced desired attitudeqd_red *= q;}

// mix full and reduced desired attitudeQuatf q_mix = qd_red.inversed * qd;q_mix *= math::signNoZero(q_mix(0));// catch numerical problems with the domain of acosf and asinfq_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

// quaternion attitude control law, qe is rotation from q to qdconstQuatf qe = q.inversed * qd;

// using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)// also taking care of the antipodal unit quaternion ambiguityconstVector3f eq = 2.f* math::signNoZero(qe(0)) * qe.imag;

// calculate angular rates setpointmatrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

// Feed forward the yaw setpoint rate.// yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)// and multiply it by the yaw setpoint rate (yaw_sp_move_rate).// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame// such that it can be added to the rates setpoint.rate_setpoint += q.inversed.dcm_z * yawspeed_feedforward;

// limit ratesfor(inti = 0; i < 3; i++) {rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));}

returnrate_setpoint;}

代码解读

为方便大家阅读,代码中原有的英文注释给大家保存,每一行代码的注释我们放在代码上方。

matrix::Vector3f AttitudeControl::update(matrix::Quatf q, matrix::Quatf qd, float yawspeed_feedforward){// ensure input quaternions are exactly normalized because acosf(1.00001) == NaN

//对当前姿态四元数进行归一化,其实四元数本来就是归一化的,但是在计算机中存储传输时1.0可能会被存储成1.000001,而后面在运算反余弦函数时就会出现acosf(1.00001) == NaN

q.normalize;

//对期望姿态四元数进行归一化,原因同当前姿态四元数

qd.normalize;

// calculate reduced desired attitude neglecting vehicle's yaw to prioritize roll and pitch

//计算当前姿态四元数对应的机体坐标系Z轴单位向量,在机载NED系下表示

constVector3f e_z = q.dcm_z;

//计算期望姿态四元数对应的机体坐标系Z轴单位向量,在机载NED系下表示

constVector3f e_z_d = qd.dcm_z;

//算去除旋转误差后仅代表倾斜误差的四元数,计算公式如下:

Quatf qd_red(e_z, e_z_d);

//无旋转运动时,直接赋值为期望四元数(这个判断条件我觉得是有问题的,以后再去细究吧)

if(fabsf(qd_red(1)) > (1.f- 1e-5f) || fabsf(qd_red(2)) > (1.f- 1e-5f)) {// In the infinitesimal corner case where the vehicle and thrust have the completely opposite direction,// full attitude control anyways generates no yaw input and directly takes the combination of// roll and pitch leading to the correct desired yaw. Ignoring this case would still be totally safe and stable.qd_red = qd;

}

//将旋转轴从N系转换到B系:

//结合当前姿态,我们可以得到仅代表倾斜运动的期望四元数

//所以qd_red *= q是把上面两步骤合成了一步

else{// transform rotation from current to desired thrust vector into a world frame reduced desired attitudeqd_red *= q;}

//根据期望四元数和倾斜期望四元数可以得到代表旋转运动的期望四元数 :

// mix full and reduced desired attitudeQuatf q_mix = qd_red.inversed * qd;

//根据旋转运动的性质,q_mix一定可以表示为(cos(α_mix/2),0,0,sin(α_mix/2))

q_mix *= math::signNoZero(q_mix(0));// catch numerical problems with the domain of acosf and asinfq_mix(0) = math::constrain(q_mix(0), -1.f, 1.f);q_mix(3) = math::constrain(q_mix(3), -1.f, 1.f);

//限制旋转误差,最后结合倾斜运动和受限制旋转运动组成新的期望四元数:

qd = qd_red * Quatf(cosf(_yaw_w * acosf(q_mix(0))), 0, 0, sinf(_yaw_w * asinf(q_mix(3))));

//根据新四元数姿态期望计算四元数误差:

// quaternion attitude control law, qe is rotation from q to qdconstQuatf qe = q.inversed * qd;

//根据实际意义选取姿态误差为:eq=2*sign(qe(0))*qe(1:3)

// using sin(alpha/2) scaled rotation axis as attitude error (see quaternion definition by axis angle)// also taking care of the antipodal unit quaternion ambiguityconstVector3f eq = 2.f* math::signNoZero(qe(0)) * qe.imag;

//根据姿态误差计算期望角速度:

// calculate angular rates setpointmatrix::Vector3f rate_setpoint = eq.emult(_proportional_gain);

//增加偏航角速度前馈:

// Feed forward the yaw setpoint rate.// yaw_sp_move_rate is the feed forward commanded rotation around the world z-axis,// but we need to apply it in the body frame (because _rates_sp is expressed in the body frame).// Therefore we infer the world z-axis (expressed in the body frame) by taking the last column of R.transposed (== q.inversed)// and multiply it by the yaw setpoint rate (yaw_sp_move_rate).// This yields a vector representing the commanded rotatation around the world z-axis expressed in the body frame// such that it can be added to the rates setpoint.rate_setpoint += q.inversed.dcm_z * yawspeed_feedforward;

//角速度期望输出限幅

// limit ratesfor(inti = 0; i < 3; i++) {rate_setpoint(i) = math::constrain(rate_setpoint(i), -_rate_limit(i), _rate_limit(i));}

returnrate_setpoint;}

本文章转载至公众号:无人机系统技术,此公众号作者为北航博士,意在分享无人机开发经验,阐述无人机相关技术知识,交流无人机开发过程中遇到的问题,解读开源飞控px4和ardupilot的算法和代码。

总结

本篇内容对PX4中实现姿态控制器的代码进行了详细解读,经过算法公式的推导,代码的逐行分析,大家应该对PX4中多旋翼飞行器的姿态控制器设计与实现有了清晰的认识,不过纸上得来终觉浅,绝知此事要躬行,光看不练无异于纸上谈兵,大家可以在matlab中做做仿真,或者在搭建的飞控开发平台上根据自己的理解来实现姿态控制,在实战中感受飞控算法的魅力。

分享到