7.基于马尔可夫链MDP的路径规划

Uncertainties in Planning

在实际应用程序中，执行和状态估计都不是完美的。从机器人的角度来看，不确定性可以分为两类，用于表示机器人可以使用多少信息

•执行不确定性:滑移、崎岖地形、风、空气阻力、控制错误,等等。

•状态估计不确定性:传感器噪声，校准误差，不完善估计，部分可观性等

•不确定性nondeterministic:机器人不知道什么类型的不确定性或者它的行为会受到干扰。

•概率性probalistic:机器人通过观察对不确定性进行估计和收集统计数据。

Planning with Uncertainties

为了正式地描述这个概念，我们首先引入两个决策者来对不确定性的产生来进行建模，然后是对不确定性规划的类型。

Decision Makers

• 机器人是基于已知的状态从而进行完美的全面执行规划的主要决策者。
• 环境(Nature)给机器人制定的计划的执行增加的不确定性是机器人无法预测的
• Robot与Nature处于对抗关系
  

彼此不知道对方怎么做,彼此独立的空间

两者也会相互影响,比如在人参与的路径规划中,人会躲避机器人.

基于最坏情况的分析

Nature要做的动作大致知道

$\theta$通过积分消去

Markov Decision Process

某一个状态S的价值可以用该状态下的所有动作的价值来表述

某一个动作A的价值可以用该状态后继状态的价值来表述

$P(x_{k+1}|x_k,u_k)$

在不确定性下进行计划的第一个困难在于正确地建立我们的MDP模型的问题,也就是各个量如何去描述

例子

X_G目标区域,$l(x_k,u_k,{\theta }_k)$

$\sigma \left({\theta }_1,{\theta }_2\right)$表示随机误差;
生成轨迹在执行时候可能并不如预期,因为存在着干扰(nature),需要走一步看一步.

定义一个(反馈)规划,由π生成的决策树.
定义一个轨迹集合

需要根据x的状态来实时选择需要执行的轨迹

对特定的规划π定义成本(而不是轨迹):

不确定性的最坏情况分析(worst-case),取不同trajectory的代价的
模型:

概率分析的预期案例分析(expected-case),平均不同trajectory的代价.
模型:

Minimax Cost Planning

假定是非确定性模型,基于最坏情况分析,去找到这样的最佳规划π *

直接解非常困难,需要枚举所有的结果.
常采用动态规划方法构造最优解。
核心是寻找优化轨迹的第k阶段到第k + 1阶段的递归关系

find optimal solution

可以直接得到最终状态F的最优目标成本
$$G_F^{\ast }=l_F\left(x_F\right)$$
从阶段K到阶段F = K + 1的所有最优一步方案的成本为

一般来说,一旦$G_{k+1}^{\ast }$给定,那么$G_k^{\ast }$能够计算出来的

min…minmax 可以放到里面
因此，求解极大极小成本计划的动态规划递推式为

从局部递归到全局最优状态
2:$l(s_1,u_1,{\theta }_1)$
0:终点处的代价
假定$S_2$的代价为无穷inf.
Nondeterministic Dijkstra
区别在于

算法实现:

对S_2来说,有两个前继节点,对S_1进行处理,发现G=7,

如何改进左边的算法为A* ?
这个算法可以处理故障情况吗?
如何跟踪最优规划π * ?

Pros/Cons of Minimax Cost Planning

• 鲁棒性好
• 过于谨慎
• 相对于一般路径规划,更难于计算解
  • 尤其是不确定性的Dijkstra或A*不适用时, 也就是构造图不易,需要处理连续的情况.
  • 即使有不确定的A应用，仍然比使用A计算单个路径要复杂得多。

Expected Cost Planning

基于概率模型,根据期望情况分析,找到这样的最优计划π *,不同之处在于会对"nature"进行估计,会取均值

同样使用动态规划求解.也可以直接得到最终状态F的最优目标成本
$$G_F^{\ast }=l_F\left(x_F\right)$$
从阶段K到阶段F = K + 1的所有最优一步方案的成本为

取均值而非最大值

一般来说,一旦$G_{k+1}^{\ast }$给定,那么$G_k^{\ast }$能够计算出来的

