草稿

文档维护：Arvin

网页部署：Arvin

▶

写在前面：Ubuntu20.04和Windows系统安装教程

周记

决策过程（笔记）

决策过程的概念

马尔可夫决策过程

safe RL

将驾驶过程近似为马尔科夫过程，即下个状态只与当前状态有关系。

主要贡献：

• **证明马尔科夫假设在策略梯度法中的不必要性；**使用策略梯度迭代的方法求解最优策略；使用baseline subtraction的方法，最小化对累积奖励估计的方差。

• 将问题分解为两个组成部分：一个是"Desires"策略，需要学习；而是带有硬约束的轨迹规划，不需要学习。“Desires”策略提高了驾驶的舒适性，轨迹规划的硬约束保证了驾驶的安全性；

• 引入有向无环图，即状态集。

MPDM: Multipolicy Decision-Making 多策略决策

• 将车辆行为建模成合理、安全的策略以减少不确定性；
• 通过前向模拟考虑周车对自身的影响；
• 评估不同策略下的模拟结果，选择最优的策略；

待解决问题：

• 周车的策略生命周期
• 后验更新
• 组合爆炸，关键场景

不确定性感知的决策过程

POMDP部分可观测马尔可夫决策过程

定义：

• $S$：状态集

• $A$：动作集

• $T$：一个集合，它指定给定前一个状态和动作的下一个状态的条件概率

  $T(s',a,s)=Pr(s'|s,a)$

• $R$：$S \times A \rightarrow R$，奖励函数

  $R(s_t, a_t)$是指在$t$时刻状态$s_t$下采取动作$a_t$后获得的奖励。

• $\Omega$：观测值的集合

