一种云台漂移补偿方法

摘要

本发明实施例公开了一种云台漂移补偿方法，采用本发明提供的云台漂移补偿方法，能够集合陀螺和采用滤波和融合的IMU各自的优点，实现高性能惯性稳定并实时自动解决视线角漂移问题的补偿算法，使云台搭载的成像设备视线角能够稳定、持续地指向重点区域进行较长时间的拍摄。

权利要求

1.一种云台漂移补偿方法，其特征在于，所述方法包括：

利用陀螺仪得到惯性角速度作为稳定回路的反馈信号；

在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的测角元件得到所述电机旋转的角位置初值和变化量；

获取预先配置的陀螺漂移时间阈值和陀螺漂移角度阈值中最先到达的阈值并根据所述最先到达的阈值计算陀螺漂移速度和陀螺漂移方向；其中，根据|α-α0+ζ-ζ0|＞THangle or time＞THtime计算陀螺漂移速度Vfwbias如下：其中，THtime为陀螺漂移时间阈值，THangle为陀螺漂移角度阈值，云台惯性稳定角漂移载荷的视线角在[-THangle,THangle]之内，α表示通过惯性测量单元获得的惯性角位置，α0表示通过惯性测量单元获得的惯性角位置初值，ζ表示通过测角元件获取转动的绝对角位置，ζ0表示通过测角元件获取转动的绝对角位置初值；

根据陀螺仪的初始信号和所述陀螺漂移速度确定陀螺漂移速度修正参数；

根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿。

2.根据权利要求1所述的云台漂移补偿方法，其特征在于，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿之后，所述方法还包括：当完成陀螺漂移补偿后，重复执行上述步骤。

3.根据权利要求1所述的云台漂移补偿方法，其特征在于，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿之后，所述方法还包括：判断补偿后的陀螺漂移速度是否小于误差允许阈值，若小于则结束漂移补偿。

4.根据权利要求1所述的云台漂移补偿方法，其特征在于，所述陀螺漂移时间阈值为5秒至10秒。

5.根据权利要求2所述的云台漂移补偿方法，其特征在于，

根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿，其中，陀螺漂移速度修正参数为：其中，

Vcmd表示陀螺漂移速度修正参数，Vopt表示陀螺漂移补偿速度，Vfwbias表示陀螺漂移速度。

6.根据权利要求5所述的云台漂移补偿方法，其特征在于，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿，包括：根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台采用匀加速或匀减速沿所述方向的反向进行漂移补偿，其中，陀螺漂移补偿速度为其中，ACC为固定加速度，0.5·ACC·Topt2＝|α-α0+ζ-ζ0|，t表示修正漂移过程的时间，t∈[0,Topt]，Topt表示修正漂移的最大时间。

7.根据权利要求6所述的云台漂移补偿方法，其特征在于，当完成陀螺漂移补偿后，重复执行上述步骤，包括：通过判断|α-α0+ζ-ζ0|≤THsta是否成立，若不成立则继续进行陀螺漂移补偿；其中，THsta表示陀螺漂移误差允许的角度阈值。

8.根据权利要求5所述的云台漂移补偿方法，其特征在于，判断补偿后的陀螺漂移速度是否小于误差允许阈值，若小于则结束漂移补偿，包括：当有Vfwbias≤THbias时，则退出漂移补偿；其中，THbias表示陀螺漂移误差允许的角速度阈值。

9.根据权利要求1所述的云台漂移补偿方法，其特征在于，所述惯性测量单元具有滤波和融合算法，所述测角元件包括编码器或感应同步器，所述在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的测角元件得到所述电机旋转的角位置初值和变化量，包括：在云台惯性稳定时通过所述惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的编码器或感应同步器得到所述电机旋转的角位置初值和变化量。

说明书

一种云台漂移补偿方法

技术领域

[0001] 本发明涉及动基座稳定成像技术领域，尤其涉及一种云台漂移补偿方法。

背景技术

