一种姿态自校正云台摄像机控制装置及其方法

摘要

本发明公开了一种姿态自校正云台摄像机控制装置，包含：轨道；轨道平台，设置在轨道上，可沿轨道滑动；轨道电机，与轨道平台连接，提供轨道平台运动的动力；云台摄像机，安装在轨道平台上，可随轨道平台一起运动；云台电机，与所述云台摄像机连接，提供云台摄像机俯仰及偏转的动力；第一传感器，设置在轨道平台上，可随轨道平台一起运动；第二传感器，设置在云台摄像机上，可随云台摄像机俯仰及偏转；嵌入式微控制器，其信号输入端分别连接第一传感器及第二传感器，其输出端分别连接轨道电机及云台电机。本发明还公开了一种控制方法。本发明能够补偿由于机械磨损，热胀冷缩等因素造成的行进距离误差及偏转、俯仰角度误差，实现姿态自校正。

权利要求

1.一种姿态自校正云台摄像机控制方法，其特征在于，该控制方法采用一种姿态自校正云台摄像机控制装置来实现，其包含：轨道；

轨道平台，设置在轨道上，可沿轨道滑动；

轨道电机，与轨道平台连接，提供轨道平台运动的动力；

云台摄像机，安装在轨道平台上，可随轨道平台一起运动；

云台电机，与所述云台摄像机连接，提供云台摄像机俯仰及偏转的动力；

第一传感器，设置在轨道平台上，可随轨道平台一起运动；

第二传感器，设置在云台摄像机上，可随云台摄像机俯仰及偏转；

嵌入式微控制器，其信号输入端分别连接第一传感器及第二传感器，其输出端分别连接轨道电机及云台电机；

第三传感器，设置在云台摄像机上，可随云台摄像机俯仰及偏转，所述第三传感器连接嵌入式微控制器的信号输入端；

所述的第一传感器及第二传感器均为加速度计，所述第三传感器为一陀螺仪；

所述的姿态自校正云台摄像机控制方法包含轨道平台行进距离自校正方法及云台摄像机俯仰、偏转自校正方法，其中所述轨道平台行进距离自校正方法包含：A1、嵌入式微控制器根据第一传感器采集的数据计算出轨道平台实际行进的距离；

A2、嵌入式微控制器将轨道平台实际行进的距离与预设行进距离进行对比得到行进距离误差；

A3、嵌入式微控制器判断行进距离误差是否满足允许行进距离误差；

A31、若是，则不作处理；

A32、若否，则执行误差补偿步骤，嵌入式微控制器发出命令控制轨道电机运动，使得轨道平台运动行进距离误差值，达到预设行进距离；

所述云台摄像机俯仰、偏转自校正方法包含：

B1、嵌入式微控制器将第二传感器及第三传感器的数据进行融合后，计算出云台摄像机实际偏转的角度及实际俯仰的角度；

B2、嵌入式微控制器将云台摄像机实际偏转的角度与预设偏转角度进行对比得到偏转角度误差，将云台摄像机实际俯仰的角度与预设俯仰角度进行对比得到俯仰角度误差；

B3、嵌入式微控制器判断偏转角度误差是否满足允许偏转角度误差及判断俯仰角度误差是否满足允许俯仰角度误差；

B31、若是，则不作处理；

B32、若否，则执行误差补偿步骤，嵌入式微控制器发出命令控制云台电机运动，使得云台摄像机运动偏转角度误差值，达到预设偏转角度，或使得云台摄像机运动俯仰角度误差值，达到预设俯仰角度。

2.如权利要求1所述的姿态自校正云台摄像机控制方法，其特征在于，所述的轨道平台行进距离自校正方法中步骤A1之前包含：A01、轨道电机接收嵌入式微控制器的命令后开始运动，移动预设行进距离，同时第一传感器开始采集数据，嵌入式微控制器开始计时；

A02、当轨道平台停止移动时，嵌入式微控制器停止计时，第一传感器停止采集数据，并将数据发送至嵌入式微控制器。

