CN114339057A - 一种三维定位同步跟踪摄像系统及控制方法 - Google Patents

Abstract

本发明提供了一种三维定位同步跟踪摄像系统，包括：三维定位同步跟踪控制板、摄像装置、三维定位同步跟踪云台装置；三维定位同步跟踪控制板与摄像装置均安装在三维定位同步跟踪云台装置上；进一步的，三维定位同步跟踪云台包括旋转盘、用于驱动旋转盘转动的第一驱动组件，三维定位同步跟踪控制板与摄像装置均通过电机支架安装在旋转盘上；摄像装置通过第二驱动组件转动安装在旋转盘上。本发明所述的一种三维定位同步跟踪摄像系统，具有水平和垂直方向均可360°连续旋转，可变旋转速度且定位精度高，延迟低，同时包含抗震以及IP66的防护等级和稳定性高、成本低的特点。

Description

一种三维定位同步跟踪摄像系统及控制方法

技术领域

本发明属于三维定位同步跟踪视频监控技术领域，尤其是涉及一种三维定位同步跟踪摄像系统及控制方法。

背景技术

核环吊作业区作为环境恶劣的重要行业，对生产作业区域进行严密监控，有效保障生产安全有序进行，及时发现异常状况，将异常进程制止在萌芽状态。

目前市面上大多数的监控项目中，摄像系统会安装在室内或者室外的普通环境，定点观测，所以需要摄像系统具有基础的定速旋转运动。但是由于核环境现场的工作环境的复杂性，以及需要摄像系统自动瞄准观测目标，根据后方传递来的数据信号，跟随目标观测物一起运动，所以对摄像系统提出了更高的要求，可变转速以及高精度的跟踪归位。现有的可360°连续调整视角的摄像系统中，大多数都采用直流电机带动齿轮副的配合，进行摄像系统的旋转工作，且由于直流电机在实际应用时很难进行摄像系统的变速操作，且传统的齿轮副或者涡轮蜗杆传动，在动力传递时会产生噪音以及当由于加工精度不高时，其本身机构会产生一定的震动和冲击，且不便于维修维护。在实际核环境现场使用时，由于使用环境多为振动环境，且随时会跟随被观测物体进行加速减速等运动，且会进行回归初始位置或者回归预设位置的操作。所以急需一款可以进行水平方向和垂直方向均可连续360°旋转的可变速度的高精度摄像系统，防护等级高且抗震动的摄像系统，使作业过程更加高效、安全。

本专利设计解决了以下几个问题：

1、一般云台速度恒定的问题：本系统采用双步进电机的方式，使云台水平和垂直方向均可360°连续旋转，且由于步进电机调速控制较为方便，故解决了一般云台机只能以恒定速度旋转的问题。

2、一般云台机定位精度低：本系统采用同步带传动的传动方式，使传动更加平稳有效，去除齿隙间隙对云台定位精度的影响，并且在云台水平和垂直旋转的机构中增加了光电开关，使记录云台初始位置的信息更为准确，计算预置位的难度大大降低，故提高了云台机的定位精度。

3、一般云台机制造成本高昂：一般云台机的传动形式为齿轮传动，齿轮传动对于加工来说，需要较高精度的机械设备来进行加工，并且加工之后的零部件需要调质或者其他热处理工艺，所以齿轮的制造成本较高，并且齿轮配合在进行装配时要求的装配精度较高，所以需要较高素质的人员进行安装调试，故成本较高。本系统采用同步带传送，同步带轮为基础标准型带轮，同步带也为国标带型，制造和生产组装工艺较为简单，制造以及物料成本较为低廉。

4、解决了一般云台机不能在核辐射环境下使用的问题。由于在核电站环境下存在核辐射，核辐射会影响机芯的正常工作，所以本系统采用不锈钢外筒加铅玻璃的设计方式，最大限度的避免核辐射对摄像机芯的影响。

发明内容

有鉴于此，本发明旨在提出一种三维定位同步跟踪摄像系统，以解决因核环境条件下一般摄像系统的防护能力低和摄像系统本身不能准确的变速跟踪被观测物以及不能准确的回归预置位点的问题。

为达到上述目的，本发明的技术方案是这样实现的：

一种三维定位同步跟踪摄像控制方法，包括以下步骤：

S1、根据跟踪摄像机的三维坐标和被跟踪物体的三维坐标，计算被跟踪物体与跟踪摄像机的单倍图像的焦距；

S2、根据单倍图像的焦距，计算出跟踪摄像机变化的变倍值，然后计算出变倍值对应的变倍电机坐标值；

S3、根据跟踪摄像机当前变倍电机坐标值坐标，计算出预定坐标与当前变倍电机坐标的差值，根据差值计算出云台速度；

S4、根据跟踪摄像机的三维坐标和被跟踪物体的三维坐标，计算得到垂直、水平角度；

S5、根据垂直、水平角度以及云台速度控制云台转动。

进一步的，步骤S1中，计算被跟踪物体与跟踪摄像机的单倍图像的焦距的步骤如下：

设定跟踪摄像机的三维坐标为

、被跟踪物体坐标为

，根据两点距离的公式计算被跟踪物体与跟踪摄像机的距离，两点距离公式如下：

。

进一步的，步骤S2中经过光学变倍公式计算得到摄像机变倍数，其中光学变倍公式如下：

