似然场法定位检测 似然场最小二乘问题构建 机器人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+\th...