CN113340324B - 一种基于深度确定性策略梯度的视觉惯性自校准方法 - Google Patents
一种基于深度确定性策略梯度的视觉惯性自校准方法 Download PDFInfo
- Publication number
- CN113340324B CN113340324B CN202110583354.3A CN202110583354A CN113340324B CN 113340324 B CN113340324 B CN 113340324B CN 202110583354 A CN202110583354 A CN 202110583354A CN 113340324 B CN113340324 B CN 113340324B
- Authority
- CN
- China
- Prior art keywords
- calibration
- camera
- network
- action
- state
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
- 238000000034 method Methods 0.000 title claims abstract description 64
- 230000000007 visual effect Effects 0.000 title claims abstract description 62
- 230000009471 action Effects 0.000 claims abstract description 54
- 239000011159 matrix material Substances 0.000 claims abstract description 45
- 230000033001 locomotion Effects 0.000 claims abstract description 43
- 230000006870 function Effects 0.000 claims abstract description 36
- 230000008569 process Effects 0.000 claims abstract description 34
- 238000004422 calculation algorithm Methods 0.000 claims abstract description 29
- 238000005457 optimization Methods 0.000 claims abstract description 14
- 238000012546 transfer Methods 0.000 claims abstract description 14
- 230000002787 reinforcement Effects 0.000 claims abstract description 11
- 238000005259 measurement Methods 0.000 claims abstract description 9
- 238000012216 screening Methods 0.000 claims abstract description 3
- 239000003795 chemical substances by application Substances 0.000 claims description 18
- 230000009466 transformation Effects 0.000 claims description 14
- 238000013519 translation Methods 0.000 claims description 11
- 230000001133 acceleration Effects 0.000 claims description 9
- 238000012897 Levenberg–Marquardt algorithm Methods 0.000 claims description 6
- 238000013528 artificial neural network Methods 0.000 claims description 6
- 238000000354 decomposition reaction Methods 0.000 claims description 6
- 238000005070 sampling Methods 0.000 claims description 6
- 238000012549 training Methods 0.000 claims description 6
- 238000006243 chemical reaction Methods 0.000 claims description 4
- 238000012937 correction Methods 0.000 claims description 4
- 230000006399 behavior Effects 0.000 claims description 3
- 238000013527 convolutional neural network Methods 0.000 claims description 3
- 238000013461 design Methods 0.000 claims description 3
- 238000005315 distribution function Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 3
- 230000003287 optical effect Effects 0.000 claims description 3
- 238000005295 random walk Methods 0.000 claims description 3
- 230000035945 sensitivity Effects 0.000 claims description 3
- NAWXUBYGYWOOIX-SFHVURJKSA-N (2s)-2-[[4-[2-(2,4-diaminoquinazolin-6-yl)ethyl]benzoyl]amino]-4-methylidenepentanedioic acid Chemical compound C1=CC2=NC(N)=NC(N)=C2C=C1CCC1=CC=C(C(=O)N[C@@H](CC(=C)C(O)=O)C(O)=O)C=C1 NAWXUBYGYWOOIX-SFHVURJKSA-N 0.000 claims description 2
- 230000005484 gravity Effects 0.000 claims description 2
- 239000000463 material Substances 0.000 claims 1
- 238000004364 calculation method Methods 0.000 description 5
- 230000006872 improvement Effects 0.000 description 4
- 230000007704 transition Effects 0.000 description 4
- 230000007547 defect Effects 0.000 description 1
- 230000001419 dependent effect Effects 0.000 description 1
- 238000010586 diagram Methods 0.000 description 1
- 238000001914 filtration Methods 0.000 description 1
- 238000009434 installation Methods 0.000 description 1
- 238000013507 mapping Methods 0.000 description 1
- 238000012986 modification Methods 0.000 description 1
- 230000004048 modification Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 230000001360 synchronised effect Effects 0.000 description 1
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
- G01C25/005—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass initial alignment, calibration or starting-up of inertial devices
Landscapes
- Engineering & Computer Science (AREA)
- Manufacturing & Machinery (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Image Analysis (AREA)
Abstract
一种基于深度确定性策略梯度的视觉惯性自校准方法，具体包括以下步骤：(1)建立相机和IMU测量误差模型，利用非线性优化算法求解相机‑IMU参数；(2)计算Fisher信息矩阵，并分析视觉惯性标定系统可观测性；(3)将标定过程建模为马尔可夫决策过程，明确智能体的状态空间、动作空间、状态转移函数和奖励函数；(4)设计基于深度确定性策略梯度的强化学习标定算法，确定不可观测的离散运动序列，从校准数据集筛选出可观测子序列用于参数标定。该方法能有效简化标定过程和降低计算量，无需专业的人工标定和精准的初始化参数，可以智能化选择完全可观测的动作序列进行参数标定。
Description
技术领域
本发明涉及传感器技术领域，特别是涉及一种基于深度确定性策略梯度的视觉惯性自校准方法。
背景技术
精确的相机-IMU校准参数对视觉惯性同步定位与地图构建系统至关重要，不准确校准参数将降低系统性能，甚至定位失效。在智能体视觉惯性导航系统长时间运行的情况下，需要对视觉惯性自校准，补偿随着时间的推移而造成的校准误差，如传感器碰撞、重新安装和IMU温漂等。自校准在智能体导航系统中引起了极大的研究热潮。
视觉惯性传感器标定一般采用特定的离线例程或者依靠充分激励运动的自校准算法。现有智能体视觉惯性传感器校准存在以下不足：1.离线校准算法繁琐且耗时，需要专业技术人员反复移动标定板，并且在相机-IMU传感器碰撞或重新安装后需要重复标定。2.基于非线性滤波的视觉惯性在线标定算法高度依赖于准确的初始化参数，在标定同等数量参数时，在线自校准需要更为复杂的初始化过程，计算成本更高。3.使得相机-IMU标定参数空间完全可观测的校准数据集的收集并非易事。
针对离线标定过程复杂、在线标定计算量大的问题，提出一种基于深度确定性策略梯度(Deep Deterministic Policy Gradient，DDPG)的智能视觉惯性标定方法。利用Fisher信息矩阵分析视觉惯性标定系统可观测性，将标定过程建模为马尔可夫决策过程，设计基于深度确定性策略梯度的强化学习标定算法，确定不可观测的离散运动序列，从校准数据集筛选出可观测子序列用于参数标定。该方法能有效简化标定过程和降低计算量，无需专业的人工标定和精准的初始化参数，可以智能化选择完全可观测的动作子序列进行参数标定，为视觉惯性导航系统长时间自主运行提供高精度的标定参数。
发明内容
本发明针对视觉惯性离线标定过程复杂、在线标定计算量大的问题，本发明提供一种基于深度确定性策略梯度的视觉惯性自校准方法，该方法能有效简化标定过程和降低计算量，无需专业的人工标定和精准的初始化参数，可以智能化选择完全可观测的动作序列进行参数标定。
本发明提供一种基于深度确定性策略梯度的视觉惯性自校准方法，其特征在于：包括以下步骤：
步骤一，建立相机和IMU测量误差模型，利用非线性优化算法求解相机-IMU参数；
步骤二，计算Fisher信息矩阵，并分析视觉惯性标定系统可观测性；
步骤三，将标定过程建模为马尔可夫决策过程；
步骤四，设计基于深度确定性策略梯度的强化学习标定算法，从惯性数据集筛选出可观测子片段进行参数标定。
作为本发明进一步改进，步骤一的具体方法如下：
S1.1：建立相机误差模型，由标准的相机针孔模型，世界坐标系下三维空间点
到相机二维像素平面点zk，m＝(u，v)T转换关系为：
其中，Zc为比例因子，表示相机光心到图像平面的有效距离，在考虑图像畸变和图像白噪声后，参考点投影完整的相机针孔模型定义为
其中，fp(·)为相机透视投影函数，tk为IMU数据采样时间，td为IMU和相机之间的时间偏移，
为相机模型待标定的参数，TIW为IMU到世界坐标系的变换矩阵，
相机到IMU的外参变换矩阵由相机到IMU的旋转变换矩阵RCI和平移变换矩阵pCI构成，相机内参矩阵
中f＝(fu，fv)为x轴和y轴上的归一化焦距，C＝(cu，cv)为像素平面的中心，
为零均值和标准差σC的高斯白噪声过程；
S1.2：建立IMU误差模型，建立陀螺仪和加速度计的误差模型为：
其中，
分别为陀螺仪和加速度计误差补偿后的旋转角速率和加速度，ωWI(t)，aW(t)分别为世界坐标系下实际测量的旋转角速率和加速度，Sω，Mω分别是陀螺仪的尺度误差和非正交误差，Sa，Ma分别是加速度计的尺度误差和非正交误差，Tg为重力敏感度即陀螺仪对加速度的敏感程度，
分别为陀螺仪和加速度计的随机游走误差，
分别为陀螺仪和加速度计的零均值高斯白噪声；
S1.3：非线性优化，参数标定估计器转化为非线性最小二乘优化问题，相机和IMU测量误差模型描述为X(t)＝f(Θ，t)+η形式，Θ是包含所有估计参数的矢量，t表示测量记录和模型评估的瞬间，η为零均值和方差σ2I的高斯白噪声过程，传感器i在[t1，…，tN]时刻的实际测量值为
其参数标定目标函数表述为：
IMU的偏置误差项为
偏置项的目标函数定义为
标定参数估计值由使得J最小的Θ确定，采用Levenberg-Marquardt算法进行非线性优化；
作为本发明进一步改进，步骤二的具体方法如下：
非线性优化根据初始估计值
并通过迭代优化
直到收敛，Levenberg-Marquardt算法需要误差项的雅可比矩阵之和G，迭代更新如式(7)所示
其中，T为误差协方差矩阵，
为当前估计
的误差值。GTT-1G可表示Fisher信息矩阵Fisher Information Matrix，FIM和估计协方差矩阵
的逆。令T-1＝L-1L为误差协方差矩阵的Cholesky分解，式(7)可重新定义为
采用QR分解；
LGH＝QnRn (9)
其中Π为置换矩阵，Qn是列正交的m×n矩阵，Rn是n×n上三角矩阵，式(8)转化为
在式(10)中Rn矩阵用来计算FIM和估计协方差
的逆，并进一步计算归一化协方差
视觉惯性标定参数空间的信息增益通过以下度量来评估；
使得H指标最小化的参数即为最大化信息增益，用于后续强化学习的修正反馈设计。
作为本发明进一步改进，步骤三的具体方法如下：
马尔可夫决策过程由每个时刻t的状态St∈S，动作At∈A，状态转移函数St+1＝f(St，At)和奖励函数Rt(St，At)定义，以视觉惯性标定系统为智能体，通过连续的动作来寻找视觉/IMU可观测的校准动作空间；
S3.1动作，为了得到视觉惯性传感器参数标定的可观测动作空间，使传感器平台进行相应的平移和旋转动作，其中，平移运动分为X轴，Y轴，Z轴运动，并根据移动的速度分为慢速和快速两种；旋转运动分为绕X轴，绕Y轴，绕Z轴运动，并根据旋转速度和旋转方向分为慢速、快速、顺时针、逆时针四种情况，定义的动作空间，一共包含20个基本动作库，停止动作是在完成所有参数的可观测校准空间后执行，强化学习框架从中选择建议的运动动作，由这些动作序列连续起来形成最终校准轨迹；
S3.2状态，视觉惯性标定过程的状态St定义为标定系统的各个参数Θ，并且参数会随着视觉惯性传感器随载体的运动进行更新；
S3.3状态转移函数，当智能体选择一个动作At时，状态转移函数St+1＝f(St，At)描述从当前状态St将转移到St+1的概率模型，当视觉惯性组件随载体执行正确的动作时，使得校准参数Θ被观察到；
S3.4奖励函数，奖励被视为采取行动后的反馈，在强化学习训练过程中，如果选择的动作能够将状态转移到更好的状态，则智能体将获得正的奖励；否则，将返回零奖励，奖励函数Rt(St，At)定义如下；
其中，
为待标定参数的可观测协方差，ε是使得校准参数空间可观测的隐式编码界限值，只有当标定参数协方差小于界限值，并能够生成可观测的校准轨迹时，智能体才能获得正的修正反馈。
作为本发明进一步改进，步骤四的具体方法如下：
在深度确定性策略梯度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网络，将动作模型和奖励模型参数化为神经网络，每个模型包含编码状态和目前精简的信息，输入为动作历史序列A0：t和校准状态序列S0：t，全连接网络来预测当前的奖励Rt(S0：t，A0：t)；
Q网络和策略网络的训练过程如下；
训练一个小批量数据后，通过随机梯度下降Random Gradient Descent，SGD算法更新在线网络的参数，然后通过软更新算法更新目标网络的参数，软更新是一种运行平均算法；
其中，τ为0.001。
本发明具有的优点在于：
1、本发明提供一种基于深度确定性策略梯度模型的视觉惯性自校准算法，能够有效解决视觉惯性离线标定过程复杂、在线标定计算量大的问题。
2、本发明提供一种基于深度确定性策略梯度模型的视觉惯性自校准标定算法，分析视觉惯性标定系统的可观测性，确定不可观测的离散运动序列，采用深度确定性策略梯度智能选择可观测校准动作子序列进行参数标定。该方法能有效简化标定过程和降低计算量，无需专业的人工标定和精准的初始化参数，可以智能化选择完全可观测的动作序列进行参数标定，为视觉惯性导航系统长时间自主运行提供高精度的标定参数。
附图说明
图1是视觉惯性标定马尔可夫决策过程的动作空间示意图；
图2是视觉惯性标定马尔可夫决策过程的状态转移示意图；
图3是基于深度确定性策略梯度模型的视觉惯性自校准算法流程图。
具体实施方式
下面结合附图与具体实施方式对本发明作进一步详细描述：
本发明提供一种基于深度确定性策略梯度模型的视觉惯性自校准算法，分析视觉惯性标定系统的可观测性，采用深度确定性策略梯度智能选择可观测校准动作子序列进行标定，为机器人视觉惯性导航系统长时间自主运行提供高精度的标定参数，有效解决视觉惯性离线标定过程复杂、在线标定计算量大的问题。
本申请基于深度确定性策略梯度模型的视觉惯性自校准算法流程图如图3所示，具体步骤如下：
步骤(1)所述的视觉惯性标定方法如下：
S1.1：建立相机误差模型。由标准的相机针孔模型，世界坐标系下三维空间点
到相机二维像素平面点zk，m＝(u，v)T转换关系为：
其中，Zc为比例因子，表示相机光心到图像平面的有效距离。在考虑图像畸变和图像白噪声后，参考点投影完整相机针孔模型定义为
其中，fp(·)为相机透视投影函数，tk为IMU数据采样时间，td为IMU和相机之间的时间偏移，
为相机模型待标定的参数，TIW为IMU到世界坐标系的变换矩阵，
相机到IMU的外参变换矩阵由相机到IMU的旋转变换矩阵RCI和平移变换矩阵PCI构成。相机内参矩阵
中f＝(fu，fv)为x轴和y轴上的归一化焦距，C＝(cu，cv)为像素平面的中心。
为零均值和标准差σC的高斯白噪声过程。
S1.2：建立IMU误差模型。建立陀螺仪和加速度计的误差模型为：
其中，
分别为陀螺仪和加速度计误差补偿后的旋转角速率和加速度，ωWI(t)，aW(t)分别为世界坐标系下实际测量的旋转角速率和加速度，Sω，Mω分别是陀螺仪的尺度误差和非正交误差，Sa，Ma分别是加速度计的尺度误差和非正交误差。Tg为重力敏感度(陀螺仪对加速度的敏感程度)，
分别为陀螺仪和加速度计的随机游走误差，
分别为陀螺仪和加速度计的零均值高斯白噪声；
S1.3：非线性优化。参数标定估计器转化为非线性最小二乘优化问题，相机和IMU测量误差模型描述为X(t)＝f(Θ，t)+η形式，Θ是包含所有估计参数的矢量，t表示测量记录和模型评估的瞬间，η为零均值和方差σ2I的高斯白噪声过程，传感器i在[t1，…，tN]时刻的实际测量值为
其参数标定目标函数可以表述为：
IMU的偏置误差项为
偏置项的目标函数定义为
标定参数估计值由使得J最小的Θ确定，可采用Levenberg-Marquardt算法进行非线性优化。
步骤(2)所述的视觉惯性标定的可观测性分析，其具体方法如下：
非线性优化根据初始估计值
并通过迭代优化
直到收敛。Levenberg-Marquardt算法需要误差项的雅可比矩阵之和G，迭代更新如式(7)所示
其中，T为误差协方差矩阵，
为当前估计
的误差值。GTT-1G可表示Fisher信息矩阵(Fisher Information Matrix，FIM)和估计协方差矩阵
的逆。令T-1＝L-1L为误差协方差矩阵的Cholesky分解，式(7)可重新定义为
采用QR分解。
LGH＝QnRn (9)
其中Π为置换矩阵，Qn是列正交的m×n矩阵，Rn是n×n上三角矩阵，式(8)可转化为
在式(8)中Rn矩阵用来计算FIM和估计协方差
的逆，并进一步计算归一化协方差
视觉惯性标定参数空间的信息增益可以通过以下度量来评估。
使得H指标最小化的参数即为最大化信息增益，可以用于接下来强化学习的奖励设计。
步骤(3)所述的马尔可夫决策过程，其具体方法如下：
马尔可夫决策过程由每个时刻t的状态St∈S，动作At∈A，状态转移函数St+1＝f(St，At)和奖励函数Rt(St，At)定义。以视觉惯性标定系统为智能体，通过连续的动作来寻找视觉/IMU可观测的校准动作空间。
S3.1动作。为了得到视觉惯性传感器参数标定的可观测动作空间，可以使传感器平台进行相应的平移和旋转动作。其中，平移运动为{X轴，Y轴，Z轴}，并根据移动的速度分为慢速和快速两种；旋转运动为{绕X轴，绕Y轴，绕Z轴}，并根据旋转速度和旋转方向分为慢速、快速、顺时针、逆时针四种情况。我们定义的动作空间如图1所示，一共包含20个基本动作库，停止动作是在完成所有参数的可观测校准空间后执行。强化学习框架从中选择建议的运动动作，可以是平移和旋转运动的组合(如绕X轴的右螺旋运动)，由这些动作序列连续起来形成最终校准轨迹。
S3.2状态。视觉惯性标定过程的状态St可定义为标定系统的各个参数Θ，并且参数会随着视觉惯性传感器随载体的运动进行更新。
S3.3状态转移函数。当智能体选择一个动作At时，状态转移函数St+1＝f(St，At)描述从当前状态St将转移到St+1的概率模型。当视觉惯性组件随载体执行正确的动作时，使得校准参数Θ可以被观察到。如图2所示，圆圈对应可能的随机动作At(t＝1，2，…)，+R表示对这动作转换的奖励，正方形框表示待标定的参数fx，fy等，P为每个动作到某个状态的转移概率。
S3.4奖励函数。奖励可以被视为采取行动后的反馈。在强化学习训练过程中，如果选择的动作能够将状态转移到更好的状态，则智能体将获得正的奖励；否则，将返回零奖励。本文奖励函数Rt(St，At)定义如下。
其中，
为待标定参数的可观测协方差，ε是使得校准参数空间可观测的隐式编码界限值，只有当标定参数协方差小于界限值，并能够生成可观测的校准轨迹时，智能体才能获得正的奖励。
步骤(4)所述的基于深度确定性策略梯度的视觉惯性自校准算法，其具体方法如下：
在深度确定性策略梯度(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网络)。将动作模型和奖励模型参数化为神经网络，每个模型包含编码状态和目前精简的信息，输入为动作历史序列A0：t和校准状态序列S0：t，全连接网络来预测当前的奖励Rt(S0：t，A0：t)。
Q网络和策略网络的训练过程如下。
训练一个小批量数据后，通过随机梯度下降(Random Gradient Descent，SGD)算法更新在线网络的参数，然后通过软更新算法更新目标网络的参数。
软更新是一种运行平均算法。
其中，τ为0.001。
本发明公开了一种适用于机器人、无人机等智能体的基于深度确定性策略梯度模型的视觉惯性自校准算法，分析视觉惯性标定系统的可观测性，采用深度确定性策略梯度智能选择可观测校准动作子序列进行标定，为机器人视觉惯性导航系统长时间自主运行提供高精度的标定参数。该方法能有效简化标定过程和降低计算量，无需专业的人工标定和精准的初始化参数，可以智能化选择完全可观测的动作序列进行参数标定。
以上所述，仅是本发明的较佳实施例之一，并非是对本发明作任何其他形式的限制，而依据本发明的技术实质所作的任何修改或等同变化，仍属于本发明所要求保护的范围。
Claims (1)
1.一种基于深度确定性策略梯度的视觉惯性自校准方法，其特征在于：包括以下步骤：
步骤一，建立相机和IMU测量误差模型，利用非线性优化算法求解相机-IMU参数；
步骤一的具体方法如下：
S1.1：建立相机误差模型，由标准的相机针孔模型，世界坐标系下三维空间点
到相机二维像素平面点zk,m＝(u,v)T转换关系为：
其中，Zc为比例因子，表示相机光心到图像平面的有效距离，Rcw为相机到世界坐标系的外参旋转变换矩阵，
为相机到世界坐标系的平移变换矩阵，
为相机内参矩阵，f＝(fu,fv)为x轴和y轴上的归一化焦距，C＝(u0,v0)为像素平面的中心，在考虑图像畸变和图像白噪声后，参考点投影完整的相机针孔模型定义为
其中，fp(·)为相机透视投影函数，tk为IMU数据采样时间，td为IMU和相机之间的时间偏移，
为相机模型待标定的参数，TIW为IMU到世界坐标系的变换矩阵，
为相机到IMU的外参变换矩阵，由相机到IMU的旋转变换矩阵RCI和平移变换矩阵pCI构成，
为零均值和标准差σC的高斯白噪声过程；
S1.2：建立IMU误差模型，建立陀螺仪和加速度计的误差模型为：
其中，
分别为陀螺仪和加速度计误差补偿后的旋转角速率和加速度，ωWI(t),aW(t)分别为世界坐标系下实际测量的旋转角速率和加速度，Sω,Mω分别是陀螺仪的尺度误差和非正交误差，Sa,Ma分别是加速度计的尺度误差和非正交误差，Tg为重力敏感度即陀螺仪对加速度的敏感程度，
分别为陀螺仪和加速度计的随机游走误差，
分别为陀螺仪和加速度计的零均值高斯白噪声；
S1.3：非线性优化，参数标定估计器转化为非线性最小二乘优化问题，相机和IMU测量误差模型描述为X(t)＝f(Θ,t)+η形式，Θ是包含所有估计参数的矢量，t表示测量记录和模型评估的瞬间，η为零均值和方差σ2I的高斯白噪声过程，传感器i在[t1,…,tN]时刻的实际测量值为
其参数标定目标函数表述为：
IMU的偏置误差项为
偏置项的目标函数定义为
标定参数估计值由使得J最小的Θ确定，采用Levenberg-Marquardt算法进行非线性优化；
步骤二，计算Fisher信息矩阵，并分析视觉惯性标定系统可观测性；
步骤二的具体方法如下：
非线性优化根据初始估计值
并通过迭代优化
直到收敛，Levenberg-Marquardt算法需要误差项的雅可比矩阵之和G，迭代更新如式(7)所示
其中，T为误差协方差矩阵，
为当前估计
的误差值，GTT-1G可表示Fisher信息矩阵Fisher Information Matrix,FIM和估计协方差矩阵
的逆，令T-1＝L-1L为误差协方差矩阵的Cholesky分解，式(7)可重新定义为
采用QR分解；
LGΠ＝QnRn (9)
其中Π为置换矩阵，Qn是列正交的m×n矩阵，Rn是n×n上三角矩阵，式(8)转化为
在式(10)中Rn矩阵用来计算FIM和估计协方差
的逆，并进一步计算归一化协方差
视觉惯性标定参数空间的信息增益通过以下度量来评估；
使得H指标最小化的参数即为最大化信息增益，用于后续强化学习的修正反馈设计；
步骤三，将标定过程建模为马尔可夫决策过程；
步骤三的具体方法如下：
马尔可夫决策过程由每个时刻t的状态St∈S,动作At∈A，状态转移函数St+1＝f(St,At)和奖励函数Rt(St,At)定义，以视觉惯性标定系统为智能体，通过连续的动作来寻找视觉/IMU可观测的校准动作空间；
S3.1动作，为了得到视觉惯性传感器参数标定的可观测动作空间，使传感器平台进行相应的平移和旋转动作，其中，平移运动分为X轴，Y轴，Z轴运动，并根据移动的速度分为慢速和快速两种；旋转运动分为绕X轴，绕Y轴，绕Z轴运动，并根据旋转速度和旋转方向分为慢速、快速、顺时针、逆时针四种情况，定义的动作空间，一共包含20个基本动作库，停止动作是在完成所有参数的可观测校准空间后执行，强化学习框架从中选择建议的运动动作，由这些动作序列连续起来形成最终校准轨迹；
S3.2状态，视觉惯性标定过程的状态St定义为标定系统的各个参数Θ，并且参数会随着视觉惯性传感器随载体的运动进行更新；
S3.3状态转移函数，当智能体选择一个动作At时，状态转移函数St+1＝f(St,At)描述从当前状态St将转移到St+1的概率模型，当视觉惯性组件随载体执行正确的动作时，使得校准参数Θ被观察到；
S3.4奖励函数，奖励被视为采取行动后的反馈，在强化学习训练过程中，如果选择的动作能够将状态转移到更好的状态，则智能体将获得正的奖励；否则，将返回零奖励，奖励函数Rt(St,At)定义如下；
其中，
为待标定参数的可观测协方差，ε是使得校准参数空间可观测的隐式编码界限值，只有当标定参数协方差小于界限值，并能够生成可观测的校准轨迹时，智能体才能获得正的修正反馈；
步骤四，设计基于深度确定性策略梯度的强化学习标定算法，从惯性数据集筛选出可观测子片段进行参数标定；
在深度确定性策略梯度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网络，将动作模型和奖励模型参数化为神经网络，每个模型包含编码状态和目前精简的信息，输入为动作历史序列A0:t和校准状态序列S0:t，全连接网络来预测当前的奖励Rt(S0:t,A0:t)；
Q网络和策略网络的训练过程如下；
训练一个小批量数据后，通过随机梯度下降Random Gradient Descent，SGD算法更新在线网络的参数，然后通过软更新算法更新目标网络的参数，软更新是一种运行平均算法；
其中，τ为0.001。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110583354.3A CN113340324B (zh) | 2021-05-27 | 2021-05-27 | 一种基于深度确定性策略梯度的视觉惯性自校准方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202110583354.3A CN113340324B (zh) | 2021-05-27 | 2021-05-27 | 一种基于深度确定性策略梯度的视觉惯性自校准方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN113340324A CN113340324A (zh) | 2021-09-03 |
| CN113340324B true CN113340324B (zh) | 2022-04-29 |
Family
ID=77471775
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202110583354.3A Active CN113340324B (zh) | 2021-05-27 | 2021-05-27 | 一种基于深度确定性策略梯度的视觉惯性自校准方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN113340324B (zh) |
Families Citing this family (2)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN114047766B (zh) * | 2021-11-22 | 2023-11-21 | 上海交通大学 | 面向室内外场景长期应用的移动机器人数据采集系统及方法 |
| CN114218867B (zh) * | 2021-12-20 | 2022-06-28 | 暨南大学 | 基于熵优化安全强化学习的特种设备流程控制方法及系统 |
Family Cites Families (6)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN115338859A (zh) * | 2016-09-15 | 2022-11-15 | 谷歌有限责任公司 | 机器人操纵的深度强化学习 |
| US11147459B2 (en) * | 2018-01-05 | 2021-10-19 | CareBand Inc. | Wearable electronic device and system for tracking location and identifying changes in salient indicators of patient health |
| CN110673620B (zh) * | 2019-10-22 | 2020-10-27 | 西北工业大学 | 一种基于深度强化学习的四旋翼无人机航线跟随控制方法 |
| CN112162555B (zh) * | 2020-09-23 | 2021-07-16 | 燕山大学 | 混合车队中基于强化学习控制策略的车辆控制方法 |
| CN112668235B (zh) * | 2020-12-07 | 2022-12-09 | 中原工学院 | 基于离线模型预训练学习的ddpg算法的机器人控制方法 |
| CN112286218B (zh) * | 2020-12-29 | 2021-03-26 | 南京理工大学 | 基于深度确定性策略梯度的飞行器大迎角摇滚抑制方法 |
-
2021
- 2021-05-27 CN CN202110583354.3A patent/CN113340324B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN113340324A (zh) | 2021-09-03 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN111136660B (zh) | 机器人位姿定位方法及系统 | |
| CN106772524B (zh) | 一种基于秩滤波的农业机器人组合导航信息融合方法 | |
| CN113340324B (zh) | 一种基于深度确定性策略梯度的视觉惯性自校准方法 | |
| CN110398257A (zh) | Gps辅助的sins系统快速动基座初始对准方法 | |
| CN109188026B (zh) | 适用于mems加速度计的自动标定的深度学习方法 | |
| CN111156987A (zh) | 基于残差补偿多速率ckf的惯性/天文组合导航方法 | |
| CN107883965A (zh) | 基于光学信息交互多模型强跟踪容积卡尔曼滤波导航方法 | |
| CN111238535A (zh) | 一种基于因子图的imu误差在线标定方法 | |
| Kecskés et al. | Simultaneous calibration of a hexapod robot and an IMU sensor model based on raw measurements | |
| CN115046545A (zh) | 一种深度网络与滤波结合的定位方法 | |
| CN116583855A (zh) | 用于建模和控制部分可测量系统的方法和系统 | |
| CN114047766B (zh) | 面向室内外场景长期应用的移动机器人数据采集系统及方法 | |
| CN115388899A (zh) | 基于变分贝叶斯的移动机器人视觉惯性融合slam方法 | |
| CN115711616A (zh) | 一种室内室外穿越无人机的平顺定位方法及装置 | |
| CN117268381B (zh) | 一种航天器状态的判断方法 | |
| Luo et al. | End‐Effector Pose Estimation in Complex Environments Using Complementary Enhancement and Adaptive Fusion of Multisensor | |
| Liu et al. | LGC-Net: A lightweight gyroscope calibration network for efficient attitude estimation | |
| CN109655057B (zh) | 一种六推无人机加速器测量值的滤波优化方法及其系统 | |
| CN116625407A (zh) | 一种智能微姿态测量方法与系统 | |
| Fiedler et al. | A probabilistic moving horizon estimation framework applied to the visual-inertial sensor fusion problem | |
| CN115930971A (zh) | 一种机器人定位与建图的数据融合处理方法 | |
| Girrbach et al. | Adaptive compensation of measurement delays in multi-sensor fusion for inertial motion tracking using moving horizon estimation | |
| CN110849392A (zh) | 一种机器人的里程计数据校正方法及机器人 | |
| CN112556721B (zh) | 导航装置滤波器的随机误差的标定方法及系统 | |
| Zhang et al. | Mounting Misalignment and Time Offset Self-Calibration Online Optimization Method for Vehicular Visual-Inertial-Wheel Odometer System |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |