欢迎来到知嘟嘟! 联系电话:13336804447 卖家免费入驻,海量在线求购! 卖家免费入驻,海量在线求购!
知嘟嘟
我要发布
联系电话:13336804447
知嘟嘟经纪人
收藏
专利号: 2024114695870
申请人: 南京邮电大学
专利类型:发明专利
专利状态:已下证
专利领域: 测量;测试
更新日期:2026-11-16
缴费截止日期: 暂无
价格&联系人
年费信息
委托购买

摘要:

权利要求书:

1.一种基于流形处理的视觉惯性滤波方法,其特征是,包括:S1,基于惯性IMU传感器得到机器人的位姿信息,基于流形的IMU预积分方法对所述位姿信息进行积分操作得到所述位姿信息增量,基于所述位姿信息增量得到更新后的位姿信息;

S2,基于更新后的位姿信息和多个地标点位置信息进行李群构建,将状态空间嵌入到所述李群之中,得到机器人在当前时刻包含偏差信息的完整状态;

S3,基于所述李群和偏差信息,构建基于UKF的不变卡尔曼滤波的当前时刻的不确定性表示、状态模型以及测量模型;

S4,基于无偏输入和所述当前时刻的状态模型更新下一时刻的状态模型;基于当前状态传播的不确定性和过程噪声协方差矩阵更新下一时刻的状态协方差矩阵分解因子;

S5,若所述下一时刻存在地标的视觉观测数据,则基于所述测量模型计算出视觉观测值并结合所述视觉观测值进行该时刻的状态模型更新和状态协方差矩阵分解因子的更新,否则跳转至步骤S4;

S6,进入再下一个时刻后,重复进行步骤S4-S5。

2.根据权利要求1所述的视觉惯性滤波方法,其特征是,在步骤S1中,所述位姿信息包括旋转矩阵、速度向量和位置向量;所述基于流形的IMU预积分方法对所述位姿信息进行积分操作得到所述位姿信息增量,具体通过以下公式获得:,

式中,分别表示机器人从时刻i到时刻j的旋转增量、速度增量和位置增量,和分别表示机器人在时刻i到时刻j的旋转矩阵,和分别表示机器人在时刻i到时刻j的速度向量,和分别表示机器人在时刻i到时刻j的位置向量;表示从机器人在时刻i到时刻j的旋转噪声,即角度误差,为零均值高斯噪声;表示使用指数映射(exp),将角度误差从李代数映射回到李群,分别表示机器人从时刻i到时刻j的速度噪声和位置噪声,均为零均值高斯噪声,表示从时刻i到时刻j的时间间隔,表示重力向量。

3.根据权利要求1所述的视觉惯性滤波方法,其特征是,在步骤S1中,所述更新后的位姿信息包括更新后的旋转矩阵、速度向量和位置向量;

在步骤S2中,所述基于更新后的位姿信息和多个地标点位置信息进行李群构建包括通过更新后的旋转矩阵、速度向量、位置向量以及多个地标点的位置构建李群,所述李群表示当前时刻n机器人的状态向量,所述李群的表达式如下:,

式中,表示一个的零矩阵,表示一个的单位矩阵,m表示机器人的位置信息,p表示所观测到的地标点的数量;

在步骤S2中,所述得到机器人在当前时刻包含偏差信息的完整状态包括:机器人在当前时刻n包含偏差信息的完整状态表示为,其中是惯性IMU传感器的偏差向量,表示为:,

式中,是陀螺仪的偏差,是加速度计的偏差,两者组合为一个偏差向量。

4.根据权利要求1所述的视觉惯性滤波方法,其特征是,在步骤S3中,所述不确定性表示包括状态的不确定性表示和偏差的不确定性表示,所述状态的不确定性表示通过状态误差表示,所述偏差的不确定性表示通过偏差误差表示,所述状态误差和所述偏差误差服从均值为0、协方差为的高斯分布,表示为:,

状态协方差矩阵描述了状态误差和偏差误差的不确定性大小,状态协方差矩阵进行Cholesky分解能够得到Cholesky因子,分解过程如下:,式中,S为状态协方差矩阵进行Cholesky分解得到的Cholesky因子,所述Cholesky分解是一种将正定矩阵分解为下三角矩阵及其转置的分解方法。

5.根据权利要求1所述的视觉惯性滤波方法,其特征是,机器人在当前时刻n的状态向量的运算如下:,

式中,表示机器人在当前时刻n的状态向量,表示状态向量的状态均值,表示状态误差,表示状态误差通过指数映射exp从李代数映射回到李群,修正状态均值;

惯性IMU传感器在当前时刻n的偏差向量的运算如下:,

式中,表示惯性IMU传感器在当前时刻n的偏差向量,表示偏差向量的偏差均值,表示偏差误差;

在步骤S3中,所述状态模型f表达式为:

,

表示机器人在前一时刻n-1的状态向量,表示惯性IMU传感器在前一时刻n-1的偏差向量,表示所述状态模型f的输入向量,包含惯性IMU传感器测量的角速度和加速度,表示当前时刻所述状态模型f的实际输入,去除了惯性IMU传感器偏差的影响,表示过程噪声,具体为系统的随机噪声,服从高斯分布,表示为,其中为过程噪声的协方差矩阵。

