当前位置: 首页 > news >正文

Odrive源码分析(四) 位置爬坡算法

Odrive中自带一个简单的梯形速度爬坡算法,本文分析下这部分代码。

代码如下:

#include <cmath>
#include "odrive_main.h"
#include "utils.hpp"// A sign function where input 0 has positive sign (not 0)
float sign_hard(float val) {return (std::signbit(val)) ? -1.0f : 1.0f;
}// Symbol                     Description
// Ta, Tv and Td              Duration of the stages of the AL profile
// Xi and Vi                  Adapted initial conditions for the AL profile
// Xf                         Position set-point
// s                          Direction (sign) of the trajectory
// Vmax, Amax, Dmax and jmax  Kinematic bounds
// Ar, Dr and Vr              Reached values of acceleration and velocitybool TrapezoidalTrajectory::planTrapezoidal(float Xf, float Xi, float Vi,float Vmax, float Amax, float Dmax) {float dX = Xf - Xi;  // Distance to travelfloat stop_dist = (Vi * Vi) / (2.0f * Dmax); // Minimum stopping distancefloat dXstop = std::copysign(stop_dist, Vi); // Minimum stopping displacementfloat s = sign_hard(dX - dXstop); // Sign of coast velocity (if any)Ar_ = s * Amax;  // Maximum Acceleration (signed)Dr_ = -s * Dmax; // Maximum Deceleration (signed)Vr_ = s * Vmax;  // Maximum Velocity (signed)// If we start with a speed faster than cruising, then we need to decel instead of accel// aka "double deceleration move" in the paperif ((s * Vi) > (s * Vr_)) {Ar_ = -s * Amax;}// Time to accel/decel to/from Vr (cruise speed)Ta_ = (Vr_ - Vi) / Ar_;Td_ = -Vr_ / Dr_;// Integral of velocity ramps over the full accel and decel times to get// minimum displacement required to reach cuising speedfloat dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;// Are we displacing enough to reach cruising speed?if (s*dX < s*dXmin) {// Short move (triangle profile)Vr_ = s * std::sqrt(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);Td_ = std::max(0.0f, -Vr_ / Dr_);Tv_ = 0.0f;} else {// Long move (trapezoidal profile)Tv_ = (dX - dXmin) / Vr_;}// Fill in the rest of the values used at evaluation-timeTf_ = Ta_ + Tv_ + Td_;Xi_ = Xi;Xf_ = Xf;Vi_ = Vi;yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); // pos at end of accel phasereturn true;
}TrapezoidalTrajectory::Step_t TrapezoidalTrajectory::eval(float t) {Step_t trajStep;if (t < 0.0f) {  // Initial ConditiontrajStep.Y   = Xi_;trajStep.Yd  = Vi_;trajStep.Ydd = 0.0f;} else if (t < Ta_) {  // AcceleratingtrajStep.Y   = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);trajStep.Yd  = Vi_ + Ar_*t;trajStep.Ydd = Ar_;} else if (t < Ta_ + Tv_) {  // CoastingtrajStep.Y   = yAccel_ + Vr_*(t - Ta_);trajStep.Yd  = Vr_;trajStep.Ydd = 0.0f;} else if (t < Tf_) {  // Decelerationfloat td     = t - Tf_;trajStep.Y   = Xf_ + 0.5f*Dr_*SQ(td);trajStep.Yd  = Dr_*td;trajStep.Ydd = Dr_;} else if (t >= Tf_) {  // Final ConditiontrajStep.Y   = Xf_;trajStep.Yd  = 0.0f;trajStep.Ydd = 0.0f;} else {// TODO: report error here}return trajStep;
}

首先当需要控制电机运动到某个位置时,会调用函数,该函数会调用上面的函数planTrapezoidal。

void Controller::move_to_pos(float goal_point) {axis_->trap_traj_.planTrapezoidal(goal_point, pos_setpoint_, vel_setpoint_,axis_->trap_traj_.config_.vel_limit,axis_->trap_traj_.config_.accel_limit,axis_->trap_traj_.config_.decel_limit);axis_->trap_traj_.t_ = 0.0f;trajectory_done_ = false;
}

然后会在control对象中调用eval函数不断的计算出下一时刻的目标位置和速度。

        case INPUT_MODE_TRAP_TRAJ: {if(input_pos_updated_){move_to_pos(input_pos_);input_pos_updated_ = false;}// Avoid updating uninitialized trajectoryif (trajectory_done_)break;if (axis_->trap_traj_.t_ > axis_->trap_traj_.Tf_) {// Drop into position control mode when done to avoid problems on loop counter delta overflowconfig_.control_mode = CONTROL_MODE_POSITION_CONTROL;pos_setpoint_ = axis_->trap_traj_.Xf_;vel_setpoint_ = 0.0f;torque_setpoint_ = 0.0f;trajectory_done_ = true;} else {TrapezoidalTrajectory::Step_t traj_step = axis_->trap_traj_.eval(axis_->trap_traj_.t_);pos_setpoint_ = traj_step.Y;vel_setpoint_ = traj_step.Yd;torque_setpoint_ = traj_step.Ydd * config_.inertia;axis_->trap_traj_.t_ += current_meas_period;}

那么关键就是两个函数planTrapezoidal和函数eval,当位置更新时调用前者,周期性调用后者,后者的输出更新到位置闭环和速度闭环中实现轨迹跟随。

planTrapezoidal代码分析如下:

    //计算出加速阶段和减速阶段需要的事件Ta_ = (Vr_ - Vi) / Ar_;Td_ = -Vr_ / Dr_;//如果能跑到最大速度,那么计算加速阶段和减速阶段运行的位移float dXmin = 0.5f*Ta_*(Vr_ + Vi) + 0.5f*Td_*Vr_;if (s*dX < s*dXmin) {//如果是短位移,这里算出三角规划的速度,这里看下面的公式推导Vr_ = s * std::sqrt(std::max((Dr_*SQ(Vi) + 2*Ar_*Dr_*dX) / (Dr_ - Ar_), 0.0f));//重新计算加速时间和减速时间,匀速阶段为0Ta_ = std::max(0.0f, (Vr_ - Vi) / Ar_);Td_ = std::max(0.0f, -Vr_ / Dr_);Tv_ = 0.0f;} else {//如果是长位移,那么走梯形速度,这里得出匀速阶段的时间Tv_ = (dX - dXmin) / Vr_;}//计算出本次规划需要的总时间Tf_ = Ta_ + Tv_ + Td_;Xi_ = Xi;Xf_ = Xf;Vi_ = Vi;//计算加速阶段结束时的位置,即加速阶段的位移。yAccel_ = Xi + Vi*Ta_ + 0.5f*Ar_*SQ(Ta_); 
  • 三角规划公式推导如下:

eval代码分析如下:

    if (t < 0.0f) {  //初始条件,不会进入trajStep.Y   = Xi_;trajStep.Yd  = Vi_;trajStep.Ydd = 0.0f;} else if (t < Ta_) {  //加速阶段trajStep.Y   = Xi_ + Vi_*t + 0.5f*Ar_*SQ(t);  //按照加速阶段计算当前时刻的位置trajStep.Yd  = Vi_ + Ar_*t;   //一阶导数,即当前时刻的速度trajStep.Ydd = Ar_;           //二阶导数即当前时刻的加速度} else if (t < Ta_ + Tv_) {  //匀速阶段trajStep.Y   = yAccel_ + Vr_*(t - Ta_);  //按照匀速阶段计算当前时刻的位置trajStep.Yd  = Vr_;     //一阶导数,即当前时刻的速度trajStep.Ydd = 0.0f;    //二阶导数即当前时刻的加速度为0} else if (t < Tf_) {  //减速阶段float td     = t - Tf_;trajStep.Y   = Xf_ + 0.5f*Dr_*SQ(td);  //按照减速阶段计算当前时刻的位置trajStep.Yd  = Dr_*td;   //一阶导数,即当前时刻的速度trajStep.Ydd = Dr_;      //二阶导数即当前时刻的减速度} else if (t >= Tf_) {  //规划完成trajStep.Y   = Xf_;    trajStep.Yd  = 0.0f; trajStep.Ydd = 0.0f;} else {// TODO: report error here}

相关文章:

Odrive源码分析(四) 位置爬坡算法

Odrive中自带一个简单的梯形速度爬坡算法&#xff0c;本文分析下这部分代码。 代码如下&#xff1a; #include <cmath> #include "odrive_main.h" #include "utils.hpp"// A sign function where input 0 has positive sign (not 0) float sign_ha…...

[Unity Shader][图形渲染] Shader数学基础11 - 复合变换详解

在图形学与Shader编程中,复合变换是将平移、旋转和缩放等基本几何变换组合在一起,从而实现更复杂的物体变换效果。复合变换的本质是通过矩阵的串联操作,依次应用多个变换。 本文将介绍复合变换的数学原理、矩阵计算方法及注意事项,并结合实际编程中的实现细节帮助你掌握其…...

使用Python实现智能家居控制系统:开启智慧生活的钥匙

友友们好! 我的新专栏《Python进阶》正式启动啦!这是一个专为那些渴望提升Python技能的朋友们量身打造的专栏,无论你是已经有一定基础的开发者,还是希望深入挖掘Python潜力的爱好者,这里都将是你不可错过的宝藏。 在这个专栏中,你将会找到: ● 深入解析:每一篇文章都将…...

使用 HTML5 Canvas 实现动态蜈蚣动画

使用 HTML5 Canvas 实现动态蜈蚣动画 1. 项目概述 我们将通过 HTML 和 JavaScript 创建一个动态蜈蚣。蜈蚣由多个节段组成&#xff0c;每个节段看起来像一个小圆形&#xff0c;并且每个节段上都附带有“脚”。蜈蚣的头部会在画布上随机移动。 完整代码在底部&#xff01;&…...

计算机视觉目标检测——DETR(End-to-End Object Detection with Transformers)

计算机视觉目标检测——DETR(End-to-End Object Detection with Transformers) 文章目录 计算机视觉目标检测——DETR(End-to-End Object Detection with Transformers)摘要Abstract一、DETR算法1. 摘要&#xff08;Abstract&#xff09;2. 引言&#xff08;Introduction&#…...

uniapp .gitignore

打开HBuilderX&#xff0c;在项目根目录下新建文件 .gitignore复制下面内容 #忽略unpackge目录下除了res目录的所有目录 unpackage/* !unpackage/res/#忽略.hbuilderx目录 .hbuilderx# 忽略node_modules目录下的所有文件 node_modules/# 忽略锁文件 package-lock.json yarn.l…...

JavaWeb Servlet的反射优化、Dispatcher优化、视图(重定向)优化、方法参数值获取优化

目录 1. 背景2. 实现2.1 pom.xml2.2 FruitController.java2.3 DispatcherServlet.java2.4 applicationContext.xml 3. 测试 1. 背景 前面我们做了Servlet的一个案例。但是存在很多问题&#xff0c;现在我们要做优化&#xff0c;优化的步骤如下&#xff1a; 每个Fruit请求都需…...

备忘一个FDBatchMove数据转存的问题

使用FDBatchMove的SQL导入excel表到sql表&#xff0c;设置条件时一头雾水&#xff0c;函数不遵守sql的规则。 比如替换字段的TAB键值为空&#xff0c;replace(字段名,char(9),)竟然提示错误&#xff0c;百思不得其解。 试遍了几乎所有的函数&#xff0c;竟然是chr(9)。 这个…...

CEF127 编译指南 MacOS 篇 - 编译 CEF(六)

1. 引言 经过前面的准备工作&#xff0c;我们已经完成了所有必要的环境配置。本文将详细介绍如何在 macOS 系统上编译 CEF127。通过正确的编译命令和参数配置&#xff0c;我们将完成 CEF 的构建工作&#xff0c;最终生成可用的二进制文件。 2. 编译前准备 2.1 确认环境变量 …...

【更新】LLM Interview

课程链接&#xff1a;BV1o217YeELo 文章目录 LLM基础相关1. LLMs概述2. 大语言模型尺寸3. LLMs的优势与劣势4. 常见的大模型分类5. 目前主流的LLMs开源模型体系有哪些&#xff08;Prefix Decoder&#xff0c;Causal Decoder&#xff0c;Encoder-Decoder的区别是什么&#xff09…...

Django 视图中使用 Redis 缓存优化查询性能

在 Web 应用程序开发中,查询数据库是一个常见的操作,但如果查询过于频繁或耗时,就会影响应用程序的性能。为了解决这个问题,我们可以使用缓存技术,将查询结果暂时存储在内存中,从而减少对数据库的访问。本文将介绍如何在 Django 视图中使用 Redis 缓存来优化查询性能。 © …...

正则表达式解析与功能说明

正则表达式解析与功能说明 表达式说明 String regex "\\#\\{TOASRTRINNG\\((.*?)((.*?))\\)(\\})";该正则表达式的作用是匹配形如 #{TOASRTRINNG(...)} 的字符串格式。以下是正则表达式的详细解析&#xff1a; 拆解与解析 1. \\# 匹配&#xff1a;# 字符。说明…...

STUN服务器实现NAT穿透

NAT穿透的问题 在现代网络环境中&#xff0c;大多数设备都位于NAT(网络地址转换)设备后面。这给点对点(P2P)通信带来了挑战&#xff0c;因为NAT会阻止外部网络直接访问内部设备。STUN(Session Traversal Utilities for NAT)服务器就是为了解决这个问题而设计的。 STUN是什么?…...

音视频入门基础:MPEG2-TS专题(19)——FFmpeg源码中,解析TS流中的PES流的实现

一、引言 FFmpeg源码在解析完PMT表后&#xff0c;会得到该节目包含的视频和音频信息&#xff0c;从而找到音视频流。TS流的音视频流包含在PES流中。FFmpeg源码通过调用函数指针tss->u.pes_filter.pes_cb指向的回调函数解析PES流的PES packet&#xff1a; /* handle one TS…...

tomcat的安装以及配置(基于linuxOS)

目录 安装jdk环境 yum安装 验证JDK环境 安装tomcat应用 yum安装 ​编辑 使用yum工具进行安装 配置tomcat应用 关闭防火墙和selinux 查看端口开启情况 ​编辑 访问tomcat服务 安装扩展包 重启服务 查看服务 源码安装 进入tomcat官网进行下载 查找自己要用的to…...

因子分解(递归)

1.素分解式(简单版) 任务描述 编写函数&#xff0c;输出一个正整数的素数分解式。主函数的功能为输入若干正整数&#xff08;大于1&#xff09;&#xff0c;输出每一个数的素分解式。素数分解式是指将整数写成若干素数(从小到大)乘积的形式。例如&#xff1a; 202*2*5 362*2*…...

【Python】pandas库---数据分析

大学毕业那年&#xff0c;你成了社会底层群众里&#xff0c;受教育程度最高的一批人。 前言 这是我自己学习Python的第四篇博客总结。后期我会继续把Python学习笔记开源至博客上。 上一期笔记有关Python的NumPy数据分析&#xff0c;没看过的同学可以去看看&#xff1a;【Pyt…...

RabbitMQ 的7种工作模式

RabbitMQ 共提供了7种⼯作模式,进⾏消息传递,. 官⽅⽂档:RabbitMQ Tutorials | RabbitMQ 1.Simple(简单模式) P:⽣产者,也就是要发送消息的程序 C:消费者,消息的接收者 Queue:消息队列,图中⻩⾊背景部分.类似⼀个邮箱,可以缓存消息;⽣产者向其中投递消息,消费者从其中取出消息…...

负载均衡式在线OJ

文章目录 项目介绍所用技术与开发环境所用技术开发环境 项目框架compiler_server模块compiler编译功能comm/util.hpp 编译时的临时文件comm/log.hpp 日志comm/util.hpp 时间戳comm/util.hpp 检查文件是否存在compile_server/compiler.hpp 编译功能总体编写 runner运行功能资源设…...

【3D打印机】启庞KP3S热床加热失败报错err6

最近天冷&#xff0c;打印机预热突然失败&#xff0c;热床无法加热&#xff0c;过了一段时间报错err6&#xff0c;查看另一篇资料说是天气冷原因&#xff0c;导致代码的PID控温部分达不到预期加热效果&#xff0c;从而自检报错&#xff0c;然后资料通过修改3D打印机代码的方式进…...

手游刚开服就被攻击怎么办?如何防御DDoS?

开服初期是手游最脆弱的阶段&#xff0c;极易成为DDoS攻击的目标。一旦遭遇攻击&#xff0c;可能导致服务器瘫痪、玩家流失&#xff0c;甚至造成巨大经济损失。本文为开发者提供一套简洁有效的应急与防御方案&#xff0c;帮助快速应对并构建长期防护体系。 一、遭遇攻击的紧急应…...

智慧工地云平台源码,基于微服务架构+Java+Spring Cloud +UniApp +MySql

智慧工地管理云平台系统&#xff0c;智慧工地全套源码&#xff0c;java版智慧工地源码&#xff0c;支持PC端、大屏端、移动端。 智慧工地聚焦建筑行业的市场需求&#xff0c;提供“平台网络终端”的整体解决方案&#xff0c;提供劳务管理、视频管理、智能监测、绿色施工、安全管…...

深入浅出:JavaScript 中的 `window.crypto.getRandomValues()` 方法

深入浅出&#xff1a;JavaScript 中的 window.crypto.getRandomValues() 方法 在现代 Web 开发中&#xff0c;随机数的生成看似简单&#xff0c;却隐藏着许多玄机。无论是生成密码、加密密钥&#xff0c;还是创建安全令牌&#xff0c;随机数的质量直接关系到系统的安全性。Jav…...

【SQL学习笔记1】增删改查+多表连接全解析(内附SQL免费在线练习工具)

可以使用Sqliteviz这个网站免费编写sql语句&#xff0c;它能够让用户直接在浏览器内练习SQL的语法&#xff0c;不需要安装任何软件。 链接如下&#xff1a; sqliteviz 注意&#xff1a; 在转写SQL语法时&#xff0c;关键字之间有一个特定的顺序&#xff0c;这个顺序会影响到…...

NXP S32K146 T-Box 携手 SD NAND(贴片式TF卡):驱动汽车智能革新的黄金组合

在汽车智能化的汹涌浪潮中&#xff0c;车辆不再仅仅是传统的交通工具&#xff0c;而是逐步演变为高度智能的移动终端。这一转变的核心支撑&#xff0c;来自于车内关键技术的深度融合与协同创新。车载远程信息处理盒&#xff08;T-Box&#xff09;方案&#xff1a;NXP S32K146 与…...

springboot整合VUE之在线教育管理系统简介

可以学习到的技能 学会常用技术栈的使用 独立开发项目 学会前端的开发流程 学会后端的开发流程 学会数据库的设计 学会前后端接口调用方式 学会多模块之间的关联 学会数据的处理 适用人群 在校学生&#xff0c;小白用户&#xff0c;想学习知识的 有点基础&#xff0c;想要通过项…...

20个超级好用的 CSS 动画库

分享 20 个最佳 CSS 动画库。 它们中的大多数将生成纯 CSS 代码&#xff0c;而不需要任何外部库。 1.Animate.css 一个开箱即用型的跨浏览器动画库&#xff0c;可供你在项目中使用。 2.Magic Animations CSS3 一组简单的动画&#xff0c;可以包含在你的网页或应用项目中。 3.An…...

C#中的CLR属性、依赖属性与附加属性

CLR属性的主要特征 封装性&#xff1a; 隐藏字段的实现细节 提供对字段的受控访问 访问控制&#xff1a; 可单独设置get/set访问器的可见性 可创建只读或只写属性 计算属性&#xff1a; 可以在getter中执行计算逻辑 不需要直接对应一个字段 验证逻辑&#xff1a; 可以…...

基于IDIG-GAN的小样本电机轴承故障诊断

目录 🔍 核心问题 一、IDIG-GAN模型原理 1. 整体架构 2. 核心创新点 (1) ​梯度归一化(Gradient Normalization)​​ (2) ​判别器梯度间隙正则化(Discriminator Gradient Gap Regularization)​​ (3) ​自注意力机制(Self-Attention)​​ 3. 完整损失函数 二…...

【FTP】ftp文件传输会丢包吗?批量几百个文件传输,有一些文件没有传输完整,如何解决?

FTP&#xff08;File Transfer Protocol&#xff09;本身是一个基于 TCP 的协议&#xff0c;理论上不会丢包。但 FTP 文件传输过程中仍可能出现文件不完整、丢失或损坏的情况&#xff0c;主要原因包括&#xff1a; ✅ 一、FTP传输可能“丢包”或文件不完整的原因 原因描述网络…...