变倍值=A+(△H*Q/F)*(1/Q)

=A+△H/F；

计算出变倍值对应的变倍电机坐标值通过查表方式得到电机坐标值。

进一步的，步骤S3中计算预定坐标的方法包括以下步骤：

设最近3次坐标点，(X₁,Y₁,Z₁)、(X₂,Y₂,Z₂)、(X₃,Y₃,Z₃)，(X₄,Y₄,Z₄)为预定坐标；

三维坐标通过拆分为X、Y、Z轴三组的方式进行预定坐标计算：

已知X₁、X₂、X₃，3个坐标点，求出预定坐标中的X₄，

固定时间间隔为t，V1为X₁X₂之间的速度，V2为X₂X₃之间的速度，V3为需要计算的速度，计算方法：

加速度a=(V2-V1)/t；V3=V2+a*t；

得到速度V3后，进而计算预定坐标中的X₄：X₄=X₃+V3*t；

同理得出：Y₄、Z₄，最终得到预定坐标(X₄,Y₄,Z₄)；

t为固定时间间隔，V1为X₁与X₂之间的速度，V2为X₂与X₃之间的速度，V3为需要计算的速度；

计算云台速度过程包括：根据预定坐标d，与当前实时的坐标m进行差值计算，得到云台电机差值n=d-m，每次坐标获取时间t相同，则得到速度V=n/t，实时计算云台转动速度。

进一步的，步骤S4中垂直、水平角度的计算过程具体如下：

设定AB两坐标点的x轴坐标差值和z轴坐标差值，作为三角形的邻边与对边，用反三角函数计算出水平旋转角度；

利用B坐标点，在以A坐标点为中心，x轴与z轴形成的平面上的投影

,形成以A、B、C为顶点的三角形，先计算出对边BC和邻边AC的长度，再利用反三角函数计算出垂直旋转角度∠BAC。

一种三维定位同步跟踪摄像系统，包括：三维定位同步跟踪控制板、摄像装置、三维定位同步跟踪云台装置；

三维定位同步跟踪控制板与摄像装置均安装在三维定位同步跟踪云台装置上；

三维定位同步跟踪云台包括旋转盘、用于驱动旋转盘转动的第一驱动组件，三维定位同步跟踪控制板与摄像装置均通过电机支架安装在旋转盘上；

摄像装置通过第二驱动组件转动安装在旋转盘上。

进一步的，三维定位同步跟踪云台还包括下固定壳，第一驱动组件安装在下固定壳上，第一驱动组件包括第一电机、第一同步带轮，第一电机安装在下固定壳内，第一电机转轴端贯穿固定壳后与第一同步带轮连接，下固定壳顶部设有转轴，旋转盘安装在转轴上，旋转盘底部安装有第一从动带轮，第一同步带轮与第一从动带轮通过皮带连接；

三维定位同步跟踪云台还包括第一外筒，第一外筒通过螺栓固定在旋转盘上，第一外筒的底部与下固定壳之间安装有轴承。

进一步的，第二驱动组件包括第二电机、第二同步带轮，第二电机通过螺栓固定在电机支架上，第二电机的转轴端与第二同步带轮连接，电机支架上安装有第二从动带轮，第二从动带轮与第二同步带轮通过皮带连接，第二从动带轮连接有第一联轴器，摄像装置外壁安装有第二联轴器，第二联轴器贯穿第一外筒后与第一联轴器相连；

电机支架通过螺栓固定在旋转盘上，三维定位同步跟踪控制板通过螺栓固定在电机支架上，三维定位同步跟踪控制板分别与第一电机、第二电机、摄像装置电性连接；

摄像装置包括第二外筒、第二外筒的一端安装有后盖，另一端安装有遮光套，机芯组合位于第二外筒内，并通过螺栓安装在后盖上，机芯组合上粘贴有铅玻璃，铅玻璃端面与遮光套贴合。

进一步的，三维定位同步跟踪控制板包括电源电路、CPU控制电路、基准检测电路、步进电机控制电路、通信接口电路；

电源电路分别CPU控制电路、基准检测电路、步进电机控制电路、通信接口电路供电；

CPU控制电路用于对基准检测电路、步进电机控制电路、通信接口电路的接收信息或反馈信息。

相对于现有技术，本发明所述的一种三维定位同步跟踪摄像系统及控制方法具有以下有益效果：

（1）本发明所述的一种三维定位同步跟踪摄像系统，具有水平和垂直方向均可360°连续旋转，可变旋转速度且定位精度高，延迟低，同时包含抗震以及IP66的防护等级和稳定性高、成本低的特点。

（2）本发明所述的一种三维定位同步跟踪摄像系统，实现了三维定位同步跟踪摄像系统对被跟踪物体的三维定位同步跟踪，同时兼顾连续运转及平滑变倍。

（3）本发明所述的一种三维定位同步跟踪摄像系统，整体采用不锈钢材质，摄像装置部分使用铅玻璃，摄像系统出线采用低烟无卤线缆，使本系统具有抗辐射能力。

（4）本发明所述的一种三维定位同步跟踪摄像系统，整体采用不锈钢材质，各部位连接紧密，结构性好，提高了云台的整体强度，同时兼顾密封性。可应对多种复杂的外部环境。

（5）本发明所述的一种三维定位同步跟踪摄像系统，云台采用同步带传动，提高了设备整体运行的平稳性，摄像装置部分使用铅玻璃，更加增加了本系统在核环境条件下工作的稳定性。

