阅读说明：本技术 一种基线可变的双目云台的控制方法 (Control method of binocular tripod head with variable base line ) 是由 纪刚 柴琦 蒋云飞 朱慧 杨丰拓 安帅 于 2020-04-03 设计创作，主要内容包括：本发明公开了一种基线可变的双目云台的控制方法,包括如下步骤：获取双目云台所在机体的姿态；进行图像对比获取目标点的像素偏移量,从而得到双目云台在俯仰和水平旋转方向的绝对调整角度；正逆运动学解算获取双目云台的调整量；建立关于基线的系统运动方程；通过模糊自适应PID控制驱动双目云台调整姿态。本发明所公开的控制方法在基线发生变化时,通过模糊自适应PID控制方法可快速准确地调整双目云台的航向角和俯仰角,实现了对运动目标的精确追踪。(The invention discloses a control method of a binocular tripod head with a variable baseline, which comprises the following steps: acquiring the posture of a machine body where a binocular pan-tilt is located; carrying out image comparison to obtain the pixel offset of a target point, thereby obtaining the absolute adjustment angle of the binocular head in the pitching and horizontal rotating directions; solving and obtaining the adjustment quantity of the binocular head by forward and inverse kinematics; establishing a system equation of motion with respect to a baseline; and driving the binocular head to adjust the attitude through fuzzy self-adaptive PID control. According to the control method disclosed by the invention, when the base line changes, the course angle and the pitch angle of the binocular head can be quickly and accurately adjusted through the fuzzy self-adaptive PID control method, and the accurate tracking of the moving target is realized.)

一种基线可变的双目云台的控制方法

技术领域

本发明涉及一种基线可变的双目云台的控制方法。

背景技术

双目云台利用视差原理，能够从两张平面图像中得到立体的信息，构建三维立体视图。为了构建完整的三维地图，双目云台往往需要固定在移动的机体上，并根据机体的移动调整自身的运动，实现在移动中对移动目标的跟随。

但目前双目云台一般采用定基线的方式，两相机之间的距离固定，这种成像方式存在一个较大的缺陷，生成的立体图像只在部分区域可以获得清晰的立体效果。

目前，可变基线双目相机可以解决只有局部区域图像清晰的问题，通过调整基线长度，使得在较大距离范围的拍摄对象都能呈现较好的三位立体效果。

但是随着基线的变动，两台相机间的距离会发生改变，双目云台的转动惯量也会随之改变，对于传统的PID控制方法，一次设定PID参数的方法并不能很好的适应系统运动参数发生改变的情况，动态特性不够理想，云台的运动可能会出现不平稳的现象，不能快速调整到位，导致获得的图像质量不好，或者不能快速获得清晰正确的三维立体影像。

发明内容

为解决上述技术问题，本发明提供了一种基线可变的双目云台的控制方法，以达到在基线长度改变时，保证双目云台运动的准确性和快速性的目的。

为达到上述目的，本发明的技术方案如下：

一种基线可变的双目云台的控制方法，包括如下步骤：

步骤一，获取双目云台所在机体的姿态；

步骤二，进行图像对比获取目标点的像素偏移量，从而得到双目云台在俯仰和水平旋转方向的绝对调整角度；

步骤三，正逆运动学解算获取双目云台的调整量；

步骤四，建立关于基线的系统运动方程；

步骤五，通过模糊自适应PID控制驱动双目云台调整姿态。

上述方案中，步骤一具体方法如下：

(1)双目云台安装在可移动的机体上，通过传感器获取机体当前的姿态四元数Q＝[q i j k]^T，四元数由一个实数q和三个虚数i、j、k组成，三个虚数分别表征绕三个轴的旋转；

(2)根据机体的姿态四元数获取机体的旋转矩阵

将旋转矩阵R_d表示为

(3)根据旋转矩阵获取当前机体的欧拉角Ω，得到被控姿态；

机体当前状态的欧拉角为 为机体航向角，β_d为机体俯仰角，α_d为机体横滚角；

上述方案中，步骤二具体方法如下：

通过目标识别算法识别出目标在相机图像中的像素位置，并对该目标点的像素位置变化进行监控，当目标点处于相机追踪框内时，不产生双目云台运动指令；当目标点超出相机追踪框时，根据目标点的像素位置与画幅中心点间像素位置的像素差(Δu，Δv)，得到双目云台分别在俯仰和水平旋转方向的绝对调整角度Δβ和

