CN112305547B - 一种机器人防跌落检测方法 - Google Patents

一种机器人防跌落检测方法 Download PDF

Info

Publication number
CN112305547B
CN112305547B CN202011123094.3A CN202011123094A CN112305547B CN 112305547 B CN112305547 B CN 112305547B CN 202011123094 A CN202011123094 A CN 202011123094A CN 112305547 B CN112305547 B CN 112305547B
Authority
CN
China
Prior art keywords
distance
ultrasonic sensor
robot
value
formula
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
CN202011123094.3A
Other languages
English (en)
Other versions
CN112305547A (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.)
Shandong New Generation Information Industry Technology Research Institute Co Ltd
Original Assignee
Shandong New Generation Information Industry Technology Research Institute 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 Shandong New Generation Information Industry Technology Research Institute Co Ltd filed Critical Shandong New Generation Information Industry Technology Research Institute Co Ltd
Priority to CN202011123094.3A priority Critical patent/CN112305547B/zh
Publication of CN112305547A publication Critical patent/CN112305547A/zh
Application granted granted Critical
Publication of CN112305547B publication Critical patent/CN112305547B/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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/88—Sonar systems specially adapted for specific applications
    • 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
    • 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
    • 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
    • G01S15/00—Systems using the reflection or reradiation of acoustic waves, e.g. sonar systems
    • G01S15/86—Combinations of sonar systems with lidar systems; Combinations of sonar systems with systems not using wave reflection

Abstract

一种机器人防跌落检测方法，由于采用卡尔曼滤波算法进行最优估计，通过采用多次采样、迭代计算更新估计结构，从而消除超声波传感器数据不确定带来的误差，其工作稳定、结果可靠性高。整个控制方法结构简单，算法简明，适合FPGA的并行处理结构及底层设计的特点，运行速度较快，可以实时监测。采用超声波传感器转动扫描的方式，其成本较低，实现简单，可提前检测到台阶并预警，有效防止机器人的跌落。

Description

