Minimum Snap轨迹规划详解(3)闭式求解
如果QP问题只有等式约束没有不等式约束,那么是可以闭式求解(close form)的。闭式求解效率要快很多,而且只需要用到矩阵运算,不需要QPsolver。
这里介绍Nicholas Roy文章中闭式求解的方法。
1. QP等式约束构建
闭式法中的QQ矩阵计算和之前一样(参照文章一),但约束的形式与之前略为不同,在之前的方法中,等式约束只要构造成[...]p=b[...]p=b的形式就可以了,而闭式法中,每段poly都构造成
A_ip_i=d_i,~A_i=[A_0~A_t]^T_i,~d_i=[d_0,d_T]_i
其中 d0,dTd_0,d_T为第 ii段poly的起点和终点的各阶导数组成的向量,比如只考虑PVA:d0=[p0,v0,a0]Td_0=[p_0,v_0,a_0]^T,当然也可以把jerk,snap等加入到向量。注意:这里是不管每段端点的PVA是否已知,都写进来。
块合并各段轨迹的约束方程得到
\underbrace{A_{total}}_{k(n+1) \times 6k} \left[ \begin{matrix} p_1 \\ \vdots \\ p_k \\ \end{matrix} \right] = \left[ \begin{matrix} d_1 \\ \vdots \\ d_k \\ \end{matrix} \right] =\underbrace{ \left[ \begin{matrix} p_1(t_0)\\ v_1(t_0)\\ a_1(t_0)\\ p_1(t_1)\\ v_1(t_1)\\ a_1(t_1)\\ \vdots \\ p_k(t_{k-1})\\ v_k(t_{k-1})\\ a_k(t_{k-1})\\ p_k(t_k)\\ v_k(t_k)\\ a_k(t_k)\\ \end{matrix} \right]}_{6k \times 1}
kk为轨迹段数,nn为轨迹的阶数,设只考虑pva, AtotalA_{total}的size为 (norder+1)k×6k(n_{order}+1)k \times 6k。这里为了简化,没有把每段poly的timestamp都改成从0开始,一般,为了避免timestamp太大引起数值问题,每段poly的timestamp都成0开始。
由上式可以看到, AtotalA_{total}是已知的(怎么构造可参见文章一种的等式约束构造方法),而 dd中只有少部分(起点、终点的pva等)是已知的,其他大部分是未知的。如果能够求出dd,那么轨迹参数可以通过 p=A−1dp=A^{-1}d很容易求得。
2. 如何求d?
闭式法的思路是:将dd向量中的变量分成两部分:”d中所有已知量组成的Fix部分dFd_F”和”所有未知量组成的Free部分dPd_P”。然后通过推导,根据dFd_F求得dPd_P,从而得到dd,最后求得pp。
下面介绍整个推导过程,
2.1. 消除重复变量(连续性约束)
可以会发现,上面构造等式约束时,并没有加入连续性约束,连续性约束并不是直接加到等式约束中。考虑到连续性(这里假设PVA连续),dd向量中很多变量其实重复了,即
p_i(t_i)=p_{i+1}(t_i),~~v_i(t_i)=v_{i+1}(t_i),~~a_i(t_i)=a_{i+1}(t_i)
因此需要一个映射矩阵将一个变量映射到两个重复的变量上,怎么映射?
- 如[aa]=[11]a\left[ \begin{matrix} a\\a \end{matrix} \right]=\left[ \begin{matrix} 1\\1 \end{matrix} \right]a,将变量aa映射到左边向量中的两个变量。
所以构造映射矩阵M6k×3(k+1)M_{6k\times 3(k+1)}:
\underbrace{ \left[ \begin{matrix} d_1 \\ \vdots \\ d_k \\ \end{matrix} \right]}_{6k\times1}= \underbrace{ \left[ \begin{matrix} 1\\ &1\\ &&1\\ &&&1\\ &&&&1\\ &&&&&1\\ &&&1\\ &&&&1\\ &&&&&1\\ &&&&&&1\\ &&&&&&&1\\ &&&&&&&&1\\ &&&&&&&&&\ddots \end{matrix} \right]}_{M} \underbrace{ \left[\begin{matrix} p(t_0)\\ v(t_0)\\ a(t_0)\\ p(t_1)\\ v(t_1)\\ a(t_1)\\ p(t_2)\\ v(t_2)\\ a(t_2)\\ \vdots\\ p(t_k)\\ v(t_k)\\ a(t_k)\\ \end{matrix} \right]}_{3(k+1)\times 1}
即d=Md′d=Md'。
2.2 向量元素置换
消除掉重复变量之后,需要调整d′d'中的变量,把fix部分和free部分分开排列,可以左成一个置换矩阵CC,使得
d' = C\left[\begin{matrix}d_F\\d_P\end{matrix}\right]。
C矩阵怎么构造?
- 举个例子,设d′=⎡⎣⎢⎢⎢⎢abcd⎤⎦⎥⎥⎥⎥d' = \left[\begin{matrix}a\\b\\c\\d\end{matrix}\right],其中a,c,da,c,d是已知(dFd_F),bb未知(dPd_P),构造一个4×44\times4的单位阵,取dFd_F所在的(1,3,4)列放到左边,再取dPd_P所在的(2)列放到右边,就构造出置换矩阵CC:
⎡⎣⎢⎢⎢⎢abcd⎤⎦⎥⎥⎥⎥=⎡⎣⎢⎢⎢⎢1000001000010100⎤⎦⎥⎥⎥⎥C⎡⎣⎢⎢⎢⎢acdb⎤⎦⎥⎥⎥⎥
\left[ \begin{matrix} a\\b\\c\\d \end{matrix}\right] = \underbrace{\left[\begin{matrix}1&0&0&0\\0&0&0&1\\0&1&0&0\\0&0&1&0 \end{matrix}\right]}_C\left[\begin{matrix}a\\c\\d\\b\end{matrix}\right]
2.3 转成无约束优化问题
由上面两步可得
\begin{equation} d=MC\left[\begin{matrix}d_F\\d_P\end{matrix}\right]\\ p=A^{-1}d=\underbrace{A^{-1}MC}_K\left[\begin{matrix}d_F\\d_P\end{matrix}\right] = K\left[\begin{matrix}d_F\\d_P\end{matrix}\right] \end{equation}
代入优化函数
\begin{equation} \begin{aligned} \min J&=p^TQp\\ J&= \left[\begin{matrix}d_F\\d_P\end{matrix}\right]^T\underbrace{K^TQK}_R\left[\begin{matrix}d_F\\d_P\end{matrix}\right]\\ &=\left[\begin{matrix}d_F\\d_P\end{matrix}\right]^T \left[\begin{matrix}R_{FF} & R_{FP}\\R_{PF}&R_{PP}\end{matrix}\right] \left[\begin{matrix}d_F\\d_P\end{matrix}\right]\\ &=d_F^TR_{FF}d_F+d_F^TR_{FP}d_P+d_P^TR_{PF}d_F+d_P^TR_{PP}d_P\\ Q对称 \Rightarrow R对称\Rightarrow &=d_F^TR_{FF}d_F+2d_F^TR_{FP}d_P+d_P^TR_{PP}d_P \end{aligned}\\ \end{equation}
令JJ对dPd_P的导数∂J∂dP=0\frac{\partial J}{\partial{d_P}}=0求极值点:
\begin{equation} \Rightarrow 2d_F^TR_{FP}+2d_P^TR_{PP}d_P=0~~(注意R_{PP}^T=R_{PP})\\ \Rightarrow d_p = -R_{PP}^{-1}R_{FP}^Td_F \end{equation}
至此求得dPd_P,从而求出pp。
3. 闭式法步骤
总结一下整个闭式法的步骤:
- 先确定轨迹阶数(比如5阶),再确定dd向量中的约束量(pva),进而根据各段的时间分配求得Atotal。A_{total}。
- 根据连续性约束构造映射矩阵MM,并确定dd向量中哪些量是Fix(比如起点终点pva,中间点的p等),哪些量是Free,进而构造置换矩阵CC,并求得K=A−1MCK=A^{-1}MC。
- 计算QP目标函数中的Q(minJerk/Snap\min Jerk/Snap)并计算R=KTQKR=K^TQK,根据fix变量的长度将R拆分成RFF,RFP,RPF,RPPR_{FF},R_{FP},R_{PF},R_{PP}四块。
- 填入已知变量得到dFd_F,并根据dp=−R−1PPRTFPdFd_p = -R_{PP}^{-1}R_{FP}^Td_F计算得到d_P。
- 根据公式 p=K[dFdP]计算得到轨迹参数p。p=K\left[\begin{matrix}d_F\\d_P\end{matrix}\right]计算得到轨迹参数p。
闭式法主要计算量就在A矩阵的求逆,其他计算基本上是矩阵构造,所以效率比较高,但由于没有不等式约束,所以在中间点只能加强约束,corridor不能直接加到QP问题中,只能是通过压点来实现corridor。
在对计算效率要求比较高或者不想用QPsolver时,可以使用闭式法求解。代码见这里,由于效果和文章一中的效果一样,这里就不再贴图。
参考文献
- Richter C, Bry A, Roy N. Polynomial trajectory planning for aggressive quadrotor flight in dense indoor environments[M]//Robotics Research. Springer International Publishing, 2016: 649-666.
Minimum Snap轨迹规划详解(3)闭式求解相关推荐
- 【附源码和详细的公式推导】Minimum Snap轨迹生成,闭式求解Minimum Snap问题,机器人轨迹优化,多项式轨迹路径生成与优化
该篇博客内容较多,工作量也很大,难免在理解上表达上有错误,如有发现烦请指教.如有问题在博客中留言,或者github的仓库中提Issues都行,看到后我会尽快回复. Minimum Snap源代码:Mi ...
- (四)路径规划算法---QP解决Minimum Snap轨迹优化问题
QP解决Minimum Snap轨迹优化问题 文章目录 QP解决Minimum Snap轨迹优化问题 1. 多项式的次数确定 2. Minimum Snap案例分析 2.1 轨迹的多项式表达 2.2 ...
- 消息轨迹全景图详解-独门秘籍
简介: 消息轨迹全景图详解-独门秘籍 消息轨迹全景图详解-独门秘籍 消息轨迹全景图详解-独门秘籍 阿里云 IoT 企业物联网平台上线了消息轨迹全景图功能,帮助 IoT 开发者追踪消息通信的完整轨迹,快 ...
- 天津理工大学《操作系统》实验二,存储器的分配与回收算法实现,代码详解,保姆式注释讲解
天津理工大学<操作系统>实验二,存储器的分配与回收算法实现,代码详解,保姆式注释讲解 实验内容 1. 本实验是模拟操作系统的主存分配,运用可变分区的存储管理算法设计主存分配和回收程序,并不 ...
- 详解Vue响应式原理
摘要: 搞懂Vue响应式原理! 作者:浪里行舟 原文:深入浅出Vue响应式原理 Fundebug经授权转载,版权归原作者所有. 前言 Vue 最独特的特性之一,是其非侵入性的响应式系统.数据模型仅仅是 ...
- jenkins流水线(jenkinsfile)详解,保姆式教程
jenkins流水线(jenkinsfile)详解 在学习本篇文章的时候,可以去看看jenkins官网的文档,jenkins官网支持中文,观看十分便捷 此教程使用的是gitee+docker+jenk ...
- [HR规划]人力资源规划详解(zt)
一.什么是人力资源规划? 人力资源计划是指根据企业的发展规划,通过企业未来的人力资源的需要和供给状况的分析及估计.对职务编制.人员配置.教育培训.人力资源管理政策.招聘和选择等内容进行的人力资源部门的 ...
- iOS 地图导航路线规划详解
虽然是转载的,还是说几句吧.网上百度地图导航路线规划倒是挺多的,苹果自带的高德导航确实挺少,研究了好久发现就这个讲的稍微全一点,把需要用到的类什么的都讲清楚了.不过高德有个方法可以跳转到它自己的地图上 ...
- android 定位轨迹功能,小步外勤怎么看轨迹 定位轨迹功能详解
小步外勤是专为企业管理人员开发的考勤管理APP,可实现多地考勤.实时定位.员工轨迹.通讯录.照片上传等众多功能,利用定位功能对外勤人员进行有效管理,下面就跟小编了解下具体步骤吧. 小步外勤 类别:学习 ...
最新文章
- python入门指南bl-Vue 3 高阶指南之 Map
- 奇异值分解 VS 特征值分解
- rideo选中 vue_适用于 Vue 的播放器组件Vue-Video-Player操作
- [DOM Event Learning] Section 2 概念梳理 什么是事件 DOM Event
- 【好文链接】什么是最小二乘法?
- nginx中文url php,使Nginx服务器支持中文URL的相关配置详解
- 详细分析开源软件 ExifTool 的任意代码执行漏洞 (CVE-2021-22204)
- 解题报告 noi 2005 智慧珠游戏(BT 搜索)
- BigData | 一文带你搞清楚“数据倾斜”
- 已经围上为何不算目_万达为何至今不上市,并非王健林真的钱多,许家印的境遇说明一切...
- Linux服务器配置——安装CentOS以及源配置
- Carrot2 in action_初试身手—融入自己的中文分词器
- 如何用计算机装手机系统,如何使用手机给电脑安装Windows10系统?
- 数据洪流时代,企业转型需要修建自己的“都江堰”
- MATLAB 3db波束宽度提取,秒换算毫秒(3db波束宽度估算公式)
- 韩国网站的一些弱口令
- 【陈工笔记】# LaTeX中,单元格数据居中方式 #
- python 删除txt文本指定内容
- CryENGINE 3
- 夺命雷公狗---Smarty NO:03 设计篇1
热门文章
- Windows下oracle client安装exp导出imp导入表
- C语言 三角函数 时钟周期,1.3.1 三角函数的周期性PPT及课堂实录内容
- 思源科技java面试_【企业招聘笔试真题】思源Java开发工程师笔试题
- MATLAB小波分解/重构wavedec/waverec函数中L和C的意义
- BES2300x笔记(6) -- 如何修改蓝牙名称
- Android上基于HSV颜色模型实现取色盘功能
- MobaXterm导入MobaXterm.ini配置
- 程序员都比较容易胖?配合曲仙止弹豆浆健康饮食跟肉肉说再见
- 基于瑞萨RH850的BootLoader调试问题记录
- 电脑各种文件自动加密软件用哪个好?选择文件加密软件评判因素主要有哪些?