CN110570463B - 一种目标状态估计方法、装置和无人机 - Google Patents

Abstract

本发明实施例涉及一种目标状态估计方法、装置和无人机，所述方法包括：获取摄像装置采集的目标图像，并对目标图像进行图像识别，以获取目标在目标图像中的目标图像区域；获取无人机周围环境的原始点云，根据目标图像区域和原始点云获取第一目标位置；获取摄像装置的姿态信息，并基于目标图像区域获得第一图像位置，根据第一图像位置和姿态信息获取第二目标位置；基于所述目标图像区域获取第二图像位置；根据所述第一目标位置或者所述第二目标位置对目标状态进行初始化；将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态。本发明实施例不需强假设条件，状态估计的精度高。

Description

一种目标状态估计方法、装置和无人机

技术领域

本发明实施例涉及无人飞行器技术领域，特别涉及一种目标状态估计方法、装置和无人机。

背景技术

利用无人机对运动目标进行跟踪已得到广泛应用，利用无人机对目标进行跟踪时，需要根据目标的位置，规划合适的路径，在保持跟踪的前提下避开障碍物飞行。因此，在无人机跟踪目标过程中，获取目标的精确状态至关重要。

目前，较多采用的方法是假设地面为平面，并假设无人机对地高度为无人机相对于目标的高度，然后通过飞控或其他模块获得该对地高度、在上述假设条件下利用特定的几何关系估计目标状态。

在实现本发明过程中，发明人发现上述方法至少存在如下问题：在无人机下方非地面(例如山丘、楼梯)的场合，假设条件不成立，无人机获得的对地高度存在误差，从而导致依赖该对地高度进行的目标状态估计精度较差。

发明内容

本发明实施例的目的是提供一种精度高的目标状态估计方法、装置和无人机。

第一方面，本发明实施例提供了一种目标状态估计方法，所述方法用于无人机，所述无人机包括摄像装置，所述方法包括：

获取所述摄像装置采集的目标图像，并对所述目标图像进行图像识别，以获取目标在所述目标图像中的目标图像区域；

获取所述无人机周围环境的原始点云，根据所述目标图像区域和所述原始点云获取第一目标位置；

获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置；

基于所述目标图像区域获取第二图像位置；

根据所述第一目标位置或者所述第二目标位置对目标状态进行初始化；

将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态；

其中，所述目标图像为二维图像，所述第一和第二图像位置均为所述目标图像区域在所述目标图像中的位置表示方式。

第二方面，本发明实施例提供了一种目标状态估计方法，所述方法用于无人机，所述无人机包括摄像装置，所述方法包括：

获取所述摄像装置采集的目标图像，并对所述目标图像进行图像识别，以获取目标在所述目标图像中的目标图像区域；

获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置；

基于所述目标图像区域获取第二图像位置；

根据所述第二目标位置对目标状态进行初始化；

将所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态，

其中，所述目标图像为二维图像，所述第一和第二图像位置均为所述目标图像区域在所述目标图像中的位置表示方式。

在一些实施例中，所述根据所述目标图像区域和所述原始点云获取第一目标位置，包括：

将所述原始点云变换至所述目标图像所在的图像坐标系下，获得所述原始点云对应的图像点云；

确定是否存在目标对应的图像点云，所述目标对应的图像点云为位于所述目标图像区域的图像点云，如果存在所述目标对应的图像点云，则根据所述目标对应的图像点云获得第一目标位置。

在一些实施例中，所述姿态信息为所述摄像装置的俯仰角；

所述获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置，包括：

获取所述摄像装置的俯仰角，如果所述俯仰角小于或者等于预设角度阈值，则基于所述目标图像区域获得第一图像位置，并根据所述第一图像位置、所述俯仰角以及所述目标与所述摄像装置构建的几何关系获得第二目标位置。

在一些实施例中，所述基于所述目标图像区域获取第二图像位置，包括：

如果所述目标图像区域的大小与所述目标图像的区域的大小之比值大于或者等于预设比例阈值，则基于所述目标图像区域获得第二图像位置。

在一些实施例中，所述目标图像区域为所述目标在所述目标图像中的最小外接框；

所述目标对应的图像点云为各图像点云中坐标位于所述最小外接框内的图像点云。

在一些实施例中，所述根据所述目标对应的图像点云获得第一目标位置，包括：

根据所述目标对应的图像点云的世界坐标获得所述目标相对于所述无人机的距离和高度。

在一些实施例中，所述第一图像位置为所述目标在所述目标图像中的最小外接框的最高点和最低点；

所述根据所述第一图像位置、所述俯仰角以及所述目标与所述摄像装置构建的几何关系获得第二目标位置，包括：

基于所述最高点的纵坐标v_min、所述最低点的纵坐标v_max、所述目标的预设高度M和所述俯仰角，根据所述目标与所述摄像装置构建的几何关系获得所述第二目标位置。

在一些实施例中，所述基于所述最高点的纵坐标v_min、所述最低点的纵坐标v_max、所述目标的预设高度M和所述俯仰角，根据所述目标与所述摄像装置构建的几何关系获得所述第二目标位置，包括：

根据所述最低点的纵坐标v_max获得所述最低点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角β；

根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角α；

根据所述夹角β和所述夹角α获得由所述最高点与所述摄像装置的光心之间的连线、与所述最低点与所述摄像装置的光心之间的连线所构成的夹角

其中，

根据所述夹角β、所述摄像装置的俯仰角θ，获得所述最低点与所述摄像装置的光心之间的连线与竖直方向构成的夹角δ，其中，

根据所述夹角δ、所述夹角

所述预设高度M和所述几何关系获得所述目标相对于所述无人机的距离和高度。

在一些实施例中，所述根据所述夹角δ、所述夹角

所述预设高度M和所述几何关系获得所述目标相对于所述无人机的距离和高度，包括：

通过以下二元方程获得所述最低点与所述摄像装置的光心的连线长度L₂，以及所述最高点与所述摄像装置的光心的连线长度L₂：

通过公式d＝L₂sinδ计算所述目标相对于所述无人机的距离d；

通过公式

计算所述目标相对于所述无人机的高度h。

在一些实施例中，所述根据所述最低点的纵坐标v_max获得所述最低点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角β，包括：

利用以下公式计算所述夹角β：

β＝tan^-1(v_max-c_y)/f_y；

其中，c_y为所述摄像装置的主点纵坐标，f_y为所述摄像装置在y轴方向上的焦距；

所述根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角α，包括：

利用以下公式计算所述夹角α：

α＝tan^-1(v_min-c_y)/f_y。

在一些实施例中，所述目标图像区域为所述目标在所述目标图像中的最小外接框，所述第二图像位置为所述最小外接框的中心坐标。

在一些实施例中，所述根据所述第一目标位置或者所述第二目标位置对目标状态进行初始化，包括：

判断是否获取到所述第一目标位置，

若是，则根据所述第一目标位置对所述目标状态进行初始化，

若否，则根据所述第二目标位置对所述目标状态进行初始化。

在一些实施例中，所述将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态，包括：

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置时，将三者按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中任意两项时，将该任意两项按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当仅获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中的一项时，则将该项作为所述扩展卡尔曼滤波器的测量值，获得目标状态。