（6）本发明所述的一种三维定位同步跟踪摄像系统，摄像系统在拥有传统摄像系统的所有功能的前提下，增加了可变速旋转以及提高了定位精度，且选用了成本较为低的传动方式，故降低了成本，增加了性能，同时增加了系统在核环境下工作的可靠性。

附图说明

构成本发明的一部分的附图用来提供对本发明的进一步理解，本发明的示意性实施例及其说明用于解释本发明，并不构成对本发明的不当限定。在附图中：

图1为本发明实施例所述的一种三维定位同步跟踪摄像系统结构图；

图2为本发明实施例所述的摄像装置结构剖视图；

图3为本发明实施例所述的第二外筒结构图；

图4为本发明实施例所述的电源电路图；

图5为本发明实施例所述的CPU控制电路图一；

图6为本发明实施例所述的CPU控制电路图一；

图7为本发明实施例所述的基准检测电路图；

图8为本发明实施例所述的步进电机控制电路图；

图9为本发明实施例所述的通信接口电路图；

图10为本发明实施例所述的三维定位同步跟踪摄像系统控制示意图；

图11为本发明实施例所述的摄像机1倍焦距时对应被跟踪物体距离示意图；

图12为本发明实施例所述的三维坐标示意图。

附图标记说明：

1、三维定位同步跟踪云台装置；2、摄像装置； 3、第一同步带轮；4、第一从动带轮；5、转轴；6、旋转盘；7、电机支架；8、第一外筒；9、上盖；10、第一联轴器；11、三维定位同步跟踪控制板；12、轴承；101、下固定壳；102、第一电机；13、第二同步带轮；14、第二从动带轮；21、后盖；22、第二外筒；23、机芯组合；24、遮光套；25、铅玻璃；26、第二联轴器。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。

在本发明的描述中，需要理解的是，术语“中心”、“纵向”、“横向”、“上”、“下”、“前”、“后”、“左”、“右”、“竖直”、“水平”、“顶”、“底”、“内”、“外”等指示的方位或位置关系为基于附图所示的方位或位置关系，仅是为了便于描述本发明和简化描述，而不是指示或暗示所指的装置或元件必须具有特定的方位、以特定的方位构造和操作，因此不能理解为对本发明的限制。此外，术语“第一”、“第二”等仅用于描述目的，而不能理解为指示或暗示相对重要性或者隐含指明所指示的技术特征的数量。由此，限定有“第一”、“第二”等的特征可以明示或者隐含地包括一个或者更多个该特征。在本发明的描述中，除非另有说明，“多个”的含义是两个或两个以上。

在本发明的描述中，需要说明的是，除非另有明确的规定和限定，术语“安装”、“相连”、“连接”应做广义理解，例如，可以是固定连接，也可以是可拆卸连接，或一体地连接；可以是机械连接，也可以是电连接；可以是直接相连，也可以通过中间媒介间接相连，可以是两个元件内部的连通。对于本领域的普通技术人员而言，可以通过具体情况理解上述术语在本发明中的具体含义。

下面将参考附图并结合实施例来详细说明本发明。

实施例1

一种三维定位同步跟踪摄像系统，包括：三维定位同步跟踪控制板11、摄像装置2、三维定位同步跟踪云台装置1；

三维定位同步跟踪控制板11与摄像装置2均安装在三维定位同步跟踪云台装置1上；

三维定位同步跟踪云台包括旋转盘6、用于驱动旋转盘6转动的第一驱动组件，三维定位同步跟踪控制板11与摄像装置2均通过电机支架7安装在旋转盘6上；

摄像装置2通过第二驱动组件转动安装在旋转盘6上。

三维定位同步跟踪云台还包括下固定壳101，第一驱动组件安装在下固定壳101上，第一驱动组件包括第一电机102、第一同步带轮3，第一电机102安装在下固定壳101内，第一电机102转轴5端贯穿固定壳后与第一同步带轮3连接，下固定壳101顶部设有转轴5，旋转盘6安装在转轴5上，旋转盘6底部安装有第一从动带轮4，第一同步带轮3与第一从动带轮4通过皮带连接。

如图1所示，第一电机102驱动第一同步带轮3转动，第一同步带轮3带动第一从动带轮4转动，第一从动带轮4带动旋转盘6转动。

三维定位同步跟踪云台还包括第一外筒8，第一外筒8通过螺栓固定在旋转盘6上，第一外筒8的底部与下固定壳101之间安装有轴承12。

如图1所示，旋转盘6转动时，带动第一外筒8转动，第一外筒8通过螺栓固定在旋转盘6上，为了保持第一外筒8的稳定，以及使第一外筒8内部形成封闭空间，因此第一外筒8底部与下固定壳101之间安装轴承12，第一外筒8的顶部设有上盖9，使第一外筒8内部安装在旋转盘6上的零件起到保护作用，上盖9与第一外筒8之间可进行拆卸，便于对第一外筒8内部的零件进行维修，以及便于第一外筒8的安装及拆卸。

第二驱动组件包括第二电机、第二同步带轮13，第二电机通过螺栓固定在电机支架7上，第二电机的转轴5端与第二同步带轮13连接，电机支架7上安装有第二从动带轮14，第二从动带轮14与第二同步带轮13通过皮带连接，第二从动带轮14连接有第一联轴器10，摄像装置2外壁安装有第二联轴器26，第二联轴器26贯穿第一外筒8后与第一联轴器10相连。