因此，求解期望成本计划的动态规划递推式为

它有一个特殊的名字叫做Bellman最优方程,这是一个关键的公式,此时需要给定Natrue动作的概率分布.

那$G_{k+1}^{\ast }(x_{(}k+1)=s_2)?$

算法

将所有状态的G值初始化为有限值(会影响到收敛的速度)

特性:
通过进行值迭代得到最优值。

• 最优性与迭代顺序无关。
• 收敛速度可能取决于迭代顺序。
  求解Bellman最优方程可以通过Bellman更新方程方法。
  

注意G的值的变化

思考
算法何时收敛?
左算法如何改进?
如何证明收敛性?
如何获得最佳计划${\pi }^{\ast }$?

Pros/Cons

•概率最优
•反映平均表现
•特定的单次执行可能不是最佳的
•要求不确定性分布
•比普通路径更难计算
•遍历整个地图(状态)
•受初始化和迭代顺序的影响

Real Time Dynamic Programming

对于Expected cost Planning的加速扩展.

Introduction

1.将所有状态的G值初始化为可容许值;
2.随机遵循贪婪政策选择结果，直到达到目标;
3. 更新所有已经访问过的路径(状态);
4.重置为$x_s$，重复执行2-4，直到当前贪婪策略的所有状态都满足
Bellman误差< $\Delta \left(x_k\right)=\Vert G\left(x_k\right)-G\left(x_{k+1}\right)\Vert$

需要注意加入随机的选择(如在S2上,一般为S1下一步,但要有概率选择S4作为下一步).便于找到最优的的解

优点
1.非常有效的替代代价迭代
2.不计算所有状态的值
3.集中计算相关的状态

作业

本章作业相比于前面几章比较简单，这次思路讲解我根据代码主要分两部分来说，第一部分是MDP框架，如何去建模MDP问题，主要涉及到graph_node.py中heuristic 函数的实现等；
第二部分是DP和RTDP两种算法的流程，主要是RTDP中expected_cost_uk 和 bellman_error等部分的实现，其实这部分的实现DP中已有，过程是类似的，RTDP 和 DP 的区别在于map的 g_value的初始化策略不同， 并且采用 greedy policy + exploration 的策略来计算相关的 state，cost 和 error 的计算方法与 DP 相同。
首先，复习一下MDP框架。MDP model包括以下四个部分
1.state space X
2.action space U
3.state transition function P
4.cost function l(或者说reward)，本次作业是定义cost function来迭代。

以上四个部分代码分别对应MDP各个要素的建模

模型建立好之后，就是如何搜索到最优路径，上图展示了DP和RTDP两种方式。
可以看出对于DP和RTDP：
RTDP有初始化state g_value的过程，而DP则对初始化的g_value没有要求。
RTDP先用贪心算法搜索出路径后，然后只更新了路径上的节点，这一策略相比于DP减少了计算量。
请大家注意不要忽略了RTDP中的 state back_up过程。

这里我们的程序要跑两次，第一次在主函数运行build_up_graph即为RTDP的g_value初始化过程，完成后会保存结果，之后屏蔽掉主函数的build_up_graph函数即可。

这是build_up_graph初始化g_value部分的具体实现，这里我采用了欧氏距离/3来初始化g_value，主要是为了保证g_value的adimissible，作业里有同学/4、/5等，都是可以的，我们这里max_vel=5，取5可能比较保险，能够保证解的最优性。

这里展示了二者的第二点区别，即更新策略不同。DP更新所有节点，而RTDP仅更新已搜索到路径上的点，因此减少了计算量。

这是RTDP中作业代码的具体实现
主要是对贪心算法搜索到的路径上的点更新g_value，并计算bellman error
其他步骤的的代码实现比较简单，这里就不做详细介绍了

这里左图是作业里出现的问题，代码更新及cost部分有误，导致不收敛。
右图是的干涉情况是正常的，waypoint没有落入障碍物栅格就好。

最后是输出路径最优性的判断，由于我们的cost function是走一步加1，所以应该用动作步数(路径段数)来判断是否达到最优，和距离没有关系。