3.如权利要求1所述的姿态自校正云台摄像机控制方法，其特征在于，所述的轨道平台行进距离自校正方法中步骤A32之后进一步包含步骤A4，嵌入式微控制器保存行进距离误差。

4.如权利要求1所述的姿态自校正云台摄像机控制方法，其特征在于，所述的所述云台摄像机俯仰、偏转自校正方法中步骤B1之前包含：B01、云台电机接收嵌入式微控制器的命令后开始运动，移动预设偏转角度及预设俯仰角度，同时第二传感器及第三传感器开始采集数据，嵌入式微控制器开始计时；

B02、当云台电机停止移动时，嵌入式微控制器停止计时，第二传感器及第三传感器停止采集数据，并将数据发送至嵌入式微控制器。

5.如权利要求1所述的姿态自校正云台摄像机控制方法，其特征在于，所述的云台摄像机俯仰、偏转自校正方法中步骤B32之后进一步包含步骤B4，嵌入式微控制器保存偏转角度误差及俯仰角度误差。

6.如权利要求1或3所述的姿态自校正云台摄像机控制方法，其特征在于，所述允许行进距离误差为±5厘米。

7.如权利要求1或5所述的姿态自校正云台摄像机控制方法，其特征在于，所述允许偏转角度误差为±2度，所述允许俯仰角度误差为±2度。

说明书

一种姿态自校正云台摄像机控制装置及其方法

技术领域

[0001] 本发明涉及视频监控领域，具体涉及一种姿态自校正云台摄像机控制装置及其方法。

背景技术

[0002] 在无人值守的变电站、机房的远程监控等需要移动监控的场所，平时并没有相关的工作人员在值班，运维人员在控制中心通过远程控制云台装置来操作监控场所里的活动轨道摄像机对设备进行日常的巡检工作，活动轨道摄像机也可以在无人操作的情况下通过预先设置的轨道位置和角度，实现自动巡航，定时对指定的位置进行巡检，以方便对特定的设备进行观察和分析。

[0003] 传统的活动轨道摄像机远程控制云台装置是通过控制芯片编程，控制步进电机和齿轮结构来到达预先设定的机位，通过步进电机来控制行进距离，通过齿轮结构来提供行进的动力并控制云台的偏转角度等，这些都是由电机和齿轮等硬件组合来实现云台控制的动作。这类结构的云台装置通过长时间的运行后，由于机械磨损，热胀冷缩等因素的影响，云台操作活动轨道摄像机的行进距离和偏转角度会与预先设定的值产生偏差，导致活动云台不能达到预设的位置，活动轨道摄像头的行进距离与偏转角度的偏差也会越来越大，后期需要经常进行手工校正才能正常运行，校正工作往往需要专业人员来完成，浪费人力、物力与时间，直接影响正常生产工作的开展。

发明内容

[0004] 本发明的目的在于提供一种姿态自校正云台摄像机控制装置及其方法，能够补偿由于机械磨损，热胀冷缩等因素造成的行进距离误差、偏转俯仰角度误差，实现云台摄像机的姿态自校正。

[0005] 为了达到上述目的，本发明通过以下技术方案实现：一种姿态自校正云台摄像机控制装置，其特点是，包含：

[0006] 轨道；

[0007] 轨道平台，设置在轨道上，可沿轨道滑动；

[0008] 轨道电机，与轨道平台连接，提供轨道平台运动的动力；

[0009] 云台摄像机，安装在轨道平台上，可随轨道平台一起运动；

[0010] 云台电机，与所述云台摄像机连接，提供云台摄像机俯仰及偏转的动力；

[0011] 第一传感器，设置在轨道平台上，可随轨道平台一起运动；

[0012] 第二传感器，设置在云台摄像机上，可随云台摄像机俯仰及偏转；

[0013] 嵌入式微控制器，其信号输入端分别连接第一传感器及第二传感器，其输出端分别连接轨道电机及云台电机。

[0014] 姿态自校正云台摄像机控制装置进一步包含一第三传感器，设置在云台摄像机上，可随云台摄像机俯仰及偏转，所述第三传感器连接嵌入式微控制器的信号输入端。

[0015] 所述的第一传感器及第二传感器均为加速度计，所述第三传感器为一陀螺仪。