如图1和图3所示，通过第一电机102驱动旋转盘6带动第一外筒8及安装在旋转盘6上的零件实现360度旋转，摄像装置2安装在外筒上，因此通过外筒带动摄像装置2以自身为基准水平360度旋转，通过第二电机驱动第二同步带轮13转动，第二同步带轮13带动第二从动带轮14转动，第二从动带轮14带动第一联轴器10转动，第一联轴器10带动第二联轴器26转动，第二联轴器26与摄像装置2一体，因此实现了以摄像装置2为准的垂直360旋转。

电机支架7通过螺栓固定在旋转盘6上，三维定位同步跟踪控制板11通过螺栓固定在电机支架7上，三维定位同步跟踪控制板11分别与第一电机102、第二电机、摄像装置2电性连接。

三维定位同步跟踪控制板11用于控制两个电机的运转，可实现控制摄像装置2水平及垂直方向的转动，以及用于控制摄像装置2的运行动作，并为整个系统供电。

摄像装置2包括第二外筒22、第二外筒22的一端安装有后盖21，另一端安装有遮光套24，机芯组合23位于第二外筒22内，并通过螺栓安装在后盖21上，机芯组合23上粘贴有铅玻璃25，铅玻璃25端面与遮光套24贴合。

如图2所示，机芯组合23采用摄像机内部摄像组件，均采用现有技术，此处不再详细介绍，机芯组件通过螺栓固定在第二外套筒内，与摄像头结构类似。

如图4至图8所述，三维定位同步跟踪控制板11包括电源电路、CPU控制电路、基准检测电路、步进电机控制电路、通信接口电路；

电源电路分别CPU控制电路、基准检测电路、步进电机控制电路、通信接口电路供电；

CPU控制电路用于对基准检测电路、步进电机控制电路、通信接口电路的接收信息或反馈信息。

电源电路为整个系统供电；CPU控制电路将接收到包含三维坐标的PLC指令转换为三维定位同步跟踪摄像系统的控制指令；通信接口电路将TTL信号和RS485信号相互转换，并具有过压过流保护功能；基准检测电路将采集基准信号用于判断是否到达基准点，用于0点基准校准；步进电机控制电路用于步进电机控制，进而控制云台转动到指定角度。

电源电路输入电压AC24V，为整个系统提供DC5V和DC3.3V电压。输入端加入自恢复保险，进行过流保护。

CPU控制电路以STM32F407VGT6单片机作为处理器，接收外部PLC控制模块的RS485控制信号，根据PLC提供的三维定位同步跟踪摄像系统的三维坐标和被跟踪物体的三维坐标，经过计算得到垂直、水平角度，通过控制步进电机进行角度变化；以及被跟踪物体与相机的距离，根据单倍图像的焦距，计算出应该变化的倍数，然后计算出变倍数对应的变倍电机坐标值；通过采集相机当前坐标计算出预定坐标与当前坐标的差值，计算出需要选择的云台速度，一起发送给相机，进行变倍处理，进而实现被跟踪物体运行与相机变倍同步且连续。

通信接口电路实现RS485信号和TTL信号相互转换。接口设置自恢复保险与TVS二极管SMBJ5.0A组成的保护电路，能够承受AC48V的误接电压。

基准检测电路实现0点基准校准，三维定位同步跟踪摄像系统开机后，会进行自检，通过检测水平和垂直2个基准点，最终进行校准。

步进电机控制电路用于控制步进电机，当三维坐标变化时，云台角度需要进行变化，通过控制步进电机转动，进而使云台达到对应角度。

如图5、图6所示，CPU控制电路包括主控芯片U5芯片，指示灯子电路、下载端口芯片P8、电容滤波子电路、升压子电路、晶振子电路，指示灯子电路包括发光二极管D11、发光二极管D12、发光二极管D13，发光二极管D11一端连接有电阻R44、电阻R44的另一端与3.3V电源连接，电阻R44的另一端设有LED_SYSTEM接口，LED_SYSTEM接口与U5芯片的81引脚连接，发光二极管D12一端连接有电阻R45、电阻R45的另一端与3.3V电源连接，电阻R45的另一端设有LED_DATA接口，LED_DATA接口与U5芯片的82引脚连接，发光二极管D13一端连接有电阻R49、电阻R49的另一端与3.3V电源连接，电阻R49的另一端设有LED_ERROR接口，LED_ERROR接口与U5芯片的84引脚连接，U5芯片的14引脚连接有复位子电路，复位子电路包括：电阻R57、电容C19，电阻R57一端与电容C19一端连接，电容C19的另一端接地，电容C57的另一端连接3.3V电源；

下载端口芯片P8的2引脚设有SWCLK接口，SWCLK接口与U5芯片的76引脚连接，下载端口芯片P8的2引脚还连接有电阻R60，电阻R60的另一端接地；

下载端口芯片P8的4引脚设有SWDIO接口，SWDIO接口与U5芯片的72引脚连接，端口芯片P8的4引脚还连接有电阻R61，电阻R61的另一端连接3.3V电源；下载端口芯片P8的6引脚接地，10引脚哭啊不会饿5V电源；

