似然场全局定位，如何实现优化？

似然场法定位检测 似然场最小二乘问题构建 机器人Robot在地图World中的位姿表示为\(\boldsymbol{x}\),激光雷达扫描得到的点云表示为\(\{p_i^R\}\),其中\(^R\)表示在机器人坐标系下的坐标，\(_i\)表示点云中第i个点。 \[\boldsymbol{x}=[x, y, \theta]^{\rm{T}} \] 那么，点云中机器人坐标系下的某个扫描点\(\boldsymbol{p}_i^B\)的距离与角度为\(r_i\),\(\rho_i\)，那么根据当前激光的位姿，可以将它转换到世界坐标系下： \[p_i^W=[x+r_i\cos(\rho_i+\theta),y+r_i\sin(\rho_i+\theta)]^\top \] 下面思考这样一个问题： 理论上我们应该已知点云\(\{p_i^R\}\)，如何求解机器人在地图World中的位姿\(x\)，使得点云上所有的点经过变换后，都在地图上对应位置的黑线上？ 对于机器人一帧扫描得到的点云，我们能否建立一个最小二乘的优化问题，使得点云中的每个点与地图中的对应点之间的距离最小？通过这样子就可以让扫描得到的点云都尽量的落到地图黑线上。 采用高斯似然场法将扫描数据与栅格地图进行配准。 似然场围绕每一个 地图边界点 产生，随距离增大而逐渐增大，其范围和衰减过程可自行定义。（从图中的表现为图像灰度代表值，黑色为0，白色为1,其余为0-1），我们暂且先把这个场定义为似然场\(\pi\)。 似然场中的读数可直接作为配准时的目标函数，也即要求某个扫描点\(p_i^R\),经机器人位姿\(\boldsymbol{x}\)变换后，得到世界坐标系上的点\(p_i^W\),同时，存在一个世界坐标系下的似然场\(\pi\)。这个点落在似然场\(\pi\)中的读数为\(\pi(p_i^W)\) 于是机器人位姿\(x\)，可通过优化问题得到，最小二乘问题构建如下，其中n为点云中点的个数，\(\boldsymbol{p}_i^W\)为点云中第i个点在世界坐标系下的坐标，\(\|\cdot\|_2\)为L2范数,欧氏距离. \[\boldsymbol{x}^*=\arg\min_{x}\sum_{i=1}^n\|\pi(\boldsymbol{p}_i^W)\|_2^2 \] π 函数对位姿\(x\)的雅可比矩阵可由链式求导法则 \[\frac{\partial\pi}{\partial\boldsymbol{x}}=\frac{\partial\pi}{\partial\boldsymbol{p}_i^W}\frac{\partial\boldsymbol{p}_i^W}{\partial\boldsymbol{x}} \] 对于：\(\frac{\partial\pi}{\partial p_{i}^{W}}\) 似然场是以图像形式储存，因此必须对\(\boldsymbol{p}^W_i\)按照某种分辨率进行采样，设\(\boldsymbol{p}^W_i\)到图像坐标\(\boldsymbol{p}^f_i\)的转换关系为 ​$$p_i^f=\alpha p_i^W+c$$ α 表示缩放系数，\(\boldsymbol{c}\)表示距离图像中心的偏移量。 坐标系转换为：Robot 坐标系 --> World 坐标系 --> 似然场坐标系 那么 \[\frac{\partial\pi}{\partial\boldsymbol{p}_i^W}=\frac{\partial\pi}{\partial\boldsymbol{p}_i^f}\frac{\partial\boldsymbol{p}_i^f}{\partial\boldsymbol{p}_i^W}=\alpha[\Delta\pi_x,\Delta\pi_y]^\top \] 其中\(\frac{\partial\pi}{\partial p_{i}^{f}}=[\Delta\pi_{x},\Delta\pi_{y}]^{\top}\)为似然场在图像上的梯度。 对于：\(\frac{\partial\boldsymbol{p}_i^W}{\partial\boldsymbol{x}}\) 这里\(\boldsymbol{x}\)描述了世界坐标系 W 下机器人位姿。