在一些实施例中，所述扩展卡尔曼滤波器包括预测阶段和更新阶段；

在更新阶段，将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，更新更新方程，获得更新值；

在预测阶段，将所述更新阶段得到的更新值代入所述预测阶段的预测方程，获得所述目标状态。

第三方面，本发明实施例提供了一种目标状态估计装置，所述装置用于无人机，所述无人机包括摄像装置，所述装置包括：

目标图像区域获取模块，用于获取所述摄像装置采集的目标图像，并对所述目标图像进行图像识别，以获取目标在所述目标图像中的目标图像区域；

第一目标位置获取模块，用于获取所述无人机周围环境的原始点云，根据所述目标图像区域和所述原始点云获取第一目标位置；

第二目标位置获取模块，用于获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置；

第二图像位置获取模块，用于基于所述目标图像区域获取第二图像位置；

初始化模块，用于根据所述第一目标位置或者所述第二目标位置对目标状态进行初始化；

递推模块，用于将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态；

其中，所述目标图像为二维图像，所述第一和第二图像位置均为所述目标图像区域在所述目标图像中的位置表示方式。

第四方面，本发明实施例提供了一种目标状态估计装置，所述装置用于无人机，所述无人机包括摄像装置，所述装置包括：

目标图像区域获取模块，用于获取所述摄像装置采集的目标图像，并对所述目标图像进行图像识别，以获取目标在所述目标图像中的目标图像区域；

第二目标位置获取模块，用于获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置；

第二图像位置获取模块，用于基于所述目标图像区域获取第二图像位置；

初始化模块，用于根据所述第二目标位置对目标状态进行初始化；

递推模块，用于将所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态；

其中，所述目标图像为二维图像，所述第一和第二图像位置均为所述目标图像区域在所述目标图像中的位置表示方式。

在一些实施例中，所述第一目标位置获取模块具体用于：

将所述原始点云变换至所述目标图像所在的图像坐标系下，获得所述原始点云对应的图像点云；

确定是否存在目标对应的图像点云，所述目标对应的图像点云为位于所述目标图像区域的图像点云，如果存在所述目标对应的图像点云，则根据所述目标对应的图像点云获得第一目标位置。

在一些实施例中，所述姿态信息为所述摄像装置的俯仰角；

所述第二目标位置获取模块具体用于：

获取所述摄像装置的俯仰角，如果所述俯仰角小于或者等于预设角度阈值，则基于所述目标图像区域获得第一图像位置，并根据所述第一图像位置、所述俯仰角以及所述目标与所述摄像装置构建的几何关系获得第二目标位置。

在一些实施例中，所述第二图像位置获取模块具体用于：

如果所述目标图像区域的大小与所述目标图像的区域的大小之比值大于或者等于预设比例阈值，则基于所述目标图像区域获得第二图像位置。

在一些实施例中，所述目标图像区域为所述目标在所述目标图像中的最小外接框；

所述目标对应的图像点云为各图像点云中坐标位于所述最小外接框内的图像点云。

在一些实施例中，所述第一目标位置获取模块具体用于：

根据所述目标对应的图像点云的世界坐标获得所述目标相对于所述无人机的距离和高度。

在一些实施例中，所述第一图像位置为所述目标在所述目标图像中的最小外接框的最高点和最低点；

所述第二目标位置获取模块具体用于：

基于所述最高点的纵坐标v_min、所述最低点的纵坐标v_max、所述目标的预设高度M和所述俯仰角，根据所述目标与所述摄像装置构建的几何关系获得所述第二目标位置。

在一些实施例中，所述第二目标位置获取模块具体用于：

根据所述最低点的纵坐标v_max获得所述最低点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角β；

根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角α；

根据所述夹角β和所述夹角α获得由所述最高点与所述摄像装置的光心之间的连线、与所述最低点与所述摄像装置的光心之间的连线所构成的夹角

其中，

根据所述夹角β、所述摄像装置的俯仰角θ，获得所述最低点与所述摄像装置的光心之间的连线与竖直方向构成的夹角δ，其中，

根据所述夹角δ、所述夹角

所述预设高度M和所述几何关系获得所述目标相对于所述无人机的距离和高度。

在一些实施例中，所述第二目标位置获取模块具体用于：

通过以下二元方程获得所述最低点与所述摄像装置的光心的连线长度L₂，以及所述最高点与所述摄像装置的光心的连线长度L₂：

通过公式d＝L₂sinδ计算所述目标相对于所述无人机的距离d；

通过公式

计算所述目标相对于所述无人机的高度h。

在一些实施例中，所述第二目标位置获取模块具体用于：

利用以下公式计算所述夹角β：

β＝tan^-1(v_max-c_y)/f_y；

其中，c_y为所述摄像装置的主点纵坐标，f_y为所述摄像装置在y轴方向上的焦距；

所述根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角α，包括：

利用以下公式计算所述夹角α：

α＝tan^-1(v_min-c_y)/f_y。

在一些实施例中，所述目标图像区域为所述目标在所述目标图像中的最小外接框，所述第二图像位置为所述最小外接框的中心坐标。

在一些实施例中，所述初始化模块具体用于：

判断是否获取到所述第一目标位置，

若是，则根据所述第一目标位置对所述目标状态进行初始化，

若否，则根据所述第二目标位置对所述目标状态进行初始化。

在一些实施例中，所述递推模块具体用于：

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置时，将三者按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中任意两项时，将该任意两项按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当仅获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中的一项时，则将该项作为所述扩展卡尔曼滤波器的测量值，获得目标状态。

在一些实施例中，所述递推模块具体用于：

所述扩展卡尔曼滤波器包括预测阶段和更新阶段；

在更新阶段，将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，更新更新方程，获得更新值；

在预测阶段，将所述更新阶段得到的更新值代入所述预测阶段的预测方程，获得所述目标状态。

第五方面，本发明实施例提供了一种无人机，所述无人机包括机身、与所述机身相连的机臂、设于所述机臂的动力系统、设置于所述机身的跟踪系统；其中，所述跟踪系统包括控制器，所述控制器包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行上述的方法。

第六方面，本发明实施例提供了一种非易失性计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被无人机执行时，使所述无人机执行上述的方法。

第七方面，本申请实施例还提供了一种计算机程序产品，所述计算机程序产品包括存储在非易失性计算机可读存储介质上的计算机程序，所述计算机程序包括程序指令，当所述程序指令被无人机执行时，使所述无人机执行上述的方法。

本发明实施例的目标状态估计方法、装置和无人机，先对目标图像进行图像识别获得所述目标在目标图像中的目标图像区域，然后根据无人机周围环境的原始点云和目标图像区域获取第一目标位置、根据所述目标图像区域确定的第一图像位置和摄像装置的姿态信息获取第二目标位置、根据目标图像区域获取第二图像位置，再利用扩展卡尔曼滤波器、用第一目标位置或者第二目标位置对目标状态进行初始化、将第一目标位置和/或第二目标位置和/或第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态。本发明实施例不需强假设条件，不依赖地平面和外界的高度信息，可实现对基于目标跟踪的无人机自主避障系统中的目标状态的精确估计。

附图说明

