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打印机代码的方式进…...

19c补丁后oracle属主变化,导致不能识别磁盘组
补丁后服务器重启,数据库再次无法启动 ORA01017: invalid username/password; logon denied Oracle 19c 在打上 19.23 或以上补丁版本后,存在与用户组权限相关的问题。具体表现为,Oracle 实例的运行用户(oracle)和集…...

多模态2025:技术路线“神仙打架”,视频生成冲上云霄
文|魏琳华 编|王一粟 一场大会,聚集了中国多模态大模型的“半壁江山”。 智源大会2025为期两天的论坛中,汇集了学界、创业公司和大厂等三方的热门选手,关于多模态的集中讨论达到了前所未有的热度。其中,…...
【Linux】shell脚本忽略错误继续执行
在 shell 脚本中,可以使用 set -e 命令来设置脚本在遇到错误时退出执行。如果你希望脚本忽略错误并继续执行,可以在脚本开头添加 set e 命令来取消该设置。 举例1 #!/bin/bash# 取消 set -e 的设置 set e# 执行命令,并忽略错误 rm somefile…...
Linux链表操作全解析
Linux C语言链表深度解析与实战技巧 一、链表基础概念与内核链表优势1.1 为什么使用链表?1.2 Linux 内核链表与用户态链表的区别 二、内核链表结构与宏解析常用宏/函数 三、内核链表的优点四、用户态链表示例五、双向循环链表在内核中的实现优势5.1 插入效率5.2 安全…...
Spring Boot 实现流式响应(兼容 2.7.x)
在实际开发中,我们可能会遇到一些流式数据处理的场景,比如接收来自上游接口的 Server-Sent Events(SSE) 或 流式 JSON 内容,并将其原样中转给前端页面或客户端。这种情况下,传统的 RestTemplate 缓存机制会…...
MySQL 隔离级别:脏读、幻读及不可重复读的原理与示例
一、MySQL 隔离级别 MySQL 提供了四种隔离级别,用于控制事务之间的并发访问以及数据的可见性,不同隔离级别对脏读、幻读、不可重复读这几种并发数据问题有着不同的处理方式,具体如下: 隔离级别脏读不可重复读幻读性能特点及锁机制读未提交(READ UNCOMMITTED)允许出现允许…...

LeetCode - 394. 字符串解码
题目 394. 字符串解码 - 力扣(LeetCode) 思路 使用两个栈:一个存储重复次数,一个存储字符串 遍历输入字符串: 数字处理:遇到数字时,累积计算重复次数左括号处理:保存当前状态&a…...
在 Nginx Stream 层“改写”MQTT ngx_stream_mqtt_filter_module
1、为什么要修改 CONNECT 报文? 多租户隔离:自动为接入设备追加租户前缀,后端按 ClientID 拆分队列。零代码鉴权:将入站用户名替换为 OAuth Access-Token,后端 Broker 统一校验。灰度发布:根据 IP/地理位写…...

SpringBoot+uniapp 的 Champion 俱乐部微信小程序设计与实现,论文初版实现
摘要 本论文旨在设计并实现基于 SpringBoot 和 uniapp 的 Champion 俱乐部微信小程序,以满足俱乐部线上活动推广、会员管理、社交互动等需求。通过 SpringBoot 搭建后端服务,提供稳定高效的数据处理与业务逻辑支持;利用 uniapp 实现跨平台前…...
Java 加密常用的各种算法及其选择
在数字化时代,数据安全至关重要,Java 作为广泛应用的编程语言,提供了丰富的加密算法来保障数据的保密性、完整性和真实性。了解这些常用加密算法及其适用场景,有助于开发者在不同的业务需求中做出正确的选择。 一、对称加密算法…...