一种机器人防跌落检测方法
技术领域
本发明涉及机器人跌落检测技术领域，具体涉及一种机器人防跌落检测方法。
背景技术
传统机器人防跌落采用的主要方法为将超声波、红外、激光等测距传感器垂直朝下安装在机器人最前方的底部，朝下测量倒地距离，当探出桌面或台阶时，距离发生变化，判断为即将跌落，停止前进。这种方法需要将传感器安置在探出桌面、台阶的位置，只有已经到达边缘时才能检测出并停止前进，由于传感器不稳定且检测防错余量空间较小，存在因误判而跌落的风险。
发明内容
本发明为了克服以上技术的不足，提供了一种可以提前检测台阶，有效防止机器人跌落的检测方法。
本发明克服其技术问题所采用的技术方案是：
一种机器人防跌落检测方法，包括如下步骤：
a)机器人前端的下方转动安装有超声波传感器，超声波传感器的初始位置垂直向下，驱动装置驱动超声波传感器向上转动90°后再驱动其向下转动至初始位置，并使超声波传感器循环往复运动；
b)超声波传感器转动至相对竖直状态的初始位置小于30°时，超声波传感器检测的台阶边缘的距离为准确距离，当超声波传感器转动至相对竖直状态的初始位置大于30°时，其检测的台阶边缘的距离在100cm-300cm时，检测距离为Sedge；
c)超声波传感器检测到距离台阶边缘距离为100cm-300cm时，机器人内的FPGA芯片以Nms为采样周期采集超声波传感器输出的距离检测值，当Mms内出现十次存在从准确距离到Sedge突变时，记录超声波传感器此时的转动角度θ，记录此时测量的准确距离x1；
d)通过公式S1＝x1×sinθ计算机器人距离台阶边缘的距离S1；
e)通过公式
Figure BDA0002732496280000011
计算位置估计值概率分布S1′，式中N为正态分布，
Figure BDA0002732496280000021
为超声波传感器的位置距离方差；
f)经过时间t，机器人底盘里程计或IMU记录机器人行进距离为vt，其中v为机器人的惯性传感器测量的速度值，通过公式
Figure BDA0002732496280000022
计算得到距离估计值概率S2′，式中
Figure BDA0002732496280000023
为惯性传感器的噪声方差，a为正态分布期望，
Figure BDA0002732496280000024
为方差值；
g)再次使用超声波传感器测量台阶边缘，得到的测量距离为z2，通过公式
Figure BDA0002732496280000025
计算得到距离测量值z2的正态分布Z2′；
h)根据卡尔曼滤波算法，通过公式
Figure BDA0002732496280000026
计算得到当前距离的初步估计概率分布b2；
i)通过公式
Figure BDA0002732496280000027
计算当前位置的最优估计的概率分布c2′；
j)通过公式
Figure BDA0002732496280000028
计算当前距离值的最优估计c2，FPGA芯片以c2为当前距离，以
Figure BDA0002732496280000029
为当前方差对t时刻之后的时刻进行迭代运算并更新超声波传感器测量的与台阶边缘的距离估计结果；
k)当距离估计结果小于跌落阈值k时，机器人停止运动。
进一步的，驱动装置包括设置于机器人内的电机、安装于电机输出轴上的带轮Ⅰ以及同轴连接于超声波传感器上的带轮Ⅱ，带轮Ⅰ与带轮Ⅱ之间通过皮带传动连接。
优选的，步骤a)中超声波传感器的转动时线速度为10cm/s。
优选的，步骤c)中N取值为1，M取值为20。
本发明的有益效果是：由于采用卡尔曼滤波算法进行最优估计，通过采用多次采样、迭代计算更新估计结构，从而消除超声波传感器数据不确定带来的误差，其工作稳定、结果可靠性高。整个控制方法结构简单，算法简明，适合FPGA的并行处理结构及底层设计的特点，运行速度较快，可以实时监测。采用超声波传感器转动扫描的方式，其成本较低，实现简单，可提前检测到台阶并预警，有效防止机器人的跌落。
附图说明
图1为本发明的流程示意图。
具体实施方式
下面结合附图1对本发明做进一步说明。
一种机器人防跌落检测方法，包括如下步骤：
a)机器人前端的下方转动安装有超声波传感器，超声波传感器的初始位置垂直向下，驱动装置驱动超声波传感器向上转动90°后再驱动其向下转动至初始位置，并使超声波传感器循环往复运动。
b)超声波传感器转动至相对竖直状态的初始位置小于30°时，超声波传感器检测的台阶边缘的距离为准确距离，当超声波传感器转动至相对竖直状态的初始位置大于30°时，其检测的台阶边缘的距离在100cm-300cm时，检测距离为Sedge。以距地高度1m桌面，超声波传感器距桌面10cm为例，当机器人行进到靠近桌面边缘，且当超声波传感器转动角度较小(小于30°)时，检测与桌面距离为准确值，大于30°继续转动时，存在三种不确定状态：准确距离(小于20cm)、满量程最大不确定值Smax(接收不到回波检测不出距离，一般为大于1000cm的值)、检测到桌面边缘Sedge(一般为100cm-300cm的值)，三种值发生无规律或有规律不确定互相变化，越靠近桌面边缘变化越频繁，不确定性越大。
c)超声波传感器检测到距离台阶边缘距离为100cm-300cm时，机器人内的FPGA芯片以Nms为采样周期采集超声波传感器输出的距离检测值，当Mms内出现十次存在从准确距离到Sedge突变时，记录超声波传感器此时的转动角度θ，记录此时测量的准确距离x1。机器人的FPGA处理器平台进行数据的实时计算和处理，在FPGA内部采用Verilog硬件描述语言进行数字逻辑设计各功能模块实现功能。
d)通过公式S1＝x1×sinθ计算机器人距离台阶边缘的距离S1。
e)此计算距离由于传感器值不确定性存在较大误差，采样间隔足够小时估计值统计呈近似正态分布，此时通过公式
Figure BDA0002732496280000041
计算位置估计值概率分布S1′，式中N为正态分布，
Figure BDA0002732496280000042
为超声波传感器的位置距离方差，该值可以预先采用超声波传感器进行多次角度测量检测得到。
f)经过时间t，机器人底盘里程计或IMU记录机器人行进距离为vt，其中v为机器人的惯性传感器测量的速度值，通过公式
Figure BDA0002732496280000043
计算得到距离估计值概率S2′，式中
Figure BDA0002732496280000044
为惯性传感器的噪声方差，可以由惯性传感器多次实验测量得到，a为正态分布期望，
Figure BDA0002732496280000045
为方差值。
g)再次使用超声波传感器测量台阶边缘，得到的测量距离为z2，通过公式
Figure BDA0002732496280000046
计算得到距离测量值z2的正态分布Z2′。
h)根据卡尔曼滤波算法，通过公式
Figure BDA0002732496280000047
计算得到当前距离的初步估计概率分布b2。
i)根据卡尔曼滤波算法S2′和Z2′的方差作为对数据的可相信权重，因此通过公式
Figure BDA0002732496280000048
计算当前位置的最优估计的概率分布c2′。
j)通过公式
Figure BDA0002732496280000049
计算当前距离值的最优估计c2，FPGA芯片以c2为当前距离，以
Figure BDA00027324962800000410
为当前方差对t时刻之后的时刻进行迭代运算并更新超声波传感器测量的与台阶边缘的距离估计结果。
k)当距离估计结果小于跌落阈值k时，机器人停止运动，从而实现提前跌落预警，防止机器人跌落。
由于采用卡尔曼滤波算法进行最优估计，通过采用多次采样、迭代计算更新估计结构，从而消除超声波传感器数据不确定带来的误差，其工作稳定、结果可靠性高。整个控制方法结构简单，算法简明，适合FPGA的并行处理结构及底层设计的特点，运行速度较快，可以实时监测。采用超声波传感器转动扫描的方式，其成本较低，实现简单，可提前检测到台阶并预警，有效防止机器人的跌落。进一步的，驱动装置包括设置于机器人内的电机、安装于电机输出轴上的带轮Ⅰ以及同轴连接于超声波传感器上的带轮Ⅱ，带轮Ⅰ与带轮Ⅱ之间通过皮带传动连接。
优选的，步骤a)中超声波传感器的转动时线速度为10cm/s。
优选的，步骤c)中N取值为1，M取值为20。
最后应说明的是：以上所述仅为本发明的优选实施例而已，并不用于限制本发明，尽管参照前述实施例对本发明进行了详细的说明，对于本领域的技术人员来说，其依然可以对前述各实施例所记载的技术方案进行修改，或者对其中部分技术特征进行等同替换。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种机器人防跌落检测方法，其特征在于，包括如下步骤：
a)机器人前端的下方转动安装有超声波传感器，超声波传感器的初始位置垂直向下，驱动装置驱动超声波传感器向上转动90°后再驱动其向下转动至初始位置，并使超声波传感器循环往复运动；
b)超声波传感器转动至相对竖直状态的初始位置小于30°时，超声波传感器检测的台阶边缘的距离为准确距离，当超声波传感器转动至相对竖直状态的初始位置大于30°时，其检测的台阶边缘的距离在100cm-300cm时，检测距离为Sedge；
c)超声波传感器检测到距离台阶边缘距离为100cm-300cm时，机器人内的FPGA芯片以Nms为采样周期采集超声波传感器输出的距离检测值，当Mms内出现十次存在从准确距离到Sedge突变时，记录超声波传感器此时的转动角度θ，记录此时测量的准确距离x1；
d)通过公式S1＝x1×sinθ计算机器人距离台阶边缘的距离S1；
e)通过公式
Figure FDA0002732496270000016
计算位置估计值概率分布S1′，式中N为正态分布，
Figure FDA0002732496270000017
为超声波传感器的位置距离方差；
f)经过时间t，机器人底盘里程计或IMU记录机器人行进距离为vt，其中v为机器人的惯性传感器测量的速度值，通过公式
Figure FDA0002732496270000011
计算得到距离估计值概率S2′，式中
Figure FDA0002732496270000012
为惯性传感器的噪声方差，a为正态分布期望，
Figure FDA0002732496270000013
为方差值；
g)再次使用超声波传感器测量台阶边缘，得到的测量距离为z2，通过公式
Figure FDA0002732496270000014
计算得到距离测量值z2的正态分布Z2′；
h)根据卡尔曼滤波算法，通过公式
Figure FDA0002732496270000015
计算得到当前距离的初步估计概率分布b2；
i)通过公式
Figure FDA0002732496270000021
计算当前位置的最优估计的概率分布c2′；
j)通过公式
Figure FDA0002732496270000022
计算当前距离值的最优估计c2，FPGA芯片以c2为当前距离，以
Figure FDA0002732496270000023
为当前方差对t时刻之后的时刻进行迭代运算并更新超声波传感器测量的与台阶边缘的距离估计结果；
k)当距离估计结果小于跌落阈值k时，机器人停止运动。
2.根据权利要求1所述的机器人防跌落检测方法，其特征在于：驱动装置包括设置于机器人内的电机、安装于电机输出轴上的带轮Ⅰ以及同轴连接于超声波传感器上的带轮Ⅱ，带轮Ⅰ与带轮Ⅱ之间通过皮带传动连接。
3.根据权利要求1所述的机器人防跌落检测方法，其特征在于：步骤a)中超声波传感器的转动时线速度为10cm/s。
4.根据权利要求1所述的机器人防跌落检测方法，其特征在于：步骤c)中N取值为1，M取值为20。
CN202011123094.3A 2020-10-20 2020-10-20 一种机器人防跌落检测方法 Active CN112305547B (zh)