一个或多个实施例通过与之对应的附图中的图片进行示例性说明，这些示例性说明并不构成对实施例的限定，附图中具有相同参考数字标号的元件表示为类似的元件，除非有特别申明，附图中的图不构成比例限制。

图1是本发明实施例目标状态估计方法和装置的应用场景示意图；

图2是本发明无人机的一个实施例的结构示意图；

图3a是本发明目标状态估计方法的一个实施例的流程示意图；

图3b是本发明目标状态估计方法的一个实施例的流程示意图；

图4是本发明目标状态估计方法的一个实施例中获取第一目标位置的流程示意图；

图5是本发明目标状态估计方法的一个实施例中目标和跟踪系统的摄像装置满足的几何关系示意图；

图6是本发明目标状态估计方法的一个实施例中获取第二目标位置的流程示意图；

图7是本发明目标状态估计方法的一个实施例中获取第二图像位置的流程示意图；

图8a是本发明目标状态估计装置的一个实施例的结构示意图；

图8b是本发明目标状态估计装置的一个实施例的结构示意图；

图9是本发明无人机的一个实施例中跟踪系统的控制器的硬件结构示意图。

具体实施方式

为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有作出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。

本发明实施例提供的目标状态估计方法和装置可以应用于如图1所示的应用场景，在图1所示的应用场景中，包括无人机100和目标300。无人机100可以用于跟踪目标300，在无人机100跟踪目标300的过程中，有可能会遇到障碍物400。无人机100需跟踪目标300的同时躲避障碍物400实现正常飞行。

其中，无人机100可以为合适的无人飞行器包括固定翼无人飞行器和旋转翼无人飞行器，例如直升机、四旋翼机和具有其它数量的旋翼和/或旋翼配置的飞行器。无人机100还可以是其他可移动物体，例如载人飞行器、航模、无人飞艇、无人热气球和机器人等。目标300可以为任何合适的可移动或不可移动物体，包括交通工具、人、动物、建筑物、山川河流等。障碍物400例如建筑物、山体、树木、森林、信号塔或其他可移动或不可移动物体(图1中只示出了一个障碍物，实际应用中可能会有更多障碍物或者没有障碍物)。

其中，在一些实施例中，请参照图2，无人机100包括机身10、与所述机身10相连的机臂(图中未示出)、设于机臂的动力系统(图中未示出)和设于机身10的控制系统。动力系统用于提供无人机100飞行的推力、升力等，控制系统是无人机100的中枢神经，可以包括多个功能性单元，例如飞控系统20、跟踪系统30、路径规划系统50、视觉系统40以及其他具有特定功能的系统。跟踪系统30和视觉系统40均包括摄像装置和控制芯片，跟踪系统30用于获得跟踪目标的状态、跟踪距离(即无人机100距目标的距离)等，视觉系统40用于提供环境地图或无人机周围环境的原始点云等。飞控系统20包括各类传感器(例如陀螺仪、加速计等)，飞控系统20用于获得实时的无人机位置以及控制无人机飞行姿态等。路径规划系统50用于对路径进行规划，并指示飞控系统20控制无人机100的飞行姿态以使无人机100按指定路径飞行。

实际应用时，飞控系统20、路径规划系统50可以设置于机身10内部，跟踪系统30和视觉系统40可以设置于机身10外部并固定于机身10上。跟踪系统30和视觉系统40的摄像装置可以组成单目或双目视觉系统，其中，摄像装置可以为高清数码相机或其他摄像装置，摄像装置可以设置于任何利于拍摄的合适位置，在一些实施例中，跟踪系统30的摄像装置通过云台安装于机身10的底部，视觉系统40的摄像装置设置于机身10的前部和/或下部。其中，各个系统可以分别设置，在一些实施例中，上述系统中的部分或全部也可以集成在一个或多于一个的装置中。

在一些实施例中，无人机100根据目标特征对目标进行跟踪，其中，在部分实施例中，目标特征事先存储于无人机100中，在部分实施例中，目标特征通过其他途径获得。在无人机100的一些应用场景中还包括电子设备200，目标特征可以通过电子设备200发送给无人机100。具体的，电子设备200可以显示无人机100拍摄的图片，由用户对图片中的目标进行框选，用户框选的目标图片上传无人机100后，无人机100可以根据该框选的目标图片提取目标特征。无人机100和电子设备200之间，可以通过分别设置在各自内部的无线通信模块(例如信号接收器、信号发送器等)建立通信连接，上传或者下发数据/指令。其中，电子设备200例如智能手机、平板电脑、电脑、遥控器等。

无人机100在跟踪目标和躲避障碍物飞行的过程中，需要根据目标状态规划合适的路径，按规划的路径飞行以在保持跟踪的前提下躲避障碍物。因此，目标状态的估计至关重要。本发明实施例利用多种方法获得目标位置的相关值，并将该目标位置的相关值作为扩展卡尔曼滤波器的测量值，利用扩展卡尔曼滤波器进行递推估计获得目标状态的估计值。本发明实施例不需强假设条件，状态估计的精度高。其中，目标状态例如目标的位置、速度等。

图3a为本发明实施例提供的一种目标状态估计方法的流程示意图，所述方法可以由无人机执行(例如图1中的无人机100，具体的，在一些实施例中，所述方法由无人机100中的跟踪系统执行)，如图3a所示，所述方法包括：

101：获取所述摄像装置采集的目标图像，并对所述目标图像进行图像识别，以获取目标在所述目标图像中的目标图像区域。

其中，在一些实施例中，可以利用无人机跟踪系统的摄像装置获得目标图像。对目标图像进行图像识别，可以预先基于目标特征训练跟踪器进行识别。其中，跟踪器可以是其他装置通过训练跟踪模型获得之后直接加载在无人机100上的。在另一些实施例中，跟踪器是无人机100自身通过训练跟踪模型获得的。所述跟踪器可以为基于核相关滤波算法(Kernel Correlation Filter，KCF)的跟踪器，也可以采用其他相关滤波跟踪器。

将目标图像输入跟踪器即可获得目标在目标图像中的目标图像区域，其中，在一些实施例中，所述目标图像区域为所述目标在所述目标图像中的最小外接框，所述最小外接框例如在所述目标图像中框住目标的最小外接矩形区域。

102：获取所述无人机周围环境的原始点云，根据所述目标图像区域和所述原始点云获取第一目标位置。

其中，该周围环境的原始点云可以通过视觉系统的摄像装置，例如深度传感器或者双目相机获得。当跟踪系统获取的图像和视觉系统获取的图像的视角存在重叠区域时，如果恰好目标位于该重叠区域，则能确定视觉系统获取的周围环境的原始点云中哪些位置点是目标对应的位置点，通过这些位置点可以确定目标的第一目标位置。其中，所述第一目标位置例如目标相对于无人机的距离和高度。但是当目标不在所述重叠区域或者不存在重叠区域时，通过此种方法将无法获得第一目标位置。

因此，在其中一些实施例中，可以先判断目标是否位于重叠区域，如果目标位于重叠区域，再获取第一目标位置。具体的，如图4所示，步骤102包括：

1021：将所述原始点云变换至所述目标图像所在的图像坐标系下，获得所述原始点云对应的图像点云。

