CN112013868B - 一种基于视觉惯导里程计的自适应参数警犬姿态估计方法 - Google Patents

一种基于视觉惯导里程计的自适应参数警犬姿态估计方法 Download PDF

Info

Publication number
CN112013868B
CN112013868B CN202010881455.4A CN202010881455A CN112013868B CN 112013868 B CN112013868 B CN 112013868B CN 202010881455 A CN202010881455 A CN 202010881455A CN 112013868 B CN112013868 B CN 112013868B
Authority
CN
China
Prior art keywords
pose
posture
transformation matrix
police dog
camera
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
CN202010881455.4A
Other languages
English (en)
Other versions
CN112013868A (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.)
Hohai University HHU
Original Assignee
Hohai University HHU
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 Hohai University HHU filed Critical Hohai University HHU
Priority to CN202010881455.4A priority Critical patent/CN112013868B/zh
Publication of CN112013868A publication Critical patent/CN112013868A/zh
Application granted granted Critical
Publication of CN112013868B publication Critical patent/CN112013868B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C23/00—Combined instruments indicating more than one navigational value, e.g. for aircraft; Combined measuring devices for measuring two or more variables of movement, e.g. distance, speed or acceleration
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01K—ANIMAL HUSBANDRY; AVICULTURE; APICULTURE; PISCICULTURE; FISHING; REARING OR BREEDING ANIMALS, NOT OTHERWISE PROVIDED FOR; NEW BREEDS OF ANIMALS
    • A01K15/00—Devices for taming animals, e.g. nose-rings or hobbles; Devices for overturning animals in general; Training or exercising equipment; Covering boxes
    • A—HUMAN NECESSITIES
    • A01—AGRICULTURE; FORESTRY; ANIMAL HUSBANDRY; HUNTING; TRAPPING; FISHING
    • A01K—ANIMAL HUSBANDRY; AVICULTURE; APICULTURE; PISCICULTURE; FISHING; REARING OR BREEDING ANIMALS, NOT OTHERWISE PROVIDED FOR; NEW BREEDS OF ANIMALS
    • A01K29/00—Other apparatus for animal husbandry
    • 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