电容滤波子电路包括电容C10、电容C11、电容C12、电容C13、电容C14、电容C15、电容C16，七个电容均相互并联，电容C16的一端连接U5芯片的3.3V电源，另一端接地；

升压子电路BOOT包括电阻R56、电阻R59，电阻R56的一端与电阻R59的一端连接，电阻R56的另一端接地，电阻R56的另一端设有BOOT0接口，BOOT0接口与U5芯片的94引脚连接，电阻R59的另一端设有BOOT1接口，BOOT1接口与U5芯片的37引脚连接；

晶振子电路包括压电晶体X1、电容C20、电容C23，电容C20一端与压电晶体X1的一端连接，电容C20的另一端与电容C23的另一端连接，电容C23的另一端与压电晶体X1的另一端连接，压电晶体X1的两端分别设有OSC_IN接口、OSC_OUT接口，OSC_IN接口与U5芯片的12引脚连接，OSC_OUT接口与U5芯片的13引脚连接。

如图7所示，基准检测电路包括：SA1芯片、SA2芯片，SA1芯片的1引脚通过电阻R14与电源模块5V电源端连接，SA1芯片的2引脚与电阻R14的一端连接，SA1芯片的3引脚与4引脚均接地，4引脚与接地端串联有电阻R12,4引脚上连接有电阻R10，电阻R10的另一端延伸有L_SET1接口，L_SET2接口与CPU控制电路连接；

SA2芯片的1引脚通过电阻R22与电源模块5V电源端连接，SA2芯片的2引脚与电阻R22的一端连接，SA2芯片的3引脚与4引脚均接地，4引脚与接地端串联有电阻R18,4引脚上连接有电阻R17，电阻R17的另一端延伸有L_SET2接口，L_SET2接口与CPU控制电路连接。

如图8所示，步进电机控制电路包括P4芯片、P6芯片，P4芯片的1引脚连接有电阻R16、晶体管Q2，电阻R16的一端与晶体管Q2的集电极连接，电阻R16的另一端连接5V电源，晶体管Q2的发射极接地，晶体管Q2的基集连接有电阻R23，电阻R23的另一端连接有电阻R19，电阻R19的另一端连接电阻R16，电阻R23的另一端还设有MOTOR1_DIR接口，MOTOR1_DIR接口与U5芯片的43引脚，P4芯片的2引脚连接有晶体管Q1、电阻R7，电阻R7的一端与晶体管Q1的集电极连接，电阻R7的另一端连接5V电源，晶体管Q1的发射极接地，晶体管Q1的基集连接电阻R13，电阻R13的另一端连接有电阻R9，电阻R9的另一端与电阻R7的另一端连接，电阻R13的一端设有接口MOTOR1_CP接口，MOTOR1_CP接口与U5芯片的44引脚连接；

P6芯片的1引脚连接有电阻R34、晶体管Q4，电阻R34的一端与晶体管Q4的集电极连接，电阻R34的另一端连接5V电源，晶体管Q4的发射极接地，晶体管Q4的基集连接有电阻R37，电阻R37的另一端连接有电阻R35，电阻R35的另一端连接电阻R34，电阻R37的另一端还设有MOTOR2_DIR接口，MOTOR2_DIR接口与U5芯片的45引脚，P6芯片的2引脚连接有晶体管Q3、电阻R24，电阻R24的一端与晶体管Q3的集电极连接，电阻R24的另一端连接5V电源，晶体管Q3的发射极接地，晶体管Q3的基集连接电阻R29，电阻R29的另一端连接有电阻R27，电阻R27的另一端与电阻R24的另一端连接，电阻R29的一端设有接口MOTOR2_CP接口，MOTOR2_CP接口与U5芯片的46引脚连接。

如图9所示，通信接口电路包括485接口、TTL接口，485接口包括U3芯片、P3芯片，P3芯片的1引脚与U3芯片的6引脚连接，P3芯片的1引脚与U3芯片的6引脚之间还串联有熔断器F3，P3芯片的2引脚与U3芯片的7引脚连接，P3芯片的2引脚与U3芯片的7引脚之间串联有熔断器F2，P3芯片的2引脚与U3芯片的7引脚之间连接有二极管D6，二极管D6的另一端连接在P3芯片的1引脚与U3芯片的6引脚之间的线路上，P3芯片的2引脚与U3芯片的7引脚之间连接有二极管D5，二极管D5的另一端连接有二极管D7，二极管D7的另一端连接在P3芯片的1引脚与U3芯片的6引脚之间的线路上，二极管D5与二极管D7之间接地；

U3芯片的1引脚连接有电阻R6，电阻R6的另一端设有USART1_RX接口，USART1_RX接口与U5芯片的69引脚连接，U3芯片的2引脚与3引脚连接，3引脚连接有电阻R8，电阻R8的另一端设有USART1_DIR接口，USART1_DIR接口与U5芯片的89引脚连接，U3芯片的4引脚连接有电阻R11，电阻R11的另一端设有接口USARTA1_TX接口，USARTA1_TX接口与U5芯片的68引脚连接；U3芯片的8引脚连接5V电源，5引脚接地；

TTL接口包括P7芯片，P7芯片的1引脚连接3.3V电源，P7芯片的2引脚连接有电阻R31，电阻R31的另一端设有接口USART3_RX接口，USART3_RX接口与U5芯片的56引脚连接，P7芯片的3引脚连接有电阻R30，电阻R30的另一端设有USART3_TX接口，USART3_TX接口与U5芯片的47引脚连接，P7芯片的4引脚接地。

