Apollo基础 - Frenet坐标系

Frenet与笛卡尔坐标系的转换详细推导见:b站老王 自动驾驶决策规划学习记录(四)
在这里插入图片描述
在这里插入图片描述
Apollo相关代码:
modules/common/math/cartesian_frenet_conversion.h

#pragma once
#include <array>
#include "modules/common/math/vec2d.h"
namespace apollo {
namespace common {
namespace math {
// Notations:
// s_condition = [s, s_dot, s_ddot]
// s: 纵向坐标 w.r.t reference line. 
// s_dot: ds / dt
// s_ddot: d(s_dot) / dt
// d_condition = [d, d_prime, d_pprime]
// d: 横向坐标 w.r.t. reference line 
// d_prime: dd / ds
// d_pprime: d(d_prime) / ds
// l: the same as d.
class CartesianFrenetConverter {public:CartesianFrenetConverter() = delete;// 将笛卡尔坐标系转换为Frenet坐标系。将一个二维运动解耦为两个独立相对于参考线的一维运动。横向运动是纵向累积距离s的函数,以更好地满足非完整约束。static void cartesian_to_frenet(const double rs, const double rx,const double ry, const double rtheta,const double rkappa, const double rdkappa,const double x, const double y,const double v, const double a,const double theta, const double kappa,std::array<double, 3>* const ptr_s_condition,std::array<double, 3>* const ptr_d_condition);static void cartesian_to_frenet(const double rs, const double rx,const double ry, const double rtheta,const double x, const double y, double* ptr_s,double* ptr_d);// 将Frenet框架中的车辆状态转换为笛卡尔框架。将两个相对于参考线的独立1d移动合并为2d移动static void frenet_to_cartesian(const double rs, const double rx,const double ry, const double rtheta,const double rkappa, const double rdkappa,const std::array<double, 3>& s_condition,const std::array<double, 3>& d_condition,double* const ptr_x, double* const ptr_y,double* const ptr_theta,double* const ptr_kappa, double* const ptr_v,double* const ptr_a);// given sl point extract x, y, theta, kappastatic double CalculateTheta(const double rtheta, const double rkappa,const double l, const double dl);static double CalculateKappa(const double rkappa, const double rdkappa,const double l, const double dl,const double ddl);static Vec2d CalculateCartesianPoint(const double rtheta, const Vec2d& rpoint,const double l);/*** @brief: given sl, theta, and road's theta, kappa, extract derivative l,*second order derivative l:*/static double CalculateLateralDerivative(const double theta_ref,const double theta, const double l,const double kappa_ref);// given sl, theta, and road's theta, kappa, extract second order derivativestatic double CalculateSecondOrderLateralDerivative(const double theta_ref, const double theta, const double kappa_ref,const double kappa, const double dkappa_ref, const double l);
};}  // namespace math
}  // namespace common
}  // namespace apollo

modules/common/math/cartesian_frenet_conversion.cc

cartesian_to_frenet函数:计算出SL的一阶导和二阶导数

void CartesianFrenetConverter::cartesian_to_frenet(const double rs, const double rx, const double ry, const double rtheta,const double rkappa, const double rdkappa, const double x, const double y,const double v, const double a, const double theta, const double kappa,std::array<double, 3>* const ptr_s_condition,std::array<double, 3>* const ptr_d_condition) {const double dx = x - rx;const double dy = y - ry;const double cos_theta_r = std::cos(rtheta);const double sin_theta_r = std::sin(rtheta);const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;ptr_d_condition->at(0) =std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);const double delta_theta = theta - rtheta;const double tan_delta_theta = std::tan(delta_theta);const double cos_delta_theta = std::cos(delta_theta);const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;const double kappa_r_d_prime =rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);ptr_d_condition->at(2) =-kappa_r_d_prime * tan_delta_theta +one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *(kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);ptr_s_condition->at(0) = rs;ptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d;const double delta_theta_prime =one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;ptr_s_condition->at(2) =(a * cos_delta_theta -ptr_s_condition->at(1) * ptr_s_condition->at(1) *(ptr_d_condition->at(1) * delta_theta_prime - kappa_r_d_prime)) /one_minus_kappa_r_d;
}