6.根据权利要求1所述的视觉惯性滤波方法,其特征是,在步骤S3中,所述测量模型表达式为:,

式中,表示在当前时刻n通过视觉传感器观测得到的实际观测向量,表示测量模型,描述在当前时刻n状态向量和观测噪声的情况下,视觉传感器对地标的观测值;观测噪声服从高斯分布,表示观测噪声的协方差矩阵,测量模型进一步展开为:,

式中,包含了视觉传感器对所有个地标点的观测数据,p表示地标点数量,表示对第i个地标的观测数据,具体为摄像机坐标系中地标在图像平面上的归一化像素位置,i表示地标点序号。

7.根据权利要求5所述的视觉惯性滤波方法,其特征是,在步骤S4中,所述基于无偏输入和所述当前时刻的状态模型更新下一时刻的状态模型,具体包括:S4.1,设定当前时刻n状态模型的输入为状态均值,偏差均值,状态协方差矩阵的Cholesky分解因子,输入向量以及过程噪声的协方差矩阵;

其中,将前一时刻的偏差均值从输入向量中去除,得到当前时刻n的无偏输入向量,则;

将协方差矩阵的Cholesky分解因子与进行矩阵构建,得到为:,

式中,blkdiag表示构建一个块对角矩阵,表示对过程噪声协方差矩阵的Cholesky分解,将当前时刻n的状态均值保留,并令;

S4.2,更新状态均值和偏差均值;基于无偏输入向量和状态向量,通过所述状态模型来传播状态向量和偏差,得到更新后的状态均值和偏差均值:,

式中,和分别表示更新后的状态均值和偏差均值;

以更新后的状态均值作为机器人下一时刻的状态向量,以更新后的偏差均值作为惯性IMU传感器在下一时刻的偏差向量,对状态模型f进行更新。

8.根据权利要求7所述的视觉惯性滤波方法,其特征是,在步骤S4中,所述基于当前状态传播的不确定性和过程噪声协方差矩阵更新下一时刻的状态协方差矩阵分解因子,包括:S4.3,Sigma点是一组能够捕捉系统状态不确定性的特殊点,所述协方差矩阵能够生成2个Sigma点,Sigma点反应了状态的不确定性,Sigma点通过以下公式获得:,

式中,为缩放系数,用于调节Sigma点的范围,表示从矩阵中提取第列,用于生成Sigma点,分别表示生成的状态误差的Sigma点、偏差误差的Sigma点和过程噪声的Sigma点;

将生成的Sigma点通过状态模型进行传播得到传播后的Sigma点,运算过程如下:,

式中,分别表示传播后状态误差和偏差误差生成的Sigma点;

将传播后的Sigma点通过以下公式从李群映射回到李代数中,,

式中,表示Sigma点从李群映射回李代数,表示映射回李代数中的Sigma点,能够在局部线性空间中更新Cholesky因子;

S4.4,基于从李群映射回到李代数的Sigma点、传播后的Sigma点的信息以及过程噪声的协方差矩阵,通过QR分解更新Cholesky因子,得到更新后下一时刻的Cholesky因子,所述QR分解通过以下公式获得:,,

式中,qr函数表示QR分解;

以更新后的Cholesky因子对状态协方差矩阵分解因子进行更新。

9.根据权利要求8所述的视觉惯性滤波方法,其特征是,若所述下一时刻存在地标的视觉观测数据,则基于所述测量模型计算出视觉观测值并结合所述视觉观测值进行该时刻的状态模型更新和状态协方差矩阵分解因子的更新,包括:S5.1,首先根据测量模型,计算未加观测噪声的地标观测值:,

式中,为观测噪声,为步骤S4.2中更新的状态均值;

S5.2,基于步骤S4.4更新后的Cholesky因子,生成新的Sigma点:,

式中,分别表示生成的状态误差和偏差误差的Sigma点,再通过指数映射将李代数中的Sigma点,转换为李群中的Sigma点,具体通过以下公式获得:,

式中,表示Sigma点通过指数映射exp从李代数映射回到李群,表示映射回李群的Sigma点,将Sigma点代入测量模型,计算带有噪声的地标观测值的Sigma点:,

式中,为带有噪声的地标观测值的Sigma点,为观测噪声的Sigma点,由观测噪声的协方差矩阵生成;

S5.3,通过实际测量值和预测测量值的比较,计算出状态的校正值和偏差的校正值,同时更新Cholesky因子确保协方差矩阵的数值稳定性,所述QR分解通过以下公式获得:,,

式中,为实际观测值,由视觉传感器直接观测获得的值,为无噪声的预测测量值,由步骤S5.1计算得到,和分别为带有噪声的测量和状态误差的Sigma点,为观测噪声的协方差矩阵;

S5.4,使用状态的校正值和偏差的校正值更新状态均值,偏差均值:,

经过状态更新过程,得到更新后的状态均值、偏差均值以及更新后的状态协方差矩阵的Cholesky因子;

以更新后的状态均值作为机器人下一时刻的状态向量,以更新后的偏差均值作为惯性IMU传感器在下一时刻的偏差向量,对状态模型f再次进行更新;

以更新后的Cholesky因子对状态协方差矩阵分解因子进行更新。