如图4所示，电源电路模块包括P1芯片、P2芯片、BD1芯片、U1芯片、U2芯片，BD1芯片的1引脚与U1芯片的1引脚连接，BD1芯片的2引脚与P1芯片1引脚连接，BD1芯片的3引脚与P1芯片2引脚连接，BD1芯片的3引脚与P1芯片2引脚之间串联有熔断器F1，BD1芯片的2引脚还与P2芯片的1引脚连接，BD1芯片的3引脚还与P2芯片的2引脚连接；

BD1芯片的1引脚与U1芯片的1引脚之间连接有电容C1、电容C2、电容C3、电容C4，每个电容的另一端均接地，BD1芯片的1引脚与U1芯片的1引脚之间连接有电阻R3，电阻R3另一端连接有二极管D2，二极管D2的另一端接地；

U1芯片的2引脚与U2芯片的3引脚连接，U1芯片的2引脚与U2芯片的3引脚之间连接有串联有电感L1、电阻R62，电感L1与电阻R62之间连接有电容C5、电容C6、电容C8、电阻R4，三个电容的另一端均接地，电阻R4的另一端连接有二极管D3，二极管D3的另一端接地；

电感L1的另一端连接有二极管D1，二极管D1的另一端接地，电感L1的一端连接有电阻R2，电阻R2的另一端与U1芯片的4引脚连接，U1芯片的4引脚连接有电阻R1，电阻R1的另一端接地；

U2芯片的1引脚接地，U2芯片的2、4引脚相连，U2芯片的2引脚连接有电阻R63、电阻R63的另一端延伸有3.3V接线端；电阻R63与2引脚之间连接有电容C7、电容C9、电阻R5，两个电容的另一端接地，电阻R5的另一端连接有二极管D4，二极管D4的另一端接地U2芯片的3引脚延伸有5.5V电源接口。

实施例2

如图10至图12所示，一种三维定位同步跟踪摄像控制方法，包括以下步骤：

S1、根据跟踪摄像机的三维坐标和被跟踪物体的三维坐标，计算被跟踪物体与跟踪摄像机的单倍图像的焦距；

S2、根据单倍图像的焦距，计算出跟踪摄像机变化的变倍值，然后计算出变倍值对应的变倍电机坐标值；

S3、根据跟踪摄像机当前变倍电机坐标值坐标，计算出预定坐标与当前变倍电机坐标的差值，根据差值计算出云台速度；

S4、根据跟踪摄像机的三维坐标和被跟踪物体的三维坐标，计算得到垂直、水平角度；

S5、根据垂直、水平角度以及云台速度控制云台转动。

步骤S1中计算被跟踪物体与跟踪摄像机的单倍图像的焦距具体包括以下过程：

设定跟踪摄像机的三维坐标为

、被跟踪物体坐标为

，根据两点距离的公式计算被跟踪物体与跟踪摄像机的距离，两点距离公式如下：

。

步骤S2中经过光学变倍公式计算得到摄像机变倍数，其中光学变倍公式如下：

变倍值=A+(△H*Q/F)*(1/Q)

=A+△H/F；

计算出变倍值对应的变倍电机坐标值通过查表方式得到电机坐标值。

步骤S3中计算预定坐标的方法包括以下步骤：

设最近3次坐标点，(X₁,Y₁,Z₁)、(X₂,Y₂,Z₂)、(X₃,Y₃,Z₃)，(X₄,Y₄,Z₄)为预定坐标；

三维坐标通过拆分为X、Y、Z轴三组的方式进行预定坐标计算：

已知X₁、X₂、X₃，3个坐标点，求出预定坐标中的X₄，

固定时间间隔为t，V1为X₁X₂之间的速度，V2为X₂X₃之间的速度，V3为需要计算的速度，计算方法：

加速度a=(V2-V1)/t；V3=V2+a*t；

得到速度V3后，进而计算预定坐标中的X₄：X₄=X₃+V3*t；

同理得出：Y₄、Z₄，最终得到预定坐标(X₄,Y₄,Z₄)；

t为固定时间间隔，V1为X₁与X₂之间的速度，V2为X₂与X₃之间的速度，V3为需要计算的速度；

计算云台速度过程包括：根据预定坐标d，与当前实时的坐标m进行差值计算，得到云台电机差值n=d-m，每次坐标获取时间t相同，则得到速度V=n/t，实时计算云台转动速度。

如：速度5°/s。则让云台电机以此速度进行运转。

步骤S4中垂直、水平角度的计算过程具体如下：

AB两点的x轴坐标差值和z轴坐标差值，作为三角形的邻边与对边，用反三角函数计算出水平旋转角度；

利用B坐标点，在以A坐标点为中心，x轴与z轴形成的平面上的投影

,形成以A、B、C为顶点的三角形，先计算出对边BC和邻边AC的长度，再利用反三角函数计算出垂直旋转角度∠BAC。

三维定位同步跟踪摄像系统具体实现方式：

如图10至图12所示，自动跟踪云台，上电启动后，会通过检测云台内部定位机构，进行复位操作。同时通过定位机构，及时修正在运行过程中可能出现的偏差，保证每次控制都精确定位，减少误差。