cartesian_to_frenet作用是计算出SL的一阶导数和二阶导数

void CartesianFrenetConverter::cartesian_to_frenet(const double rs, const double rx, const double ry, const double rtheta,const double rkappa, const double rdkappa, const double x, const double y,const double v, const double a, const double theta, const double kappa,std::array<double, 3>* const ptr_s_condition,std::array<double, 3>* const ptr_d_condition)

前六个参数是中心线上投影的点的信息 ( s r , x r , y r , θ r , k r , k r ′ ) (s_r,x_r,y_r,\theta_r,k_r,k^{'}_r) (sr,xr,yr,θr,kr,kr),中间的六个参数是轨迹上相应点的信息 ( x , y , v , a , θ , k ) (x,y,v,a,\theta,k) (x,y,v,a,θ,k),最后两个参数是输出,分别对应 ( s , s ˙ , s ¨ ) (s,\dot{s} ,\ddot{s} ) (s,s˙,s¨) ( l , l ′ , l ′ ′ ) (l,l',l'') (l,l,l′′)

  const double dx = x - rx;const double dy = y - ry;const double cos_theta_r = std::cos(rtheta);const double sin_theta_r = std::sin(rtheta);const double cross_rd_nd = cos_theta_r * dy - sin_theta_r * dx;ptr_d_condition->at(0) =std::copysign(std::sqrt(dx * dx + dy * dy), cross_rd_nd);

std::copysign是C++标准库中的一个实用函数,用于复制一个数的符号并返回一个新的数
接受两个参数:x和y。它返回一个新的数,其值与x相同,但符号与y相同

在这里插入图片描述

  const double delta_theta = theta - rtheta;const double tan_delta_theta = std::tan(delta_theta);const double cos_delta_theta = std::cos(delta_theta);const double one_minus_kappa_r_d = 1 - rkappa * ptr_d_condition->at(0);ptr_d_condition->at(1) = one_minus_kappa_r_d * tan_delta_theta;

l ′ = ( 1 − k r l ) t a n ( θ x − θ r ) l'=(1-k_rl)tan(\theta_x-\theta_r) l=(1krl)tan(θxθr)

  const double kappa_r_d_prime =rdkappa * ptr_d_condition->at(0) + rkappa * ptr_d_condition->at(1);ptr_d_condition->at(2) =-kappa_r_d_prime * tan_delta_theta +one_minus_kappa_r_d / cos_delta_theta / cos_delta_theta *(kappa * one_minus_kappa_r_d / cos_delta_theta - rkappa);

在这里插入图片描述

  ptr_s_condition->at(0) = rs; // sptr_s_condition->at(1) = v * cos_delta_theta / one_minus_kappa_r_d; // s'const double delta_theta_prime =one_minus_kappa_r_d / cos_delta_theta * kappa - rkappa;ptr_s_condition->at(2) =(a * cos_delta_theta -ptr_s_condition->at(1) * ptr_s_condition->at(1) *(ptr_d_condition->at(1) * delta_theta_prime - kappa_r_d_prime)) /one_minus_kappa_r_d; // s''
}

在这里插入图片描述
frenet_to_cartesian函数:计算笛卡尔坐标系下的轨迹点 ( x , y , θ , k , v , a ) (x,y,\theta,k,v,a) (x,y,θ,k,v,a)

