Ardupilot倾转分离函数thrust_heading_rotation_angles

Ardupilot四元数姿态控制函数attitude_controller_run_quat解析 Ardupilot四元数姿态控制函数attitude_controller_run_quat解析源码解析细节讲解thrust_heading_rotation_angles()update_ang_vel_target_from_att_error()角度前馈量旋转解释姿态更新部分 在开始阅读本文之前,建议可以先看一下前面几篇博文,当然你基础好直接看也是没事的~ 详解APM的开方控制器sqrt_controller Ardupilot倾转分离函数thrust_heading_rotation_angles 阅读详情

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的倾转分离竟然没有效果?

在这里插入图片描述
如上图所示,期望姿态欧拉角表示为(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)
在这里插入图片描述
结果对比可发现经调整之后的黑色坐标系相对于蓝色坐标系,Z轴已经完成对准。
在这里插入图片描述

Ardupilot姿态控制器 PID控制流程 APM的控制器主要分为attitude姿态控制器和pos位置控制器。这系列文章主要来讲一下姿态控制器内部的实现。本文主要目的是为了解析APM源码然后方便自己将其实现在自己的水下航行器控制器上的,因此将会以ArduCopter和ArduSub为例进行解析,其他无人机类型也是类似的,也可用作参考。 一、PID姿态控制器 1.1 Copter姿态控制官方原图 在真正开始解析源码之前,可以看一下Ardupilot官方网站上开发者手册对于直升机姿态控制器的描述(虽然官方的文档已经很久没更新了,4.x版本之后的.. 阅读详情

相关推荐

PX4姿态控制算法详解

本篇文章首发于公众号:无人机系统技术。更多无人机技术相关文章请关注此公众号,有问题也可在公众号回复“加群”进入技术交流群进行交流。 倾转分离 今天的内容我们来解析开源飞控软件PX4中关于多旋翼飞行器的姿态控制算法。 首先,PX4位置控制器的输出会转换成四元数的期望传递给姿态控制器。姿态控制器在得到期望四元数之后并不是马上进行角速率期望的计算,而是先进行倾转分离的操作。 那什么是倾转分离?倾转其实是...

weixin_44616080的博客 3780

ArduPilot姿态控制方法解析---(倾转分离)

ArduPilot飞控因其成熟稳定,受到了大多飞控研究者的膜拜,以及大多航模玩具的青睐。但是,即使已经发展了十几年,仍有很多人对其中的控制算法和原理一知半解。本文给出了一些参考和个人见解,仅供参考。

Chasing的博客 4756

Pixhawk之姿态控制篇(1)_源码算法分析(超级有料)

一、开篇         姿态控制篇终于来了、来了、来了~~~         心情爽不爽?愉悦不愉悦?开心不开心?         喜欢的话就请我吃顿饭吧,哈哈。         其实这篇blog一周前就应该写的,可惜被上一篇blog霸占了。但是也不算晚,整理了很多算法基础知识,使得本篇blog更充实。一人之力总是有限的,难免有不足之处,大家见谅,有写的不好的地方劳烦指正。看到标题了吧,

. 6万+

PX4中关于多旋翼飞行器的姿态控制算法-倾转分离

首先,在飞行器实际飞行中,电机的能力是有限的,除了用来平衡飞行器本身的重力之外,留给姿态机动的空间就不是特别充足。而倾斜和旋转两种运动的原理是不同的,倾斜运动是由不同电机差速运动产生的升力差形成的力矩驱使的,而旋转运动是由不同电机差速运动产生的反扭矩驱使的,前者在数值上往往要比后者大很多。倾转其实是倾斜和旋转的意思,英文里一般用tilt和torsion表示,倾斜对应我们飞行器也就是机体轴系Z轴方向的变化,旋转对应我们飞行器是指绕机体轴系Z轴的旋转运动,倾转分离就是将倾斜和旋转运动从总的姿态期望中剥离出来。

m0_73918300的博客 409

Ardupilot姿态控制详解(完结篇)

本文主要是对过去写的一些APM中姿态控制流程及函数博文的汇总。 按照之前写的博文以及个人认为的学习顺序,给出APM姿态控制器的学习顺序如下: APM姿态旋转理论基础 Ardupilot姿态控制器 PID控制流程(整定后的P控制器) 详解APM的开方控制器sqrt_controller Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析 Ardupilot倾转分离函数thrust_heading_rotation_angles Ardupilot四元数姿态控制

moumde的博客 7025

PX4 姿态环控制算法

论文:Nonlinear Quadrocopter Attitude Control。PX4中四旋翼姿态环控制算法,是根据四元数计算得来,不是传统的欧拉角计算。将PX4的姿态控制数据读取出来,与Matlab计算结果对比, 完全一致。1、倾转分离,优先控制俯仰、其次滚转,最后偏航;2、四元数的虚分量作为角速度环的期望。

hkzuibang的博客 1683

arducopter--parachute

先梳理个手动开伞流程: 有两种方法:其一,通过mavlink控制,这种方式比较安全,可以在地面站上做保护提示: handleMessage函数中包含解降落伞的case: * 在传过来 MAVLINK_MSG_ID_COMMAND_LONG (76) 包中,识别command,为CMD 208为开伞,自动起飞如下 enable parachute后,先传param1 为0 ,在开伞时选择2开伞。 ...

笔记 756

PX4项目学习::(一)项目架构

下图所示为PX4的软件顶层架构。我们可以将上图从中间分成两个看,其中右边为传感器采集(GPS、optical_flow、inertial_sensors)回来的数据,,​ px4的控制部分程序,采用的是分层设计。

one__leaf的博客 3849

ArduPilot中的李代数魔法

上,也就是对应的三维李代数向量上。这种“用高维姿态表示,在低维误差空间进行更新”的方法,正是李代数思想的应用。,它的维度是4,有归一化约束。为了保证滤波器的线性更新,EKF内部的误差状态通常定义在姿态四元数所处的。:在 ArduPilot 的 EKF3(扩展卡尔曼滤波)状态向量中,姿态部分被表示为。类负责处理四元数(代表姿态,属于李群),而 to_axis_angle():李代数的核心是“将旋转表示为向量”。的调用逻辑,例如在姿态控制器的更新函数中。方法的作用,正是将目标姿态与当前姿态之间的。

daad777的专栏 183

Ardusub源码解析学习(一)——Ardusub主程序

Ardusub源码解析学习(一)前言一、准备工作二、Ardusub.cpp解析2.1 scheduler table2.2 get_scheduler_tasks()2.3 fast_loop()2.3.1 flight_mode.cpp2.3.2 control_stabilize.cpp2.3.3 AC_AttitudeControl.cpp三、总结 前言 最近因为课题项目需要,正在学习水下机器人的控制。水下机器人实际上就是一个用于水下的无人机系统(当然有些不严谨哈,这里特指ROV),内

moumde的博客 9797

ardupilot中一个四元数和三维矢量相乘

本节主要记录ardupilot中一个四元数和一个三维矢量相乘的含义,推导出其中运算的本质含义。

陌城烟雨 216

Pixhawk之姿态控制

原文地址:http://blog.csdn.net/qq_21842557 1、写在前面             无人机控制部分主要分为两个部分,姿态控制部分和位置控制部分;位置控制可用远程遥控控制,而姿态控制一般由无人机系统自动完成。姿态控制是非常重要的,因为无人机的位置变化都是由姿态变化引起的。         下图阐释了PX4源码中的两个环路控制,分为姿态控制和位置控制。

zhangmh2011的博客 4079

ardupilot 为什么要采样四元数姿态控制

本节主要说明清楚ardupilot姿态控制为什么要用到四元数,欢迎批评指正!!!

陌城烟雨 1119

浅谈APM系列-----update_flight_mode(ModeAltHold)

update_flight_mode(ModeAltHold) 这里只看ModeAltHold。 位置:X:\ardupilot\ArduCopter\mode.cpp // update_flight_mode - calls the appropriate attitude controllers based on flight mode(根据飞行模式调用适当的姿态控制器) // ...

Hi_wangtao的博客 1556

Pixhawk飞控源码

本资源为开源飞控模块pixhawk源码,1.7.0版本,可以供四旋翼爱好者或固定翼爱好者提供飞控代码参考,希望对大家有用。

diffangle(in1,in2):diffangle 计算两组角度之间的差异(in1-in2)-matlab开发

diffangle 计算两组角度(以度为单位)之间的差异(in1-in2)。 差角被定义为两个可能的角度中较小的一个,并带有适当的符号。 范围内的结果 (-180,180]。 当仅提供单个向量参数时,diffangle 的作用类似于 Matlab 的 diff 来计算 [in1(2)-in1(1) in1(3)-in1(1) ...]

基于Simulink的PX4四旋翼姿态控制

基于Simulink的“UAV Toolbox Support Package for PX4 Autopilots”工具箱,进行PX4的算法设计

weixin_42763508的博客 2021

别再羡慕专业版!用这个批处理脚本,一键给Win11家庭版装上官方沙盒

本文详细介绍了如何通过批处理脚本在Windows 11家庭版上安装官方Windows沙盒(Windows Sandbox),无需复杂操作即可享受专业版功能。脚本自动激活系统内置组件,提供安全隔离的虚拟化环境,适用于软件测试、安全浏览等场景。

weixin_26757925的博客 501

pixhawk学习

 原文转载至https://blog.csdn.net/u013181595/article/details/80976610 1硬件架构分析     Pixhawk是一款基于ARM芯片的32位开源飞控,由ETH的computer vision and geometry group的博士生Lorenz Meier开发。最初采用的是分体式的设计即px4(由px4fmu和px4io两个组件组成),...

热爱钻研,定位问题,善于总结,注重提升自己,具有广泛的软件开发兴趣,爱好在开源论坛交流学习。 5506

常见机械臂模型不用找!Mujoco这儿都有!

高质量的模型集合,franka panda,so-arm100,ur之类的机械臂都有。常见机械臂模型不用找!Mujoco这儿都有!大概346M,网速不行的可以下载zip。

darcsdn 993

保姆级教程:在Ubuntu 20.04上搞定PX4飞控与T265的视觉定位(附避坑指南)

本文提供在Ubuntu 20.04上配置PX4飞控与T265视觉定位系统的详细教程,涵盖驱动安装、ROS与MAVROS桥接、PX4参数调优及故障排查。通过实测验证的步骤和避坑指南,帮助开发者实现厘米级精度的视觉定位,特别适合室内无人机应用。

weixin_29038155的博客 252
上一篇: Ardupilot前馈及平滑函数input_euler_angle_roll_pitch_yaw解析
下一篇: Ardupilot四元数姿态控制函数attitude_controller_run_quat解析
moumde
博客等级 码龄10年 454粉丝 40原创
评论
成就一亿技术人!
拼手气红包6.0元
还能输入1000个字符
 
 条评论被折叠 查看
添加红包

请填写红包祝福语或标题

个

红包个数最小为10个

元

红包金额最低5元

当前余额3.43元 前往充值 >
需支付:10.00元
成就一亿技术人!
领取后你会自动成为博主和红包主的粉丝 规则
hope_wisdom
发出的红包
实付元
使用余额支付
点击重新获取
扫码支付
钱包余额 0

抵扣说明:

1.余额是钱包充值的虚拟货币,按照1:1的比例进行支付金额的抵扣。
2.余额无法直接购买下载,可以购买VIP、付费专栏及课程。

余额充值