一. 轨迹规划是什么?

在机器人导航过程中,如何控制机器人从A点移动到B点,通常称之为运动规划。运动规划一般又分为两步:

1、路径规划:在地图(栅格地图、四\八叉树、RRT地图等)中搜索一条从A点到B点的路径,由一系列离散的空间点(waypoint)组成。

2、轨迹规划:由于路径点可能比较稀疏、而且不平滑,为了能更好的控制机器人运动,需要将稀疏的路径点变成平滑的曲线或稠密的轨迹点,也就是轨迹。

2. 轨迹是什么?注意低次项在前

轨迹一般用n阶多项式(polynomial)来表示,即

其中p0,p1,...,pn为轨迹参数(n+1个),设参数向量p=[p0,p1,...,pn]T,则轨迹可以写成向量形式。

对于任意时刻t,可以根据参数计算出轨迹的位置P(osition),速度V(elocity),加速度A(cceleration),jerk,snap等。

一个多项式曲线过于简单,一段复杂的轨迹很难用一个多项式表示,所以将轨迹按时间分成多段,每段各用一条多项式曲线表示,形如:

k为轨迹的段数,为第i段轨迹的参数向量。

此外,实际问题中的轨迹往往是二维、三维甚至更高维,通常每个维度单独求解轨迹。

3. Minimum Snap轨迹规划

轨迹规划的目的:求轨迹的多项式参数p1,...,pk。
我们可能希望轨迹满足一系列的约束条件,比如:希望设定起点和终点的位置、速度或加速度,希望相邻轨迹连接处平滑(位置连续、速度连续等),希望轨迹经过某些路径点,设定最大速度、最大加速度等,甚至是希望轨迹在规定空间内(Obstruction check)等等。
通常满足约束条件的轨迹有无数条,而实际问题中,往往需要一条特定的轨迹,所以又需要构建一个最优的函数,在可行的轨迹中找出“最优”的那条特定的轨迹。  所以,我们将问题建模(fomulate)成一个约束优化问题,形如:

这样,就可以通过最优化的方法求解出目标轨迹参数p。注意:这里的轨迹参数p是多端polynomial组成的大参数向量

我们要做的就是:将优化问题中的f(p)函数和Aeq,beq,Aieq,bieq参数给列出来,然后丢到优化器中求解轨迹参数p。

Minimum Snap顾名思义,Minimum Snap中的最小化目标函数是Snap(加速度的二阶导),当然你也可以最小化Acceleration(加速度)或者Jerk(加速度的导数),至于它们之间有什么区别,quora上有讨论。一般不会最小化速度。

4. 一个简单的例子

给定包含起点终点在内的k+1个二维路径点pt0,pt1,...,ptk,pti=(xi,yi),给定起始速度和加速度为v0,a0,末端加速度为ve,ae,给定时间T,规划出经过所有路径点的平滑轨迹。

a. 初始轨迹分段与时间分配

根据路径点,将轨迹分为k段,计算每段的距离,按距离平分时间T(匀速时间分配),得到时间序列t0,t1,...,tk对x,y维度单独规划轨迹。后面只讨论一个维度。

时间分配的方法:匀速分配或梯形分配,假设每段polynomial内速度满足匀速或梯形速度变化,根据每段的距离将总时间T分配到每段。

这里的轨迹分段和时间分配都是初始分配,在迭代算法中,如果 Obstruction check和 feasibility check不满足条件,会插点或增大某一段的时间。

b. 构建优化函数

Minimum Snap的优化函数为:

其中,

注意:r,c为矩阵的行索引和列索引,索引从0开始,即第一行 r=0

可以看到,问题建模成了一个数学上的二次规划(Quadratic Programming,QP)问题。

c. 构建等式约束方程

  1. 设定某一个点的位置、速度、加速度或者更高为一个特定的值,可以构成一个等式约束。
    例如:

由于要过中间点,对中间点的位置也构建等式约束,方法同上。

2. 相邻段之间的位置、速度、加速度连续可以构成一个等式约束

例如第i、i+1段的位置连续构成的等式约束为

合并所有等式约束,得到

等式约束个数=3(起始PVA)+k-1(中间点的p)+3(终点pva)+3(k-1)(中间点PVA连续)=4k+2。

d. 构建不等式约束

不等式约束与等式约束类似,也是设置某个点的P、V、A小于某一特定值,从而构建Aieqp=bieq

,不等式约束一般是在corridor中用的比较多,这里暂时先不使用不等式约束。

e. 求解

利用QP求解器进行求解,在MATLAB中可以使用quadprog() 函数,C++的QP求解器如OOQP,也可以自己去网上找。

参考资料:

1、深蓝学院

2、https://blog.csdn.net/q597967420/article/details/76099491