其中，f为相机的焦距。

上述方案中，步骤三具体方法如下：

(1)对世界坐标系的坐标x_w，y_w，z_w进行变换，得到机体坐标系的坐标x_d，y_d，z_d；

(2)对机体坐标系的坐标x_d，y_d，z_d进行变换，得到相机坐标系的坐标x_c，y_c，z_c；

(3)对相机坐标系的坐标x_c，y_c，z_c进行变换，得到图像物理坐标系的坐标x，y；

(4)对图像物理坐标系的坐标x，y进行变换，得到图像像素坐标系的坐标u，v，从而得到世界坐标系的坐标x_w，y_w，z_w到图像像素坐标系的坐标u，v的正运动学变换公式；

(5)对正运动学变换公式两端取逆，得到图像像素坐标系的坐标u，v到世界坐标系的坐标x_w，y_w，z_w的逆运动学变换公式；

(6)将目标点的像素位置与画幅中心点间像素位置的像素差(Δu，Δv)和双目云台分别在俯仰和水平旋转方向的绝对调整角度Δβ和代入逆运动学变换公式，得到双目云台的相对俯仰角和航向角的角度改变量分别为β′_c，从而使目标回到相机画幅中心，完成追踪。

上述方案中，步骤三具体方法如下：

(1)对世界坐标系的坐标x_w，y_w，z_w进行变换的公式如下：

其中，

机体坐标系为直角坐标系，Z_d轴作为机体横滚轴，表征机体的横滚运动；X_d轴作为机体俯仰轴，表征机体的俯仰运动；Y_d轴作为机体航向轴，表征机体的水平旋转运动；R_d为世界坐标系到机体坐标系的旋转矩阵，t_d表示世界坐标系到机体坐标系的平移，tx_d，ty_d，tz_d分别代表机体坐标系原点相对于世界坐标系原点在x_w，y_w，z_w方向上的平移量，β_d，θ_d表示机体的航向角，俯仰角和横滚角，由安装在机体上的位移传感器获得；

(2)对机体坐标系的坐标x_d，y_d，z_d进行变换的公式如下：

其中，

R_c为机体坐标系到相机坐标系旋转矩阵，t_c表示机体坐标系到相机坐标系的平移，t_xc，t_yc，t_zc分别代表相机坐标系原点相对于机体坐标系原点在x_d，y_d，z_d方向上的平移量，由双目云台在机体上的相对安装位置获得；β_c，θ_c分别表示相机的航向角，俯仰角和横滚角，通过角度传感器获得；此处因为双目云台不能进行翻转，所以相对于机体的翻滚角θ_c＝0；

(3)对相机坐标系的坐标x_c，y_c，z_c进行变换的公式如下：

其中，z_c为目标到相机的垂直距离，f为相机焦距；

(4)对图像物理坐标系的坐标x，y进行变换的公式如下：

其中，

dx，dy分别为图像像素点在图像物理坐标系x，y轴上的物理尺寸，γ为相机的扭曲因子；

将公式(8)(9)(10)(11)整合，得到从世界坐标系的坐标x_w，y_w，z_w到图像像素坐标系的坐标u，v的正运动学变换公式：

对公式(12)两端取逆，得到从图像像素坐标系的坐标u，v到世界坐标系的坐标x_w，y_w，z_w的逆运动学变公式：

(5)将目标点的像素位置与画幅中心点间像素位置的像素差(Δu，Δv)和公式(6)，(7)所得到的绝对调整角度Δβ分别代入公式(13)中，可得方程组

通过对方程组(14)求解，得出矩阵

从而得到当相机像素坐标系的平移量为Δu，Δv时，对应双目云台的相对航向角和俯仰角的角度改变量为β′_c，可以使目标回到相机画幅的中心，完成追踪。

上述方案中，步骤四建立系统运动方程如下：

其中，M₁+mL是双目云台进行水平转动的转动惯量，m为单个相机的质量，L为双目云台基线的长度，M₁为去掉相机后双目云台的水平转动惯量，M₂是双目云台和相机进行俯仰运动的转动惯量；C₁，C₂分别为双目云台改变航向和俯仰时的等效阻尼，Q₁，Q₂分别为双目云台改变航向和俯仰的等效弹性系数；β_c分别为双目云台航向角和俯仰角，分别为双目云台航向角速度和俯仰角速度，分别为双目云台航向角加速度和俯仰角加速度；T₁，T₂分别为驱动双目云台进行改变航向和俯仰的两个输入力矩。