Landscapes

  • Engineering & Computer Science (AREA)
  • Remote Sensing (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Environmental Sciences (AREA)
  • Biodiversity & Conservation Biology (AREA)
  • Animal Husbandry (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Aviation & Aerospace Engineering (AREA)
  • Animal Behavior & Ethology (AREA)
  • Zoology (AREA)
  • Automation & Control Theory (AREA)
  • Image Analysis (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，运用双目视觉惯导里程计模块，设计一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，该方法包括：双目视觉惯导里程计模块得到相机六自由度位姿数据和相机连续轨迹数据，用警犬的站姿态进行初始化，同时计算出站姿态的识别范围，初始化站姿态结束，检测相机位姿的改变，在警犬的静态姿态估计中，加入自适应参数学习算法；本发明所提供的基于视觉惯导里程计的自适应参数警犬姿态估计方法，通过自学习特定姿态数据、自更新姿态识别界定范围，不仅能广泛适配于警犬的姿态估计，还能降低其复杂的系统架构带来的潜在系统不稳定性，具有更高的识别精度和可靠性。

Description

一种基于视觉惯导里程计的自适应参数警犬姿态估计方法
技术领域
本发明属于计算机视觉和模式识别领域，尤其涉及一种基于视觉惯导里程计的自适应参数警犬姿态估计方法。
背景技术
视觉惯导里程计(VIO),融合了视觉里程计技术和惯导技术来估计六自由度位姿，其综合了惯性传感器的稳定性和视觉里程计的高精度，所需设备简单且易于安装，非常适合用于获取物体的位姿和轨迹信息。目前常用的双目视觉惯导里程计主要由惯性传感器和双目相机在数据融合后实现测准，可得到相机的实时位姿和运动轨迹。
传统的警犬等中小型动物姿态估计方法主要使用多种传感器数据融合得到姿态数据，映射到动物的特定姿态模式，辅以机器学习算法程序来增强动物姿态模式识别。但是，单惯性传感器的定位误差随时间累积迅速增大，也无法进行警犬的动态姿态估计；其次多种传感器融合构成的系统硬件架构复杂，且随着传感器增多，系统可靠性降低。同时，使用传统机器学习算法程序进行警犬姿态的模式识别，需要大量已经标记好的数据进行训练，对一定数量的警犬进行姿态数据采集、手动标定数据和数据集训练测试都具有较高的时间代价和成本代价；训练得到的参数结果只针对特定的警犬，无法应用于多种类的警犬姿态估计，不具备可行性和普适性。
发明内容
本发明所要解决的技术问题是针对背景技术的不足，提供一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，使用安装在警犬身上的双目视觉惯导里程计模块设计出警犬静动态姿态的估计和自适应参数学习算法，该方法通过自学习特定姿态数据、自更新姿态识别界定范围，不仅能广泛适配于警犬的姿态估计，还能降低其复杂的系统架构带来的潜在系统不稳定性，具有更高的识别精度和可靠性。
为解决上述技术问题，本发明采用的技术方案是：
一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，包括双目视觉惯导里程计模块，所述双目视觉惯导里程计模块包含惯性传感器和双目相机，在警犬的侧身安装双目相机，双目相机内置惯性传感器，以双目相机的两个焦距中点为坐标原点o，x轴指向警犬前进方向，y轴指向警犬上方，z轴指向警犬前进方向的左侧，建立相机坐标系o-xyz；双目相机通过内置的惯性传感器输出相机位置参数；相机位置参数包括偏航角θz，俯仰角θy，翻滚角θx、坐标原点o的当前坐标(x，y，z)；
运用双目视觉惯导里程计模块，设计一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，具体方法包括如下步骤：
步骤1、首先，用警犬的站姿态进行初始化，同时计算出站姿态的识别范围；
步骤2、从初始化站姿态结束，检测相机位姿的改变：若相机的地面坐标(xz)未发生改变，即警犬未发生位移，则发生的是坐卧立站之间的静态姿态转换,静态姿态中通过分析相机坐标系的y轴变化和相机的翻滚角角度变化来识别警犬的静态姿态；若检测到相机的地面坐标(xz)发生改变，即警犬开始位移，则能识别到警犬的奔跑行走跳跃等动态姿态，通过分析相机坐标系的xz轴变化和相机的偏航角及俯仰角角度变化能获得警犬的轨迹、朝向和趋向，通过分析相机坐标系的y轴变化和相机的翻滚角角度变化可获得警犬的动态姿态变化，识别警犬的动态姿态；重复以上识别过程，检测警犬的静动态姿态之间的转换；
步骤3、在对警犬的静态姿态估计中，加入自适应参数学习算法。
作为本发明的进一步优选方案，所述步骤1具体如下：
初始化双目相机上的位姿坐标系，得到初步的y轴零点；在初始化的tini时间内每间隔Δt获得一次站姿态变换矩阵，保存在初始化站姿态变换矩阵数组中，在初始化站姿态变换矩阵数组中，以初始化y轴坐标数组和初始化θz角度数组为例，假设它们在初始化的静态站姿态中分别满足高斯分布：
Figure BDA0002654246120000021
Figure BDA0002654246120000022
其中，ys、θzs分别为在tini时间内初始y轴坐标和初始θz角度的均值，σs1、σs2为分别为各自高斯函数的均方差，f(yi)、f(θzi)分别为第i个测得的初始y轴坐标yi、第i个测得的初始角度θzi在tini时间内的出现频率，其中i为在tini时间内测得的y轴坐标和θz角度的编号，范围为1到n；
将初始y轴坐标和初始θz角度分别进行高斯分布拟合，采用最优化估计中的最小二乘法来估算参数，令ys和θzs分别为初始化数组的平均值，R(yi)和R(θzi)为每个在tini时间内测得的y轴坐标和θz角度数值的频率：
R(yi)＝count(yi)/n
R(θzi)＝count(θzi)/n
其中count(yi)和count(θzi)为ys和θzs在初始化数组中的出现频数，得到如下两个目标函数：
Figure BDA0002654246120000031
Figure BDA0002654246120000032
通过求取min(L(σs1))和min(L(σs2))，得到满足高斯分布的σs1和σs2均方差，从而得到警犬站姿态的y轴坐标和θz角度的识别范围，同理得到其他位姿数据：
Figure BDA0002654246120000033
作为本发明的进一步优选方案，所述步骤2具体如下：
步骤2.1、若在t时刻警犬活动改变了位姿数据，结束初始化站姿态；
步骤2.2、间隔Δt时间重复获得当前的相机位姿变换矩阵，如果发现相机地面坐标(xz)未发生变化，则等到位姿变化停止时，将变化停止后的稳定变换矩阵和初始化的站姿态变化矩阵进行比较，检测位姿数据是否变化，完成警犬静态姿态的识别，等待下一次警犬活动；
步骤2.3、间隔Δt时间重复获得当前的相机位姿变换矩阵，如果检测到相机地面坐标(xz)发生变化，则获取对应的相机位姿变换矩阵，与Δt时间之前的相机位姿变换矩阵作比较，得到基于地面坐标系(xz轴构成面)上的距离变化Δx和Δz，计算瞬时速度并保存在速度数组中：
Figure BDA0002654246120000034
将每次获得的动态变换矩阵和初始化的站姿态变化矩阵进行比较，将每次获得的瞬时速度和速度标准进行比较，得出警犬动态姿态的识别，等待下一次警犬活动：
步骤2.4、将动态姿态变换矩阵存储在数组中，继续判断，如未有停止指令，重复进行步骤2.2和步骤2.3，通过不断测算进行警犬静动态姿态的估计。
作为本发明的进一步优选方案，在所述步骤2.2中，将变化停止后的稳定变换矩阵和初始化的站姿态变化矩阵进行如下具体比较：
(1)如果当前相机y轴坐标和θz角度落入站姿态识别范围内：
Figure BDA0002654246120000041
则输出为站姿态，将当前变换矩阵保存到站姿态变换矩阵数组；
(2)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同：
Figure BDA0002654246120000042
其中，h为警犬身上双目相机到地面的高度，
则输出为坐姿态，将当前变换矩阵保存到坐姿态变换矩阵数组；
(3)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同：
Figure BDA0002654246120000043
则输出为立姿态，将当前变换矩阵保存到立姿态变换矩阵数组；
(4)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同：
Figure BDA0002654246120000044
则输出为卧姿态，将当前变换矩阵保存到卧姿态变换矩阵数组；
(5)如果全部不符合，则输出为未知姿态，将当前变换矩阵保存到未知姿态变换矩阵数组以备查看。
作为本发明的进一步优选方案，在步骤2.3中，将每次获得的动态变换矩阵和初始化的站姿态变化矩阵进行比较，将每次获得的瞬时速度和速度标准进行比较，具体比较步骤如下：
(1)如果当前动态位姿落在如下的识别范围内：
Figure BDA0002654246120000045
则输出为奔跑姿态；
(2)如果当前动态位姿落在如下的识别范围内：
Figure BDA0002654246120000051
则输出为行走姿态；
(3)如果当前动态位姿落在如下的识别范围内：
Figure BDA0002654246120000052
则输出为跳跃姿态。
作为本发明的进一步优选方案，所述步骤3具体如下：
步骤3.1、在识别过程中可获得一定数量的站坐卧立静态姿态的实际数据，将每一种静态姿态变换矩阵数组分别提取，依次进行警犬姿态的自适应参数学习计算；
步骤3.2、假设总共有n个变换矩阵，以一组测得的坐姿态的变换矩阵数组为例，同步骤1将这一序列进行高斯分布拟合操作，令ysd、θzsd分别为坐标和角度的数组均值，使用最小二乘法获得其均方差σsd1和σsd2，此时得到初步的识别范围：
Figure BDA0002654246120000053
步骤3.3、为防止自学习过程出现异常值和缺失值的影响，则将上述所得到的初步的识别范围更新为加常数的宽限范围：
Figure BDA0002654246120000054
其中cy、cr称为加常数，一般根据不同姿态序列的实际情况使用不同的常数；
步骤3.4、搜索坐姿态变换矩阵数组，如果发现存在ysd[i]或θzsd[i]落入加常数区间：
ysd[i]∈[y'sdmin,ysdmin)或ysd[i]∈(ysdmax,y'sdmax]
θzsd[i]∈[θ'zsdmin,θzsdmin)或θzsd[i]∈(θzsdmax,θ'zsdmax]
则更新步骤3.2中初步识别范围的上下限参数：
ysdmin＝ysd[i]或ysdmax＝ysd[i]
θzsdmin＝θzsd[i]或θzsdmax＝θzsd[i]
将修改后的上下限参数ysdmin/ysdmax和θzsdmin/θzsdmax代入步骤3.3，更新加常数的宽限范围[y'sdmin,y'sdmax]和[θ'zsdmin,θ'zsdmax]，获得最终的识别范围区间；
步骤3.5、识别范围自更新后，将使用了加常数c的宽限范围替代坐姿态等静态姿态中预设的y和θz的上下限范围，进一步精准限定静态姿态的识别范围。
本发明具有如下有益效果：
1.本发明仅使用双目惯导里程计来进行警犬的姿态估计，相比使用多种传感器融合进行警犬的姿态估计方法，不仅能降低其复杂的系统架构带来的潜在系统不稳定性，具有更高的识别精度和可靠性；
2.本发明提出的警犬姿态估计方法拥有一个完备的警犬静动态姿态识别体系，其只需要根据警犬的站姿态初始化就能得到其他静动态姿态的识别范围区间，其简单快捷高效的特点能满足基本的警犬姿态估计需求，具有很好的通用性；
3.本发明针对警犬的姿态估计提出了一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，该自适应参数学习算法方法通过自学习、自更新的操作，使用加常数的宽限范围优化姿态识别区间，不仅可以在一定程度上得到更具描述意义的姿态识别范围，还能避免可能出现的过度优化，是兼具准确性、完备性和实用性的警犬姿态估计。
附图说明
图1是本发明一种基于视觉惯导里程计的自适应参数警犬姿态估计方法的警犬坐标系xyz与相机坐标系xzy的对应示意图；
图2是本发明一种基于视觉惯导里程计的自适应参数警犬姿态估计方法的警犬静动态姿态估计的流程图；
图3是本发明一种基于视觉惯导里程计的自适应参数警犬姿态估计方法的自适应参数学习算法流程图。
具体实施方式
下面结合附图和具体较佳实施方式对本发明作进一步详细的说明。
下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。基于本发明中的实施例，本领域普通技术人员在没有做出创造性劳动前提下所获得的所有其他实施例，都属于本发明保护的范围。
本发明提供的一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，包括双目视觉惯导里程计模块，
双目视觉惯导里程计模块包含惯性传感器和双目相机，在此以双目相机(内置惯性传感器)为该模块的基础装置，双目相机将位姿数据以变换矩阵T的形式输出：
Figure BDA0002654246120000071
其中，左上角的R是旋转矩阵，用来描述相机的旋转；右上角的t是平移向量，用来描述相机的位移，左下角为0向量。
欧拉角(俯仰角，翻滚角，偏航角)是另外一种直观的、符合人类感知和便于算法设计的描述旋转的方式，将欧拉角转换为旋转矩阵公式如下：
Figure BDA0002654246120000072
Figure BDA0002654246120000073
Figure BDA0002654246120000074
Figure BDA0002654246120000075
Figure BDA0002654246120000076
其中，θ为通用的角度参数，Rz(θz)、Ry(θy)、Rx(θx)是绕不同轴旋转的欧拉角转换得到的旋转矩阵，在此使用Z-Y-X欧拉角体系，θz对应于偏航角，θy对应于俯仰角，θx对应于翻滚角。
令r11＝cosθycosθz，r12＝sinθxsinθycosθz-cosθxsinθz，r13＝cosθxsinθycosθz+sinθxsinθz，r21＝cosθysinθzr22＝sinθxsinθysinθz+cosθxcosθz，r23＝cosθxsinθysinθz-sinθxcosθz，r31＝-sinθy，r32＝sinθxcosθy，r33＝cosθxcosθy；
将上述R用r11至r33表示为：
Figure BDA0002654246120000081
解方程，可将旋转矩阵转换为欧拉角：
θx＝atan2(r32，r33)
Figure BDA0002654246120000082
θz＝atan2(r21，r11)
其中atan2(y，x)表示y/x的反正切，其返回值在[-π，+π]之间；
在警犬的侧身安装双目相机，双目相机内置惯性传感器；然后，以双目相机的两个焦距中点为坐标原点o，x轴指向警犬前进方向，y轴指向警犬上方，z轴指向警犬前进方向的左侧，建立相机坐标系o-xyz；双目相机通过内置的惯性传感器输出相机位置参数，相机位置参数包括坐标原点o的当前坐标(x，y，z)和偏航角θz、俯仰角θy、翻滚角θx，所述偏航角θz、俯仰角θy和翻滚角θx的逆时针方向均为正方向。
如图2所示，运用双目视觉惯导里程计模块，设计一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，具体方法包括如下步骤：
步骤1、首先，用警犬的站姿态进行初始化，同时计算出站姿态的识别范围；
步骤2、从初始化站姿态结束，检测相机位姿的改变：若相机的地面坐标(xz)未发生改变，即警犬未发生位移，则主要发生的是坐卧立站之间的静态姿态转换，静态姿态中主要通过分析相机坐标系的y轴变化和相机的翻滚角角度变化来识别警犬的静态姿态；
若检测到相机的地面坐标(xz)发生改变，即警犬开始位移，则能识别到警犬的奔跑行走跳跃等动态姿态，通过分析相机坐标系的xz轴变化和相机的偏航角及俯仰角角度变化能获得警犬的轨迹、朝向和趋向，通过分析相机坐标系的y轴变化和相机的翻滚角角度变化可获得警犬的动态姿态变化，识别警犬的动态姿态。重复这个识别过程，也能检测
到静动态姿态之间的转换；
步骤3、对于警犬相对重要的静态姿态估计中，设计加入了自适应参数学习算法，能尽可能地适配不同的警犬，也可适配类警犬等中小型动物的姿态估计。
在上述步骤1中：
初始化双目相机上的位姿坐标系，得到初步的y轴零点。在tini时间内每间隔Δt获得一次站姿态变换矩阵，在本实施例中，默认使用相机基于设定正方向上的变换矩阵，保存在初始化站姿态变换矩阵数组中,在本实施例中tini取10s，Δt取0.2s；
在初始化站姿态变换矩阵数组，以初始化y轴坐标数组和初始化θz角度(双目相机的翻滚角)数组为例，假设它们在初始化的静态站姿态中分别满足高斯分布：
Figure BDA0002654246120000091
Figure BDA0002654246120000092
其中，ys、θzs分别为在tini时间内初始y轴坐标和初始θz角度的均值，σs1、σs2为分别为各自高斯函数的均方差，f(yi)、f(θzi)分别为第i个测得的初始y轴坐标yi、第i个测得的初始角度θzi在tini时间内的出现频率，其中i为在tini时间内测得的y轴坐标和θz角度的编号，范围为1到n；
将初始y轴坐标和初始θz角度分别进行高斯分布拟合，本实施例采用最优化估计中的最小二乘法来估算参数，令ys和θzs分别为初始化数组的平均值，R(yi)和R(θzi)为每个在tini时间内测得的y轴坐标和θz角度数值的频率：
R(yi)＝count(yi)/n
R(θzi)＝count(θzi)/n，
其中count(yi)和count(θzi)为ys和θzs在初始化数组中的出现频数，得到如下两个目标函数：
Figure BDA0002654246120000101
Figure BDA0002654246120000102
通过求取min(L(σs1))和min(L(σs2))，得到满足高斯分布的σs1和σs2均方差，从而得到警犬站姿态的y轴坐标和θz角度的识别范围，同理可得到其他位姿数据：
Figure BDA0002654246120000103
在上述步骤2中，具体步骤如下：
步骤2.1、在t时刻，警犬活动改变了位姿数据，结束初始化站姿态；
步骤2.2、间隔Δt时间重复获得当前的相机位姿变换矩阵；在本实施例中，Δt取0.2s，将新获得的位姿变换矩阵和先前的位姿变换矩阵相比，如果相机地面坐标(xz)未发生变化，则等到位姿变化停止时，将变化停止后的稳定变换矩阵和初始化的站姿态变化矩阵进行比较：
(1)如果当前相机y轴坐标和θz角度落入站姿态识别范围内：
Figure BDA0002654246120000104
即当前位姿在站姿态的识别范围内，则输出为站姿态，将当前变换矩阵保存到站姿态变换矩阵数组；
(2)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同，其中，h为警犬身上双目相机到地面的高度；在本实施例中，测得警犬的h为0.32m：
Figure BDA0002654246120000105
即如果当前位姿y轴坐标相比站姿态y轴坐标有0-16.0cm的降低，当前翻滚角角度相比站姿态翻滚角角度发生逆时针旋转，处于30-60度的范围区间，其他位姿数据在站姿态范围内相同，则输出为坐姿态，将当前变换矩阵保存到坐姿态变换矩阵数组；
(3)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同：
Figure BDA0002654246120000111
即当前位姿y轴坐标相比站姿态y轴坐标有超过16.0cm的升高，当前翻滚角角度相比站姿态翻滚角角度发生逆时针旋转，处于50-90度的范围区间，其他位姿数据在站姿态范围内相同，则输出为立姿态，将当前变换矩阵保存到立姿态变换矩阵数组；
(4)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同：
Figure BDA0002654246120000112
即当前位姿y轴坐标相比站姿态y轴坐标有超过19.2cm的降低，当前翻滚角角度处于0-30度的范围区间，其他位姿数据在站姿态范围内相同，则输出为卧姿态，将当前变换矩阵保存到卧姿态变换矩阵数组；
(5)如果全部不符合，则输出为未知姿态，将当前变换矩阵保存到未知姿态变换矩阵数组以备查看。
静态姿态识别完成后，检测位姿数据是否变化，等待下一次警犬活动。
步骤2.3、间隔Δt时间重复获得当前的相机位姿变换矩阵，Δt取0.2s。将新获得的位姿变换矩阵和先前的位姿变换矩阵相比，如果相机地面坐标(xz)发生变化，则获取对应的相机位姿变换矩阵，与Δt时间之前的相机位姿变换矩阵作比较，得到基于地面坐标系(xz轴构成面)上的距离变化Δx和Δz，计算瞬时速度并保存在速度数组中：
Figure BDA0002654246120000113
将0.2s获得的动态变换矩阵和初始化的站姿态变化矩阵进行比较，将计算得到的瞬时速度和速度标准进行比较：
(1)如果当前动态位姿落在如下的识别范围内：
Figure BDA0002654246120000114
即当前位姿y轴坐标相比站姿态y轴坐标处于[ys-0.16,ys+0.54]的区间范围，瞬时速度不小于5m/s，则输出为奔跑姿态；
(2)如果当前动态位姿落在如下的识别范围内：
Figure BDA0002654246120000121
即当前位姿y轴坐标相比站姿态y轴坐标处于[ys-0.16,ys+0.54]的区间范围，瞬时速度小于5m/s，则输出为行走姿态；
(3)如果当前动态位姿落在如下的识别范围内：
Figure BDA0002654246120000122
即当前位姿y轴坐标相比站姿态y轴坐标处于(ys+0.54,+∞)的区间范围，瞬时速度不小于2m/s，则输出为跳跃姿态；
将动态姿态变换矩阵存储在数组中，继续判断。
步骤2.4、如未有停止指令，重复进行步骤2.2和步骤2.3，通过不断的测算进行警犬静动态姿态的估计。
所述步骤3具体如下：
步骤3.1、在识别过程中可获得一定数量的站坐卧立静态姿态的实际数据。将每一种静态姿态变换矩阵数组分别提取，依次进行警犬姿态的自适应参数学习算法。
步骤3.2、以一组测得的坐姿态的变换矩阵数组为例(假设总共有n个变换矩阵)，同步骤1将这一序列进行高斯分布拟合操作，令ysd、θzsd分别为坐标和角度的数组均值，
使用最小二乘法获得其均方差σsd1和σsd2，此时得到初步的识别范围：
Figure BDA0002654246120000123
步骤3.3、为防止自学习过程出现异常值和缺失值的影响，则将其识别范围更新为加常
数的宽限范围：
Figure BDA0002654246120000124
其中cy、cr称为加常数，一般根据不同姿态序列的实际情况使用不同的常数，在这段坐姿态变换矩阵数组序列中，cy取0.010,cr取1°；
步骤3.4、搜索坐姿态变换矩阵数组，如果发现存在ysd[i]或θzsd[i]落入加常数区间：
ysd[i]∈[y'sdmin,ysdmin)或ysd[i]∈(ysdmax,y'sdmax]
θzsd[i]∈[θ'zsdmin,θzsdmin)或θzsd[i]∈(θzsdmax,θ'zsdmax]
则更新第2点中初步识别范围的上下限参数：
ysdmin＝ysd[i]或ysdmax＝ysd[i]
θzsdmin＝θzsd[i]或θzsdmax＝θzsd[i]
将修改后的上下限参数ysdmin/ysdmax和θzsdmin/θzsdmax代入第3点，更新加常数的宽限范围[y'sdmin,y'sdmax]和[θ'zsdmin,θ'zsdmax]，获得最终的识别范围区间；
步骤3.5、识别范围自更新后，将使用了加常数c的宽限范围替代坐姿态等静态姿态中预设的y和θz的上下限(详见步骤2)，进一步精准限定静态姿态的识别范围。这样既能通过自适应参数学习算法来得到更具姿态描述意义的识别区间，也能在一定程度上避免过度优化，出现不灵敏的问题。
至此，通过该实施例演示结合双目视觉惯导里程计模块和警犬姿态估计和自适应参数学习算法，设计实现基于视觉惯导里程计的自适应参数警犬姿态估计方法。
以上实施例仅为说明本发明的技术思想，不能以此限定本发明的保护范围，凡是按照本发明提出的技术思想，在技术方案基础上所做的任何改动，均落入本发明保护范围之内。上面对本发明的实施方式作了详细说明，但是本发明并不限于上述实施方式，在本领域普通技术人员所具备的知识范围内，还可以在不脱离本发明宗旨的前提下做出各种变化。

Claims (4)

1.一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，其特征在于：包括双目视觉惯导里程计模块，所述双目视觉惯导里程计模块包含惯性传感器和双目相机，在警犬的侧身安装双目相机，双目相机内置惯性传感器，以双目相机的两个焦距中点为坐标原点o，x轴指向警犬前进方向，y轴指向警犬上方，z轴指向警犬前进方向的左侧，建立相机坐标系o-xyz；双目相机通过内置的惯性传感器输出相机位置参数；
相机位置参数包括偏航角θz，俯仰角θy，翻滚角θx、坐标原点o的当前坐标(x，y，z)；
运用双目视觉惯导里程计模块，设计一种基于视觉惯导里程计的自适应参数警犬姿态估计方法，具体方法包括如下步骤：
步骤1、首先，用警犬的站姿态进行初始化，同时计算出站姿态的识别范围，具体如下：
初始化双目相机上的位姿坐标系，得到初步的y轴零点；在初始化的tini时间内每间隔Δt获得一次站姿态变换矩阵，保存在初始化站姿态变换矩阵数组中，在初始化站姿态变换矩阵数组中，以初始化y轴坐标数组和初始化θz角度数组为例，假设它们在初始化的静态站姿态中分别满足高斯分布：
Figure FDA0003598562560000011
Figure FDA0003598562560000012
其中，ys、θzs分别为在tini时间内初始y轴坐标和初始θz角度的均值，σs1、σs2为分别为各自高斯函数的均方差，f(yi)、f(θzi)分别为第i个测得的初始y轴坐标yi、第i个测得的初始角度θzi在tini时间内的出现频率，其中i为在tini时间内测得的y轴坐标和θz角度的编号，范围为1到n；
将初始y轴坐标和初始θz角度分别进行高斯分布拟合，采用最优化估计中的最小二乘法来估算参数，令ys和θzs分别为初始化数组的平均值，R(yi)和R(θzi)为每个在tini时间内测得的y轴坐标和θz角度数值的频率：
R(yi)＝count(yi)/n
R(θzi)＝count(θzi)/n
其中count(yi)和count(θzi)为ys和θzs在初始化数组中的出现频数，得到如下两个目标函数：
Figure FDA0003598562560000021
Figure FDA0003598562560000022
通过求取min(L(σs1))和min(L(σs2))，得到满足高斯分布的σs1和σs2均方差，从而得到警犬站姿态的y轴坐标和θz角度的识别范围，同理得到其他位姿数据：
Figure FDA0003598562560000023
步骤2、从初始化站姿态结束，检测相机位姿的改变：若相机的地面坐标未发生改变，即警犬未发生位移，则发生的是坐卧立站之间的静态姿态转换,静态姿态中通过分析相机坐标系的y轴变化和相机的翻滚角角度变化来识别警犬的静态姿态；若检测到相机的地面坐标发生改变，即警犬开始位移，则能识别到警犬的奔跑、行走、跳跃动态姿态，通过分析相机坐标系的xz轴变化和相机的偏航角及俯仰角角度变化能获得警犬的轨迹、朝向和趋向，通过分析相机坐标系的y轴变化和相机的翻滚角角度变化获得警犬的动态姿态变化，识别警犬的动态姿态；重复以上识别过程，检测警犬的静动态姿态之间的转换；
步骤3、在对警犬的静态姿态估计中，加入自适应参数学习算法，具体步骤如下：
步骤3.1、在识别过程中获得一定数量的站坐卧立静态姿态的实际数据，将每一种静态姿态变换矩阵数组分别提取，依次进行警犬姿态的自适应参数学习计算；
步骤3.2、假设总共有n个变换矩阵，每组测得的坐姿态的变换矩阵数组，同步骤1将这一序列进行高斯分布拟合操作，令ysd、θzsd分别为坐标和角度的数组均值，使用最小二乘法获得其均方差σsd1和σsd2，此时得到初步的识别范围：
Figure FDA0003598562560000024
步骤3.3、为防止自学习过程出现异常值和缺失值的影响，则将上述所得到的初步的识别范围更新为加常数的宽限范围：
Figure FDA0003598562560000031
其中cy、cr称为加常数，一般根据不同姿态序列的实际情况使用不同的常数；
步骤3.4、搜索坐姿态变换矩阵数组，如果发现存在ysd[i]或θzsd[i]落入加常数区间：
ysd[i]∈[y'sdmin,ysdmin)或ysd[i]∈(ysdmax,y'sdmax]
θzsd[i]∈[θ'zsdmin,θzsdmin)或θzsd[i]∈(θzsdmax,θ'zsdmax]
则更新步骤3.2中初步识别范围的上下限参数：
ysdmin＝ysd[i]或ysdmax＝ysd[i]
θzsdmin＝θzsd[i]或θzsdmax＝θzsd[i]
将修改后的上下限参数ysdmin/ysdmax和θzsdmin/θzsdmax代入步骤3.3，更新加常数的宽限范围[y'sdmin,y'sdmax]和[θ'zsdmin,θ'zsdmax]，获得最终的识别范围区间；
步骤3.5、识别范围自更新后，将使用了加常数c的宽限范围替代坐姿态静态姿态中预设的y和θz的上下限范围，进一步精准限定静态姿态的识别范围。
2.根据权利要求1所述的基于视觉惯导里程计的自适应参数警犬姿态估计方法，其特征在于：所述步骤2具体如下：
步骤2.1、若在t时刻警犬活动改变了位姿数据，结束初始化站姿态；
步骤2.2、间隔Δt时间重复获得当前的相机位姿变换矩阵，如果发现相机地面坐标未发生变化，则等到位姿变化停止时，将变化停止后的稳定变换矩阵和初始化的站姿态变化矩阵进行比较，检测位姿数据是否变化，完成警犬静态姿态的识别，等待下一次警犬活动；
步骤2.3、间隔Δt时间重复获得当前的相机位姿变换矩阵，如果检测到相机地面坐标发生变化，则获取对应的相机位姿变换矩阵，与Δt时间之前的相机位姿变换矩阵作比较，得到基于地面坐标系上的距离变化Δx和Δz，计算瞬时速度并保存在速度数组中：
Figure FDA0003598562560000041
将每次获得的动态变换矩阵和初始化的站姿态变化矩阵进行比较，将每次获得的瞬时速度和速度标准进行比较，得出警犬动态姿态的识别，等待下一次警犬活动：
步骤2.4、将动态姿态变换矩阵存储在数组中，继续判断，如未有停止指令，重复进行步骤2.2和步骤2.3，通过不断测算进行警犬静动态姿态的估计。
3.根据权利要求2所述的基于视觉惯导里程计的自适应参数警犬姿态估计方法，其特征在于：在所述步骤2.2中，将变化停止后的稳定变换矩阵和初始化的站姿态变化矩阵进行如下具体比较：
(1)如果当前相机y轴坐标和θz角度落入站姿态识别范围内：
Figure FDA0003598562560000042
则输出为站姿态，将当前变换矩阵保存到站姿态变换矩阵数组；
(2)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同：
Figure FDA0003598562560000043
其中，h为警犬身上双目相机到地面的高度，
则输出为坐姿态，将当前变换矩阵保存到坐姿态变换矩阵数组；
(3)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同：
Figure FDA0003598562560000044
则输出为立姿态，将当前变换矩阵保存到立姿态变换矩阵数组；
(4)如果当前位姿落在如下的识别范围内，其他位姿数据在站姿态识别范围内相同：
Figure FDA0003598562560000045
则输出为卧姿态，将当前变换矩阵保存到卧姿态变换矩阵数组；
(5)如果全部不符合，则输出为未知姿态，将当前变换矩阵保存到未知姿态变换矩阵数组以备查看。
4.根据权利要求2所述的基于视觉惯导里程计的自适应参数警犬姿态估计方法，其特征在于：在步骤2.3中，将每次获得的动态变换矩阵和初始化的站姿态变化矩阵进行比较，将每次获得的瞬时速度和速度标准进行比较，具体比较步骤如下：
(1)如果当前动态位姿落在如下的识别范围内：
Figure FDA0003598562560000051
则输出为奔跑姿态；
(2)如果当前动态位姿落在如下的识别范围内：
Figure FDA0003598562560000052
则输出为行走姿态；
(3)如果当前动态位姿落在如下的识别范围内：
Figure FDA0003598562560000053
则输出为跳跃姿态。
CN202010881455.4A 2020-08-27 2020-08-27 一种基于视觉惯导里程计的自适应参数警犬姿态估计方法 Active CN112013868B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202010881455.4A CN112013868B (zh) 2020-08-27 2020-08-27 一种基于视觉惯导里程计的自适应参数警犬姿态估计方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202010881455.4A CN112013868B (zh) 2020-08-27 2020-08-27 一种基于视觉惯导里程计的自适应参数警犬姿态估计方法

Publications (2)

Publication Number Publication Date
CN112013868A CN112013868A (zh) 2020-12-01
CN112013868B true CN112013868B (zh) 2022-07-29

Family

ID=73503467

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202010881455.4A Active CN112013868B (zh) 2020-08-27 2020-08-27 一种基于视觉惯导里程计的自适应参数警犬姿态估计方法

Country Status (1)

Country Link
CN (1) CN112013868B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114727081B (zh) * 2022-06-09 2022-09-06 深圳新智联软件有限公司 投影仪投影校正方法、装置及投影仪

Family Cites Families (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
IL173604A (en) * 2006-02-08 2013-01-31 E Afikim Milking Systems Agricultural Cooperative Ltd Sa A device and method for recording animal poses, especially for live animals
CN101826197B (zh) * 2009-03-06 2012-05-09 北京百奥金生物科技有限公司 一种监控动物日常活动的方法及系统
WO2011120529A1 (en) * 2010-03-31 2011-10-06 Københavns Universitet Model for classifying an activity of an animal
US20160073614A1 (en) * 2013-09-13 2016-03-17 Kyle Douglas Lampe System and Method for Detection of Lameness in Sport Horses and other Quadrupeds
WO2016200564A1 (en) * 2015-06-08 2016-12-15 Kyle Lampe System and method for detection of lameness in sport horses and other quadrupeds
CN105850773A (zh) * 2016-03-29 2016-08-17 西北农林科技大学 一种基于微惯性传感器的生猪姿态监测装置及方法
CN107018921A (zh) * 2016-11-04 2017-08-08 深圳市沃特沃德股份有限公司 终端和宠物姿态检测方法和装置
CN109673529A (zh) * 2018-12-24 2019-04-26 公安部南京警犬研究所 基于多传感器的警犬姿态识别数据背心及姿态识别方法

Also Published As

Publication number Publication date
CN112013868A (zh) 2020-12-01

Similar Documents

Publication Publication Date Title
CN110116407B (zh) 柔性机器人位姿测量方法及装置
CN107862719B (zh) 相机外参的标定方法、装置、计算机设备和存储介质
CN107590836B (zh) 一种基于Kinect的充电桩动态识别与定位方法及系统
CN110108269B (zh) 基于多传感器数据融合的agv定位方法
CN104858870A (zh) 基于末端编号靶球的工业机器人测量方法
CN113021358B (zh) 机械臂工具坐标系原点标定方法、装置和电子设备
CN112734765A (zh) 基于实例分割与多传感器融合的移动机器人定位方法、系统及介质
CN109465829A (zh) 一种基于转换矩阵误差模型的工业机器人几何参数辨识方法
CN113028990B (zh) 一种基于加权最小二乘的激光跟踪姿态测量系统及方法
CN112013868B (zh) 一种基于视觉惯导里程计的自适应参数警犬姿态估计方法
CN111331604A (zh) 一种基于机器视觉的阀门旋拧柔顺作业方法
CN115294198A (zh) 一种基于视觉的机械臂全局性能测量系统及方法
Chen et al. Self-calibration of environmental camera for mobile robot navigation
CN114510068A (zh) 一种基于信息融合的多无人机协同态势感知方法及系统
CN111645067B (zh) 一种高智能机器人环境感知方法及系统
CN111121818B (zh) 一种无人车中相机与二维码的标定方法
CN116929336A (zh) 一种基于最小误差的激光反光柱slam建图方法
CN109048911B (zh) 一种基于矩形特征的机器人视觉控制方法
CN110749327B (zh) 一种合作环境下的车辆导航方法
CN115585810A (zh) 一种基于室内全局视觉的无人车定位方法和装置
CN115436968A (zh) 一种基于激光雷达的位图化重定位方法
CN111649746B (zh) 一种融合惯导测量和ArUco标记的定位与导航方法
Kheng et al. Stereo vision with 3D coordinates for robot arm application guide
CN112614189A (zh) 一种基于摄像头和3d激光雷达的联合标定方法
Song et al. Localization Method Based on Image Processing for Autonomous Driving of Mobile Robot in the Linear Infrastructure

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