px4ctrl里calculateControl的代码解析

px4ctrl的代码里将目标加速度转成飞控的rpy的calculateControl代码

LinearControl::LinearControl(Parameter_t &param) : param_(param)
{resetThrustMapping();
}
quadrotor_msgs::Px4ctrlDebug
LinearControl::calculateControl(const Desired_State_t &des,const Odom_Data_t &odom,const Imu_Data_t &imu,Controller_Output_t &u)
{/* WRITE YOUR CODE HERE *///compute disired accelerationEigen::Vector3d des_acc(0.0, 0.0, 0.0);Eigen::Vector3d Kp,Kv;Kp << param_.gain.Kp0, param_.gain.Kp1, param_.gain.Kp2;Kv << param_.gain.Kv0, param_.gain.Kv1, param_.gain.Kv2;des_acc = des.a + Kv.asDiagonal() * (des.v - odom.v) + Kp.asDiagonal() * (des.p - odom.p);des_acc += Eigen::Vector3d(0,0,param_.gra);u.thrust = computeDesiredCollectiveThrustSignal(des_acc);double roll,pitch,yaw,yaw_imu;double yaw_odom = fromQuaternion2yaw(odom.q);//calculateControldouble sin = std::sin(yaw_odom);double cos = std::cos(yaw_odom);roll = (des_acc(0) * sin - des_acc(1) * cos )/ param_.gra;pitch = (des_acc(0) * cos + des_acc(1) * sin )/ param_.gra;// yaw = fromQuaternion2yaw(des.q);yaw_imu = fromQuaternion2yaw(imu.q);//calculateControl// Eigen::Quaterniond q = Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())//   * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX())//   * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY());Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw,Eigen::Vector3d::UnitZ())* Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());u.q = imu.q * odom.q.inverse() * q;/* WRITE YOUR CODE HERE */px4CtrlUtil_->log_des_p_z_=des.p.z();//used for debug// debug_msg_.des_p_x = des.p(0);// debug_msg_.des_p_y = des.p(1);// debug_msg_.des_p_z = des.p(2);debug_msg_.des_v_x = des.v(0);debug_msg_.des_v_y = des.v(1);debug_msg_.des_v_z = des.v(2);debug_msg_.des_a_x = des_acc(0);debug_msg_.des_a_y = des_acc(1);debug_msg_.des_a_z = des_acc(2);debug_msg_.des_q_x = u.q.x();debug_msg_.des_q_y = u.q.y();debug_msg_.des_q_z = u.q.z();debug_msg_.des_q_w = u.q.w();debug_msg_.des_thr = u.thrust;// Used for thrust-accel mapping estimationtimed_thrust_.push(std::pair<ros::Time, double>(ros::Time::now(), u.thrust));while (timed_thrust_.size() > 100){timed_thrust_.pop();}return debug_msg_;
}

第一部分的代码：

    Eigen::Vector3d des_acc(0.0, 0.0, 0.0);Eigen::Vector3d Kp,Kv;//    读取参数Kp << param_.gain.Kp0, param_.gain.Kp1, param_.gain.Kp2;Kv << param_.gain.Kv0, param_.gain.Kv1, param_.gain.Kv2;// 依照目标位置和速度，对比当前位置和速度，调整目标直线加速度（距离远则加速度增加）des_acc = des.a + Kv.asDiagonal() * (des.v - odom.v) + Kp.asDiagonal() * (des.p - odom.p);des_acc += Eigen::Vector3d(0,0,param_.gra);// 油门的具体算法这里就不讨论了u.thrust = computeDesiredCollectiveThrustSignal(des_acc);

第二部分的代码：本文重点讨论的部分

    double roll,pitch,yaw,yaw_imu;double yaw_odom = fromQuaternion2yaw(odom.q);//calculateControldouble sin = std::sin(yaw_odom);double cos = std::cos(yaw_odom);roll = (des_acc(0) * sin - des_acc(1) * cos )/ param_.gra;pitch = (des_acc(0) * cos + des_acc(1) * sin )/ param_.gra;// yaw = fromQuaternion2yaw(des.q);yaw_imu = fromQuaternion2yaw(imu.q);//calculateControl// Eigen::Quaterniond q = Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())//   * Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX())//   * Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY());Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw,Eigen::Vector3d::UnitZ())* Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());u.q = imu.q * odom.q.inverse() * q;

这里要先介绍下三个坐标系：

ROS坐标系（也就是odom坐标系）：x向前，y向左，z向上（roll增加向左，pitch向上，yaw向左）

机体坐标系：x向前，y向右，z向下（roll增加向右，pitch向下，yaw向右）。

Eigen坐标系：

这个不太清楚，使用程序里的代码

Eigen::Quaterniond q = Eigen::AngleAxisd(yaw,Eigen::Vector3d::UnitZ())* Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());