上述方案中，步骤五的具体方法如下：

(1)将计算得到的双目云台的相对航向角和俯仰角的角度改变量，和通过传感器获得的双目云台在航向角和俯仰角的实际改变量进行比较，得出两者之间的差值，并求得差值随时间的变化率；

(2)将差值和差值变化率输入模糊自适应参数PID控制器，该控制器对输入量进行模糊化处理，查询模糊矩阵表进行参数调整，然后对输出量进行去模糊处理，在双目云台基线长度改变时，产生对电机转动的最佳控制量；

(3)最佳控制量通过驱动器放大和转化生成驱动信号，从而驱动电机转动，使双目云台到达预定的航向角和俯仰角。

上述方案中，电机转动的最佳控制量D(t)的计算公式如下：

其中，K_p为比例系数，K_i为积分系数，K_d为微分系数；

在模糊自适应PID控制过程中动态调整K_p,K_i，K_d的值，在双目云台基线的长度L改变时，产生对电机转动的最佳控制量D(t)。

通过上述技术方案，本发明提供的一种基线可变的双目云台的控制方法引入模糊自适应PID控制算法驱动云台机构运动，有利于提高系统运动参数发生变化的情况下双目云台运动的准确性和快速性。

本发明结合目标追踪算法和自适应控制算法，实现了双目云台对运动目标的追踪。

附图说明

为了更清楚地说明本发明实施例或现有技术中的技术方案，下面将对实施例或现有技术描述中所需要使用的附图作简单地介绍。

图1为本发明实施例所公开的一种基线可变的双目云台的控制方法流程示意图；

图2为双目云台目标追踪示意图；

图3为双目云台结构示意图；

图4为双目云台模糊PID控制过程示意图；

图5为模糊PID参数生成过程流程示意图；

图6为隶属度函数图；

图7为传统PID控制和模糊PID控制在确定参数下的表现；

图8为传统PID控制和模糊PID控制在参数改变时的表现。

图中，A、物体；B、相机一；C、相机二；D、传统PID控制曲线；E、模糊自适应PID控制曲线；F、目标点；G、相机追踪框；H、平移平台；I、俯仰机构；J、水平旋转机构。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述。

本发明提供了一种基线可变的双目云台的控制方法，如图1所示，具体实施方式如下：

步骤一，获取双目云台所在机体的姿态；

(1)双目云台安装在可移动的机体上，通过传感器获取机体当前的姿态四元数Q＝[q i j k]^T，四元数由一个实数q和三个虚数i、j、k组成，三个虚数分别表征绕三个轴的旋转；

(2)根据机体的姿态四元数获取机体的旋转矩阵

将旋转矩阵R_d表示为

(3)根据旋转矩阵获取当前机体的欧拉角Ω，得到被控姿态；

机体当前状态的欧拉角为 为机体航向角，β_d为机体俯仰角，α_d为机体横滚角；

步骤二，进行图像对比获取目标点的像素偏移量，从而得到双目云台在俯仰和水平旋转方向的绝对调整角度；

通过目标识别算法识别出目标在相机图像中的像素位置，并对该目标点的像素位置变化进行监控，如图2所示，当目标点F处于相机追踪框G内时，不产生双目云台运动指令；当目标点超出相机追踪框时，根据目标点的像素位置与画幅中心点间像素位置的像素差(Δu，Δv)，得到双目云台分别在俯仰和水平旋转方向的绝对调整角度Δβ和

其中，f为相机的焦距。

步骤三，正逆运动学解算获取双目云台的调整量；

(1)对世界坐标系的坐标x_w，y_w，z_w进行变换，得到机体坐标系的坐标x_d，y_d，z_d；

变换公式如下：

其中，

机体坐标系为直角坐标系，Z_d轴作为机体横滚轴，表征机体的横滚运动；X_d轴作为机体俯仰轴，表征机体的俯仰运动；Y_d轴作为机体航向轴，表征机体的水平旋转运动；R_d为世界坐标系到机体坐标系的旋转矩阵，t_d表示世界坐标系到机体坐标系的平移，t_xd，t_yd，t_zd分别代表机体坐标系原点相对于世界坐标系原点在x_w，y_w，z_w方向上的平移量，β_d，θ_d表示机体的航向角，俯仰角和横滚角，由安装在机体上的位移传感器获得；

