一种基于最优姿态路径的云台控制方法及其系统与流程

[0001]
本发明属于云台设备控制技术领域，涉及一种基于最优姿态路径的云台控制方法及其系统。


背景技术：

[0002]
本部分的陈述仅仅是提供了与本申请相关的背景技术信息，不必然构成在先技术。
[0003]
云台是负载增稳的设备，在使用无人机进行输电线路巡检工作时，需要用到三轴云台去固定拍摄设备对杆塔关键部位检视点进行拍照。三轴云台一般按照横滚轴-俯仰轴-偏航轴的旋转顺序控制旋转各个轴，以带动拍摄设备跟踪拍摄杆塔。
[0004]
据发明人了解，传统的云台操作方式是在悬停点拍摄完后控制云台回中，然后无人机前往下一个悬停点，调整无人机机头朝向拍摄对象，微调云台相机拍摄对象，此种控制方式虽然控制方式简单，但是随着巡检工作量加大，这种传统的控制方法频繁控制无人机制动、悬停和加速操作，一方面增加了飞控的不稳定性，另一方面对无人机电池电量消耗也很大，同时拍摄效率整体也不高。


技术实现要素：

[0005]
本申请为了解决上述问题，提出了一种基于最优姿态路径的云台控制方法及其系统，本申请优先通过三轴云台相机的姿态控制，去匹配最优的无人机的飞行姿态路径，通过无人机最小的控制矢量，节省无人机电池电量和提高飞行效率，同时也提高了单次飞行拍摄工作量。
[0006]
根据一些实施例，本申请采用如下技术方案：
[0007]
一种基于最优姿态路径的云台控制方法，包括以下步骤：
[0008]
在无人机飞往悬停点的过程中，预先根据悬停点和拍摄对象的三维空间坐标信息，结合无人机及云台当前姿态，计算云台相机最优姿态路径应该调整的横滚角、俯仰角和偏航角，若在云台相机三轴姿态的预设阈值范围内，按照计算的各个角度调整云台姿态，若超出云台相机三轴姿态预设阈值范围，计算并提前通过无人机三维姿态进行补偿。
[0009]
通过上述技术方案，首先，在无人机飞往下一悬停点的过程中就进行提前调整，能够保证当无人机到达悬停点时，三轴云台相机已经完成预定转向设置，并且拍摄对象已经处于三轴云台相机视野之内，节约无人机的悬停时间。其次，通过拟合无人机姿态和云台姿态，利用无人机姿态补偿云台相机三轴姿态的调整，能够实现无人机姿态的最小矢量控制。
[0010]
作为可选择的实施方式，根据悬停点和拍摄对象的三维空间坐标信息的具体过程为：进行无人机云台的校准和自检，获取巡检任务中的悬停点三维位置信息和拍摄对象位置信息。
[0011]
作为可选择的实施方式，悬停点三维位置信息为悬停点的gps位置信息。
[0012]
作为可选择的实施方式，计算云台相机姿态应该调整的横滚角、俯仰角和偏航角
的具体过程包括：根据世界坐标系和相机坐标系的转换关系，计算出拍摄对象在相机坐标系中的位置。具体步骤如下：
[0013]
步骤(1)：p(x
w
,y
w
,z
w
)为拍摄物在世界坐标系中的位置坐标，
[0014]
令r
cw
，t表示世界坐标系转换到相机坐标系的旋转矩阵和平移矢量，则p(x
w
,y
w
,z
w
)在相机坐标系的坐标为：其中
[0015][0016]
下标cw代表大地坐标系转换到相机坐标系的简称，r
cwx
(φ)、r
cwy
(θ)、r
cwz
(ψ)代表从相机坐标系到大地坐标系需要绕着x、y、z轴旋转的矩阵，φ、θ、ψ分别为无人机姿态的横滚角、俯仰角和偏航角，根据无人机的初始朝向，还需左乘一个初始旋转r
cw0
，此时
[0017]
r
cw
＝r
cw0
×
(r
cwx
(φ)
×
r
cwy
(θ)
×
r
cwz
(ψ))，式中
[0018]
步骤(2)：根据步骤(1)计算出来的p(x
c
,y
c
,z
c
)为拍摄物在相机坐标系中的位置坐标，因为相机坐标系中相机光心为原点，根据拍摄物和相机光心的相对位置信息计算出云台相机的俯仰角和偏航角
[0019]
此处俯仰角θ
p
和偏航角ψ
p
是相对于上一位置的偏移量，横滚角认为初始位置，因为默认水平方向进行拍摄。
[0020]
作为可选择的实施方式，若超出云台相机三轴姿态预设阈值范围，计算并提前通过无人机三维姿态进行补偿的具体过程为：计算云台相机的目标姿态和预设阈值范围之间的差值，并以此差值通过控制无人机三维姿态，进行补偿。具体步骤如下：
[0021]
步骤(3)：在步骤(2)，判断计算出的θ
p
和偏航角ψ
p
是否超出云台相机因机械设计限定的预设阈值，此处假设俯仰角预设阈值偏航角预设阈值
[0022]
步骤(4)：在步骤(2)，如果计算出来的偏航角就需要通过控制无人
机的偏航角ψ来补偿云台相机的阈值，因为无人机和云台相机可以视为同一个坐标系，无人机以当前机头方向为基准需要调整的偏航角等效的相当于调整了云台相机的偏航角ψ
p
；如果计算出来的偏航角就不需要通过调整无人机机头(偏航角ψ)，进入下一步；
[0023]
步骤(5)：根据云台需要调整的姿态角度，设置转动速率，保证云台相机匀速快速调整姿态；
[0024]
注意，这里提到云台相机的俯仰角预设阈值是不存在的，因为在航点规划悬停点的时候已经考虑到在的范围内设置悬停点。
[0025]
作为可选择的实施方式，还包括以下步骤：判断所述悬停点是否为巡检任务中最后一个悬停点，如果是，控制无人机返航，如果不是，则循环执行上述步骤。
[0026]
一种基于最优姿态路径的云台控制系统，包括：
[0027]
定位模块，用于定位无人机三维坐标信息；
[0028]
前端ai处理模块，用于拟合无人机飞控数据，根据悬停点和拍摄对象的三维空间坐标信息，结合无人机及云台当前姿态，计算云台相机最优姿态路径应该调整的横滚角、俯仰角和偏航角，若在云台相机三轴姿态的预设阈值范围内，按照计算的各个角度调整云台姿态，若超出云台相机三轴姿态预设阈值范围，计算并通过无人机三维姿态进行补偿。
[0029]
作为可选择的实施方式，所述前端ai处理模块与云台控制系统通信，并发送计算云台相机应该调整的横滚角、俯仰角和偏航角，所述云台控制系统包括主控mcu模块、esc横滚角驱动模块、esc俯仰角驱动模块、esc偏航角驱动模块和imu姿态模块，上述各个模块之间通过can总线与前端ai处理模块通讯。
[0030]
一种无人机，采用上述方法进行自身姿态控制和云台控制，或搭载有上述系统。
[0031]
一种计算机可读存储介质，其中存储有多条指令，所述指令适于由终端设备的处理器加载并执行所述的一种基于最优姿态路径的云台控制方法。
[0032]
一种终端设备，包括处理器和计算机可读存储介质，处理器用于实现各指令；计算机可读存储介质用于存储多条指令，所述指令适于由处理器加载并执行所述的一种基于最优姿态路径的云台控制方法。
[0033]
与现有技术相比，本申请的有益效果为：
[0034]
1、本申请创新性提出了一种基于最优姿态路径的云台控制方法，通过拟合无人机姿态和云台姿态方法，实现无人机姿态的最小矢量控制方法，主要由视觉处理技术、无人机飞控技术、云台控制技术组成，三者协调工作，实现了无人机在输电线路环境下的自动巡检，解决了输电线路场景下无人机巡检效率低下的问题，实现了自身的省电高效功能。
[0035]
2、本申请设计了一种基于最优姿态路径的云台控制系统。在无人机飞往悬停点的过程中，根据悬停点和拍摄对象的三维空间坐标信息在云台相机三轴姿态的控制范围内，提前计算出云台相机目标姿态的横滚角、俯仰角、偏航角，当计算的三轴云台相机转动范围
超出预设阈值区间，在驶向悬停点过程中实时计算并调整无人机机头朝向，最终达到当无人机到达悬停点时，三轴云台相机已经完成预定转向设置，并且拍摄对象已经处于三轴云台相机视野之内。
[0036]
3、本申请实现了无人机姿态和三轴云台相机姿态的拟合控制，最大限度的节省了无人机姿态控制，大大节省了无人机电池电量；在到达悬停点前已经完成无人机和云台相机姿态转换调整，可以直接拍摄，故节省了悬停时间，提高了单次巡检任务的工作量。
附图说明
[0037]
构成本申请的一部分的说明书附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。
[0038]
图1(a)-(c)是本实施例的三轴云台相机示意图；
[0039]
图2是本实施例的杆塔巡检顺序示意图；
[0040]
图3是杆塔实物图；
[0041]
图4是本实施例的基于最优姿态路径的云台控制系统流程图；
[0042]
图5是本实施例的无人机云台控制系统框架图；
[0043]
图6是坐标系说明图。
具体实施方式：
[0044]
下面结合附图与实施例对本申请作进一步说明。
[0045]
应该指出，以下详细说明都是例示性的，旨在对本申请提供进一步的说明。除非另有指明，本文使用的所有技术和科学术语具有与本申请所属技术领域的普通技术人员通常理解的相同含义。
[0046]
需要注意的是，这里所使用的术语仅是为了描述具体实施方式，而非意图限制根据本申请的示例性实施方式。如在这里所使用的，除非上下文另外明确指出，否则单数形式也意图包括复数形式，此外，还应当理解的是，当在本说明书中使用术语“包含”和/或“包括”时，其指明存在特征、步骤、操作、器件、组件和/或它们的组合。
[0047]
在本实施例中，三轴云台相机是可以三轴旋转的，如图1(a)-(c)所示。三轴云台相机的控制系统，包括主控mcu模块，esc横滚角驱动模块，esc俯仰角驱动模块，esc偏航角驱动模块，imu姿态模块，上述各个模块之间通过can总线完成与前端ai处理模块通讯和固件升级，使用can总线提升了云台相机响应性能，降低通讯误码率。
[0048]
一种基于最优姿态路径的云台控制方法，在无人机飞往悬停点的过程中，根据悬停点和拍摄对象的三维空间坐标信息在云台相机三轴姿态的控制范围内，提前计算出云台相机姿态应该调整的横滚角φ
p
、俯仰角θ
p
、偏航角ψ
p
，当计算的三轴云台相机转动范围超出预设阈值区间，在飞向下一个悬停点过程中实时计算并调整无人机机头朝向，最终达到当无人机到达悬停点时，三轴云台相机已经完成预定转向设置，并且拍摄对象已经处于三轴云台相机视野之内。此种方法，优先通过三轴云台相机的姿态控制，去匹配最优的无人机的飞行姿态路径，通过无人机最小的控制矢量，节省无人机电池电量和提高飞行效率。
[0049]
悬停点的飞行顺序根据杆塔巡检顺序确定，如图2所示和图3所示。
[0050]
具体的，如图4所示，包括以下步骤：
[0051]
步骤(1)：前端ai处理模块读取巡检任务，控制无人机云台校准和自检，起飞驶向悬停点，并进入下一步；
[0052]
步骤(2)：前端ai处理模块提前读取驶向悬停点gps位置信息和拍摄对象位置信息，获取当前无人机姿态和当前三轴云台相机初始姿态，计算并调整三轴云台相机最终姿态，如果三轴云台相机调整的最终姿态位置，在三轴云台相机预设阈值范围内，则进入下一步；否则，进入步骤(4)；
[0053]
步骤(3)：无人机到达悬停点，三轴云台相机已经调整到拍摄位置，前端ai处理模块控制云台相机拍照搜寻比对照片中的拍摄对象，并进行实时缺陷判断，进入步骤(5)；
[0054]
步骤(4)：前端ai处理模块根据步骤(2)计算云台相机的目标姿态和预设阈值范围之间的差值，通过控制无人机飞控系统的三维姿态(俯仰，横滚，偏航)来对三轴云台相机的姿态进行补偿，并进入步骤(2)；
[0055]
步骤(5)：完成当前悬停点拍摄后，无人机机头方向不变驶向下一个悬停点，云台相机姿态不回中，如果已经是最后一个悬停点，则进入下一步，否则进入步骤(2)；
[0056]
步骤(6)：三轴云台相机完成拍摄任务，并安全返航。
[0057]
通过拟合无人机姿态和云台姿态方法，实现无人机姿态的最小矢量控制这个目的，因为云台姿态控制相较于无人机姿态控制更加简捷和省电。
[0058]
在采用基于最优姿态路径来控制云台相机转动时，云台相机有m个自由度，云台相机转动的角速度为w＝[w1,...,w
m
]，末端的线速度为v＝[v1,...,v
m
]，两者具有如下关系：
[0059]
v＝j
v
×
w
[0060]
其中，在此处云台相机有三个轴，所以m＝3；
[0061]
在云台相机姿态转换拟合中，涉及到四个坐标系：世界坐标系、相机坐标系、图像坐标系和像素坐标系，我们需要计算世界坐标系和像素坐标系的转换关系，如图6中所示。
[0062]
o
w-x
w
y
w
z
w
：世界坐标系，描述相机位置，单位m；
[0063]
o
c-x
c
y
c
z
c
：相机坐标系，光心为原点，单位m；
[0064]
o-xy：图像坐标系，光心为图像中点，单位mm；
[0065]
uv：像素坐标系，原点为图像左上角，单位pixel；
[0066]
其中，p：世界坐标系中的一点，即为世界中真实的一点；
[0067]
p：点p在图像中成相点，在图像坐标系中的坐标为(x,y)，在像素坐标系中坐标为(u,v)；
[0068]
f：相机焦距，等于o与o
c
的距离，f＝||o-o
c
||；
[0069]
世界坐标系中的一点p(x
w
,y
w
,z
w
)，需要求出其像素坐标系中的位置，推到过程如下：
[0070]
(1)世界坐标系->相机坐标系
[0071]
令r
cw
，t表示世界坐标系转换到相机坐标系的旋转矩阵和平移矢量，则p在相机坐
标系的坐标为：其中
[0072][0073]
下标cw代表大地坐标系转换到相机坐标系的简称，r
cwx
(φ)、r
cwy
(θ)、r
cwz
(ψ)代表从相机坐标系到大地坐标系需要绕着x、y、z轴旋转的矩阵，φ、θ、分别为无人机姿态的横滚角、俯仰角和偏航角，根据相机的初始朝向，还需左乘一个初始旋转r
cw0
，此时
[0074]
r
cw
＝r
cw0
×
(r
cwx
(φ)
×
r
cwy
(θ)
×
r
cwz
(ψ))，式中
[0075]
(2)相机坐标系->图像坐标系
[0076]
则单位为mm；
[0077]
(3)图像坐标系->像素坐标系
[0078][0079][0080]
将(1)(2)(3)联合起来，可以得出：
[0081][0082]
其中为内参数函数，为外参数矩阵。
[0083]
上述步骤(4)的具体控制步骤如下：
[0084]
(1)、p(x
w
,y
w
,z
w
)为拍摄物在世界坐标系中的位置坐标，p(x
c
,y
c
,z
c
)为拍摄物在相机坐标系中的位置坐标，因为相机坐标系中相机光心为原点，根据拍摄物和相机光心的相对位置信息计算出云台相机的俯仰角和偏航角
[0085]
此处俯仰角θ
p
和偏航角ψ
p
是相对于上一位置的偏移量，横滚角默认为初始位置，因为默认水平方向进行拍摄；
[0086]
(2)、判断计算出的θ
p
和偏航角ψ
p
是否超出云台相机因机械设计限定的预设阈值，此处假设俯仰角预设阈值偏航角预设阈值
[0087]
(3)、如果计算出来的偏航角就需要通过控制无人机的偏航角ψ来补偿云台相机的阈值，因为无人机和云台相机可以视为同一个坐标系，这里先将云台相机偏航角ψ
p
调整到最大值然后无人机以当前机头方向为基准需要调整的偏航角等
效的相当于调整了云台相机的偏航角ψ
p
；同理，如果偏航角无人机则反向调整如果计算出来的偏航角就不需要通过调整无人机机头(偏航角ψ)，进入下一步；
[0088]
(4)、根据云台需要调整的姿态角度，设置转动速率，保证云台相机匀速快速调整姿态；
[0089]
这里提到云台相机的俯仰角预设阈值是不存在的，因为在航点规划悬停点的时候已经考虑到在的范围内设置悬停点。
[0090]
上述步骤的核心是拟合无人机姿态和云台姿态方法，实现无人机姿态的最小矢量控制这个目的，因为云台姿态控制相较于无人机姿态控制更加简捷和省电。
[0091]
实施系统为，如图5所示，一种基于最优姿态路径的云台控制系统，包括：
[0092]
无人机，用于搭载三轴云台相机，并执行飞行参数指令；
[0093]
rtk模块，用于定位无人机三维坐标信息；
[0094]
三轴云台相机，用于控制相机拍摄角度并执行拍照任务；
[0095]
前端ai处理模块，用于拟合无人机飞控数据，rtk模块数据，和三轴云台相机姿态，根据后台设置航点任务，下发飞控命令控制无人机飞行，控制云台相机姿态，锁定杆塔检视点并拍照。
[0096]
本领域内的技术人员应明白，本申请的实施例可提供为方法、系统、或计算机程序产品。因此，本申请可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本申请可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。
[0097]
本申请是参照根据本申请实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。
[0098]
这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。
[0099]
这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一
个方框或多个方框中指定的功能的步骤。
[0100]
以上所述仅为本申请的优选实施例而已，并不用于限制本申请，对于本领域的技术人员来说，本申请可以有各种更改和变化。凡在本申请的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本申请的保护范围之内。
[0101]
上述虽然结合附图对本申请的具体实施方式进行了描述，但并非对本申请保护范围的限制，所属领域技术人员应该明白，在本申请的技术方案的基础上，本领域技术人员不需要付出创造性劳动即可做出的各种修改或变形仍在本申请的保护范围以内。