和 Quaternions - Visualisation 对照来看

是（roll增加向右，pitch向下，yaw向左），也就是说yaw和ROS的相同，roll和pitch的和机体坐标系的相同。

------------------------------------------------------------------------------------------------------

有了坐标系的知识后，以下要做的步骤是：

1. 将ROS坐标系的X，Y轴的加速度acc(0)和acc(1)转换成机体坐标系的roll,pitch

2. 将ROS坐标系的yaw和机体坐标系的roll，pitch带入

Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw,Eigen::Vector3d::UnitZ())* Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());

计算在ROS（odom）坐标系下的目标姿态

3. 使用 u.q = imu.q * odom.q.inverse() * q;的公式，计算将imu的姿态（飞控当前的姿态）转换成目标姿态。发给飞控。

--------------------------------------------------------------------------------------------

首先我们先看一下如何将ROS坐标系的X，Y轴的加速度acc(0)和acc(1)转换成机体坐标系的roll,pitch.

注：这里的acc和des_acc是相同的。

这里先了解下，对于机体坐标而言，roll增加向右，pitch增加向前，可以看一下的网址

https://arklab.tw/codrone-lite-pro_arduino_getting-started_-part-3/

以ROS的yaw角在0-90度为例，acc(0) 为正时，提供一个向前和向右的力，

向右：最后roll是正值，此时 sin 是正值，所以 roll = acc(0) * sin

向前：最后pitch是正值，此时cos 是正值，所以 pitch = acc(0) * cos

acc(1) 为正时，提供一个向前和向左的力，所以

向左：最后roll是负值，此时 cos 是正值，所以 roll = - acc(1) * cos

向前：最后pitch是正值，此时sin 是正值，所以 pitch = acc(1) * sin

sin和cos是为了把x方向和y方向的力，分解成机体坐标系里朝前和朝右(机体坐标系y轴朝右)的力

所以合并后的公式就是

roll = (des_acc(0) * sin - des_acc(1) * cos )/ param_.gra;
pitch = (des_acc(0) * cos + des_acc(1) * sin )/ param_.gra;

但这里为什么要除以param_.gra呢？我问了chatgpt得到的公式是

可以看到，最后将直线加速度转成roll和pitch都要除以g

这里画一下图帮助理解

可以试着依照上述步骤验证其它三个象限，都会得到同样的公式，或是尝试解析下chatgpt里说的公式。

----------------------------------------------------------------

2. 将ROS坐标系的yaw和机体坐标系的roll，pitch带入

Eigen::Quaterniond q = Eigen::AngleAxisd(des.yaw,Eigen::Vector3d::UnitZ())* Eigen::AngleAxisd(pitch,Eigen::Vector3d::UnitY())* Eigen::AngleAxisd(roll,Eigen::Vector3d::UnitX());

计算在ROS（odom）坐标系下的目标姿态

由于刚刚说了Eigen的坐标系yaw的方向和ROS相同，roll和pitch和机体坐标系相同，所以我们吧des.yaw和刚刚计算的roll和pitch带入。得到的就是在odom坐标系下的目标位姿态。

可以想象下，首先飞机朝ROS坐标系的前方，依照ROS的坐标系旋转yaw角后，依照机体坐标系的roll和pitch旋转机体，得到的就是在ROS坐标系下的目标姿态。

----------------------------------------------------------------

3. 使用 u.q = imu.q * odom.q.inverse() * q;的公式，计算将imu的姿态（飞控当前的姿态）转换成目标姿态。发给飞控。

这段代码可以这样理解 T_imu_target =  T_imu_world * T_world_odom * T_odom_target

这里的T_imu_world的意思是从世界坐标转到imu坐标的变换，由于这里只有位姿，所以就是imu.q,其余同理。

首先imu和odom开机的原点就是world，也就是世界坐标的原点，imu.q就是当前飞控的位姿对应初始位姿T_imu_world。 同理 odom.q = T_odom_world，而odom.q.inverse() 就是T_imu_world。q是目标姿态和当前odom姿态的变换，就是T_odom_target。

所以最后计算的结果就是imu（飞控）的姿态到目标q的变换。

后面都是debug的代码就不解释了

本文来自互联网用户投稿，该文观点仅代表作者本人，不代表本站立场。本站仅提供信息存储空间服务，不拥有所有权，不承担相关法律责任。如若转载，请注明出处：http://www.rhkb.cn/news/382699.html

如若内容造成侵权/违法违规/事实不符，请联系长河编程网进行投诉反馈email:809451989@qq.com，一经查实，立即删除！

相关文章

【Apache Doris】数据副本问题排查指南

【Apache Doris】数据副本问题排查指南