(2)对机体坐标系的坐标x_d，y_d，z_d进行变换，得到相机坐标系的坐标x_c，y_c，z_c；

变换公式如下：

其中，

R_c为机体坐标系到相机坐标系旋转矩阵，t_c表示机体坐标系到相机坐标系的平移，t_xc，t_yc，t_zc分别代表相机坐标系原点相对于机体坐标系原点在x_d，y_d，z_d方向上的平移量，由双目云台在机体上的相对安装位置获得；β_c，θ_c分别表示相机的航向角，俯仰角和横滚角，通过角度传感器获得；此处因为双目云台不能进行翻转，所以相对于机体的翻滚角θ_c＝0；

(3)对相机坐标系的坐标x_c，y_c，z_c进行变换，得到图像物理坐标系的坐标x，y；

变换公式如下：

其中，a_c为目标到相机的垂直距离，f为相机焦距；

(4)对图像物理坐标系的坐标x，y进行变换，得到图像像素坐标系的坐标u，v，从而得到世界坐标系的坐标x_w，y_w，z_w到图像像素坐标系的坐标u，v的正运动学变换公式；

变换的公式如下：

其中，

dx，dy分别为图像像素点在图像物理坐标系x，y轴上的物理尺寸，γ为相机的扭曲因子；

将公式(8)(9)(10)(11)整合，得到从世界坐标系的坐标x_w，y_w，z_w到图像像素坐标系的坐标u，v的正运动学变换公式：

(5)对公式(12)两端取逆，得到从图像像素坐标系的坐标u，v到世界坐标系的坐标x_w，y_w，z_w的逆运动学变公式：

(6)将目标点的像素位置与画幅中心点间像素位置的像素差(Δu，Δv)和公式(6)，(7)所得到的绝对调整角度Δβ分别代入公式(13)中，可得方程组

通过对方程组(14)求解，得出矩阵

从而得到当相机像素坐标系的平移量为Δu，Δv时，对应双目云台的相对航向角和俯仰角的角度改变量为β′_c，可以使目标回到相机画幅的中心，完成追踪。

步骤四，建立关于基线的系统运动方程；

双目云台的结构如图3所示，相机一A和相机二B位于平移平台H上，平移平台H位于俯仰机构I上，俯仰机构I位于水平旋转机构J上。平移平台H上设置丝杠，控制两相机之间的基线长度，俯仰机构I通过俯仰电机和蜗轮蜗杆控制俯仰运动，即调整俯仰角的改变量；水平旋转机构J通过水平旋转电机和蜗轮蜗杆控制水平旋转运动，即调整航向角的改变量。具体结构可参见本申请人在先提交的专利号为“2020101641874”，名称为“一种可自由调节角度的摄像头旋转云台”的专利。

系统运动方程如下：

其中，M₁+mL是双目云台进行水平转动的转动惯量，m为单个相机的质量，L为双目云台基线的长度，M₁为去掉相机后双目云台的水平转动惯量，M₂是双目云台和相机进行俯仰运动的转动惯量；C₁，C₂分别为双目云台改变航向和俯仰时的等效阻尼，Q₁，Q₂分别为双目云台改变航向和俯仰的等效弹性系数；β_c分别为双目云台航向角和俯仰角，分别为双目云台航向角速度和俯仰角速度，分别为双目云台航向角加速度和俯仰角加速度；T₁，T₂分别为驱动双目云台进行改变航向和俯仰的两个输入力矩。

步骤五，通过模糊自适应PID控制驱动双目云台调整姿态，如图4所示。

(1)将计算得到的双目云台的相对航向角和俯仰角的角度改变量，和通过传感器获得的双目云台在航向角和俯仰角的实际改变量进行比较，得出两者之间的差值，并求得差值随时间的变化率；

(2)将差值和差值变化率输入模糊自适应参数PID控制器，该控制器对输入量进行模糊化处理，查询模糊矩阵表进行参数调整，然后对输出量进行去模糊处理，在双目云台基线长度改变时，产生对俯仰电机和水平旋转电机转动的最佳控制量；

电机转动的最佳控制量D(t)的计算公式如下：

其中，K_p为比例系数，K_i为积分系数，K_d为微分系数；

在传统PID控制中是一个常数，在本发明的模糊自适应PID控制过程中动态调整K_p，K_i，K_d的值，在双目云台基线的长度L改变时，产生对电机转动的最佳控制量D(t)。