[0002] 近年来，各类动基座成像云台广泛用于航拍、无人车、机器人等领域，特别是利用无人机进行航拍，它能够快速、灵活、清晰、准确地获取感兴趣区域的照片和视频信息。稳定云台是用于补偿无人机等载体的姿态运动、振动，保证云台上搭载的成像设备实现清晰、稳定拍摄的必要装置。

[0003] 稳定云台的核心部件是惯性传感器，常见的惯性器件有陀螺和惯性测量单元(IMU，Inertial Measurement Unit)。首先，单纯依靠陀螺测量角速度进行云台的稳定控制时，由于陀螺漂移的影响，视线角会在惯性空间中产生漂移，无法可靠地指向某一区域，必须依靠遥控装置发出角速度指令使漂移的视线角重新回到目标区域。另一方面，大部分的IMU通常采用卡尔曼滤波(KF)等方法，综合利用IMU中的陀螺和加速度计信息能够避免惯性角速度测量乃至惯性角度测量中的漂移问题。但是，为了保证输出信号质量而进行滤波和融合的同时也会带来输出信号严重的相位滞后。这种相位滞后的信号会限制稳定云台用于实现惯性稳定的控制算法的增益，从而限制了云台的稳定性能。

发明内容

[0004] 有鉴于此，本发明实施例提供了一种云台漂移补偿方法，能够集合陀螺和采用滤波和融合的IMU各自的优点，实现高性能惯性稳定并实时自动解决视线角漂移问题的补偿算法，使云台搭载的成像设备视线角能够稳定、持续地指向重点区域进行较长时间的拍摄。

[0005] 本发明提供一种云台漂移补偿方法，所述方法包括：

[0006] 利用陀螺仪得到惯性角速度作为稳定回路的反馈信号；

[0007] 在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的测角元件得到所述电机旋转的角位置初值和变化量；

[0008] 获取预先配置的陀螺漂移时间阈值和陀螺漂移角度阈值中最先到达的阈值并根据所述最先到达的阈值计算陀螺漂移速度和陀螺漂移方向；

[0009] 根据陀螺仪的初始信号和所述陀螺漂移速度确定陀螺漂移速度修正参数；

[0010] 根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿。

[0011] 可选地，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿之后，所述方法还包括：

[0012] 当完成陀螺漂移补偿后，重复执行上述步骤。

[0013] 可选地，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿之后，所述方法还包括：

[0014] 判断补偿后的陀螺漂移速度是否小于误差允许阈值，若小于则结束漂移补偿。

[0015] 可选地，所述利用陀螺仪得到惯性角速度作为稳定回路的反馈信号，包括：

[0016] 读取陀螺仪三轴陀螺信号分别是ω、μ、τ，经过坐标变换得到惯性空间中角速度将角速度τ'作为云台惯性稳定的反馈信号。

[0017] 可选地，所述在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量，包括：

[0018] 在云台惯性稳定时通过惯性测量单元获取转动的绝对角位置初值为ζ0，运动过程中的绝对角位置为ζ，计算得到惯性角位置初值分别为α0，运动过程中惯性角位置为α；

[0019] 所述获取预先配置的陀螺漂移时间阈值和陀螺漂移角度阈值中最先到达的阈值并根据所述最先到达的阈值计算陀螺漂移速度和陀螺漂移方向，包括：

[0020] 根据|α-α0+ζ-ζ0|>THangle or time>THtime计算陀螺漂移角速度Vfwbias如下：

[0021]

[0022] 其中，THangle为陀螺漂移角度阈值，云台惯性稳定角漂移载荷的视线角在[-THangle,THangle]之内；

[0023] 其中，THtime为陀螺漂移时间阈值，所述陀螺漂移时间阈值为5秒至10秒。

[0024] 可选地，根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿，其中，漂移补偿速度为：

[0025]

[0026] 可选地，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿，包括：

[0027] 根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台采用匀加速或匀减速沿所述方向的反向进行漂移补偿，其中，漂移补偿速度为

[0028]

[0029] 其中，ACC为固定加速度，0.5·ACC·Topt2＝|α-α0+ζ-ζ0|。

[0030] 可选地，所述当完成陀螺漂移补偿后，重复执行上述步骤，包括：