[0016] 一种姿态自校正云台摄像机控制方法，其特点是，包含轨道平台行进距离自校正方法及云台摄像机俯仰、偏转自校正方法，其中所述轨道平台行进距离自校正方法包含：

[0017] A1、嵌入式微控制器根据第一传感器采集的数据计算出轨道平台实际行进的距离；

[0018] A2、嵌入式微控制器将轨道平台实际行进的距离与预设行进距离进行对比得到行进距离误差；

[0019] A3、嵌入式微控制器判断行进距离误差是否满足允许行进距离误差；

[0020] A31、若是，则不作处理；

[0021] A32、若否，则执行误差补偿步骤，嵌入式微控制器发出命令控制轨道电机运动，使得轨道平台运动行进距离误差值，达到预设行进距离；

[0022] 所述云台摄像机俯仰、偏转自校正方法包含：

[0023] B1、嵌入式微控制器将第二传感器及第三传感器的数据融合后，计算出云台摄像机实际偏转的角度及实际俯仰的角度；

[0024] B2、嵌入式微控制器将云台摄像机实际偏转的角度与预设偏转角度进行对比得到偏转角度误差，将云台摄像机实际俯仰的角度与预设俯仰角度进行对比得到俯仰角度误差；

[0025] B3、嵌入式微控制器判断偏转角度误差是否满足允许偏转角度误差及判断俯仰角度误差是否满足允许俯仰角度误差；

[0026] B31、若是，则不作处理；

[0027] B32、若否，则执行误差补偿步骤，嵌入式微控制器发出命令控制云台电机运动，使得云台摄像机运动偏转角度误差值，达到预设偏转角度，或使得云台摄像机运动俯仰角度误差值，达到预设俯仰角度。

[0028] 所述的轨道平台行进距离自校正方法中步骤A1之前包含：

[0029] A01、轨道电机接收嵌入式微控制器的命令后开始运动，移动预设行进距离，同时第一传感器开始采集数据，嵌入式微控制器开始计时；

[0030] A02、当轨道平台停止移动时，嵌入式微控制器停止计时，第一传感器停止采集数据，并将数据发送至嵌入式微控制器。

[0031] 所述的轨道平台行进距离自校正方法中步骤A32之后进一步包含步骤A4，嵌入式微控制器保存行进距离误差。

[0032] 所述的所述云台摄像机俯仰、偏转自校正方法中步骤B1之前包含：

[0033] B01、云台电机接收嵌入式微控制器的命令后开始运动，移动预设偏转角度及预设俯仰角度，同时第二传感器及第三传感器开始采集数据，嵌入式微控制器开始计时；

[0034] B02、当云台电机停止移动时，嵌入式微控制器停止计时，第二传感器及第三传感器停止采集数据，并将数据发送至嵌入式微控制器。

[0035] 所述的云台摄像机俯仰、偏转自校正方法中步骤B32之后进一步包含步骤B4，嵌入式微控制器保存偏转角度误差及俯仰角度误差。

[0036] 所述允许行进距离误差为±5厘米。

[0037] 所述允许偏转角度误差为±2度，所述允许俯仰角度误差为±2度。

[0038] 本发明一种姿态自校正云台摄像机控制装置及其方法与现有技术相比具有以下优点：结合加速度计与陀螺仪等传感器技术来检测轨道平台及云台摄像机的姿态变化，并结合嵌入式软件来精确计算轨道平台的实际行进距离和云台摄像机偏转角度及俯仰角度，再通过与预设值进行对比来计算出因长时间运行而产生的机械误差，当发现产生的机械误差值超过允许的范围后，进行误差补偿，实现轨道平台行进距离上的位置修正和云台摄像机偏转角度、俯仰角度的位置修正，从而实现云台摄像机姿态的自动校正。

附图说明

[0039] 图1为本发明一种姿态自校正云台摄像机控制装置的整体结构示意图；

[0040] 图2为轨道平台行进距离自校正方法流程图；

[0041] 图3为云台摄像机俯仰、偏转自校正方法流程图；

[0042] 图4为云台摄像机旋转角度矢量图；

