区块链技术博客
www.b2bchain.cn

Ardupilot倾转分离函数thrust_heading_rotation_angles

这篇文章主要介绍了Ardupilot倾转分离函数thrust_heading_rotation_angles的讲解,通过具体代码实例进行16223 讲解,并且分析了Ardupilot倾转分离函数thrust_heading_rotation_angles的详细步骤与相关技巧,需要的朋友可以参考下https://www.b2bchain.cn/?p=16223

本文实例讲述了2、树莓派设置连接WiFi,开启VNC等等的讲解。分享给大家供大家参考文章查询地址https://www.b2bchain.cn/7039.html。具体如下:

Ardupilot倾转分离函数thrust_heading_rotation_angles

  • 什么是轴角分离
  • 源码分析
  • 一些细节补充
  • 效果显示及进一步修改

本文主要写一下自己对于APM倾转分离(轴角分离)函数的一些学习笔记及思考。

ZING已经很好地分析了APM轴角分离函数,以下是我参考的他的一些博文。

[飞控]姿态误差(四)-APM如何计算姿态误差
[飞控]倾转分离(一)-APM的倾转分离竟然没有效果?
[飞控]倾转分离(补充)-等效旋转矢量(轴角)与旋转矩阵

后续分析过程会主要引用来自上面三篇,侵删。

什么是轴角分离

引文来源:https://zinghd.gitee.io/tilt_torsion_3/
1.起源
姿态中,roll 和 pitch 的改变来自于靠桨直接的力矩调整,调整很快,十几个毫秒就能到位。
而yaw的改变是靠桨速度差产生的旋转力矩来调整的调整比较慢,要快一百个毫秒才能到位。

2.问题
通常旋翼飞机的80%的能量用于油门控制抵抗重力,20%的能量用于控制姿态。
当 roll pitch yaw 都有较大误差时,20%的能量由三个控制器共同使用,但是由于 yaw 响应较慢,会导致 yaw 的误差一直都比较大,占用大部分姿态控制的能量,反而影响了整个姿态控制。

3.思路
因为旋翼稳定飞行的第一要素是保证桨平面( roll pitch )的精确控制,即保证桨平面没有误差 ,
yaw是不是没有误差并不重要。那么我们计算出真实姿态误差时,把误差分成两个部分tilt(pitch和roll)和 torsion(yaw),
但是呢,不直接把 torsion 给控制器
做一点限制,比如,「限幅」或者「衰减」
然后重新合成一个处理后的误差。
这样控制器就好认为,yaw 的误差并不大 ,大部分能量可以留给 tilt(pitch和roll)。

4.算法步骤
① 通过对齐 当前机体坐标系 z 轴 和 期望机体坐标系 z 轴 得到 tilt 误差
② 把 tilt 误差 转到 地理系 或者 转到 机体系
③ 总误差 – tilt 误差 = torsion 误差
④ 限制 torsion 误差 可以使用「限幅」方法 或者「衰减」方法
⑤ 把限制后的 torsion 和 tilt 组合成新的总误差

 

源码分析

以下内容备注来自我自己的一些考虑