【Apache Doris】数据副本问题排查指南 一、问题现象二、问题定位三、问题处理 本文主要分享Doris中数据副本异常的问题现象、问题定位以及如何处理此类问题。 一、问题现象 问题日志 查询报错 Failed to initialize storage reader, tablet{tablet_id}.xxx.xxx问题说明 查…
阅读更多...
Linux下docker快速安装gitea

Linux下docker快速安装gitea

之前在服务器上装的gitlab来管理个人项目&#xff0c;但是gitlab服务启动后能明显感受到占用资源比较严重。最近服务器到期&#xff0c;换了个服务器还没来得及装gitlab&#xff0c;刚好最近接触到gitea&#xff0c;网上是这么说的 占用资源少&#xff0c;适合个人开发者&…
阅读更多...
DeepFaceLive黄仙人高清模型416DFM

DeepFaceLive黄仙人高清模型416DFM

链接&#xff1a;https://pan.baidu.com/s/1b5nu23Z93bDcxgNyyR3KjA?pwdj8kz 提取码&#xff1a;j8kz 黄羿DF-5M-UDT416_320_80_80_26DFM
阅读更多...
【ProtoBuf】通讯录实现（网络版）

【ProtoBuf】通讯录实现（网络版）

Protobuf 还常用于通讯协议、服务端数据交换场景。那么在这个示例中&#xff0c;我们将实现一个网络版本的通讯录&#xff0c;模拟实现客户端与服务端的交互&#xff0c;通过 Protobuf 来实现各端之间的协议序列化。 需求如下&#xff1a; 客户端可以选择对通讯录进行以下操…
阅读更多...
关于pycharm上push项目到gitee失败原因

关于pycharm上push项目到gitee失败原因

版权声明&#xff1a;本文为博主原创文章&#xff0c;如需转载请贴上原博文链接&#xff1a;https://blog.csdn.net/u011628215/article/details/140577821?spm1001.2014.3001.5502 前言&#xff1a;最近新建项目push上gitee都没有问题&#xff0c;但是当在gitee网站进行了一个…
阅读更多...
新生上大学提前去西藏旅游有什么要注意的，语言上该怎么办？

新生上大学提前去西藏旅游有什么要注意的，语言上该怎么办？

新生前往西藏旅游并提前适应大学生活是一次充满挑战与发现的旅程。在准备过程中&#xff0c;重要的是要对高原反应有所准备&#xff0c;了解其症状并采取预防措施&#xff0c;同时携带必要的防晒和保暖衣物以应对极端的气候条件。在交通和饮食方面&#xff0c;选择安全可靠的选…
阅读更多...
第二证券：回购队伍持续扩容！这类公司行动

第二证券：回购队伍持续扩容！这类公司行动

近期&#xff0c;上市公司密布发布回购方案&#xff0c;一些中期成果较好的公司赫然在列。除了发布正式的回购方案&#xff0c;部分上市公司控股股东、实践控制人、董事长、总经理提议回购公司股份。 绩优股活泼回购 7月23日晚&#xff0c;包括圣泉集团、中科三环、翔楼新材在…
阅读更多...
IntelliJ IDEA 直接在软件中更新为最新版

IntelliJ IDEA 直接在软件中更新为最新版

当我们的 IDEA 工具许久没有更新&#xff0c;已经拖了好几个版本&#xff0c;想跨大版本更新&#xff0c;比如从2020.2.1 -> 2023.x.x 此时&#xff0c;我们菜单栏点击 Help -> Check for Updates… &#xff0c;右下角会有提示更新&#xff0c;如下图&#xff1a; 点…
阅读更多...
Unity Shader - 2024 工具篇

Unity Shader - 2024 工具篇

目录 IDE 工具建议 IDE工具 Sublime 3 大势所趋&#xff0c;但是Sublime 使用插件还是相当的不习惯 代码跳转 Go to definite IDE 工具建议 () what is the best ide for coding shaderlab - #4 by DaveAstator - Unity Engine - Unity Discussions​​​​​​​I IDE工…
阅读更多...
大模型学习笔记 - LLM模型架构

大模型学习笔记 - LLM模型架构

LLM 模型架构 LLM 模型架构 1. LLM 核心模型 Transformer2. 详细配置 2.1 归一化方法2.2 归一化模块位置2.3 激活函数2.4 位置编码 2.4.1 绝对位置编码2.4.2 相对位置编码2.4.3 旋转位置编码 RoPE2.4.4 ALiBi位置编码 2.5 注意力机制 2.5.1 完整自注意力机制2.5.2 稀疏注意力机…
阅读更多...
计算机技术基础 （bat 批处理）Note4

计算机技术基础 （bat 批处理）Note4

计算机技术基础 &#xff08;bat 批处理&#xff09;Note4 本节主要讲解一些 bat 批处理文件中的一些特殊符号&#xff0c;包括 , %, > 和 >>, |, ^, & 和 && 和 ||, " ", ,, ;, ()。 回显屏蔽符 回显屏蔽符 : 这个字符在批处理中的意思是关…
阅读更多...
数据结构中的八大金刚--------八大排序算法

