CN110325822A - 云台位姿修正方法和装置 - Google Patents

云台位姿修正方法和装置 Download PDF

Info

Publication number
CN110325822A
CN110325822A CN201880011522.8A CN201880011522A CN110325822A CN 110325822 A CN110325822 A CN 110325822A CN 201880011522 A CN201880011522 A CN 201880011522A CN 110325822 A CN110325822 A CN 110325822A
Authority
CN
China
Prior art keywords
holder
compensation device
pose
vertical compensation
speed
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.)
Granted
Application number
CN201880011522.8A
Other languages
English (en)
Other versions
CN110325822B (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 Dajiang Innovations Technology Co Ltd
Original Assignee
Shenzhen Dajiang Innovations Technology Co Ltd
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 Dajiang Innovations Technology Co Ltd filed Critical Shenzhen Dajiang Innovations Technology Co Ltd
Publication of CN110325822A publication Critical patent/CN110325822A/zh
Application granted granted Critical
Publication of CN110325822B publication Critical patent/CN110325822B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Classifications

    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/0094—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot involving pointing a payload, e.g. camera, weapon, sensor, towards a fixed or moving target
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M11/00—Stands or trestles as supports for apparatus or articles placed thereon Stands for scientific apparatus such as gravitational force meters
    • F16M11/02—Heads
    • F16M11/18—Heads with mechanism for moving the apparatus relatively to the stand
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M11/00—Stands or trestles as supports for apparatus or articles placed thereon Stands for scientific apparatus such as gravitational force meters
    • F16M11/02—Heads
    • F16M11/04—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M11/00—Stands or trestles as supports for apparatus or articles placed thereon Stands for scientific apparatus such as gravitational force meters
    • F16M11/02—Heads
    • F16M11/04—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand
    • F16M11/043—Allowing translations
    • F16M11/046—Allowing translations adapted to upward-downward translation movement
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M11/00—Stands or trestles as supports for apparatus or articles placed thereon Stands for scientific apparatus such as gravitational force meters
    • F16M11/02—Heads
    • F16M11/04—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand
    • F16M11/06—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand allowing pivoting
    • F16M11/10—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand allowing pivoting around a horizontal axis
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M11/00—Stands or trestles as supports for apparatus or articles placed thereon Stands for scientific apparatus such as gravitational force meters
    • F16M11/02—Heads
    • F16M11/04—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand
    • F16M11/06—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand allowing pivoting
    • F16M11/12—Means for attachment of apparatus; Means allowing adjustment of the apparatus relatively to the stand allowing pivoting in more than one direction
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C21/00—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00
    • G01C21/10—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration
    • G01C21/12—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning
    • G01C21/16—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation
    • G01C21/165—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments
    • G01C21/1656—Navigation; Navigational instruments not provided for in groups G01C1/00 - G01C19/00 by using measurements of speed or acceleration executed aboard the object being navigated; Dead reckoning by integrating acceleration or speed, i.e. inertial navigation combined with non-inertial navigation instruments with passive imaging devices, e.g. cameras
    • G—PHYSICS
    • G03—PHOTOGRAPHY; CINEMATOGRAPHY; ANALOGOUS TECHNIQUES USING WAVES OTHER THAN OPTICAL WAVES; ELECTROGRAPHY; HOLOGRAPHY
    • G03B—APPARATUS OR ARRANGEMENTS FOR TAKING PHOTOGRAPHS OR FOR PROJECTING OR VIEWING THEM; APPARATUS OR ARRANGEMENTS EMPLOYING ANALOGOUS TECHNIQUES USING WAVES OTHER THAN OPTICAL WAVES; ACCESSORIES THEREFOR
    • G03B17/00—Details of cameras or camera bodies; Accessories therefor
    • G03B17/56—Accessories
    • G—PHYSICS
    • G03—PHOTOGRAPHY; CINEMATOGRAPHY; ANALOGOUS TECHNIQUES USING WAVES OTHER THAN OPTICAL WAVES; ELECTROGRAPHY; HOLOGRAPHY
    • G03B—APPARATUS OR ARRANGEMENTS FOR TAKING PHOTOGRAPHS OR FOR PROJECTING OR VIEWING THEM; APPARATUS OR ARRANGEMENTS EMPLOYING ANALOGOUS TECHNIQUES USING WAVES OTHER THAN OPTICAL WAVES; ACCESSORIES THEREFOR
    • G03B17/00—Details of cameras or camera bodies; Accessories therefor
    • G03B17/56—Accessories
    • G03B17/561—Support related camera accessories
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D1/00—Control of position, course or altitude of land, water, air, or space vehicles, e.g. automatic pilot
    • G05D1/02—Control of position or course in two dimensions
    • G05D1/021—Control of position or course in two dimensions specially adapted to land vehicles
    • G05D1/0268—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means
    • G05D1/027—Control of position or course in two dimensions specially adapted to land vehicles using internal positioning means comprising intertial navigation means, e.g. azimuth detector
    • G—PHYSICS
    • G05—CONTROLLING; REGULATING
    • G05D—SYSTEMS FOR CONTROLLING OR REGULATING NON-ELECTRIC VARIABLES
    • G05D3/00—Control of position or direction
    • G05D3/12—Control of position or direction using feedback
    • F—MECHANICAL ENGINEERING; LIGHTING; HEATING; WEAPONS; BLASTING
    • F16—ENGINEERING ELEMENTS AND UNITS; GENERAL MEASURES FOR PRODUCING AND MAINTAINING EFFECTIVE FUNCTIONING OF MACHINES OR INSTALLATIONS; THERMAL INSULATION IN GENERAL
    • F16M—FRAMES, CASINGS OR BEDS OF ENGINES, MACHINES OR APPARATUS, NOT SPECIFIC TO ENGINES, MACHINES OR APPARATUS PROVIDED FOR ELSEWHERE; STANDS; SUPPORTS
    • F16M2200/00—Details of stands or supports

Abstract

一种云台位姿修正方法和装置，所述装置包括与云台相连接的竖直补偿装置、装设于竖直补偿装置上的视觉模块(2)和装设于竖直补偿装置上的惯性测量单元(1)，竖直补偿装置用于补偿云台沿竖直方向的移动，视觉模块(2)和惯性测量单元(1)均与竖直补偿装置电连接；竖直补偿装置用于，基于惯性测量单元(1)获取云台的第一位姿；基于视觉模块(2)获取竖直补偿装置的第二位姿；并根据第二位姿，对第一位姿进行修正。

Description

云台位姿修正方法和装置
技术领域
本发明涉及云台控制领域，尤其涉及一种云台位姿修正方法和装置。
背景技术
位姿估计机器人控制要解决的关键问题之一，其主要工作是根据各种运动状态传感器的数据得到满足控制带宽、动态性能、稳定性和精度要求的位置、速度、姿态以及航向信息，能够提供即时位姿信息的系统称为导航系统。常见的导航系统包括：惯性导航系统、全球导航卫星系统(Global Navigation Satellite System，GNSS)、多普勒导航系统、视觉导航系统等。使用多种不同的导航系统对同一信息源进行测量，从测量值当中提取各导航系统误差并进行修正的技术称为组合导航技术，组合导航技术是多传感器信息融合状态估计领域的重要应用之一。
惯性-GNSS组合导航为常用的组合导航之一。传统的惯性-GNSS组合导航将NED坐标系作为导航坐标系，需要指北的航向观测，一般使用地磁传感器提供参考航向，而地磁传感器易受电流和磁场干扰；并且传统的惯性-GNSS组合导航使用经纬度表示位置，因此需要GNSS提供经纬度形式的位置测量，而当使用场合为室内环境时，GNSS导航无法工作；常规的单点GNSS存在m级别的定位和测速误差，而在某些情况下速度控制精度要求达到mm级别，因此仅使用惯性-GNSS组合导航无法满足精度要求。
发明内容
本发明提供一种云台位姿修正方法和装置。
具体地，本发明是通过如下技术方案实现的：
根据本发明的第一方面，提供一种云台位姿修正方法，其中，云台与竖直补偿装置连接，所述竖直补偿装置补偿所述云台沿竖直方向的移动，所述竖直补偿装置上装设有视觉模块和惯性测量单元，所述方法包括：
基于所述惯性测量单元获取所述云台的第一位姿；
基于所述视觉模块获取所述竖直补偿装置的第二位姿；
根据所述第二位姿，对所述第一位姿进行修正。
根据本发明的第二方面，提供一种云台位姿修正装置，包括与云台相连接的竖直补偿装置、装设于所述竖直补偿装置上的视觉模块和装设于所述竖直补偿装置上的惯性测量单元，所述竖直补偿装置用于补偿所述云台沿竖直方向的移动，所述视觉模块和所述惯性测量单元均与所述竖直补偿装置电连接；
所述竖直补偿装置用于，基于所述惯性测量单元获取所述云台的第一位姿；基于所述视觉模块获取所述竖直补偿装置的第二位姿；并根据所述第二位姿，对所述第一位姿进行修正。
由以上本发明实施例提供的技术方案可见，本发明采用惯性-视觉组合导航方式，基于视觉模块获得的第二姿态对基于惯性测量单元获得的第一姿态进行修正，得到满足控制带宽及精度要求的位姿，本发明的惯性-视觉组合导航方式不受电流和磁场干扰，适用于对室内外多种环境。
附图说明
为了更清楚地说明本发明实施例中的技术方案，下面将对实施例描述中所需要使用的附图作简单地介绍，显而易见地，下面描述中的附图仅仅是本发明的一些实施例，对于本领域普通技术人员来讲，在不付出创造性劳动性的前提下，还可以根据这些附图获得其他的附图。
图1是本发明一实施例中的云台位姿修正装置的结构示意图；
图2是本发明一实施例中的云台位姿修正装置的结构示意图；
图3是本发明另一实施例中的云台位姿修正装置的结构示意图；
图4是本发明一实施例中的云台位姿修正方法的方法流程图；
图5是本发明一具体实施例中的云台位姿修正方法的方法流程图。
附图标记：1：惯性测量单元；2：视觉模块；3：本体；31：机身；32：基座；33：手持部；4：轴臂；5：电机；6：角速度传感器。
具体实施方式
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
下面结合附图，对本发明的云台位姿修正方法和装置进行详细说明。在不冲突的情况下，下述的实施例及实施方式中的特征可以相互组合。
参见图1，为本发明实施例的云台位姿修正装置的结构示意图，其中，云台与竖直补偿装置连接，所述竖直补偿装置补偿所述云台沿竖直方向的移动。本实施例中，云台通过所述竖直补偿设备挂载在可移动物体(比如用户、无人机、机器人)上，可移动物体移动时，存在竖直方向上的运动，该竖直方向上的运动会使得云台上的相机画面不稳定，故本实施例通过竖直补偿装置补偿云台沿竖直方向的移动，从而改善云台直接挂载在可移动物体上并随着可移动物体移动时产生的竖直方向的运动，保证相机画面平稳。
进一步地，本实施例的竖直补偿装置上装设有视觉模块2和惯性测量单元1(IMU，Inertial measurement unit)。结合图2和图3，所述竖直补偿装置包括本体3和用于连接所述云台的轴臂4，所述轴臂4转动，能够补偿所述云台沿竖直方向的移动。可选地，所述本体3上设有电机5，所述电机5用于驱动所述轴臂4转动，而在其他实施例中，也可由其他驱动装置驱动所述轴臂4转动。所述惯性测量单元1设于所述轴臂4上，所述惯性测量单元1可以装设于所述轴臂4连接所述云台的一端，当然，所述惯性测量也可以装设于所述轴臂4上的其他任意位置。
所述视觉模块2设于所述本体3上，本实施例的视觉模块2的检测方向可以朝上也可以朝下。具体地，当所述云台位姿修正系统位于室外环境中，所述视觉模块2朝下；当所述云台位姿修正系统位于室内时，所述视觉模块2可以朝上或朝下。本实施例的视觉模块2的检测方向大致平行于竖直方向，视觉模块2的检测方向相对竖直方向允许存在一个较小的倾斜角(可根据经验值设定该倾斜角的角度范围)。优选地，所述视觉模块2竖直朝上或竖直朝下监控。进一步结合图2和图3，本实施例的本体3可以包括机身31和与所述机身31固定连接的基座32，所述视觉模块2设于所述基座32上。
在一些例子中，云台通过基座32搭载在无人机、移动机器人或者其他可移动设备上。无人机、移动机器人或者其他可移动设备移动过程中，存在竖直方向的运动而影响相机(云台上的相机)画面，竖直补偿装置对该竖直方向的运动进行补偿，抵消竖直方向的运动对相机画面的影响。
在其他一些例子中，又结合图2和图3，所述竖直补偿装置为手持装置，所述补偿装置可以包括手持部33，所述手持部33与所述机身31固定连接。用户手持所述手持部33，从而带动所述竖直补偿装置整体移动，用户在行走的过程中，竖直方向上随着步频产生运动而影响相机画面，竖直补偿装置对该竖直方向的运动进行补偿，抵消竖直方向的运动对相机画面的影响。
需要说明的是，下述实施例中，定义机体坐标系{b}-Obxbybzb如下：坐标系原点Ob为所述轴臂4连接所述云台的轴端的平面几何中心；xb轴在机身31的垂直对称面内并平行于基座32底面指向前方；yb轴垂直于机身31的垂直对称面指向机身31右方；zb轴在机身31的垂直对称面内，与xb轴垂直并指向机身31下方。
定义基座32坐标系{p}-Opxpypzp如下：坐标系原点Op为轴臂4的中心，即轴臂4旋转中心线与机身31垂直对称面的交点；xp轴在机身31的垂直对称面内并平行于基座32底面指向前方；yp轴垂直于机身31的垂直对称面指向机身31右方；zp轴在机身31的垂直对称面内，与xp轴垂直并指向机身31下方。
记相机坐标系为{c}-Ocxcyczc，导航坐标系为{n}-Onxnynzn。导航坐标系原点On由系统开始工作时相机坐标系原点Oc在地面上的竖直投影确定，导航坐标系坐标轴由视觉模块2的输出确定。视觉模块2的输出相机坐标系{c}相对导航坐标系{n}的位姿，在一些例子中，视觉模块2输出所述竖直补偿装置的参考位置参考速度和参考姿态在另一些例子中，视觉模块2输出所述竖直补偿装置的参考位置和参考速度
图4为本发明实施例提供的一种云台位姿修正方法的方法流程图。所述方法的执行主体可以为竖直补偿装置中的处理器，也可以为独立设置的控制单元，并且该控制单元与竖直补偿装置中的处理器通信连接。如图4所示，所述方法可以包括如下步骤：
步骤S401：基于所述惯性测量单元1获取所述云台的第一位姿；
其中，所述第一位姿可以包括所述云台的速度、位置和姿态。
在本实施例中，所述惯性测量单元1可以包括陀螺仪和加速度计，可选地，所述陀螺仪为三轴陀螺仪，所述加速度计为三轴加速度计。步骤S401具体包括：基于所述陀螺仪获取所述云台的角速度，基于所述加速度计获取所述云台的比力，接着，根据所述角速度和所述比力，计算所述云台的姿态、速度和位置。
具体而言，云台姿态的更新过程包括：根据所述角速度和所述比力，设计姿态更新公式；根据所述姿态更新公式，对所述云台的姿态进行更新。
本实施例中，姿态更新公式的设计过程如下：
陀螺仪的理想输出为机体坐标系{b}相对惯性系{i}的转动角速率在{b}系的投影，记为陀螺仪实际输出记为加速度计的理想输出为比力在{b}系的投影，记为fb，加速度计实际输出记为fb。
采用四元数作为{n}系对{b}系的姿态表示，则无误差的理想四元数微分方程由下式确定：
公式(1)中姿态角速率由下式确定：
公式(2)中由最新的姿态更新值确定，和分别是地球自转角速率和位置角速率。本发明实施例适用场合为低速、短距离、近地面的移动拍摄，因此和可近似忽略，故有在实际系统中，由于陀螺仪测量误差和导航解算误差的存在，四元数微分方程的实际解算通过下式进行：
确定的实际机体坐标系记为{b′}。对式(1)所示的四元数微分方程进行离散化求解并取一阶近似，可得如下所示的四元数更新公式：
按照公式(4)进行姿态四元数更新得到姿态矩阵实际上建立起了捷联惯导的数学平台。
进一步地，云台速度的更新过程包括：根据所述角速度和所述比力，设计速度更新公式；根据所述速度更新公式，对所述云台的速度进行更新。具体地，本发明实施例采用下式作为近似的速度更新公式：
更进一步地，云台位置的更新过程包括：根据所述角速度和所述比力，设计位置更新公式；根据所述位置更新公式，对所述云台的位置进行更新。具体地，本发明实施例采用下式作为近似的位置更新公式：
需要说明的是，本发明实施例中的姿态更新公式、速度更新公式和位置更新公式并不限于上述实施例的设计方式。
步骤S402：基于所述视觉模块2获取所述竖直补偿装置的第二位姿；
所述视觉模块可以包括视觉里程计，也可以为视觉惯性里程计。例如，在其中一实施例中，所述视觉模块包括视觉里程计，所述第二位姿包括所述竖直补偿装置的速度和位置。在另一实施例中，参见图5，所述视觉模块包括视觉惯性里程计，所述第二位姿包括所述竖直补偿装置的速度、位置和姿态。
在一些例子中，所述竖直补偿装置上还可以装设有TOF测距模块(Time ofFlightMeasurement，飞行时差测距)，本实施例通过TOF测距模块的检测结果对视觉模块2的检测结果进行修正，具体地，所述竖直补偿装置通过TOF测距模块检测获得所述竖直补偿装置的位置，对视觉模块2获取的所述补偿装置的位置进行修正，以获得精确的竖直补偿装置的位置。
在一些例子中，所视觉模块可以替换为UWB定位装置(Ultra Wideband)，通过UWB定位装置对所述竖直补偿装置的位姿进行测量，本发明的惯性-UWB组合导航方式也不受电流和磁场干扰，适用于对室内外多种环境。
由于本实施例的视觉模块2固定在基座32上，视觉模块2直接输出的所述竖直补偿装置的参考速度和位置所在的坐标系与第一位姿所在坐标系不同，无法使用视觉模块2直接输出的所述竖直补偿装置的参考速度和参考位置作为第一位姿的参考，对第一位姿进行修正，本实施例需要对视觉模块2直接输出的所述竖直补偿装置的参考速度和参考位置均进行坐标转换，获得第二位姿，第二位姿与第一位姿在同一坐标系。
具体而言，在本实施例中，所述轴臂4或上装设有角速度传感器6，用于获取所述轴臂4的关节角。在步骤S202之前，所述方法还可以包括：基于所述角速度传感器6，获得所述轴臂4的关节角。在其他实施例中，还可以基于驱动所述轴臂4转动电机5的关节角确定所述轴臂4的关节角。需要说明的是，本实施例对角速度传感器6的类型不作限定，可选择现有任意角速度传感器6。
本实施例的步骤S402包括：根据所述关节角，对所述视觉模块2输出的所述竖直补偿装置的参考速度进行坐标转换，获得所述竖直补偿装置的速度，再根据竖直补偿装置的速度对云台的速度进行修正即可。进一步地，步骤S402还包括：根据所述关节角，对所述视觉模块2输出的所述竖直补偿装置的参考位置进行坐标转换，获得所述竖直补偿装置的位置，再通过竖直补偿装置的位置对云台的位置进行修正。
在视觉模块2包括视觉惯性里程计时，本实施例的步骤S402还包括：基于所述视觉惯性里程计输出的参考姿态，构建所述参考姿态的参考方向余弦矩阵；根据所述参考方向余弦矩阵，获得所述竖直补偿装置的姿态。所述根据所述方向余弦矩阵，获得所述竖直补偿装置的姿态包括：根据所述参考方向余弦矩阵，获得所述竖直补偿装置的姿态修正值；根据所述姿态修正值，获得所述竖直补偿装置的姿态。至此，可以通过该竖直装置的姿态对云台的姿态进行修正。
步骤S403：根据所述第二位姿，对所述第一位姿进行修正。
根据上述步骤S402获得的第二位姿对上述步骤S401获得的第一位姿进行修正，即可获得云台的位姿估计值。再根据该位姿估计值控制云台的位姿，即可确保云台位姿的精确性。需要说明的是，本发明实施例中，对于三轴(偏航轴、俯仰轴、横滚轴)云台，对第一位姿进行修正，或者对云台位姿进行修正，实际是指修正云台在偏航轴、俯仰轴和/或横滚轴方向的位姿。
其中，步骤S403可采用回路反馈、最优估计或其他算法将第一位姿和第二位姿进行组合，实现惯性-视觉组合导航。在本实施例中，采用卡尔曼滤波(最优估计算法的一种)来融合第一位姿和第二位姿。以下实施例将具体说明采用卡尔曼滤波的融合第一位姿和第二位姿的实现过程。
在本实施例中，步骤S401还包括：基于所述陀螺仪获取所述云台的角速度，基于所述加速度计获取所述云台的比力；接着，根据所述角速度和所述比力，计算所述第一位姿的误差。具体地，所述根据所述角速度和所述比力，计算所述第一位姿的误差包括：根据所述角速度和所述比力，构建所述第一位姿的姿态误差、速度误差和位置误差，根据所述姿态误差、速度误差和位置误差，计算所述第一位姿的误差。
进一步地，步骤S403具体包括：对所述第一位姿的误差近似处理，获得卡尔曼滤波器；将所述第二位姿作为观测值，经所述卡尔曼滤器波获得修正值；根据所述修正值，对所述第一位姿进行修正，实现对云台在竖直方向的位姿的修正。本实施例中，对所述第一位姿的误差近似处理是指去掉所述第一位姿的误差中影响较小的误差项。
在一具体实施例中，参见图5，本实施例的云台的适用场合为低速、短距离、近地面的移动拍摄。陀螺仪的测量误差模型为：
其中，nr为陀螺仪测量噪声，并假设nr为高斯白噪声；b为陀螺仪零偏，并假设为形式的随机游走过程，其中nw为高斯白噪声。用表示陀螺仪零偏估计，认为是常值零偏，则根据该陀螺仪的测量误差模型获得：
进一步，定义陀螺仪零偏误差为：
则
定义姿态解算的状态量为根据四元数微分方程和陀螺仪测量误差模型有：
对于状态估计量获得：
结合上述公式，姿态误差方程的计算过程如下：
用表示引起的误差四元数，根据四元数乘法获得：
公式(9)对时间求导，并根据姿态解算系统的状态方程可得：
考虑陀螺仪测量误差模型(7)，公式(10)可写为：
记φ为{b′}系相对{b}系的姿态角偏差并认为φ是小角度，则的近似表达式为带入公式(11)可得：
则姿态误差的状态方程为：
进一步地，速度误差的计算过程：
根据比力方程，无误差的理想速度值根据如下微分方程确定：
其中，gn表示重力加速度在导航坐标系的表示。由于本发明实施例适用场合为低速、短距离、近地面的移动拍摄，因此和可近似忽略，故近似的速度误差计算公式如下：
其中，表示加速度计零偏在导航坐标系的投影。
进一步地，位置误差的计算过程：
与常规组合导航使用经纬度描述位置不同，本发明实施例使用视觉导航进行位置测量，且适用场合为近地面短距离运动，因此可使用公式(15)所示的距离形式的位置误差方程：
综合姿态误差、速度误差以及位置误差的计算公式可得第一位姿的误差(即组合导航系统误差方程)：
其中，系统状态量X为：
状态转移矩阵F为：
其中，为的反对称矩阵，为的反对称矩阵。
系统噪声向量w为：
w＝[nr nw na]T
其中，nr为陀螺仪噪声，nw为陀螺仪随机游走噪声，na为加速度计噪声。
噪声分配矩阵G为：
对式(16)进行离散化并取一阶近似，得到离散化的第一位姿的误差计算公式，利用该离散化的第一位姿的误差计算公式设计卡尔曼滤波器。
更进一步地，在本实施例中，视觉模块2包括视觉惯性里程计，本实施例是根据视觉惯性里程计的输出结果设计上述卡尔曼滤波器的观测值的，具体设计过程如下：
视觉惯性里程计输出的参考姿态为参考方向余弦矩阵为本实施例采用视觉惯性里程计输出的参考航向作为组合导航系统的航向观测，并认为{b}系和{c}系航向完全对齐。
设导航坐标系{n}三个轴向的正单位向量分别为：
则航向参考向量在{b}系的投影(即{b}系x方向的参考向量)为：
根据比力方程，完全静止时重力参考向量在{b}系的单位投影(即{b}系z方向的参考向量)为：
根据坐标系的正交关系，由和可以得到{b}系y方向的参考向量由 构建参考姿态的方向余弦矩阵如下：
由可得参考姿态四元数则姿态修正四元数为：
公式(20)中为当前最新的姿态四元数估计值。小角度条件下，根据上式可得姿态修正的观测量如下：
其中，和均为误差四元数。
本实施例的姿态修正的观测方程为：
其中，vφ为姿态观测噪声。
其中，Hφ＝[I3×3 03×12]，vφ＝[vφx vφy vφz]T。
将公式(22)作为卡尔曼滤波器的姿态观测方程，经过卡尔曼滤波器输出的姿态修正值利用对公式(4)得到的云台的姿态更新值进行修正，即可得到修正后的姿态输出，实现对云台姿态的修正。
视觉惯性里程计输出的速度、位置向量为相机坐标系{c}相对{n}系的速度和位置，在此需要得到{b}系的速度和位置观测。本实施例不考虑机械误差，在轴臂4旋转的过程中，轴臂4的平行四边形机构保证了轴端平面始终平行于基座32底面，因此{b}系和{p}系之间只存在平移运动。
根据视觉惯性里程计的输出以及机械结构的几何和动力学传递关系，求解轴臂4的参考速度Vr n、参考位置Pr n，具体公式如下：
其中，是{p}系到{n}系的方向余弦矩阵，ΔPp是Ob到Oc的相对位置向量在{p}系中的投影，ΔVp是Ob到Oc的相对速度向量在{p}系中的投影。定义[Ox Oy Oz]T为{p}系中表示的Oc到Op的位置偏移向量；定义轴臂4的关节角度为α，轴臂4与基座32地面平行时α＝0，规定逆时针方向为正方向；定义轴臂4长度L为轴臂4的旋转中心线到轴端(即轴臂4连接云台的一端)的长度，则ΔPp按如下公式计算：
ΔVp按如下公式计算：
根据式(23)、式(24)得到的参考速度向量Vr n和参考位置向量Pr n，由此获得组合导航系统的速度观测方程和位置观测方程，分别为：
其中：Hφ＝[03×3 I3×3 03×9]，vV＝[vVx vVy vVz]T；
HP＝[03×6 I3×3 03×6]，vP＝[vPx vPy vPz]T；
vV为速度观测噪声，HP为位置观测噪声。
将公式(27)作为卡尔曼滤波器的速度观测方程，经过卡尔曼滤波器输出速度修正值，利用速度修正值对公式(5)得到的云台的速度更新值进行修正，即可得到修正后的速度输出，实现对云台速度的修正。并且，将公式(28)作为卡尔曼滤波器的位置观测方程，经过卡尔曼滤波器输出位置修正值，利用位置修正值对公式(6)得到的云台的位置更新值进行修正，即可得到修正后的位置输出，实现对云台位置的修正。
本发明实施例采用惯性-视觉组合导航方式，基于视觉模块2获得的第二姿态对基于惯性测量单元1获得的第一姿态进行修正，得到满足控制带宽及精度要求的位姿，本发明的惯性-视觉组合导航方式不受电流和磁场干扰，适用于对室内外多种环境。
结合图1至图3，本发明实施例还提供一种云台位姿修正装置，该装置可以包括与云台相连接的竖直补偿装置、装设于所述竖直补偿装置上的视觉模块2和装设于所述竖直补偿装置上的惯性测量单元1，所述竖直补偿装置用于补偿所述云台沿竖直方向的移动，所述视觉模块2和所述惯性测量单元1均与所述竖直补偿装置电连接。
所述竖直补偿装置用于，基于所述惯性测量单元1获取所述云台的第一位姿；基于所述视觉模块2获取所述竖直补偿装置的第二位姿；并根据所述第二位姿，对所述第一位姿进行修正。
进一步地，所述竖直补偿装置包括本体3和用于连接所述云台的轴臂4，所述轴臂4转动，能够补偿所述云台沿竖直方向的移动；所述惯性测量单元1设于所述轴臂4上，所述视觉模块2设于所述本体3上。
进一步地，所述视觉模块2包括视觉里程计，所述第二位姿包括所述竖直补偿装置的速度和位置。
进一步地，所述视觉模块2包括视觉惯性里程计，所述第二位姿包括所述竖直补偿装置的速度、位置和姿态。
进一步地，所述竖直补偿装置包括用于连接所述云台的轴臂4，所述轴臂4转动，补偿所述云台沿竖直方向的移动，所述轴臂4上装设有角速度传感器6；所述竖直补偿装置用于，基于所述角速度传感器6，获得所述轴臂4的关节角。
进一步地，所述第一位姿包括所述云台的速度；所述竖直补偿装置用于，根据所述关节角，对所述视觉模块2输出的所述竖直补偿装置的参考速度进行坐标转换，获得所述竖直补偿装置的速度。
进一步地，所述第一位姿包括所述云台的位置；所述竖直补偿装置用于，根据所述关节角，对所述视觉模块2输出的所述竖直补偿装置的参考位置进行坐标转换，获得所述竖直补偿装置的位置。
进一步地，所述竖直补偿装置用于，基于所述视觉惯性里程计输出的参考姿态，构建所述参考姿态的参考方向余弦矩阵；根据所述参考方向余弦矩阵，获得所述竖直补偿装置的姿态。
进一步地，所述竖直补偿装置用于，根据所述参考方向余弦矩阵，获得所述竖直补偿装置的姿态修正值；根据所述姿态修正值，获得所述竖直补偿装置的姿态。
进一步地，所述第一位姿包括所述云台的速度、位置和姿态。
进一步地，所述惯性测量单元1包括陀螺仪和加速度计；所述竖直补偿装置用于，基于所述陀螺仪获取所述云台的角速度；基于所述加速度计获取所述云台的比力；根据所述角速度和所述比力，计算所述云台的姿态、速度和位置。
进一步地，所述竖直补偿装置用于，根据所述角速度和所述比力，设计姿态更新公式；根据所述姿态更新公式，对所述云台的姿态进行更新。
进一步地，所述竖直补偿装置用于，根据所述角速度和所述比力，设计速度更新公式；根据所述速度更新公式，对所述云台的速度进行更新。
进一步地，所述竖直补偿装置用于，根据所述角速度和所述比力，设计位置更新公式；根据所述位置更新公式，对所述云台的位置进行更新。
进一步地，所述惯性测量单元1包括陀螺仪和加速度计；所述竖直补偿装置用于，基于所述陀螺仪获取所述云台的角速度；基于所述加速度计获取所述云台的比力；根据所述角速度和所述比力，计算所述第一位姿的误差。
进一步地，所述竖直补偿装置用于，根据所述角速度和所述比力，构建所述第一位姿的姿态误差、速度误差和位置误差；根据所述姿态误差、速度误差和位置误差，计算所述第一位姿的误差。
进一步地，所述竖直补偿装置用于，对所述第一位姿的误差近似处理，获得卡尔曼滤波器；将所述第二位姿作为观测值，经所述卡尔曼滤器波获得修正值；根据所述修正值，对所述第一位姿进行修正。
对于装置实施例而言，由于其基本对应于方法实施例，所以相关之处参见方法实施例的部分说明即可。以上所描述的装置实施例仅仅是示意性的，其中所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部模块来实现本实施例方案的目的。本领域普通技术人员在不付出创造性劳动的情况下，即可以理解并实施。
需要说明的是，在本文中，诸如第一和第二等之类的关系术语仅仅用来将一个实体或者操作与另一个实体或操作区分开来，而不一定要求或者暗示这些实体或操作之间存在任何这种实际的关系或者顺序。术语“包括”、“包含”或者其任何其他变体意在涵盖非排他性的包含，从而使得包括一系列要素的过程、方法、物品或者设备不仅包括那些要素，而且还包括没有明确列出的其他要素，或者是还包括为这种过程、方法、物品或者设备所固有的要素。在没有更多限制的情况下，由语句“包括一个……”限定的要素，并不排除在包括所述要素的过程、方法、物品或者设备中还存在另外的相同要素。
以上对本发明实施例所提供的云台位姿修正方法和装置进行了详细介绍，本文中应用了具体个例对本发明的原理及实施方式进行了阐述，以上实施例的说明只是用于帮助理解本发明的方法及其核心思想；同时，对于本领域的一般技术人员，依据本发明的思想，在具体实施方式及应用范围上均会有改变之处，综上所述，本说明书内容不应理解为对本发明的限制。

Claims (34)

1.一种云台位姿修正方法，其特征在于，云台与竖直补偿装置连接，所述竖直补偿装置补偿所述云台沿竖直方向的移动，所述竖直补偿装置上装设有视觉模块和惯性测量单元，所述方法包括：
基于所述惯性测量单元获取所述云台的第一位姿；
基于所述视觉模块获取所述竖直补偿装置的第二位姿；
根据所述第二位姿，对所述第一位姿进行修正。
2.根据权利要求1所述的方法，其特征在于，所述竖直补偿装置包括本体和用于连接所述云台的轴臂，所述轴臂转动，能够补偿所述云台沿竖直方向的移动；
所述惯性测量单元设于所述轴臂上，所述视觉模块设于所述本体上。
3.根据权利要求1所述的方法，其特征在于，所述视觉模块包括视觉里程计，所述第二位姿包括所述竖直补偿装置的速度和位置。
4.根据权利要求1所述的方法，其特征在于，所述视觉模块包括视觉惯性里程计，所述第二位姿包括所述竖直补偿装置的速度、位置和姿态。
5.根据权利要求3或4所述的方法，其特征在于，所述竖直补偿装置包括用于连接所述云台的轴臂，所述轴臂转动，补偿所述云台沿竖直方向的移动，所述轴臂上装设有角速度传感器；
所述基于所述视觉模块获取所述竖直补偿装置的第二位姿前，包括：
基于所述角速度传感器，获得所述轴臂的关节角。
6.根据权利要求5所述的方法，其特征在于，所述第一位姿包括所述云台的速度；
所述基于所述视觉模块获取所述竖直补偿装置的第二位姿，包括：
根据所述关节角，对所述视觉模块输出的所述竖直补偿装置的参考速度进行坐标转换，获得所述竖直补偿装置的速度。
7.根据权利要求5所述的方法，其特征在于，所述第一位姿包括所述云台的位置；
所述基于所述视觉模块获取所述竖直补偿装置的第二位姿，包括：
根据所述关节角，对所述视觉模块输出的所述竖直补偿装置的参考位置进行坐标转换，获得所述竖直补偿装置的位置。
8.根据权利要求4所述的方法，其特征在于，所述基于所述视觉模块获取所述竖直补偿装置的第二位姿，包括：
基于所述视觉惯性里程计输出的参考姿态，构建所述参考姿态的参考方向余弦矩阵；
根据所述参考方向余弦矩阵，获得所述竖直补偿装置的姿态。
9.根据权利要求8所述的方法，其特征在于，所述根据所述方向余弦矩阵，获得所述竖直补偿装置的姿态，包括：
根据所述参考方向余弦矩阵，获得所述竖直补偿装置的姿态修正值；
根据所述姿态修正值，获得所述竖直补偿装置的姿态。
10.根据权利要求1所述的方法，其特征在于，所述第一位姿包括所述云台的速度、位置和姿态。
11.根据权利要求10所述的方法，其特征在于，所述惯性测量单元包括陀螺仪和加速度计；
所述基于所述惯性测量单元获取所述云台的第一位姿，包括：
基于所述陀螺仪获取所述云台的角速度；
基于所述加速度计获取所述云台的比力；
根据所述角速度和所述比力，计算所述云台的姿态、速度和位置。
12.根据权利要求11所述的方法，其特征在于，所述根据所述角速度，计算所述云台的姿态、速度和位置，包括：
根据所述角速度和所述比力，设计姿态更新公式；
根据所述姿态更新公式，对所述云台的姿态进行更新。
13.根据权利要求11所述的方法，其特征在于，所述根据所述角速度和所述比力，计算所述云台的姿态、速度和位置，包括：
根据所述角速度和所述比力，设计速度更新公式；
根据所述速度更新公式，对所述云台的速度进行更新。
14.根据权利要求11所述的方法，其特征在于，所述根据所述角速度和所述比力，计算所述云台的姿态、速度和位置，包括：
根据所述角速度和所述比力，设计位置更新公式；
根据所述位置更新公式，对所述云台的位置进行更新。
15.根据权利要求10所述的方法，其特征在于，所述惯性测量单元包括陀螺仪和加速度计；
所述基于所述惯性测量单元获取所述云台的第一位姿，包括：
基于所述陀螺仪获取所述云台的角速度；
基于所述加速度计获取所述云台的比力；
根据所述角速度和所述比力，计算所述第一位姿的误差。
16.根据权利要求15所述的方法，其特征在于，所述根据所述角速度和所述比力，计算所述第一位姿的误差，包括：
根据所述角速度和所述比力，构建所述第一位姿的姿态误差、速度误差和位置误差；
根据所述姿态误差、速度误差和位置误差，计算所述第一位姿的误差。
17.根据权利要求15所述的方法，其特征在于，所述根据所述第二位姿，对所述第一位姿进行修正，包括：
对所述第一位姿的误差近似处理，获得卡尔曼滤波器；
将所述第二位姿作为观测值，经所述卡尔曼滤器波获得修正值；
根据所述修正值，对所述第一位姿进行修正。
18.一种云台位姿修正装置，其特征在于，包括与云台相连接的竖直补偿装置、装设于所述竖直补偿装置上的视觉模块和装设于所述竖直补偿装置上的惯性测量单元，所述竖直补偿装置用于补偿所述云台沿竖直方向的移动，所述视觉模块和所述惯性测量单元均与所述竖直补偿装置电连接；
所述竖直补偿装置用于，基于所述惯性测量单元获取所述云台的第一位姿；基于所述视觉模块获取所述竖直补偿装置的第二位姿；并根据所述第二位姿，对所述第一位姿进行修正。
19.根据权利要求18所述的装置，其特征在于，所述竖直补偿装置包括本体和用于连接所述云台的轴臂，所述轴臂转动，能够补偿所述云台沿竖直方向的移动；
所述惯性测量单元设于所述轴臂上，所述视觉模块设于所述本体上。
20.根据权利要求18所述的装置，其特征在于，所述视觉模块包括视觉里程计，所述第二位姿包括所述竖直补偿装置的速度和位置。
21.根据权利要求18所述的装置，其特征在于，所述视觉模块包括视觉惯性里程计，所述第二位姿包括所述竖直补偿装置的速度、位置和姿态。
22.根据权利要求20或21所述的装置，其特征在于，所述竖直补偿装置包括用于连接所述云台的轴臂，所述轴臂转动，补偿所述云台沿竖直方向的移动，所述轴臂上装设有角速度传感器；
所述竖直补偿装置用于，基于所述角速度传感器，获得所述轴臂的关节角。
23.根据权利要求22所述的装置，其特征在于，所述第一位姿包括所述云台的速度；
所述竖直补偿装置用于，根据所述关节角，对所述视觉模块输出的所述竖直补偿装置的参考速度进行坐标转换，获得所述竖直补偿装置的速度。
24.根据权利要求22所述的装置，其特征在于，所述第一位姿包括所述云台的位置；
所述竖直补偿装置用于，根据所述关节角，对所述视觉模块输出的所述竖直补偿装置的参考位置进行坐标转换，获得所述竖直补偿装置的位置。
25.根据权利要求21所述的装置，其特征在于，所述竖直补偿装置用于，基于所述视觉惯性里程计输出的参考姿态，构建所述参考姿态的参考方向余弦矩阵；
根据所述参考方向余弦矩阵，获得所述竖直补偿装置的姿态。
26.根据权利要求25所述的装置，其特征在于，所述竖直补偿装置用于，根据所述参考方向余弦矩阵，获得所述竖直补偿装置的姿态修正值；
根据所述姿态修正值，获得所述竖直补偿装置的姿态。
27.根据权利要求18所述的装置，其特征在于，所述第一位姿包括所述云台的速度、位置和姿态。
28.根据权利要求27所述的装置，其特征在于，所述惯性测量单元包括陀螺仪和加速度计；
所述竖直补偿装置用于，基于所述陀螺仪获取所述云台的角速度；
基于所述加速度计获取所述云台的比力；
根据所述角速度和所述比力，计算所述云台的姿态、速度和位置。
29.根据权利要求28所述的装置，其特征在于，所述竖直补偿装置用于，根据所述角速度和所述比力，设计姿态更新公式；
根据所述姿态更新公式，对所述云台的姿态进行更新。
30.根据权利要求28所述的装置，其特征在于，所述竖直补偿装置用于，根据所述角速度和所述比力，设计速度更新公式；
根据所述速度更新公式，对所述云台的速度进行更新。
31.根据权利要求28所述的装置，其特征在于，所述竖直补偿装置用于，根据所述角速度和所述比力，设计位置更新公式；
根据所述位置更新公式，对所述云台的位置进行更新。
32.根据权利要求27所述的装置，其特征在于，所述惯性测量单元包括陀螺仪和加速度计；
所述竖直补偿装置用于，基于所述陀螺仪获取所述云台的角速度；
基于所述加速度计获取所述云台的比力；
根据所述角速度和所述比力，计算所述第一位姿的误差。
33.根据权利要求32所述的装置，其特征在于，所述竖直补偿装置用于，根据所述角速度和所述比力，构建所述第一位姿的姿态误差、速度误差和位置误差；
根据所述姿态误差、速度误差和位置误差，计算所述第一位姿的误差。
34.根据权利要求32所述的装置，其特征在于，所述竖直补偿装置用于，对所述第一位姿的误差近似处理，获得卡尔曼滤波器；
将所述第二位姿作为观测值，经所述卡尔曼滤器波获得修正值；
根据所述修正值，对所述第一位姿进行修正。
CN201880011522.8A 2018-04-25 2018-04-25 云台位姿修正方法和装置 Active CN110325822B (zh)

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
PCT/CN2018/084499 WO2019205034A1 (zh) 2018-04-25 2018-04-25 云台位姿修正方法和装置

Publications (2)

Publication Number Publication Date
CN110325822A true CN110325822A (zh) 2019-10-11
CN110325822B CN110325822B (zh) 2023-06-27

Family

ID=68112759

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201880011522.8A Active CN110325822B (zh) 2018-04-25 2018-04-25 云台位姿修正方法和装置

Country Status (4)

Country Link
US (1) US11274788B2 (zh)
EP (1) EP3786757B1 (zh)
CN (1) CN110325822B (zh)
WO (1) WO2019205034A1 (zh)

Cited By (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111399476A (zh) * 2020-03-13 2020-07-10 江西憶源多媒体科技有限公司 一种基于图像对齐的监控云台失控实时检测方法
WO2022077284A1 (zh) * 2020-10-14 2022-04-21 深圳市大疆创新科技有限公司 用于可移动平台的位置姿态确定方法、相关装置和系统

Families Citing this family (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP2020020630A (ja) 2018-07-31 2020-02-06 セイコーエプソン株式会社 姿勢推定方法、姿勢推定装置及び移動体
CN113406964B (zh) * 2021-05-19 2022-11-18 浙江华飞智能科技有限公司 运动参数调节方法、装置、存储介质及电子装置
CN117474906B (zh) * 2023-12-26 2024-03-26 合肥吉麦智能装备有限公司 基于脊柱x光图像匹配的术中x光机复位方法

Citations (29)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6965397B1 (en) * 1999-11-22 2005-11-15 Sportvision, Inc. Measuring camera attitude
US20100138079A1 (en) * 2006-02-17 2010-06-03 Oegren Petter Method for Teleoperating an Unmanned Ground Vehicle With a Pan Camera and Such a Ground Vehicle
CN201750495U (zh) * 2010-07-05 2011-02-16 杭州晨安机电技术有限公司 一种姿态自校正车载摄像机
CN102355574A (zh) * 2011-10-17 2012-02-15 上海大学 机载云台运动目标自主跟踪系统的图像稳定方法
CN102707734A (zh) * 2012-06-19 2012-10-03 上海大学 基于惯性姿态传感器的自稳定云台
US20140270743A1 (en) * 2013-03-15 2014-09-18 Freefly Systems, Inc. Method and system for enabling pointing control of an actively stabilized camera
CN104301715A (zh) * 2014-10-15 2015-01-21 天津市亚安科技股份有限公司 实现摄像机与云台3d预置精确联动的方法及装置
CN104508346A (zh) * 2013-12-10 2015-04-08 深圳市大疆创新科技有限公司 非正交轴载体
WO2015101822A1 (en) * 2014-01-02 2015-07-09 Mastortech Limited Camera stabilisation mounting
CN104854428A (zh) * 2013-12-10 2015-08-19 深圳市大疆创新科技有限公司 传感器融合
CN205534966U (zh) * 2016-02-01 2016-08-31 深圳市大疆创新科技有限公司 竖向增稳机构、云台装置及拍摄设备
CN106029501A (zh) * 2014-12-23 2016-10-12 深圳市大疆创新科技有限公司 Uav全景成像
CN106200693A (zh) * 2016-08-12 2016-12-07 东南大学 土地调查小型无人机的云台实时控制系统及控制方法
CN106231192A (zh) * 2016-08-04 2016-12-14 北京二郎神科技有限公司 一种图像采集方法和装置
CN106292741A (zh) * 2016-09-27 2017-01-04 成都普诺思博科技有限公司 一种基于无刷电机的移动机器人云台系统
WO2017011945A1 (zh) * 2015-07-17 2017-01-26 深圳市尚腾影科技有限公司 姿态数据输入装置及方法、云台控制装置及方法
CN106953553A (zh) * 2017-03-12 2017-07-14 纳恩博（北京）科技有限公司 一种云台、及云台电机的控制方法和装置
WO2017132813A1 (zh) * 2016-02-01 2017-08-10 深圳市大疆灵眸科技有限公司 竖向增稳机构、云台装置及拍摄设备
CN107079103A (zh) * 2016-05-31 2017-08-18 深圳市大疆灵眸科技有限公司 云台控制方法、装置和云台
CN107077146A (zh) * 2016-12-30 2017-08-18 深圳市大疆灵眸科技有限公司 用于云台的控制方法、控制系统、云台和无人飞行器
CN107065926A (zh) * 2017-04-12 2017-08-18 普宙飞行器科技（深圳）有限公司 全向避障装置、云台、云台的控制方法及避障控制方法
CN206417213U (zh) * 2016-12-30 2017-08-18 深圳市大疆灵眸科技有限公司 云台和无人飞行器
CN107223220A (zh) * 2016-12-30 2017-09-29 深圳市大疆灵眸科技有限公司 云台控制方法、装置及云台
CN107235013A (zh) * 2017-07-28 2017-10-10 深圳普思英察科技有限公司 车载导航定位全景云台
CN107257747A (zh) * 2015-02-28 2017-10-17 奥迪股份公司 用于补偿竖直运动的方法
WO2017177426A1 (zh) * 2016-04-14 2017-10-19 深圳市大疆灵眸科技有限公司 竖向增稳机构、云台装置、支撑装置及拍摄设备
CN107466385A (zh) * 2016-08-03 2017-12-12 深圳市大疆灵眸科技有限公司 一种云台控制方法及系统
CN107741748A (zh) * 2017-10-13 2018-02-27 杭州数尔安防科技股份有限公司 一种双轴位置自动矫正的装置
CN207191468U (zh) * 2017-09-11 2018-04-06 深圳市大疆创新科技有限公司 云台相机及具有该云台相机的无人机

Family Cites Families (4)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN102768042B (zh) 2012-07-11 2015-06-24 清华大学 一种视觉-惯性组合导航方法
CN103900473A (zh) 2014-03-31 2014-07-02 浙江大学 一种基于摄像头与重力感应器融合的智能移动设备六自由度位姿估计方法
CN104698485B (zh) 2015-01-09 2017-10-03 中国电子科技集团公司第三十八研究所 基于bd、gps及mems的组合导航系统及导航方法
CN104833352B (zh) 2015-01-29 2016-06-29 西北工业大学 多介质复杂环境下高精度视觉/惯性组合导航方法

Patent Citations (32)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6965397B1 (en) * 1999-11-22 2005-11-15 Sportvision, Inc. Measuring camera attitude
US20100138079A1 (en) * 2006-02-17 2010-06-03 Oegren Petter Method for Teleoperating an Unmanned Ground Vehicle With a Pan Camera and Such a Ground Vehicle
CN201750495U (zh) * 2010-07-05 2011-02-16 杭州晨安机电技术有限公司 一种姿态自校正车载摄像机
CN102355574A (zh) * 2011-10-17 2012-02-15 上海大学 机载云台运动目标自主跟踪系统的图像稳定方法
CN102707734A (zh) * 2012-06-19 2012-10-03 上海大学 基于惯性姿态传感器的自稳定云台
US20140270743A1 (en) * 2013-03-15 2014-09-18 Freefly Systems, Inc. Method and system for enabling pointing control of an actively stabilized camera
CN104854428A (zh) * 2013-12-10 2015-08-19 深圳市大疆创新科技有限公司 传感器融合
CN104508346A (zh) * 2013-12-10 2015-04-08 深圳市大疆创新科技有限公司 非正交轴载体
US20160273921A1 (en) * 2013-12-10 2016-09-22 SZ DJI Technology Co., Ltd. Sensor fusion
WO2015101822A1 (en) * 2014-01-02 2015-07-09 Mastortech Limited Camera stabilisation mounting
CN104301715A (zh) * 2014-10-15 2015-01-21 天津市亚安科技股份有限公司 实现摄像机与云台3d预置精确联动的方法及装置
CN106029501A (zh) * 2014-12-23 2016-10-12 深圳市大疆创新科技有限公司 Uav全景成像
CN107257747A (zh) * 2015-02-28 2017-10-17 奥迪股份公司 用于补偿竖直运动的方法
WO2017011945A1 (zh) * 2015-07-17 2017-01-26 深圳市尚腾影科技有限公司 姿态数据输入装置及方法、云台控制装置及方法
WO2017132813A1 (zh) * 2016-02-01 2017-08-10 深圳市大疆灵眸科技有限公司 竖向增稳机构、云台装置及拍摄设备
CN107278246A (zh) * 2016-02-01 2017-10-20 深圳市大疆灵眸科技有限公司 竖向增稳机构、云台装置及拍摄设备
CN205534966U (zh) * 2016-02-01 2016-08-31 深圳市大疆创新科技有限公司 竖向增稳机构、云台装置及拍摄设备
WO2017177426A1 (zh) * 2016-04-14 2017-10-19 深圳市大疆灵眸科技有限公司 竖向增稳机构、云台装置、支撑装置及拍摄设备
CN107079103A (zh) * 2016-05-31 2017-08-18 深圳市大疆灵眸科技有限公司 云台控制方法、装置和云台
CN107466385A (zh) * 2016-08-03 2017-12-12 深圳市大疆灵眸科技有限公司 一种云台控制方法及系统
WO2018024239A1 (zh) * 2016-08-04 2018-02-08 北京二郎神科技有限公司 混合式图像稳定系统
CN106231192A (zh) * 2016-08-04 2016-12-14 北京二郎神科技有限公司 一种图像采集方法和装置
CN106200693A (zh) * 2016-08-12 2016-12-07 东南大学 土地调查小型无人机的云台实时控制系统及控制方法
CN106292741A (zh) * 2016-09-27 2017-01-04 成都普诺思博科技有限公司 一种基于无刷电机的移动机器人云台系统
CN107077146A (zh) * 2016-12-30 2017-08-18 深圳市大疆灵眸科技有限公司 用于云台的控制方法、控制系统、云台和无人飞行器
CN206417213U (zh) * 2016-12-30 2017-08-18 深圳市大疆灵眸科技有限公司 云台和无人飞行器
CN107223220A (zh) * 2016-12-30 2017-09-29 深圳市大疆灵眸科技有限公司 云台控制方法、装置及云台
CN106953553A (zh) * 2017-03-12 2017-07-14 纳恩博（北京）科技有限公司 一种云台、及云台电机的控制方法和装置
CN107065926A (zh) * 2017-04-12 2017-08-18 普宙飞行器科技（深圳）有限公司 全向避障装置、云台、云台的控制方法及避障控制方法
CN107235013A (zh) * 2017-07-28 2017-10-10 深圳普思英察科技有限公司 车载导航定位全景云台
CN207191468U (zh) * 2017-09-11 2018-04-06 深圳市大疆创新科技有限公司 云台相机及具有该云台相机的无人机
CN107741748A (zh) * 2017-10-13 2018-02-27 杭州数尔安防科技股份有限公司 一种双轴位置自动矫正的装置

Cited By (3)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN111399476A (zh) * 2020-03-13 2020-07-10 江西憶源多媒体科技有限公司 一种基于图像对齐的监控云台失控实时检测方法
CN111399476B (zh) * 2020-03-13 2023-01-10 江西憶源多媒体科技有限公司 一种基于图像对齐的监控云台失控实时检测方法
WO2022077284A1 (zh) * 2020-10-14 2022-04-21 深圳市大疆创新科技有限公司 用于可移动平台的位置姿态确定方法、相关装置和系统

Also Published As

Publication number Publication date
WO2019205034A1 (zh) 2019-10-31
EP3786757A4 (en) 2021-12-08
EP3786757A1 (en) 2021-03-03
CN110325822B (zh) 2023-06-27
EP3786757B1 (en) 2022-09-21
US20210033242A1 (en) 2021-02-04
US11274788B2 (en) 2022-03-15

Similar Documents

Publication Publication Date Title
CN110325822A (zh) 云台位姿修正方法和装置
US9541392B2 (en) Surveying system and method
Foxlin et al. Miniature six-DOF inertial system for tracking HMDs
CN102591353B (zh) 飞行体的飞行控制系统
US6975959B2 (en) Orientation and navigation for a mobile device using inertial sensors
JP4782111B2 (ja) 輸送手段の位置、姿勢、および／または飛行方向を推定するシステムおよび方法
Shen et al. Optical flow sensor/INS/magnetometer integrated navigation system for MAV in GPS-denied environment
US8826550B2 (en) Geographically north-referenced azimuth determination
US20090089001A1 (en) Self-calibrated azimuth and attitude accuracy enhancing method and system (SAAAEMS)
CN104698486B (zh) 一种分布式pos用数据处理计算机系统实时导航方法
CN109269471A (zh) 一种新型gnss接收机倾斜测量系统及方法
CN109186597B (zh) 一种基于双mems-imu的室内轮式机器人的定位方法
WO2015188409A1 (zh) 基于mems惯导的双四元数动中通天线控制方法及系统
CN105928515B (zh) 一种无人机导航系统
CN108375383B (zh) 多相机辅助的机载分布式pos柔性基线测量方法和装置
CN107621266B (zh) 基于特征点跟踪的空间非合作目标相对导航方法
US11226203B2 (en) Low cost INS
CN109931955A (zh) 基于状态相关李群滤波的捷联惯性导航系统初始对准方法
CN106352897B (zh) 一种基于单目视觉传感器的硅mems陀螺误差估计与校正方法
CN110296719A (zh) 一种在轨标定方法
Konrad et al. State estimation for a multirotor using tight-coupling of gnss and inertial navigation
Ruotsalainen et al. Mitigation of attitude and gyro errors through vision aiding
CN110068325A (zh) 一种车载ins/视觉组合导航系统的杆臂误差补偿方法
CN111141285A (zh) 一种航空重力测量装置
CN114264302B (zh) 倾斜测量设备及其的惯性导航系统的初始化方法

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