Priority Applications (1)

Application Number Priority Date Filing Date Title
CN202011123094.3A CN112305547B (zh) 2020-10-20 2020-10-20 一种机器人防跌落检测方法

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202011123094.3A CN112305547B (zh) 2020-10-20 2020-10-20 一种机器人防跌落检测方法

Publications (2)

Publication Number Publication Date
CN112305547A CN112305547A (zh) 2021-02-02
CN112305547B true CN112305547B (zh) 2022-05-13

Family

ID=74327992

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202011123094.3A Active CN112305547B (zh) 2020-10-20 2020-10-20 一种机器人防跌落检测方法

Country Status (1)

Country Link
CN (1) CN112305547B (zh)

Families Citing this family (1)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN114699688B (zh) * 2022-05-27 2022-08-12 江苏中创机器人研究院有限公司 一种防护性能好的消防用机器人

Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB201319513D0 (en) * 2010-12-30 2013-12-18 Irobot Corp A method of operating a mobile robot
CN106569489A (zh) * 2015-10-13 2017-04-19 录可系统公司 具有视觉导航功能的扫地机器人及其导航方法
CN107045352A (zh) * 2017-05-31 2017-08-15 珠海市微半导体有限公司 基于多红外的机器人避障装置、其控制方法及机器人沿边控制方法
CN107092259A (zh) * 2017-06-08 2017-08-25 纳恩博（北京）科技有限公司 机器人的防跌落方法和装置
CN109186463A (zh) * 2018-09-04 2019-01-11 浙江梧斯源通信科技股份有限公司 应用于可移动机器人的防跌落方法
CN109602341A (zh) * 2019-01-23 2019-04-12 珠海市微半导体有限公司 一种基于虚拟边界的清洁机器人跌落控制方法及芯片
CN111309015A (zh) * 2020-02-25 2020-06-19 华南理工大学 一种融合多传感器的变电站巡检机器人定位导航系统
CN111531549A (zh) * 2020-06-18 2020-08-14 北京海益同展信息科技有限公司 一种机器人系统及定位导航方法

