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中自带一个简单的梯形速度爬坡算法,本文分析下这部分代码。 代码如下: #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 创建一个动态蜈蚣。蜈蚣由多个节段组成,每个节段看起来像一个小圆形,并且每个节段上都附带有“脚”。蜈蚣的头部会在画布上随机移动。 完整代码在底部!&…...

计算机视觉目标检测——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. 摘要(Abstract)2. 引言(Introduction&#…...

uniapp .gitignore
打开HBuilderX,在项目根目录下新建文件 .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的一个案例。但是存在很多问题,现在我们要做优化,优化的步骤如下: 每个Fruit请求都需…...
备忘一个FDBatchMove数据转存的问题
使用FDBatchMove的SQL导入excel表到sql表,设置条件时一头雾水,函数不遵守sql的规则。 比如替换字段的TAB键值为空,replace(字段名,char(9),)竟然提示错误,百思不得其解。 试遍了几乎所有的函数,竟然是chr(9)。 这个…...

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

【更新】LLM Interview
课程链接:BV1o217YeELo 文章目录 LLM基础相关1. LLMs概述2. 大语言模型尺寸3. LLMs的优势与劣势4. 常见的大模型分类5. 目前主流的LLMs开源模型体系有哪些(Prefix Decoder,Causal Decoder,Encoder-Decoder的区别是什么)…...
Django 视图中使用 Redis 缓存优化查询性能
在 Web 应用程序开发中,查询数据库是一个常见的操作,但如果查询过于频繁或耗时,就会影响应用程序的性能。为了解决这个问题,我们可以使用缓存技术,将查询结果暂时存储在内存中,从而减少对数据库的访问。本文将介绍如何在 Django 视图中使用 Redis 缓存来优化查询性能。 © …...
正则表达式解析与功能说明
正则表达式解析与功能说明 表达式说明 String regex "\\#\\{TOASRTRINNG\\((.*?)((.*?))\\)(\\})";该正则表达式的作用是匹配形如 #{TOASRTRINNG(...)} 的字符串格式。以下是正则表达式的详细解析: 拆解与解析 1. \\# 匹配:# 字符。说明…...
STUN服务器实现NAT穿透
NAT穿透的问题 在现代网络环境中,大多数设备都位于NAT(网络地址转换)设备后面。这给点对点(P2P)通信带来了挑战,因为NAT会阻止外部网络直接访问内部设备。STUN(Session Traversal Utilities for NAT)服务器就是为了解决这个问题而设计的。 STUN是什么?…...
音视频入门基础:MPEG2-TS专题(19)——FFmpeg源码中,解析TS流中的PES流的实现
一、引言 FFmpeg源码在解析完PMT表后,会得到该节目包含的视频和音频信息,从而找到音视频流。TS流的音视频流包含在PES流中。FFmpeg源码通过调用函数指针tss->u.pes_filter.pes_cb指向的回调函数解析PES流的PES packet: /* handle one TS…...

tomcat的安装以及配置(基于linuxOS)
目录 安装jdk环境 yum安装 验证JDK环境 安装tomcat应用 yum安装 编辑 使用yum工具进行安装 配置tomcat应用 关闭防火墙和selinux 查看端口开启情况 编辑 访问tomcat服务 安装扩展包 重启服务 查看服务 源码安装 进入tomcat官网进行下载 查找自己要用的to…...
因子分解(递归)
1.素分解式(简单版) 任务描述 编写函数,输出一个正整数的素数分解式。主函数的功能为输入若干正整数(大于1),输出每一个数的素分解式。素数分解式是指将整数写成若干素数(从小到大)乘积的形式。例如: 202*2*5 362*2*…...

【Python】pandas库---数据分析
大学毕业那年,你成了社会底层群众里,受教育程度最高的一批人。 前言 这是我自己学习Python的第四篇博客总结。后期我会继续把Python学习笔记开源至博客上。 上一期笔记有关Python的NumPy数据分析,没看过的同学可以去看看:【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
最近天冷,打印机预热突然失败,热床无法加热,过了一段时间报错err6,查看另一篇资料说是天气冷原因,导致代码的PID控温部分达不到预期加热效果,从而自检报错,然后资料通过修改3D打印机代码的方式进…...

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

智慧工地云平台源码,基于微服务架构+Java+Spring Cloud +UniApp +MySql
智慧工地管理云平台系统,智慧工地全套源码,java版智慧工地源码,支持PC端、大屏端、移动端。 智慧工地聚焦建筑行业的市场需求,提供“平台网络终端”的整体解决方案,提供劳务管理、视频管理、智能监测、绿色施工、安全管…...
深入浅出:JavaScript 中的 `window.crypto.getRandomValues()` 方法
深入浅出:JavaScript 中的 window.crypto.getRandomValues() 方法 在现代 Web 开发中,随机数的生成看似简单,却隐藏着许多玄机。无论是生成密码、加密密钥,还是创建安全令牌,随机数的质量直接关系到系统的安全性。Jav…...

【SQL学习笔记1】增删改查+多表连接全解析(内附SQL免费在线练习工具)
可以使用Sqliteviz这个网站免费编写sql语句,它能够让用户直接在浏览器内练习SQL的语法,不需要安装任何软件。 链接如下: sqliteviz 注意: 在转写SQL语法时,关键字之间有一个特定的顺序,这个顺序会影响到…...

NXP S32K146 T-Box 携手 SD NAND(贴片式TF卡):驱动汽车智能革新的黄金组合
在汽车智能化的汹涌浪潮中,车辆不再仅仅是传统的交通工具,而是逐步演变为高度智能的移动终端。这一转变的核心支撑,来自于车内关键技术的深度融合与协同创新。车载远程信息处理盒(T-Box)方案:NXP S32K146 与…...

springboot整合VUE之在线教育管理系统简介
可以学习到的技能 学会常用技术栈的使用 独立开发项目 学会前端的开发流程 学会后端的开发流程 学会数据库的设计 学会前后端接口调用方式 学会多模块之间的关联 学会数据的处理 适用人群 在校学生,小白用户,想学习知识的 有点基础,想要通过项…...

20个超级好用的 CSS 动画库
分享 20 个最佳 CSS 动画库。 它们中的大多数将生成纯 CSS 代码,而不需要任何外部库。 1.Animate.css 一个开箱即用型的跨浏览器动画库,可供你在项目中使用。 2.Magic Animations CSS3 一组简单的动画,可以包含在你的网页或应用项目中。 3.An…...
C#中的CLR属性、依赖属性与附加属性
CLR属性的主要特征 封装性: 隐藏字段的实现细节 提供对字段的受控访问 访问控制: 可单独设置get/set访问器的可见性 可创建只读或只写属性 计算属性: 可以在getter中执行计算逻辑 不需要直接对应一个字段 验证逻辑: 可以…...

基于IDIG-GAN的小样本电机轴承故障诊断
目录 🔍 核心问题 一、IDIG-GAN模型原理 1. 整体架构 2. 核心创新点 (1) 梯度归一化(Gradient Normalization) (2) 判别器梯度间隙正则化(Discriminator Gradient Gap Regularization) (3) 自注意力机制(Self-Attention) 3. 完整损失函数 二…...
【FTP】ftp文件传输会丢包吗?批量几百个文件传输,有一些文件没有传输完整,如何解决?
FTP(File Transfer Protocol)本身是一个基于 TCP 的协议,理论上不会丢包。但 FTP 文件传输过程中仍可能出现文件不完整、丢失或损坏的情况,主要原因包括: ✅ 一、FTP传输可能“丢包”或文件不完整的原因 原因描述网络…...