针对核环吊，设备运行过程中，摄像机的绝对坐标会随之改变，故需要PLC提供两个跟踪摄像机的三维坐标，及被跟踪物体的三维坐标。

A、摄像机变倍过程

假设三维定位同步跟踪摄像系统坐标为

、被跟踪物体坐标为

，计算两点之间的距离

。

得到两点距离d后，用此距离d配合被跟踪物体尺寸，经过光学变倍公式计算得到对应变倍数N，进而转化成摄像机变倍指令，控制摄像机变倍到对应倍数。同时根据PLC传送的物体运行速度，对摄像机变倍速度实时进行控制，使变化更为流畅。

被跟踪物体同步连续追踪功能，经过测试，得到被跟踪物体的运行与相机变倍的规律：

a、被跟踪物体与相机的距离和相机变倍值成正比；

b、被跟踪物体运行速度（m/s）与相机变倍速度（倍/s）成正比。

根据上述规律，当相机处于1倍变倍视角为60°，被跟踪物体长11.54m，图像正好看到被跟踪物体边缘的时候，物距(物体距相机的距离)为(11.54/2)*tan60°≈10m，被跟踪物体与相机的直线距离每增加10m，相机变倍数增加1倍，可使图像刚好观测到被跟踪物体边缘(参见附图3)。

被跟踪物体运行速度采样频率为Q(Hz)，当前相机变倍倍数为A倍，实际单倍焦距为F(m)，根据相邻两次被跟踪物体高度差△H(m)计算被跟踪物体运行速度为△H*Q(m/s)，得出相机变倍速度调整为△H*Q/F(倍/s)，此时被跟踪物体运行速度与相机变倍速度同步。

同时采用相机变倍位置预判的机制，相机以△H*Q/F(倍/s)的变倍速度，得出下一个采样周期的目标变倍值为：

变倍值=A+(△H*Q/F)*(1/Q)

=A+△H/F；

得到变倍值后，通过例如表1查表的方式对电机坐标值进行确认，进而控制摄像机进行变倍，进而实现自动跟踪中的变倍跟踪；

表1变倍电机坐标值：

在被跟踪物体运次过程中系统实时读取相机的实际变倍位置，以确保相机变倍的准确性。

在被跟踪物体升降运行换向时，设置一个30cm的阈值，当被跟踪物体反向运行时，运行距离小于30cm时，摄像机不跟踪，避免被跟踪物体在上下小幅调整时图像往复抖动给人眼造成不适感。

内部程序根据以上算法，在各种被跟踪物体运行速度下可实时调整变倍速度，同时改变相机变焦电机驱动坐标位置值从而调整相机变倍位置，实现摄像机同步连续追踪被跟踪物体的目的，并实时读取相机的实际变倍位置形成闭环机制，保证系统运行的稳定性。

B、云台转动过程：

对于云台转动，采取转向分解，分为两个轴向，分别为水平和垂直轴向。水平、垂直旋转角度：360°。

用AB两点的x轴坐标差值和z轴坐标差值，作为三角形的邻边与对边，用反三角函数计算出水平旋转角度。

利用B点，在以A点为中心，x轴与z轴形成的平面上的投影

，形成以A、B、C为顶点的三角形，先计算出对边BC和邻边AC的长度，再利用反三角函数计算出垂直旋转角度∠BAC。

得到两个轴向对应角度后，通过主控板，对轴向电机进行控制，转到对应角度，同时通过被跟踪物体的运行速度，对云台的转动速度进行实时控制，使云台转动过程流畅。

具体结构如下：

本系统接入外部电源，为电源电路供电，电源电路为整个系统供电，第一电机102驱动第一同步带轮 3转动，第一同步带轮 3带动第一从动带轮4转动，第一从动带轮4转动带动旋转盘6转动，旋转盘6带动第一外筒8转动，第二电机及摄像装置2安装在旋转盘6上，因此在旋转盘6与第一外筒8转动的情况下带动摄像装置2水平旋转360度，第二电机的电机轴带动第二同步带轮13转动，第二同步带轮13带动第二从动带轮14，第二从动带轮14带动摄像装置2水平360度旋转，因此摄像装置2可实现水平及垂直的360度旋转。

以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (9)

1.一种三维定位同步跟踪摄像控制方法，其特征在于包括以下步骤：

S1、根据跟踪摄像机的三维坐标和被跟踪物体的三维坐标，计算被跟踪物体与跟踪摄像机的单倍图像的焦距；

S2、根据单倍图像的焦距，计算出跟踪摄像机变化的变倍值，然后计算出变倍值对应的变倍电机坐标值；

S3、根据跟踪摄像机当前变倍电机坐标值坐标，计算出预定坐标与当前变倍电机坐标的差值，根据差值计算出云台速度；

S4、根据跟踪摄像机的三维坐标和被跟踪物体的三维坐标，计算得到垂直、水平角度；

S5、根据垂直、水平角度以及云台速度控制云台转动。

2.根据权利要求1所述的一种三维定位同步跟踪摄像控制方法，其特征在于：步骤S1中，计算被跟踪物体与跟踪摄像机的单倍图像的焦距的步骤如下：

设定跟踪摄像机的三维坐标为

、被跟踪物体坐标为

，根据两点距离的公式计算被跟踪物体与跟踪摄像机的距离，两点距离公式如下：

。