因为原始点云位于视觉系统的摄像装置坐标系下，目标图像位于跟踪系统的摄像装置的图像坐标系下，因此，需将二者先统一到一个相同的坐标系。在本实施例中，将原始点云变换到目标图像所在的图像坐标系。可以先将视觉系统的摄像装置坐标系下的原始点云坐标变换到无人机的机身坐标系下，再从机身坐标系变换到图像坐标系。如果以p表示视觉系统的摄像装置坐标系下某点的三维坐标，p′表示该点投影到图像坐标系下的二维坐标，则有：

其中，T1为所述视觉系统的摄像装置坐标系至机身坐标系的转换矩阵，T2为机身坐标系至图像坐标系的转换矩阵，K_intrinsic为跟踪系统的摄像装置的内参矩阵，s表示为使左侧矩阵的第三个坐标为1提取的公因数。由于T1、T2和K_intrinsic可以通过事先标定获得，因此通过上式可以获得原始点云中任一点投影至图像坐标系的二维坐标，即获得原始点云在图像坐标系下对应的图像点云。

1022：确定是否存在目标对应的图像点云，所述目标对应的图像点云为位于所述目标图像区域的图像点云，如果存在所述目标对应的图像点云，则根据所述目标对应的图像点云获得第一目标位置。

如果目标位于双目相机的深度图和云台图像视角的重叠区域，则原始点云投影至图像坐标系获得的图像点云中存在位于目标图像区域内的图像点云，该部分图像点云即是目标对应的图像点云。在目标图像区域为目标在所述目标图像中的最小外接框的场合，确定是否存在目标对应的图像点云，即是确定是否存在坐标位于最小外接框内的图像点云。

根据目标对应的图像点云获得第一目标位置。例如，根据目标对应的图像点云的世界坐标获得目标相对于无人机的距离和高度。首先，建立无人机100周围环境的三维点云地图，具体的，在其中一些实施例中，将视觉系统获取的所述原始点云和与目标对应的图像点云均转换至世界坐标系下，并在世界坐标系下将二者进行点云合并，并采用体素滤波、或者八叉树等方法将所有点云进行后处理，得到无人机100周围环境的三维点云地图。然后，根据处理后的目标对应的图像点云的世界坐标可以获得目标在所述三维点云地图中的位置，该位置可以是目标对应的各个图像点云的世界坐标的平均值。根据目标位置和无人机通过飞控系统获得的无人机位置即可获得目标相对于无人机的距离和高度。

在其中一些实施例中，获得目标对应的图像点云后，还可以对该图像点云进行滤波处理，以去除不属于目标的图像点云。具体的，可以采用高斯滤波法、半径滤波法、聚类法、平滑法、连通域法等滤波算法进行滤波处理。

103：获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置。

其中，第一图像位置为目标图像区域相关的位置坐标。

其中，跟踪系统的摄像装置与目标之间满足一定的几何关系，通过该几何关系和其中的一些已知量可以获得第二目标位置。其中，第二目标位置例如目标相对于无人机的距离和高度。所述姿态信息为表示跟踪系统的摄像装置的状态或姿态的信息，包括摄像装置的俯仰角等。

图5为目标和摄像装置满足的其中一种几何关系的示意图，通过这样的几何关系可以求得第二目标位置。在摄像装置的姿态信息为摄像装置的俯仰角的场合(图中θ表示摄像装置的俯仰角)，当目标靠近无人机下方时，摄像装置的俯仰角较大，有可能导致目标在跟踪系统获取的图像中的侧面占比较小，此时，利用该方法获得的第二目标位置精确度不高。因此，在其中一些实施例中，为了提高整体计算的精确度，在所述俯仰角较大时，例如大于预设的预设角度阈值时，不利用此方法获得第二目标位置，即不将第二目标位置作为扩展卡尔曼滤波器的测量值。只有当所述俯仰角小于或者等于预设角度阈值时，才利用该方法获得第二目标位置。

即在其中一些实施例中，如图6所示，步骤103包括：

1031：获取所述摄像装置的俯仰角，如果所述俯仰角小于或者等于预设角度阈值，则基于所述目标图像区域获得第一图像位置，并根据所述第一图像位置、所述俯仰角以及所述目标与所述摄像装置构建的几何关系获得第二目标位置。

其中，预设角度阈值可以根据实际应用情况取值，例如可以取60度左右(以水平位置为0度，向下旋转60度)。第一图像位置例如目标图像区域相关的坐标位置，在其中一个实施例中，请参照图5，第一图像位置为目标在目标图像中的最小外接框的最高点和最低点的二维坐标(图5中用v_min表示所述最高点的纵坐标，用v_max表示所述最低点的纵坐标)。由于无人机跟踪的目标的种类通常是事先已知的，因此目标的高度可以事先设定，例如可以通过用户将目标的预设高度输入电子设备中，再通过电子设备发送给无人机。

已知最高点的纵坐标v_min、最低点的纵坐标v_max、目标的预设高度M和俯仰角，根据目标与摄像装置构建的特定几何关系可以获得第二目标位置。以下以图5为例说明计算获得第二目标位置的过程。具体的，在其中一些实施例中，可以通过以下步骤实现：

A：根据最低点的纵坐标v_max获得最低点与摄像装置的光心的连线与所述摄像装置的光轴之间的夹角β。

具体的，在其中一些实施例中，可以利用以下公式计算所述夹角β：

β＝tan^-1(v_max-c_y)/f_y；

其中，c_y为所述摄像装置的主点纵坐标，f_y为所述摄像装置在y轴方向上的焦距。

B：根据最高点的纵坐标v_min获得最高点与摄像装置的光心的连线与摄像装置的光轴之间的夹角α。

具体的，在其中一些实施例中，可以利用以下公式计算所述夹角α：

C：根据夹角β和夹角α获得由最高点与摄像装置的光心之间的连线、与最低点与摄像装置的光心之间的连线所构成的夹角

其中，

D：根据夹角β、摄像装置的俯仰角θ，获得最低点与摄像装置的光心之间的连线与竖直方向构成的夹角δ，其中，

E：根据夹角δ、夹角

预设高度M和所述几何关系获得所述目标相对于所述无人机的距离和高度。

具体的，在其中一个实施例中，目标和摄像装置满足以下关系

以及

其中，L₂为最低点与摄像装置的光心连线长度，L₁为最高点与摄像装置的光心连线长度，通过求解上述二元方程可以求解L₁和L₂。

从而通过公式d＝L₂sinδ可以计算获得目标相对于无人机的距离d，通过公式

可以计算获得目标相对于无人机的高度h。

104：基于所述目标图像区域获取第二图像位置。

其中，第二图像位置可以是目标图像区域在目标图像中的相关坐标位置，例如最小外接框的中心的二维坐标。当目标图像区域在目标图像中占比较小时，例如占比小于预设比例阈值，将第二图像位置作为扩展卡尔曼滤波器的测量值将无法提高精度。因此，仅在目标图像区域与所述目标图像的区域之比值大于或者等于预设比例阈值时，才获取该第二图像位置，并将获取的第二图像位置作为扩展卡尔曼滤波器的测量值。其中，预设比例阈值可以根据实际应用情况取值，例如0.1％。

即，在其中一些实施例中，如图7所示，步骤104包括：

