《Trajectory modification considering dynamic constraints of autonomous robots》理解

版权声明:本文为博主原创文章,未经博主允许不得转载。 https://blog.csdn.net/HERO_CJN/article/details/87881437

        eletic band(橡皮筋)的定义:连接起始、目标点,并让这个路径可以变形,变形的条件就是将所有约束当做橡皮筋的外力。关于time eletic band的简述:起始点、目标点状态由用户/全局规划器指定,中间插入N个控制橡皮筋形状的控制点(机器人姿态),当然,为了显示轨迹的运动学信息,我们在点与点之间定义运动时间Time,即为Timed-Elastic-Band算法。

一、Timed Elastic Band概述

定义机器人位姿:,其中分别对应机器人在map坐标系(或者世界坐标系)的位置和姿态,文章中称其为configuration。

则空间内configuration序列(如figure 1)记录如下:

                                                         

 两个configuration间的时间间隔定义为:,表示机器人由一个configuration运动到另一个configuration所需时间;

记录时间序列:

将configuration及时间序列合并:

通过加权多目标优化获取最优的路径点,即最优的Q:

                                                                   

其中为最优结果,为考虑各种约束的目标函数,为个目标函数的权值。

在此应注意,每个目标函数只与elastic band中的某几个连续状态有关,而非整条band。故优化为对稀疏矩阵模型的优化。

二、约束目标函数

1、跟随路径和避障约束

约束主要有两个目标:跟随已知的全局规划路径和避障。两个目标函数均十分相似,跟随路径施力将elastic bands拉向全局路径,而避障约束施力使得elastic bands远离障碍物。configuration序列与全局路径点序列或障碍物的最近距离记为,如figure 4所示。跟随路径目标以configuration距全局路径的允许最大距离作为约束,避障目标以configuration距障碍物的允许最小距离作为约束。

                                 

约束以惩罚函数实现:

                                                   

 值得注意的是,目标函数的梯度可视为对elastic band施加的外力。

2、速度和加速度约束

由速度和加速度组成的动力学约束可以用类似1中运动学约束的惩罚函数表示。以figure 4为例,机器人运动的平均线速度和角速度可以通过相邻的configuration和时间间隔计算得到。

                                                               

类似的,线速度约束可以表示为:

                                                                 

 机器人的平均线加速度用同样的近似计算得到:

                                                                    

角加速度同理可计算得到。

3、最快路径约束

目标函数即为最小化时间间隔序列的二次方。

                                                                           

目标函数使得机器人获得最快路径,路径上的各configuration点在时间上均匀分开,而非传统的空间上求最短路径。

三、teb实现

teb的控制流程如下图:

                                              

teb优化问题(公式4)可以转化为hyper-graph问题。configurations和以及时间间隔作为nodes,目标函数以及其他约束函数为edges,各nodes由edges连接起来构成hyper-graph。该graph中,每一个约束都为一条edge,并且每条edge允许连接的nodes的数目是不受限制的。

(a)中展示了两个configurations(s0,s1),一个时间间隔ΔT0,一个障碍点O1组成的hyper-graph。速度约束需要计算平均速度,则该约束与s0,s1,

ΔT0相关,该edge将三个nodes连接起来。障碍物约束将障碍物和离障碍物最近的configuration连接起来。注意障碍物点是固定的,算法不应也不能对其位置进行优化。

(b)中则展示了更大的teb hyper-graph

                         

在确认能进行teb优化后(Verify trajectory),将计算控制量直接用于机器人系统。

猜你喜欢

转载自blog.csdn.net/HERO_CJN/article/details/87881437