void CartesianFrenetConverter::frenet_to_cartesian(const double rs, const double rx, const double ry, const double rtheta,const double rkappa, const double rdkappa,const std::array<double, 3>& s_condition,const std::array<double, 3>& d_condition, double* const ptr_x,double* const ptr_y, double* const ptr_theta, double* const ptr_kappa,double* const ptr_v, double* const ptr_a) {ACHECK(std::abs(rs - s_condition[0]) < 1.0e-6)<< "The reference point s and s_condition[0] don't match";const double cos_theta_r = std::cos(rtheta);const double sin_theta_r = std::sin(rtheta);*ptr_x = rx - sin_theta_r * d_condition[0];*ptr_y = ry + cos_theta_r * d_condition[0];const double one_minus_kappa_r_d = 1 - rkappa * d_condition[0];const double tan_delta_theta = d_condition[1] / one_minus_kappa_r_d;const double delta_theta = std::atan2(d_condition[1], one_minus_kappa_r_d);const double cos_delta_theta = std::cos(delta_theta);*ptr_theta = NormalizeAngle(delta_theta + rtheta);const double kappa_r_d_prime =rdkappa * d_condition[0] + rkappa * d_condition[1];*ptr_kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *cos_delta_theta * cos_delta_theta) /(one_minus_kappa_r_d) +rkappa) *cos_delta_theta / (one_minus_kappa_r_d);const double d_dot = d_condition[1] * s_condition[1];*ptr_v = std::sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *s_condition[1] * s_condition[1] +d_dot * d_dot);const double delta_theta_prime =one_minus_kappa_r_d / cos_delta_theta * (*ptr_kappa) - rkappa;*ptr_a = s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +s_condition[1] * s_condition[1] / cos_delta_theta *(d_condition[1] * delta_theta_prime - kappa_r_d_prime);
}

ACHECK函数,它是一个简单的断言函数,用于检查一个条件是否满足。如果条件不满足,它将抛出一个异常,并附带一个错误信息。在这段代码中,rs和s_condition[0]是两个浮点数,它们被用于计算参考点的位置。std::abs(rs - s_condition[0]) < 1.0e-6是一个比较表达式,用于检查rs和s_condition[0]之间的差异是否小于1.0e-6。如果这个条件不满足,ACHECK函数将抛出一个异常,并附带一个错误信息,指出参考点的位置不匹配。

ACHECK(std::abs(rs - s_condition[0]) < 1.0e-6)<< "The reference point s and s_condition[0] don't match";

在这里插入图片描述

const double cos_theta_r = std::cos(rtheta);const double sin_theta_r = std::sin(rtheta);*ptr_x = rx - sin_theta_r * d_condition[0];*ptr_y = ry + cos_theta_r * d_condition[0];

在这里插入图片描述

const double one_minus_kappa_r_d = 1 - rkappa * d_condition[0];const double tan_delta_theta = d_condition[1] / one_minus_kappa_r_d;const double delta_theta = std::atan2(d_condition[1], one_minus_kappa_r_d);const double cos_delta_theta = std::cos(delta_theta);*ptr_theta = NormalizeAngle(delta_theta + rtheta);

在这里插入图片描述
在这里插入图片描述

const double kappa_r_d_prime =rdkappa * d_condition[0] + rkappa * d_condition[1];*ptr_kappa = (((d_condition[2] + kappa_r_d_prime * tan_delta_theta) *cos_delta_theta * cos_delta_theta) /(one_minus_kappa_r_d) +rkappa) *cos_delta_theta / (one_minus_kappa_r_d);const double d_dot = d_condition[1] * s_condition[1];*ptr_v = std::sqrt(one_minus_kappa_r_d * one_minus_kappa_r_d *s_condition[1] * s_condition[1] +d_dot * d_dot);

在这里插入图片描述

const double delta_theta_prime =one_minus_kappa_r_d / cos_delta_theta * (*ptr_kappa) - rkappa;*ptr_a = s_condition[2] * one_minus_kappa_r_d / cos_delta_theta +s_condition[1] * s_condition[1] / cos_delta_theta *(d_condition[1] * delta_theta_prime - kappa_r_d_prime);

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

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

相关文章

怎么一边讲PPT一边录视频 如何一边录制PPT一边录制人像 录屏软件免费录屏 PPT录制怎么录制

