CN110221302B - 环境探测装置及其修正方法、系统、便携设备及存储介质 - Google Patents

环境探测装置及其修正方法、系统、便携设备及存储介质 Download PDF

Info

Publication number
CN110221302B
CN110221302B CN201910441487.XA CN201910441487A CN110221302B CN 110221302 B CN110221302 B CN 110221302B CN 201910441487 A CN201910441487 A CN 201910441487A CN 110221302 B CN110221302 B CN 110221302B
Authority
CN
China
Prior art keywords
imu module
environment detection
detection device
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.)
Active
Application number
CN201910441487.XA
Other languages
English (en)
Other versions
CN110221302A (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.)
Shanghai Gaozhi Communication Research Institute Co ltd
Shanghai Gaozhi Network Co ltd
Shanghai Gaozhi Special Vehicle Co ltd
Shanghai Gaozhi Science And Technology Development Co ltd
Shanghai Jiaotong University
Original Assignee
Shanghai Gaozhi Communication Research Institute Co ltd
Shanghai Gaozhi Network Co ltd
Shanghai Gaozhi Special Vehicle Co ltd
Shanghai Gaozhi Science And Technology Development Co ltd
Shanghai Jiaotong University
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 Shanghai Gaozhi Communication Research Institute Co ltd, Shanghai Gaozhi Network Co ltd, Shanghai Gaozhi Special Vehicle Co ltd, Shanghai Gaozhi Science And Technology Development Co ltd, Shanghai Jiaotong University filed Critical Shanghai Gaozhi Communication Research Institute Co ltd
Priority to CN201910441487.XA priority Critical patent/CN110221302B/zh
Publication of CN110221302A publication Critical patent/CN110221302A/zh
Application granted granted Critical
Publication of CN110221302B publication Critical patent/CN110221302B/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
    • 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三轴测量值；