• $O$：观测概率的集合，表示执行动作$a$后，系统转移到状态$s'$后，观测值为$o$的概率：

  $O(o,s',a)=O(o|s',a)=Pr(o|s',a)$

• $\gamma$：$\gamma \in [0, 1]$，折扣因子

优化决策过程：

在POMDP中，智能体在每个时间步$t$的动作决策取决于当前的置信状态$b_t$。置信状态通过历史观测和动作进行更新。

智能体选择动作$a_t$的策略$\pi$是一个从置信状态到动作的映射，表示为$\pi(b_t)=a_t$。最优策略$\pi*$最大化从当前置信状态开始的期望折扣总奖励：

$$\pi^*= \arg \max_{\pi}E_{\pi}[\textstyle \sum_{t=0}^{\infty} \gamma^t R(s_t, a_t)|b_0]$$

置信状态更新：

置信状态$b(s')$的更新取决于前一个置信状态$b(s)$，最后一个动作$a$和最后观测值$o\in \Omega$决定

$b(s)$表示环境处于状态$s$的概率，已知$b(s)$，动作$a$和观测$o$，有：

$$b'(s')=\frac{O(o|s',a)\sum_{s\in S}T(s'|s,a)b(s)}{\sum_{s'\in S} O(o|s',a)\sum_{s \in S}T(s'|s,a)b(s)}$$

举例：

老虎问题

求解

常规方法1：连续状态的“置信MDP”

常规方法2：

常规方法3：

端到端自动驾驶

Transfuser

提出了一种新颖的端到端驱动架构，主要包含两个方面。

1. 用于融合多传感器模态（图像和雷达）的Transforms架构。
2. 提出了一种自回归航路点预测网络。

Problem Setting

任务目标：城市环境中点对点导航任务，目标是在安全响应其他动态智能体并遵守交通规则的情况下完成给定路线。

**模仿学习（Imitation Learning）：**模仿学习专家行为策略$\pi^{*}$。本文完成的是输入到路径点的映射，然后路径点提供给单独的底层控制器输出动作。首先是专家数据集size大小为$Z$的$a$，策略$\pi$使用收集到的数据$\mathcal{D}$和损失函数$\mathcal{L}$以监督的方式进行训练。

$$\mathop{\mathrm{argmin}}_{\pi} \mathbb{E}_{(\mathcal{X},\mathcal{W})\sim\mathcal{D}}\left[\mathcal{L}(\mathcal{W},\pi(\mathcal{X}))\right]$$

**全局规划器：**遵循CARLA0.9.10的标准协议，假设高级目标位置$g$是由GPS提供坐标，目标位置是稀疏的，可能相距数百米，而不是由策略$\pi$预测的局部路径点。

Input and Output Parameterization

**输入：**雷达点云和RGB图像。

1. 雷达：将雷达点云转换成固定分辨率的2D BEV网格上的2-bin直方图。本文考虑了自驾车前方32米内的点和两侧16米内的点，从而包含了一个32m × 32m的BEV网格。我们将网格划分为0.125m × 0.125m的块，得到256 × 256像素的分辨率。对于直方图，我们将高度维度离散成2 bins，分别代表地平面上/下和地平面上的点。我们还在与LiDAR点云相同的256 × 256 BEV空间中栅格化了2D目标位置，并将该通道连接到2 bins。这将产生一个大小为256 × 256像素的三通道伪图像。我们在BEV中表示目标位置，因为与透视图像域相比，这与路点预测的相关性更好。
2. RGB图像：对于RGB输入，使用三个摄像头（面向前，60°左和60°右）。每个相机有一个水平视场120°。我们以960 × 480像素的分辨率提取图像，将其裁剪为320 × 160以消除边缘的径向畸变。这三个未失真的图像组成成一个单一的图像输入到编码器，其分辨率为704 × 160像素和132°FOV。

**输出：**以小车自身坐标系，预测小车在BEV空间中的未来轨迹$\mathcal{W}$。轨迹由一系列2D航路点组成$\{W_t=(x_t,y_t)\}^T_{t=1}$，我们令$T=4$，这是逆动力学模型所需的默认航路点数。

Multi-Model Fusion Transformer

本文的思想就是利用Transformers的自注意力机制将多模态传感器数据融合在一起。如下图所示：

将输入序列表示为$F^{in} \in \mathbb{R}^{N \times D_{f}}$，$N$表示序列的token数量，每个token用一个特征向量$D_f$表示。Transformer使用线性投影计算一系列的$Q,K,V$：

$$\mathbf{Q}=\mathbf{F}^{in}\mathbf{M}^{q},\mathbf{~K}=\mathbf{F}^{in}\mathbf{M}^{k},\mathbf{~V}=\mathbf{F}^{in}\mathbf{M}^{v}$$

其中$M^q \in \mathbb{R}^{D_f \times D_q},M^q \in \mathbb{R}^{D_f \times D_k},M^q \in \mathbb{R}^{D_f \times D_v}$都是权重矩阵。然后使用$Q$和$K$之间的缩放点积来计算注意力权重，汇总每个query的值。

$$\mathbf{A}={\mathrm{softmax}}\left({\frac{\mathbf{Q}\mathbf{K}^{T}}{\sqrt{D_{k}}}}\right)\mathbf{V}$$

最后，transformer使用非线性变换计算输出特征，$\mathbf{F}^{out}$的shape和$\mathbf{F}^{in}$一样。

$$\mathbf{F}^{out}=MLP(\mathbf{A})+\mathbf{F}^{in}$$

transformer在整个网络结构中多次应用注意力机制，从而产生$L$个注意力层。

Waypoint Prediction Network

如图所示，为了提高计算效率，将512维特征向量通过MLP（包含256和128个单元的2个隐藏层）将其降维至64维，然后将其传递给使用gru实现的自回归路点网络。用64维特征向量初始化GRU的隐藏状态。GRU的更新门控制在隐藏状态下编码到输出和下一个时间步长的信息流。它还将当前位置和目标位置作为输入，这使得网络能够专注于隐藏状态下的相关上下文，以预测下一个路点。除了编码器之外，提供了目标位置的GPS坐标（注册到自我-车辆坐标框架）作为GRU的输入，因为它可以更直接地影响路点预测。之后使用一个单层GRU，然后是一个线性层，该线性层接受隐藏状态，并预测自我-车辆当前坐标系中T = 4个未来时间步长的微分路径点$\{\delta w_t\}^T_{t=1}$。因此，预测未来的路点由$\left<!–swig83–>=\mathrm{Ax}+\mathrm{Bu}\\mathrm{y}=\mathrm{Cx}+\mathrm{Du}\end{cases} \tag{1}

其中，$\mathrm{x}(t) \in \mathrm{R^n}, \mathrm{u}(t) \in \mathrm{R^m}$，初始条件是$\mathrm{x(0)}$。 在此，我们设计一个状态反馈控制器

\mathrm{u=-Kx} \tag{2}

$$为了使控制器达到期望的稳定性能，将2式带入1式$$

\mathrm{\dot{x}=(A-BK)x=A_cx} \tag{3}

设定系统中的各个状态量都可知，式(1)所示的开环系统，传递函数的极点就是系统矩阵A的特征值。现在变换成了式(2)的闭环形式，通过配置反馈矩阵K KK，可以使得闭环系统达到所期望的系统状态。 ### LQR LQR的目标就是找到一组控制量$\mathrm{u_0}, \mathrm{u_1}, ...$使得同时有$\mathrm{x_0}, \mathrm{x_1}, ...$足够小（系统达到稳定状态），$\mathrm{u_0}, \mathrm{u_1}, ...$足够小（控制量进行小的变化），选取代价函数为

\mathrm J=\frac12 \int_0^\infty\mathrm x^\mathrm{T}\mathrm Q\mathrm x+\mathrm u^\mathrm{T}\mathrm R\mathrm u \mathrm d\mathrm t \tag{4}

其中，$Q, R$就是需要设计的半正定矩阵和正定矩阵。 1. 将$\mathrm{u}=-\mathrm{Kx}$代入代价函数，有

\mathrm J=\frac12 \int_0^\infty\mathrm x^\mathrm{T}(\mathrm Q+\mathrm K^\mathrm{T}\mathrm R\mathrm K)\mathrm x \mathrm d\mathrm t \tag{5}

2. 假设存在一个常量矩阵$P$使得，

\frac{\mathrm d}{\mathrm d\mathrm t}\left(\mathrm x^\mathrm{T}\mathrm P\mathrm x\right)=-\mathrm x^\mathrm{T}\left(\mathrm Q+\mathrm K^\mathrm{T}\mathrm R\mathrm K\right)\mathrm x \tag{6}

$$3. 把式6带入式5，有$$

\mathrm J=-\frac{1}{2} \int_0^\infty\frac{\mathrm d}{\mathrm d\mathrm t}\mathrm x^\mathrm{T}(\mathrm P)\mathrm x =\frac{1}{2}\mathrm x^\mathrm{T}(0)\mathrm P\mathrm x(0) \tag{7}

式7的意思就是，t趋近于无穷时，系统状态向量$\mathrm{x(t)}$趋近于0，这样就直接结算除了积分方程。 4. 把式6左边微分展开

\mathrm x^\mathrm{T}\mathrm P\mathrm x+\mathrm x^\mathrm{T}\mathrm P\mathrm x+\mathrm x^\mathrm{T}\mathrm Q\mathrm x+\mathrm x^\mathrm{T}\mathrm K^\mathrm{T}\mathrm R\mathrm K\mathrm x=0

状态变量$\mathrm{x}$的微分用式3表示

\mathrm x^\mathrm{T}\mathrm A^\mathrm{T}\mathrm P\mathrm x+\mathrm x^\mathrm{T}\mathrm P\mathrm A_\mathrm{c}\mathrm x+\mathrm x^\mathrm{T}\mathrm Q\mathrm x+\mathrm x^\mathrm{T}\mathrm K^\mathrm{T}\mathrm R\mathrm K\mathrm x=0

$$整理后，有$$

\mathrm{x^T(A_c^{T}P~+PA_c+Q+K^T RK)x=0} \tag{8}

$$这样，就又回到了二次型的问题，如果式8要有解，那么括号里面的部分必须等于0。$$

\mathrm{A_c^{T}P~+PA_c+Q+K^{T}RK=0} \tag{9}

把$A_c=A-BK$代入式9

\mathrm{(A-BK)^{T}P~+P(A-BK)+Q+K^{T}RK=0} \tag{10}

$$$$

\mathrm A^\mathrm{T}\mathrm P+\mathrm P\mathrm A+\mathrm Q+\mathrm K^\mathrm{T}\mathrm R\mathrm K-\mathrm K^\mathrm{T}\mathrm B^\mathrm{T}\mathrm P-\mathrm P\mathrm B\mathrm K=0 \tag{11}

5. 式11还是一个关于$\mathrm{K}$的二次型，这样会导致计算量太复杂，我们令$\mathrm{K=R^{-1}B^{T}P}$，然后式11， 可以化为

\mathrm A^\mathrm{T}\mathrm P+\mathrm P\mathrm A+\mathrm Q+\mathrm K^\mathrm{T}\mathrm R(\mathrm R^{-1}\mathrm B^\mathrm{T}\mathrm P)-\mathrm K^\mathrm{T}\mathrm B^\mathrm{T}\mathrm P-\mathrm P\mathrm B(\mathrm R^{-1}\mathrm B^\mathrm{T}\mathrm P)=0

$$$$

\mathrm A^\mathrm{T}\mathrm P+\mathrm P\mathrm A+\mathrm Q-\mathrm P\mathrm B\mathrm R^{-1}\mathrm B^\mathrm{T}\mathrm P=0 \tag{12}

式12中，$A, B, Q, R$都是已知量，那么通过式12可以求解出$P$，式12就是著名的Raccati方程。 ### 差速机器人 前面我们已经知道了差速机器人运动学模型

\begin{cases}\dot{x}=V\cos\theta\\dot{y}=V\sin\theta\\dot{\theta}=\omega\end{cases}

$$我们对其离散化$$

X_e(k+1)=\begin{bmatrix}1&0&-Tv_r\sin\theta_r\0&1&Tv_r\cos\theta_r\0&0&1\end{bmatrix}X_e(k)+\begin{bmatrix}T\cos\theta_r&0\T\sin\theta_r&0\0&T\end{bmatrix}u(k)

# LATEX ## 命令行安装 ### 安装 * 安装latex发行版TeX Live

sudo apt-get install texlive-full

* 安装XeLatex编译引擎

sudo apt-get install texlive-xetex

* 安装中文支持包，使用的是xeCjK

sudo apt-get install texlive-lang-chinese

* 安装图形化界面

sudo apt-get install texstudio

### 配置 设置中文 Options->Configure TeXstudio->General->Language更改为zh-CN即可  设置中文编译器，默认编译器设置为XaLaTex  ### 测试 新建文本，输入以下内容：

\documentclass{article}
\usepackage{xeCJK}
\begin{document}
	
	\section{前言}
	
	\section{关于数学部分}
	数学、中英文皆可以混排。You can intersperse math, Chinese and English (Latin script) without adding extra environments.
	
	這是繁體中文。
\end{document}

按F5进行编译 ## 源码安装 * 创建挂载点

sudo mkdir /mnt/cdrom

* 挂载下载到的 texlive.iso 文件到挂载点

sudo mount -o loop ./texlive.iso /mnt/cdrom

* cd 到挂载目录

cd /mnt/cdrom

* 使用 root 权限安装 TexLive

sudo ./install-tl

* 安装完 TeXLive 后要及时卸载挂载的镜像，方便以后挂载其他镜像文件

sudo umount -v /mnt/cdrom

* 设置环境变量

sudo gedit ~/.bashrc

插入：

export MANPATH=${MANPATH}:/usr/local/texlive/2024/texmf-dist/doc/man
export INFOPATH=${INFOPATH}:/usr/local/texlive/2024/texmf-dist/doc/info
export PATH=${PATH}:/usr/local/texlive/2024/bin/x86_64-linux

然后

source ~/.bashrc

* 安装字体库（宋体）

cd  /usr/share/fonts
mkdir chinese

将本地C:\Windows\Fonts字体文件拷到chinese文件夹下，注意将 ttc后缀改成ttf

sudo mv ~/02_study/Fonts /usr/share/fonts/chinese/

* 执行下列命令查看是否成功

fc-list | grep "SimSun"

# 决策过程 ## 隐形马尔可夫模型 > [参考链接](https://blog.csdn.net/weixin_42175217/article/details/105442777) ### 定义 **隐马尔可夫模型**（英语：Hidden Markov Model；[缩写](https://zh.wikipedia.org/wiki/縮寫)：**HMM**），或称作**隐性马尔可夫模型**，是[统计](https://zh.wikipedia.org/wiki/统计)[模型](https://zh.wikipedia.org/wiki/模型)，用来描述一个含有隐含未知参数的[马尔可夫过程](https://zh.wikipedia.org/wiki/马尔可夫过程)。 在**正常的**马尔可夫模型中，状态对于观察者来说是直接可见的。这样状态的转换概率便是全部的参数。而在隐马尔可夫模型中，状态并不是直接可见的，但受状态影响的某些变量则是可见的。每一个状态在可能输出的观测上都有一[概率分布](https://zh.wikipedia.org/wiki/概率分布)。因此输出观测的序列能够透露出状态序列的一些信息。 * $Q$：所有可能的隐藏状态集合；

Q = {q_1, q_2, q_3, …, q_N}

* $V$：所有可能的观测状态的集合：

V={v_1, v_2, v_3, …,v_M}

* $I$：对应的长度为$T$的状态序列

I={i_1, i_2, …, i_T}

* $O$：对应的长度为$T$观察序列

O={o_i, o_2, …, o_T}

* 两个重要假设 * 齐次马尔科夫假设：任意时刻的隐藏状态只依赖于它前一个隐藏状态。如果在时刻$t$的隐藏状态是$i_t=q_i$，在时刻$t+1$的隐藏状态是$i_{i+1}=q_j$，则从时刻$t$到时刻$t+1$的HMM状态转移概率$a_{ij}$可以表示为：

a_{ij}=P(i_{t+1}=q_j|i_t=q_i)
$$
这样$a_{ij}$可以组成马尔科夫链的状态转移矩阵$A$：
$$
A = [a_{ij}]_{N \times N}
$$

• 观测独立性假设：任意时刻的观察状态只仅仅依赖于当前时刻的隐藏状态。如果在时刻$t$的隐藏状态是$i_t=q_j$，而对应的观察状态为$o_t=v_k$，则该时刻观察状态$v_k$在隐藏状态$q_j$下生成的概率为$b_j(k)$，满足：

  $$b_j(k)=P(o_t=v_k|i_t=q_j)$$

  这样$b_j(k)$可以组成马尔科夫链的状态转移矩阵$B$：

  $$B=[b_j(k)]_{N \times M}$$

• 初始隐藏状态概率$\Pi$：

  $$\Pi=[\pi(i)]_N$$

  其中$\pi(i)=P(i_1=q_i)$

一个HMM模型，可以由隐藏状态初始概率分布$\Pi$，状态转移概率矩阵$A$和观测状态概率矩阵$B$决定。$\Pi，A$决定状态序列，$B$决定观测序列。因此，HMM模型可以由一个三元组$\lambda$表示如下：

$$\lambda=(A, B, \Pi)$$

实例

假设我们有3个盒子，每个盒子里都有红色和白色两种球，这三个盒子里球的数量分别是：

盒子	1	2	3
红球数	5	4	7
白球数	5	6	3

按照下面的方法从盒子里抽球，开始的时候，从第一个盒子抽球的概率是0.2，从第二个盒子抽球的概率是0.4，从第三个盒子抽球的概率是0.4。以这个概率抽一次球后，将球放回。然后从当前盒子转移到下一个盒子进行抽球。规则是：如果当前抽球的盒子是第一个盒子，则以0.5的概率仍然留在第一个盒子继续抽球，以0.2的概率去第二个盒子抽球，以0.3的概率去第三个盒子抽球。如果当前抽球的盒子是第二个盒子，则以0.5的概率仍然留在第二个盒子继续抽球，以0.3的概率去第一个盒子抽球，以0.2的概率去第三个盒子抽球。如果当前抽球的盒子是第三个盒子，则以0.5的概率仍然留在第三个盒子继续抽球，以0.2的概率去第一个盒子抽球，以0.3的概率去第二个盒子抽球。如此下去，直到重复三次，得到一个球的颜色的观测序列:

$$O=\{红， 白，红\}$$

在这个过程中，观察者只能看到球的颜色序列，不能看到球是从哪个盒子里取出的。

则按照HMM定义：

观测集合为：

$$V=\{红，白\}, M=2$$

状态集合为：

$$Q=\{盒子1， 盒子2， 盒子3\}, N=3$$

观测序列长度为2，状态序列长度为3。

初始状态分布为：

$$\Pi=(0.2, 0.4, 0.4)^T$$

状态转移概率分布矩阵为：

$$A=\begin{pmatrix}0.5&0.2&0.3\\0.3&0.5&0.2\\0.2&0.3&0.5\end{pmatrix}$$

观测状态概率矩阵为：

$$B=\begin{pmatrix}0.5&0.5\\0.4&0.6\\0.7&0.3\end{pmatrix}$$

求观测序列的概率

我们已知HMM模型的参数$\lambda=(A, B, \Pi)$，同时我们也已经得到观测序列$O=\{o_1, o_2, ..., o_T\}$，现在我们要求观测序列$O$在模型$\lambda$下出现的条件概率$P(O|\lambda)$。

暴力求解法

首先，任意一个隐藏序列$I=\{i_1, i_2, ..., i_T\}$出现的概率是：

$$P(i|\lambda)=\pi_{i_1}a_{i_1i_2}a_{i_2i_3}...a_{i_{T-1}i_T}$$

对于固定的状态序列$I=\{i_1,i_2,...,i_T\}$，我们要求的观察序列$O=\{o_1,o_2,...,o_T\}$出现的概率是：

$$P(O|I,\lambda)=b_{i_1}(o_1)b_{i_2}(o_2)...b_{i_T}(o_T)$$

则$O$和$I$联合出现的概率是：

$$P(O, I \mid \lambda)=P(I \mid \lambda) P(O \mid I, \lambda)=\pi_{i_{1}} b_{i_{1}}\left(o_{1}\right) a_{i_{12} i_{2}} b_{i_{2}}\left(o_{2}\right) \ldots a_{i_{T-1} i_{T}} b_{i_{T}}\left(o_{T}\right)$$

然后求边缘概率分布，即可得到观测序列$O$在模型$\lambda$下出现的概率$P(O|\lambda)$：

$$P(O|\lambda)=\sum_IP(O,I|\lambda)=\sum_{i_1i_2...i_T}\pi_{i_1}b_{i_1}(o_1)a_{i_{\text{F}2}}b_{i_2}(o_2)\ldots a_{i_{T-1}i_T}b_{i_T}(o_T)$$

虽然上述方法有效，但是如果隐藏状态数$N$非常多，预测状态有$N^T$种组合，算法复杂度是$O(TN^T)$阶的。

前向算法

前向后向算法是前向算法和后向算法的统称，这两个算法都可以用于求HMM观测序列的概率。

前向算法本质上属于动态规划的算法，也就是我们要通过局部状态递推的公式，然后一步步从子问题的最优解拓展到整个问题的最优解。

前向概率：定义时刻$t$时隐藏状态为$q_i$，观测状态的序列为$o_1,o_2,...,o_t$的概率为前向概率，记为：

$$\alpha_t(i)=P(o_1,o_2,\ldots o_t,i_t=q_i|\lambda)$$

假设在时刻$t$时各个隐藏状态的前向概率，现在我们需要递推出时刻$t+1$时各个隐藏状态的前向概率。

我们可以基于时刻$t$时各个隐藏状态的前向概率，再乘以对应的状态转移概率，即$\alpha_t(j)a_{ji}$就是在时刻$t$观测到$o_1,o_2,o_3,...,o_t$，并且时刻$t+1$隐藏状态$q_i$的概率。对于时刻$t$各个隐藏状态的前向概率进行求和，在时刻$t+1$的隐藏状态$q_i$观测到$o_{t+1}$的概率为$b_i(o_{t+1})$。

$$\alpha_{t+1}(i)=\Big[\sum_{j=1}^N\alpha_t(j)a_{ji}\Big]b_i(o_{t+1})$$

完整步骤如下：

1. 计算时刻1的各个隐藏状态前向概率：

   $$\alpha_1(i)=\pi_ib_i(o_1),\mathrm{~}i=1,2,\ldots N$$

2. 递推时刻2,3,…,T时刻的前向概率：

   $$\alpha_{t+1}(i)=\left[\sum_{j=1}^N\alpha_t(j)a_{ji}\right]b_i(o_{t+1}), i=1,2,\ldots N$$

3. 计算最终结果：

   $$P(O|\lambda)=\sum_{i=1}^N\alpha_T(i)$$

   从递推公式可以看出，算法时间复杂度是$O(TN^2)$，比暴力解法的时间复杂度$O(TN^T)$少了几个数量级。

后向算法

后向算法与前向算法十分类似，后向算法用的是“后向概率”。定义时刻$t$时隐藏状态为$q_i$，从时刻$t+1$到最后时刻$T$的观测状态的序列为$o_{t+1},o_{t+2},...o_T$的概率为后向概率，记为：

$$\beta_t(i)=P(o_{t+1},o_{t+2},...o_T|i_t=q_i,\lambda)$$

后向概率的动态规划与前向概率相反。现已找到时刻$t+1$时各隐藏状态的后向概率$\beta_{t+1}(j)$，现递推出时刻$t$时各个隐藏状态的后向概率。我们可以计算出观测状态的序列为$o_{t+2},o_{t+3},...o_T$，$t$时隐藏状态为$q_i$，时刻$t+1$隐藏状态为$q_j$的概率为$a_{ij}\beta_{t+1}(j)$，接着可以得到观测状态的序列为$o_{t+1},o_{t+2},...o_T$，t时隐藏状态为$q_i$，时刻$t+1$隐藏状态为$q_j$的概率为$a_{ij}b_j(o_{t+1})\beta_{t+1}(j)$，则把所有隐藏状态的概率加起来可以得到观测状态的序列为$o_{t+1},o_{t+2},...o_T$，$t$时隐藏状态为$q_i$的概率为$\sum_{j=1}^Na_{ij}b_j(o_{t+1})\beta_{t+1}(j)$，这个概率即为时刻$t$的后向概率。即递推公式如下：

$$\beta_t(i)=\sum_{j=1}^Na_{ij}b_j(o_{t+1})\beta_{t+1}(j)$$

完整步骤如下：

1. 计算时刻$T$的各个隐藏状态后向概率：

   $$\beta_T(i)=1, i=1,2,\ldots N$$

2. 递推时刻$T-1,T-2,...1$时刻的前向概率：

   $$\beta_t(i)=\sum_{j=1}^Na_{ij}b_j(o_{t+1})\beta_{t+1}(j), i=1,2,\ldots N$$

3. 计算最终结果：

   $$P(O|\lambda)=\sum_{i=1}^N\pi_ib_i(o_1)\beta_1(i)$$

   从递推公式可以看出，算法时间复杂度是$O(TN^2)$。

例子

以前文例子完成一遍前向算法的流程

1. 时刻1各个隐藏状态的前向概率：

   \begin{eqnarray} \alpha_1(1)&=\pi_1b_1(o_1)=0.2*0.5=0.1 \\ \alpha_1(2)&=\pi_2b_2(o_1)=0.4*0.4=0.16 \\ \alpha_1(3)&=\pi_3b_3(o_1)=0.4*0.7=0.28 \end{eqnarray}

2. 时刻2和时刻3

   时刻2

   \begin{eqnarray} \alpha_2(1)&=(0.1*0.5+0.16*0.3+0.28*0.2)*0.5=0.077 \\ \alpha_2(2)&=(0.1*0.2+0.16*0.5+0.28*0.3)*0.6=0.1104 \\ \alpha_2(3)&=(0.1*0.3+0.16*0.2+0.28*0.5)*0.3=0.0606 \end{eqnarray}

   时刻3

   \begin{eqnarray} \alpha_3(1)&=(0.077*0.5+0.1104*0.3+0.28*0.2)*0.5=0.04187 \\ \alpha_3(2)&=(0.077*0.2+0.1104*0.5+0.28*0.3)*0.6=0.03551 \\ \alpha_3(3)&=(0.077*0.3+0.1104*0.2+0.28*0.5)*0.3=0.05284 \end{eqnarray}

3. 计算最终结果

   $$P(O|\lambda)=\sum_{i=1}^{3}\alpha_3(i)=0.13022$$

HMM常用概率的计算

1. 给定模型$\lambda$和观测序列$O_t$在时刻$t$处于状态$q_i$的概率记为：

   $$\gamma_t(i)=P(i_t=q_i|O,\lambda)=\frac{P(i_t=q_i,O|\lambda)}{P(O|\lambda)}$$

   利用前向概率和后向概率的定义可知：

   $$P(i_t=q_i,O|\lambda)=\alpha_t(i)\beta_t(i)$$

   于是可以得到：

   $$\gamma_t(i)=\frac{\alpha_t(i)\beta_t(i)}{\sum_{j=1}^N\alpha_t(j)\beta_t(j)}$$

2. 给定模型$\lambda$和观测序列$O$，在时刻$t$处于状态$q_i$，且时刻$t+1$处于状态$q_j$的概率为：

   $$\xi_t(i,j)=P(i_t=q_i,i_{t+1}=q_j|O,\lambda)=\frac{P(i_t=q_i,i_{t+1}=q_j,O|\lambda)}{P(O|\lambda)}$$

   而$P(i_t=q_i，i_{t+1}=q_j,O|\lambda)$可以由前向后向概率来表示为：

   $$\xi_t(i,j)=\frac{\alpha_t(i)a_{ij}b_j(o_{t+1})\beta_{t+1}(j)}{\sum_{r=1s=1}^N\sum_{s=1}^N\alpha_t(r)a_{rs}b_s(o_{t+1})\beta_{t+1}(s)}$$

   从而最终我们得到$\xi_t(i,j)$的表达式如下：

   $$\xi_t(i,j)=\frac{\alpha_t(i)a_{ij}b_j(o_{t+1})\beta_{t+1}(j)}{\sum_{r=1s=1}^N\sum_{s=1}^N\alpha_t(r)a_{rs}b_s(o_{t+1})\beta_{t+1}(s)}$$

3. 将$\gamma_t(i)$和$\xi_t(i,j)$在各个时刻求和可以得到：

   在观测序列$O$下状态$i$出现的期望值$\sum_{t=1}^{T}\gamma_{t}(i)$

   在观测序列$O$下由状态$i$转移的期望值$\sum_{t=1}^{T-1}\gamma_t(i)$

   在观测序列$O$下由状态$i$转移到状态$j$的期望值$\sum_{t=1}^{T-1}\xi_t(i,j)$

HMM模型参数求解概述

情况一

我们已知$D$个长度为$T$的观测序列和对应的隐藏状态序列，即$\{(O_1,I_1),(O_2,I_2),...,(O_D,I_D)\}$是已知的，此时我们可以很容易得最大似然来求解参数。

假设样本从隐藏状态$q_i$转移到$q_j$的频率计数是$A_{ij}$，那么状态转移矩阵求得为：

$$A=\begin{bmatrix}a_{ij}\end{bmatrix},\text{ 其中}a_{ij}=\frac{A_{ij}}{\sum_{s=1}^NA_{is}}$$

假设样本隐藏状态为$q_j$且观测状态为$v_k$的频率计数是$B_{jk}$，那么观测状态概率矩阵为：

$$B=\begin{bmatrix}b_j(k)\end{bmatrix},\text{ 其中}b_j(k)=\frac{B_{jk}}{\sum_{s=1}^MB_{js}}$$

假设所有样本中初始隐藏状态为$q_i$的频率计数为$C(i)$，那么初始概率分布为：

$$\Pi=\pi(i)=\frac{C(i)}{\sum_{s=1}^NC(s)}$$

情况二

只知道$D$个长度为$T$的观测序列，即${(O_1),(O_2),...(O_D)}$是已知的，此时能不能求出合适的HMM模型参数？下面介绍一种名为鲍姆-韦尔奇算法。

原理

鲍姆-韦尔奇算法原理使用的就是EM算法的原理，那么需要在$E$步求出联合分布$P(O,I|\lambda)$基于条件概率$P(I|O,\bar{\lambda})$的期望，其中$\bar{\lambda}$为当前的模型参数，然后再$M$步最大化这个期望，得到更新的模型参数$\lambda$，接着不停的进行$EM$迭代，直到模型参数的值收敛为止。

首先先看看$E$步，当前模型参数为$\bar{\lambda}$，联合分布$P(O,I|\lambda)$基于条件概率$P(I|O,\bar{\lambda})$的期望表达式为：

$$P_n(k)=C_n^kp^kq^{n-k}, k=0,1,2,\cdots,n 。$$

在$M$步，我们极大化上式，然后得到更新后的模型参数如下：

$$\overline{\lambda}=arg\max_{\lambda}\sum_{I}P(I|O,\overline{\lambda})logP(O,I|\lambda)$$

通过不断的$E$步和$M$步的迭代，直到$\bar{\lambda}$收敛。

流程

输入：$D$个观测序列样本$\{(O_1),(O_2),...(O_D)\}$

输出：HMM模型参数

1. 随机初始化所有的$\pi_i,a_{ij},b_j(k)$

2. 对于每个样本$d=1,2,...D$，用前向后向算法计算$\gamma_t^{(d)}(i), \xi_t^{(d)}(i,j),t=1,2...T$

3. 更新模型参数

   $$\pi_i=\frac{\sum_{d=1}^D\gamma_1^{(d)}(i)}D$$

   $$a_{ij}=\frac{\sum_{d=1}^D\sum_{t=1}^{T-1}\xi_t^{(d)}(i,j)}{\sum_{d=1}^D\sum_{t=1}^{T-1}\gamma_t^{(d)}(i)}$$

   $$b_j(k)=\frac{\sum_{d=1}^D\sum_{t=1,o_t^{(d)}=v_k}^T\gamma_t^{(d)}(j)}{\sum_{d=1}^D\sum_{t=1}^T\gamma_t^{(d)}(j)}$$

4. 如果$\pi_i,a_{ij},b_j(k)$的值已经收敛，则算法结束，否则回到第2步继续迭代。

实验步骤

• 启动小车控制节点

  sudo ip link set can0 up type can bitrate 500000
  roslaunch scout_base my_car.launch

• 启动r3_live节点

  roslaunch livox_ros_driver livox_lidar_msg.launch
  roslaunch realsense2_camera rs_d435_camera_with_model.launch
  roslaunch r3live r3live_bag.launch

• 录制bag包

  rosbag record -o test_wheel_0.1.bag /odom /aft_mapped_to_init /cmd_vel

• 轨迹跟踪

  roslaunch scout_unitree_control scout_tracking.launch

1. open_straight
2. open_curve
3. open_highstraight
4. wheel_straight_delay0.1
5. wheel_curve_delay0.1
6. wheel_highstraight_delay0.1
7. wheel_straight_delay0.2
8. wheel_highstraight_delay0.2
9. Lvio_straight_delay0.2
10. Lvio_highstraight_delay0.2

april_odom

• apriltag_ros::AprilTagDetectionArray消息类型

  std_msgs/Header header
    uint32 seq
    time stamp
    string frame_id
  apriltag_ros/AprilTagDetection[] detections
    int32[] id
    float64[] size
    geometry_msgs/PoseWithCovarianceStamped pose
      std_msgs/Header header
        uint32 seq
        time stamp
        string frame_id
      geometry_msgs/PoseWithCovariance pose
        geometry_msgs/Pose pose
          geometry_msgs/Point position
            float64 x
            float64 y
            float64 z
          geometry_msgs/Quaternion orientation
            float64 x
            float64 y
            float64 z
            float64 w
        float64[36] covariance

其他

工控机各种库的版本

查看ros版本命令roscore，ros版本noetic。

查看pcl版本命令dpkg -l libpcl-dev，pcl版本1.8.1。

查看eigen3版本命令dpkg -s libeigen3-dev | grep 'Version'，eigen3版本3.3.4。

查看opencv版本命令pkg-config --modversion opencv，opencv版本

报错记录

报错一

**描述：**使用eigen库中求逆运算时报错

undefined reference to `Eigen::MatrixBase<Eigen::Matrix<double, 2, 2, 0, 2, 2> >::inverse() const'

**原因：**头文件引用错误

解决：

#include <Eigen/LU>