// 将NED坐标系简称n系,机体坐标系简称b系 // 期望姿态表示为tb,当前姿态表示为cb(注意cb即为当前机体坐标系b系) // 注意四元数表示的是三维空间中的旋转而不是点  程序开始// // thrust_heading_rotation_angles() - 计算两个有序旋转,以将att_from_quat四元数移动到att_to_quat四元数。 // 第一个旋转校正推力矢量,第二个旋转校正航向矢量。 ///上面的是官方注释 // att_to_quat:传入参数,期望姿态四元数。方向:tb->n // att_from_quat:传入参数,当前姿态四元数。方向:cb->n。同时也表示从机体b系向n系的旋转。 // att_diff_angle:传入传出参数,姿态误差 // thrust_vec_dot:传入传出参数,按轴角法进行Z轴对其时的旋转角 void AC_AttitudeControl::thrust_heading_rotation_angles(Quaternion& att_to_quat, const Quaternion& att_from_quat, Vector3f& att_diff_angle, float& thrust_vec_dot) { 	///获取姿态的期望Z轴和当前Z轴/  	// 从四元数att_to_quat计算出旋转矩阵att_to_rot_matrix 	// 旋转矩阵att_to_rot_matrix用于将期望姿态旋转到n系 	// 并把期望姿态的Z轴转换到n系下的期望Z轴att_to_thrust_vec 	// 旋转方向:tb->n     Matrix3f att_to_rot_matrix;      att_to_quat.rotation_matrix(att_to_rot_matrix);      Vector3f att_to_thrust_vec = att_to_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);	// 期望姿态的标准z轴与旋转矩阵相乘得到n系下的期望z轴表示  	// 从四元数att_from_quat计算出旋转矩阵att_from_rot_matrix 	// 旋转矩阵att_from_rot_matrix用于将当前机体坐标系cb转换到n系 	// 并将当前姿态的Z轴转换到NED坐标系下的期望Z轴att_from_thrust_vec 	// 旋转方向:cb->n     Matrix3f att_from_rot_matrix;     att_from_quat.rotation_matrix(att_from_rot_matrix);     Vector3f att_from_thrust_vec = att_from_rot_matrix * Vector3f(0.0f, 0.0f, 1.0f);  	///下面通过轴角法和四元数配合,以Z轴误差计算tilt倾角误差/ 	// 构建轴角(n系下)  	// 叉乘获取旋转向量(转轴)thrust_vec_cross(叉乘后不一定是单位向量) 	// 如c=a×b,表示绕着向量c将a旋转到b,|c|=|a|*|b|,向量c正交于向量a和b 	// thrust_vec_cross即为转轴 	// 旋转方向:att_from_thrust_vec -> att_to_thrust_vec     Vector3f thrust_vec_cross = att_from_thrust_vec % att_to_thrust_vec;  	// 点乘获取旋转角thrust_vec_dot 	// 表示向量att_from_thrust_vec转向向量att_to_thrust_vec的夹角 	// 由于均为单位向量因此直接求arccos,之后限定在-1~1之间即可     thrust_vec_dot = acosf(constrain_float(att_from_thrust_vec * att_to_thrust_vec, -1.0f, 1.0f));      // 单位化旋转向量thrust_vec_cross(转换为单位向量)     float thrust_vector_length = thrust_vec_cross.length();     if (is_zero(thrust_vector_length) || is_zero(thrust_vec_dot)) {         thrust_vec_cross = Vector3f(0, 0, 1);         thrust_vec_dot = 0.0f;     } else {         thrust_vec_cross /= thrust_vector_length; }  	// 轴角构造完毕,转轴为单位向量thrust_vec_cross,转角为thrust_vec_dot 	// 旋转方向:n系下向量att_from_thrust_vec(当前姿态z轴)-> 向量att_to_thrust_vec(期望姿态z轴)  	// 下面开始将Z轴对准,即通过Z轴误差获取倾角误差(不考虑Yaw转角)  	// 轴角转换到四元数 	// 四元数thrust_vec_correction_quat表示在n系下以thrust_vec_cross为转轴,thrust_vec_dot为转角的旋转过程 	// 此处似乎是对轴角和四元数的旋转方向有争议     Quaternion thrust_vec_correction_quat;	     thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot);  	// 将倾角误差由n系转向机体坐标b系 	// 四元数att_from_quat表示有机体b系向n系旋转的过程,n = q×b×inv(q) 	// b = inv(q) ×n×q 	// 此处q为单位四元数,因此其逆等于共轭 	// 注意此处并非标准的四元数旋转描述,因为不是0标量四元数,详见下方 	thrust_vec_correction_quat = att_from_quat.inverse() * thrust_vec_correction_quat * att_from_quat;  	// 机体b系下的倾角误差获取完毕  	// 计算b系下的yaw转角误差 	// 注意:旋转矩阵B的逆 * 旋转矩阵A 等价于 旋转A与旋转B作差,由A减去B(但不是减法的意思,只是表明旋转姿态的差异) 	// 依次左乘:att_from_quat.inverse() * att_to_quat = 期望姿态 – 当前姿态 = 姿态总误差(total error) 	// thrust_vec_correction_quat.inverse() * total error = 姿态总误差 – 倾角误差(tilt error) = yaw转角误差(torsion error)     Quaternion yaw_vec_correction_quat = thrust_vec_correction_quat.inverse() * att_from_quat.inverse() * att_to_quat;  	// 倾角误差四元数转换回轴角 	// 并通过att_diff_angle获取到roll和pitch上的误差角     Vector3f rotation;     thrust_vec_correction_quat.to_axis_angle(rotation);     att_diff_angle.x = rotation.x;     att_diff_angle.y = rotation.y;  	// 转角误差四元数转换回轴角形式 	// 通过att_diff_angle获取到yaw上的误差角(roll和pitch上的应该为0)     yaw_vec_correction_quat.to_axis_angle(rotation); 	att_diff_angle.z = rotation.z;  	// 到此处获得了倾转的所有姿态误差,然而并没有对YAW角误差进行限制      // Todo: Limit roll an pitch error based on output saturation and maximum error.      // 基于最大加速度限制偏航误差-更新以包括输出饱和度和最大误差     // 当前,该限制基于使用SQRT控制器的线性部分的最大加速度     // 应该将其更新为基于角度限制,饱和度或基于用户定义的参数     if (!is_zero(_p_angle_yaw.kP()) && fabsf(att_diff_angle.z) > AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP()) {         att_diff_angle.z = constrain_float(wrap_PI(att_diff_angle.z), -AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP(), AC_ATTITUDE_ACCEL_Y_CONTROLLER_MAX_RADSS / _p_angle_yaw.kP());         yaw_vec_correction_quat.from_axis_angle(Vector3f(0.0f, 0.0f, att_diff_angle.z));         att_to_quat = att_from_quat * thrust_vec_correction_quat * yaw_vec_correction_quat;     } } 

 

