当前位置: 首页 > 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打印机代码的方式进…...

19c补丁后oracle属主变化,导致不能识别磁盘组

补丁后服务器重启&#xff0c;数据库再次无法启动 ORA01017: invalid username/password; logon denied Oracle 19c 在打上 19.23 或以上补丁版本后&#xff0c;存在与用户组权限相关的问题。具体表现为&#xff0c;Oracle 实例的运行用户&#xff08;oracle&#xff09;和集…...

多模态2025:技术路线“神仙打架”,视频生成冲上云霄

文&#xff5c;魏琳华 编&#xff5c;王一粟 一场大会&#xff0c;聚集了中国多模态大模型的“半壁江山”。 智源大会2025为期两天的论坛中&#xff0c;汇集了学界、创业公司和大厂等三方的热门选手&#xff0c;关于多模态的集中讨论达到了前所未有的热度。其中&#xff0c;…...

【Linux】shell脚本忽略错误继续执行

在 shell 脚本中&#xff0c;可以使用 set -e 命令来设置脚本在遇到错误时退出执行。如果你希望脚本忽略错误并继续执行&#xff0c;可以在脚本开头添加 set e 命令来取消该设置。 举例1 #!/bin/bash# 取消 set -e 的设置 set e# 执行命令&#xff0c;并忽略错误 rm somefile…...

Linux链表操作全解析

Linux C语言链表深度解析与实战技巧 一、链表基础概念与内核链表优势1.1 为什么使用链表&#xff1f;1.2 Linux 内核链表与用户态链表的区别 二、内核链表结构与宏解析常用宏/函数 三、内核链表的优点四、用户态链表示例五、双向循环链表在内核中的实现优势5.1 插入效率5.2 安全…...

Spring Boot 实现流式响应(兼容 2.7.x)

在实际开发中&#xff0c;我们可能会遇到一些流式数据处理的场景&#xff0c;比如接收来自上游接口的 Server-Sent Events&#xff08;SSE&#xff09; 或 流式 JSON 内容&#xff0c;并将其原样中转给前端页面或客户端。这种情况下&#xff0c;传统的 RestTemplate 缓存机制会…...

MySQL 隔离级别:脏读、幻读及不可重复读的原理与示例

一、MySQL 隔离级别 MySQL 提供了四种隔离级别,用于控制事务之间的并发访问以及数据的可见性,不同隔离级别对脏读、幻读、不可重复读这几种并发数据问题有着不同的处理方式,具体如下: 隔离级别脏读不可重复读幻读性能特点及锁机制读未提交(READ UNCOMMITTED)允许出现允许…...

LeetCode - 394. 字符串解码

题目 394. 字符串解码 - 力扣&#xff08;LeetCode&#xff09; 思路 使用两个栈&#xff1a;一个存储重复次数&#xff0c;一个存储字符串 遍历输入字符串&#xff1a; 数字处理&#xff1a;遇到数字时&#xff0c;累积计算重复次数左括号处理&#xff1a;保存当前状态&a…...

在 Nginx Stream 层“改写”MQTT ngx_stream_mqtt_filter_module

1、为什么要修改 CONNECT 报文&#xff1f; 多租户隔离&#xff1a;自动为接入设备追加租户前缀&#xff0c;后端按 ClientID 拆分队列。零代码鉴权&#xff1a;将入站用户名替换为 OAuth Access-Token&#xff0c;后端 Broker 统一校验。灰度发布&#xff1a;根据 IP/地理位写…...

SpringBoot+uniapp 的 Champion 俱乐部微信小程序设计与实现,论文初版实现

摘要 本论文旨在设计并实现基于 SpringBoot 和 uniapp 的 Champion 俱乐部微信小程序&#xff0c;以满足俱乐部线上活动推广、会员管理、社交互动等需求。通过 SpringBoot 搭建后端服务&#xff0c;提供稳定高效的数据处理与业务逻辑支持&#xff1b;利用 uniapp 实现跨平台前…...

Java 加密常用的各种算法及其选择

在数字化时代&#xff0c;数据安全至关重要&#xff0c;Java 作为广泛应用的编程语言&#xff0c;提供了丰富的加密算法来保障数据的保密性、完整性和真实性。了解这些常用加密算法及其适用场景&#xff0c;有助于开发者在不同的业务需求中做出正确的选择。​ 一、对称加密算法…...