Family Cites Families (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
KR101620449B1 (ko) * 2014-06-26 2016-05-12 삼성전자 주식회사 로봇 청소기 및 로봇 청소기의 제어방법
US11576543B2 (en) * 2014-07-18 2023-02-14 Ali Ebrahimi Afrouzi Robotic vacuum with rotating cleaning apparatus

Patent Citations (8)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
GB201319513D0 (en) * 2010-12-30 2013-12-18 Irobot Corp A method of operating a mobile robot
CN106569489A (zh) * 2015-10-13 2017-04-19 录可系统公司 具有视觉导航功能的扫地机器人及其导航方法
CN107045352A (zh) * 2017-05-31 2017-08-15 珠海市微半导体有限公司 基于多红外的机器人避障装置、其控制方法及机器人沿边控制方法
CN107092259A (zh) * 2017-06-08 2017-08-25 纳恩博（北京）科技有限公司 机器人的防跌落方法和装置
CN109186463A (zh) * 2018-09-04 2019-01-11 浙江梧斯源通信科技股份有限公司 应用于可移动机器人的防跌落方法
CN109602341A (zh) * 2019-01-23 2019-04-12 珠海市微半导体有限公司 一种基于虚拟边界的清洁机器人跌落控制方法及芯片
CN111309015A (zh) * 2020-02-25 2020-06-19 华南理工大学 一种融合多传感器的变电站巡检机器人定位导航系统
CN111531549A (zh) * 2020-06-18 2020-08-14 北京海益同展信息科技有限公司 一种机器人系统及定位导航方法

Non-Patent Citations (4)

* Cited by examiner, † Cited by third party
Title
Development of gait measurement robot for prevention of falls in the elderly;Ayanori Yorozu et al.;《 2014 11th International Conference on Informatics in Control, Automation and Robotics (ICINCO)》;20140903;第1-9页 *
Fall Prevention and Vibration Suppression of Wheelchair Using Rider Motion State;Isseki Takahashi et al.;《2018 International Power Electronics Conference (IPEC-Niigata 2018 -ECCE Asia)》;20180524;第575-582页 *
一种六足仿生机器人的避障和防跌落功能设计;王廷毓 等;《电子世界》;20180423;第148-149页 *
楼梯清洁机器人防跌落防碰撞研究;张磊 等;《机械设计与制造》;20181031;第240-244页 *

Also Published As

Publication number Publication date
CN112305547A (zh) 2021-02-02

Similar Documents

Publication Publication Date Title
US9804598B2 (en) Autonomous mobile body
CN112305547B (zh) 一种机器人防跌落检测方法
WO2019052285A1 (zh) 机器人打滑的检测方法和建图方法及芯片
US8261609B2 (en) Aerodynamic measurement probe and helicopter equipped with the probe
KR101338143B1 (ko) 이동 로봇의 슬립 감지 장치 및 방법
CN109782325A (zh) 基于粒子滤波和多传感器信息融合的列车速度估计方法
US20190243379A1 (en) Navigation of robotic pool cleaner
CN109506652B (zh) 一种基于地毯偏移的光流数据融合方法及清洁机器人
EP3894883A1 (en) Vehicle odometry and motion direction determination
CN109444894B (zh) 道路积水深度快速移动测量装置及方法
CN102818556A (zh) 车载道路坡度检测方法及装置
CN110133667B (zh) 基于移动前视声呐的水下三维检测系统
JP2019057185A (ja) 飛行装置、飛行方法及びプログラム
CN108937701B (zh) 擦窗机器人及其状态检测方法
CN112050808B (zh) 一种水下无人航行器浮出水面检测方法
CN111055877B (zh) 一种宽温范围智能铁鞋及其防溜状态判断方法
CN107884593B (zh) 一种以拖拉机为动力的农业机具速度监测系统与方法
CN113138398A (zh) 一种基于无人机大气激光雷达的气溶胶消光系数反演方法
CN109738906A (zh) 一种基于多帧匹配的激光雷达式车辆宽度测量方法
CN109752728A (zh) 一种基于多帧平均的激光雷达式车辆宽度测量方法
CN110353570A (zh) 自主行动装置及其回避方法
KR20080041890A (ko) 로봇청소기의 외력감지방법, 이를 기록한 기록매체 및 이를이용한 로봇 청소기
CN113341171A (zh) 一种具有低延迟特性的列车测速降噪滤波方法及装置
KR20180085154A (ko) 로봇청소기
US20170276482A1 (en) Multi-sensor target location registration

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
TA01 Transfer of patent application right

Effective date of registration: 20220419

Address after: 250104 floors 11-12, building 3, future entrepreneurship Plaza, north section of Gangxing Third Road, high tech Zone, Jinan City, Shandong Province

Applicant after: Shandong new generation Information Industry Technology Research Institute Co.,Ltd.

Address before: 250104 1st floor, R & D building, No. 2877, Suncun Town, Licheng District, Jinan City, Shandong Province

Applicant before: JINAN INSPUR HIGH-TECH TECHNOLOGY DEVELOPMENT Co.,Ltd.

TA01 Transfer of patent application right
GR01 Patent grant
GR01 Patent grant