CN110221302A - 环境探测装置及其修正方法、***、便携设备及存储介质 - Google Patents
环境探测装置及其修正方法、***、便携设备及存储介质 Download PDFInfo
- Publication number
- CN110221302A CN110221302A CN201910441487.XA CN201910441487A CN110221302A CN 110221302 A CN110221302 A CN 110221302A CN 201910441487 A CN201910441487 A CN 201910441487A CN 110221302 A CN110221302 A CN 110221302A
- Authority
- CN
- China
- Prior art keywords
- detection device
- environmental detection
- module
- imu module
- axis
- 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
Links
Classifications
-
- 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
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C25/00—Manufacturing, calibrating, cleaning, or repairing instruments or devices referred to in the other groups of this subclass
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S13/00—Systems using the reflection or reradiation of radio waves, e.g. radar systems; Analogous systems using reflection or reradiation of waves whose nature or wavelength is irrelevant or unspecified
- G01S13/88—Radar or analogous systems specially adapted for specific applications
- G01S13/93—Radar or analogous systems specially adapted for specific applications for anti-collision purposes
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01S—RADIO DIRECTION-FINDING; RADIO NAVIGATION; DETERMINING DISTANCE OR VELOCITY BY USE OF RADIO WAVES; LOCATING OR PRESENCE-DETECTING BY USE OF THE REFLECTION OR RERADIATION OF RADIO WAVES; ANALOGOUS ARRANGEMENTS USING OTHER WAVES
- G01S7/00—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00
- G01S7/02—Details of systems according to groups G01S13/00, G01S15/00, G01S17/00 of systems according to group G01S13/00
- G01S7/40—Means for monitoring or calibrating
-
- Y—GENERAL TAGGING OF NEW TECHNOLOGICAL DEVELOPMENTS; GENERAL TAGGING OF CROSS-SECTIONAL TECHNOLOGIES SPANNING OVER SEVERAL SECTIONS OF THE IPC; TECHNICAL SUBJECTS COVERED BY FORMER USPC CROSS-REFERENCE ART COLLECTIONS [XRACs] AND DIGESTS
- Y02—TECHNOLOGIES OR APPLICATIONS FOR MITIGATION OR ADAPTATION AGAINST CLIMATE CHANGE
- Y02A—TECHNOLOGIES FOR ADAPTATION TO CLIMATE CHANGE
- Y02A90/00—Technologies having an indirect contribution to adaptation to climate change
- Y02A90/10—Information and communication technologies [ICT] supporting adaptation to climate change, e.g. for weather forecasting or climate simulation
Landscapes
- Engineering & Computer Science (AREA)
- Radar, Positioning & Navigation (AREA)
- Remote Sensing (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Computer Networks & Wireless Communication (AREA)
- Electromagnetism (AREA)
- Manufacturing & Machinery (AREA)
- Automation & Control Theory (AREA)
- Navigation (AREA)
- Gyroscopes (AREA)
Abstract
本发明提供一种环境探测装置及其修正方法、***、便携设备及存储介质,包括以下步骤:基于预设参数对IMU模块进行初始校准;基于IMU模块采集的数据判断所述环境探测装置是否静止,并在所述环境探测装置处于非静止状态时,对所述IMU模块的航向角进行倾斜补偿;对所述IMU模块的航向角进行误差修正,以获取所述IMU模块的实际偏移角度;基于IMU模块采集的数据判断所述环境探测装置是否直线移动;若是,基于所述实际偏移角度对所述雷达采集的方位角进行修正。本发明的环境探测装置及其修正方法、***、便携设备及存储介质基于IMU进行运动状态检测,并基于运动状态修正雷达采集的数据,从而有效地减少误报。
Description
技术领域
本发明涉及物联网设备的技术领域,特别是涉及一种环境探测装置及其修正方法、***、 便携设备及存储介质。
背景技术
现有的雷达探测设备,无论是车用、船用的,在某些场景下,需要维持雷达的探测方向 即波束方向在一固定位置,以避免偏离可能有障碍物的方位,造成探测目标的丢失。其原因 在于,雷达的作用在于探测周围环境中的障碍物,据以进行避障处理。在车辆行驶的环境下, 需要采集车辆前方、两侧及下方的障碍物;船只也是类似,尤其是船只常常处于颠簸环境下, 需要实时动态调整其雷达的波束方向。而现有盲人穿戴的智能避障设备中,当盲人在向前行 走时,设备由于走路而产生晃动,发生小幅度的偏转,而雷达在固定角度范围内进行回波检 测,此时收到的回波可能并不是当前盲人行走方向上的,从而导致障碍物方位的偏移,故存 在类似的动态调整雷达波束方向的需求。
现有的调整方式,是通过机械电控的方案,在该方案中,雷达是可动的;在检测到车、 船、或人运动而令雷达波束方向变化时,通过控制机械运动来调整雷达的位置,以令波束方 向维持在固定位置。
但是,这样一来,会增加机械结构、电机等部件,增加雷达探测设备的设计难度及设计 成本,而且对雷达位置调整的精度、实时性也有非常高的要求;且既需要计算雷达位置,又 要进行机械控制,对精度和实时性要求非常高,极难产业应用。
发明内容
鉴于以上所述现有技术的缺点,本发明的目的在于提供一种环境探测装置及其修正方法、 ***、便携设备及存储介质,基于惯性测量单元(Inertial Measurement Unit,IMU)进行运动 状态检测,并基于运动状态修正雷达采集的数据,从而有效地减少误报。
为实现上述目的及其他相关目的,本发明提供一种环境探测装置的修正方法,应用于环 境探测装置上,所述环境探测装置包括雷达和IMU模块,且所述IMU模块的Z轴纵向设置; 所述环境探测装置的修正方法包括以下步骤:基于预设参数对IMU模块进行初始校准;基于 IMU模块采集的数据判断所述环境探测装置是否静止,并在所述环境探测装置处于非静止状 态时,对所述IMU模块的航向角进行倾斜补偿;对所述IMU模块的航向角进行误差修正, 以获取所述IMU模块的实际偏移角度;基于IMU模块采集的数据判断所述环境探测装置是 否直线移动;若是,基于所述实际偏移角度对所述雷达采集的方位角进行修正。
于本发明一实施例中,通过以下步骤获取所述预设参数:
获取所述IMU模块在静止状态下的预设数量组x,y,z三轴测量值;
在所述预设数量组三轴测量值中任意选取八组三轴测量值,并基于最小二乘法求解方程 组以得到X,其中r,s,t为所述IMU模块的实际三轴,or,os, ot分别为r,s,t轴固定偏差,kr,ks,kt分别为r,s,t轴比例因子,β为t轴和z轴的夹角, α为t轴和z轴构成的平面到x轴的夹角,
获取预设个数的X后,计算选取sum最小时对应的X作为所述预 设参数,m为所述预设数量。
于本发明一实施例中,基于IMU模块采集的数据判断所述环境探测装置是否静止包括以 下步骤:
计算所述IMU模块的加速度幅值其中,AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度;
计算所述IMU模块的角速度幅值其中ωX、ωY、ωZ分别为所述 IMU模块在X、Y、Z轴的角速度;
计算一定滑动窗口内时刻j到时刻i之间加速度的方差σn;
设定和当C1、C2和 C3的取值均为1时,判定所述环境探测装置处于静止状态,否则判定所述环境探测装置处于 非静止状态;其中,Aj是j时刻的加速度值,是时刻j到时刻i内 加速度的均值,n为窗口内数据的个数,Athl、Athh分别为加速度幅值下限值和上限值,ωth为 角速度幅值阈值,σth为加速度方差阈值。
于本发明一实施例中,基于机器学习算法获取所述加速度幅值下限值和上限值、所述角 速度幅值阈值和所述加速度方差阈值。
于本发明一实施例中,对所述IMU模块的航向角进行倾斜补偿包括以下步骤:
计算所述IMU模块的俯仰角和翻滚角其中AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度;
基于所述俯仰角和所述翻滚角计算所述IMU模块在X、Y轴补偿后的磁感应强度其中HX、HY和HZ分别为所述IMU模块在X、Y、Z轴的磁感应强度;
计算倾斜补偿后的航向角
于本发明一实施例中,基于IMU模块采集的数据判断所述环境探测装置是否直线移动包 括以下步骤:
计算前一时刻和当前时刻所述IMU模块倾斜补偿后的航向角差值
计算前一时刻到当前时刻的时间段内所述IMU模块所有绕Z轴角速度ω1,ω1…ωn中大 于设定的阈值角速度ωth的个数占总个数n的百分比N;
设定当C4和C5的取值均为1时,判 定所述环境探测装置进行直线移动,否则判定所述环境探测装置进行非直线移动;其中,Q 为预设阈值,P为预设百分比。
于本发明一实施例中,基于机器学习算法获取所述预设阈值和所述预设百分比。
于本发明一实施例中,对所述IMU模块的航向角进行误差修正,以获取所述IMU模块的 实际偏移角度包括以下步骤:
计算倾斜补偿后的航向角在相邻两个时刻的差值
基于所述IMU模块的x轴或y轴角速度计算所述IMU模块的角度
计算所述实际偏移角度其中h为动态权值。
于本发明一实施例中,修正后的所述雷达采集的方向角为其中,r为障碍物距离所述环境检测装置的距离,α为所述雷达采集的方向角,vox和voy为所述雷达采集的障碍物相对于所述环境检测装置的X和Y方向的速度,为所述实际偏移角度,Δt为时间间隔。
对应地,本发明提供一种环境探测装置的修正***,应用于环境探测装置上,所述环境 探测装置包括雷达和IMU模块,且所述IMU模块的Z轴纵向设置;
所述环境探测装置的修正***包括校准模块、补偿模块、获取模块和修正模块;
所述校准模块用于基于预设参数对IMU模块进行初始校准;
所述补偿模块用于基于IMU模块采集的数据判断所述环境探测装置是否静止,并在所述 环境探测装置处于非静止状态时,对所述IMU模块的航向角进行倾斜补偿;
所述获取模块用于对所述IMU模块的航向角进行误差修正,以获取所述IMU模块的实际 偏移角度;
所述修正模块用于基于IMU模块采集的数据判断所述环境探测装置是否直线移动;若是, 基于所述实际偏移角度对所述雷达采集的方位角进行修正。
本发明提供一种存储介质,其上存储有计算机程序,该程序被处理器执行时实现上述的 环境探测装置的修正方法。
本发明提供一种控制终端,包括:处理器及存储器;
所述存储器用于存储计算机程序;
所述处理器用于执行所述存储器存储的计算机程序,以使所述控制终端执行上述的环境 探测装置的修正方法。
本发明提供一种环境探测装置,包括上述的控制终端、雷达和IMU模块;
所述IMU模块的Z轴纵向设置,与所述控制终端相连,用于采集所述环境探测装置的角 速度、加速度和磁感应强度,并发送至所述控制终端;
所述雷达与所述控制终端相连,用于基于所述控制终端修正后的方位角进行障碍物探测。
于本发明一实施例中,所述雷达采用毫米波雷达或厘米波雷达。
最后,本发明提供一种便携设备,包括上述的环境探测装置。
于本发明一实施例中,所述便携设备包括:腰带、眼镜、拐杖、服装、佩饰、及***物中的一或多种组合。
于本发明一实施例中,所述便携设备为导盲设备。
如上所述,本发明所述的环境探测装置及其修正方法、***、便携设备及存储介质,具 有以下有益效果:
(1)基于IMU进行运动状态检测,并基于运动状态修正雷达采集的数据,从而有效地 减少误报;
(2)在不需要机械电控手段的情况下就能达成现有技术的动态调整雷达的需求,降低成 本和设备设计难度。
附图说明
图1显示为本发明的环境探测装置的修正方法于一实施例中的流程图;
图2显示为三轴传感器的非正交误差模型示意图;
图3显示为直线移动状态下所述环境探测装置发生小幅度偏转于一实施例中的示意图;、
图4显示为前后时刻障碍物的方位于一实施例中的示意图;
图5显示为本发明的环境探测装置的修正***于一实施例中的结构示意图;
图6显示为本发明的控制终端于一实施例中的结构示意图;
图7显示为本发明的环境探测装置于一实施例中的结构示意图;
图8显示为本发明的便携装置于一实施例中的结构示意图。
元件标号说明
51 校准模块
52 补偿模块
53 获取模块
54 修正模块
61 处理器
62 存储器
71 控制终端
72 雷达
73 IMU模块
具体实施方式
以下通过特定的具体实例说明本发明的实施方式,本领域技术人员可由本说明书所揭露 的内容轻易地了解本发明的其他优点与功效。本发明还可以通过另外不同的具体实施方式加 以实施或应用,本说明书中的各项细节也可以基于不同观点与应用,在没有背离本发明的精 神下进行各种修饰或改变。需说明的是,在不冲突的情况下,以下实施例及实施例中的特征 可以相互组合。
需要说明的是,以下实施例中所提供的图示仅以示意方式说明本发明的基本构想,遂图 式中仅显示与本发明中有关的组件而非按照实际实施时的组件数目、形状及尺寸绘制,其实 际实施时各组件的型态、数量及比例可为一种随意的改变,且其组件布局型态也可能更为复 杂。
本发明的环境探测装置及其修正方法、***、便携设备及存储介质基于IMU进行运动状 态检测,并基于运动状态修正雷达采集的数据,防止由于雷达偏转导致的雷达方位角的偏移, 从而准确地进行前方障碍物检测,有效地减少误报。
如图1所示,于一实施例中,本发明的环境探测装置的修正方法应用于环境探测装置上, 所述环境探测装置包括雷达和IMU模块。所述IMU模块的Z轴纵向设置,且X、Y、Z方向分别指向0°方向、90°方向(正前方)和重力反方向。当所述雷达模块在水平方向上发生 偏移时,基于所述IMU模块获取实际偏移角度,从而实现对所述雷达采集的方位角的修正。 其中,雷达可以获取障碍物的XYZ坐标轴、方向角和距离。IMU模块包含陀螺仪、加速度 计和磁力计,传输回来的数据分别代表3个轴测量的角速度、加速度和磁感应强度,可以得 到俯仰角roll(x轴与地面的角度)、翻滚角pitch(Y轴与地面的角度)和航向角yaw(绕z轴 旋转的角度)等。优选地,所述雷达可以采用毫米波雷达或厘米波雷达。
具体地,所述环境探测装置的修正方法包括以下步骤:
步骤S1、基于预设参数对IMU模块进行初始校准。
具体地,在所述IMU模块进行使用前,首先需要进行初始校准,以保证后续其采集数据 的准确性。所述IMU模块的误差来主要来自于三部分,即噪声、尺度因子和轴偏差。在理想 状态下三轴传感器的三个感应轴(即x轴,y轴,z轴)是完全正交的、没有误差存在。在静 止状态下它们的测量值为(x,y,z),则x2+y2+z2=c2。其中,对于陀螺仪:c=7.292x10- 5rad/s (即角速度为地球自转角速率,根据不同IMU的精度,可以考虑忽略不计,即c=0);对于 加速度计:c=9.81m/s2(即地球引力加速度);对于磁力计:c=0.5Gauss(即地磁场强度)。在 实际使用中,由于固定偏差和比例误差的存在,则(kr(r-or))2+(ks(s-os))2+ (kt(t-ot))2=c2。其中,r,s,t为所述IMU模块的实际三轴(即r轴,s轴,t轴)下的测 量值,or,os,ot分别为r,s,t轴固定偏差,kr,ks,kt分别为r,s,t轴比例因子,r,s, t为传感器的三轴测量值。因为同一个芯片双轴正交误差很小,三轴传感器的r、s可以认为 是正交的,故非正交误差模型如图2所示。其中β为t轴和z轴的夹角。α为t轴和z轴构成 的平面到x轴的夹角。需要说明的是,磁力计的z轴与加速度计、陀螺仪相反。
r、s、t轴对应的单位向量为令该空间中任意一个向 量在r、s、t轴上的投影为且则可得 (r,s,t)和(x,y,z)的关系如下:
结合三轴传感器的特性公式x2+y2+z2=c2和误差来源,需要求解 or,os,ot,kr,ks,kt,α,β这8个未知量,即求解方程组即可 获取初始校准所述IMU模块的参数。
于本发明一实施例中,通过以下步骤获取所述预设参数:
11)获取所述IMU模块在静止状态下的预设数量组三轴测量值。其中,将IMU模块静止 地放置于不同的位置,每次采集一定数量组,如100组数据,对采集的数据求均值,得到的 值为一组(r、s、t)数据。重复上述步骤,知道得到预设数量m组测量值。
12)在所述预设数量组三轴测量值中任意选取八组三轴测量值,即陀螺仪的三轴测量值、 加速度计的三轴测量值或磁力计的三轴测量值;并基于最小二乘法求解方程组以得到一个X。
13)获取预设个数的X后,计算选取sum最小时对应的X作为所述预设参数,m为所述预设数量。具体地,多次重复上述步骤11)和12),获得预设个数的 X,将所述X带入sum最小时对应的X选取为所述IMU模块的预设参 数,从而对所述IMU模块进行初始校准。
因此,基于上述算法分别获取陀螺仪、加速度计和磁力计的预设参数,并分别对陀螺仪、 加速度计和磁力计进行初始校准。
步骤S2、基于IMU模块采集的数据判断所述环境探测装置是否静止,并在所述环境探测 装置处于非静止状态时,对所述IMU模块的航向角进行倾斜补偿。
具体地,所述环境探测装置通过具有以下三种运动状态:
1)静止状态
2)直线移动状态
3)非直线移动状态,方向发生较大偏转
在本发明中,通过所述IMU模块的加速度和角速度来判断所述环境探测装置是否处于静 止状态。于本发明一实施例中,基于IMU模块采集的数据判断所述环境探测装置是否静止包 括以下步骤:
a)计算所述IMU模块的加速度计采集的加速度幅值其中,AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度。
b)计算所述IMU模块的陀螺仪采集的角速度幅值其中ωX、ωY、 ωZ分别为所述IMU模块在X、Y、Z轴的角速度。
c)选取一定滑动窗口,计算所述滑动窗口内时刻j到时刻i之间加速度的方差σn。
d)设定和当C1、 C2和C3的取值均为1时,判定所述环境探测装置处于静止状态,否则判定所述环境探测装置 处于非静止状态;其中,Aj是j时刻的加速度值,是时刻j到时刻 i内加速度的均值,n为窗口内数据的个数,Athl、Athh分别为加速度幅值下限值和上限值,ωth为 角速度幅值阈值,σth为加速度方差阈值。
在本发明中,记录所述环境探测装置的运动数据,使用机器学***面。具体地,记录所述IMU模块的加速度幅值、角速度幅值和加速度方差, 分别对这三项进行训练,得出对应的阈值。
具体地,航向角可以通过电磁感应强度计算得出。由于X、Y两轴的磁感应强度合成后 总指向地磁北极,故可通过测量敏感轴与地磁北极的夹角来实现航向角的检测,即当磁力计不在水平位置的时候,可对所述IMU模块的航向角进行倾斜补 偿,减小航向角检测的误差。于本发明一实施例中,对所述IMU模块的航向角进行倾斜补偿 包括以下步骤:
a)计算所述IMU模块的俯仰角和翻滚角其中 AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度;
b)基于所述俯仰角和所述翻滚角计算所述IMU模块在X、Y轴补偿后的磁感应强度其中HX、HY和HZ分别为所述IMU模块在X、Y、Z轴的磁感应强度;
c)计算倾斜补偿后的航向角
步骤S3、对所述IMU模块的航向角进行误差修正,以获取所述IMU模块的实际偏移角 度。
具体地,IMU的***主要还会存在两种误差,即积分误差和漂移误差。积分误差主要存 在于位置估算中得到导航坐标系的位移加速度进行的积分运算中,而积分运算与最终的位移 距离具有直接联系,积分误差会随时间不断累积,因此会影响位置估算结果。由于位置对雷 达的数据修正没有直接影响,为了简化起见,在本发明中可以不进行修正。航向漂移误差主 要因为大多室内环境磁场环境混乱,同时铁制品的靠近对磁力计的靠近具有很大的影响,而 且由于陀螺仪传感器器件本身存在静态漂移误差和动态漂移误差,导致航向角的解算不准确, 航向发生漂移形成航向漂移误差。
在本发明中,结合磁力计和陀螺仪进行航向角差值的修正。于本发明一实施例中,对所 述IMU模块的航向角进行误差修正,以获取所述IMU模块的实际偏移角度包括以下步骤:
a)计算倾斜补偿后的航向角在相邻两个时刻的差值
b)基于陀螺仪采集的所述IMU模块的x轴或y轴角速度计算所述IMU模块的角度其中对x轴或y轴角速度进行积分,得到所述角度
c)计算所述实际偏移角度其中h为动态权值,通过机器 学习或实验数据拟合获取。
步骤S4、基于IMU模块采集的数据判断所述环境探测装置是否直线移动;若是,基于所 述实际偏移角度对所述雷达采集的方位角进行修正。
具体地,当所述IMU模块进行直线移动时,雷达需要探测前方障碍物,故需要基于所述 实际偏移角度对所述雷达采集的方位角进行修正。
在直行过程中,所述环境探测装置的航向角变化较小,故可以利用前后时刻的航向差值 及角速度进行直行判定。于本发明一实施例中,基于IMU模块采集的数据判断所述环境探测 装置是否直线移动包括以下步骤:
a)计算前一时刻和当前时刻所述IMU模块倾斜补偿后的航向角差值
b)计算前一时刻到当前时刻时间段内所述IMU模块所有绕Z轴角速度ω1,ω1…ωn中大 于设定的阈值角速度ωth的个数占总个数n的百分比N;
c)设定当C4和C5的取值均为1时, 判定所述环境探测装置进行直线移动,否则判定所述环境探测装置进行非直线移动;其中, Q为预设阈值,P为预设百分比。
在本发明中,记录所述环境探测装置的运动数据,使用机器学***面。具体地,记录所述IMU模块的航向角差值、绕Z轴角 速度和超过角速度阈值的个数,分别对这三项进行训练,得出对应的阈值。需要说明的是, 首先需要确定角速度阈值,再得出所述预设百分比。
在直行情况下,行人的实际行走方向为沿着y轴正方向,但由于走路影响,所述环境探 测装置的二维坐标轴可能会发生小幅度偏转为x’轴和y’轴。雷达获取的方位角为α,障碍 物到所述环境探测装置的距离为r,则该时间段内且经过修正后的航向差值为通过雷达 获取障碍物相对于所述环境探测装置的速度为vo,如图3所示。故前一时刻的实际方位角为 由于障碍物可能在移动,取很短的时间间隔Δt,障碍物到所述环境探测装置的 距离近似不变为r,如图4所示。故修正后的所述雷达采集的方向角为其中,和voy为所述雷达采集的障碍物相对于所述环境检测装置的X和Y方向的速度。需要说明的是,Δt的大小为预设的,是根据实际情况设定一个很短的时间间隔。
如图5所示,于一实施例中,本发明的环境探测装置的修正***应用于环境探测装置上, 所述环境探测装置包括雷达和IMU模块,且所述IMU模块的Z轴纵向设置;所述环境探测 装置的修正***包括校准模块51、补偿模块52、获取模块53和修正模块54。
所述校准模块51用于基于预设参数对IMU模块进行初始校准。
所述补偿模块52与所述校准模块51相连,用于基于IMU模块采集的数据判断所述环境 探测装置是否静止,并在所述环境探测装置处于非静止状态时,对所述IMU模块的航向角进 行倾斜补偿。
所述获取模块53与所述补偿模块53相连,用于对所述IMU模块的航向角进行误差修正, 以获取所述IMU模块的实际偏移角度。
所述修正模块54与所述获取模块53相连,用于基于IMU模块采集的数据判断所述环境 探测装置是否直线移动;若是,基于所述实际偏移角度对所述雷达采集的方位角进行修正。
需要说明的是,上述校准模块51、补偿模块52、获取模块53和修正模块54的结构和原 理与上述环境探测装置的修正方法中的步骤一一对应,故在此不再赘述。
需要说明的是,应理解以上装置的各个模块的划分仅仅是一种逻辑功能的划分,实际实 现时可以全部或部分集成到一个物理实体上,也可以物理上分开。且这些模块可以全部以软 件通过处理元件调用的形式实现,也可以全部以硬件的形式实现,还可以部分模块通过处理 元件调用软件的形式实现,部分模块通过硬件的形式实现。例如:x模块可以为单独设立的 处理元件,也可以集成在上述装置的某一个芯片中实现。此外,x模块也可以以程序代码的 形式存储于上述装置的存储器中,由上述装置的某一个处理元件调用并执行以上x模块的功 能。其它模块的实现与之类似。这些模块全部或部分可以集成在一起,也可以独立实现。这 里所述的处理元件可以是一种集成电路,具有信号的处理能力。在实现过程中,上述方法的 各步骤或以上各个模块可以通过处理器元件中的硬件的集成逻辑电路或者软件形式的指令完 成。以上这些模块可以是被配置成实施以上方法的一个或多个集成电路,例如:一个或多个 特定集成电路(ApplicationSpecificIntegratedCircuit,简称ASIC),一个或多个微处理器(Digital Singnal Processor,简称DSP),一个或者多个现场可编程门阵列(FieldProgrammableGateArray, 简称FPGA)等。当以上某个模块通过处理元件调度程序代码的形式实现时,该处理元件可 以是通用处理器,如中央处理器(CentralProcessingUnit,简称CPU)或其它可以调用程序代 码的处理器。这些模块可以集成在一起,以片上***(System-on-a-chip,简称SOC)的形式 实现。
本发明的存储介质上存储有计算机程序,该程序被处理器执行时实现上述的环境探测装 置的修正方法。所述存储介质包括:ROM、RAM、磁碟、U盘、存储卡或者光盘等各种可以 存储程序代码的介质。
如图6所示,于一实施例中,本发明的控制终端包括:处理器61及存储器62。
所述存储器62用于存储计算机程序。
所述存储器62包括:ROM、RAM、磁碟、U盘、存储卡或者光盘等各种可以存储程序 代码的介质。
所述处理器61与所述存储器62相连,用于执行所述存储器存储的计算机程序,以使所 述控制终端执行上述的环境探测装置的修正方法。
优选地,所述处理器61可以是通用处理器,包括中央处理器(Central ProcessingUnit,简 称CPU)、网络处理器(Network Processor,简称NP)等;还可以是数字信号处理器(Digital Signal Processor,简称DSP)、专用集成电路(Application SpecificIntegrated Circuit,简称ASIC)、现 场可编程门阵列(Field Programmable Gate Array,简称FPGA)或者其他可编程逻辑器件、分立 门或者晶体管逻辑器件、分立硬件组件。
如图7所示,于一实施例中,本发明的环境探测装置包括上述的控制终端71、雷达72和 IMU模块73。
所述IMU模块73的Z轴纵向设置,与所述控制终端71相连,用于采集所述环境探测装 置的角速度、加速度和磁感应强度,并发送至所述控制终端71。
其中,所述IMU模73块包含陀螺仪、加速度计和磁力计,传输回来的数据分别代表3个轴测量的角速度、加速度和磁感应强度,可以得到俯仰角roll(x轴与地面的角度)、翻滚角pitch(Y轴与地面的角度)和航向角yaw(绕z轴旋转的角度)等
所述雷达72与所述控制终端71相连,用于基于所述控制终端71修正后的方位角进行障 碍物探测。
于本发明一实施例中,所述雷达72采用毫米波雷达或厘米波雷达。
如图8所示,于一实施例中,本发明的便携设备包括上述的环境探测装置。
于本发明一实施例中,所述便携设备包括:腰带、眼镜、拐杖、服装、佩饰、及***物中的一或多种组合。
于本发明一实施例中,所述便携设备为导盲设备。当所述便携设备为导盲设备时,可设 置于盲人腰间。由于盲人行走带来的雷达在水平面上的偏移,导致雷达对障碍物的探测出现 偏差。此时基于IMU模块采集的实际偏移角度校正所述雷达采集的方位角,从而保证雷达对 前方障碍物的准确探测。
综上所述,本发明的环境探测装置及其修正方法、***、便携设备及存储介质基于IMU 进行运动状态检测,并基于运动状态修正雷达采集的数据,从而有效地减少误报;在不需要 机械电控手段的情况下就能达成现有技术的动态调整雷达的需求,降低成本和设备设计难度。 因此,本发明有效克服了现有技术中的种种缺点而具高度产业利用价值。
上述实施例仅例示性说明本发明的原理及其功效,而非用于限制本发明。任何熟悉此技 术的人士皆可在不违背本发明的精神及范畴下,对上述实施例进行修饰或改变。因此,举凡 所属技术领域中具有通常知识者在未脱离本发明所揭示的精神与技术思想下所完成的一切等 效修饰或改变,仍应由本发明的权利要求所涵盖。
Claims (17)
1.一种环境探测装置的修正方法,其特征在于,应用于环境探测装置上,所述环境探测装置包括雷达和IMU模块,且所述IMU模块的Z轴纵向设置;
所述环境探测装置的修正方法包括以下步骤:
基于预设参数对IMU模块进行初始校准;
基于IMU模块采集的数据判断所述环境探测装置是否静止,并在所述环境探测装置处于非静止状态时,对所述IMU模块的航向角进行倾斜补偿;
对所述IMU模块的航向角进行误差修正,以获取所述IMU模块的实际偏移角度;
基于IMU模块采集的数据判断所述环境探测装置是否直线移动;若是,基于所述实际偏移角度对所述雷达采集的方位角进行修正。
2.根据权利要求1所述的环境探测装置的修正方法,其特征在于,通过以下步骤获取所述预设参数:
获取所述IMU模块在静止状态下的预设数量组x,y,z三轴测量值;
在所述预设数量组三轴测量值中任意选取八组三轴测量值,并基于最小二乘法求解方程组以得到X,其中r,s,t为所述IMU模块的实际三轴,or,os,ot分别为r,s,t轴固定偏差,kr,ks,kt分别为r,s,t轴比例因子,β为t轴和z轴的夹角,α为t轴和z轴构成的平面到x轴的夹角,
获取预设个数的X后,计算选取sum最小时对应的X作为所述预设参数,m为所述预设数量。
3.根据权利要求1所述的环境探测装置的修正方法,其特征在于,基于IMU模块采集的数据判断所述环境探测装置是否静止包括以下步骤:
计算所述IMU模块的加速度幅值其中,AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度;
计算所述IMU模块的角速度幅值其中ωX、ωY、ωZ分别为所述IMU模块在X、Y、Z轴的角速度;
计算一定滑动窗口内时刻j到时刻i之间加速度的方差σn;
设定和当C1、C2和C3的取值均为1时,判定所述环境探测装置处于静止状态,否则判定所述环境探测装置处于非静止状态;其中,Aj是j时刻的加速度值,是时刻j到时刻i内加速度的均值,n为窗口内数据的个数,Athl、Athh分别为加速度幅值下限值和上限值,ωth为角速度幅值阈值,σth为加速度方差阈值。
4.根据权利要求3所述的环境探测装置的修正方法,其特征在于,基于机器学习算法获取所述加速度幅值下限值和上限值、所述角速度幅值阈值和所述加速度方差阈值。
5.根据权利要求1所述的环境探测装置的修正方法,其特征在于,对所述IMU模块的航向角进行倾斜补偿包括以下步骤:
计算所述IMU模块的俯仰角和翻滚角其中AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度;
基于所述俯仰角和所述翻滚角计算所述IMU模块在X、Y轴补偿后的磁感应强度其中HX、HY和HZ分别为所述IMU模块在X、Y、Z轴的磁感应强度;
计算倾斜补偿后的航向角
6.根据权利要求1所述的环境探测装置的修正方法,其特征在于,基于IMU模块采集的数据判断所述环境探测装置是否直线移动包括以下步骤:
计算前一时刻和当前时刻所述IMU模块倾斜补偿后的航向角差值
计算前一时刻到当前时刻的时间段内所述IMU模块所有绕Z轴角速度ω1,ω1…ωn中大于设定的阈值角速度ωth的个数占总个数n的百分比N;
设定当C4和C5的取值均为1时,判定所述环境探测装置进行直线移动,否则判定所述环境探测装置进行非直线移动;其中,Q为预设阈值,P为预设百分比。
7.根据权利要求6所述的环境探测装置的修正方法,其特征在于,基于机器学习算法获取所述预设阈值和所述预设百分比。
8.根据权利要求1所述的环境探测装置的修正方法,其特征在于,对所述IMU模块的航向角进行误差修正,以获取所述IMU模块的实际偏移角度包括以下步骤:
计算倾斜补偿后的航向角在相邻两个时刻的差值
基于所述IMU模块的x轴或y轴角速度计算所述IMU模块的角度
计算所述实际偏移角度其中h为动态权值。
9.根据权利要求1所述的环境探测装置的修正方法,其特征在于,修正后的所述雷达采集的方向角为其中,r为障碍物距离所述环境检测装置的距离,α为所述雷达采集的方向角,vox和voy为所述雷达采集的障碍物相对于所述环境检测装置的X和Y方向的速度,为所述实际偏移角度,Δt为时间间隔。
10.一种环境探测装置的修正***,其特征在于,应用于环境探测装置上,所述环境探测装置包括雷达和IMU模块,且所述IMU模块的Z轴纵向设置;
所述环境探测装置的修正***包括校准模块、补偿模块、获取模块和修正模块;
所述校准模块用于基于预设参数对IMU模块进行初始校准;
所述补偿模块用于基于IMU模块采集的数据判断所述环境探测装置是否静止,并在所述环境探测装置处于非静止状态时,对所述IMU模块的航向角进行倾斜补偿;
所述获取模块用于对所述IMU模块的航向角进行误差修正,以获取所述IMU模块的实际偏移角度;
所述修正模块用于基于IMU模块采集的数据判断所述环境探测装置是否直线移动;若是,基于所述实际偏移角度对所述雷达采集的方位角进行修正。
11.一种存储介质,其上存储有计算机程序,其特征在于,该程序被处理器执行时实现权利要求1至9中任一项所述的环境探测装置的修正方法。
12.一种控制终端,其特征在于,包括:处理器及存储器;
所述存储器用于存储计算机程序;
所述处理器用于执行所述存储器存储的计算机程序,以使所述控制终端执行权利要求1至9中任一项所述的环境探测装置的修正方法。
13.一种环境探测装置,其特征在于,包括权利要求12所述的控制终端、雷达和IMU模块;
所述IMU模块的Z轴纵向设置,与所述控制终端相连,用于采集所述环境探测装置的角速度、加速度和磁感应强度,并发送至所述控制终端;
所述雷达与所述控制终端相连,用于基于所述控制终端修正后的方位角进行障碍物探测。
14.根据权利要求13所述的环境探测装置,其特征在于,所述雷达采用毫米波雷达或厘米波雷达。
15.一种便携设备,其特征在于,包括权利要求13或14所述的环境探测装置。
16.根据权利要求15所述的便携设备,其特征在于,所述便携设备包括:腰带、眼镜、拐杖、服装、佩饰、及***物中的一或多种组合。
17.根据权利要求15所述的便携设备,其特征在于,所述便携设备为导盲设备。
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910441487.XA CN110221302B (zh) | 2019-05-24 | 2019-05-24 | 环境探测装置及其修正方法、***、便携设备及存储介质 |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201910441487.XA CN110221302B (zh) | 2019-05-24 | 2019-05-24 | 环境探测装置及其修正方法、***、便携设备及存储介质 |
Publications (2)
Publication Number | Publication Date |
---|---|
CN110221302A true CN110221302A (zh) | 2019-09-10 |
CN110221302B CN110221302B (zh) | 2023-04-18 |
Family
ID=67818356
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201910441487.XA Active CN110221302B (zh) | 2019-05-24 | 2019-05-24 | 环境探测装置及其修正方法、***、便携设备及存储介质 |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN110221302B (zh) |
Cited By (8)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110736968A (zh) * | 2019-10-16 | 2020-01-31 | 清华大学 | 基于深度学习的雷达异常状态诊断方法 |
CN111623799A (zh) * | 2020-05-21 | 2020-09-04 | 北京嘀嘀无限科技发展有限公司 | 终端的状态检测方法、装置、终端和存储介质 |
CN111665498A (zh) * | 2020-05-20 | 2020-09-15 | 浙江大华技术股份有限公司 | 环境视图的测绘方法、装置、便携式探测设备 |
CN111780746A (zh) * | 2020-03-27 | 2020-10-16 | 宁波小遛共享信息科技有限公司 | 方向角的检测方法、装置、电子设备和代步工具 |
CN111928844A (zh) * | 2020-06-11 | 2020-11-13 | 嘉兴市纳杰微电子技术有限公司 | 一种一般mems陀螺在agv应用上的模型*** |
CN112051844A (zh) * | 2020-08-17 | 2020-12-08 | 尚科宁家(中国)科技有限公司 | 自移动机器人及其控制方法 |
CN112797979A (zh) * | 2020-12-31 | 2021-05-14 | 苏州精源创智能科技有限公司 | 一种应用于agv的惯性姿态导航*** |
CN113790738A (zh) * | 2021-08-13 | 2021-12-14 | 上海智能网联汽车技术中心有限公司 | 一种基于智能云台imu的数据补偿方法 |
Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101344391A (zh) * | 2008-07-18 | 2009-01-14 | 北京工业大学 | 基于全功能太阳罗盘的月球车位姿自主确定方法 |
WO2013131471A1 (zh) * | 2012-03-06 | 2013-09-12 | 武汉大学 | 一种惯性测量单元的快速标定方法 |
CN103968827A (zh) * | 2014-04-09 | 2014-08-06 | 北京信息科技大学 | 一种可穿戴式人体步态检测的自主定位方法 |
US9304198B1 (en) * | 2013-10-04 | 2016-04-05 | Sandia Corporation | Navigator alignment using radar scan |
CN109099913A (zh) * | 2018-10-10 | 2018-12-28 | 格物感知(深圳)科技有限公司 | 一种基于mems惯性器件的穿戴式导航装置和方法 |
-
2019
- 2019-05-24 CN CN201910441487.XA patent/CN110221302B/zh active Active
Patent Citations (5)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN101344391A (zh) * | 2008-07-18 | 2009-01-14 | 北京工业大学 | 基于全功能太阳罗盘的月球车位姿自主确定方法 |
WO2013131471A1 (zh) * | 2012-03-06 | 2013-09-12 | 武汉大学 | 一种惯性测量单元的快速标定方法 |
US9304198B1 (en) * | 2013-10-04 | 2016-04-05 | Sandia Corporation | Navigator alignment using radar scan |
CN103968827A (zh) * | 2014-04-09 | 2014-08-06 | 北京信息科技大学 | 一种可穿戴式人体步态检测的自主定位方法 |
CN109099913A (zh) * | 2018-10-10 | 2018-12-28 | 格物感知(深圳)科技有限公司 | 一种基于mems惯性器件的穿戴式导航装置和方法 |
Cited By (11)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN110736968A (zh) * | 2019-10-16 | 2020-01-31 | 清华大学 | 基于深度学习的雷达异常状态诊断方法 |
CN110736968B (zh) * | 2019-10-16 | 2021-10-08 | 清华大学 | 基于深度学习的雷达异常状态诊断方法 |
CN111780746A (zh) * | 2020-03-27 | 2020-10-16 | 宁波小遛共享信息科技有限公司 | 方向角的检测方法、装置、电子设备和代步工具 |
CN111665498A (zh) * | 2020-05-20 | 2020-09-15 | 浙江大华技术股份有限公司 | 环境视图的测绘方法、装置、便携式探测设备 |
CN111623799A (zh) * | 2020-05-21 | 2020-09-04 | 北京嘀嘀无限科技发展有限公司 | 终端的状态检测方法、装置、终端和存储介质 |
CN111928844A (zh) * | 2020-06-11 | 2020-11-13 | 嘉兴市纳杰微电子技术有限公司 | 一种一般mems陀螺在agv应用上的模型*** |
CN111928844B (zh) * | 2020-06-11 | 2023-11-03 | 嘉兴市纳杰微电子技术有限公司 | 一种mems陀螺在agv应用上的模型*** |
CN112051844A (zh) * | 2020-08-17 | 2020-12-08 | 尚科宁家(中国)科技有限公司 | 自移动机器人及其控制方法 |
CN112051844B (zh) * | 2020-08-17 | 2023-08-04 | 尚科宁家(中国)科技有限公司 | 自移动机器人及其控制方法 |
CN112797979A (zh) * | 2020-12-31 | 2021-05-14 | 苏州精源创智能科技有限公司 | 一种应用于agv的惯性姿态导航*** |
CN113790738A (zh) * | 2021-08-13 | 2021-12-14 | 上海智能网联汽车技术中心有限公司 | 一种基于智能云台imu的数据补偿方法 |
Also Published As
Publication number | Publication date |
---|---|
CN110221302B (zh) | 2023-04-18 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN110221302A (zh) | 环境探测装置及其修正方法、***、便携设备及存储介质 | |
US10337884B2 (en) | Method and apparatus for fast magnetometer calibration | |
CN104718561B (zh) | 基于消失点确定的传感器校准和位置估计 | |
EP2946167B1 (en) | Method and apparatus for determination of misalignment between device and pedestrian | |
US20130249784A1 (en) | Method and Device for Pose Tracking Using Vector Magnetometers | |
CN104296776A (zh) | 用于磁力计校准和补偿的***和方法 | |
CN107421523A (zh) | 方位角校准方法、装置、存储介质和计算机设备 | |
JPWO2014010727A1 (ja) | 移動体の進行方向推定装置及び進行方向推定方法 | |
CN106643792A (zh) | 惯性测量单元和地磁传感器整体标定装置及标定方法 | |
Shockley | Ground vehicle navigation using magnetic field variation | |
CN102997935B (zh) | 一种基于光学和惯性组合测量的自主gnc仿真试验*** | |
CN110864692A (zh) | 掘进机的位姿确定方法 | |
US20140249750A1 (en) | Navigational and location determination system | |
CN103105166B (zh) | 一种运动练习拍的运动数据处理方法及*** | |
Tao et al. | Precise displacement estimation from time-differenced carrier phase to improve PDR performance | |
EP4269044A1 (en) | Slope location correction method and apparatus, robot and readable storage medium | |
Saini et al. | Implementation of Multi-Sensor GPS/IMU Integration Using Kalman Filter for Autonomous Vehicle | |
RU2629539C1 (ru) | Способ измерения магнитного курса подвижного объекта | |
KR20170092356A (ko) | 3축 지자기 센서의 방위각 보정 시스템 | |
RU2653967C1 (ru) | Способ автономной ориентации подвижных объектов | |
Wang et al. | Testbed development for a novel approach towards high accuracy indoor localization with smartphones | |
Liu et al. | Optical flow measurement of human walking | |
Trojnacki et al. | Determination of motion parameters with inertial measurement units–Part 1: mathematical formulation of the algorithm | |
RU2010132599A (ru) | Способ определения ускорения силы тяжести на движущемся объекте и устройство для его осуществления | |
Ostroumov | Interpolation of magnetic field characteristics by spline functions |
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 |