1041：如果所述目标图像区域与所述目标图像的区域之比值大于或者等于预设比例阈值，则基于所述目标图像区域获得第二图像位置。

105：根据所述第一目标位置或者所述第二目标位置对目标状态进行初始化。

以下以目标状态为目标的位置和速度为例说明利用扩展卡尔曼滤波器中进行递推估计目标状态的过程，定义目标状态量为X＝[p_x,p_y,p_z,v_x,v_y,v_z]^T。其中，(p_x,p_y,p_z)为世界坐标系下目标的位置，(v_x,v_y,v_z)为世界坐标系下目标的速度。在扩展卡尔曼滤波器中，需先对目标状态进行初始化，具体的，可以利用第一目标位置或者第二目标位置对目标的位置(p_x,p_y,p_z)进行初始化，速度初始化为(0,0,0)，协方差P初始化为：

利用第一目标位置或者第二目标位置对目标的位置进行初始化，例如，如果获取到第一目标位置，则利用第一目标位置对目标的位置进行初始化，否则，利用第二目标位置对目标的位置进行初始化。当然，也可以选择第一目标位置和第二目标位置中的任何一个对目标的位置进行初始化。

106：将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态。

其中，可以将第一目标位置、第二目标位置和第二图像位置中的一个或多个作为扩展卡尔曼滤波器的测量值，获得目标状态X。

在其中一个实施例中，在保证精确度的前提下尽量多的将第一目标位置、第二目标位置和第二图像位置中的位置值作为扩展卡尔曼滤波器的测量值，获得目标状态X。即如果获取到第一目标位置和第二目标位置，则将第一目标位置和第二目标位置均作为扩展卡尔曼滤波器的测量值；如果获取到第一目标位置、第二目标位置和第二图像位置，则将第一目标位置、第二目标位置和第二图像位置均作为扩展卡尔曼滤波器的测量值。在本实施例中，步骤106包括：

a：如果目标图像区域存在目标对应的图像点云，则将第一目标位置作为扩展卡尔曼滤波器的测量值更新目标状态和协方差。

b：如果摄像装置的俯仰角小于或者等于预设角度阈值，则将第二目标位置作为扩展卡尔曼滤波器的测量值更新目标状态和协方差；否则，不将第二目标位置作为扩展卡尔曼滤波器的测量值。

c：如果所述目标图像区域与所述目标图像的区域之比值大于或者等于预设比例阈值，则将第二图像位置作为扩展卡尔曼滤波器的测量值更新目标状态和协方差；否则，不将第二图像位置作为扩展卡尔曼滤波器的测量值。

扩展卡尔曼滤波算法的核心思想在于预测+测量反馈，它由两部分组成，第一部分是状态预测方程，第二部分是观测方程。在预测阶段，要通过系统状态方程计算的预测值作为先验信息，之后在观测部分再重新更新这个信息。扩展卡尔曼滤波算法为现有技术，具体相关内容可以参见现有技术，以下以其中一个实施例为例，说明利用扩展卡尔曼滤波器获得目标状态的过程。

在预测阶段，目标状态和协方差方程分别为：

X’(k)＝F·X’(k-1)                (1)

P’(k)＝F·P’(k-1)F^T+Q          (2)

其中X‘(k)为目标状态预测值，P‘(k)为协方差预测值，Q为噪声矩阵；

在预测阶段

在预测阶段，通过初始值对式(1)和式(2)进行迭代可以获得预测值X’(k)和P’(k)。

在观测阶段，更新方程为：

X(k)＝X’(k)+K·err  (3)

P(k)＝(I-K·H)·P’(k)  (4)

其中，K为卡尔曼增益，

K＝P’(k)·H·S               (5)

S＝M·P’(k)·H^T+R          (6)

其中，R为测量噪声方差，H为状态到观测的转换矩阵，err为测量值与估计值之间的误差。由于R和H为已知量，则S可以通过式(6)获得，从而K可以通过式(5)获得。因此，只要求出err即可通过式(3)获得经更新的X(k)。

将更新后的X(k)和P(k)作为预测方程的输入值，即将X(k)作为X’(k-1)，P(k)作为P’(k-1)，最终得到预测的目标状态X’(k)和P’(k)。

如果第一目标位置、第二目标位置和第二图像位置均作为测量值，则在观测阶段，将三者按任意顺序对更新方程(3)进行迭代更新，获得更新值X(k)。如果将第一目标位置、第二目标位置和第二图像位置中的任意两项作为测量值，则将该任意两项按任意顺序对更新方程(3)进行迭代更新，获得更新值X(k)。如果仅将第一目标位置、第二目标位置和第二图像位置中的一项作为测量值，则利用该项对更新方程(3)进行迭代更新，获得更新值X(k)。

以将第一目标位置、第二目标位置和第二图像位置均作为测量值为例说明，将预测阶段获得的X‘(k)以及利用第一目标位置获得的err代入更新方程(3)获得第一更新值。再将该第一更新值作为下一次更新的输入，即将该第一更新值作为X‘(k)、将该第一更新值和利用第二目标位置获得的err代入更新方程(3)获得第二更新值。然后，继续将该第二更新值作为下一次更新的输入，即将该第二更新值作为X‘(k)、将该第二更新值和利用第二图像位置获得的err代入更新方程(3)获得最终的更新值。

测量值第一目标位置、第二目标位置和第二图像位置，其err求法各不相同，对于第二图像位置而言，以第二图像位置为最小外接框的中心坐标为例说明。测量值为最小外接框中心坐标[b_x,b_y]^T，估计值在图像坐标系的投影坐标[x,y]^T为：

其中，M_mask为位置掩膜，具体表达式为：

T_w2g为从世界坐标系到相机坐标系的转换矩阵，该矩阵可从飞控系统获得，K_intrinsic为跟踪系统摄像装置的内参矩阵，可通过相机标定技术获得,s表示为使左侧矩阵的第三个坐标为1提取的公因数。

则，在该实施例中,err为：

对于第一目标位置和第二目标位置而言，以第一目标位置和第二目标位置分别为目标相对于无人机的距离和高度为例说明。测量值为获得的距离d和高度h。

距离d的估计值d′的表达式为：

d′＝M_dis·T_w2g·M_mask·X

其中，M_dis＝[0,0,1]，则err＝d-d′。

高度h的估计值h′的表达式为：

h′＝M_altitude·X

其中，M_altitude＝[0,0,1,0,0,0]，则err＝h-h′。

本发明实施例先对目标图像进行图像识别获得所述目标在目标图像中的目标图像区域，然后根据无人机周围环境的原始点云和目标图像区域获取第一目标位置、根据所述目标图像区域确定的第一图像位置和摄像装置的姿态信息获取第二目标位置、根据目标图像区域获取第二图像位置，再利用扩展卡尔曼滤波器、用第一目标位置或者第二目标位置对目标状态进行初始化、将第一目标位置和/或第二目标位置和/或第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态。本发明实施例不依赖地平面假设和无人机对地高度数据,在目标被遮挡情况下亦可对目标运动进行较精确预测，鲁棒性好、精度高。其中，需要说明的是，本发明实施例中的序号仅用于标识各方法步骤，并不用于表示先后顺序。

