【技术实现步骤摘要】
本专利技术涉及视觉导航，特别涉及一种视觉惯性slam方法。
技术介绍
1、近年来，随着自动驾驶、机器人、元宇宙以及增强现实等应用的快速发展，同时定位和建图(simultaneous localization and mapping，slam)是其核心技术，并得到了工业界和学术界的广泛关注。slam是指搭载特定传感器的主体，在没有环境先验信息的情况下，于运动过程中建立环境的模型，同时估计自己的运动。slam的主要应用领域包括自主移动机器人、增强现实、自动驾驶、移动终端以及无人机系统等。slam的研究使用滤波器理论，一般将里程计的输入作为预测过程的输入，视觉传感器的输出作为更新过程的输入，卡尔曼滤波是最经典的滤波方法，但是卡尔曼滤波只能处理线性模型，现实生活中slam系统往往都是非线性问题，于是研究者将研究热点转移到扩展卡尔曼滤波(extended kalmanfilter，ekf)上，目前ekf是大部分slam采用的滤波算法。然而，ekf算法的缺点是线性化处理需要计算雅可比矩阵和海森矩阵，该步骤计算繁琐容易出错，且ekf的精度最多为二阶，而ukf算法不利用泰勒级数进行线性化近似且无须计算雅可比矩阵和海塞矩阵，而是借用一组sigma点通过无迹变换(unscented transformation，ut)来处理均值和协方差的非线性传递，其非线性近似精度超过二阶，当使用ukf近似处理非线性系统的均值和协方差时，在可实现性、收敛性和滤波精度上该算法都明显优于ekf算法。但是，以上均要求系统模型准确且噪声干扰为白噪声，当系统噪声出现不确定性
技术实现思路
1、针对现有技术中存在的不足，本专利技术提供了一种基于改进的自适应ukf的视觉惯性轮速计slam方法，解决外界噪声不确定的情况下，基于ukf的视觉惯性轮速计slam系统精度低和实时性不够的问题。
2、本专利技术的目的是这样实现的：一种基于改进的自适应ukf的视觉惯性轮速计slam方法，其特征在于，包括以下步骤：
3、步骤1)对机器人slam系统建立状态方程；
4、步骤2)建立视觉传感器和轮速计的量测模型；
5、步骤3)根据状态空间模型确立无迹变换采样点和权值；
6、步骤4)将采样点进行非线性函数传递，并更新系统状态预测和状态预测协方差矩阵；
7、步骤5)量测更新，并用改进的sage-husa算法对过程噪声和量测噪声协方差矩阵进行更新；
8、步骤6)引入收敛因子，更新状态估计、滤波增益和状态估计协方差矩阵。
9、作为本专利技术的进一步限定，步骤1)具体为：
10、对于一个装配了imu和视觉传感器的机器人slam系统，选取相机的姿态、速度、位置和imu的加速度和角速度偏差为系统状态；建立机器人起始位置为原点建立世界坐标系，以正东方向为世界坐标系的x轴的正方向，以正北方向为世界坐标系的y轴的正方向，根据右手准则确定世界坐标系的z轴正方向；建立以相机为中心的机体坐标系，以平行于相机横轴指向右方为机体坐标系x轴的正方向，以相机正前方为机体坐标系y轴的正方向，根据右手准则确定机体坐标系的z轴正方向；选取相机的姿态、速度、位置和imu的加速度和角速度偏差为系统状态，则系统的状态方程如下：
11、xk＝f(xk-1)+qk
12、其中，
13、qk为系统噪声，设定为高斯白噪声；
14、其中，[rk-1 vk-1 sk-1 bω,k-1 ba,k-1]t表示k-1时刻的系统状态，rk-1表示相机在k-1时刻的旋转矩阵，vk-1表示相机在k-1时刻的速度，sk-1表示相机在k-1时刻的位置，bω,k-1表示imu在k-1时刻的加速度误差，ba,k-1表示imu在k-1时刻的角速度误差，为系统各变量的高斯白噪声，wω为imu陀螺仪的高斯白噪声，wa为imu加速度计的高斯白噪声，为imu角速度偏差的高斯白噪声，为imu加速度偏差的高斯白噪声，[ωk-1 ak-1]t表示k-1时刻的角速度和加速度，gg为重力加速度，δt为采样时间间隔。
15、作为本专利技术的进一步限定，步骤2)具体包括：
16、步骤2-1)建立视觉传感器的量测模型，选取相机所测得像素坐标为测量值，针对相机给出的测量数据的量测方程如下：
17、zk,camera＝hcamera(xk-1)+rk,camera
18、其中，
19、hcamera(xk-1)＝ki3×4tcwpi′，rk,camera为相机的量测噪声，设定为高斯白噪声；
20、其中，k为相机的内参矩阵，i3×4为3×4的矩阵，且i3×4＝[i3×3 03×1]，i3×3为3×3的单位矩阵，03×1为3×1的列向量；tcw为从世界坐标系到相机坐标系的变换矩阵，且其中，rcw和tcw分别表示从世界坐标系到相机坐标系的旋转矩阵和平移向量；p′i为pi的齐次坐标，且pi为相机观测点世界坐标系下的三维坐标；
21、步骤2-2)建立轮速计的量测模型，选取轮速计所测量的车轮移动距离、转动的角度和运动半径作为测量值，针对轮速计给出的测量数据的量测方程如下：
22、zk,wheel＝hwheel(xk-1)+rk,wheel
23、其中，
24、
25、rk,wheel为轮速计的量测噪声，设定为高斯白噪声；
26、其中，[xk-1 yk-1 θk-1]t为k-1时刻轮速计的量测数据，xk-1表示机器人k-1时刻所在的位于x轴方向的位置，yk-1表示机器人k-1时刻所在的位于y轴方向的位置，θk-1表示机器人k-1时刻绕z轴旋转的角度；经过采样时间δk，δdk为车轮移动距离、δθk为转动的角度、rk为运动半径，且有如下关系：
27、
28、其中，δdl为左轮运行距离，δdr为右轮运行距离，p为两个车轮之间的距离，r为车轮半径，τ为编码器的参数，σl为在采样时间δk内左轮编码器产生的脉冲数，σr为在采样时间δk内右轮编码器产生的脉冲数。
29、作为本专利技术的进一步限定，步骤3)根据状态空间模型确立无迹变换采样点和权值，具体过程如下：
30、利用以下公式计算得到2n+1个sigma点χ以及各点对应的权值w；
31、
32、各sigma点的权值如下：
33、
34、其中，x的均值为方差为p的状态向量，χi表示sigma点，n表示状态向量的维数；λ＝α2(n+μ)-n为缩放比例参数，α可调节sigma采样点围绕的范围大小，μ是次缩放比例参数；β为状态分布参数；为矩阵(n+λ)p的平方根的第i列；下标m为均值；c为协方差；上标i为第几个采样点。
35、作为本专利技术的进一步限定，步骤4)将采样点进行非线性函数传递，并更新系统状态预测和状本文档来自技高网...
【技术保护点】
1.一种基于改进的自适应UKF的视觉惯性轮速计SLAM方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于改进的自适应UKF的视觉惯性轮速计SLAM方法，其特征在于，步骤1)具体为：
3.根据权利要求2所述的基于改进的自适应UKF的视觉惯性轮速计SLAM方法，其特征在于，步骤2)具体包括：
4.根据权利要求3所述的基于改进的自适应UKF的视觉惯性轮速计SLAM方法，其特征在于，步骤3)根据状态空间模型确立无迹变换采样点和权值，具体过程如下：
5.根据权利要求4所述的基于改进的自适应UKF的视觉惯性轮速计SLAM方法，其特征在于，步骤4)将采样点进行非线性函数传递，并更新系统状态预测和状态预测协方差矩阵，具体过程如下：
6.根据权利要求5所述的基于改进的自适应UKF的视觉惯性轮速计SLAM方法，其特征在于，步骤5)量测更新，并用改进的Sage-Husa算法对过程噪声和量测噪声协方差矩阵进行更新；具体过程如下：
7.根据权利要求1所述的基于改进的自适应UKF的视觉惯性轮速计SLAM方法，其特征在于，
...【技术特征摘要】
1.一种基于改进的自适应ukf的视觉惯性轮速计slam方法，其特征在于，包括以下步骤：
2.根据权利要求1所述的一种基于改进的自适应ukf的视觉惯性轮速计slam方法，其特征在于，步骤1)具体为：
3.根据权利要求2所述的基于改进的自适应ukf的视觉惯性轮速计slam方法，其特征在于，步骤2)具体包括：
4.根据权利要求3所述的基于改进的自适应ukf的视觉惯性轮速计slam方法，其特征在于，步骤3)根据状态空间模型确立无迹变换采样点和权值，具体过程如下：
5.根据权利要求4所述的基于改...
还没有人留言评论。发表了对其他浏览者有用的留言会获得科技券。