数据结构中的八大金刚--------八大排序算法

目录 引言 一&#xff1a;InsertSort(直接插入排序) 二&#xff1a;ShellSort(希尔排序) 三&#xff1a;BubbleSort(冒泡排序) 四&#xff1a; HeapSort(堆排序) 五&#xff1a;SelectSort(直接选择排序) 六&#xff1a;QuickSort(快速排序) 1.Hoare版本 2.前后指针版本 …
阅读更多...
【Hot100】LeetCode—416. 分割等和子集

【Hot100】LeetCode—416. 分割等和子集

目录 题目1- 思路2- 实现⭐152. 乘积最大子数组——题解思路 3- ACM 实现 题目 原题连接&#xff1a;416. 分割等和子集 1- 思路 理解为背包问题 思路&#xff1a; 能否将均分的子集理解为一个背包&#xff0c;比如对于 [1,5,11,5]&#xff0c;判断能否凑齐背包为 11 的容量…
阅读更多...
Linux云计算 |【第二阶段】AUTOMATION-DAY2

Linux云计算 |【第二阶段】AUTOMATION-DAY2

主要内容&#xff1a; 部署GitLab、配置管理GitLab、CI/CD概述、Jenkins概述、部署Jenkins&#xff08;初始化、拷贝插件&#xff09; 一、GitLab概述 GitLab 是一个基于 Web 的 Git 仓库管理工具&#xff0c;它提供了一个集成的开发环境和代码管理平台。GitLab 不仅支持 Git…
阅读更多...
便携式自动气象站：科技赋能气象观测

便携式自动气象站：科技赋能气象观测

便携式自动气象站&#xff0c;顾名思义&#xff0c;就是一款集成了多种气象传感器&#xff0c;能够自动进行气象观测和数据记录的设备。它体积小巧、重量轻&#xff0c;便于携带和快速部署&#xff0c;可以在各种环境下进行气象数据的实时监测。同时&#xff0c;通过内置的无线…
阅读更多...
keil单步调试需要点击多次

keil单步调试需要点击多次

问题描述&#xff1a;在进行调试的时候&#xff0c;点击单步调试&#xff0c;总是需要点击好几次才可以运行到下一条语句。 我点击单步调试的时候&#xff0c;会在红色框内进行单步运行&#xff0c;也就是在汇编代码内单步执行。 解决办法&#xff1a; 关闭汇编窗口 就是这个小…
阅读更多...
案例研究｜柯尼卡美能达软件开发（大连）有限公司基于DataEase构筑内部数据可视化体系

案例研究｜柯尼卡美能达软件开发（大连）有限公司基于DataEase构筑内部数据可视化体系

柯尼卡美能达软件开发&#xff08;大连&#xff09;有限公司于2007年5月25日注册成立。公司以“洞悉在工作的人们真实情况&#xff0c;探寻他们的愿望&#xff0c;持续提供使人们更加幸福的服务”为使命&#xff0c;致力于系统品质测试服务、软件开发服务、IT安全服务、高级BPO…
阅读更多...
SpringBoot缓存注解使用

SpringBoot缓存注解使用

背景 除了 RedisTemplate 外&#xff0c; 自Spring3.1开始&#xff0c;Spring自带了对缓存的支持。我们可以直接使用Spring缓存技术将某些数据放入本机的缓存中&#xff1b;Spring缓存技术也可以搭配其他缓存中间件(如Redis等)进行使用&#xff0c;将某些数据写入到缓存中间件…
阅读更多...
Golang | Leetcode Golang题解之第264题丑数II

Golang | Leetcode Golang题解之第264题丑数II

题目&#xff1a; 题解&#xff1a; func nthUglyNumber(n int) int {dp : make([]int, n1)dp[1] 1p2, p3, p5 : 1, 1, 1for i : 2; i < n; i {x2, x3, x5 : dp[p2]*2, dp[p3]*3, dp[p5]*5dp[i] min(min(x2, x3), x5)if dp[i] x2 {p2}if dp[i] x3 {p3}if dp[i] x5 {p5…
阅读更多...
Xcode学习笔记

Xcode学习笔记

Xcode学习笔记 前言一、在Mac上安装Xcode并做点简单设置1.查看一下Xcode的版本 二、使用Xcode新建一个Playground三、swift基础-变量1.swift是什么2.变量是什么3.建立变量4.改变变量5.小帖士 四、swift基础-变量命名规范1.使用小驼峰命名法2.使用有意义且描述性的名称3.避免使用…
阅读更多...
最新文章
推荐文章

Copyright @ 2022~2023

友情链接： 我的编程经验分享 一条长河网 尧图网站开发