[发明专利]一种手持式RTK接收机的姿态初始化方法、设备、存储介质

权利要求书

1.一种手持式RTK接收机的姿态初始化方法，其特征在于，本方法包括以下步骤：

步骤S1、选取一帧图像对应的位姿作为参考坐标系，基于视觉约束计算相机相对参考坐标系的位置，基于IMU测量模型计算预积分量；所述图像由所述相机输出；

步骤S2、根据所述相机相对参考坐标系的位置及所述预积分量，动态估计重力在参考坐标系下的方向与大小；

步骤S3、根据重力在导航坐标系和参考坐标系下的投影，确定调平矩阵并将VIO的位置投影至水平系；

步骤S4、选取初始化时间区间，计算所述初始化时间区间内的GNSS位置增量和VIO估计的位置增量；

步骤S5、根据初始化时间区间起、止时刻的GNSS位置增量与VIO估计的位置增量计算航向角初值，完成姿态初始化。

2.如权利要求1所述的一种手持式RTK接收机的姿态初始化方法，其特征在于，所述步骤S1中基于视觉约束计算相机相对参考坐标系的位置，具体为，

基于特征点建立重投影误差函数：

其中，是特征点在第i帧图像下的观测像素坐标，s_f是特征点的深度值，默认设为1；所选图像记为c₀帧，和分别是第i、j帧相机坐标系到参考坐标系c₀的欧式变换矩阵，其中R和t分别表示旋转矩阵和平移向量，表示维度为4x4的矩阵，表示维度为3x3的矩阵，表示维度为3x1的向量；

是在第i帧图像观测到的特征点经过位姿变换到第j帧图像上的投影，为预测值；

根据特征点在第j帧图像上的观测像素坐标与预测值计算重投影残差r_c

是特征点在第j帧图像上的观测像素坐标，与预测值相减得到重投影残差r_c；

基于图像帧之间的多对特征点对，通过光束平差法解算第i帧图像与第j帧图像的欧式变换矩阵通过连续跟踪特征点对，解算出相机在参考坐标系下的位置和姿态。

3.如权利要求1所述的一种手持式RTK接收机的姿态初始化方法，其特征在于，所述步骤S1中，基于IMU测量模型计算预积分量，具体为，

从第i帧图像时刻的位置速度以及姿态基于IMU的测量得到第j帧图像时刻的位置速度以及姿态将与参考坐标系相关的b系相对于c₀系的方向余弦矩阵从积分项中提出来，得到：

为参考坐标系下的重力，分别为在第i帧图像时刻载体坐标系的加速度与角速度，为b_t系相对于b_i系的方向余弦矩阵，为b_t系相对于b_i系的单位四元数；

将积分量单独记为即

4.如权利要求3所述的一种手持式RTK接收机的姿态初始化方法，其特征在于，所述步骤S2中，根据所述相机相对参考坐标系的位置及所述预积分量，动态估计重力在参考坐标系下的方向与大小，求解以下最小二乘问题，具体为，

其中，

为待估计变量，为c₀系相对于b_k系的方向余弦矩阵，为b_k+1系相对于c₀系的方向余弦矩阵，是b系下相机与IMU的臂杆值，s表示相机估计轨迹与真实轨迹之间的尺度因子，和是第k和k+1时刻根据视觉观测估计的载体位置，和是第k和k+1时刻之间IMU观测积分得到的位置、速度增量。

5.如权利要求1所述的一种手持式RTK接收机的姿态初始化方法，其特征在于，所述步骤S3中，对齐重力在导航坐标系与参考坐标系下投影的两个向量，具体为，

其中，u和θ是一组旋转向量，u是单位旋转轴，θ是单位旋转轴对应的旋转角；gⁿ＝[0,0,-9.81]^T为重力在导航坐标系下的投影，是重力在参考坐标系下的投影；

所述调平矩阵为