CN110132270B - 自动扫雪装置定位方法 - Google Patents

自动扫雪装置定位方法 Download PDF

Info

Publication number
CN110132270B
CN110132270B CN201910511007.2A CN201910511007A CN110132270B CN 110132270 B CN110132270 B CN 110132270B CN 201910511007 A CN201910511007 A CN 201910511007A CN 110132270 B CN110132270 B CN 110132270B
Authority
CN
China
Prior art keywords
odom
automatic snow
information
positioning
sweeping device
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
CN201910511007.2A
Other languages
English (en)
Other versions
CN110132270A (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 Hanyang Tech Co ltd
Original Assignee
Shenzhen Hanyang Tech 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 Hanyang Tech Co ltd filed Critical Shenzhen Hanyang Tech Co ltd
Priority to CN201910511007.2A priority Critical patent/CN110132270B/zh
Priority to PCT/CN2019/091797 priority patent/WO2020248293A1/zh
Publication of CN110132270A publication Critical patent/CN110132270A/zh
Priority to US17/305,817 priority patent/US20210341566A1/en
Application granted granted Critical
Publication of CN110132270B publication Critical patent/CN110132270B/zh
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/0257—Hybrid positioning
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C17/00—Compasses; Devices for ascertaining true or magnetic north for navigation or surveying purposes
    • 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/20—Instruments for performing navigational calculations
    • G—PHYSICS
    • G01—MEASURING; TESTING
    • G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
    • G01C22/00—Measuring distance traversed on the ground by vehicles, persons, animals or other moving solid bodies, e.g. using odometers, using pedometers
    • 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
    • G01S5/00—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations
    • G01S5/02—Position-fixing by co-ordinating two or more direction or position line determinations; Position-fixing by co-ordinating two or more distance determinations using radio waves
    • G01S5/10—Position of receiver fixed by co-ordinating a plurality of position lines defined by path-difference measurements, e.g. omega or decca systems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/25—Fusion techniques
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V20/00—Scenes; Scene-specific elements
    • G06V20/10—Terrestrial scenes

Landscapes

  • Engineering & Computer Science (AREA)
  • Radar, Positioning & Navigation (AREA)
  • Remote Sensing (AREA)
  • Physics & Mathematics (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Automation & Control Theory (AREA)
  • Theoretical Computer Science (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Evolutionary Biology (AREA)
  • Evolutionary Computation (AREA)
  • General Engineering & Computer Science (AREA)
  • Artificial Intelligence (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Control Of Position, Course, Altitude, Or Attitude Of Moving Bodies (AREA)
  • Navigation (AREA)

Abstract

本发明公开了一种自动扫雪装置定位方法，包括以下步骤：步骤S10，获取UBW定位系统所发送的第一定位信息；步骤S20，获取旋转编码器所发送的第一转动信息；步骤S30，获取电子罗盘所发送的第一读数信息；步骤S40，获取陀螺仪所发送的第一方向角信息；步骤S50，基于第一定位信息和第一转动信息进行第一融合计算，以确定自动扫雪装置的第一位置信息；步骤S60，基于第一读数信息和第一方向角信息进行第二融合计算，以确定出自动扫雪装置的第一方向信息。本发明，能够获取到自动扫雪装置更多的定位信息，以能确定其实时姿势，且更加精准。

Description

自动扫雪装置定位方法
技术领域
本发明属于自动化设备定位技术领域，尤其涉及一种自动扫雪装置定位方法。
背景技术
目前，对于能够移动的机器人或者电子设备的定位方法很多，比如专利申请号为CN201310469003，申请名称为一种室内移动机器人定位方法，其利用单超声波传感器测距信息与航迹推算信息来确定机器人坐标，但实际无法对于机器人的具体朝向进行定位，即无法得知机器人的姿势，比如对于自动扫雪装置，其具体姿势，即车头的朝向方向就无法得知，存在获取的定位信息少、适用性低问题。
因此，现有技术有待于改善。
发明内容
本发明的主要目的在于提出一种自动扫雪装置定位方法，旨在解决背景技术中所提及的技术问题，以能够获取到自动扫雪装置更多的定位信息，且更加精准。
本发明的一种自动扫雪装置定位方法，包括以下步骤：
步骤S10，获取UWB定位系统所发送的第一定位信息；
步骤S20，获取旋转编码器所发送的第一转动信息；
步骤S30，获取电子罗盘所发送的第一读数信息；
步骤S40，获取陀螺仪所发送的第一方向角信息；
步骤S50，基于第一定位信息和第一转动信息进行第一融合计算，以确定自动扫雪装置的第一位置信息；
步骤S60，基于第一读数信息和第一方向角信息进行第二融合计算，以确定出自动扫雪装置的第一方向信息。
优选地，UWB定位系统包括第一基站、第一标签、第二标签和第三标签，第一基站设置在自动扫雪装置上，第一标签的设置位置、第二标签的设置位置和第三标签的设置位置均不在同一条直线上。
优选地，步骤S50具体包括：
步骤S51，对UWB定位系统所发送的第一定位信息进行校正计算，以确定第二定位信息；
步骤S52，对第一转动信息进行计算，以确定ODOM增量；
步骤S53，基于第二定位信息和ODOM增量进行融合计算，以确定自动扫雪装置的第一位置信息。
优选地，步骤S51中，校正计算所利用的计算公式为Rmodify＝(Ruwb_raw-buwb)/kuwb；其中，Rmodify表示第二定位信息，Ruwb_raw表示UWB定位系统所发送的第一定位信息，kuwb表示用最小二乘法拟合直线的方法得到的斜率，buwb表示用最小二乘法拟合直线的方法得到截距。
优选地，步骤S52中，利用到的公式包括
Figure GDA0002927687690000021
Figure GDA0002927687690000022
xodom＝dodomcosγ；yodom＝-dodomsinγ；其中，dodom表示自动扫雪装置走过的距离，ntick_left表示自动扫雪装置上左轮所转动的tick值；ntick_right表示自动扫雪装置上右轮所转动的tick值；Ntick_1m表示自动扫雪装置每移动1米所对应的tick值；lwidth表示自动扫雪装置的左轮和右轮之间的距离。
优选地，在步骤S53中，利用到的公式包括xodom_i＝xodom_i-1+Δxodom；yodom_i＝yodom_i-1+Δyodom；
Figure GDA0002927687690000029
Figure GDA0002927687690000024
Δxodom_car＝ΔT×(vx_odom_i-1+vx_odom_i)/2；Δyodom_car＝ΔT×(vy_odom_i-1+vy_odom2_i)/2；其中，ΔT是一次融合计算的时间步长，
Figure GDA0002927687690000025
是i时刻车在地图坐标系上的方向角；以及
Figure GDA0002927687690000026
Figure GDA0002927687690000027
其中，R是UWB定位系统所获取的第一定位信息中所包含的噪声的方差矩阵，Q是ODOM增量中包含的噪声的方差矩阵，I是单位阵。K是卡尔曼滤波系数矩阵，P是误差的协方差矩阵，
Figure GDA0002927687690000028
表示i时刻自动扫雪装置的第一位置信息。
优选地，UWB定位系统中的第一标签、第二标签和第三标签构建一个地图坐标系。
优选地，第一方向信息包括自动扫雪装置的车头方向与地图坐标系中x轴的夹角。
本发明的自动扫雪装置定位方法，有益效果如下：
基于步骤S50、步骤S60，对于步骤S10中获取UWB定位系统所发送的第一定位信息以及步骤S20中获取旋转编码器所发送的第一转动信息进行第一融合计算，确定出自动扫雪装置的第一位置信息；提高自动扫雪装置的第一位置信息的准确性；以及对于步骤S30中获取电子罗盘所发送的第一读数信息和步骤S40所获取陀螺仪所发送的第一方向角信息进行第二融合计算，以确定自动扫雪装置的第一方向信息，即能得知自动扫雪装置的实时姿势，比如车头的朝向，即后续的扫雪方向。
附图说明
图1为本发明自动扫雪装置定位方法第一实施例的流程示意图；
图2为本发明自动扫雪装置定位方法第一实施例中步骤S50的细化流程示意图；
图3为本发明自动扫雪装置定位方法中电子罗盘的第一读数校正后的示意图；
图4为本发明自动扫雪装置定位方法中第一标签、第二标签和第三标签分别与第一基站的距离的示意图；
图5为本发明自动扫雪装置定位方法中第一标签、第二标签、第三标签、第一基站的示意图；
图6为本发明自动扫雪装置定位方法中步骤S50的第一定位信息和第一转动信息进行第一融合计算后的示意图；
图7为本发明自动扫雪装置定位方法中步骤S60的第一读数信息和第一方向角信息进行第二融合计算后的示意图；
图8为本发明自动扫雪装置定位方法中自动扫雪装置的结构示意图；
图9为本发明自动扫雪装置定位方法中自动扫雪装置的原理框图；
图10为本发明自动扫雪装置定位方法中第一标签的结构示意图；
图11为本发明自动扫雪装置定位方法中自动扫雪装置的右视图；
图12为本发明自动扫雪装置定位方法中自动扫雪装置的主视图；
图13为本发明自动扫雪装置定位方法中自动扫雪装置的左视图；
图14为本发明自动扫雪装置定位方法中自动扫雪装置与充电平台配合的示意图；
图15为本发明自动扫雪装置定位方法中充电平台的主视图；
图16为本发明自动扫雪装置定位方法中自动扫雪装置的俯视图；
图17为本发明自动扫雪装置定位方法中自动扫雪装置的内部结构示意图。
本发明目的的实现、功能特点及优点将结合实施例，参照附图做进一步说明。
具体实施方式
应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。需要注意的是，相关术语如“第一”、“第二”等可以用于描述各种组件，但是这些术语并不限制该组件。这些术语仅用于区分一个组件和另一组件。例如，不脱离本发明的范围，第一组件可以被称为第二组件，并且第二组件类似地也可以被称为第一组件。术语“和/或”是指相关项和描述项的任何一个或多个的组合。
如图1、图2、图9所示，图1为本发明自动扫雪装置定位方法第一实施例的流程示意图；图2为本发明自动扫雪装置定位方法第一实施例中步骤S50的细化流程示意图。
本发明的一种自动扫雪装置定位方法，包括以下步骤：
步骤S10，获取UWB定位系统所发送的第一定位信息；在步骤S10中，UWB定位系统包括第一基站304、第一标签、第二标签和第三标签，第一基站设置在自动扫雪装置上，第一标签的设置位置、第二标签的设置位置和第三标签的设置位置均不在同一条直线上。其中，UWB定位系统中利用到的第一标签、第二标签、第三标签和第一基站均是现有的无载波通信技术，但，；在步骤S10中，搭建的UWB定位系统，以第一标签设置位置作为地图坐标系原点，第二标签设置位置作为地图坐标系X轴方向，第三标签设置位置作为地图坐标系Y轴方向，即利用三个标签构建了XOY直角坐标系；即优选地，UWB定位系统中的第一标签、第二标签和第三标签构建一个地图坐标系，该地图坐标系为XOY直角坐标系。
在步骤S10中，XOY直角坐标系中，第一标签、第二标签和第三标签之间的距离分别为R01、R02、R12。令第一标签所在位置为地图坐标系的原点，第二标签在地图坐标系x轴正向上；第三标签在地图坐标系Y轴方向上；则三个标签的坐标分别为：第一标签：(0.0，0.0)；第二标签：(R01，0.0)；第三标签：(R02cosθ，R02sinθ)；其中，θ是第三标签与第一标签的连线与x轴的夹角，其值可用余弦公式求出：
Figure GDA0002927687690000041
在构建了上述XOY直角坐标系后，在UWB系统发送第一定位信息至自动扫雪装置，第一定位信息包括第一基站在地图坐标系的位置，令三个定位标签的坐标分别为(x1，y1),(x2，y2)和(x3，y3)，它们到第一基站的距离分别为R1、R2和R3，利用到的公式如下，
Figure GDA0002927687690000042
其中，
Figure GDA0002927687690000043
Figure GDA0002927687690000044
其中，dxi＝xi-x1，dyi＝yi-y1，
Figure GDA0002927687690000045
Figure GDA0002927687690000046
Figure GDA0002927687690000047
表示第一基站在地图坐标系的位置。
例如，例：三个标签的坐标分别为(0.0,0.0),(4.0,0.0),(0.0,3.0)，测得到第一基站的距离(y轴)如图4：计算出第一基站的位置如图5。
步骤S20，获取旋转编码器302所发送的第一转动信息；旋转编码器用于获取自动扫雪装置的车轮转动的tick值；
步骤S30，获取电子罗盘303所发送的第一读数信息；电子罗盘设置于自动扫雪装置上；
步骤S40，获取陀螺仪305所发送的第一方向角信息；陀螺仪设置于自动扫雪装置上；
在步骤S10、步骤S20、步骤S30和步骤S40获取的四个数据，即第一定位信息、第一转动信息、第一读数信息和第一方向角信息；上述四个数据均发送至自动扫雪装置中的控制器300，控制器300基于预存的算法、公式，基于上述四个数据便于执行后续的步骤S50、步骤S60；
步骤S50，基于第一定位信息和第一转动信息进行第一融合计算，以确定自动扫雪装置的第一位置信息；
步骤S60，基于第一读数信息和第一方向角信息进行第二融合计算，以确定出自动扫雪装置的第一方向信息。
本发明的自动扫雪装置定位方法，有益效果如下：
基于步骤S50、步骤S60，对于步骤S10中获取UWB定位系统所发送的第一定位信息以及步骤S20中获取旋转编码器所发送的第一转动信息进行第一融合计算，确定出自动扫雪装置的第一位置信息；提高自动扫雪装置的第一位置信息的准确性；以及对于步骤S30中获取电子罗盘所发送的第一读数信息和步骤S40所获取陀螺仪所发送的第一方向角信息进行第二融合计算，以确定自动扫雪装置的第一方向信息，即能得知自动扫雪装置的实时姿势，比如车头的朝向。
更具体地，如图2所示，步骤S50具体包括：
步骤S51，对UWB定位系统所发送的第一定位信息进行校正计算，以确定第二定位信息；
步骤S52，对第一转动信息进行计算，以确定ODOM增量；
步骤S53，基于第二定位信息和ODOM增量进行融合计算，以确定自动扫雪装置的第一位置信息。
在步骤S51中，第一定位信息包括第一基站的位置；即由此来表示自动扫雪装置在地图坐标系的位置；校正计算所利用的计算公式为Rmodify＝(Ruwb_raw-buwb)/kuwb(公式1-3)；其中，Rmodify表示第二定位信息，Ruwb_raw表示UWB定位系统所发送的第一定位信息，kuwb表示用最小二乘法拟合直线的方法得到的斜率，buwb表示用最小二乘法拟合直线的方法得到截距。
优选地，步骤S52中，利用到的公式包括
Figure GDA0002927687690000061
Figure GDA0002927687690000062
yodom＝-dodomsinγ(公式1-7)；其中，dodom表示自动扫雪装置走过的距离，ntick_left表示自动扫雪装置上左轮所转动的tick值；ntick_right表示自动扫雪装置上右轮所转动的tick值；Ntick_1m表示自动扫雪装置每移动1米所对应的tick值；lwidth表示自动扫雪装置的左轮和右轮之间的距离。
其中，该ODOM增量除以测量时间，可得到该段时间内的平均速度：
vx_odom＝xodom/Δt (3-1)
vy_odom＝yodom/Δt (3-2)
优选地，在步骤S53中，利用到的公式包括xodom_i＝xodom_i-1+Δxodom(公式1-8)；yodom_i＝yodom_i-1+Δyodom(公式1-9)；
Figure GDA00029276876900000610
Figure GDA0002927687690000064
Figure GDA0002927687690000065
Δxodom_car＝ΔT×(vx_odom_i-1+vx_odom_i)/2(公式1-12)；Δyodom_car＝ΔT×(vy_odom_i-1+vy_odom2_i)/2(公式1-13)；其中，ΔT是一次融合计算的时间步长，
Figure GDA0002927687690000066
是i时刻车在地图坐标系上的方向角；以及
Figure GDA0002927687690000067
Figure GDA0002927687690000068
其中，R是UWB定位系统所获取的第一定位信息中所包含的噪声的方差矩阵，Q是ODOM增量中包含的噪声的方差矩阵，I是单位阵。K是卡尔曼滤波系数矩阵，P是误差的协方差矩阵，
Figure GDA0002927687690000069
表示i时刻自动扫雪装置的第一位置信息，xodom_i表示i时刻x轴方向ODOM增量，xodom_i-1表示i-1时刻x轴方向ODOM增量，yodom_i表示i时刻y轴方向ODOM增量，yodom_i-1表示i-1时刻y轴方向ODOM增量，Δxodom_car表示从i-1时刻至i时刻自动扫雪装置朝x轴正方向移动的ODOM增量，Δyodom_car表示从i-1时刻至i时刻自动扫雪装置朝y轴正方向移动的ODOM增量。
如图8所示，优选地，第一方向信息包括自动扫雪装置的车头88方向与地图坐标系中x轴的夹角；车头方向表示自动扫雪装置的扫雪方向；因此，在本定位方法中能够获取到自动扫雪装置的车头方向是很关键的；相当于了解到自动扫雪装置的实时姿势；实现全面监控。
其中，对于步骤S60，基于第一读数信息和第一方向角信息进行第二融合计算，以确定出自动扫雪装置的第一方向信息。步骤S60具体包括：对第一读数信息进行校正处理，以获取方向角
Figure GDA0002927687690000071
将方向角
Figure GDA0002927687690000072
和第一方向角信息进行第二融合计算，以确定出自动扫雪装置的第一方向信息。
在步骤S60中，电子罗盘的第一读数校正处理如下：
自动扫雪装置进行一圈原地旋转，记录自动扫雪装置原地旋转时的电子罗盘x和y轴的读数分别为mx_raw和my_raw，分别取其最大和最小值，计算校正参数k和b：
kx＝1
Figure GDA0002927687690000073
bx＝(mx_max+mx_min)/2 (公式2-2)
by＝(my_max+my_min)/2 (公式2-3)
Figure GDA0002927687690000074
Figure GDA0002927687690000075
令校正后x和y方向的磁场强度分别为
Figure GDA0002927687690000076
和
Figure GDA0002927687690000077
假设自动扫雪装置只在水平地面上作业，则计算方向角的
Figure GDA0002927687690000078
方法如下：
Figure GDA0002927687690000079
当
Figure GDA00029276876900000710
时，若
Figure GDA00029276876900000711
则
Figure GDA00029276876900000712
若
Figure GDA00029276876900000713
则
Figure GDA00029276876900000714
Figure GDA00029276876900000715
当
Figure GDA00029276876900000716
时，若
Figure GDA00029276876900000717
则
Figure GDA00029276876900000718
若
Figure GDA00029276876900000719
则
Figure GDA00029276876900000720
当
Figure GDA00029276876900000721
时，
Figure GDA00029276876900000722
当
Figure GDA00029276876900000723
时，若
Figure GDA00029276876900000724
则
Figure GDA00029276876900000725
若
Figure GDA00029276876900000726
则
Figure GDA00029276876900000727
Figure GDA00029276876900000728
计算出的方向角
Figure GDA0002927687690000081
加上当地的地磁偏角(可前往一些现有网站进行查询，比如http://www.magnetic-declination.com)，即可得到其在大地坐标系上的方向角。
如图3所示，在一次对电子罗盘的校正中，原始数据包括2411组数据，为点10；经校正共取得2411组数据，如图中点11。根据这些数据计算出校正参数：kx＝1.0；bx＝0.66；ky＝0.9439；by＝0.17；对上述数据校正后的结果如图中点11。点11组成的图形更接近正圆，且圆心位置更靠近原点，证明校正计算有效。
举个例子，自动扫雪装置对准x轴时，测得电子罗盘读数为mx＝0.17,my＝2.05。校正后为mx_n＝-0.49,my_n＝1.7745。深圳的磁偏角为-2.9833°，则计算得到扫雪车的朝向(即坐标系x轴与大地坐标系正北方向的夹角)为102.45°。
在步骤S60中，将方向角
Figure GDA0002927687690000082
和第一方向角信息进行第二融合计算包括：计算i时刻陀螺仪给出的方向角
Figure GDA0002927687690000083
Figure GDA0002927687690000084
计算i时刻的第一方向信息
Figure GDA0002927687690000085
第一方向信息
Figure GDA0002927687690000086
表示自动扫雪装置的车头方向与地图坐标系中x轴的夹角。
Figure GDA0002927687690000087
Figure GDA0002927687690000088
其中R是电子罗盘所发送的第一读数信息中包含的噪声的方差，Q是陀螺仪所发送的第一方向角信息中包含的噪声的方差。K是卡尔曼滤波系数，P是误差的协方差，K和P均可看作中间变量。
例如，自动扫雪装置以0.4m/s的速度沿x轴正向运动。UWB定位系统测距最大误差为0.1m，旋转编码器给出的定位结果始终存在5mm每米的误差。融合计算结果如图6：图6中，点是UWB定位系统发送的第一定位信息；直线是旋转编码器发送的第一转动信息；曲线是经过步骤S50的第一融合计算后的数据，可见步骤S50的融合计算很好地消除了旋转编码器定位的累积误差，融合结果的噪声也明显比UWB定位系统的噪声小。噪声可理解为误差。
如图7所示，自动扫雪装置以0.2rad/s的速度旋转。实验取得100组数据，结果如图：可见步骤S60的融合计算给出了较好的结果。
在执行步骤S50、步骤S60后，即将第一位置信息和第一方向信息计算出来后，自动扫雪装置的控制器控制通讯模块将第一位置信息和第一方向信息发送至监控中心，监控中心包括终端。
本发明还提供了一种自动扫雪装置定位系统，包括自动扫雪装置、第一标签、第二标签、第三标签和第一基站；如图10所示，为第一标签的结构示意图；第一标签包括标签1000和具有插入部的底座1001；如图11所示，自动扫雪装置上设置有尾灯900；如图12所示，自动扫雪装置侧面上设置有第一超声波传感器902，用于避障；自动扫雪装置上设置有履带901，以便于在雪地中移动；如图13所示，自动扫雪装置上设置有摄像头970，第一基站设置于摄像头970内；自动扫雪装置上设置有抛雪筒971，可360度转动，抛雪距离可调；自动扫雪装置的车头上设置有叶片972，实现扫雪、抛雪功能；如图14所示，自动扫雪装置定位系统还包括充电平台1100；如图15所示，充电平台上设置有红外LED灯2001；如图16所示，充电平台上设置有第一开关2002，实现开关功能、；以及充电适应模块2003，以适应左右距离误差，降低充电导航精度；导向缓冲橡胶轮2004；充电电极2005；车体到位传感器2006，以检测到自动扫雪装置靠近时，充电电极对其进行充电；如图17所示，自动扫雪装置内包括第一驱动电机1046，以对于叶片972进行供能驱动；以及行走驱动模块1040；耐低温充电电池1041；被动充电通电模块1042，以应于与充电电极相通，实现被动充电；第一光电传感器1043，对于齿轮齿计数，确定对应旋转角度；直流减速电机1044，对抛雪筒971进行供能驱动；第二光电传感器，对旋转原点进行检测。
以上仅为本发明的优选实施例，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。

Claims (5)

1.一种自动扫雪装置定位方法，其特征在于，包括以下步骤：
步骤S10，获取UWB定位系统所发送的第一定位信息；
步骤S20，获取旋转编码器所发送的第一转动信息；
步骤S30，获取电子罗盘所发送的第一读数信息；
步骤S40，获取陀螺仪所发送的第一方向角信息；
步骤S50，基于第一定位信息和第一转动信息进行第一融合计算，以确定自动扫雪装置的第一位置信息；
步骤S60，基于第一读数信息和第一方向角信息进行第二融合计算，以确定出自动扫雪装置的第一方向信息；
其中，UWB定位系统包括第一基站、第一标签、第二标签和第三标签，第一基站设置在自动扫雪装置上，第一标签的设置位置、第二标签的设置位置和第三标签的设置位置均不在同一条直线上，以第一标签设置位置作为XOY直角坐标系原点，第二标签设置位置作为地图坐标系X轴方向，第三标签设置位置作为地图坐标系Y轴方向；
步骤S50具体包括：
步骤S51，对UWB定位系统所发送的第一定位信息进行校正计算，以确定第二定位信息；
步骤S52，对第一转动信息进行计算，以确定ODOM增量；
步骤S53，基于第二定位信息和ODOM增量进行融合计算，以确定自动扫雪装置的第一位置信息；
在步骤S53中，利用到的公式包括：
xodom_i＝xodom_i-1+Δxodom；yodom_i＝yodom_i-1+Δyodom；
Figure FDA0003083538820000011
Figure FDA0003083538820000012
Δxodom_car＝ΔT×(vx_odom_i-1+vx_odom_i)/2；
Δyodom_car＝ΔT×(vy_odom_i-1+vy_odom2_i)/2；
其中，ΔT是一次融合计算的时间步长，
Figure FDA0003083538820000013
是i时刻自动扫雪装置在地图坐标系上的方向角；以及
Figure FDA0003083538820000021
Figure FDA0003083538820000022
其中，R是UWB定位系统所获取的第一定位信息中所包含的噪声的方差矩阵，Q是ODOM增量中包含的噪声的方差矩阵，I是单位阵，K是卡尔曼滤波系数矩阵，P是误差的协方差矩阵，
Figure FDA0003083538820000023
表示i时刻自动扫雪装置的第一位置信息，xodom_i表示i时刻x轴方向ODOM增量，xodom_i-1表示i-1时刻x轴方向ODOM增量，yodom_i表示i时刻y轴方向ODOM增量，yodom_i-1表示i-1时刻y轴方向ODOM增量，Δxodom_car表示从i-1时刻至i时刻自动扫雪装置朝x轴正方向移动的ODOM增量，Δyodom_car表示从i-1时刻至i时刻自动扫雪装置朝y轴正方向移动的ODOM增量。
2.如权利要求1所述自动扫雪装置定位方法，其特征在于，步骤S51中，校正计算所利用的计算公式为Rmodify＝(Ruwb_raw-buwb)/kuwb；其中，Rmodify表示第二定位信息，Ruwb_raw表示UWB定位系统所发送的第一定位信息，kuwb表示用最小二乘法拟合直线的方法得到的斜率，buwb表示用最小二乘法拟合直线的方法得到截距。
3.如权利要求1所述自动扫雪装置定位方法，其特征在于，在步骤S52中，利用到的公式包括
Figure FDA0003083538820000024
xodom＝dodomcosγ；yodom＝-dodomsinγ；其中，dodom表示自动扫雪装置走过的距离，ntick_left表示自动扫雪装置上左轮所转动的tick值；ntick_right表示自动扫雪装置上右轮所转动的tick值；Ntick_1m表示自动扫雪装置每移动1米所对应的tick值；lwidth表示自动扫雪装置的左轮和右轮之间的距离。
4.如权利要求1所述自动扫雪装置定位方法，其特征在于，UWB定位系统中的第一标签、第二标签和第三标签构建一个地图坐标系。
5.如权利要求1所述自动扫雪装置定位方法，其特征在于，第一方向信息包括自动扫雪装置的车头方向与地图坐标系中x轴的夹角。
CN201910511007.2A 2019-06-13 2019-06-13 自动扫雪装置定位方法 Active CN110132270B (zh)

Priority Applications (3)

Application Number Priority Date Filing Date Title
CN201910511007.2A CN110132270B (zh) 2019-06-13 2019-06-13 自动扫雪装置定位方法
PCT/CN2019/091797 WO2020248293A1 (zh) 2019-06-13 2019-06-19 自动扫雪装置定位方法
US17/305,817 US20210341566A1 (en) 2019-06-13 2021-07-15 Systems and methods for positioning devices

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN201910511007.2A CN110132270B (zh) 2019-06-13 2019-06-13 自动扫雪装置定位方法

Publications (2)

Publication Number Publication Date
CN110132270A CN110132270A (zh) 2019-08-16
CN110132270B true CN110132270B (zh) 2021-08-31

Family

ID=67581425

Family Applications (1)

Application Number Title Priority Date Filing Date
CN201910511007.2A Active CN110132270B (zh) 2019-06-13 2019-06-13 自动扫雪装置定位方法

Country Status (3)

Country Link
US (1) US20210341566A1 (zh)
CN (1) CN110132270B (zh)
WO (1) WO2020248293A1 (zh)

Families Citing this family (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US11426059B2 (en) * 2018-06-02 2022-08-30 Ankon Medical Technologies (Shanghai) Co., Ltd. Control system for capsule endoscope
CN112179352A (zh) * 2020-09-30 2021-01-05 北京小米移动软件有限公司 空间地图构建方法及装置、移动控制方法及装置、介质
CN114595789B (zh) * 2022-02-10 2024-01-26 湖南三一中型起重机械有限公司 超起棘轮射频标签的自动标定方法、控制器及工程车辆
WO2024065303A1 (zh) * 2022-09-28 2024-04-04 深圳汉阳科技有限公司 一种定位杆结构及扫雪车
CN115639521B (zh) * 2022-10-24 2023-05-05 青岛柯锐思德电子科技有限公司 一种基于uwb的移动目标运动状态判断方法

Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4825159B2 (ja) * 2007-03-29 2011-11-30 富士通株式会社 無線測位システム、移動体、そのプログラム
CN103410512A (zh) * 2013-08-15 2013-11-27 中国矿业大学 融合地质环境信息的采煤机绝对定位装置与方法
CN106052684A (zh) * 2016-06-16 2016-10-26 济南大学 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
CN106352869A (zh) * 2016-08-12 2017-01-25 上海理工大学 移动机器人室内定位系统及其计算方法
CN106643694A (zh) * 2016-11-04 2017-05-10 航天科工智能机器人有限责任公司 一种机器人室内定位方法

Family Cites Families (13)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US6661342B2 (en) * 2001-06-04 2003-12-09 Time Domain Corporation System and method for using impulse radio technology to track the movement of athletes and to enable secure communications between the athletes and their teammates, fans or coaches
US10337884B2 (en) * 2013-03-05 2019-07-02 Invensense, Inc. Method and apparatus for fast magnetometer calibration
CN105446350B (zh) * 2014-09-26 2018-05-29 科沃斯机器人股份有限公司 自移动机器人移动界限划定方法
EP4332612A2 (en) * 2015-03-07 2024-03-06 Verity AG Distributed localization systems and methods and self-localizing apparatus
US20180356492A1 (en) * 2015-06-16 2018-12-13 Michael Hamilton Vision based location estimation system
CN107044103B (zh) * 2016-02-06 2021-08-10 苏州宝时得电动工具有限公司 自动行走除雪设备
DE17781905T1 (de) * 2016-04-12 2019-06-19 Positec Power Tools (Suzhou) Co., Ltd Vorrichtung mit automatischer bewegung für automatisches arbeitssystem und steuerungsverfahren dafür
CN107544483A (zh) * 2016-06-23 2018-01-05 苏州宝时得电动工具有限公司 一种路径控制方法及装置
CN107974996B (zh) * 2016-10-25 2021-04-06 苏州宝时得电动工具有限公司 一种自动行走设备及其控制方法以及自动收线电缆盘
WO2018142183A1 (en) * 2017-01-31 2018-08-09 Vestergaard Company A/S System, unit and method for recovering deicing liquids from airport apron surfaces
CN107190696A (zh) * 2017-07-06 2017-09-22 深圳汉阳科技有限公司 全自动扫雪机
CN207113893U (zh) * 2017-08-15 2018-03-16 深圳市大疆创新科技有限公司 定位系统
CN109040961A (zh) * 2018-08-28 2018-12-18 深圳前海中电慧安科技有限公司 一种终端定位方法和系统

Patent Citations (5)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
JP4825159B2 (ja) * 2007-03-29 2011-11-30 富士通株式会社 無線測位システム、移動体、そのプログラム
CN103410512A (zh) * 2013-08-15 2013-11-27 中国矿业大学 融合地质环境信息的采煤机绝对定位装置与方法
CN106052684A (zh) * 2016-06-16 2016-10-26 济南大学 采用多模式描述的移动机器人imu/uwb/码盘松组合导航系统及方法
CN106352869A (zh) * 2016-08-12 2017-01-25 上海理工大学 移动机器人室内定位系统及其计算方法
CN106643694A (zh) * 2016-11-04 2017-05-10 航天科工智能机器人有限责任公司 一种机器人室内定位方法

Also Published As

Publication number Publication date
US20210341566A1 (en) 2021-11-04
CN110132270A (zh) 2019-08-16
WO2020248293A1 (zh) 2020-12-17

Similar Documents

Publication Publication Date Title
CN110132270B (zh) 自动扫雪装置定位方法
CN102121827B (zh) 一种移动机器人定位系统及其定位方法
EP3603372A1 (en) Moving robot, method for controlling the same, and terminal
Lee et al. Robust mobile robot localization using optical flow sensors and encoders
CN103674015B (zh) 一种无轨化定位导航方法及装置
CN201917661U (zh) 一种移动机器人定位系统
CN108362288B (zh) 一种基于无迹卡尔曼滤波的偏振光slam方法
CN108387236B (zh) 一种基于扩展卡尔曼滤波的偏振光slam方法
CN108733039A (zh) 一种机器人室内导航定位的方法与装置
CN106066179A (zh) 一种基于ros操作系统的机器人位置丢失找回方法和控制系统
CN103207634A (zh) 一种智能车辆中差分gps与惯性导航数据融合的系统和方法
CN110026993B (zh) 一种基于uwb及热释电红外传感器的人体跟随机器人
CN108801269A (zh) 一种室内云机器人导航系统及方法
CN106840152A (zh) 一种面向室内移动机器人的高精度组合导航系统及方法
US11920950B2 (en) System and method for generating precise road lane map data
CN105044754A (zh) 一种基于多传感器融合的移动平台室外定位方法
CN109708632A (zh) 一种面向移动机器人的激光雷达/ins/地标松组合导航系统及方法
WO2016059904A1 (ja) 移動体
CN104089649A (zh) 一种室内环境数据采集系统及采集方法
CN108036792A (zh) 一种用于移动机器人的里程计与测量位姿的数据融合方法
CN102542634A (zh) 一种目标车辆行驶状态测量系统
CN103308047B (zh) 一种具有人工在线修正功能的移动机器人定位方法
CN108253956A (zh) 一种智能捡网球机器人的定位系统、方法及装置
CN109258059B (zh) 一种割草机位置的确定方法和装置
CN106325269B (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