[0031] 通过判断|α-α0+ζ-ζ0|≤THsta是否成立，若不成立则继续进行陀螺漂移补偿。

[0032] 可选地，所述判断补偿后的陀螺漂移速度是否小于误差允许阈值，若小于则结束漂移补偿，包括：

[0033] 当有Vfwbias≤THbias时，则退出漂移补偿。

[0034] 可选地，所述惯性测量单元具有滤波和融合算法，所述测角元件包括编码器或感应同步器，所述在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的测角元件得到所述电机旋转的角位置初值和变化量，包括：

[0035] 在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的编码器或感应同步器得到所述电机旋转的角位置初值和变化量。

[0036] 从以上技术方案可以看出，本发明实施例具有以下优点：

[0037] 本发明实施例公开了一种云台漂移补偿方法，采用本发明提供的云台漂移补偿方法，能够集合陀螺和采用滤波和融合的IMU各自的优点，实现高性能惯性稳定并实时自动解决视线角漂移问题的补偿算法，使云台搭载的成像设备视线角能够稳定、持续地指向重点区域进行较长时间的拍摄。

附图说明

[0038] 图1是本发明实施例中提供的一种云台漂移补偿方法的流程图；

[0039] 图2是本发明实施例中提供的一种云台漂移补偿方法中云台的结构示意图。

具体实施方式

[0040] 为了使本技术领域的人员更好地理解本发明方案，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分的实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都应当属于本发明保护的范围。

[0041] 本发明的说明书和权利要求书及上述附图中的术语“第一”、“第二”、“第三”、“第四”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。应该理解这样使用的数据在适当情况下可以互换，以便这里描述的实施例能够以除了在这里图示或描述的内容以外的顺序实施。此外，术语“包括”和“具有”以及他们的任何变形，意图在于覆盖不排他的包含，例如，包含了一系列步骤或单元的过程、方法、系统、产品或设备不必限于清楚地列出的那些步骤或单元，而是可包括没有清楚地列出的或对于这些过程、方法、产品或设备固有的其它步骤或单元。

[0042] 结合图1所示，本发明提供一种云台漂移补偿方法，所述方法包括：

[0043] S101、利用陀螺仪得到惯性角速度作为稳定回路的反馈信号；

[0044] S102、在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的测角元件得到所述电机旋转的角位置初值和变化量；

[0045] S103、获取预先配置的陀螺漂移时间阈值和陀螺漂移角度阈值中最先到达的阈值并根据所述最先到达的阈值计算陀螺漂移速度和陀螺漂移方向；

[0046] S104、根据陀螺仪的初始信号和所述陀螺漂移速度确定陀螺漂移速度修正参数；

[0047] S105、根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿。

[0048] 可选地，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿之后，所述方法还包括：

[0049] 当完成陀螺漂移补偿后，重复执行上述步骤。

[0050] 可选地，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿之后，所述方法还包括：

[0051] 判断补偿后的陀螺漂移速度是否小于误差允许阈值，若小于则结束漂移补偿。

[0052] 可选地，所述利用陀螺仪得到惯性角速度作为稳定回路的反馈信号，包括：

[0053] 读取陀螺仪三轴陀螺信号分别是ω、μ、τ，经过坐标变换得到惯性空间中角速度将角速度τ'作为云台惯性稳定的反馈信号。

[0054] 可选地，所述在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量，包括：

[0055] 在云台惯性稳定时通过惯性测量单元获取转动的绝对角位置初值为ζ0，运动过程中的绝对角位置为ζ，计算得到惯性角位置初值分别为α0，运动过程中惯性角位置为α；

[0056] 所述获取预先配置的陀螺漂移时间阈值和陀螺漂移角度阈值中最先到达的阈值并根据所述最先到达的阈值计算陀螺漂移速度和陀螺漂移方向，包括：

[0057] 根据|α-α0+ζ-ζ0|>THangle or time>THtime计算陀螺漂移角速度Vfwbias如下：

[0058]

[0059] 其中，THangle为陀螺漂移角度阈值，云台惯性稳定角漂移载荷的视线角在[-THangle,THangle]之内；

