基于马尔可夫决策过程的运动规划

Uncertainties in Planning

直到目前为止，我们做的规划都是假设没有不确定性的

• 认为机器人的执行是完美的
• 机器人对于自身的状态的估计是完全正确的

而在实际应用中，执行和状态估计都不是完美的

• 执行的不确定性：打滑，路面崎岖，风的影响，空气阻力，控制误差等等
• 状态估计的不确定性：传感器噪音，校准误差，估计不完美，部分可观测等等

从数学上看，不确定性分为两类，

• Nondeterministic：机器人完全不知道自己的动作会受到怎样的干扰，存在什么样的不确定性，比如，机器人想向前移动一段距离，但是会受到自然环境的影响，发生偏移，但是不知道会偏多远。
• Probabilistic：机器人可以通过经验或者数据积累，可以大体估计受到的干扰是多大。

Planning with Uncertainties

Decision Marker

在之前的决策中，基于完美的执行和状态估计，planning中的角色只有机器人一个， 而在带有不确定性的路径规划中，引入了Nature，用以给机器人的planning增加不确定性，并且这对机器人来说是不可预测的。

Independent Game

• 定义\(U\)为机器人的action space，\(u\in U\)表示机器人的一个action
• 定义\(\Theta\)为Nature的action space，\(\theta \in \Theta\)表示Nature的一个action
• 定义一个函数\(L: U \times \Theta\to \mathbb{R}\cup \left \{ \infty \right \}\)，衡量机器人的action和Nature的action相互作用的一个结果，称为代价函数(cost function)或反向奖励函数(negative reward function)

Dependent Game

• 定义\(U\)为机器人的action space，\(u\in U\)表示机器人的一个action
• 定义\(\Theta\)为Nature的action space，\(\theta \in \Theta\)表示Nature的一个action，但是Nature的action space是依赖机器人的action space的
• 定义一个函数\(L: U \times \Theta\to \mathbb{R}\cup \left \{ \infty \right \}\)，衡量机器人的action和Nature的action相互作用的一个结果，称为代价函数(cost function)或反向奖励函数(negative reward function)

One-step Worst-Case Analysis

• 在Nondeterministic模型下，在Independent Game中，Nature的action发生的概率P\((\theta)\)，和在Dependent Game中，Nature的action发生的概率P\((\theta|u_k)\)，都是无法知道的
• 机器人对Nature的行为是无法预测的，想象每次Nature选择的action都是对机器人最不利的（cost 最大）
• 因此，需要在最坏的情况下做决策，枚举Nature的action space筛选出最不利的情况，再选择机器人的action把不利降到最低

\[u^*=\underset{u\in U}{\mathop{\mathrm{argmin}}}\left\{\underset{\theta\in\Theta}{\mathrm{max}}\{L(u,\theta)\}\right\} \]

One-step Expected-Case Analysis

• 在Probabilistic模型下，在Independent Game中，Nature的action发生的概率P\((\theta)\)，和在Dependent Game中，Nature的action发生的概率P\((\theta|u_k)\)，大致是知道的
• 对于机器人的一个action，Nature随机选择action，而且这个action的概率是可以知道的
• 因此，对于机器人的一个action，我们取Nature各个action产生的结果的期望值，选择机器人的action使这个期望值最低

\[u^*=\underset{u\in U}{\operatorname{argmin}}\{E_\theta[L(u,\theta)]\} \]

Discrete Planning with Nature

• 定义非空的状态空间\(X = \left \{ s_s,s_1,s_2,s_3,s_4,s_g \right \}\) 包含初始状态\(s_s\)
• 定义目标状态空间\(X_F = \left \{s_g \right \}\)
• 定义robot的action space：\(U = \left\{ u_s,u_1,u_2,u_3,u_4 \right\}\)
• 定义Nature的action space：\(\Theta = \left\{ \theta_0, \theta_1, \theta_2 \right\}\)