一些细节补充

关于数学原理部分,我之前已经有一篇博文汇总过了:APM姿态旋转理论基础。因此其中一些数学原理的函数如轴角与四元数、四元数与旋转矩阵之间的相互转换此处不再补充。

另外关于叉乘和点乘的定义,可以参看这篇:向量内积(点乘)和外积(叉乘)概念及几何意义


// 轴角转换到四元数 // 四元数thrust_vec_correction_quat表示在n系下以thrust_vec_cross为转轴,thrust_vec_dot为转角的旋转过程 // 此处似乎是对轴角和四元数的旋转方向有争议 Quaternion thrust_vec_correction_quat;	 thrust_vec_correction_quat.from_axis_angle(thrust_vec_cross, thrust_vec_dot); 

关于上面这段函数,参考的博文:姿态误差(四)-APM如何计算姿态误差,认为轴角转换到四元数后旋转方向改变,轴角旋转:当前->期望,而转换成四元数后:期望->当前。理由是轴角法描述的是向量的旋转,而四元数描述的是坐标系的旋转。

具体可以参照这篇解释博文:[飞控]倾转分离(补充)-等效旋转矢量(轴角)与旋转矩阵

然而参照轴角转换为四元数以及四元数表示旋转的公式(懒得打了),我个人依旧认为旋转方向是不变的(望打脸)。

不过此处相对来说并不那么重要,先了解姿态误差计算流程即可。

 

效果显示及进一步修改

MATLAB代码来自:[飞控]倾转分离(一)-APM的倾转分离竟然没有效果?

Ardupilot倾转分离函数thrust_heading_rotation_angles
如上图所示,期望姿态欧拉角表示为(60, 60, 25),att_diff_angle误差为(0.8194, 1.0325, -0.2072)

绿色表示当前姿态,与黑色NED坐标系重合,红色表示期望姿态,蓝色表示APM中轴角分离之后得到的att_diff_angle误差,然而可以看到其与期望姿态的Z轴并没有对齐,但是APM程序中明明是按照对齐Z轴进行姿态误差计算的,这是为什么呢?

因为APM程序中并没有对倾转分离后的z轴误差做处理,忽略处理了倾斜时产生的z轴误差,抑制效果不好。

正确做法是对torsion误差(YAW)按一定的比例进行缩小,限制旋转,然后用四元数乘法,重新叠加两个旋转,得到新的总旋转。

att_diff_angle.z = rotation.z; 这段代码后面,可添加如下更新

// 以下内容未经过验证,仅提供思路 att_diff_angle.z = 0.5 * att_diff_angle.z;// torsion误差比例缩小进行衰减 yaw_vec_correction_quat.from_axis_angle(att_diff_angle.z);	// torsion误差转换为四元数形式 Quaternion att_diff_angle_quat = thrust_vec_correction_quat * yaw_vec_correction_quat;	// torsion和tilt旋转叠加 att_diff_angle_quat.to_axis_angle(rotation);	// 转换为轴角 att_diff_angle.x = rotation.x; att_diff_angle.y = rotation.y; att_diff_angle.z = rotation.z; 

MATLAB实现效果如下,黑色部分为经过torsion误差衰减之后的att_diff_angle。误差表示为(0.7651, 1.0740, -0.0881)
Ardupilot倾转分离函数thrust_heading_rotation_angles
结果对比可发现经调整之后的黑色坐标系相对于蓝色坐标系,Z轴已经完成对准。
Ardupilot倾转分离函数thrust_heading_rotation_angles

本文转自互联网,侵权联系删除Ardupilot倾转分离函数thrust_heading_rotation_angles

赞(0) 打赏
部分文章转自网络,侵权联系删除b2bchain区块链学习技术社区 » Ardupilot倾转分离函数thrust_heading_rotation_angles
分享到: 更多 (0)

评论 抢沙发

  • 昵称 (必填)
  • 邮箱 (必填)
  • 网址

b2b链

联系我们联系我们