3.根据权利要求1所述的一种三维定位同步跟踪摄像控制方法，其特征在于：步骤S2中经过光学变倍公式计算得到摄像机变倍数，其中光学变倍公式如下：

变倍值=A+(△H*Q/F)*(1/Q)

=A+△H/F；

计算出变倍值对应的变倍电机坐标值通过查表方式得到电机坐标值。

4.根据权利要求1所述的一种三维定位同步跟踪摄像控制方法，其特征在于：步骤S3中计算预定坐标的方法包括以下步骤：

设最近3次坐标点，(X₁,Y₁,Z₁)、(X₂,Y₂,Z₂)、(X₃,Y₃,Z₃)，(X₄,Y₄,Z₄)为预定坐标，

三维坐标通过拆分为X、Y、Z轴三组的方式进行预定坐标计算：

已知X₁、X₂、X₃，3个坐标点，求出预定坐标中的X₄，

固定时间间隔为t，V1为X₁X₂之间的速度，V2为X₂X₃之间的速度，V3为需要计算的速度，计算方法：

加速度a=(V2-V1)/t；V3=V2+a*t；

得到速度V3后，进而计算预定坐标中的X₄：X₄=X₃+V3*t；

同理得出：Y₄、Z₄，最终得到预定坐标(X₄,Y₄,Z₄)；

t为固定时间间隔，V1为X₁与X₂之间的速度，V2为X₂与X₃之间的速度，V3为需要计算的速度；

计算云台速度过程包括：根据预定坐标d，与当前实时的坐标m进行差值计算，得到云台电机差值n=d-m，每次坐标获取时间t相同，则得到速度V=n/t，实时计算云台转动速度。

5.根据权利要求1所述的一种三维定位同步跟踪摄像控制方法，其特征在于：步骤S4中垂直、水平角度的计算过程具体如下：

设定AB两坐标点的x轴坐标差值和z轴坐标差值，作为三角形的邻边与对边，用反三角函数计算出水平旋转角度；

利用B坐标点，在以A坐标点为中心，x轴与z轴形成的平面上的投影C(x2，y1，z2)，形成以A、B、C为顶点的三角形，先计算出对边BC和邻边AC的长度，再利用反三角函数计算出垂直旋转角度∠BAC。

6.一种三维定位同步跟踪摄像系统，其特征在于包括：三维定位同步跟踪控制板(11)、摄像装置(2)、三维定位同步跟踪云台装置(1)；

三维定位同步跟踪控制板(11)与摄像装置(2)均安装在三维定位同步跟踪云台装置(1)上；

三维定位同步跟踪云台包括旋转盘(6)、用于驱动旋转盘(6)转动的第一驱动组件，三维定位同步跟踪控制板(11)与摄像装置(2)均通过电机支架(7)安装在旋转盘(6)上；

摄像装置(2)通过第二驱动组件转动安装在旋转盘(6)上。

7.根据权利要求6所述的一种三维定位同步跟踪摄像系统，其特征在于：三维定位同步跟踪云台还包括下固定壳(101)，第一驱动组件安装在下固定壳(101)上，第一驱动组件包括第一电机(102)、第一同步带轮(3)，第一电机(102)安装在下固定壳(101)内，第一电机(102)转轴(5)端贯穿固定壳后与第一同步带轮(3)连接，下固定壳(101)顶部设有转轴(5)，旋转盘(6)安装在转轴(5)上，旋转盘(6)底部安装有第一从动带轮(4)，第一同步带轮(3)与第一从动带轮(4)通过皮带连接；

三维定位同步跟踪云台还包括第一外筒(8)，第一外筒(8)通过螺栓固定在旋转盘(6)上，第一外筒(8)的底部与下固定壳(101)之间安装有轴承(12)。

8.根据权利要求6所述的一种三维定位同步跟踪摄像系统，其特征在于：第二驱动组件包括第二电机、第二同步带轮(13)，第二电机通过螺栓固定在电机支架(7)上，第二电机的转轴(5)端与第二同步带轮(13)连接，电机支架(7)上安装有第二从动带轮(14)，第二从动带轮(14)与第二同步带轮(13)通过皮带连接，第二从动带轮(14)连接有第一联轴器(10)，摄像装置(2)外壁安装有第二联轴器(26)，第二联轴器(26)贯穿第一外筒(8)后与第一联轴器(10)相连；

电机支架(7)通过螺栓固定在旋转盘(6)上，三维定位同步跟踪控制板(11)通过螺栓固定在电机支架(7)上，三维定位同步跟踪控制板(11)分别与第一电机(102)、第二电机、摄像装置(2)电性连接；

摄像装置(2)包括第二外筒(22)、第二外筒(22)的一端安装有后盖(21)，另一端安装有遮光套(24)，机芯组合(23)位于第二外筒(22)内，并通过螺栓安装在后盖(21)上，机芯组合(23)上粘贴有铅玻璃(25)，铅玻璃(25)端面与遮光套(24)贴合。

9.根据权利要求6所述的一种三维定位同步跟踪摄像系统，其特征在于：三维定位同步跟踪控制板包括电源电路、CPU控制电路、基准检测电路、步进电机控制电路、通信接口电路；

电源电路分别CPU控制电路、基准检测电路、步进电机控制电路、通信接口电路供电；

CPU控制电路用于对基准检测电路、步进电机控制电路、通信接口电路的接收信息或反馈信息。

Images