随着新媒体技术的发展&#xff0c;短视频和直播越来越火。越来越多的小伙伴加入了视频制作的大军&#xff0c;那么你想知道怎么一边讲PPT一边录视频&#xff0c;如何一边录制PPT一边录制人像吗&#xff1f; 一、怎么一边讲PPT一边录视频 我们可以借助PPT本身自带的屏幕录制功能…

Linux的发展历程:从诞生到全球应用

一、前言 Linux作为一个开源操作系统&#xff0c;经历了令人瞩目的发展历程。从最初的创意到如今在全球范围内得到广泛应用&#xff0c;Linux不仅是技术的杰出代表&#xff0c;更是开源精神的典范。本文将追溯Linux的发展历程&#xff0c;深入了解它是如何从一个个人项目演变为…

【docker笔记】Docker容器数据卷

Docker容器数据卷 卷就是目录或者文件&#xff0c;存在于一个或多个容器中&#xff0c;由docker挂载到容器&#xff0c;但不属于联合文件系统&#xff0c;因此能够绕过Union File System提供一些用于持续存储或共享数据的特性 卷的设计目的就是数据的持久化&#xff0c;完全独…

kotlin map{}和mapOf{}

map{}的作用 map 让集合中的每个元素应用给定的转换函数&#xff08;transform&#xff09;&#xff0c;然后生成并返回一个新的 List<R> val numbers listOf(1, 2, 3, 4, 5) //map 让集合中的每个元素应用给定的转换函数&#xff08;transform&#xff09;&#xff0…

VMware Workstation安装以及配置模板机

文章目录 一、VMware Workstation软件下载安装1、下载2、安装 二、CentOS7模板机安装1、创建虚拟机2、安装系统 三、网络配置 一、VMware Workstation软件下载安装 1、下载 下载地址&#xff1a;https://download3.vmware.com/software/wkst/file/VMware-workstation-full-15…

css中的变量和辅助函数

变量 --name 两个破折号加变量名称&#xff08;可以在当前的选择器内定义&#xff09;var(--*) 命名规则 body {--深蓝: #369;background-color: var(--深蓝); } 变量值只能做用属性值&#xff0c;不能用做属性名。变量命名不能包含 $,[,^,(,% 等字符 普通字符局限在只要是数…

软件测试|MySQL逻辑运算符使用详解

简介 在MySQL中&#xff0c;逻辑运算符用于处理布尔类型的数据&#xff0c;进行逻辑判断和组合条件。逻辑运算符主要包括AND、OR、NOT三种&#xff0c;它们可以帮助我们在查询和条件语句中进行复杂的逻辑操作。本文将详细介绍MySQL中逻辑运算符的使用方法和示例。 AND运算符 …

邮政快递单号查询入口,对快递单号进行提前签收分析

一款优秀的快递单号筛选软件能够给你的工作和生活带来极大的便利。通过合理选择和使用该软件&#xff0c;你将能够轻松管理、高效筛选快递单号&#xff0c;提升工作效率和生活品质。不妨试试我们的【快递批量查询高手】&#xff0c;让你的物流管理更加智能、便捷&#xff01; …

FreeRTOS——信号量

学习目标 理解信号量的概念掌握信号量发流程掌握二进制信号量熟悉计数型信号量掌握互斥信号量熟悉递归互斥信号量学习内容 概念 在 FreeRTOS 中,信号量(Semaphore)是一种用于实现任务之间同步和资源共享的机制。它是一种计数型的同步原语,用于控制对共享资源的访问和保护…

Pytest成魔之路 —— fixture 之大解剖!

1. 简介 fixture是pytest的一个闪光点&#xff0c;pytest要精通怎么能不学习fixture呢&#xff1f;跟着我一起深入学习fixture吧。其实unittest和nose都支持fixture&#xff0c;但是pytest做得更炫。 fixture是pytest特有的功能&#xff0c;它用pytest.fixture标识&#xff0c…

Java Servlet 的MIME类型和SpringMVC对其的替代注解