[0060] 其中，THtime为陀螺漂移时间阈值，所述陀螺漂移时间阈值为5秒至10秒。

[0061] 可选地，根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿，其中，漂移补偿速度为：

[0062]

[0063] 可选地，所述根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台沿所述方向的反向进行漂移补偿，包括：

[0064] 根据所述陀螺漂移方向和所述陀螺漂移速度修正参数控制云台采用匀加速或匀减速沿所述方向的反向进行漂移补偿，其中，漂移补偿速度为

[0065] 其中，ACC为固定加速度，0.5·ACC·Topt2＝|α-α0+ζ-ζ0|。

[0066] 可选地，所述当完成陀螺漂移补偿后，重复执行上述步骤，包括：

[0067] 通过判断|α-α0+ζ-ζ0|≤THsta是否成立，若不成立则继续进行陀螺漂移补偿。

[0068] 可选地，所述判断补偿后的陀螺漂移速度是否小于误差允许阈值，若小于则结束漂移补偿，包括：

[0069] 当有Vfwbias≤THbias时，则退出漂移补偿。

[0070] 可选地，所述惯性测量单元具有滤波和融合算法，所述测角元件包括编码器或感应同步器，所述在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的编码器或感应同步器得到所述电机旋转的角位置初值和变化量，包括：

[0071] 在云台惯性稳定时通过惯性测量单元获得惯性角位置的初值及变化量,并通过与电机同轴安装的编码器或感应同步器得到所述电机旋转的角位置初值和变化量。

[0072] 本发明提供一种云台漂移补偿方法，一方面采用未加滤波的陀螺信号直接做惯性稳定的反馈信号；另一方面采用带有滤波和融合的IMU修正陀螺漂移，实现自动的漂移补偿，能够集合陀螺和采用滤波和融合的IMU各自的优点，实现高性能惯性稳定并实时自动解决视线角漂移问题的补偿算法，使云台搭载的成像设备视线角能够稳定、持续地指向重点区域进行较长时间的拍摄。

[0073] 本实施方式针对图2所示的三轴框架结构和IMU与基座固连的安装方式阐述本算法的具体实施方式，不同的框架结构和IMU不同的安装方式，只影响IMU角位置信号的解算，即坐标变换，并不影响本算法的使用；

[0074] 如图2所示，三轴IMU固定在基座上，三轴陀螺和相机固定在内框架中，外框架绕方位旋转，中框架绕横滚轴旋转，内框架绕俯仰轴旋转；

[0075] 其中，所述的坐标变换需要规定好正方向；

[0076] 所述的坐标变换矩阵包括基座到外框、外框到中框、中框到内框的三个变换矩阵，外框相对于基座的方位转角是δ，则变换矩阵为 中框相对于外框的横滚角是 则变换矩阵为 内框相对中框的俯仰角是σ，则变换矩阵为

[0077] 由于上述坐标变换属于该领域内常识性内容，在多数参考书中可以查到，因此推导过程此处忽略；

[0078] 假设，由IMU直接得到的方位角度、横滚角度和俯仰角度分别为α、β和γ，经过坐标变换转换到对应外框架的方位惯性角度为α'，中框架的横滚惯性角度为β'，内框架的俯仰惯性角度为γ'；

[0079] 则，经过上述坐标变换得到对应方位外框架的惯性角度α'＝α；

[0080] 对应横滚中框架的惯性角度β'＝sinδ·γ+cosδ·β；

[0081] 对应俯仰内框架的惯性角度

[0082] 通常，测角元件跟电机同轴安装，可直接得到电机转动的角度信号；

[0083] 在云台惯性稳定状态中，分别对上述三个轴在惯性空间中使用本发明阐述的补偿算法，可以实现三轴云台惯性稳定角漂移补偿；

[0084] 其中，经过坐标变换后，三个轴之间的运算是完全解耦的，因此本实施方式只针对其中一个轴做阐述，这里以方位轴为例；

[0085] (1)读取内框架的三轴陀螺信号分别是ω、μ、τ，经过坐标变换得到惯性空间中方位轴的角速度