Minimum Snap轨迹规划详解(1)轨迹规划的更多相关文章

  1. Minimum Snap轨迹规划详解(3)闭式求解

    如果QP问题只有等式约束没有不等式约束,那么是可以闭式求解(close form)的.闭式求解效率要快很多,而且只需要用到矩阵运算,不需要QPsolver. 这里介绍Nicholas Roy文章中闭式 ...

  2. 企业sudo权限规划详解 (实测一个堆命令搞定)

    简述问题:         随着公司的服务器越来越多,人员流动性也开始与日俱增,以往管理服务器的陈旧思想应当摒弃,公司需要有 更好更完善的权限体系,经过多轮沟通和协商,公司一致决定重新整理规划权限体系 ...

  3. lattice planner 规划详解

    大家好,我是来自百度智能驾驶事业群的许珂诚.今天很高兴能给大家分享Apollo 3.0新发布的Lattice规划算法. Lattice算法隶属于规划模块.规划模块以预测模块.routing模块.高精地 ...

  4. Minimum Snap轨迹规划详解(2)corridor与时间分配

    在上一篇文章中,我们得到的轨迹并不是很好,与路径差别有点大,我们期望规划出的轨迹跟路径大致重合,而且不希望有打结的现象,而且希望轨迹中的速度和加速度不超过最大限幅值.为了解决这些问题有两种思路: 思路 ...

  5. Kubernetes K8S之通过helm部署metrics-server与HPA详解

    Kubernetes K8S之通过helm部署metrics-server与 Horizontal Pod Autoscaling (HPA)详解 主机配置规划 服务器名称(hostname) 系统版 ...

  6. Usage、Usage Minimum和Usage Maximum项目详解

    (1)一个产生多个数据域(Report Count>1)的主项目之前有一个以上的[用途]时,每个[用途]与一个数据域依次对应,如果数据域个数(Report Count)超过[用途]的个数,则剩余 ...

  7. 超级强大的SVG SMIL animation动画详解

    本文花费精力惊人,具有先驱前瞻性,转载规则以及申明见文末,当心予以追究.本文地址:http://www.zhangxinxu.com/wordpress/?p=4333 //zxx: 本文的SVG在有 ...

  8. [转]超级强大的SVG SMIL animation动画详解

    超级强大的SVG SMIL animation动画详解 本文花费精力惊人,具有先驱前瞻性,转载规则以及申明见文末,当心予以追究.本文地址:http://www.zhangxinxu.com/wordp ...

  9. org.apache.log4j.Logger详解

    org.apache.log4j.Logger 详解 1. 概述 1.1. 背景 在应用程序中添加日志记录总的来说基于三个目的 :监视代码中变量的变化情况,周期性的记录到文件中供其他应用进行统计分析工 ...

随机推荐

  1. SPI驱动程序设计

    一.SPI驱动子系统架构 m25p80.c: static int __devinit m25p_probe(struct spi_device *spi) { struct flash_platfo ...

  2. 【LeetCode 31】下一个排列

    题目链接 [题解] 从右往左找第一个下降的位置i(即满足nums[i]<nums[i+1]); 然后在[i+1..len-1]这个区间里面找到一个最大的下标k,使得nums[k]>nums ...

  3. window10安装mysql-5.7.20-winx64.zip

    window10安装mysql--winx64.zip 原文 https://www.cnblogs.com/ericli-ericli/p/6916285.html D:\share\src\win ...

  4. [CSP-S模拟测试]:计划(前缀和)

    题目传送门(内部题32) 输入格式 第一行,三个正整数$n,m,q$.第二行,$n$个正整数$a_i$,保证$1\leqslant a_i\leqslant n$.接下来$q$行,每行两个正整数$k, ...

  5. mybatis原理与设计模式-日志模块- 适配器模式

    在讲设计模式之前,得先知道java程序设计中得六大原则,才能更好得理解我们得系统为什么需要设计模式 1 单一职责原则 一个类只负责一种职责,只有这种职责的改变会导致这个类的变更.绕口一点的正统说法:不 ...

  6. 用php写爬虫去爬数据

    参考文档1 参考文档2 这里是我自己写的一个小需求 <?php /** 采集http://www.959.cn/school,即时更新的最新的文章内容:每分钟采集一次.采集结束后实时入库并展示. ...

  7. 【转】Java类MemoryUsage查看虚拟机的使用情况

    原文地址:https://www.cnblogs.com/xubiao/p/5465473.html Java类MemoryUsage,通过MemoryUsage可以查看Java 虚拟机的内存池的内存 ...

  8. 字母所对应的Unicode编码

    A~Z                65~90 a~z                 97~122 public class Unicode { public static void main(S ...

  9. shell编程:向函数中传递参数

    cal.sh sh cal.sh 20 + 10 实现这样传参的函数(shell不是一个严谨的编程语言,参数这种是不用定义的,函数中直接引用,shell执行中直接写) #!/bin/bash # ca ...

  10. 集中式日志系统 ELK 协议栈详解

    简介 在我们日常生活中,我们经常需要回顾以前发生的一些事情:或者,当出现了一些问题的时候,可以从某些地方去查找原因,寻找发生问题的痕迹.无可避免需要用到文字的.图像的等等不同形式的记录.用计算机的术语 ...