在Java Servlet中&#xff0c;MIME&#xff08;Multipurpose Internet Mail Extensions&#xff09;类型通常用于指定将要发送给客户端的内容的类型。以下是一些常见的MIME类型&#xff0c;可以在Servlet中使用&#xff1a; 1. 文本类型&#xff1a; - **text/plain:** 普通…

9. 回文数(Java)

题目描述&#xff1a; 给你一个整数 x &#xff0c;如果 x 是一个回文整数&#xff0c;返回 true &#xff1b;否则&#xff0c;返回 false 。 回文数是指正序&#xff08;从左向右&#xff09;和倒序&#xff08;从右向左&#xff09;读都是一样的整数。 例如&#xff0c;121 …

【算法Hot100系列】搜索插入位置

💝💝💝欢迎来到我的博客,很高兴能够在这里和您见面!希望您在这里可以感受到一份轻松愉快的氛围,不仅可以获得有趣的内容和知识,也可以畅所欲言、分享您的想法和见解。 推荐:kwan 的首页,持续学习,不断总结,共同进步,活到老学到老导航 檀越剑指大厂系列:全面总结 jav…

运用AI翻译漫画(二)

构建代码 构建这个PC桌面应用&#xff0c;我们需要几个步骤&#xff1a; 在得到第一次的显示结果后&#xff0c;经过测试&#xff0c;有很大可能会根据结果再对界面进行调整&#xff0c;实际上也是一个局部的软件工程中的迭代开发。 界面设计 启动Visual Studio 2017, 创建…

并发程序设计--D10线程池及gdb调试多线程

线程池 概念&#xff1a; 通俗的讲就是一个线程的池子&#xff0c;可以循环的完成任务的一组线程集合 必要性&#xff1a; 我们平时创建一个线程&#xff0c;完成某一个任务&#xff0c;等待线程的退出。但当需要创建大量的线程时&#xff0c;假设T1为创建线程时间&#xf…

贯穿设计模式-中介模式+模版模式

样例代码 涉及到的项目样例代码均可以从https://github.com/WeiXiao-Hyy/Design-Patterns.git获取 需求 购买商品时会存在着朋友代付的场景&#xff0c;可以抽象为购买者&#xff0c;支付者和中介者之间的关系 -> 中介者模式下单&#xff0c;支付&#xff0c;发货&#xff0…

VR思政情景实训教学

在传统的思政教育中&#xff0c;学生通常是通过课本阅读和讲堂听讲来获取知识。虽然这种方式可以传递基础知识&#xff0c;但对于学生的思维开拓和情感体验存在一定的局限性。而VR思政情景实训教学应用则能够打破这种传统方式的限制&#xff0c;为学生提供沉浸式的学习体验。 通…

什么是软件测试

一、软件测试的定义 软件测试的经典定义是在规定条件下对程序进行操作&#xff0c;以发现错误&#xff0c;对软件质量进行评估。因为软件是由文档、数据以及程序组成的&#xff0c;所以软件测试的对象也就不仅仅是程序本身&#xff0c;而是包括软件形成过程的文档、数据以及程…

什么是博若莱新酒节?

在红酒圈儿里混&#xff0c;一定不能不知道博若莱新酒节&#xff0c;这是法国举世闻名的以酒为主题的重要节日之一。现已成为世界范围内庆祝当年葡萄收获和酿制的节日&#xff0c;被称为一年一度的酒迷盛会。 云仓酒庄的品牌雷盛红酒LEESON分享博若莱位于法国勃艮第南部&#x…

Spark Core------算子介绍

RDD基本介绍 什么是RDD RDD:英文全称Resilient Distributed Dataset&#xff0c;叫做弹性分布式数据集&#xff0c;是Spark中最基本的数据抽象&#xff0c;代表一个不可变、可分区、里面的元素可并行计算的集合。 Resilient弹性&#xff1a;RDD的数据可以存储在内存或者磁盘…