在高维空间中，给定某点处海森矩阵函数的梯度怎么求和 该点处梯度方向，如何计算沿着梯度方向的曲率？

来源：蜘蛛抓取(WebSpider) 时间：2024-01-05 01:30 标签： 矩阵函数的梯度怎么求
本文是对《自动驾驶决策规划技术理论与实践》的学习总结，看本文前请先了解传统A*算法混合A*是经典A*算法的著名改进版本，致力于生成更符合车辆动力学的路径混合A*与和A*算法的区别1，增加了θ维度经典A*只考虑了x，y两个维度，混合A*在此基础上引入了θ维度。在经典A*中，相当于把车当成了一个质点，而混合A*中加入了表示朝向的θ，用x，y表示车辆后轴中心。这样，搜索空间就从之前的二维表格变成了x-y-θ的状态空间。在混合A*中，我们可以理解成是在一块棋盘格上，从初始的小线段向外扩展，每次可以扩展一小段曲线，节点可以在棋盘格中的任意位置，直到连接上终点的小线段图左是非常经典的A*算法，在A*算法中每一个状态格都当作一个像素点，也就是机器人路径由每个方格中心的连线组成。图中心是Field D*算法，它相对A*和后面发展的几种算法来讲最大的区别是允许路径的落脚点在方格的四周。 图右是Hybrid A*算法，它允许路径选择的落脚点在状态方格里面，而且其中的连线用某种曲线代替。2，改变了节点的扩展方式经典A*中，扩展节点的方式就是找相邻节点。但是由于混合A*要考虑车辆动力学，且为此增加了θ维度，所以混合A*并不能简单认为相邻的节点就是扩展目标。他的扩展需要考虑车辆动力学。混合A*要求将车辆的v，φ在单位时间（1s）内采样为某个常值，使得车辆以初始位姿在单位时间1s内以v，φ形式行驶。也就是认为车辆在单位时间（1s）内，以恒定的速度和转角做匀速运动的。辅以车辆动力学的公式，比如自行车模型，可以得到车辆在这1s内的轨迹。轨迹末端落入的三维网格就是要扩展的子节点。假设速度v有两种取值可能: V_max 和 -V_max ；角速度φ有三种取值可能：φ_max, 0, -φ_max。那么至少可扩展6个子节点。每个节点都代表了一种V和φ取值下的一段轨迹apollo中的节点扩展方式和上述有区别，apollo中是这样的：假设每次向外扩展的距离是固定的，默认为0.25m，然后将最大前轮转角进行等间隔采样。这样就确定了往哪个方向走和走多远，就可以找到下一个节点了。3，扩展了节点的字段相比于经典的A*，由于要考虑车辆动力学，所以每个节点的含义也更丰富了。其中经典A*包含的字段包括：f，g，h，父节点。混合A*新增的字段包括：1，对应的位姿[x, y, θ]2，节点是由何种v，φ扩展而来3，由父节点到本节点的轨迹，一般用等时间间隔的密集有序散点集表示4，丰富了遍历搜索策略引入Reeds-Shepp曲线生成机制是混合A*的特色。Reeds-Shepp曲线能够以基本符合车辆动力学约束的方式衔接始末位姿，但是完全不考虑避障因素。另外常用的一个曲线是dubins曲线，RS曲线与dubins曲线类似，都是圆+直线的路径，区别在于RS曲线允许车辆倒车。Reeds-Shepp曲线是只由三部分组成，向左的圆弧，向右的圆弧，直线。对应到汽车中就是向左转，向右转，直行。所以该路径是一些受车辆转弯半径约束的圆弧和直线。虽然Reeds-Shepp曲线没有考虑避障因素，但是因为它生成速度足够快，所以可以“先生成符合动力学的路径再校验这条路径是否满足碰撞条件“。具体的，在混合A*执行过程中，可间歇性地触发Reeds-Shepp曲线来生成一条衔接当前位姿和终点位姿的曲线，再判断是否符合碰撞条件，如果符合，那问题就解决了，如果不符合，那抛弃这次结果，继续执行正常的搜索逻辑。相当于时不时地尝试抄下小道，如果通就万事大吉，如果不通也没事，继续往前走。这种在传统A*搜索基础上加入Reeds-Shepp曲线的方式，大大提高混合A*的搜索速度。因为终止位姿也能够满足，所以常用在泊车场景中。这一操作在接近目标位姿或者非常稀疏的环境中比较有作用。如果每次尝试连接都需要做碰撞检测，会极大增加计算量。但是在apollo中，由于使用混合A*的时候，离目标车位已经不远了，所以在apollo中是每个迭代都尝试用RS曲线连接终点，而不是间歇性的。5，完善了碰撞检测方式经典A*算法中，判断是否碰撞只是检查对应网格是否被占据。而在混合A*中，需要将整条轨迹均匀采样，检测各采样时刻车辆位姿是否与障碍物碰撞。6，丰富了g的含义在经典A*中，g是从起点到当前节点的距离。但是距离单个因素不足以完整体现轨迹的合理性。此外还有轨迹的平滑程度。混合A*中，从当前节点扩展到子节点的距离可以直接由v*单位时间 得到，此外，混合A*中加入了对控制量v，φ频繁切换的惩罚。具体的可以算出子节点和父节点的控制量（v，φ）的差值，将这些差值加权后放入f中7，丰富了h的含义在经典A*中，用曼哈顿距离来估计h的值。而在混合A*中，h有两种计算方式，最后取其中较大的一个。一种是符合车辆动力学，但是不考虑碰撞检测的距离，记为h1；另一种是考虑碰撞检测，但是不考虑车辆动力学的距离，记为h2。最后h取两者中较大的一个。混合A*中，h1使用Reeds-Shepp曲线来计算。h2也是取两者之间的较大值（曼哈顿距离 和 Dijkstra算出的距离）。Dijkstra 搜索时，以混合A*的终点为起点，不设置终点，全图范围内搜索，所以封闭列表存储了终点到所有位置的最短距离 g(x)，因此可以用作查找表，而不是在混合A* 进行时启动新的搜索。在apollo中，没有h1，h2也只有Dijkstra算出的距离这一种。也就是整个h就是等于Dijkstra算出的距离伪代码输入：环境布局，始末位姿
输出：联通起点和终点的路径
# 初始化
初始化Openlist为空，iter=0，ready_flag=false
为初始节点完善相关属性：g = 0；计算h；parent_idx = null ；is_in_Openlist = 0；is_in_Closedlist = 0
将初始节点加入Openlist，并将初始节点的is_in_Openlist置为1
# 循环
while Openlist ！= 空 and ready_flag ！= true：
iter = iter + 1
从Openlist中选取一个代价f最小的节点，记为Node_current
if （iter - 1）可以被 N_rs 整除：
使用Reeds-Shepp曲线生成当前节点到终点的路径
if 生成的路径满足碰撞检测：
ready_flag = true
定义中止节点Node_end
设置Node_end的父节点为当前节点
设置Node_end的轨迹为刚生成的那段轨迹
break
拓展Node_current，将其所有子节点放置在临时集合A中
for each Node_child in 集合A ：
if (Node_child 超出地图边界)) or (Node_child的is_in_Closedlist==1):
continue
if Node_child的is_in_Openlist==0 :
if 碰撞检测不通过：
设置Node_child的is_in_Closelist置为1
continue
计算Node_child的历史代价g，预期代价h，总代价f，轨迹
将Node_child的父节点记为 parent_idx = Node_current
将Node_child放入Openlist，并将Node_child的is_in_Openlist置为1
else ：
计算将Node_child的父节点换成Node_current的话，总代价f_alternative
if f_alternative < Node_child原有的f ：
将Node_child的父节点更新为 parent_idx = Node_current
更新Node_child的历史代价g和总代价f
将Node_current从Openlist中移除。
将Node_current对应属性改为：is_in_Openlist = 0；is_in_Closedlist = 1
if 集合A中某节点位姿与终点位姿偏差在某一较小范围内 ：
ready_flag = true
将该节点定义为Node_end
break
if ready_flag ！= true：
return
#表示没有找到路径
# 输出路径
构建空列表B，将Node_end的轨迹放入列表B
设置Node_current为Node_end
while Node_current的父节点不是空 ：
将Node_current的父节点的轨迹加入集合B
将Node_current的父节点设置为Node_current
将列表B逆序打印就是路径补充事项1，完备性经典A*算法是完备的，也就是说如果路径存在，那么用经典A*算法一定可以找到那条路径混合A*算法是不完备的，也就是说可能找不到一条联通始末的路径。这是因为经典A*算法的搜索空间可以是全空间，但是混合A*算法的搜索空间需要满足车辆动力学，无法保证遍历所有节点，所以成功率不一定是100%，算法不具备完备性。2，工程化技巧可以把那些能够提前确定的部分做成离线的数据表，在线就可以不用计算，直接查表会快很多。1，扩展子节点的前向模拟部分，也就是计算可能的轨迹，可以离线完成。在线只需要将轨迹以其起点为轴进行旋转就可形成衔接父子节点的轨迹2，h1的计算。可以以较高分辨率采样枚举所有可能的始末位姿对，提前将对应路径长度记下来，在线调用时只需要差值就可以得出h1的值。3，泊车场景经常是从一个空旷的地方，停到一个狭窄的地方。如果调换始末位姿，那么在刚开始搜索的时候，就可以因为有障碍物而屏蔽掉大部分可能路径，避免了过早的”开枝散叶“，缩小了搜索空间，节约了搜索时间来都来了，不点个赞再走吗？
本文是对《自动驾驶决策规划技术理论与实践》的学习总结，看本文前请先了解传统A*算法混合A*是经典A*算法的著名改进版本，致力于生成更符合车辆动力学的路径混合A*与和A*算法的区别1，增加了θ维度经典A*只考虑了x，y两个维度，混合A*在此基础上引入了θ维度。在经典A*中，相当于把车当成了一个质点，而混合A*中加入了表示朝向的θ，用x，y表示车辆后轴中心。这样，搜索空间就从之前的二维表格变成了x-y-θ的状态空间。在混合A*中，我们可以理解成是在一块棋盘格上，从初始的小线段向外扩展，每次可以扩展一小段曲线，节点可以在棋盘格中的任意位置，直到连接上终点的小线段图左是非常经典的A*算法，在A*算法中每一个状态格都当作一个像素点，也就是机器人路径由每个方格中心的连线组成。图中心是Field D*算法，它相对A*和后面发展的几种算法来讲最大的区别是允许路径的落脚点在方格的四周。 图右是Hybrid A*算法，它允许路径选择的落脚点在状态方格里面，而且其中的连线用某种曲线代替。2，改变了节点的扩展方式经典A*中，扩展节点的方式就是找相邻节点。但是由于混合A*要考虑车辆动力学，且为此增加了θ维度，所以混合A*并不能简单认为相邻的节点就是扩展目标。他的扩展需要考虑车辆动力学。混合A*要求将车辆的v，φ在单位时间（1s）内采样为某个常值，使得车辆以初始位姿在单位时间1s内以v，φ形式行驶。也就是认为车辆在单位时间（1s）内，以恒定的速度和转角做匀速运动的。辅以车辆动力学的公式，比如自行车模型，可以得到车辆在这1s内的轨迹。轨迹末端落入的三维网格就是要扩展的子节点。假设速度v有两种取值可能: V_max 和 -V_max ；角速度φ有三种取值可能：φ_max, 0, -φ_max。那么至少可扩展6个子节点。每个节点都代表了一种V和φ取值下的一段轨迹apollo中的节点扩展方式和上述有区别，apollo中是这样的：假设每次向外扩展的距离是固定的，默认为0.25m，然后将最大前轮转角进行等间隔采样。这样就确定了往哪个方向走和走多远，就可以找到下一个节点了。3，扩展了节点的字段相比于经典的A*，由于要考虑车辆动力学，所以每个节点的含义也更丰富了。其中经典A*包含的字段包括：f，g，h，父节点。混合A*新增的字段包括：1，对应的位姿[x, y, θ]2，节点是由何种v，φ扩展而来3，由父节点到本节点的轨迹，一般用等时间间隔的密集有序散点集表示4，丰富了遍历搜索策略引入Reeds-Shepp曲线生成机制是混合A*的特色。Reeds-Shepp曲线能够以基本符合车辆动力学约束的方式衔接始末位姿，但是完全不考虑避障因素。另外常用的一个曲线是dubins曲线，RS曲线与dubins曲线类似，都是圆+直线的路径，区别在于RS曲线允许车辆倒车。Reeds-Shepp曲线是只由三部分组成，向左的圆弧，向右的圆弧，直线。对应到汽车中就是向左转，向右转，直行。所以该路径是一些受车辆转弯半径约束的圆弧和直线。虽然Reeds-Shepp曲线没有考虑避障因素，但是因为它生成速度足够快，所以可以“先生成符合动力学的路径再校验这条路径是否满足碰撞条件“。具体的，在混合A*执行过程中，可间歇性地触发Reeds-Shepp曲线来生成一条衔接当前位姿和终点位姿的曲线，再判断是否符合碰撞条件，如果符合，那问题就解决了，如果不符合，那抛弃这次结果，继续执行正常的搜索逻辑。相当于时不时地尝试抄下小道，如果通就万事大吉，如果不通也没事，继续往前走。这种在传统A*搜索基础上加入Reeds-Shepp曲线的方式，大大提高混合A*的搜索速度。因为终止位姿也能够满足，所以常用在泊车场景中。这一操作在接近目标位姿或者非常稀疏的环境中比较有作用。如果每次尝试连接都需要做碰撞检测，会极大增加计算量。但是在apollo中，由于使用混合A*的时候，离目标车位已经不远了，所以在apollo中是每个迭代都尝试用RS曲线连接终点，而不是间歇性的。5，完善了碰撞检测方式经典A*算法中，判断是否碰撞只是检查对应网格是否被占据。而在混合A*中，需要将整条轨迹均匀采样，检测各采样时刻车辆位姿是否与障碍物碰撞。6，丰富了g的含义在经典A*中，g是从起点到当前节点的距离。但是距离单个因素不足以完整体现轨迹的合理性。此外还有轨迹的平滑程度。混合A*中，从当前节点扩展到子节点的距离可以直接由v*单位时间 得到，此外，混合A*中加入了对控制量v，φ频繁切换的惩罚。具体的可以算出子节点和父节点的控制量（v，φ）的差值，将这些差值加权后放入f中7，丰富了h的含义在经典A*中，用曼哈顿距离来估计h的值。而在混合A*中，h有两种计算方式，最后取其中较大的一个。一种是符合车辆动力学，但是不考虑碰撞检测的距离，记为h1；另一种是考虑碰撞检测，但是不考虑车辆动力学的距离，记为h2。最后h取两者中较大的一个。混合A*中，h1使用Reeds-Shepp曲线来计算。h2也是取两者之间的较大值（曼哈顿距离 和 Dijkstra算出的距离）。Dijkstra 搜索时，以混合A*的终点为起点，不设置终点，全图范围内搜索，所以封闭列表存储了终点到所有位置的最短距离 g(x)，因此可以用作查找表，而不是在混合A* 进行时启动新的搜索。在apollo中，没有h1，h2也只有Dijkstra算出的距离这一种。也就是整个h就是等于Dijkstra算出的距离伪代码输入：环境布局，始末位姿
输出：联通起点和终点的路径
# 初始化
初始化Openlist为空，iter=0，ready_flag=false
为初始节点完善相关属性：g = 0；计算h；parent_idx = null ；is_in_Openlist = 0；is_in_Closedlist = 0
将初始节点加入Openlist，并将初始节点的is_in_Openlist置为1
# 循环
while Openlist ！= 空 and ready_flag ！= true：
iter = iter + 1
从Openlist中选取一个代价f最小的节点，记为Node_current
if （iter - 1）可以被 N_rs 整除：
使用Reeds-Shepp曲线生成当前节点到终点的路径
if 生成的路径满足碰撞检测：
ready_flag = true
定义中止节点Node_end
设置Node_end的父节点为当前节点
设置Node_end的轨迹为刚生成的那段轨迹
break
拓展Node_current，将其所有子节点放置在临时集合A中
for each Node_child in 集合A ：
if (Node_child 超出地图边界)) or (Node_child的is_in_Closedlist==1):
continue
if Node_child的is_in_Openlist==0 :
if 碰撞检测不通过：
设置Node_child的is_in_Closelist置为1
continue
计算Node_child的历史代价g，预期代价h，总代价f，轨迹
将Node_child的父节点记为 parent_idx = Node_current
将Node_child放入Openlist，并将Node_child的is_in_Openlist置为1
else ：
计算将Node_child的父节点换成Node_current的话，总代价f_alternative
if f_alternative < Node_child原有的f ：
将Node_child的父节点更新为 parent_idx = Node_current
更新Node_child的历史代价g和总代价f
将Node_current从Openlist中移除。
将Node_current对应属性改为：is_in_Openlist = 0；is_in_Closedlist = 1
if 集合A中某节点位姿与终点位姿偏差在某一较小范围内 ：
ready_flag = true
将该节点定义为Node_end
break
if ready_flag ！= true：
return
#表示没有找到路径
# 输出路径
构建空列表B，将Node_end的轨迹放入列表B
设置Node_current为Node_end
while Node_current的父节点不是空 ：
将Node_current的父节点的轨迹加入集合B
将Node_current的父节点设置为Node_current
将列表B逆序打印就是路径补充事项1，完备性经典A*算法是完备的，也就是说如果路径存在，那么用经典A*算法一定可以找到那条路径混合A*算法是不完备的，也就是说可能找不到一条联通始末的路径。这是因为经典A*算法的搜索空间可以是全空间，但是混合A*算法的搜索空间需要满足车辆动力学，无法保证遍历所有节点，所以成功率不一定是100%，算法不具备完备性。2，工程化技巧可以把那些能够提前确定的部分做成离线的数据表，在线就可以不用计算，直接查表会快很多。1，扩展子节点的前向模拟部分，也就是计算可能的轨迹，可以离线完成。在线只需要将轨迹以其起点为轴进行旋转就可形成衔接父子节点的轨迹2，h1的计算。可以以较高分辨率采样枚举所有可能的始末位姿对，提前将对应路径长度记下来，在线调用时只需要差值就可以得出h1的值。3，泊车场景经常是从一个空旷的地方，停到一个狭窄的地方。如果调换始末位姿，那么在刚开始搜索的时候，就可以因为有障碍物而屏蔽掉大部分可能路径，避免了过早的”开枝散叶“，缩小了搜索空间，节约了搜索时间来都来了，不点个赞再走吗？

我要回帖

更多关于 矩阵函数的梯度怎么求 的文章

 

随机推荐