在所述预设数量组三轴测量值中任意选取八组三轴测量值，并基于最小二乘法求解方程组
Figure BDA0002072149400000021
以得到X，其中r，s，t为所述IMU模块的实际三轴，or，os，ot分别为r，s，t轴固定偏差，kr，ks，kt分别为r，s，t轴比例因子，β为t轴和z轴的夹角，α为t轴和z轴构成的平面到x轴的夹角，
Figure BDA0002072149400000022
获取预设个数的X后，计算
Figure BDA0002072149400000023
选取sum最小时对应的X作为所述预设参数，m为所述预设数量。
于本发明一实施例中，基于IMU模块采集的数据判断所述环境探测装置是否静止包括以下步骤：
计算所述IMU模块的加速度幅值
Figure BDA0002072149400000024
其中，AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度；
计算所述IMU模块的角速度幅值
Figure BDA0002072149400000025
其中ωX、ωY、ωZ分别为所述IMU模块在X、Y、Z轴的角速度；
计算一定滑动窗口内时刻j到时刻i之间加速度的方差σn；
设定
Figure BDA0002072149400000026
和
Figure BDA0002072149400000027
当C1、C2和C3的取值均为1时，判定所述环境探测装置处于静止状态，否则判定所述环境探测装置处于非静止状态；其中，
Figure BDA0002072149400000028
Aj是j时刻的加速度值，
Figure BDA00020721494000000211
是时刻j到时刻i内加速度的均值，n为窗口内数据的个数，Athl、Athh分别为加速度幅值下限值和上限值，ωth为角速度幅值阈值，σth为加速度方差阈值。
于本发明一实施例中，基于机器学习算法获取所述加速度幅值下限值和上限值、所述角速度幅值阈值和所述加速度方差阈值。
于本发明一实施例中，对所述IMU模块的航向角进行倾斜补偿包括以下步骤：
计算所述IMU模块的俯仰角
Figure BDA0002072149400000029
和翻滚角
Figure BDA00020721494000000210
其中AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度；
基于所述俯仰角和所述翻滚角计算所述IMU模块在X、Y轴补偿后的磁感应强度
Figure BDA0002072149400000031
其中HX、HY和HZ分别为所述IMU模块在X、Y、Z轴的磁感应强度；
计算倾斜补偿后的航向角
Figure BDA0002072149400000032
于本发明一实施例中，基于IMU模块采集的数据判断所述环境探测装置是否直线移动包括以下步骤：
计算前一时刻和当前时刻所述IMU模块倾斜补偿后的航向角差值
Figure BDA0002072149400000033
计算前一时刻到当前时刻的时间段内所述IMU模块所有绕Z轴角速度ω1，ω1…ωn中大于设定的阈值角速度ωth的个数占总个数n的百分比N；
设定
Figure BDA0002072149400000034
当C4和C5的取值均为1时，判定所述环境探测装置进行直线移动，否则判定所述环境探测装置进行非直线移动；其中，Q为预设阈值，P为预设百分比。
于本发明一实施例中，基于机器学习算法获取所述预设阈值和所述预设百分比。
于本发明一实施例中，对所述IMU模块的航向角进行误差修正，以获取所述IMU模块的实际偏移角度包括以下步骤：
计算倾斜补偿后的航向角在相邻两个时刻的差值
Figure BDA0002072149400000035
基于所述IMU模块的x轴或y轴角速度计算所述IMU模块的角度
Figure BDA0002072149400000036
计算所述实际偏移角度
Figure BDA0002072149400000037
其中h为动态权值。
于本发明一实施例中，修正后的所述雷达采集的方向角为
Figure BDA0002072149400000038
其中，r为障碍物距离所述环境检测装置的距离，α为所述雷达采集的方向角，vox和voy为所述雷达采集的障碍物相对于所述环境检测装置的X和Y方向的速度，
Figure BDA0002072149400000039
为所述实际偏移角度，Δ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轴对应的单位向量为
Figure BDA0002072149400000061
令该空间中任意一个向量
Figure BDA0002072149400000062
在r、s、t轴上的投影为
Figure BDA0002072149400000063
且
Figure BDA0002072149400000064
则可得(r,s,t)和(x,y,z)的关系如下：
Figure BDA0002072149400000065
结合三轴传感器的特性公式x2+y2+z2＝c2和误差来源，需要求解or,os,ot,kr,ks,kt,α,β这8个未知量，即求解方程组
Figure BDA0002072149400000066
即可获取初始校准所述IMU模块的参数。
于本发明一实施例中，通过以下步骤获取所述预设参数：
11)获取所述IMU模块在静止状态下的预设数量组三轴测量值。其中，将IMU模块静止地放置于不同的位置，每次采集一定数量组，如100组数据，对采集的数据求均值，得到的值为一组(r、s、t)数据。重复上述步骤，知道得到预设数量m组测量值。
12)在所述预设数量组三轴测量值中任意选取八组三轴测量值，即陀螺仪的三轴测量值、加速度计的三轴测量值或磁力计的三轴测量值；并基于最小二乘法求解方程组
Figure BDA0002072149400000071
以得到一个X。
13)获取预设个数的X后，计算
Figure BDA0002072149400000072
选取sum最小时对应的X作为所述预设参数，m为所述预设数量。具体地，多次重复上述步骤11)和12)，获得预设个数的X，将所述X带入
Figure BDA0002072149400000073
sum最小时对应的X选取为所述IMU模块的预设参数，从而对所述IMU模块进行初始校准。
因此，基于上述算法分别获取陀螺仪、加速度计和磁力计的预设参数，并分别对陀螺仪、加速度计和磁力计进行初始校准。
步骤S2、基于IMU模块采集的数据判断所述环境探测装置是否静止，并在所述环境探测装置处于非静止状态时，对所述IMU模块的航向角进行倾斜补偿。
具体地，所述环境探测装置通过具有以下三种运动状态：
1)静止状态
2)直线移动状态
3)非直线移动状态，方向发生较大偏转
在本发明中，通过所述IMU模块的加速度和角速度来判断所述环境探测装置是否处于静止状态。于本发明一实施例中，基于IMU模块采集的数据判断所述环境探测装置是否静止包括以下步骤：
a)计算所述IMU模块的加速度计采集的加速度幅值
Figure BDA0002072149400000074
其中，AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度。
b)计算所述IMU模块的陀螺仪采集的角速度幅值
Figure BDA0002072149400000075
其中ωX、ωY、ωZ分别为所述IMU模块在X、Y、Z轴的角速度。
c)选取一定滑动窗口，计算所述滑动窗口内时刻j到时刻i之间加速度的方差σn。
d)设定
Figure BDA0002072149400000081
和
Figure BDA0002072149400000082
当C1、C2和C3的取值均为1时，判定所述环境探测装置处于静止状态，否则判定所述环境探测装置处于非静止状态；其中，
Figure BDA0002072149400000083
Aj是j时刻的加速度值，
Figure BDA0002072149400000084
是时刻j到时刻i内加速度的均值，n为窗口内数据的个数，Athl、Athh分别为加速度幅值下限值和上限值，ωth为角速度幅值阈值，σth为加速度方差阈值。
在本发明中，记录所述环境探测装置的运动数据，使用机器学习的方式对样本数据进行分析，选取所述加速度幅值下限值和上限值Athl、Athh、所述角速度幅值阈值ωth和所述加速度方差阈值σth。优选地，使用支持向量机SVM，将静止与行走、直行与非直行，看作线性的二分类问题。支持向量机SVM的基本想法是求解能够正确划分训练数据集并且几何间隔最大的分离超平面。具体地，记录所述IMU模块的加速度幅值、角速度幅值和加速度方差，分别对这三项进行训练，得出对应的阈值。
具体地，航向角可以通过电磁感应强度计算得出。由于X、Y两轴的磁感应强度合成后总指向地磁北极，故可通过测量敏感轴与地磁北极的夹角来实现航向角的检测，即
Figure BDA0002072149400000085
当磁力计不在水平位置的时候，可对所述IMU模块的航向角进行倾斜补偿，减小航向角检测的误差。于本发明一实施例中，对所述IMU模块的航向角进行倾斜补偿包括以下步骤：
a)计算所述IMU模块的俯仰角
Figure BDA0002072149400000086
和翻滚角
Figure BDA0002072149400000087
其中AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度；
b)基于所述俯仰角和所述翻滚角计算所述IMU模块在X、Y轴补偿后的磁感应强度
Figure BDA0002072149400000088
其中HX、HY和HZ分别为所述IMU模块在X、Y、Z轴的磁感应强度；
c)计算倾斜补偿后的航向角
Figure BDA0002072149400000089
步骤S3、对所述IMU模块的航向角进行误差修正，以获取所述IMU模块的实际偏移角度。
具体地，IMU的系统主要还会存在两种误差，即积分误差和漂移误差。积分误差主要存在于位置估算中得到导航坐标系的位移加速度进行的积分运算中，而积分运算与最终的位移距离具有直接联系，积分误差会随时间不断累积，因此会影响位置估算结果。由于位置对雷达的数据修正没有直接影响，为了简化起见，在本发明中可以不进行修正。航向漂移误差主要因为大多室内环境磁场环境混乱，同时铁制品的靠近对磁力计的靠近具有很大的影响，而且由于陀螺仪传感器器件本身存在静态漂移误差和动态漂移误差，导致航向角的解算不准确，航向发生漂移形成航向漂移误差。
在本发明中，结合磁力计和陀螺仪进行航向角差值的修正。于本发明一实施例中，对所述IMU模块的航向角进行误差修正，以获取所述IMU模块的实际偏移角度包括以下步骤：
a)计算倾斜补偿后的航向角在相邻两个时刻的差值
Figure BDA0002072149400000091
b)基于陀螺仪采集的所述IMU模块的x轴或y轴角速度计算所述IMU模块的角度
Figure BDA0002072149400000092
其中对x轴或y轴角速度进行积分，得到所述角度
Figure BDA0002072149400000093
c)计算所述实际偏移角度
Figure BDA0002072149400000094
其中h为动态权值，通过机器学习或实验数据拟合获取。
步骤S4、基于IMU模块采集的数据判断所述环境探测装置是否直线移动；若是，基于所述实际偏移角度对所述雷达采集的方位角进行修正。
具体地，当所述IMU模块进行直线移动时，雷达需要探测前方障碍物，故需要基于所述实际偏移角度对所述雷达采集的方位角进行修正。
在直行过程中，所述环境探测装置的航向角变化较小，故可以利用前后时刻的航向差值及角速度进行直行判定。于本发明一实施例中，基于IMU模块采集的数据判断所述环境探测装置是否直线移动包括以下步骤：
a)计算前一时刻和当前时刻所述IMU模块倾斜补偿后的航向角差值
Figure BDA0002072149400000095
b)计算前一时刻到当前时刻时间段内所述IMU模块所有绕Z轴角速度ω1，ω1…ωn中大于设定的阈值角速度ωth的个数占总个数n的百分比N；
c)设定
Figure BDA0002072149400000096
当C4和C5的取值均为1时，判定所述环境探测装置进行直线移动，否则判定所述环境探测装置进行非直线移动；其中，Q为预设阈值，P为预设百分比。
在本发明中，记录所述环境探测装置的运动数据，使用机器学习的方式对样本数据进行分析，选取预设阈值和预设百分比。优选地，使用支持向量机SVM，将静止与行走、直行与非直行，看作线性的二分类问题。支持向量机SVM的基本想法是求解能够正确划分训练数据集并且几何间隔最大的分离超平面。具体地，记录所述IMU模块的航向角差值、绕Z轴角速度和超过角速度阈值的个数，分别对这三项进行训练，得出对应的阈值。需要说明的是，首先需要确定角速度阈值，再得出所述预设百分比。
在直行情况下，行人的实际行走方向为沿着y轴正方向，但由于走路影响，所述环境探测装置的二维坐标轴可能会发生小幅度偏转为x’轴和y’轴。雷达获取的方位角为α，障碍物到所述环境探测装置的距离为r，则该时间段内且经过修正后的航向差值为
Figure BDA0002072149400000101
通过雷达获取障碍物相对于所述环境探测装置的速度为vo，如图3所示。故前一时刻的实际方位角为
Figure BDA0002072149400000102
由于障碍物可能在移动，取很短的时间间隔Δt，障碍物到所述环境探测装置的距离近似不变为r，如图4所示。故修正后的所述雷达采集的方向角为
Figure BDA0002072149400000103
其中，和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三轴测量值；
在所述预设数量组三轴测量值中任意选取八组三轴测量值，并基于最小二乘法求解方程组
Figure FDA0003947653030000011
以得到X，其中r，s，t为所述IMU模块的实际三轴，or，os，ot分别为r，s，t轴固定偏差，kr，ks，kt分别为r，s，t轴比例因子，β为t轴和z轴的夹角，α为t轴和z轴构成的平面到x轴的夹角，c为常量；
Figure FDA0003947653030000012
获取预设个数的X后，计算
Figure FDA0003947653030000013
选取sum最小时对应的X作为所述预设参数，m为所述预设数量。
3.根据权利要求1所述的环境探测装置的修正方法，其特征在于，基于IMU模块采集的数据判断所述环境探测装置是否静止包括以下步骤：
计算所述IMU模块的加速度幅值
Figure FDA0003947653030000014
其中，AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度；
计算所述IMU模块的角速度幅值
Figure FDA0003947653030000015
其中ωX、ωY、ωZ分别为所述IMU模块在X、Y、Z轴的角速度；
计算一定滑动窗口内时刻j到时刻i之间加速度的方差σn；
设定
Figure FDA0003947653030000021
和
Figure FDA0003947653030000022
当C1、C2和C3的取值均为1时，判定所述环境探测装置处于静止状态，否则判定所述环境探测装置处于非静止状态；其中，
Figure FDA0003947653030000023
Aj是j时刻的加速度值，
Figure FDA0003947653030000024
是时刻j到时刻i内加速度的均值，n为窗口内数据的个数，Athl、Athh分别为加速度幅值下限值和上限值，ωth为角速度幅值阈值，σth为加速度方差阈值。
4.根据权利要求3所述的环境探测装置的修正方法，其特征在于，基于机器学习算法获取所述加速度幅值下限值和上限值、所述角速度幅值阈值和所述加速度方差阈值。
5.根据权利要求1所述的环境探测装置的修正方法，其特征在于，对所述IMU模块的航向角进行倾斜补偿包括以下步骤：
计算所述IMU模块的俯仰角
Figure FDA0003947653030000025
和翻滚角
Figure FDA0003947653030000026
其中AX、AY、AZ分别为所述IMU模块在X、Y、Z轴的加速度；
基于所述俯仰角和所述翻滚角计算所述IMU模块在X、Y轴补偿后的磁感应强度
Figure FDA0003947653030000027
其中HX、HY和HZ分别为所述IMU模块在X、Y、Z轴的磁感应强度；
计算倾斜补偿后的航向角
Figure FDA0003947653030000028
6.根据权利要求1所述的环境探测装置的修正方法，其特征在于，基于IMU模块采集的数据判断所述环境探测装置是否直线移动包括以下步骤：
计算前一时刻和当前时刻所述IMU模块倾斜补偿后的航向角差值Δφ＝yaw′1-yaw′0；
计算前一时刻到当前时刻的时间段内所述IMU模块所有绕Z轴角速度ω1，ω1…ωn中大于设定的阈值角速度ωth的个数占总个数n的百分比N；
设定
Figure FDA0003947653030000031
当C4和C5的取值均为1时，判定所述环境探测装置进行直线移动，否则判定所述环境探测装置进行非直线移动；其中，Q为预设阈值，P为预设百分比。
7.根据权利要求6所述的环境探测装置的修正方法，其特征在于，基于机器学习算法获取所述预设阈值和所述预设百分比。
8.根据权利要求1所述的环境探测装置的修正方法，其特征在于，对所述IMU模块的航向角进行误差修正，以获取所述IMU模块的实际偏移角度包括以下步骤：
计算倾斜补偿后的航向角在相邻两个时刻的差值
Figure FDA0003947653030000032
基于所述IMU模块的x轴或y轴角速度计算所述IMU模块的角度
Figure FDA0003947653030000033
计算所述实际偏移角度
Figure FDA0003947653030000034
其中h为动态权值。
9.根据权利要求1所述的环境探测装置的修正方法，其特征在于，修正后的所述雷达采集的方向角为
Figure FDA0003947653030000035
其中，r为障碍物距离所述环境探测装置的距离，α为所述雷达采集的方向角，vox和voy为所述雷达采集的障碍物相对于所述环境探测装置的X和Y方向的速度，
Figure FDA0003947653030000036
为所述实际偏移角度，Δ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所述的便携设备，其特征在于，所述便携设备为导盲设备。
CN201910441487.XA 2019-05-24 2019-05-24 环境探测装置及其修正方法、系统、便携设备及存储介质 Active CN110221302B (zh)

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 CN110221302A (zh) 2019-09-10
CN110221302B true 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)