(3)最佳控制量通过驱动器放大和转化生成驱动信号，从而驱动俯仰电机和水平旋转电机转动，使双目云台到达预定的航向角和俯仰角。

自适应模糊PID控制对云台航向角，俯仰角β_c的控制过程是相同的，以下以航向角为例进行说明。

当控制系统获得指令传来的双目云台在航向角上的给定改变值并通过传感器获得当前云台在航向角上的实际改变值两者之间的差值记为E，差值E随时间的变化率为模糊自适应参数PID控制器通过获取差值E和差值变化率E_c，产生对电机转动的控制量D(t)并通过驱动器放大和转化生成驱动信号，从而驱动电机转动，使云台快速准确的到达预定航向角。

使用模糊算法首先需要对数据进行模糊化处理，如图5所示。

1.对模糊控制器的输入值E和Ec进行模糊化处理。

(1)首先确定E和Ec的模糊子集，设定NBNMNSZOPBPMPS七个语言变量分别表示负大(negative big)、负中(negative middle)、负小(negative small)、零(zero)、正大(positive big)、正中(positive middle)、正小(positive small)。则定义E和Ec的模糊子集均为{NB，NM，NS，ZO，PB，PM，PS}。

(2)引入E和Ec对应的论域和K_p，K_i，K_d的论域，都定义为{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}。

(3)引入量化函数，对于给定改变值和实际改变值都有一个最大值，我们记为所以偏差E的范围就是偏差随时间的变化率Ec范围也设为通过量化函数可以把偏差E和偏差变化率Ec都转化为[-6，6]中的对应值。这里采用线性方式量化，则其量化函数转换关系为：

(4)确定E和Ec在模糊子集上的隶属度。隶属度是一个介于0和1之间的值，用以描述对应一个输入属于某一个模糊自己的程度。采用线性的隶属度函数，其函数关系如图6所示。

确定隶属度值后偏差E和偏差变化率Ec就实现了从原本的取值到由模糊量表示的模糊值的转化。

2.完成输入数据模糊化之后，建立模糊规则库。由模糊规则库求得K_p，K_i，K_d的隶属度值。

使用控制行业成熟的模糊规则生成K_p，K_i，K_d模糊规则表，见表1、表2和表3。

表1比例系数模糊规则表

表2积分系数模糊控制表

表3微分系数模糊控制表

通过获取模糊化后的误差和误差变化率两个模糊量，通过查询对应的模糊规则表，就能获得当前模糊化的K_p，K_i，K_d三个值。

3.对于求得的目标对象还需要进行去模糊处理，以使其与具体的物理量相对应。

之前模糊自适应的输入量和输出量都采用了相同的论域，即{-6，-5，-4，-3，-2，-1，0，1，2，3，4，5，6}，因此在某一时刻输入和输出处的模糊变量隶属度是相同的。基于这一基础，采用重心法计算各输出量的量化值。

其中，k为模糊控制器输出量解模糊后的精确值，k_i为模糊控制量论域内的值，μ(k_i)为k_i的隶属度值。

根据各模糊子集的隶属度赋值表和各参数模糊控制模型，应用模糊合成推理设计分数阶PID参数的模糊矩阵表，算出参数代入下列公式计算：

式中，K′_p，K′_i，K′_d为模糊自适应调整后的值，K_p，K_i，K_d为PID参数的初始值，由常规PID整定方法得到，ΔK_p，ΔK_i，ΔK_d为模糊控制器的三个输出。

4.对模糊控制部分进行模糊PID仿真。

在simulink中搭建了模糊自适应PID控制系统和传统PID控制系统的对比仿真模型，模糊自适应PID控制的参数通过上述模糊控制表产生，传统PID控制的参数通过simulink中自带的调节器产生。

当系统的运动参数不发生改变时，如图7所示，仿真结果显示在几乎相同的响应速度下传统PID控制会产生轻微的超调。

当系统的运动参数发生改变时，如图8所示，仿真结果显示传统PID控制会产生较大超调，而在模糊自适应PID控制下，系统任然具有较高的快速性和稳定性。

对所公开的实施例的上述说明，使本领域专业技术人员能够实现或使用本发明。对这些实施例的多种修改对本领域的专业技术人员来说将是显而易见的，本文中所定义的一般原理可以在不脱离本发明的精神或范围的情况下，在其它实施例中实现。因此，本发明将不会被限制于本文所示的这些实施例，而是要符合与本文所公开的原理和新颖特点相一致的最宽的范围。