[0043] 图5为本发明实施例图。

具体实施方式

[0044] 以下结合附图，通过详细说明一个较佳的具体实施例，对本发明做进一步阐述。

[0045] 如图1所示，一种姿态自校正云台摄像机控制装置，包含：轨道；轨道平台，设置在轨道上，可沿轨道滑动；轨道电机1，与轨道平台连接，提供轨道平台运动的动力；云台摄像机，安装在轨道平台上，可随轨道平台一起运动；云台电机2，与所述云台摄像机连接，提供云台摄像机俯仰及偏转的动力；第一传感器3，设置在轨道平台上，可随轨道平台一起运动；第二传感器4，设置在云台摄像机上，可随云台摄像机俯仰及偏转；第三传感器5，设置在云台摄像机上，可随云台摄像机俯仰及偏转；嵌入式微控制器6(MCU)，其信号输入端分别连接第一传感器3、第二传感器4及第三传感器5，传感器与MCU之间还设有数模转换单元7，嵌入式微控制器6的输出端分别连接轨道电机1及云台电机2，电机与MCU之间还设有电机控制单元8。嵌入式微控制器6还连接一个后台主站9，作为远端控制中心，用于发送控制命令，后台主站9与MCU之间采用TCP/IP协议。嵌入式微控制器6与数模转换单元7之间采用SPI接口传输数据，嵌入式微控制器6与电机控制单元8采用脉冲宽度调制方式(PWM)及I/O接口连接。

[0046] 结合上述的姿态自校正云台摄像机控制装置，实现其控制方法，方法包含轨道平台行进距离自校正方法及云台摄像机俯仰、偏转自校正方法。

[0047] 如图2所示，轨道平台行进距离自校正方法包含：

[0048] A01、轨道电机接收嵌入式微控制器的命令后开始运动，移动预设行进距离，同时第一传感器开始采集数据，嵌入式微控制器开始计时；

[0049] A02、当轨道平台停止移动时，嵌入式微控制器停止计时，第一传感器停止采集数据，并将数据发送至嵌入式微控制器；

[0050] A1、嵌入式微控制器根据第一传感器采集的数据计算出轨道平台实际行进的距离；

[0051] A2、嵌入式微控制器将轨道平台实际行进的距离与预设行进距离进行对比得到行进距离误差；

[0052] A3、嵌入式微控制器判断行进距离误差是否满足允许行进距离误差；

[0053] A31、若是，则不作处理；

[0054] A32、若否，则执行误差补偿步骤，嵌入式微控制器发出命令控制轨道电机运动，使得轨道平台运动行进距离误差值，达到预设行进距离；

[0055] A4、嵌入式微控制器保存行进距离误差。

[0056] 如图3所示，云台摄像机俯仰、偏转自校正方法包含：

[0057] B01、云台电机接收嵌入式微控制器的命令后开始运动，移动预设偏转角度及预设俯仰角度，同时第二传感器及第三传感器开始采集数据，嵌入式微控制器开始计时；

[0058] B02、当云台电机停止移动时，嵌入式微控制器停止计时，第二传感器及第三传感器停止采集数据，并将数据发送至嵌入式微控制器；

[0059] B1、嵌入式微控制器将第二传感器及第三传感器的数据融合后，计算出云台摄像机实际偏转的角度及实际俯仰的角度；

[0060] B2、嵌入式微控制器将云台摄像机实际偏转的角度与预设偏转角度进行对比得到偏转角度误差，将云台摄像机实际俯仰的角度与预设俯仰角度进行对比得到俯仰角度误差；

[0061] B3、嵌入式微控制器判断偏转角度误差是否满足允许偏转角度误差及判断俯仰角度误差是否满足允许俯仰角度误差；

[0062] B31、若是，则不作处理；

[0063] B32、若否，则执行误差补偿步骤，嵌入式微控制器发出命令控制云台电机运动，使得云台摄像机运动偏转角度误差值，达到预设偏转角度，或使得云台摄像机运动俯仰角度误差值，达到预设俯仰角度；

[0064] B4、嵌入式微控制器保存偏转角度误差及俯仰角度误差。