如图：当机器人的状态处于\(s_1\)时，机器人选择的action时 \(u_1\)，如果Nature选择的干扰(action)是\(\theta_1\)，那么机器人的状态转到\(s_g\)，cost是\(2\)，如果Nature选择的干扰是\(\theta_2\)，那么机器人的状态转到\(s_2\)，cost是\(2\)。

Multi-Step Discrete Planning with Nature

状态转移方程\(f(x,u,\theta)\)：

• 离散的情况：

\[X_{k+1}(x_k,u_k)=\left \{ x_{k+1} \in  X \mid \exists \theta_k \in \Theta(x_k,u_k)\ s.t. x_{k+1}=f(x_k.u_k.\theta_k)\right \}  \]

枚举在\(x_k\)状态下机器人执行了\(u_k\)之后所有可能发生的干扰\(\theta_k\)，将\(u_k\)和\(\theta_k\)做组合，每种组合可能产生新的状态\(x_{k+1}\)，全部可能产生的状态的集合记为\(X_{k+1}\)

• 连续的情况

\[P(x_{k+1}\mid x_k,u_k)= {\textstyle \sum_{\theta_k}^{}}P(x_{k+1},\theta_k \mid x_k,u_k ) \ s.t.\left \{  \theta_k\mid x_{k+1}=f(x_k,u_k,\theta_k)\right \}   \]

\(x_{k+1}\)在\(x_k\)，\(u_k\)下的条件概率分布，等于在\(x_k\)状态下机器人执行了\(u_k\)之后所有可能发生的干扰\(\theta_k\)与\(x_{k+1}\)的联合概率分布之和

如图：

当机器人在\(s_s\)状态下，机器人的action只有一个\(u_s\)，Nature的action也只有一个\(\theta_0\)，因此作用只有一个结果s\(_2\)，即\(f(s_s,u_s,\theta_0)=s_2\)，\(X(s_s,u_s)=\left \{ f(s_s,u_s,\theta_0) \right \}=\left \{ s_2 \right \}\)

当机器人在\(s_1\)状态下，机器人的action有一个\(u_1\)，Nature的action有两个\(\theta_1\)，\(\theta_2\)，因此作用的结果有两个\(s\)\(_g\)，\(s_2\)，即\(f(s_1,u_1,\theta_1)=s_g\)，\(f(s_1,u_1,\theta_2)=s_2\)，即\(X(s_1,u_1)=\left \{ f(s_1,u_1,\theta_1) ,f(s_1,u_1,\theta_2) \right \}=\left \{ s_g,s_2 \right \}\)

• 与单步的规划不同，多步规划由一系列步骤组成，每一步被记为\(k\)，整个过程从\(k=1\)开始，无限的持续下去或者终止于最终的步骤\(k=K+1=F\)

如上图：

从起点\(s_s\)开始到终点\(s_g\)结束，经历的一系列步骤为，\(s_s\)执行\(u_s\)到达\(s_2\)，\(s_2\)执行\(u_{21}\)到达\(s_1\)，\(s_1\)执行\(u_1\)到达\(s_g\)

• 阶段可加(stage-additive)的cost function L，分段可加意味着各个stage的cost互不影响，可以分别求出每个stage的cost，再做累加，从优化的角度来说就是优化问题是可分的。

定义\(\tilde{x}_F=\left(x_1,x_2,\ldots,x_F\right),\tilde{u}_k=\left(u_1,u_2,\ldots,u_k\right),\tilde{\theta}_K=\left(\theta_1,\theta_2,\ldots,\theta_K\right)\)，分别为状态集，机器人的动作集，nature的动作集，因此cost function L是评估所有可能的方案的函数。

