(19)中华人民共和国国家知识产权局
(12)发明专利申请
(10)申请公布号 (43)申请公布日 (21)申请号 202011123094.3
(22)申请日 2020.10.20
(71)申请人 济南浪潮高新科技投资发展有限公
司
地址 250104 山东省济南市历城区孙村镇
科航路2877号研发楼一楼
(72)发明人 崔欣 高明 马辰
(74)专利代理机构 济南泉城专利商标事务所
37218
代理人 李桂存
(51)Int.Cl.
G01S 15/88(2006.01)
G01S 15/86(2020.01)
G01C 21/16(2006.01)
G01C 21/20(2006.01)
(54)发明名称
(57)摘要
一种机器人防跌落检测方法，由于采用卡尔
曼滤波算法进行最优估计，通过采用多次采样、
数据不确定带来的误差，其工作稳定、结果可靠
性高。整个控制方法结构简单，算法简明，适合
FPGA的并行处理结构及底层设计的特点，运行速
度较快，可以实时监测。采用超声波传感器转动
扫描的方式，其成本较低，实现简单，可提前检测
到台阶并预警，
有效防止机器人的跌落。权利要求书2页 说明书4页 附图1页CN 112305547 A 2021.02.02
C N 112305547
A
1.一种机器人防跌落检测方法，其特征在于，包括如下步骤：
a)机器人前端的下方转动安装有超声波传感器，超声波传感器的初始位置垂直向下，驱动装置驱动超声波传感器向上转动90°后再驱动其向下转动至初始位置，并使超声波传感器循环往复运动；
b)超声波传感器转动至相对竖直状态的初始位置小于30°时，超声波传感器检测的台阶边缘的距离为准确距离，当超声波传感器转动至相对竖直状态的初始位置大于30°时，其检测的台阶边缘的距离在100cm-300cm时，检测距离为Sedge； c)超声波传感器检测到距离台阶边缘距离为100cm-300cm时，机器人内的FPGA芯片以Nms为采样周期采集超声波传感器输出的距离检测值，当Mms内出现十次存在从准确距离到Sedge突变时，记录超声波传感器此时的转动角度θ，记录此时测量的准确距离x1；
d)通过公式S1＝x1×sinθ计算机器人距离台阶边缘的距离S1；
e)通过公式计算位置估计值概率分布S1′，式中N为正态分布，为超
声波传感器的位置距离方差；
f)经过时间t，机器人底盘里程计或IMU记录机器人行进距离为vt，其中v为机器人的惯
性传感器测量的速度值，通过公式计算得到距离估计值概率S2′，式中为惯性传感器的噪声方差，a为正态分布期望，为方差值；
g)再次使用超声波传感器测量台阶边缘，得到的测量距离为z2，通过公式
计算得到距离测量值z2的正态分布Z2′；
h)根据卡尔曼滤波算法，通过公式计算得到当前距离的初步估计概率分布b2；
i)通过公式计算当前位置的最
优估计的概率分布c2′；
j)通过公式计算当前距离值的最优估计c2，
FPGA芯片以c2为当前距离，以为当前方差对t时刻之后的时刻进行迭代运算并
更新超声波传感器测量的与台阶边缘的距离估计结果；
k)当距离估计结果小于跌落阈值k时，机器人停止运动。
2.根据权利要求1所述的机器人防跌落检测方法，其特征在于：驱动装置包括设置于机器人内的电机、安装于电机输出轴上的带轮Ⅰ以及同轴连接于超声波传感器上的带轮Ⅱ，带轮Ⅰ与带轮Ⅱ之间通过皮带传动连接。
3.根据权利要求1所述的机器人防跌落检测方法，其特征在于：步骤a)中超声波传感器的转动时线速度为10cm/s。
4.根据权利要求1所述的机器人防跌落检测方法，其特征在于：步骤c)中N取值为1，M取值为20。
一种机器人防跌落检测方法
技术领域
[0001]本发明涉及机器人跌落检测技术领域，具体涉及一种机器人防跌落检测方法。
背景技术
[0002]传统机器人防跌落采用的主要方法为将超声波、红外、激光等测距传感器垂直朝下安装在机器人最前方的底部，朝下测量倒地距离，当探出桌面或台阶时，距离发生变化，判断为即将跌落，停止前进。这种方法需要将传感器安置在探出桌面、台阶的位置，只有已经到达边缘时才能检测出并停止前进，由于传感器不稳定且检测防错余量空间较小，存在因误判而跌落的风险。
发明内容
[0003]本发明为了克服以上技术的不足，提供了一种可以提前检测台阶，有效防止机器人跌落的检测方法。
[0004]本发明克服其技术问题所采用的技术方案是：
[0005]一种机器人防跌落检测方法，包括如下步骤：
[0006]a)机器人前端的下方转动安装有超声波传感器，超声波传感器的初始位置垂直向下，驱动装置驱动超声波传感器向上转动90°后再驱动其向下转动至初始位置，并使超声波传感器循环往复运动；
[0007]b)超声波传感器转动至相对竖直状态的初始位置小于30°时，超声波传感器检测的台阶边缘的距离为准确距离，当超声波传感器转动至相对竖直状态的初始位置大于30°时，其检测的台阶边缘的距离在100cm-300cm时，检测距离为Sedge；
[0008]c)超声波传感器检测到距离台阶边缘距离为100cm-300cm时，机器人内的FPGA芯片以Nms为采样周期采集超声波传感器输出的距离检测值，当Mms内出现十次存在从准确距离到Sedge突变时，记录超声波传感器此时的转动角度θ，记录此时测量的准确距离x1；[0009]d)通过公式S1＝x1×sinθ计算机器人距离台阶边缘的距离S1；
[0010]e)通过公式计算位置估计值概率分布S1′，式中N为正态分布，
为超声波传感器的位置距离方差；
[0011]f)经过时间t，机器人底盘里程计或IMU记录机器人行进距离为vt，其中v为机器人
的惯性传感器测量的速度值，通过公式计算得到距
离估计值概率S2′，式中为惯性传感器的噪声方差，a为正态分布期望，为方差值；[0012]g)再次使用超声波传感器测量台阶边缘，得到的测量距离为z2，通过公式
计算得到距离测量值z2的正态分布Z2′；
[0013]h)根据卡尔曼滤波算法，通过公式
计算得到当前距离的初步估计概率分布b2；
[0014]i)通过公式计算当前位置的最优估计的概率分布c2′；
[0015]j)通过公式计算当前距离值的最优估计
c2，FPGA芯片以c2为当前距离，以为当前方差对t时刻之后的时刻进行迭代运算
并更新超声波传感器测量的与台阶边缘的距离估计结果；
[0016]k)当距离估计结果小于跌落阈值k时，机器人停止运动。
[0017]进一步的，驱动装置包括设置于机器人内的电机、安装于电机输出轴上的带轮Ⅰ以及同轴连接于超声波传感器上的带轮Ⅱ，带轮Ⅰ与带轮Ⅱ之间通过皮带传动连接。[0018]优选的，步骤a)中超声波传感器的转动时线速度为10cm/s。
[0019]优选的，步骤c)中N取值为1，M取值为20。
[0020]本发明的有益效果是：由于采用卡尔曼滤波算法进行最优估计，通过采用多次采样、迭代计算更新估计结构，从而消除超声波传感器数据不确定带来的误差，其工作稳定、结果可靠性高。整个控制方法结构简单，算法简明，适合FPGA的并行处理结构及底层设计的特点，运行速度较快，可以实时监测。采用超声波传感器转动扫描的方式，其成本较低，实现简单，可提前检测到台阶并预警，有效防止机器人的跌落。
附图说明
[0021]图1为本发明的流程示意图。
具体实施方式
[0022]下面结合附图1对本发明做进一步说明。
[0023]一种机器人防跌落检测方法，包括如下步骤：
[0024]a)机器人前端的下方转动安装有超声波传感器，超声波传感器的初始位置垂直向下，驱动装置驱动超声波传感器向上转动90°后再驱动其向下转动至初始位置，并使超声波传感器循环往复运动。
[0025]b)超声波传感器转动至相对竖直状态的初始位置小于30°时，超声波传感器检测的台阶边缘的距离
为准确距离，当超声波传感器转动至相对竖直状态的初始位置大于30°时，其检测的台阶边缘的距离在100cm-300cm时，检测距离为Sedge。以距地高度1m桌面，超声波传感器距桌面10cm为例，当机器人行进到靠近桌面边缘，且当超声波传感器转动角度较小(小于30°)时，检测与桌面距离为准确值，大于30°继续转动时，存在三种不确定状态：准确距离(小于20cm)、满量程最大不确定值Smax(接收不到回波检测不出距离，一般为大于1000cm的值)、检测到桌面边缘Sedge(一般为100cm-300cm的值)，三种值发生无规律或有规
豫公网安备41160202000603
站长QQ:729038198
关于我们
投诉建议