需要说明的是，上述步骤标号(例如101、102等)，仅用于标识各步骤，并不用于限定各步骤的先后顺序。

图3b示出了本发明目标状态估计方法的又一实施例，图3b所示的实施例适用于无人机无法获得周围环境地图的场合。图3b与图3a所示实施例的区别在于，图3b所示的实施例不包括步骤102，即在该实施例中，仅利用第二目标位置和/或第二图像位置对目标状态进行估计。

相应的，如图8a所示，本发明实施例还提供了一种目标状态估计装置，所述装置可以用于无人机(例如图1所示的无人机)，目标状态估计装置800包括：

目标图像区域获取模块801，用于获取所述摄像装置采集的目标图像，并对所述目标图像进行图像识别，以获取目标在所述目标图像中的目标图像区域；

第一目标位置获取模块802，用于获取所述无人机周围环境的原始点云，根据所述目标图像区域和所述原始点云获取第一目标位置；

第二目标位置获取模块803，用于获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置；

第二图像位置获取模块804，用于基于所述目标图像区域获取第二图像位置；

初始化模块805，用于根据所述第一目标位置或者所述第二目标位置对目标状态进行初始化；

递推模块806，用于将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态。

本发明实施例先对目标图像进行图像识别获得所述目标在目标图像中的目标图像区域，然后根据无人机周围环境的原始点云和目标图像区域获取第一目标位置、根据所述目标图像区域确定的第一图像位置和摄像装置的姿态信息获取第二目标位置、根据目标图像区域获取第二图像位置，再利用扩展卡尔曼滤波器、用第一目标位置或者第二目标位置对目标状态进行初始化、将第一目标位置和/或第二目标位置和/或第二图像位置作为扩展卡尔曼滤波器的测量值，获得目标状态。本发明实施例不依赖地平面假设和无人机对地高度数据,在目标被遮挡情况下亦可对目标运动进行较精确预测，鲁棒性好、精度高。

在其中一些实施例中，第一目标位置获取模块802具体用于：

将所述原始点云变换至所述目标图像所在的图像坐标系下，获得所述原始点云对应的图像点云；

确定是否存在目标对应的图像点云，所述目标对应的图像点云为位于所述目标图像区域的图像点云，如果存在所述目标对应的图像点云，则根据所述目标对应的图像点云获得第一目标位置。

在其中一些实施例中，所述姿态信息为所述摄像装置的俯仰角；

第二目标位置获取模块803具体用于：

获取所述摄像装置的俯仰角，如果所述俯仰角小于或者等于预设角度阈值，则基于所述目标图像区域获得第一图像位置，并根据所述第一图像位置、所述俯仰角以及所述目标与所述摄像装置构建的几何关系获得第二目标位置。

在其中一些实施例中，第二图像位置获取模块804具体用于：

如果所述目标图像区域的大小与所述目标图像的区域的大小之比值大于或者等于预设比例阈值，则基于所述目标图像区域获得第二图像位置。

在其中一些实施例中，所述目标图像区域为所述目标在所述目标图像中的最小外接框；

所述目标对应的图像点云为各图像点云中坐标位于所述最小外接框内的图像点云。

在其中一些实施例中，第一目标位置获取模块802具体用于：

根据所述目标对应的图像点云的世界坐标获得所述目标相对于所述无人机的距离和高度。

在其中一些实施例中，第一图像位置为所述目标在所述目标图像中的最小外接框的最高点和最低点；

第二目标位置获取模块803具体用于：

基于所述最高点的纵坐标v_min、所述最低点的纵坐标v_max、所述目标的预设高度M和所述俯仰角，根据所述目标与所述摄像装置构建的几何关系获得所述第二目标位置。

在其中一些实施例中，第二目标位置获取模块803具体用于：

根据所述最低点的纵坐标v_max获得所述最低点与所述摄像装置光心的连线与所述摄像装置的光轴之间的夹角β；

根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置光心的连线与所述摄像装置的光轴之间的夹角α；

根据所述夹角β和所述夹角α获得由所述最高点与所述摄像装置光心之间的连线、与所述最低点与所述摄像装置光心之间的连线所构成的夹角

其中，

根据所述夹角β、所述摄像装置的俯仰角θ，获得所述最低点与所述摄像装置光心之间的连线与竖直方向构成的夹角δ，其中，

根据所述夹角δ、所述夹角

所述预设高度M和所述几何关系获得所述目标相对于所述无人机的距离和高度。

在其中一些实施例中，第二目标位置获取模块803具体用于：

通过以下二元方程获得所述最低点与所述摄像装置的光心连线长度L₂，以及所述最高点与所述摄像装置的光心连线长度L₂：

通过公式d＝L₂sinδ计算所述目标相对于所述无人机的距离d；

通过公式

计算所述目标相对于所述无人机的高度h。

在其中一些实施例中，第二目标位置获取模块803具体用于：

利用以下公式计算所述夹角β：

β＝tan^-1(v_max-c_y)/f_y；

其中，c_y为所述摄像装置的主点纵坐标，f_y为所述摄像装置在y轴方向上的焦距；

所述根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置光心的连线与所述摄像装置的光轴之间的夹角α，包括：

利用以下公式计算所述夹角α：

α＝tan^-1(v_min-c_y)/f_y。

在其中一些实施例中，所述目标图像区域为所述目标在所述目标图像中的最小外接框，所述第二图像位置为所述最小外接框的中心坐标。

在其中一些实施例中，初始化模块805具体用于：

判断是否获取到所述第一目标位置，

若是，则根据所述第一目标位置对所述目标状态进行初始化，

若否，则根据所述第二目标位置对所述目标状态进行初始化。

在其中一些实施例中，递推模块806具体用于：

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置时，将三者按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中任意两项时，将该任意两项按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当仅获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中的一项时，则将该项作为所述扩展卡尔曼滤波器的测量值，获得目标状态。

在其中一些实施例中，递推模块806具体用于：

所述扩展卡尔曼滤波器包括预测阶段和更新阶段；

在更新阶段，将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，更新更新方程，获得更新值；

在预测阶段，将所述更新阶段得到的更新值代入所述预测阶段的预测方程，获得所述目标状态。

需要说明的是，上述装置可执行本申请实施例所提供的方法，具备执行方法相应的功能模块和有益效果。未在装置实施例中详尽描述的技术细节，可参见本申请实施例所提供的方法。

图8b示出了本发明目标状态估计装置的又一实施例，图8b所示的实施例适用于无人机无法获得周围环境地图的场合。图8b与图8a所示实施例的区别在于，图8b所示的实施例不包括第一目标位置获取模块802，即在该实施例中，仅利用第二目标位置和/或第二图像位置对目标状态进行估计。

图9是本发明无人机的一个实施例中跟踪系统中控制器31的硬件结构示意图，如图9所示，控制器31包括：

一个或多个处理器31a以及存储器31b，图9中以一个处理器31a为例。

处理器31a和存储器31b可以通过总线或者其他方式连接，图9中以通过总线连接为例。

