据所述固定端当前状态获取固定端在云台电机轴上的角 速度。
[0269] 干扰子单元211可以根据固定端当前状态获取固定端在云台电机轴上的角速度， 固定端在云台电机轴上的角速度包括：在俯仰电机轴上的角速度《piteh，在横滚电机轴上的 角速度COroll，和在航向电机轴上的角速度COyaw。干扰子单元211计算固定端在云台各电机 轴上的角速度，即为计算 〇 pitch、〇 roll萍卩 〇 yaw0
[0270] 具体的，作为一种实施方式，首先，固定端当前状态包括：固定端的角速度cob和固 定端的姿态角；其中
固定端的姿态角包括：横滚角Θ p俯仰角θ p，航向角 9 y。
[0271] 然后，干扰子单元211将固定端的角速度耦合到云台电机轴，根据坐标变换关系， 得到固定端在航向电机轴上的角速度《yaw、Coroll和ω yaw:
[0273] 其中，θρ，θρ 0y分别为电机相对零位值的旋转角度，由磁编码器采集得到。由 此，干扰子单元211得到固定端在云台各电机轴上的角速度：c〇piteh、ω yaw。
[0274] 差值子单元212,用于根据所述被控端当前姿态和目标姿态，获取姿态角差值。
[0275] 姿态角差值为被控端由当前姿态达到目标姿态的角度差值，差值子单元212令姿 态角差值为：俯仰角差值△ θ，横滚角差值△ γ和航向角差值Λφ。
[0276] 根据获取的被控端目标姿态：目标俯仰角Θ '、目标横滚角γ'和目标航向角 爭_f;和获取的被控端当前的欧拉角：俯仰角Θ、横滚角γ和航向角屮，差值子单元212计 算姿态角差值：
[0278] 由此，差值子单元212得到姿态角差值，包括：俯仰角差值△ Θ，横滚角差值Δ γ 和航向角差值么中。
[0279] 补偿子单元213,用于根据所述被控端的当前状态、固定端在云台电机轴上的角速 度和所述姿态角差值，获取控制被控端的补偿角速度。
[0280] 控制被控端的补偿角速度由隔离固定端干扰的反向补偿角速度和使被控端达到 目标姿态的补偿角速度组成。
[0281] 首先，补偿子单元213根据被控端当前状态中的角速度
固定端在云 台电机轴上的角速度《piteh、Coroll和ω yaw，计算隔离固定端干扰的反向补偿角速度：
[0282] 设反向补偿角速度为：横滚角速度δ ^俯仰角速度δ p、航向角速度δ y;
[0283] 则根据坐标变换关系，得到方程式：
[0286] 补偿子单元213解算上述方程，得到反向补偿角速度：横滚角速度δ ^俯仰角速 度Sp和航向角速度S y。
[0287] 然后，在本实施例中，补偿子单元213取使被控端达到目标姿态的补偿角速度为： 俯仰角速度A ·△ θ，横滚角速度A ·△ γ和航向角速度A'?φ;其中，A为预设系数，用于 精确校准补偿角速度，可根据实际情况灵活设定。
[0288] 然后，补偿子单元213计算控制被控端的补偿角速度，令：在俯仰电机轴上的补偿 角速度ωρ，在横滚电机轴上的补偿角速度％,和在航向电机轴上的补偿角速度coy;
[0289] 则有如下计算公式：
[0291] 由此，补偿子单元213计算得到控制被控端的补偿角速度，包括：在俯仰电机轴 上的补偿角速度ωρ，在横滚电机轴上的补偿角速度ωρ和在航向电机轴上的补偿角速度 ω y°
[0292] 在本实施例中，根据获取的被控端当前状态和被控端目标状态、固定端当前状态， 获取隔离固定端干扰的反向补偿角速度和使被控端达到目标姿态的补偿角速度，补偿子单 元213得到控制被控端的补偿角速度。本实施例得到的补偿角速度，能够使控制云台电机 轴转动时，实现被控端隔离机体扰动，达到目标姿态并保持稳定状态。
[0293] 进一步的，参照图10,本发明第五实施例提供一种云台控制装置，基于上述图1所 示的实施例，所述控制模块300包括：
[0294] 电流单元310,用于根据所述有效机械能，获取控制所述被控端的目标电流。
[0295] 计算模块200获取控制被控端的有效机械能后，电流单元310根据控制被控端的 有效机械能，计算控制被控端的目标电流，也即：根据控制云台各电机轴的有效机械能，分 别计算控制云台各电机轴的目标电流。
[0296] 具体的，作为一种实施例，获取的控制被控端的有效机械能为：控制俯仰电机轴的 有效机械能Ερ、控制横滚电机轴的有效机械能Ep和控制航向电机轴的有效机械能Ey。
[0297] 电流单元310设云台各电机轴的目标电流为：控制俯仰电机轴的目标电流Ip，控 制横滚电机轴的目标电流L，控制航向电机轴的目标电流Iy;则有：
[0299] 根据上述公式，电流单元310计算得到控制云台各电机轴的目标电流：IP、I^ I y。 也即，得到控制被控端的目标电流：IP、IdP I y。
[0300] 其中，需要说明的是，E的符号代表电机轴转动方向，可根据实际情况具体确定。例 如：+(正）为顺时针转动，一（负）为逆时针转动。Ip、Ir和I ^勺符号与E p、艮和E y分别 相同。
[0301 ] 控制单元320,用于根据所述目标电流，通过云台电机轴控制所述被控端保持在稳 定状态。
[0302] 在电流单元310获得控制被控端的目标电流后，控制单元320以相应的目标电流 控制云台各电机轴，使被控端达到目标姿态并保持在稳定状态。
[0303] 具体的，控制单元320以控制俯仰电机轴的目标电流Ip控制俯仰电机轴，从而使 被控端在俯仰方向达到目标俯仰角，并保持俯仰方向的稳定状态；
[0304] 控制单元320以控制横滚电机轴的目标电流Id空制横滚电机轴，从而使被控端在 横滚方向达到目标横滚角，并保持横滚方向的稳定状态；
[0305] 控制单元320以控制航向电机轴的目标电流1￥控制航向电机轴，从而使被控端在 航向方向达到目标航向角，并保持航向方向的稳定状态。
[0306] 在本实施例中，根据获取的有效机械能，电流单元310获取控制被控端的目标电 流，控制单元320以控制被控端的目标电流，控制云台各电机轴，使被控端达到目标姿态并 保持稳定，实现了云台的自稳控制效果。
[0307] 以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发 明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技 术领域，均同理包括在本发明的专利保护范围内。
【主权项】
1. 一种云台控制方法，其特征在于，所述云台控制方法包括以下步骤： 获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转 动惯量比； 根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与被控端的转动 惯量比，计算获取控制被控端的有效机械能； 根据所述有效机械能，控制所述被控端保持在稳定状态。2. 如权利要求1所述的云台控制方法，其特征在于，所述获取云台上被控端当前状态 和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比的步骤包括： 获取被控端的角速度和姿态四元数； 根据所述被控端的姿态四元数，获取被控端的旋转矩阵； 根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿态； 获取被控端的目标姿态角和目标角速度，作为被控端目标状态； 获取固定端的角速度和姿态角，作为固定端当前状态； 获取固定端与被控端的转动惯量比。3. 如权利要求1或2所述的云台控制方法，其特征在于，所述根据所述被控端当前状态 和目标状态、固定端当前状态，以及固定端与被控端的转动惯量比，计算获取控制被控端的 有效机械能的步骤包括： 根据所述被控端当前状态和目标状态，以及固定端当前状态，获取控制被控端的补偿 角速度； 根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量比以及固定端当前状 态，计算获取控制被控端的有效机械能。4. 如权利要求3所述的云台控制方法，其特征在于，所述根据所述被控端当前状态和 目标状态，以及固定端当前状态，获取控制被控端的补偿角速度的步骤包括： 根据所述固定端当前状态获取固定端在云台电机轴上的角速度； 根据所述被控端当前姿态和目标姿态，获取姿态角差值； 根据所述被控端的当前状态、固定端在云台电机轴上的角速度和所述姿态角差值，获 取控制被控端的补偿角速度。5. 如权利要求1所述的云台控制方法，其特征在于，所述根据所述有效机械能，控制所 述被控端保持在稳定状态的步骤包括： 根据所述有效机械能，获取控制所述被控端的目标电流； 根据所述目标电流，通过云台电机轴控制所述被控端保持在稳定状态。6. -种云台控制装置，其特征在于，所述云台控制装置包括： 获取模块，用于获取云台上被控端当前状态和目标状态、固定端当前状态，以及固定端 与被控端的转动惯量比； 计算模块，用于根据所述被控端当前状态和目标状态、固定端当前状态，以及固定端与 被控端的转动惯量比，计算获取控制被控端的有效机械能； 控制模块，用于根据所述有效机械能，控制所述被控端保持在稳定状态。7. 如权利要求6所述的云台控制装置，其特征在于，所述获取模块包括： 传感单元，用于获取被控端的角速度和姿态四元数； 矩阵单元，用于根据所述被控端的姿态四元数，获取被控端的旋转矩阵； 姿态单元，用于根据所述被控端的旋转矩阵，获取被控端的欧拉角，得到被控端当前姿 态； 获取单元，用于获取被控端的目标姿态角和目标角速度，作为被控端目标状态；获取固 定端与被控端的转动惯量比； 所述传感单元还用于，获取固定端的角速度和姿态角，作为固定端当前状态。8. 如权利要求6所述的云台控制装置，其特征在于，所述计算模块包括： 速度计算单元，用于根据所述被控端当前状态和目标状态，以及固定端当前状态，获取 控制被控端的补偿角速度； 能量计算单元，用于根据所述控制被控端的补偿角速度、固定端与被控端的转动惯量 比以及固定端当前状态，计算获取控制被控端的有效机械能。9. 如权利要求8所述的云台控制装置，其特征在于，所述速度计算单元包括： 干扰子单元，用于根据所述固定端当前状态获取固定端在云台电机轴上的角速度； 差值子单元，用于根据所述被控端当前姿态和目标姿态，获取姿态角差值； 补偿子单元，用于根据所述被控端的当前状态、固定端在云台电机轴上的角速度和所 述姿态角差值，获取控制被控端的补偿角速度。10. 如权利要求6所述的云台控制装置，其特征在于，所述控制模块包括： 电流单元，用于根据所述有效机械能，获取控制所述被控端的目标电流； 控制单元，用于根据所述目标电流，通过云台电机轴控制所述被控端保持在稳定状态。
【专利摘要】本发明公开了一种云台控制方法，获取云台上被控端当前姿态和目标姿态、固定端当前姿态，以及固定端与被控端的转动惯量比；根据所述被控端当前姿态和目标姿态、固定端当前姿态，以及固定端与被控端的转动惯量比，计算获取控制被控端的有效机械能；根据所述有效机械能，控制所述被控端保持在稳定状态。本发明还公开了一种云台控制装置。本发明使被控端达到目标姿态并保持在稳定状态，实现了对云台的精确控制，有效隔离机体扰动，达到了云台自稳控制效果，保障了云台负载相机的成像质量。
【IPC分类】G05D3/12
【公开号】CN105116926
【申请号】CN201510515606
【发明人】张显志
【申请人】深圳一电科技有限公司, 深圳一电航空技术有限公司
【公开日】2015年12月2日
【申请日】2015年8月20日