Families Citing this family (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN110736968B (zh) * 2019-10-16 2021-10-08 清华大学 基于深度学习的雷达异常状态诊断方法
CN111780746A (zh) * 2020-03-27 2020-10-16 宁波小遛共享信息科技有限公司 方向角的检测方法、装置、电子设备和代步工具
CN111665498B (zh) * 2020-05-20 2023-04-18 浙江大华技术股份有限公司 环境视图的测绘方法、装置、便携式探测设备
CN111623799B (zh) * 2020-05-21 2022-11-08 北京嘀嘀无限科技发展有限公司 终端的状态检测方法、装置、终端和存储介质
CN111928844B (zh) * 2020-06-11 2023-11-03 嘉兴市纳杰微电子技术有限公司 一种mems陀螺在agv应用上的模型系统
CN112051844B (zh) * 2020-08-17 2023-08-04 尚科宁家(中国)科技有限公司 自移动机器人及其控制方法
CN112797979B (zh) * 2020-12-31 2022-07-01 苏州精源创智能科技有限公司 一种应用于agv的惯性姿态导航系统
CN113790738A (zh) * 2021-08-13 2021-12-14 上海智能网联汽车技术中心有限公司 一种基于智能云台imu的数据补偿方法

Family Cites Families (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN101344391B (zh) * 2008-07-18 2011-05-11 北京工业大学 基于全功能太阳罗盘的月球车位姿自主确定方法
CN102865881B (zh) * 2012-03-06 2014-12-31 武汉大学 一种惯性测量单元的快速标定方法
US9304198B1 (en) * 2013-10-04 2016-04-05 Sandia Corporation Navigator alignment using radar scan
CN103968827B (zh) * 2014-04-09 2017-11-28 北京信息科技大学 一种可穿戴式人体步态检测的自主定位方法
CN109099913B (zh) * 2018-10-10 2023-07-07 格物智航(深圳)科技有限公司 一种基于mems惯性器件的穿戴式导航装置和方法

Also Published As

Publication number Publication date
CN110221302A (zh) 2019-09-10

Similar Documents

Publication Publication Date Title
CN110221302B (zh) 环境探测装置及其修正方法、系统、便携设备及存储介质
KR102018450B1 (ko) 단일 안테나에 기초한 ｇｎｓｓ―ｉｎｓ 차량 자세 결정 방법
Ludwig et al. Comparison of Euler estimate using extended Kalman filter, Madgwick and Mahony on quadcopter flight data
KR102017404B1 (ko) 9축 mems 센서에 기반하여 농기계의 전-자세 각도를 갱신하는 방법
CN107289930B (zh) 基于mems惯性测量单元的纯惯性车辆导航方法
KR101988786B1 (ko) 관성 항법 장치의 초기 정렬 방법
US8758275B2 (en) Moving body posture angle processing device
CN102297693B (zh) 测量物体位置和方位的方法
JP6019504B2 (ja) 移動体の進行方向推定装置及び進行方向推定方法
US8396684B2 (en) Method and system for motion tracking
KR20130127991A (ko) 이동체의 경로를 추정하는 방법 및 시스템
JP4787359B2 (ja) 物理量計測装置および物理量計測方法
JP2012173190A (ja) 測位システム、測位方法
Ludwig et al. Comparison of attitude and heading reference systems using foot mounted MIMU sensor data: Basic, Madgwick, and Mahony
US20090085867A1 (en) Error-correction apparatus and method and 3D pointing device using the error-correction apparatus
CN112362057A (zh) 基于零速修正与姿态自观测的惯性行人导航算法
JP2014240266A (ja) センサドリフト量推定装置及びプログラム
KR101226767B1 (ko) 주행 장치를 위한 위치측정 시스템 및 방법
WO2018214226A1 (zh) 一种无人车实时姿态测量方法
CN113984051A (zh) Imu与刚体的位姿融合方法、装置、设备及存储介质
CN108592902B (zh) 一种基于多传感器的定位设备及定位方法、系统和机械臂
Wongwirat et al. A position tracking experiment of mobile robot with inertial measurement unit (imu)
JP5748174B2 (ja) 移動体の相対姿勢計測方法と装置
JP2006038650A (ja) 姿勢計測方法、姿勢制御装置、方位計及びコンピュータプログラム
Zhang et al. Implementation and complexity analysis of orientation estimation algorithms for human body motion tracking using low-cost sensors

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