[发明专利]一种基于深度确定性策略梯度的视觉惯性自校准方法

权利要求书

1.一种基于深度确定性策略梯度的视觉惯性自校准方法，其特征在于：包括以下步骤：

步骤一，建立相机和IMU测量误差模型，利用非线性优化算法求解相机-IMU参数；

步骤一的具体方法如下：

S1.1：建立相机误差模型，由标准的相机针孔模型，世界坐标系下三维空间点到相机二维像素平面点z_k,m＝(u,v)^T转换关系为：

其中，Z_c为比例因子，表示相机光心到图像平面的有效距离，R_cw为相机到世界坐标系的外参旋转变换矩阵，为相机到世界坐标系的平移变换矩阵，为相机内参矩阵，f＝(f_u,f_v)为x轴和y轴上的归一化焦距，C＝(u₀,v₀)为像素平面的中心，在考虑图像畸变和图像白噪声后，参考点投影完整的相机针孔模型定义为

其中，f_p(·)为相机透视投影函数，t_k为IMU数据采样时间，t_d为IMU和相机之间的时间偏移，为相机模型待标定的参数，T_IW为IMU到世界坐标系的变换矩阵，为相机到IMU的外参变换矩阵，由相机到IMU的旋转变换矩阵R_CI和平移变换矩阵p_CI构成，为零均值和标准差σ_C的高斯白噪声过程；

S1.2：建立IMU误差模型，建立陀螺仪和加速度计的误差模型为：

其中，分别为陀螺仪和加速度计误差补偿后的旋转角速率和加速度，ω_WI(t),a^W(t)分别为世界坐标系下实际测量的旋转角速率和加速度，S_ω,M_ω分别是陀螺仪的尺度误差和非正交误差，S_a,M_a分别是加速度计的尺度误差和非正交误差，T_g为重力敏感度即陀螺仪对加速度的敏感程度，分别为陀螺仪和加速度计的随机游走误差，分别为陀螺仪和加速度计的零均值高斯白噪声；

S1.3：非线性优化，参数标定估计器转化为非线性最小二乘优化问题，相机和IMU测量误差模型描述为X(t)＝f(Θ,t)+η形式，Θ是包含所有估计参数的矢量，t表示测量记录和模型评估的瞬间，η为零均值和方差σ²I的高斯白噪声过程，传感器i在[t₁,…,t_N]时刻的实际测量值为其参数标定目标函数表述为：

IMU的偏置误差项为偏置项的目标函数定义为

标定参数估计值由使得J最小的Θ确定，采用Levenberg-Marquardt算法进行非线性优化；

步骤二，计算Fisher信息矩阵，并分析视觉惯性标定系统可观测性；

步骤二的具体方法如下：

非线性优化根据初始估计值并通过迭代优化直到收敛，Levenberg-Marquardt算法需要误差项的雅可比矩阵之和G，迭代更新如式(7)所示

其中，T为误差协方差矩阵，为当前估计的误差值，G^TT^-1G可表示Fisher信息矩阵Fisher Information Matrix,FIM和估计协方差矩阵的逆，令T^-1＝L^-1L为误差协方差矩阵的Cholesky分解，式(7)可重新定义为

采用QR分解；

LGΠ＝Q_nR_n (9)

其中Π为置换矩阵，Q_n是列正交的m×n矩阵，R_n是n×n上三角矩阵，式(8)转化为

在式(10)中R_n矩阵用来计算FIM和估计协方差的逆，并进一步计算归一化协方差视觉惯性标定参数空间的信息增益通过以下度量来评估；

使得H指标最小化的参数即为最大化信息增益，用于后续强化学习的修正反馈设计；

步骤三，将标定过程建模为马尔可夫决策过程；

步骤三的具体方法如下：

马尔可夫决策过程由每个时刻t的状态S_t∈S,动作A_t∈A，状态转移函数S_t+1＝f(S_t,A_t)和奖励函数R_t(S_t,A_t)定义，以视觉惯性标定系统为智能体，通过连续的动作来寻找视觉/IMU可观测的校准动作空间；

S3.1动作，为了得到视觉惯性传感器参数标定的可观测动作空间，使传感器平台进行相应的平移和旋转动作，其中，平移运动分为X轴，Y轴，Z轴运动，并根据移动的速度分为慢速和快速两种；旋转运动分为绕X轴，绕Y轴，绕Z轴运动，并根据旋转速度和旋转方向分为慢速、快速、顺时针、逆时针四种情况，定义的动作空间，一共包含20个基本动作库，停止动作是在完成所有参数的可观测校准空间后执行，强化学习框架从中选择建议的运动动作，由这些动作序列连续起来形成最终校准轨迹；

S3.2状态，视觉惯性标定过程的状态S_t定义为标定系统的各个参数Θ，并且参数会随着视觉惯性传感器随载体的运动进行更新；

S3.3状态转移函数，当智能体选择一个动作A_t时，状态转移函数S_t+1＝f(S_t,A_t)描述从当前状态S_t将转移到S_t+1的概率模型，当视觉惯性组件随载体执行正确的动作时，使得校准参数Θ被观察到；

S3.4奖励函数，奖励被视为采取行动后的反馈，在强化学习训练过程中，如果选择的动作能够将状态转移到更好的状态，则智能体将获得正的奖励；否则，将返回零奖励，奖励函数R_t(S_t,A_t)定义如下；

其中，为待标定参数的可观测协方差，ε是使得校准参数空间可观测的隐式编码界限值，只有当标定参数协方差小于界限值，并能够生成可观测的校准轨迹时，智能体才能获得正的修正反馈；

步骤四，设计基于深度确定性策略梯度的强化学习标定算法，从惯性数据集筛选出可观测子片段进行参数标定；

在深度确定性策略梯度Deep Deterministic Policy Gradient,DDPG，使用卷积神经网络来模拟Q-learning函数，Q网络的参数是Θ^Q，使用目标函数J_β(μ)衡量一个策略μ的性能，定义如下；

其中，S是智能体的状态，根据智能体的行为策略，生成相应的状态路径，其概率分布函数Probability Distribution Function,PDF为p^β，Q^μ(S,μ(S))是策略μ和状态采样空间的分值函数，当前Q网络负责对当前状态S使用∈-贪婪法选择动作A，获得新状态S′和奖励R，将样本放入经验回放池，对经验回放池中采样的下一状态S″使用贪婪法选择动作A′，计算目标Q网络的Q值，当目标Q网络计算出目标Q值后，当前Q网络会进行网络参数的更新，并定期把最新网络参数复制到目标Q网络；

在基于深度确定性策略梯度的视觉惯性自校准算法中，DDPG创建两个神经网络，分别为在线网络即策略网络和目标网络即Q网络，将动作模型和奖励模型参数化为神经网络，每个模型包含编码状态和目前精简的信息，输入为动作历史序列A_0:t和校准状态序列S_0:t，全连接网络来预测当前的奖励R_t(S_0:t,A_0:t)；

Q网络和策略网络的训练过程如下；

训练一个小批量数据后，通过随机梯度下降Random Gradient Descent，SGD算法更新在线网络的参数，然后通过软更新算法更新目标网络的参数，软更新是一种运行平均算法；

其中，τ为0.001。