[0065] 本实施例中，第一传感器及第二传感器均为加速度计，第三传感器采用陀螺仪。在另外的实施例中，能够实现该功能的传感器也应包含在内。本发明可以包含五个模块，分别为加速度计，轨道平台构成轨道姿态采集模块，由加速度计、陀螺仪和云台摄像机构成云台姿态采集模块，由轨道电机、齿轮和轨道构成的轨道系统平台，由云台摄像机及云台电机组成的云台摄像机系统，由MCU、数模转换单元及电机控制单元组成的系统控制模块。

[0066] 在轨道姿态采集模块中是通过采集固定在轨道平台上的加速度计的数据，然后MCU将采集得到的数据做两次积分运算处理，可以得到轨道平台的实际行进距离，公式如下所示：

[0067]

[0068]

[0069] 式中v是计算出的速度值，v0是初始速度值，a为加速度计获取的加速度值，t是运行的时间，s是计算出实际运行的距离，s0是初始距离值，其中，v0为零，s0为零。

[0070] 将计算得到的距离s对比预设行进距离可以得出轨道平台行进距离的机械误差。

[0071] 在云台姿态测量模块中陀螺仪是用来测量角速度信号的，通过对角速度积分，计算出实际变化的角度。由于陀螺仪容易受到噪声干扰，不能承受大幅的震动，同时由于温度等因素影响，容易产生漂移误差，该误差会随着时间变化逐步放大。这个时候需要用到加速度计测量的加速度数据，用这个数据与陀螺仪积分数据通过卡尔曼滤波器进行数据融合，实现对获取的姿态数据进行优化处理。MCU通过SPI接口得到数模转换单元采集获取陀螺仪、加速度计的数据后，先采用卡尔曼滤波对加速度计和陀螺仪的数据进行融合，减少姿态测量的误差，提高测量精度。当获取到融合的三轴的偏转角度数据后，建立四元数微分方程，再根据龙格-库塔法求解该方程，根据结果计算出云台摄像机实际运转的角度值，从而得知运转误差。具体公式与方法如下。

[0072] 云台摄像机的角位置可以通过由n->b某个轴转过某个角度(ωx,ωy,ωz)得到。四元数提供了这种数学描述。四元素的一般表示方程：

[0073] Q＝q0+q1i+q2j+q3k  (3)

[0074] 其中，q0，q1，q2，q3为实数，i，j，k为相互正交的单位矢量，即虚单位。

[0075] 四元素的微分方程：

[0076]

[0077] 其中， 表示云台摄像机的角速度值。

[0078] 结合公式3和公式4写成矩阵形式为：

[0079]

[0080] 其中 分别表示云台摄像机坐标系对于地理坐标系沿各个轴向的角速度分量。

[0081] 龙格-库塔是一种便于计算机求解微分方程的数值积分算法，表达为已知方程式解的初值时，在求解存在的区间各点上，逐点求出方程的近似解。若已知t时刻的Q(t)，t+H/2，t+H时刻的ω(t+H/2)和ω(t+H)，则有：

[0082]

[0083]

[0084]

[0085]

[0086]

[0087] 其中Q(t)表示t时刻的四元素矩阵，ω(t)表示t时刻的测量角速度矢量。得到四元素后，则可以推导姿态矩阵，从而推导出姿态角。如图4所示，矢量R在两个坐标上的投影关系公式为：

[0088]

[0089] 其中C表示姿态变化后载体前后坐标之间的方向余弦矩阵。

[0090] 由上述矩阵则可以推出姿态角主值，具体公式如下：

[0091]

[0092] 在云台摄像机的转动中，只涉及到偏转角和俯仰角，即对应公式中的θ角和γ角，对应的 固定不动。