存储器31b作为一种非易失性计算机可读存储介质，可用于存储非易失性软件程序、非易失性计算机可执行程序以及模块，如本申请实施例中的目标状态估计方法对应的程序指令/模块(例如，附图8a所示的目标图像区域获取模块801、第一目标位置获取模块802、第二目标位置获取模块803、第二图像位置获取模块804、初始化模块805和递推模块806)。处理器31a通过运行存储在存储器31b中的非易失性软件程序、指令以及模块，从而执行控制器的各种功能应用以及数据处理，即实现上述方法实施例的目标状态估计方法。

存储器31b可以包括存储程序区和存储数据区，其中，存储程序区可存储操作系统、至少一个功能所需要的应用程序；存储数据区可存储根据控制器的使用所创建的数据等。此外，存储器31b可以包括高速随机存取存储器，还可以包括非易失性存储器，例如至少一个磁盘存储器件、闪存器件、或其他非易失性固态存储器件。在一些实施例中，存储器31b可选包括相对于处理器31a远程设置的存储器，这些远程存储器可以通过网络连接至控制器。上述网络的实例包括但不限于互联网、企业内部网、局域网、移动通信网及其组合。

所述一个或者多个模块存储在所述存储器31b中，当被所述一个或者多个处理器31a执行时，执行上述任意方法实施例中的目标状态估计方法，例如，执行以上描述的图3a中的方法步骤101至步骤106、图4中的方法步骤1021-1022、图6中的方法步骤1031、图7中的方法步骤1041；实现图8a中的模块801-806的功能。

上述产品可执行本申请实施例所提供的方法，具备执行方法相应的功能模块和有益效果。未在本实施例中详尽描述的技术细节，可参见本申请实施例所提供的方法。

本申请实施例提供了一种非易失性计算机可读存储介质，所述计算机可读存储介质存储有计算机可执行指令，该计算机可执行指令被一个或多个处理器执行，例如图9中的一个处理器31a，可使得上述一个或多个处理器可执行上述任意方法实施例中的目标状态估计方法，执行以上描述的图3a中的方法步骤101至步骤106、图4中的方法步骤1021-1022、图6中的方法步骤1031、图7中的方法步骤1041；实现图8a中的模块801-806的功能。

以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。

通过以上的实施例的描述，本领域普通技术人员可以清楚地了解到各实施例可借助软件加通用硬件平台的方式来实现，当然也可以通过硬件。本领域普通技术人员可以理解实现上述实施例方法中的全部或部分流程是可以通过计算机程序来指令相关的硬件来完成，所述的程序可存储于一计算机可读取存储介质中，该程序在执行时，可包括如上述各方法的实施例的流程。其中，所述的存储介质可为磁碟、光盘、只读存储记忆体(Read-OnlyMemory,ROM)或随机存储记忆体(RandomAccessMemory,RAM)等。

最后应说明的是：以上实施例仅用以说明本发明的技术方案，而非对其限制；在本发明的思路下，以上实施例或者不同实施例中的技术特征之间也可以进行组合，步骤可以以任意顺序实现，并存在如上所述的本发明的不同方面的许多其它变化，为了简明，它们没有在细节中提供；尽管参照前述实施例对本发明进行了详细的说明，本领域的普通技术人员应当理解：其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换；而这些修改或者替换，并不使相应技术方案的本质脱离本发明各实施例技术方案的范围。

Claims (22)

1.一种目标状态估计方法，所述方法用于无人机，所述无人机包括摄像装置，其特征在于，所述方法包括：

获取所述摄像装置采集的目标图像，并对所述目标图像进行图像识别，以获取目标在所述目标图像中的目标图像区域；

获取所述无人机周围环境的原始点云，根据所述目标图像区域和所述原始点云获取第一目标位置；

获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置；

如果所述目标图像区域的大小与所述目标图像的区域的大小之比值大于或者等于预设比例阈值，则基于所述目标图像区域获得第二图像位置，所述目标图像区域为所述目标在所述目标图像中的最小外接框，所述第二图像位置为所述最小外接框的中心坐标；

判断是否获取到所述第一目标位置，若是，则根据所述第一目标位置对所述目标状态进行初始化，将第一目标位置作为初始值，通过迭代计算，获得预测阶段扩展卡尔曼滤波器的预测值，

若否，则根据所述第二目标位置对所述目标状态进行初始化，将第二目标位置作为初始值，通过迭代计算，获得预测阶段扩展卡尔曼滤波器的预测值；

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置时，将三者按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中任意两项时，将该任意两项按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当仅获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中的一项时，则将该项作为所述扩展卡尔曼滤波器的测量值，获得目标状态；

其中，所述目标图像为二维图像，所述第一和第二图像位置均为所述目标图像区域在所述目标图像中的位置表示方式。

2.根据权利要求1所述的方法，其特征在于，所述根据所述目标图像区域和所述原始点云获取第一目标位置，包括：

将所述原始点云变换至所述目标图像所在的图像坐标系下，获得所述原始点云对应的图像点云；

确定是否存在目标对应的图像点云，所述目标对应的图像点云为位于所述目标图像区域的图像点云，如果存在所述目标对应的图像点云，则根据所述目标对应的图像点云获得第一目标位置。

3.根据权利要求1-2任意一项所述的方法，其特征在于，所述姿态信息为所述摄像装置的俯仰角；

所述获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置，包括：

获取所述摄像装置的俯仰角，如果所述俯仰角小于或者等于预设角度阈值，则基于所述目标图像区域获得第一图像位置，并根据所述第一图像位置、所述俯仰角以及所述目标与所述摄像装置构建的几何关系获得第二目标位置。

4.根据权利要求2所述的方法，其特征在于，

所述目标对应的图像点云为各图像点云中坐标位于所述最小外接框内的图像点云。

5.根据权利要求2或4所述的方法，其特征在于，所述根据所述目标对应的图像点云获得第一目标位置，包括：

根据所述目标对应的图像点云的世界坐标获得所述目标相对于所述无人机的距离和高度。

6.根据权利要求3所述的方法，其特征在于，所述第一图像位置为所述目标在所述目标图像中的最小外接框的最高点和最低点；

所述根据所述第一图像位置、所述俯仰角以及所述目标与所述摄像装置构建的几何关系获得第二目标位置，包括：

基于所述最高点的纵坐标v_min、所述最低点的纵坐标v_max、所述目标的预设高度M和所述俯仰角，根据所述目标与所述摄像装置构建的几何关系获得所述第二目标位置。

7.根据权利要求6所述的方法，其特征在于，所述基于所述最高点的纵坐标v_min、所述最低点的纵坐标v_max、所述目标的预设高度M和所述俯仰角，根据所述目标与所述摄像装置构建的几何关系获得所述第二目标位置，包括：

根据所述最低点的纵坐标v_max获得所述最低点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角β；

根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角α；

根据所述夹角β和所述夹角α获得由所述最高点与所述摄像装置的光心之间的连线、与所述最低点与所述摄像装置的光心之间的连线所构成的夹角

其中，

根据所述夹角β、所述摄像装置的俯仰角θ，获得所述最低点与所述摄像装置的光心之间的连线与竖直方向构成的夹角δ，其中，

根据所述夹角δ、所述夹角

所述预设高度M和所述几何关系获得所述目标相对于所述无人机的距离和高度。

8.根据权利要求7所述的方法，其特征在于，所述根据所述夹角δ、所述夹角

所述预设高度M和所述几何关系获得所述目标相对于所述无人机的距离和高度，包括：

