基于似然场的全局定位

似然场法定位检测

似然场最小二乘问题构建

机器人Robot在地图World中的位姿表示为x,激光雷达扫描得到的点云表示为{piR},其中R表示在机器人坐标系下的坐标,i表示点云中第i个点。

x=[x,y,θ]T

那么,点云中机器人坐标系下的某个扫描点piB的距离与角度为ri,ρi,那么根据当前激光的位姿,可以将它转换到世界坐标系下:

piW=[x+ricos(ρi+θ),y+risin(ρi+θ)]

下面思考这样一个问题:

理论上我们应该已知点云{piR},如何求解机器人在地图World中的位姿x,使得点云上所有的点经过变换后,都在地图上对应位置的黑线上?
对于机器人一帧扫描得到的点云,我们能否建立一个最小二乘的优化问题,使得点云中的每个点与地图中的对应点之间的距离最小?通过这样子就可以让扫描得到的点云都尽量的落到地图黑线上。

采用高斯似然场法将扫描数据与栅格地图进行配准。

似然场围绕每一个 地图边界点 产生,随距离增大而逐渐增大,其范围和衰减过程可自行定义。(从图中的表现为图像灰度代表值,黑色为0,白色为1,其余为0-1),我们暂且先把这个场定义为似然场π

似然场中的读数可直接作为配准时的目标函数,也即要求某个扫描点piR,经机器人位姿x变换后,得到世界坐标系上的点piW,同时,存在一个世界坐标系下的似然场π。这个点落在似然场π中的读数为π(piW)

于是机器人位姿x,可通过优化问题得到,最小二乘问题构建如下,其中n为点云中点的个数,piW为点云中第i个点在世界坐标系下的坐标,2为L2范数,欧氏距离.

x=argminxi=1nπ(piW)22

π 函数对位姿x的雅可比矩阵可由链式求导法则

πx=πpiWpiWx


对于:πpiW
似然场是以图像形式储存,因此必须对piW按照某种分辨率进行采样,设piW到图像坐标pif的转换关系为
pif=αpiW+c
α 表示缩放系数,c表示距离图像中心的偏移量。

坐标系转换为:Robot 坐标系 --> World 坐标系 --> 似然场坐标系 那么

πpiW=πpifpifpiW=α[Δπx,Δπy]

其中πpif=[Δπx,Δπy]为似然场在图像上的梯度。


对于:piWx
这里x描述了世界坐标系 W 下机器人位姿。

x=[x,y,θ]T

我们设机器人坐标系下的某个扫描点piB的距离与角度为ri,ρi,那么根据当前激光的位姿,可以将它转换到世界坐标系下:

piW=[x+ricos(ρi+θ),y+risin(ρi+θ)]

那么

piWx=[1001risin(ρi+θ)ricos(ρi+θ)]TR2×3


从而得到最终的偏导公式

πx=πpiWpiWx=α[Δπx,Δπy][10Risin(ρi+θ)01Ricos(ρi+θ)]=[αΔπx,αΔπy,αΔπxRisin(ρi+θ)+αΔπy,Ricos(ρi+θ)]

在实际代码中,Δπx只需要对似然场上x轴相邻两个点坐标的值做差*0.5即可(如果为了更精确的梯度,可以多阶线性差值),Δπy同理。


最小二乘问题的高斯牛顿求解

高斯—牛顿迭代法的基本思想是使用泰勒级数展开式去近似地代替非线性回归模型,然后通过多次迭代,多次修正回归系数,使回归系数不断逼近非线性回归模型的最佳回归系数,最后使原模型的残差平方和达到最小。

对于一个非线性最小二乘问题:

x=argminx12||f(x)||2

高斯牛顿的思想是把f(x)利用泰勒展开,取一阶线性项近似。

f(x+Δx)=f(x)+f(x)Δx=f(x)+J(x)Δx

带入到上式,得,其中J(x)代表雅可比也就是一阶导:

12||f(x+Δx)||2=12(f(x)Tf(x)+2f(x)TJ(x)Δx+ΔxTJ(x)TJ(x)Δx)

对上式求导,令导数为0:

J(x)TJ(x)Δx=J(x)Tf(x)

H=JTJB=JTf

HΔx=B

求解,便可以获得调整增量Δx。这要求H可逆(正定),但实际情况并不一定满足这个条件,因此可能发散,另外步长Δx可能太大,也会导致发散,这个也就是config中的移动限制

posted @   DarkArc  阅读(293)  评论(0编辑  收藏  举报
阅读排行:
· 清华大学推出第四讲使用 DeepSeek + DeepResearch 让科研像聊天一样简单!
· 推荐几款开源且免费的 .NET MAUI 组件库
· 实操Deepseek接入个人知识库
· 易语言 —— 开山篇
· Trae初体验
点击右上角即可分享
微信分享提示