[0093] 在本发明的具体实施例中，后台主站指定位置和指定角度实现摄像机自动巡航，在指定时间里将指定位置的视频传输回来，如图5所示，不同位置设有不同的需要巡检的设备，摄像机不同的时刻工作在不同的位置，实现3个设备的巡检，例如在8点时将云台摄像机的位置移动到位置1，预设行进距离450cm，并将方向调整至设备1方向，云台摄像机预设偏转旋转175°，预设俯仰角度-30°；9点时将云台摄像机的位置移动到位置2，预设行进距离200cm，并将方向调整至设备2方向，云台摄像机预设偏转旋转-195°，预设俯仰角度30°；10点时将云台摄像机的位置移动到位置3，预设行进距离720cm，并将方向调整至设备3方向，
11点调回位置1位置，依次循环，实现摄像机的自动巡航，同时设定轨道平台允许行进距离误差为±5cm，云台摄像机允许偏转角度误差为±2度，云台摄像机允许俯仰角度误差为±2度。

[0094] 当时间到8点时，首先MCU控制轨道电机根据预设的行进距离450cm开始运动，同时开启轨道平台上的加速度计开始采集数据，当轨道平台运行到位置1时，MCU记下运行时间，然后根据采集到的加速度计数据和时间计算出实际运行的轨道距离S，MCU对比预设行进距离值计算出对应的行进距离误差△S＝S-450，MCU开始比较行进距离误差与允许行进距离误差值的大小，如果|△S|<5cm，则不处理，反之则执行将△S存入到MCU的存储器(FLASH)中，作为下一次轨道平台动作时新的行进距离误差补偿WS。轨道平台首次运动时行进距离误差补偿WS为0，行进距离误差补偿WS不为零时，MCU控制轨道电机根据预设的行进距离进行运动时要首先进行误差补偿，例如行进距离为100cm，行进距离误差补偿WS为+7cm，则轨道平台运动的距离为93cm。

[0095] 轨道平台运动完成后，云台摄像机开始偏转和俯仰，MCU控制云台电机根据预设的偏转角度175°及预设俯仰角度-30°开始运动，同时云台姿态采集模块的加速度计和陀螺仪采集相关数据，当云台摄像机稳定后处理采集数据，计算出云台摄像机偏转方向的角度值θX和俯仰方向的角度值θY，MCU对比预设偏转角度，计算出对应的偏转角度误差△X＝θX-175，MCU对比预设俯仰角度，计算出对应的俯仰角度误差△Y＝θY+30，MCU开始比较偏转角度误差与允许偏转角度误差值的大小，如果|△X|<2°，则不处理，反之则执行将△X存入到MCU的存储器(FLASH)中，作为下一次云台摄像机动作时新的偏转角度误差补偿WX，MCU比较俯仰角度误差与允许俯仰角度误差值的大小，如果|△Y|<2°，则不处理，反之则执行将△Y存入到MCU的存储器(FLASH)中，作为下一次云台摄像机动作时新的俯仰角度误差补偿WY。

[0096] 轨道平台首次运动时偏转角度误差补偿WX为0，当偏偏转角度误差补偿WX不为零时，MCU控制云台电机根据预设的偏转角度进行运动时要首先进行误差补偿，例如偏转角度为120°，偏转角度误差补偿WX为-9°，则云台摄像机偏转的角度为129°。

[0097] 轨道平台首次运动时偏转角度误差补偿WY为0，当偏转角度误差补偿WY不为零时，MCU控制云台电机根据预设的俯仰角度进行运动时要首先进行误差补偿，例如俯仰角度为-30°，俯仰角度误差补偿WX为-3°，则云台摄像机俯仰的角度为-27°。

[0098] 当时间到9点时，根据前述的步骤运行至位置2，对位置1存在的误差补偿要加入到首次运行预设值中，如果超过允许的误差范围则重新刷新已存储的误差补偿，作为初始误差补偿，10点时，运行至位置3，要综合位置1和位置2的误差补偿做首次误差补偿。

[0099] 每次动作后，MCU根据采集的数据自动计算行进距离误差和偏转角度误差，并根据结果做相应的误差补偿，从而实现自动校正。

[0100] 尽管本发明的内容已经通过上述优选实施例作了详细介绍，但应当认识到上述的描述不应被认为是对本发明的限制。在本领域技术人员阅读了上述内容后，对于本发明的多种修改和替代都将是显而易见的。因此，本发明的保护范围应由所附的权利要求来限定。