基于似然场的全局定位
似然场法定位检测
似然场最小二乘问题构建
机器人Robot在地图World中的位姿表示为,激光雷达扫描得到的点云表示为,其中表示在机器人坐标系下的坐标,表示点云中第i个点。
那么,点云中机器人坐标系下的某个扫描点的距离与角度为,,那么根据当前激光的位姿,可以将它转换到世界坐标系下:
下面思考这样一个问题:
理论上我们应该已知点云,如何求解机器人在地图World中的位姿,使得点云上所有的点经过变换后,都在地图上对应位置的黑线上?
对于机器人一帧扫描得到的点云,我们能否建立一个最小二乘的优化问题,使得点云中的每个点与地图中的对应点之间的距离最小?通过这样子就可以让扫描得到的点云都尽量的落到地图黑线上。
采用高斯似然场法将扫描数据与栅格地图进行配准。
似然场围绕每一个 地图边界点 产生,随距离增大而逐渐增大,其范围和衰减过程可自行定义。(从图中的表现为图像灰度代表值,黑色为0,白色为1,其余为0-1),我们暂且先把这个场定义为似然场。
似然场中的读数可直接作为配准时的目标函数,也即要求某个扫描点,经机器人位姿变换后,得到世界坐标系上的点,同时,存在一个世界坐标系下的似然场。这个点落在似然场中的读数为
于是机器人位姿,可通过优化问题得到,最小二乘问题构建如下,其中n为点云中点的个数,为点云中第i个点在世界坐标系下的坐标,为L2范数,欧氏距离.
π 函数对位姿的雅可比矩阵可由链式求导法则
对于:
似然场是以图像形式储存,因此必须对按照某种分辨率进行采样,设到图像坐标的转换关系为
α 表示缩放系数,表示距离图像中心的偏移量。
坐标系转换为:Robot 坐标系 --> World 坐标系 --> 似然场坐标系 那么
其中为似然场在图像上的梯度。
对于:
这里描述了世界坐标系 W 下机器人位姿。
我们设机器人坐标系下的某个扫描点的距离与角度为,,那么根据当前激光的位姿,可以将它转换到世界坐标系下:
那么
从而得到最终的偏导公式
在实际代码中,只需要对似然场上x轴相邻两个点坐标的值做差*0.5即可(如果为了更精确的梯度,可以多阶线性差值),同理。
最小二乘问题的高斯牛顿求解
高斯—牛顿迭代法的基本思想是使用泰勒级数展开式去近似地代替非线性回归模型,然后通过多次迭代,多次修正回归系数,使回归系数不断逼近非线性回归模型的最佳回归系数,最后使原模型的残差平方和达到最小。
对于一个非线性最小二乘问题:
高斯牛顿的思想是把利用泰勒展开,取一阶线性项近似。
带入到上式,得,其中代表雅可比也就是一阶导:
对上式求导,令导数为0:
令
求解,便可以获得调整增量。这要求可逆(正定),但实际情况并不一定满足这个条件,因此可能发散,另外步长可能太大,也会导致发散,这个也就是config中的移动限制
【推荐】编程新体验,更懂你的AI,立即体验豆包MarsCode编程助手
【推荐】凌霞软件回馈社区,博客园 & 1Panel & Halo 联合会员上线
【推荐】抖音旗下AI助手豆包,你的智能百科全书,全免费不限次数
【推荐】博客园社区专享云产品让利特惠,阿里云新客6.5折上折
【推荐】轻量又高性能的 SSH 工具 IShell:AI 加持,快人一步
· 清华大学推出第四讲使用 DeepSeek + DeepResearch 让科研像聊天一样简单!
· 推荐几款开源且免费的 .NET MAUI 组件库
· 实操Deepseek接入个人知识库
· 易语言 —— 开山篇
· Trae初体验