通过以下二元方程获得所述最低点与所述摄像装置的光心的连线长度L₂，以及所述最高点与所述摄像装置的光心的连线长度L₂：

通过公式d＝L₂sinδ计算所述目标相对于所述无人机的距离d；

通过公式

计算所述目标相对于所述无人机的高度h。

9.根据权利要求7或8所述的方法，其特征在于，所述根据所述最低点的纵坐标v_max获得所述最低点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角β，包括：

利用以下公式计算所述夹角β：

其中，c_y为所述摄像装置的主点纵坐标，f_y为所述摄像装置在y轴方向上的焦距；

所述根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角α，包括：

利用以下公式计算所述夹角α：

10.根据权利要求1所述的方法，其特征在于，所述扩展卡尔曼滤波器包括预测阶段和更新阶段；

在更新阶段，将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，更新更新方程，获得更新值；

在预测阶段，将所述更新阶段得到的更新值代入所述预测阶段的预测方程，获得所述目标状态。

11.一种目标状态估计装置，所述装置用于无人机，所述无人机包括摄像装置，其特征在于，所述装置包括：

目标图像区域获取模块，用于获取所述摄像装置采集的目标图像，并对所述目标图像进行图像识别，以获取目标在所述目标图像中的目标图像区域；

第一目标位置获取模块，用于获取所述无人机周围环境的原始点云，根据所述目标图像区域和所述原始点云获取第一目标位置；

第二目标位置获取模块，用于获取所述摄像装置的姿态信息，并基于所述目标图像区域获得第一图像位置，根据所述第一图像位置和所述姿态信息获取第二目标位置；

第二图像位置获取模块，用于如果所述目标图像区域的大小与所述目标图像的区域的大小之比值大于或者等于预设比例阈值，则基于所述目标图像区域获得第二图像位置，所述目标图像区域为所述目标在所述目标图像中的最小外接框，所述第二图像位置为所述最小外接框的中心坐标；

初始化模块，用于判断是否获取到所述第一目标位置，若是，则根据所述第一目标位置对所述目标状态进行初始化，将第一目标位置作为初始值，通过迭代，获得预测阶段扩展卡尔曼滤波器的预测值，若否，则根据所述第二目标位置对所述目标状态进行初始化，将第二目标位置作为初始值，通过迭代，获得预测阶段扩展卡尔曼滤波器的预测值；

递推模块，用于：

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置时，将三者按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中任意两项时，将该任意两项按任意顺序作为所述扩展卡尔曼滤波器的测量值，对所述扩展卡尔曼滤波器的计算进行更新，最终得到所述目标状态；

当仅获取到所述第一目标位置、所述第二目标位置和所述第二图像位置中的一项时，则将该项作为所述扩展卡尔曼滤波器的测量值，获得目标状态；

其中，所述目标图像为二维图像，所述第一和第二图像位置均为所述目标图像区域在所述目标图像中的位置表示方式。

12.根据权利要求11所述的装置，其特征在于，所述第一目标位置获取模块具体用于：

将所述原始点云变换至所述目标图像所在的图像坐标系下，获得所述原始点云对应的图像点云；

确定是否存在目标对应的图像点云，所述目标对应的图像点云为位于所述目标图像区域的图像点云，如果存在所述目标对应的图像点云，则根据所述目标对应的图像点云获得第一目标位置。

13.根据权利要求11-12任意一项所述的装置，其特征在于，所述姿态信息为所述摄像装置的俯仰角；

所述第二目标位置获取模块具体用于：

获取所述摄像装置的俯仰角，如果所述俯仰角小于或者等于预设角度阈值，则基于所述目标图像区域获得第一图像位置，并根据所述第一图像位置、所述俯仰角以及所述目标与所述摄像装置构建的几何关系获得第二目标位置。

14.根据权利要求12所述的装置，其特征在于，

所述目标对应的图像点云为各图像点云中坐标位于所述最小外接框内的图像点云。

15.根据权利要求12或14所述的装置，其特征在于，所述第一目标位置获取模块具体用于：

根据所述目标对应的图像点云的世界坐标获得所述目标相对于所述无人机的距离和高度。

16.根据权利要求13所述的装置，其特征在于，所述第一图像位置为所述目标在所述目标图像中的最小外接框的最高点和最低点；

所述第二目标位置获取模块具体用于：

基于所述最高点的纵坐标v_min、所述最低点的纵坐标v_max、所述目标的预设高度M和所述俯仰角，根据所述目标与所述摄像装置构建的几何关系获得所述第二目标位置。

17.根据权利要求16所述的装置，其特征在于，所述第二目标位置获取模块具体用于：

根据所述最低点的纵坐标v_max获得所述最低点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角β；

根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角α；

根据所述夹角β和所述夹角α获得由所述最高点与所述摄像装置的光心之间的连线、与所述最低点与所述摄像装置的光心之间的连线所构成的夹角

其中，

根据所述夹角β、所述摄像装置的俯仰角θ，获得所述最低点与所述摄像装置的光心之间的连线与竖直方向构成的夹角δ，其中，

根据所述夹角δ、所述夹角

所述预设高度M和所述几何关系获得所述目标相对于所述无人机的距离和高度。

18.根据权利要求 17所述的装置，其特征在于，所述第二目标位置获取模块具体用于：

通过以下二元方程获得所述最低点与所述摄像装置的光心的连线长度L₂，以及所述最高点与所述摄像装置的光心的连线长度L₂：

通过公式d＝L₂sinδ计算所述目标相对于所述无人机的距离d；

通过公式

计算所述目标相对于所述无人机的高度h。

19.根据权利要求17或18所述的装置，其特征在于，所述第二目标位置获取模块具体用于：

利用以下公式计算所述夹角β：

其中，c_y为所述摄像装置的主点纵坐标，f_y为所述摄像装置在y轴方向上的焦距；

所述根据所述最高点的纵坐标v_min获得所述最高点与所述摄像装置的光心的连线与所述摄像装置的光轴之间的夹角α，包括：

利用以下公式计算所述夹角α：

20.根据权利要求11所述的装置，其特征在于，所述递推模块具体用于：

所述扩展卡尔曼滤波器包括预测阶段和更新阶段；

在更新阶段，将所述第一目标位置和/或所述第二目标位置和/或所述第二图像位置作为扩展卡尔曼滤波器的测量值，更新更新方程，获得更新值；

在预测阶段，将所述更新阶段得到的更新值代入所述预测阶段的预测方程，获得所述目标状态。

21.一种无人机，其特征在于，所述无人机包括机身、与所述机身相连的机臂、设于所述机臂的动力系统、设置于所述机身的跟踪系统；其中，所述跟踪系统包括控制器，所述控制器包括：

至少一个处理器；以及，

与所述至少一个处理器通信连接的存储器；其中，

所述存储器存储有可被所述至少一个处理器执行的指令，所述指令被所述至少一个处理器执行，以使所述至少一个处理器能够执行权利要求1-10任一项所述的方法。

22.一种非易失性计算机可读存储介质，其特征在于，所述计算机可读存储介质存储有计算机可执行指令，当所述计算机可执行指令被无人机执行时，使所述无人机执行如权利要求1-10任一项所述的方法。

Images