[0086] 其中，使用τ'做云台方位轴惯性稳定的反馈信号，稳定指令为0，实现惯性空间中方位轴的稳定控制；

[0087] (2)程序初始化中，使用坐标变换，计算IMU得到的外框架方位惯性角度初始值，由测角元件得到的外框架方位电机轴绝对角位置初始值；

[0088] 其中，所述的测角元件可以采用编码器或者感应同步器，方位轴转动的绝对角位置初值为ζ0，运动过程中的绝对角位置为ζ；

[0089] 其中，IMU计算得到的方位轴惯性角位置初值分别为α0，运动过程中IMU记录的方位轴惯性角位置为α；

[0090] (3)判断陀螺漂移时间THtime或者陀螺漂移角度THangle两者是否达到阈值，以先达到的为准计算陀螺漂移速度；

[0091] 惯性稳定过程中，惯性角度变化量和框架转角变化量方向相反，正是由于陀螺漂移使得两者之和不为0，因此两者之和就是陀螺漂移角度值，根据|α-α0+ζ-ζ0|>THangle or time>THtime计算陀螺漂移角速度Vfwbias如下：

[0092]

[0093] 其中，陀螺漂移角度阈值THangle可以根据实际应用选取，使得云台惯性稳定角漂移，即载荷的视线角在[-THangle,THangle]之内；

[0094] 其中，陀螺漂移时间阈值THtime也根据实际应用选取，通常可以取5-10s即可，保证10s内完成一次陀螺漂移补偿；

[0095] (4)使用步骤(3)中计算的陀螺漂移平均值对陀螺仪输出的原始信号进行修正；

[0096] 其中，所述的修正是对原始陀螺仪信号进行计算Vfwgyro＝Vfwgyro-Vfwbias，修正后稳定状态中相机视轴漂移速率明显变慢；

[0097] (5)启动陀螺漂移补偿程序，根据步骤(3)计算的漂移速度大小和方向优化补偿速度指令，使云台的能够朝向步骤(3)中陀螺漂移的反方向快速运动，保证视线角指向重点区域，实现惯性稳定；

[0098] 其中，漂移补偿速度为

[0099] 其中，为了快速而稳定的实现补偿效果，优化的补偿速度指令为

[0100] 其中ACC为固定加速度，保证补偿过程是匀加速或匀减速运动，则有0.5·ACC·2
Topt＝|α-α0+ζ-ζ0|；

[0101] (6)通过判断|α-α0+ζ-ζ0|≤THsta是否成立，决定一次陀螺漂移补偿是否结束，陀螺漂移角度误差允许阈值THsta可根据实际应用云台指标设定；

[0102] 其中，重复步骤(1)-(6)可以实现云台惯性稳定角度漂移实时补偿；

[0103] (7)可选择加入算法结束程序，当有Vfwbias≤THbias时，可以退出上述补偿算法，此时的惯性稳定效果仍将优于未加入角漂移补偿算法之前的状态。

[0104] 采用本发明提供的云台漂移补偿方法，能够集合陀螺和采用滤波和融合的IMU各自的优点，实现高性能惯性稳定并实时自动解决视线角漂移问题的补偿算法，使云台搭载的成像设备视线角能够稳定、持续地指向重点区域进行较长时间的拍摄。

[0105] 所属领域的技术人员可以清楚地了解到，为描述的方便和简洁，上述描述的系统，装置和单元的具体工作过程，可以参考前述方法实施例中的对应过程，在此不再赘述。

[0106] 在本申请所提供的几个实施例中，应该理解到，所揭露的系统，装置和方法，可以通过其它的方式实现。例如，以上所描述的装置实施例仅仅是示意性的，例如，所述单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

[0107] 所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施例方案的目的。

[0108] 另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

[0109] 本领域普通技术人员可以理解上述实施例的各种方法中的全部或部分步骤是可以通过程序来指令相关的硬件来完成，该程序可以存储于一计算机可读存储介质中，存储介质可以包括：只读存储器(ROM，Read Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁盘或光盘等。

[0110] 以上对本发明所提供的一种云台漂移补偿方法.进行了详细介绍，对于本领域的一般技术人员，依据本发明实施例的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。