\[L\left(\tilde{x}_{F},\tilde{u}_{K},\tilde{\theta}_{K}\right)=\sum\limits_{k=1}^{K}l\left(x_{k},u_{k},\theta_{k}\right)+l_{F}\left(x_{F}\right),\\ l_{F}\left(x_{F}\right)=\left\{\begin{array}{l l}0,&\text{if}x_{F}\in X_{G},\\ \infty,&\text{otherwise.}\end{array}\right. \]

需要注意的是，这里的小写\(k\)，如\(x_k\)不是代表第\(k\)个状态，而是第\(k\)个stage的状态，如上图所示，机器人在每个stage的所处的状态是可以不同的，因此才造成了有众多方案，cost function的作用就是定量的评估每种方案，然后从众多方案中选择最优的路径，即cost最小的方案。最后的\(l_F(x_F)\)表示最终状态的cost，如果要求机器人最终必须到达指定的状态，那么此时cost为0，否则为无穷大，类似于之前OBVP问题中的 ****final state cost 。

Markov Decision Process

之前讨论的问题，在强化学习领域有现成的框架，即马尔可夫决策过程(MDP)，定义：

• state space: S or X
• robot action space: A or U
• state transition function under probabilistic model:\(P(x_{k+1}\mid x_k, u_k)\)
• state transition function under nondeterministic model:\(X_{k+1}(x_k,u_k)\)
• immediate reward(对应之前的one-step cost −\(l(x_k , u_k , θ_k )\)):\(R (x_k , x_{k+1} )\)

MDP Model Design：

1. state space：15×15的栅格
2. robot action sapce：定义了5种action，stay，right，left，up，down
3. nature action space（连续）: 定义了两种action，\(\theta_1\)，\(\theta_2\)。将robot在\(x_k\)状态下，执行\(u_k\)，到达\(x_{k+1}\)的状态转移方程建模成了以\(x_k+u_k\)为中心，\(\sigma (\theta _1,\theta _2)\)为标准差的高斯分布。
4. nature action space（连续）: 简单定义了三种action，\(\theta_1\)，\(\theta_2\)，\(\theta_3\)，\(x_{k+1}=x_k+u_k+\theta_k,k\sim\{0,1,2\}\)
5. reward：当前状态和下一个状态的距离差

与之前的path finding不同，之前找到一条path，每个点都是固定的，就一定是可行的，而在MDP中每个点的位置和我们预期的是有偏差的，在\(x_k\)，状态下robot选择了一个action，Nature对robot的action作出干扰，机器人根据干扰的大小来做判断，选择最优的action，到达x_{k+1}，接着继续做决策，最终以一棵决策树的形式呈现。以说白了是“走一步看一步”的。

Solve

• 定义一个映射(Plan)，从state sapce 映射到robot action space，表示在某个状态，机器人选择什么action

\[\pi : X \to U \]

• 定义一组轨迹，表示robot从\(x_s\)出发，在\(\pi\)的作用下能够得到的所有轨迹的集合

\[\begin{aligned}&\mathcal{H}(\pi,x_s):\text{induced by}\ \pi\ \text{and started from}\ x_s.\\ &(\tilde{x},\tilde{u},\tilde{\theta})\in\mathcal{H}(\pi,x_s):\text{a injectory or an execution.}\end{aligned} \]

不同的Plan：\(\pi\)，对应的映射可能是不同的，比如对于\(\pi_1\)在\(s_2\)状态下总是选择\(u_{21}\)这个action，而对于\(\pi_2\)在\(s_2\)状态下则是选择\(u_{24}\)这个action，进而导致产生的trajectories set不同。

• 定义一种用来衡量Plan：\(\pi\)好坏的cost，注意这里不是衡量某条轨迹，而是一个plan，一个plan能诱导多条轨迹。

\[G_{\pi}(x_s): cost-to-goal \]

1. 在【nondeterministic model】下，考虑最坏情况，认为\(G\)是由\(\pi\)诱导的所有轨迹中cost最大的那条轨迹的cost。

\[G_{\pi}\left(x_{s}\right)=\max _{(\tilde{x}, \tilde{u}, \tilde{\theta}) \in \mathcal{H}\left(\pi, x_{s}\right)}\{L(\tilde{x}, \tilde{u}, \tilde{\theta})\} \]

2. 在【probabilistic model】下，考虑期望，认为\(G\)是由\(\pi\)诱导的所有轨迹的cost的期望值。

\[G_{\pi}\left(x_{s}\right)=E_{\mathcal{H}\left(\pi,x_{s}\right)}[L(\tilde{x},\tilde{u},\tilde{\theta})] \]

Minimax Cost planning With Nondeterministic Model

• 在【nondeterministic model】下考虑最坏情况，由于机器人不知道Nature的影响是多大，因此每一步都按最坏的情况考虑
• 寻找一个最优的plan：\(\pi^*\)，使得由这个plan产生的所有轨迹的cost的最大值最小，即

\[G_{\pi^*}(x_s)=\min\limits_{\pi}\{G_\pi(x_s)\}=\min\limits_{\pi}\{\max\limits_{\mathcal{H}(\pi,x_s)}\{L(\tilde{x},\tilde{u},\tilde{\theta})\}\}. \]

• 直接解这个问题是很难的，需要枚举所有的plan，再枚举所有plan产生的轨迹，复杂度太高，通常采取动态规划(dynamic programming)的方式来解决，主要是通过寻找第k步和第k+1步的递推关系，进而一步一步求解。

Dynamic Programming

• 首先，在最终状态\(F\)的【cost -to-go】\(G_F^*\) 可以直接表示为\(G_F^* = l_F(x_F)\)
• 从第\(K\)步到第\(K+1=F\)步的最优的【one- step - plan】的 cost 由两部分组成，一部分是由第\(K\)步到第\(K+1\)步的【transmission cost】，另一步是到达\(x_F = f(x_K,u_K,\theta_K)\)的【final state cost】，即：

\[G_K^*\left(x_K\right)=\min\limits_{u_K}\max\limits_{\theta_K}\left\{l\left(x_K,u_K,\theta_K\right)+G_F^*\left(f\left(x_K,u_K,\theta_K\right)\right)\right\}\quad \]

• 更为一般的，只要给定\(G_{k+1}^*\)，就可以计算出\(G_k^*\)

\[\begin{array}{l} G_{k}^{*}\left(x_{k}\right) = \min _{u_{k}} \max _{\theta_{k}}\left[\min _{u_{k+1} \theta_{k+1}} \cdots \min _{u_{K}} \max _{\theta_{K}}\left(l\left(x_{k}, u_{k}, \theta_{k}\right)+\sum_{i = k+1}^{K} l\left(x_{i}, u_{i}, \theta_{i}\right)+l_{F}\left(x_{F}\right)\right)\right] \\ G_{k}^{*}\left(x_{k}\right) = \min _{u_{k}} \max _{\theta_{k}}[l\left(x_{k}, u_{k}, \theta_{k}\right)+\underbrace{\left.\min _{u_{k+1} \theta_{k+1}} \cdots \min _{u_{K}} \max _{\theta_{K}}\left(\sum_{i = k+1}^{K} l\left(x_{i}, u_{i}, \theta_{i}\right)+l_{F}\left(x_{F}\right)\right)\right]}_{G_{k+1}^{*}\left(x_{k+1}\right)} \end{array} \]

• 因此对于解【minimax cost】问题，动态规划的的递推方程如下，即从终点出发由局部最优解推出全局最优解。

\[G^*_k\left(x_k\right)=\min\limits_{u_k\in U(x_k)}\left\{\max\limits_{\theta_k\in\Theta\left(x_k,u_k\right)}\left\{l\left(x_k,u_k,\theta_k\right)+G^*_{k+1}\left(x_{k+1}\right)\right\}\right\} \]

Nondeterministic Dijkstra

Pros/Cons of minimax cost planning

• 由于每一步都是考虑的最坏的情况，因此对于不确定性的鲁棒性非常好。
• 每一步都是考虑的最坏的情况，因此过于保守和悲观，效率上也会存在一些问题。
• 相较于传统的路径规划，由于不确定性的存在更难去求解，而且Dijstra和A*只能处理能够显式建图的问题，对于一些连续的不能建图的问题，难以去应用。

Expected Cost planning With Probabilistic Model

• 在【probabilistic model】下，由于机器人大致Nature的影响是多大，因此每一步都按平均期望的cost来考虑
• 寻找一个最优的plan：\(\pi^*\)，使得由这个plan产生的所有轨迹的cost的期望值最小，即

\[G_{\pi^*}(x_s)=\min\limits_{\pi}\{G_\pi(x_s)\}=\min\limits_{\pi}\{E_{\mathcal{H}(\pi,x_s)}[L(\tilde{x},\tilde{u},\tilde{\theta})]\}. \]

• 同样的直接求解比较困难，需要借助动态规划求解

Dynamic Programming

• 首先，在最终状态\(F\)的【cost -to-go】\(G_F^*\) 可以直接表示为\(G_F^* = l_F(x_F)\)
• 从第\(K\)步到第\(K+1=F\)步的最优的【one- step - plan】的 cost 由两部分组成，一部分是由第\(K\)步到第\(K+1\)步的【transmission cost】，另一步是到达\(x_F = f(x_K,u_K,\theta_K)\)的【final state cost】，即：

\[G_K^*\left(x_K\right)=\min\limits_{u_K}\left\{E_{\theta_K}\left[l\left(x_K,u_K,\theta_K\right)+G_F^*\left(f\left(x_K,u_K,\theta_K\right)\right)\right]\right\}\quad \]

• 更为一般的，只要给定\(G_{k+1}^*\)，就可以计算出\(G_k^*\)

\[\begin{array}{l}G_{k}^{*}\left(x_{k}\right)=\min _{u_{k}}\left\{E_{\theta_{k}}\left[\min _{u_{k+1}, \ldots, u_{K}}\left\{E_{\theta_{k+1}, \ldots, \theta_{K}}\left[l\left(x_{k}, u_{k}, \theta_{k}\right)+\sum_{i=k+1}^{\kappa} l\left(x_{i}, u_{i}, \theta_{i}\right)+l_{F}\left(x_{F}\right)\right]\right\}\right]\right\} \\G_{k}^{*}\left(x_{k}\right)=\min _{u_{k}}\left\{E_{\theta_{k}}[l\left(x_{k}, u_{k}, \theta_{k}\right)+\underbrace{\left.\left.\min _{u_{k+1}, \ldots, u_{K}}\left\{E_{\theta_{k+1}, \ldots, \theta_{K}}\left[\sum_{i=k+1}^{K} l\left(x_{i}, u_{i}, \theta_{i}\right)+l_{F}\left(x_{F}\right)\right]\right\}\right]\right\}}_{G_{k+1}^{*}\left(x_{k+1}\right)}\right.\end{array} \]

• 进而得到递推方程，在MDP领域被称为【Bellman Optimality Equation】。

\[G_k^*\left(x_k\right)=\min\limits_{u_k\in U\left(x_k\right)}\left\{E_{\theta_k}\left[l\left(x_k,u_k,\theta_k\right)+G_{k+1}^*\left(x_{k+1}\right)\right]\right\} \]

Value Iteration

把每个状态点都带进去不断迭代，直到收敛为止

Pros/Cons of expected cost planning

• 在【probabilistic model】下能得到最优解。
• 需要对不确定性进行建模。
• 相对于传统的路径规划更难求解，需要迭代所有state，迭代速度对初始值和迭代顺序比较敏感。

Real Time Dynamic Programming

RTDP algorithm:

1. 将所有状态点的G值初始化为一个admissible的值，即设定的cost-to-goal要小于实际的
2. 贪心的选择action，直到到达目标点，这样就找到了的一条可行的路径
3. 将找到的路径上的状态点进行迭代更新
4. 重复执行第2-4步，直到两次迭代的误差小于\(\Delta\)

优点

1. 迭代效率更高
2. 不是迭代更新所有状态点
3. 只聚焦于相关的状态点进行迭代