CN112991443B - 基于视觉编码器融合的悬挂负载状态估计器及估计方法 - Google Patents

基于视觉编码器融合的悬挂负载状态估计器及估计方法 Download PDF

Info

Publication number
CN112991443B
CN112991443B CN202110041597.4A CN202110041597A CN112991443B CN 112991443 B CN112991443 B CN 112991443B CN 202110041597 A CN202110041597 A CN 202110041597A CN 112991443 B CN112991443 B CN 112991443B
Authority
CN
China
Prior art keywords
load
encoder
state estimator
coordinate system
load state
Prior art date
Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
Active
Application number
CN202110041597.4A
Other languages
English (en)
Other versions
CN112991443A (zh
Inventor
陈浩耀
任云帆
刘健恒
欧阳俊源
刘云辉
Current Assignee (The listed assignees may be inaccurate. Google has not performed a legal analysis and makes no representation or warranty as to the accuracy of the list.)
Shenzhen Graduate School Harbin Institute of Technology
Original Assignee
Shenzhen Graduate School Harbin Institute of Technology
Priority date (The priority date is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the date listed.)
Filing date
Publication date
Application filed by Shenzhen Graduate School Harbin Institute of Technology filed Critical Shenzhen Graduate School Harbin Institute of Technology
Priority to CN202110041597.4A priority Critical patent/CN112991443B/zh
Publication of CN112991443A publication Critical patent/CN112991443A/zh
Application granted granted Critical
Publication of CN112991443B publication Critical patent/CN112991443B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/70—Determining position or orientation of objects or cameras
    • G06T7/73—Determining position or orientation of objects or cameras using feature-based methods
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/80—Analysis of captured images to determine intrinsic or extrinsic camera parameters, i.e. camera calibration

Abstract

本发明公开了基于视觉编码器融合的悬挂负载状态估计器及估计方法，本发明将悬挂负载物的吊绳系于由两正交编码器构成的带高速轴承的两自由度机械结构形成的负载状态估计器中，通过两个编码器测量吊绳两个摆动方向的对应角度，从而获得基于编码器的负载状态估计器所提供的负载物的位置估计值；并通过鱼眼相机获得基于基于视觉的负载状态估计器所提供的负载物的位置估计值，将基于视觉和编码器的负载状态估计器提供的位置估计值实时融合以提供负载物的准确位置估计用于解决当前无人机运输过程中对于复杂以及光线不足的环境下，跟踪稳定性较差的问题，以及解决针对复杂场景下基于视觉方法无法获取良好的悬挂负载估计的问题。

Description

基于视觉编码器融合的悬挂负载状态估计器及估计方法
技术领域
本发明涉及无人机运输领域，具体涉及基于视觉编码器融合的悬挂负载状态估计器及估计方法。
背景技术
随着机器人技术的迅速发展，在工业制造、军事行动、民用生活中得到了广泛的应用。无论军事应用、民用，多旋翼无人机运输有着很重要的作用，它们可垂直起飞、降落、悬停。逐渐地，多旋翼无人机不仅局限于运输方面，复合无人机将成为新一代空中操作机器人，将代替人类完成危险、复杂环境的空中操作任务。
近年来，通过无人机来运送货物逐渐受到越来越多的关注，拓展无人机在人常生活中应用的可能性。通过绳子将货物与无人机相连，使得无人机再不受气流扰动的情况下携带比自身体积更大的有效载荷。此外，在复杂的环境中，如丛林和其他难以着陆的场景，无人机可以使用绳子运输货物，通过故意摆动有效载荷进行快速货物运输，可以大大提高效率，降低能耗。然而，实际的货物运输通常需要有经验的直升机飞行员。由于缺乏对负载状态的有效估计，无人机货物自主运输仍然是一个具有挑战性的问题。
带悬挂负载的无人机系统是一个复杂的欠驱动系统，当悬挂负载受到外部的扰动，如无人机运行气流，风，障碍物，人为扰动等，将不可避免地引起悬挂负载的震荡，使得无人机的运动控制变得十分困难。通过视觉和编码器相结合的悬挂负载估计器能够有效地帮助无人机控制器实现抑制各种干扰的效果。目前国内关于无人机悬挂负载状态估计只有较少的相关文献，虽有少部分相关无人机悬挂控制的发明专利，但难以找到无人机悬挂负载状态估计相关的。通常采用视觉反馈来获取负载相对于无人机的真实位置，需要提前预设运行环境的光照好，背景不复杂，并且需要特殊的标签贴于负载上，无法与环境进行友好的交互，不符合真实场景。本发明正是根据这种需求提出了基于视觉编码器融合的无人机悬挂负载状态估计策略。
现有部分论文实现了机载负载的估计，例如，发表于2018年《IEEE Robotics andAutomation Letters》的文章“Aggressive Flight With Suspended Payloads UsingVision-Based Control”通过视觉跟踪的方法来估计负载的位置，但这种方法要求负载上有一个特定的人工标签(比如一个白色的圆形标记)，而视频流的低频率和高延迟使得精确估计负载的状态变得困难。发表于2017年《IEEE International Conference on Roboticsand Automation (ICRA)》的文章“Autonomous swing-angle estimation for stableslung-load flight of multi-rotor UAVs”使用惯性测量单元(IMU)来估计负载的位置，但IMU作为一种积分式传感器，在长时间工作后可能会造成较大的累积误差。
《基于能量法的四旋翼无人机吊挂飞行系统控制方法》（公开号：CN111061282A）。该发明涉及四旋翼无人机负载飞行的控制，提出一种基于能量法的非线性控制器，实现控制四旋翼无人机位置的同时较好地抑制吊挂负载在飞行中的摆动。该发明采用的技术方案是，基于能量法的四旋翼无人机吊挂飞行系统控制方法，在设置有用于吊载物品的吊绳的无人机上实现，步骤是，建立飞行过程的非线性动力学模型，设计控制器，基于能量函数方法设计李亚普诺夫方程，运用李亚普诺夫分析方法与拉塞尔不变集理论证明提出的控制器使得四旋翼无人机渐近收敛到目标位置，同时使得摆角渐近收敛到0，进而实现无人机控制。本发明主要应用于四旋翼无人机负载飞行控制，未涉及悬挂负载估计问题。
《一种无人机机动目标定位跟踪中的视觉伺服控制方法》(公开号：CN105353772A)。该发明通过建立大地坐标系、机体坐标系、摄像机坐标系、图像坐标系、机体大地过渡坐标系，通过上述建立的坐标系之间的关系，根据目标的成像序列，进行目标的定位及目标跟踪的姿态角给定值和航线跟踪的姿态角给定值的计算，完成视觉伺服控制。仅仅使用单一固定的摄像头，其优点是无需跟踪云台和激光测距设备，有效地降低了载荷的体积和成本，提高了侦察的隐蔽性。但是单一摄像头进行跟踪目标在复杂背景下，难以稳定的跟踪目标。
发明内容
为了解决上述技术问题，本发明目的之一是提供一种基于视觉编码器融合的悬挂负载状态估计器，解决当前无人机运输过程中对于复杂以及光线不足的环境下，跟踪稳定性较差的问题，并解决针对复杂场景下基于视觉方法无法获取良好的悬挂负载估计的问题。
为实现上述发明目的，本发明采取的技术方案如下：
一种基于视觉编码器融合的悬挂负载状态估计器，所述负载状态估计器安装在飞行器的机腹，所述飞行器通过吊绳悬挂负载物；所述负载状态估计器包括：
状态估计器本体，状态估计器本体包括安装在飞行器机腹的估计器安装座、横跨在所述估计器安装座上的第一轴、安装在所述第一轴上的吊绳固定座和轴承，所述吊绳固定座上设置第二轴，所述吊绳的一端系于所述第二轴上，所述吊绳的另一端系于所述负载物上，所述轴承包括设置在所述第一轴两端的第一轴承和设置在所述第二轴两端的第二轴承；
摄像头，所述摄像头安装在所述飞行器的机腹上，所述摄像头覆盖负载物摆动范围，用于检测绳子的张紧状态并提供负载物的位置信息；
编码器，所述编码器包括第一编码器和第二编码器，所述第一轴和所述第二轴相互垂直正交设置，所述第一编码器设置在所述第一轴承的外端面侧，所述第二编码器设置在所述吊绳固定座的第一固定板或所述第二固定板上，并置于所述第二轴承的外端面侧，以使所述第一编码器和所述第二编码器相互正交，所述第一编码器和所述第二编码器用于测量所述吊绳的摆动角度。
进一步的，所述吊绳固定座包括穿设在所述第一轴上的固定块、互相平行设置的第一固定板和第二固定板，所述第一固定板和所述第二固定板与所述固定块连接，所述第一固定板和所述第二固定板之间设置所述第二轴。
优选的，所述飞行器为无人机，所述摄像头为鱼眼相机，所述第一轴承和第二轴承均为高速轴承。鱼眼相机可以作为独立实现大范围无死角图像和视频采集的全景摄像机。
优选的，所述编码器为磁编码器。
进一步的，所述估计器安装座包括一顶壁和两个侧壁，所述顶壁与所述飞行器的机腹固定连接，所述侧壁位于所述顶壁的两侧，其中一侧壁上设置安装所述第一编码器的安装槽，所述第一轴横跨设置在两个所述侧壁之间，所述第一轴穿过所述固定块并与所述固定块过盈配合连接，所述第一轴的两端通过第一轴承分别与两个所述侧壁连接，所述固定块转动从而带动所述第一轴转动；吊绳固定座的第一固定板和所述第二固定板均与所述估计器安装座的侧壁垂直设置，由于所述第二轴垂直设置在第一固定板和所述第二固定板之间，因此所述第二轴与所述第一轴互相垂直正交设置。在飞行器飞行过程中，固定块会和第一轴转动，从而第一轴承的内圈转动，第一编码器即可测量第一轴承转动的角度，进而测量固定块和吊绳沿第二轴摆动的角度，同时当吊绳和第二轴一起摆动时，则第二轴承的内圈发生转动，第二编码器即可测量第二轴承转动的角度，进而测量吊绳沿第一轴方向的摆动角度。本发明将悬挂负载物的吊绳系于由两正交编码器构成的带高速轴承的两自由度机械结构形成的负载状态估计器中，通过两个编码器测量吊绳两个摆动方向的对应角度，从而获得基于编码器的负载状态估计器所提供的负载物的位置估计值；并通过鱼眼相机检测吊绳的张紧状态，同时通过鱼眼镜头提供负载物的位置信息，从而获得基于基于视觉的负载状态估计器所提供的负载物的位置估计值，进一步的，将基于视觉和编码器的负载状态估计器提供的位置估计值实时融合以提供负载物的准确位置估计。
本发明的目的之二是提供一种基于视觉编码器融合的悬挂负载的状态估计方法，包括如下步骤：
S1.在在飞行器的机腹设置负载状态估计器，负载状态估计器包括设置在飞行器机腹的鱼眼相机和状态估计器本体，并在状态估计器本体上设置两组互相正交的轴承和两个正交的编码器，通过吊绳将负载物悬挂在所述负载状态估计器上，建立以飞行器的几何中心为原点的机体坐标系
Figure 401684DEST_PATH_IMAGE002
，以编码器的几何中心为原点的估计器坐标系
Figure 787666DEST_PATH_IMAGE004
，建立世界坐标系
Figure 832982DEST_PATH_IMAGE006
，所述世界坐标系
Figure 572399DEST_PATH_IMAGE008
与机体坐标系
Figure 590034DEST_PATH_IMAGE002
在初始位置重合；机体坐标系
Figure 678076DEST_PATH_IMAGE002
中x轴向前，y轴向右，z轴向上；估计器坐标系
Figure 538584DEST_PATH_IMAGE004
中 x轴向前，y轴向左，z轴向下，定义世界坐标系
Figure 940747DEST_PATH_IMAGE008
与机体坐标系
Figure 875205DEST_PATH_IMAGE002
在初始位置重合，在每一个坐标系中，
Figure 215706DEST_PATH_IMAGE010
分别为x轴，y轴和z轴；
S2. 在飞行器飞行过程中，获取编码器对负载物在估计器坐标系
Figure 173298DEST_PATH_IMAGE004
下的位置估计值
Figure 503785DEST_PATH_IMAGE012
，及获取基于编码器观测数据得到的负载物在估计器坐标系下
Figure 292749DEST_PATH_IMAGE004
的负载位置；
S3.将所述吊绳与飞行器连接的一端与基于编码器的负载状态估计器的原点重合，通过基于鱼眼相机视觉的负载状态估计器得到负载物在世界坐标系
Figure 925856DEST_PATH_IMAGE008
中的位置估计值
Figure 511689DEST_PATH_IMAGE014
；
S4. 将基于编码器的负载状态估计器提供的负载物的位置估计值
Figure 52392DEST_PATH_IMAGE012
和基于视觉的负载状态估计器提供的负载物的位置估计值
Figure 899125DEST_PATH_IMAGE014
进行高斯融合，得到基于视觉编码器系统得到的负载物的位置估计值
Figure 827767DEST_PATH_IMAGE016
。
进一步的，所述步骤S2还包括如下步骤：
a1.通过编码器测量吊绳的摆动角度
Figure 822268DEST_PATH_IMAGE018
，通过两个正交的编码器测量两个对应角度
Figure 104345DEST_PATH_IMAGE020
和
Figure 946530DEST_PATH_IMAGE022
，其中
Figure 983756DEST_PATH_IMAGE020
为吊绳在机体坐标系
Figure 137657DEST_PATH_IMAGE002
中z轴和y轴形成的平面中吊绳与z轴的夹角，
Figure 613637DEST_PATH_IMAGE022
为吊绳在机体坐标系
Figure 231701DEST_PATH_IMAGE002
中z轴和x轴形成的平面中吊绳与z轴的夹角，在吊绳一直张紧的条件下，通过映射函数获取相机坐标系下所述鱼眼相机到负载的位置：
Figure DEST_PATH_IMAGE023
其中，绳长
Figure DEST_PATH_IMAGE025
为飞行器机身到负载物的距离；
a2.在编码器测量过程中，每个编码器输出的信号通过模数转换获得角度数据
Figure 100002_DEST_PATH_IMAGE027
，其中获取的数字信号的最大值为
Figure 100002_DEST_PATH_IMAGE029
，因此编码器测量角度
Figure 100002_DEST_PATH_IMAGE031
可以表示为：
Figure 252877DEST_PATH_IMAGE032
其中，
Figure 956391DEST_PATH_IMAGE034
为获取的角度，
Figure 100002_DEST_PATH_IMAGE035
，
Figure DEST_PATH_IMAGE037
为负载状态估计器系统所受的高斯白噪声，其均值为零，
Figure 187128DEST_PATH_IMAGE038
为方差；
a3.根据上述步骤a2中获得的编码器测量角度
Figure 253173DEST_PATH_IMAGE040
，获得编码器对负载物在估计器坐标系
Figure 569885DEST_PATH_IMAGE042
下的位置估计值
Figure 760695DEST_PATH_IMAGE044
，
Figure 329210DEST_PATH_IMAGE044
的计算公式可以表达如下：
Figure 100002_DEST_PATH_IMAGE045
其中
Figure DEST_PATH_IMAGE047
为变换的噪声方差，
Figure DEST_PATH_IMAGE049
为非线性映射函数；
a4.基于步骤a3中的
Figure 921866DEST_PATH_IMAGE044
的计算公式，获取基于编码器观测数据得到的负载物在估计器坐标系下
Figure 284845DEST_PATH_IMAGE042
的负载位置如下：
Figure 166213DEST_PATH_IMAGE050
。
进一步的，在所述编码器测量过程中，编码器的读入数据存在噪声，设编码器噪声符合正态分布，其协方差矩阵
Figure 459791DEST_PATH_IMAGE052
，则基于编码器的负载状态估计器测量的负载物状态的条件概率密度为：
Figure 969270DEST_PATH_IMAGE054
。
进一步的，所述步骤S3包括如下步骤：
b1.通过鱼眼相机检测吊绳的张紧状态，设所述鱼眼相机的光心与编码器帧的原点的距离为
Figure 627785DEST_PATH_IMAGE056
，根据针孔摄像机模型，将鱼眼相机到有效负载物的单位向量定义为
Figure 793187DEST_PATH_IMAGE058
，
Figure 703505DEST_PATH_IMAGE058
的计算公式如下：
Figure 100002_DEST_PATH_IMAGE059
其中，
Figure 100002_DEST_PATH_IMAGE061
是来自鱼眼相机采集的图像中心的有效负载物的像素坐标；
Figure 100002_DEST_PATH_IMAGE063
是通过鱼眼相机校准获得的相机模型的内参；
b2.基于鱼眼相机视觉的负载状态估计器，通过如下计算公式获得负载物在世界坐标系
Figure 100002_DEST_PATH_IMAGE065
中的位置估计值
Figure 100002_DEST_PATH_IMAGE067
：
Figure DEST_PATH_IMAGE069
其中：
Figure DEST_PATH_IMAGE071
是
Figure 100002_DEST_PATH_IMAGE073
和估计器坐标系
Figure 160767DEST_PATH_IMAGE042
下的z轴
Figure 100002_DEST_PATH_IMAGE075
之间的夹角；
Figure 100002_DEST_PATH_IMAGE077
为估计的鱼眼相机到负载物的距离；
Figure 100002_DEST_PATH_IMAGE079
为估计器坐标系
Figure 927866DEST_PATH_IMAGE042
与机身坐标系
Figure DEST_PATH_IMAGE081
间的旋转矩阵；
Figure DEST_PATH_IMAGE083
为机身坐标系
Figure 128034DEST_PATH_IMAGE081
与世界坐标系
Figure 763415DEST_PATH_IMAGE065
的旋转矩阵；
Figure DEST_PATH_IMAGE085
为世界坐标系下负载物指向机身的单位向量。基于视觉的负载状态估计器得到的估计值和基于编码器的负载状态估计器得到的估计值均可以提供负载的位置，二者是为了系统鲁棒设计的并行观测。
进一步的，在步骤S3中，提取负载物的模板图片或提前框取目标物区域，实现飞行器飞行过程中对高速稳定的负载物跟踪，并通过相关性滤波的方法进行鲁棒检测，相关性滤波的方法包括如下步骤：采用STAPLE目标跟踪算法，利用相关性滤波（使用HOG特征）以及全局颜色直方图并使用线性加权构造评分函数：
Figure DEST_PATH_IMAGE087
其中
Figure DEST_PATH_IMAGE089
是模板匹配分数由相关性滤波获得，
Figure DEST_PATH_IMAGE091
是全局颜色直方图分数，
Figure DEST_PATH_IMAGE093
是相关的权重系数。STAPLE目标跟踪算法对运动模糊、光照变化和物体变形等都具有很强的鲁棒性。
进一步的，在步骤S2和S3同步进行的条件下，基于编码器的负载状态估计器和基于视觉的负载状态估计器同时对负载物进行位置估计时，
判断负载物是否被追踪的方法包括：判断负载物是否被跟踪取决于检测到的感兴趣区域的相似性，在基于视觉的负载状态估计器中目标检测失败时，系统将根据基于编码器的负载状态估计器的估计位置重新对基于视觉的负载状态估计器进行初始化，从而使基于视觉的负载状态估计器重新进行目标检测；
判断吊绳是否处于张紧状态的方法包括：当基于视觉的负载状态估计器跟踪负载物正常时，通过
Figure DEST_PATH_IMAGE095
来计算基于编码器的负载状态估计器和基于视觉的负载状态估计器之间的估计位置误差，以此来确定绳子的张紧状态，其中
Figure DEST_PATH_IMAGE097
为基于视觉的负载状态估计器的负载估计位置，
Figure DEST_PATH_IMAGE099
为基于编码器的负载状态估计器的负载估计位置。由于当吊绳不张紧时，基于编码器的负载状态估计器将反馈错误的估计位置，此时飞行器控制器可能产生极端输出，导致飞行器坠机，因此必须判断吊绳是否处于张紧的状态。
进一步的，所述步骤S4的融合过程包括如下步骤：
设基于整个视觉编码器系统得到的位置估计值
Figure DEST_PATH_IMAGE101
服从高斯分布
Figure 136234DEST_PATH_IMAGE102
，基于视觉以及编码器的负载状态估计器的观测结果也服从高斯分布
Figure 402130DEST_PATH_IMAGE104
，根据高斯扰动的乘积，融合后的高斯分布如下：
Figure 604442DEST_PATH_IMAGE106
Figure 43513DEST_PATH_IMAGE108
其中
Figure 523036DEST_PATH_IMAGE110
表示基于整个视觉编码器系统的负载状态估计器得到的位置估计值的均值，
Figure 569621DEST_PATH_IMAGE112
包括基于视觉的负载状态估计器得到的位置估计值的均值
Figure 134594DEST_PATH_IMAGE114
和基于编码器的负载状态估计器得到的位置估计值的均值
Figure 111778DEST_PATH_IMAGE116
；
Figure DEST_PATH_IMAGE117
表示基于整个视觉编码器系统的负载状态估计器得到的位置估计值的方差，
Figure DEST_PATH_IMAGE119
包括基于视觉的负载状态估计器得到的位置估计值的方差
Figure DEST_PATH_IMAGE121
和基于编码器的负载状态估计器得到的位置估计值的方差
Figure DEST_PATH_IMAGE123
,利用上述各轴上的方程，从鱼眼相机和编码器上获取的信息实时融合得到负载物在状态估计器坐标系
Figure 586752DEST_PATH_IMAGE042
下的位置估计。
相对于现有技术，本发明取得了有益的技术效果：
本发明通过基于视觉编码器融合的负载状态估计器来估计负载物的位置，能够提高无人机的机动性，利用基于视觉的负载状态估计器对吊绳的张紧状态进行了实际有效的评估，有助于无人机改变飞行的控制策略，同时本发明针对传统的视觉跟踪估计方法一般需要贴附人工标记物，而实际应用中，受条件限制，无法提前贴好人工标记物，导致无法范围地应用在各个场景中的问题，通过设置鱼眼相机和编码器结合的方案，既能够通过编码器实现对负载物摆动角度进行测量，又能够通过鱼眼相机对负载物和吊绳张紧状态进行实时追踪，并通过高斯融合将鱼眼相机和编码器的测量结果进行融合以提供准确的结果，降低噪声的干扰。本发明可以应用于货物运输搬运任务，尤其可以应用在灾难救援现场或复杂地形环境等人们很难到达的地方，解决复杂环境下无人机悬挂负载的跟踪问题，因此其具有很大的应用价值。
附图说明
图1为本发明基于视觉编码器融合的悬挂负载状态估计器和飞行器结合后的整体结构；
图2为本发明基于视觉编码器融合的悬挂负载状态估计器和飞行器结合后的另一视角整体结构
图3为图2中P处的放大图；
图4为吊绳在机体坐标系
Figure 991189DEST_PATH_IMAGE081
中z轴和y轴形成的平面中吊绳与z轴的夹角示意图；
图5为吊绳在机体坐标系
Figure 168092DEST_PATH_IMAGE081
中z轴和X轴形成的平面中吊绳与z轴的夹角示意图；
图6为本发明基于编码器的悬挂负载状态估计器结构爆炸图；
图7为本发明基于编码器的悬挂负载状态估计器整体结构图；
图8为本发明基于编码器的悬挂负载状态估计器的估计器安装座结构图；
图9为本发明基于编码器的悬挂负载状态估计器的吊绳固定座与第一轴装配图；
图10为本发明基于编码器的悬挂负载状态估计器的吊绳固定座与第二轴装配图；
图11为本发明的基于视觉编码器融合的悬挂负载的状态估计方法的流程图。
附图标记：
1、飞行器；2、负载物；3、吊绳；4、负载状态估计器；41、状态估计器本体；411、估计器安装座；4111、安装槽；4121、第一轴；41211、平直段；41212、安装段；4131、第一轴承；4132、第二轴承；414、吊绳固定座；4141、固定块；4142、第一固定板；4143、第二固定板；4122、第二轴；42、编码器；43、鱼眼相机。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合实施例对本发明进行进一步详细说明，但本发明要求保护的范围并不局限于下述具体实施例。
如图1和2所示，一种基于视觉编码器融合的悬挂负载状态估计器，所述负载状态估计器4安装在飞行器1的机腹，所述飞行器1通过吊绳3悬挂负载物2；所述负载状态估计器4包括：
状态估计器本体41，如图6和7所示，状态估计器本体41包括安装在飞行器1机腹的估计器安装座411、横跨在所述估计器安装座411上的第一轴4121、安装在所述第一轴4121上的吊绳固定座414和轴承，所述吊绳固定座414上设置第二轴4122，所述吊绳3的一端系于所述第二轴4122上，所述吊绳3的另一端系于所述负载物2上，所述轴承包括设置在所述第一轴4121两端的第一轴承4131和设置在所述第二轴4122两端的第二轴承4132；如图9和10所示，所述吊绳固定座414包括穿设在所述第一轴4121上的固定块4141、互相平行设置的第一固定板4142和第二固定板4143，所述第一固定板4142和所述第二固定板4143与所述固定块4141连接，所述第一固定板4142和所述第二固定板4143之间设置所述第二轴4122。固定块4141可以设置在所述第一固定板4142和第二固定板4143的上方，所述固定块4141上设置供所述第一轴4121穿过的轴孔；或所述固定块4141包括一弯折块结构，所述固定块4141的水平部与所述第一固定板4142的上端连接，所述固定块4141的垂直部与所述第二固定板4143上端连接，所述第一轴4121位于所述第二轴4122的上方，所述吊绳固定座414为一体成型结构或拼接结构。
摄像头，所述摄像头安装在所述飞行器1的机腹上，所述摄像头覆盖负载物2摆动范围，用于检测绳子的张紧状态并提供负载物2的位置信息；
编码器42，所述编码器42包括第一编码器42和第二编码器42，所述第一轴4121和所述第二轴4122相互垂直正交设置，所述第一编码器42设置在所述第一轴承4131的外端面侧，所述第二编码器42设置在所述第一固定板4142或所述第二固定板4143上并置于所述第二轴承4132的外端面侧，以使所述第一编码器42和所述第二编码器42相互正交，所述第一编码器42和所述第二编码器42用于测量所述吊绳3的摆动角度。
所述飞行器1为无人机，所述摄像头为鱼眼相机43，所述第一轴承4131和第二轴承4132均为高速轴承。鱼眼相机43可以作为独立实现大范围无死角图像和视频采集的全景摄像机。所述编码器42为磁编码器42。
如图8所示，所述估计器安装座411包括一顶壁和两个侧壁，所述顶壁与所述飞行器1的机腹固定连接，所述侧壁位于所述顶壁的两侧，其中一侧壁上设置安装所述第一编码器42的安装槽4111，所述第一轴4121横跨设置在两个所述侧壁之间，所述第一轴4121穿过所述固定块4141并与所述固定块4141过盈配合或螺纹连接，所述第一轴4121的两端通过第一轴承4131分别与两个所述侧壁连接，所述固定块4141转动从而带动所述第一轴4121转动。所述第一轴4121包括两端的与第一轴承4131装配的安装段41212和位于两端安装段41212之间的平直段41211，所述平直段41211包括至少两个相对设置的水平端面，两个相对的水平端面分别与所述固定块4141的两个相对的内侧壁接触，这样设置便于所述第一轴4121和所述固定块4141装配，同时防止吊绳固定座414和所述第一轴4121之间发生相对转动，保证二者同步转动，第二轴4122也可采用与所述第一轴4121相同的结构；吊绳固定座414的第一固定板4142和所述第二固定板4143均与所述估计器安装座411的侧壁垂直设置，由于所述第二轴4122垂直设置在第一固定板4142和所述第二固定板4143之间，因此所述第二轴4122与所述第一轴4121互相垂直正交设置。在飞行器1飞行过程中，固定块4141会和第一轴4121转动，从而第一轴承4131的内圈转动，第一编码器42即可测量第一轴承4131转动的角度，进而测量固定块4141和吊绳3沿第二轴4122摆动的角度，同时当吊绳3和第二轴4122一起摆动时，则第二轴承4132的内圈发生转动，第二编码器42即可测量第二轴承4132转动的角度，进而测量吊绳3沿第一轴4121方向的摆动角度。
本发明将悬挂负载物2的吊绳3系于由两正交编码器42构成的带高速轴承的两自由度机械结构形成的负载状态估计器4中，通过两个编码器42测量吊绳3两个摆动方向的对应角度，从而获得基于编码器42的负载状态估计器4所提供的负载物2的位置估计值；并通过鱼眼相机43检测吊绳3的张紧状态，同时通过鱼眼镜头提供负载物2的位置信息，从而获得基于基于视觉的负载状态估计器4所提供的负载物2的位置估计值，进一步的，将基于视觉和编码器42的负载状态估计器4提供的位置估计值实时融合以提供负载物2的准确位置估计。
如图11所示，一种基于视觉编码器融合的悬挂负载的状态估计方法，包括如下步骤：
S1. 如图3所示，在飞行器的机腹设置负载状态估计器，负载状态估计器包括设置在飞行器机腹的鱼眼相机和状态估计器本体，并在状态估计器本体上设置两组互相正交的轴承和两个正交的编码器，通过吊绳将负载物悬挂在所述负载状态估计器上，建立以飞行器的几何中心为原点的机体坐标系
Figure 621070DEST_PATH_IMAGE081
，以编码器的几何中心为原点的估计器坐标系
Figure 950552DEST_PATH_IMAGE042
，建立世界坐标系
Figure 525890DEST_PATH_IMAGE065
，所述世界坐标系
Figure 799876DEST_PATH_IMAGE065
与机体坐标系
Figure 446758DEST_PATH_IMAGE081
在初始位置重合；机体坐标系
Figure 552117DEST_PATH_IMAGE081
中x轴向前，y轴向右，z轴向上；估计器坐标系
Figure 501619DEST_PATH_IMAGE042
中 x轴向前，y轴向左，z轴向下，定义世界坐标系
Figure 344460DEST_PATH_IMAGE065
与机体坐标系
Figure 467136DEST_PATH_IMAGE081
在初始位置重合，在每一个坐标系中，
Figure DEST_PATH_IMAGE125
分别为x轴，y轴和z轴；
S2. 在飞行器飞行过程中，获取编码器对负载物在估计器坐标系
Figure 958161DEST_PATH_IMAGE042
下的位置估计值
Figure 875301DEST_PATH_IMAGE044
，及获取基于编码器观测数据得到的负载物在估计器坐标系下
Figure 264825DEST_PATH_IMAGE042
的负载位置；
S3.将所述吊绳与飞行器连接的一端与基于编码器的负载状态估计器的原点重合，通过基于鱼眼相机视觉的负载状态估计器得到负载物在世界坐标系
Figure 863297DEST_PATH_IMAGE065
中的位置估计值
Figure 677669DEST_PATH_IMAGE067
；
S4. 将基于编码器的负载状态估计器提供的负载物的位置估计值
Figure 359186DEST_PATH_IMAGE044
和基于视觉的负载状态估计器提供的负载物的位置估计值
Figure 829482DEST_PATH_IMAGE067
进行高斯融合，得到基于视觉编码器系统得到的负载物的位置估计值
Figure DEST_PATH_IMAGE127
。
获取所述步骤S2中的编码器对负载物在估计器坐标系
Figure 638169DEST_PATH_IMAGE042
下的位置估计值
Figure 244731DEST_PATH_IMAGE044
包括如下步骤：
a1.如图4和5所示，通过编码器测量吊绳的摆动角度
Figure DEST_PATH_IMAGE129
通过两个正交的编码器测量两个对应角度
Figure DEST_PATH_IMAGE131
和
Figure DEST_PATH_IMAGE133
，其中
Figure 910198DEST_PATH_IMAGE131
为吊绳在机体坐标系
Figure 930107DEST_PATH_IMAGE081
中z轴和y轴形成的平面中吊绳与z轴的夹角，
Figure 135960DEST_PATH_IMAGE133
为吊绳在机体坐标系
Figure 987242DEST_PATH_IMAGE081
中z轴和x轴形成的平面中吊绳与z轴的夹角，在吊绳一直张紧的条件下，通过映射函数获取相机坐标系下所述鱼眼相机到负载的位置：
Figure 682665DEST_PATH_IMAGE023
其中，绳长
Figure DEST_PATH_IMAGE135
为飞行器机身到负载物的距离；
a2.在编码器测量过程中，每个编码器输出的信号通过模数转换获得角度数据
Figure DEST_PATH_IMAGE137
，其中获取的数字信号的最大值为
Figure DEST_PATH_IMAGE138
，因此编码器测量角度
Figure 937673DEST_PATH_IMAGE040
可以表示为：
Figure 71851DEST_PATH_IMAGE032
其中，
Figure 918584DEST_PATH_IMAGE034
为获取的角度，
Figure 519330DEST_PATH_IMAGE035
，
Figure 592459DEST_PATH_IMAGE037
为负载状态估计器系统所受的高斯白噪声，其均值为零，
Figure 874536DEST_PATH_IMAGE038
为方差；
a3.根据上述步骤a2中获得的编码器测量角度
Figure 638092DEST_PATH_IMAGE040
，获得编码器对负载物在估计器坐标系
Figure 737635DEST_PATH_IMAGE042
下的位置估计值
Figure 157116DEST_PATH_IMAGE044
，
Figure 383829DEST_PATH_IMAGE044
的计算公式可以表达如下：
Figure DEST_PATH_IMAGE140
其中
Figure 205154DEST_PATH_IMAGE047
为变换的噪声方差，
Figure 741178DEST_PATH_IMAGE049
为非线性映射函数；
a4.基于步骤a3中的
Figure 444691DEST_PATH_IMAGE044
的计算公式，获取基于编码器观测数据得到的负载物在估计器坐标系下
Figure 334150DEST_PATH_IMAGE042
的负载位置如下：
Figure 885348DEST_PATH_IMAGE050
。
在所述编码器测量过程中，编码器作为传感器，由于编码器的读入数据存在噪声，设编码器噪声符合正态分布，其协方差矩阵
Figure 998798DEST_PATH_IMAGE052
，则基于编码器的负载状态估计器测量的负载物状态的条件概率密度为：
Figure DEST_PATH_IMAGE142
。
上述步骤S3中基于鱼眼相机视觉的负载状态估计器得到负载物在世界坐标系
Figure 720766DEST_PATH_IMAGE065
中的位置估计值
Figure 413916DEST_PATH_IMAGE067
的步骤如下：
b1.通过鱼眼相机检测吊绳的张紧状态，设所述鱼眼相机的光心与编码器帧的原点的距离为
Figure 6571DEST_PATH_IMAGE056
，根据针孔摄像机模型，将鱼眼相机到有效负载物的单位向量定义为
Figure 626341DEST_PATH_IMAGE058
，
Figure 242130DEST_PATH_IMAGE058
的计算公式如下：
Figure 270128DEST_PATH_IMAGE059
其中，
Figure 45186DEST_PATH_IMAGE061
是来自鱼眼相机采集的图像中心的有效负载物的像素坐标；
Figure 703701DEST_PATH_IMAGE063
是通过鱼眼相机校准获得的相机模型的内参；
b2.基于鱼眼相机视觉的负载状态估计器，通过如下计算公式获得负载物在世界坐标系
Figure 947732DEST_PATH_IMAGE065
中的位置估计值
Figure 779421DEST_PATH_IMAGE067
：
Figure DEST_PATH_IMAGE144
其中：
Figure 612248DEST_PATH_IMAGE071
是
Figure 503981DEST_PATH_IMAGE073
和估计器坐标系
Figure 359941DEST_PATH_IMAGE042
下的z轴
Figure 542792DEST_PATH_IMAGE075
之间的夹角；
Figure 167808DEST_PATH_IMAGE077
为估计的鱼眼相机到负载物的距离；
Figure 230442DEST_PATH_IMAGE079
为估计器坐标系
Figure 167174DEST_PATH_IMAGE042
与机身坐标系
Figure 809508DEST_PATH_IMAGE081
间的旋转矩阵；
Figure 85769DEST_PATH_IMAGE083
为机身坐标系
Figure 397933DEST_PATH_IMAGE081
与世界坐标系
Figure 697327DEST_PATH_IMAGE065
的旋转矩阵；
Figure 736827DEST_PATH_IMAGE085
为世界坐标系下负载物指向机身的单位向量。基于视觉的负载状态估计器得到的估计值和基于编码器的负载状态估计器得到的估计值均可以提供负载的位置，二者是为了系统鲁棒设计的并行观测。
在步骤S3中，为了实现鲁棒检测，提出利用相关性滤波的方法，只需要提取负载物的模板图片或提前框取目标物区域，即可实现飞行器飞行过程中对高速稳定的负载物跟踪，相关性滤波的方法包括如下步骤：采用STAPLE目标跟踪算法，利用相关性滤波（使用HOG特征）以及全局颜色直方图并使用线性加权构造评分函数：
Figure 133173DEST_PATH_IMAGE087
其中
Figure 475293DEST_PATH_IMAGE089
是模板匹配分数由相关性滤波获得，
Figure 399999DEST_PATH_IMAGE091
是全局颜色直方图分数，
Figure DEST_PATH_IMAGE146
是相关的权重系数。STAPLE目标跟踪算法对运动模糊、光照变化和物体变形等都具有很强的鲁棒性。
在步骤S2和S3同步进行的条件下，基于编码器的负载状态估计器和基于视觉的负载状态估计器同时对负载物进行位置估计时，判断负载物是否被追踪的方法包括：判断负载物是否被跟踪取决于检测到的感兴趣区域的相似性，在基于视觉的负载状态估计器中目标检测失败时，系统将根据基于编码器的负载状态估计器的估计位置重新对基于视觉的负载状态估计器进行初始化，从而使基于视觉的负载状态估计器重新进行目标检测；判断吊绳是否处于张紧状态的方法包括：当基于视觉的负载状态估计器跟踪负载物正常时，通过
Figure 384136DEST_PATH_IMAGE095
来计算基于编码器的负载状态估计器和基于视觉的负载状态估计器之间的估计位置误差，以此来确定绳子的张紧状态，其中
Figure 697305DEST_PATH_IMAGE097
为基于视觉的负载状态估计器的负载估计位置，
Figure 475906DEST_PATH_IMAGE099
为基于编码器的负载状态估计器的负载估计位置。由于当吊绳不张紧时，基于编码器的负载状态估计器将反馈错误的估计位置，此时飞行器控制器可能产生极端输出，导致飞行器坠机，因此需要判断吊绳是否处于张紧的状态。
所述步骤S4中将基于编码器的负载状态估计器提供的负载物的位置估计值
Figure 812209DEST_PATH_IMAGE044
和基于视觉的负载状态估计器提供的负载物的位置估计值
Figure 209823DEST_PATH_IMAGE067
进行融合过程如下：
设基于整个视觉编码器系统得到的位置估计值
Figure 252866DEST_PATH_IMAGE101
服从高斯分布
Figure 999105DEST_PATH_IMAGE102
，基于视觉以及编码器的负载状态估计器的观测结果也服从高斯分布
Figure 885021DEST_PATH_IMAGE104
，根据高斯扰动的乘积，融合后的高斯分布如下：
Figure 945381DEST_PATH_IMAGE106
Figure 983875DEST_PATH_IMAGE108
其中
Figure 901016DEST_PATH_IMAGE110
表示基于整个视觉编码器系统的负载状态估计器得到的位置估计值的均值，
Figure 415174DEST_PATH_IMAGE112
包括基于视觉的负载状态估计器得到的位置估计值的均值
Figure 138279DEST_PATH_IMAGE114
和基于编码器的负载状态估计器得到的位置估计值的均值
Figure 155914DEST_PATH_IMAGE116
；
Figure 509535DEST_PATH_IMAGE117
表示基于整个视觉编码器系统的负载状态估计器得到的位置估计值的方差，
Figure 120776DEST_PATH_IMAGE119
包括基于视觉的负载状态估计器得到的位置估计值的方差
Figure 257359DEST_PATH_IMAGE121
和基于编码器的负载状态估计器得到的位置估计值的方差
Figure 191817DEST_PATH_IMAGE123
,利用上述各轴上的方程，从鱼眼相机和编码器上获取的信息实时融合得到负载物在状态估计器坐标系
Figure 44235DEST_PATH_IMAGE042
下的位置估计。
根据上述说明书的揭示和教导，本发明所属领域的技术人员还可以对上述实施方式进行变更和修改。因此，本发明并不局限于上面揭示和描述的具体实施方式，对发明的一些修改和变更也应当落入本发明的权利要求的保护范围内。此外，尽管本说明书中使用了一些特定的术语，但这些术语只是为了方便说明，并不对发明构成任何限制。

Claims (9)

1.基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，包括如下步骤：
S1.在飞行器的机腹设置负载状态估计器，负载状态估计器包括设置在飞行器机腹的鱼眼相机和状态估计器本体，并在状态估计器本体上设置两组互相正交的轴承和两个正交的编码器，通过吊绳将负载物悬挂在负载状态估计器上，建立以飞行器的几何中心为原点的机体坐标系
Figure DEST_PATH_IMAGE001
，以编码器的几何中心为原点的估计器坐标系
Figure 153007DEST_PATH_IMAGE002
，建立世界坐标系
Figure DEST_PATH_IMAGE003
，所述世界坐标系
Figure 973196DEST_PATH_IMAGE004
与机体坐标系
Figure 794521DEST_PATH_IMAGE001
在初始位置重合；机体坐标系
Figure 674752DEST_PATH_IMAGE001
中x轴向前，y轴向右，z轴向上；估计器坐标系
Figure 315949DEST_PATH_IMAGE002
中 x轴向前，y轴向左，z轴向下，定义世界坐标系
Figure DEST_PATH_IMAGE005
与机体坐标系
Figure 907205DEST_PATH_IMAGE001
在初始位置重合，在每一个坐标系中，
Figure 848617DEST_PATH_IMAGE006
分别为x轴，y轴和z轴；
S2. 在飞行器飞行过程中，获取编码器对负载物在估计器坐标系
Figure 165328DEST_PATH_IMAGE002
下的位置估计值
Figure 293821DEST_PATH_IMAGE007
，及获取基于编码器观测数据得到的负载物在估计器坐标系下
Figure 986971DEST_PATH_IMAGE002
的负载位置；
S3.将所述吊绳与飞行器连接的一端与基于编码器的负载状态估计器的原点重合，通过基于鱼眼相机视觉的负载状态估计器得到负载物在世界坐标系
Figure 750265DEST_PATH_IMAGE008
中的位置估计值
Figure DEST_PATH_IMAGE009
；
S4. 将基于编码器的负载状态估计器提供的负载物的位置估计值
Figure 706720DEST_PATH_IMAGE010
和基于视觉的负载状态估计器提供的负载物的位置估计值
Figure DEST_PATH_IMAGE011
进行高斯融合，得到基于视觉编码器系统得到的负载物的位置估计值
Figure 56930DEST_PATH_IMAGE012
；
所述负载状态估计器(4)安装在飞行器(1)的机腹，所述飞行器(1)通过吊绳(3)悬挂负载物(2)；所述负载状态估计器(4)包括：状态估计器本体(41)、鱼眼相机(43)、编码器(42)，
状态估计器本体(41)包括安装在飞行器(1)机腹的估计器安装座(411)、横跨在所述估计器安装座(411)上的第一轴(4121)、安装在所述第一轴(4121)上的吊绳固定座(414)和轴承，所述吊绳固定座(414)上设置第二轴(4122)，所述吊绳(3)的一端系于所述第二轴(4122)上，所述吊绳(3)的另一端系于所述负载物(2)上，所述轴承包括设置在所述第一轴(4121)两端的第一轴承(4131)和设置在所述第二轴(4122)两端的第二轴承(4132)；
鱼眼相机(43)，所述鱼眼相机(43)安装在所述飞行器(1)的机腹上，所述鱼眼相机(43)覆盖负载物(2)摆动范围，用于检测绳子的张紧状态并提供负载物(2)的位置信息；
编码器(42)，所述编码器(42)包括第一编码器(42)和第二编码器(42)，所述第一轴(4121)和所述第二轴(4122)相互垂直正交设置，所述第一编码器(42)设置在所述第一轴承(4131)的外端面侧，所述第二编码器(42)设置在吊绳固定座(414)上，以使所述第一编码器(42)和所述第二编码器(42)相互正交，所述第一编码器(42)和所述第二编码器(42)用于测量所述吊绳(3)的摆动角度。
2.根据权利要求1所述的基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，所述步骤S2还包括如下步骤：
a1.通过编码器测量吊绳的摆动角度
Figure 553770DEST_PATH_IMAGE013
通过两个正交的编码器测量两个对应角度
Figure DEST_PATH_IMAGE014
和
Figure 673036DEST_PATH_IMAGE015
，其中
Figure 95665DEST_PATH_IMAGE014
为吊绳在机体坐标系
Figure 464329DEST_PATH_IMAGE001
中z轴和y轴形成的平面中吊绳与z轴的夹角，
Figure 764861DEST_PATH_IMAGE016
为吊绳在机体坐标系
Figure 800950DEST_PATH_IMAGE001
中z轴和x轴形成的平面中吊绳与z轴的夹角，在吊绳一直张紧的条件下，通过映射函数获取相机坐标系下所述鱼眼相机到负载的位置：
Figure DEST_PATH_IMAGE018
其中，绳长
Figure 364786DEST_PATH_IMAGE019
为飞行器机身到负载物的距离；
a2.在编码器测量过程中，每个编码器输出的信号通过模数转换获得角度数据
Figure DEST_PATH_IMAGE020
，其中获取的数字信号的最大值为
Figure 955168DEST_PATH_IMAGE021
，因此编码器测量角度
Figure DEST_PATH_IMAGE022
可以表示为：
Figure DEST_PATH_IMAGE024
其中，
Figure 528231DEST_PATH_IMAGE025
为获取的角度，
Figure 153248DEST_PATH_IMAGE026
，
Figure DEST_PATH_IMAGE027
为负载状态估计器系统所受的高斯白噪声，其均值为零，
Figure 935257DEST_PATH_IMAGE028
为方差；
a3.根据上述步骤a2中获得的编码器测量角度
Figure 12935DEST_PATH_IMAGE022
，获得编码器对负载物在估计器坐标系
Figure DEST_PATH_IMAGE029
下的位置估计值
Figure 655269DEST_PATH_IMAGE007
，
Figure 400371DEST_PATH_IMAGE007
的计算公式可以表达如下：
Figure DEST_PATH_IMAGE031
其中
Figure 571589DEST_PATH_IMAGE032
为变换的噪声方差，
Figure DEST_PATH_IMAGE033
为非线性映射函数；
a4.基于步骤a3中的
Figure 870983DEST_PATH_IMAGE007
的计算公式，获取基于编码器观测数据得到的负载物在估计器坐标系下
Figure 51429DEST_PATH_IMAGE034
的负载位置如下：
Figure DEST_PATH_IMAGE035
。
3.根据权利要求2所述的基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，在所述编码器测量过程中，编码器的读入数据存在噪声，设编码器噪声符合正态分布，其协方差矩阵
Figure 385458DEST_PATH_IMAGE036
，则基于编码器的负载状态估计器测量的负载物状态的条件概率密度为：
Figure 757272DEST_PATH_IMAGE038
Figure DEST_PATH_IMAGE039
。
4.根据权利要求1所述的基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，所述步骤S3包括如下步骤：
b1.通过鱼眼相机检测吊绳的张紧状态，设所述鱼眼相机的光心与编码器帧的原点的距离为
Figure 809541DEST_PATH_IMAGE040
，根据针孔摄像机模型，将鱼眼相机到有效负载物的单位向量定义为
Figure DEST_PATH_IMAGE041
，
Figure 528099DEST_PATH_IMAGE041
的计算公式如下：
Figure 247793DEST_PATH_IMAGE042
其中，
Figure DEST_PATH_IMAGE043
是来自鱼眼相机采集的图像中心的有效负载物的像素坐标；
Figure 26393DEST_PATH_IMAGE044
是通过鱼眼相机校准获得的相机模型的内参；
b2.基于鱼眼相机视觉的负载状态估计器，通过如下计算公式获得负载物在世界坐标系
Figure 565959DEST_PATH_IMAGE003
中的位置估计值
Figure 353786DEST_PATH_IMAGE009
：
Figure DEST_PATH_IMAGE045
,
Figure 662408DEST_PATH_IMAGE046
,
Figure DEST_PATH_IMAGE048
Figure DEST_PATH_IMAGE050
其中：
Figure 346330DEST_PATH_IMAGE051
是
Figure DEST_PATH_IMAGE052
和估计器坐标系
Figure 606148DEST_PATH_IMAGE002
下的z轴
Figure 197666DEST_PATH_IMAGE053
之间的夹角；
Figure 626373DEST_PATH_IMAGE054
为估计的鱼眼相机到负载物的距离；
Figure DEST_PATH_IMAGE055
为估计器坐标系
Figure 746776DEST_PATH_IMAGE002
与机身坐标系
Figure 526513DEST_PATH_IMAGE001
间的旋转矩阵；
Figure 656143DEST_PATH_IMAGE056
为机身坐标系
Figure 673778DEST_PATH_IMAGE001
与世界坐标系
Figure 496240DEST_PATH_IMAGE004
的旋转矩阵；
Figure DEST_PATH_IMAGE057
为世界坐标系下负载物指向机身的单位向量。
5.根据权利要求1所述的基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，在步骤S3中，提取负载物的模板图片或提前框取目标物区域，实现飞行器飞行过程中对高速稳定的负载物跟踪，并通过相关性滤波的方法进行鲁棒检测，相关性滤波的方法包括如下步骤：采用STAPLE目标跟踪算法，利用相关性滤波以及全局颜色直方图并使用线性加权构造评分函数：
Figure DEST_PATH_IMAGE059
其中
Figure 966536DEST_PATH_IMAGE060
是模板匹配分数由相关性滤波获得，
Figure DEST_PATH_IMAGE061
是全局颜色直方图分数，
Figure 634278DEST_PATH_IMAGE062
是相关的权重系数。
6.根据权利要求1所述的基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，在步骤S2和S3同步进行的条件下，基于编码器的负载状态估计器和基于视觉的负载状态估计器同时对负载物进行位置估计时，
判断负载物是否被追踪的方法包括：在基于视觉的负载状态估计器中目标检测失败时，系统将根据基于编码器的负载状态估计器的估计位置重新对基于视觉的负载状态估计器进行初始化，从而使基于视觉的负载状态估计器重新进行目标检测；
判断吊绳是否处于张紧状态的方法包括：当基于视觉的负载状态估计器跟踪负载物正常时，通过
Figure DEST_PATH_IMAGE063
来计算基于编码器的负载状态估计器和基于视觉的负载状态估计器之间的估计位置误差，以此来确定绳子的张紧状态，其中
Figure 739375DEST_PATH_IMAGE064
为基于视觉的负载状态估计器的负载估计位置，
Figure DEST_PATH_IMAGE065
为基于编码器的负载状态估计器的负载估计位置。
7.根据权利要求1所述的基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，所述步骤S4的融合过程包括如下步骤：
设基于整个视觉编码器系统得到的位置估计值
Figure 467159DEST_PATH_IMAGE066
服从高斯分布
Figure DEST_PATH_IMAGE067
，基于视觉以及编码器的负载状态估计器的观测结果也服从高斯分布
Figure 690330DEST_PATH_IMAGE068
，根据高斯扰动的乘积，融合后的高斯分布如下：
Figure 161763DEST_PATH_IMAGE070
Figure 888411DEST_PATH_IMAGE072
其中
Figure DEST_PATH_IMAGE073
表示基于整个视觉编码器系统的负载状态估计器得到的位置估计值的均值，
Figure 521517DEST_PATH_IMAGE074
包括基于视觉的负载状态估计器得到的位置估计值的均值
Figure DEST_PATH_IMAGE075
和基于编码器的负载状态估计器得到的位置估计值的均值
Figure 966405DEST_PATH_IMAGE076
；
Figure DEST_PATH_IMAGE077
表示基于整个视觉编码器系统的负载状态估计器得到的位置估计值的方差，
Figure 480344DEST_PATH_IMAGE078
包括基于视觉的负载状态估计器得到的位置估计值的方差
Figure DEST_PATH_IMAGE079
和基于编码器的负载状态估计器得到的位置估计值的方差
Figure 592656DEST_PATH_IMAGE080
,利用上述各轴上的方程，从鱼眼相机和编码器上获取的信息实时融合得到负载物在状态估计器坐标系
Figure 662243DEST_PATH_IMAGE002
下的位置估计。
8.根据权利要求1所述的基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，所述吊绳固定座(414)包括穿设在所述第一轴(4121)上的固定块(4141)、互相平行设置的第一固定板(4142)和第二固定板(4143)，所述第一固定板(4142)和所述第二固定板(4143)与所述固定块(4141)连接，所述第一固定板(4142)和所述第二固定板(4143)之间设置所述第二轴(4122)。
9.根据权利要求1所述的基于视觉编码器融合的悬挂负载的状态估计方法，其特征在于，所述飞行器(1)为无人机，所述第一轴承(4131)和第二轴承(4132)均为高速轴承。
CN202110041597.4A 2021-01-13 2021-01-13 基于视觉编码器融合的悬挂负载状态估计器及估计方法 Active CN112991443B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202110041597.4A CN112991443B (zh) 2021-01-13 2021-01-13 基于视觉编码器融合的悬挂负载状态估计器及估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110041597.4A CN112991443B (zh) 2021-01-13 2021-01-13 基于视觉编码器融合的悬挂负载状态估计器及估计方法

Publications (2)

Publication Number Publication Date
CN112991443A CN112991443A (zh) 2021-06-18
CN112991443B true CN112991443B (zh) 2022-09-27

Family

ID=76344382

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110041597.4A Active CN112991443B (zh) 2021-01-13 2021-01-13 基于视觉编码器融合的悬挂负载状态估计器及估计方法

Country Status (1)

Country Link
CN (1) CN112991443B (zh)

Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9223008B1 (en) * 2010-03-02 2015-12-29 Advanced Optical Systems Inc. Load tracking and stabilization
CN107615211A (zh) * 2015-05-23 2018-01-19 深圳市大疆创新科技有限公司 使用惯性传感器和图像传感器的传感器融合
JP2019050007A (ja) * 2018-11-01 2019-03-28 エスゼット ディージェイアイ テクノロジー カンパニー リミテッドＳｚ Ｄｊｉ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．，Ｌｔｄ 移動体の位置を判断する方法および装置、ならびにコンピュータ可読媒体
CN110018691A (zh) * 2019-04-19 2019-07-16 天津大学 小型多旋翼无人机飞行状态估计系统和方法

Family Cites Families (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP6411917B2 (ja) * 2015-02-27 2018-10-24 株式会社日立製作所 自己位置推定装置および移動体

Patent Citations (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US9223008B1 (en) * 2010-03-02 2015-12-29 Advanced Optical Systems Inc. Load tracking and stabilization
CN107615211A (zh) * 2015-05-23 2018-01-19 深圳市大疆创新科技有限公司 使用惯性传感器和图像传感器的传感器融合
JP2019050007A (ja) * 2018-11-01 2019-03-28 エスゼット ディージェイアイ テクノロジー カンパニー リミテッドＳｚ Ｄｊｉ Ｔｅｃｈｎｏｌｏｇｙ Ｃｏ．，Ｌｔｄ 移動体の位置を判断する方法および装置、ならびにコンピュータ可読媒体
CN110018691A (zh) * 2019-04-19 2019-07-16 天津大学 小型多旋翼无人机飞行状态估计系统和方法

Non-Patent Citations (1)

* Cited by examiner, † Cited by third party
Title
无人机机载载荷安装位姿实时测量系统设计;孙雨 等;《计测技术》;20151031;第35卷(第5期);第35-39页 *

Also Published As

Publication number Publication date
CN112991443A (zh) 2021-06-18

Similar Documents

Publication Publication Date Title
Cheng et al. An autonomous vision-based target tracking system for rotorcraft unmanned aerial vehicles
Bacik et al. Autonomous flying with quadrocopter using fuzzy control and ArUco markers
Courbon et al. Vision-based navigation of unmanned aerial vehicles
Grabe et al. Robust optical-flow based self-motion estimation for a quadrotor UAV
CN102190081B (zh) 基于视觉的飞艇定点鲁棒控制方法
Beul et al. A high-performance MAV for autonomous navigation in complex 3D environments
Weiss et al. 4dof drift free navigation using inertial cues and optical flow
Courbon et al. Visual navigation of a quadrotor aerial vehicle
Dufek et al. Visual pose stabilization of tethered small unmanned aerial system to assist drowning victim recovery
CN102654917B (zh) 运动体运动姿态感知方法及系统
Chirarattananon A direct optic flow-based strategy for inverse flight altitude estimation with monocular vision and IMU measurements
Cho et al. Autonomous ship deck landing of a quadrotor UAV using feed-forward image-based visual servoing
Strydom et al. Uav guidance: a stereo-based technique for interception of stationary or moving targets
Zhang et al. Autonomous landing on ground target of UAV by using image-based visual servo control
CN112991443B (zh) 基于视觉编码器融合的悬挂负载状态估计器及估计方法
CN108170160A (zh) 一种利用单目视觉和机载传感器旋翼无人机自主抓取方法
CN112859923A (zh) 一种无人机视觉编队飞行控制系统
Razinkova et al. Tracking a moving ground object using quadcopter UAV in a presence of noise
Salagame et al. Precision Landing of a UAV on a Moving Platform for Outdoor Applications
Mehtatt et al. Visual servo control of an unmanned ground vehicle via a moving airborne monocular camera
Yoshihata et al. Multi-camera visual servoing of a micro helicopter under occlusions
Borshchova et al. Marker-guided auto-landing on a moving platform
Abdessameud et al. Dynamic image-based tracking control for VTOL UAVs
Ajmera et al. Autonomous visual tracking and landing of a quadrotor on a moving platform
Salazar et al. Real-time stereo visual servoing control of an UAV having eight-rotors

Legal Events

Date Code Title Description
PB01 Publication
PB01 Publication
SE01 Entry into force of request for substantive examination
SE01 Entry into force of request for substantive examination
GR01 Patent grant
GR01 Patent grant