CN112478015B - 四足机器人足端触地检测方法及*** - Google Patents

四足机器人足端触地检测方法及*** Download PDF

Info

Publication number
CN112478015B
CN112478015B CN202110143895.4A CN202110143895A CN112478015B CN 112478015 B CN112478015 B CN 112478015B CN 202110143895 A CN202110143895 A CN 202110143895A CN 112478015 B CN112478015 B CN 112478015B
Authority
CN
China
Prior art keywords
contact probability
foot
leg
foot end
knee joint
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
CN202110143895.4A
Other languages
English (en)
Other versions
CN112478015A (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.)
Delu Power Technology Chengdu Co Ltd
Original Assignee
Delu Power Technology Chengdu 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 Delu Power Technology Chengdu Co Ltd filed Critical Delu Power Technology Chengdu Co Ltd
Priority to CN202110143895.4A priority Critical patent/CN112478015B/zh
Publication of CN112478015A publication Critical patent/CN112478015A/zh
Application granted granted Critical
Publication of CN112478015B publication Critical patent/CN112478015B/zh
Priority to US17/540,168 priority patent/US20220241992A1/en
Active legal-status Critical Current
Anticipated expiration legal-status Critical

Links

Images

Classifications

    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J19/00—Accessories fitted to manipulators, e.g. for monitoring, for viewing; Safety devices combined with or specially adapted for use in connection with manipulators
    • B25J19/02—Sensing devices
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06V—IMAGE OR VIDEO RECOGNITION OR UNDERSTANDING
    • G06V40/00—Recognition of biometric, human-related or animal-related patterns in image or video data
    • G06V40/20—Movements or behaviour, e.g. gesture recognition
    • G06V40/23—Recognition of whole body movements, e.g. for sport training
    • G06V40/25—Recognition of walking or running movements, e.g. gait recognition
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/0006—Exoskeletons, i.e. resembling a human figure
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B25—HAND TOOLS; PORTABLE POWER-DRIVEN TOOLS; MANIPULATORS
    • B25J—MANIPULATORS; CHAMBERS PROVIDED WITH MANIPULATION DEVICES
    • B25J9/00—Programme-controlled manipulators
    • B25J9/16—Programme controls
    • B25J9/1656—Programme controls characterised by programming, planning systems for manipulators
    • B25J9/1669—Programme controls characterised by programming, planning systems for manipulators characterised by special application, e.g. multi-arm co-operation, assembly, grasping
    • B—PERFORMING OPERATIONS; TRANSPORTING
    • B62—LAND VEHICLES FOR TRAVELLING OTHERWISE THAN ON RAILS
    • B62D—MOTOR VEHICLES; TRAILERS
    • B62D57/00—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track
    • B62D57/02—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members
    • B62D57/032—Vehicles characterised by having other propulsion or other ground- engaging means than wheels or endless track, alone or in addition to wheels or endless track with ground-engaging propulsion means, e.g. walking members with alternately or sequentially lifted supporting base and legs; with alternately or sequentially lifted feet or skid
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/11—Complex mathematical operations for solving equations, e.g. nonlinear equations, general mathematical optimization problems
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F17/00—Digital computing or data processing equipment or methods, specially adapted for specific functions
    • G06F17/10—Complex mathematical operations
    • G06F17/16—Matrix or vector computation, e.g. matrix-matrix or matrix-vector multiplication, matrix factorization
    • G—PHYSICS
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06F—ELECTRIC DIGITAL DATA PROCESSING
    • G06F18/00—Pattern recognition
    • G06F18/20—Analysing
    • G06F18/24—Classification techniques
    • G06F18/241—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches
    • G06F18/2415—Classification techniques relating to the classification model, e.g. parametric or non-parametric approaches based on parametric or probabilistic models, e.g. based on likelihood ratio or false acceptance rate versus a false rejection rate
    • 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
    • G06—COMPUTING; CALCULATING OR COUNTING
    • G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
    • G06T7/00—Image analysis
    • G06T7/20—Analysis of motion
    • G06T7/277—Analysis of motion involving stochastic approaches, e.g. using Kalman filters

Landscapes

  • Engineering & Computer Science (AREA)
  • Physics & Mathematics (AREA)
  • Theoretical Computer Science (AREA)
  • General Physics & Mathematics (AREA)
  • Data Mining & Analysis (AREA)
  • Mathematical Physics (AREA)
  • Mechanical Engineering (AREA)
  • Mathematical Optimization (AREA)
  • Mathematical Analysis (AREA)
  • Computational Mathematics (AREA)
  • General Engineering & Computer Science (AREA)
  • Pure & Applied Mathematics (AREA)
  • Robotics (AREA)
  • Computer Vision & Pattern Recognition (AREA)
  • Transportation (AREA)
  • Combustion & Propulsion (AREA)
  • Life Sciences & Earth Sciences (AREA)
  • Chemical & Material Sciences (AREA)
  • Artificial Intelligence (AREA)
  • Software Systems (AREA)
  • Databases & Information Systems (AREA)
  • Algebra (AREA)
  • Bioinformatics & Cheminformatics (AREA)
  • Multimedia (AREA)
  • Bioinformatics & Computational Biology (AREA)
  • Evolutionary Computation (AREA)
  • Evolutionary Biology (AREA)
  • Probability & Statistics with Applications (AREA)
  • Operations Research (AREA)
  • Human Computer Interaction (AREA)
  • Social Psychology (AREA)
  • Psychiatry (AREA)
  • General Health & Medical Sciences (AREA)
  • Health & Medical Sciences (AREA)
  • Computing Systems (AREA)
  • Manipulator (AREA)

Abstract

本发明涉及四足机器人足端触地检测方法及***，包括以下步骤：S1，计算每条腿的足端离地高度，根据每条腿的足端离地高度计算出其足端高度接触概率；根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；S2，根据四条腿的步态规划接触概率、足端高度接触概率、膝关节扭矩接触概率，利用卡尔曼滤波融合得到最终的四腿足端接触概率；根据所述四腿足端接触概率，判断足端触地状态。本发明在不部署多余的传感器在足端的前提下，依靠四足机器人本体基础传感器和融合算法精确估计足端与地面之间的接触状态，可解决传感器布线难题；本发明即使在崎岖地形或阶跃地形，依然可得到可靠的触地状态。

Description

四足机器人足端触地检测方法及***
技术领域
本发明涉及机器人技术领域，尤其涉及四足机器人足端触地检测方法及***。
背景技术
目前，四足机器人足端触地状态检测主要有以下三种方式：
一、根据步态规划判断当前时刻足端触地状态；
二、在足端部署布尔量传感器，如接触开关等，判断足端触地状态；
三、在足端部署气压或者压力传感器，检测足端与地面之间压力的模拟量，通过模拟量的大小判断足端接触状态。
上述三种方式各自具有以下缺陷：
方式一：要求地形平整，如果在崎岖地形或阶跃地形，根据规划得到的触地状态不可靠；
方式二：传感器安装困难，使用寿命低，长期使用不可靠；
方式三：传感器部署在足端，电气走线困难，传感器本身对机械结构、材料要求较高，整体部署成本高。
发明内容
本发明为了解决上述技术问题提供四足机器人足端触地检测方法及***。
本发明通过下述技术方案实现：
四足机器人足端触地检测方法，包括以下步骤：
S1，计算每条腿的足端离地高度，根据每条腿的足端离地高度计算出其足端高度接触概率；
根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
根据步态相位计算出步态规划接触概率；
S2，根据四条腿的步态规划接触概率、足端高度接触概率、膝关节扭矩接触概率，利用卡尔曼滤波融合得到最终的四腿足端接触概率；根据所述四腿足端接触概率，判断足端触地状态。
进一步的，采用公式（1）计算足端高度接触概率
Figure 944470DEST_PATH_IMAGE002
：
Figure 383673DEST_PATH_IMAGE004
（1）
式（1）中，
Figure 567529DEST_PATH_IMAGE006
表示足端离地高度，
Figure 254775DEST_PATH_IMAGE008
为接触时足端离地高度均值、
Figure 738846DEST_PATH_IMAGE010
为足端离地高度的方差。
进一步的，采用公式（2）计算膝关节扭矩接触概率
Figure 614529DEST_PATH_IMAGE012
：
Figure 66108DEST_PATH_IMAGE014
（2）
式（2）中，
Figure 301917DEST_PATH_IMAGE016
表示膝关节扭矩，
Figure 125648DEST_PATH_IMAGE018
为接触时膝关节扭矩均值、
Figure 421500DEST_PATH_IMAGE020
为膝关节扭矩的方差。
进一步的，四足机器人的步态是周期变化的，在每一个步态周期中步态相位在0-1之间递增，每个步态周期按腿部运动状态分为接触阶段和摆动阶段；
0≤步态相位<
Figure 94796DEST_PATH_IMAGE022
为接触阶段，
Figure 885028DEST_PATH_IMAGE024
步态相位
Figure 343691DEST_PATH_IMAGE026
1为摆动阶段；
在规划接触阶段，采用公式（3）计算步态规划接触概率
Figure 59712DEST_PATH_IMAGE028
：
Figure 987348DEST_PATH_IMAGE030
（3）
在规划摆动阶段，采用公式（4）计算步态规划接触概率
Figure 96118DEST_PATH_IMAGE028
：
Figure 387117DEST_PATH_IMAGE032
（4）
式（3）、式（4）中，
Figure 41083DEST_PATH_IMAGE034
表示当前时刻在步态周期中的相位，
Figure 439704DEST_PATH_IMAGE022
表示支撑相和摆动相转换时刻的相位，
Figure 70274DEST_PATH_IMAGE036
表示步态相位的方差。
进一步的，采用递推公式（7）计算最终的四腿足端接触概率：
Figure 254262DEST_PATH_IMAGE038
（7）
其中，
Figure 843244DEST_PATH_IMAGE040
，
Figure 729160DEST_PATH_IMAGE042
；
Figure 664886DEST_PATH_IMAGE044
，
Figure 201916DEST_PATH_IMAGE046
，
Figure 446952DEST_PATH_IMAGE048
；
Figure 570897DEST_PATH_IMAGE050
，
Figure 814709DEST_PATH_IMAGE052
；
Figure 973289DEST_PATH_IMAGE054
，
Figure 858068DEST_PATH_IMAGE056
；
式（7）中，
Figure 436686DEST_PATH_IMAGE058
表示状态转移过程噪声的协方差矩阵，
Figure 963482DEST_PATH_IMAGE060
表示观测噪声的协方差矩阵；状态转移矩阵
Figure 445410DEST_PATH_IMAGE062
，控制输入矩阵
Figure 766670DEST_PATH_IMAGE064
，
Figure 832584DEST_PATH_IMAGE066
表示状态转移过程噪声，
Figure 648225DEST_PATH_IMAGE068
为通过足端高度接触概率模型计算得到的四腿足端接触概率，
Figure 233927DEST_PATH_IMAGE070
为通过膝关节扭矩接触概率模型计算得到的四腿足端接触概率，
Figure 975356DEST_PATH_IMAGE072
为观测矩阵，
Figure 810456DEST_PATH_IMAGE074
表示观测噪声；
Figure 633050DEST_PATH_IMAGE076
、
Figure 604417DEST_PATH_IMAGE078
、
Figure 268746DEST_PATH_IMAGE080
、
Figure 59985DEST_PATH_IMAGE082
为四条腿的步态规划接触概率；
Figure 217427DEST_PATH_IMAGE084
、
Figure 512143DEST_PATH_IMAGE086
、
Figure 860953DEST_PATH_IMAGE088
、
Figure 873909DEST_PATH_IMAGE090
为四条腿的足端接触概率；
Figure 569463DEST_PATH_IMAGE092
、
Figure 984264DEST_PATH_IMAGE094
、
Figure 238397DEST_PATH_IMAGE096
、
Figure 738648DEST_PATH_IMAGE098
为四条腿的膝关节扭矩接触概率；
Figure 237894DEST_PATH_IMAGE100
、
Figure 287627DEST_PATH_IMAGE102
、
Figure 728973DEST_PATH_IMAGE104
、
Figure 201674DEST_PATH_IMAGE106
为最终输出的四腿足端接触概率；
Figure 753878DEST_PATH_IMAGE108
：
Figure 398398DEST_PATH_IMAGE110
时刻的后验状态估计值；
Figure 745065DEST_PATH_IMAGE112
：
Figure 236221DEST_PATH_IMAGE114
时刻的后验估计值，为滤波器的输出；
Figure 575804DEST_PATH_IMAGE116
：
Figure 85283DEST_PATH_IMAGE114
时刻的先验状态估计值，为滤波器的中间计算结果；
Figure 88005DEST_PATH_IMAGE118
：
Figure 315724DEST_PATH_IMAGE114
时刻滤波器的输入；
Figure 927840DEST_PATH_IMAGE120
：
Figure 557404DEST_PATH_IMAGE114
时刻的先验估计协方差矩阵；
Figure 465448DEST_PATH_IMAGE122
：
Figure 914884DEST_PATH_IMAGE110
时刻的后验估计协方差矩阵；
Figure 330691DEST_PATH_IMAGE124
：
Figure 18024DEST_PATH_IMAGE114
时刻后验估计协方差矩阵；
Figure 877396DEST_PATH_IMAGE126
：卡尔曼滤波增益；
Figure 33702DEST_PATH_IMAGE128
：足端高度接触概率模型的观测噪声；
Figure 269511DEST_PATH_IMAGE130
：膝关节扭矩接触概率模型的观测噪声；
Figure 320338DEST_PATH_IMAGE132
：表示4×4的零矩阵；
Figure 350611DEST_PATH_IMAGE134
：表示4×4的单位矩阵；
Figure 994213DEST_PATH_IMAGE136
：表示单位矩阵。
其中，通过惯性传感器进行姿态解算得到机器人的质心位置和身体姿态，结合腿部电机编码器反馈的电机位置，计算得到每条腿的足端离地高度。
四足机器人本体足端触地检测***，包括测量单元、输入单元和卡尔曼滤波单元；测量单元包括足端高度测量模块、足端高度接触概率测量模块和膝关节扭矩接触概率测量模块；输入单元包括步态规划接触概率计算模块；
足端高度测量模块：负责计算每条腿的足端离地高度；
足端高度接触概率测量模块：负责根据每条腿的足端离地高度计算出其足端高度接触概率；
膝关节扭矩接触概率测量模块：负责根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
步态规划接触概率计算模块：负责计算每条腿的步态规划接触概率；
卡尔曼滤波单元：负责根据输入的步态规划接触概率以及测量的足端高度接触概率、膝关节扭矩接触概率，计算并输出最终的足端触地概率。
与现有技术相比，本发明具有以下有益效果：
1，本发明在不部署多余的传感器在足端的前提下，依靠四足机器人本体基础传感器和融合算法精确估计足端与地面之间的接触状态，可解决传感器布线难题；
2，本发明即使在崎岖地形或阶跃地形，依然可得到可靠的触地状态。
附图说明
此处所说明的附图用来提供对本发明实施例的进一步理解，构成本申请的一部分，并不构成对本发明实施例的限定。
图1是本发明的电气原理图；
图2是本发明的流程图；
图3是步态规划接触概率模型的原理图。
具体实施方式
为使本发明的目的、技术方案和优点更加清楚明白，下面结合实施例和附图，对本发明作进一步的详细说明，本发明的示意性实施方式及其说明仅用于解释本发明，并不作为对本发明的限定。
本发明公开的四足机器人足端触地检测方法，包括以下步骤：
S1，计算每条腿的足端离地高度，根据每条腿的足端离地高度计算出其足端高度接触概率；
根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
根据步态相位计算出步态规划接触概率；
S2，根据四条腿的步态规划接触概率、足端高度接触概率、膝关节扭矩接触概率，利用卡尔曼滤波融合得到最终的四腿足端接触概率；根据所述四腿足端接触概率，判断足端触地状态。
本发明公开的四足机器人本体足端触地检测***，包括测量单元、输入单元和卡尔曼滤波单元；测量单元包括足端高度测量模块、足端高度接触概率测量模块和膝关节扭矩接触概率测量模块；输入单元包括步态规划接触概率计算模块；
足端高度测量模块：负责计算每条腿的足端离地高度；
足端高度接触概率测量模块：负责根据每条腿的足端离地高度计算出其足端高度接触概率；
膝关节扭矩接触概率测量模块：负责根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
步态规划接触概率计算模块：负责计算每条腿的步态规划接触概率；
卡尔曼滤波单元：负责根据输入的步态规划接触概率以及测量的足端高度接触概率、膝关节扭矩接触概率，计算并输出最终的足端触地概率。
基于上述四足机器人足端触地检测方法及***，本发明公开一实施例。
实施例1
如图1所示，本实施例中四足机器人每条腿三个自由度，三个电机挂在同一条CAN总线上，可反馈电机位置、转速、扭矩等数据。数据转换模块负责CAN总线数据和RS485总线数据转换；主控单元通过RS485总线与数据转换模块进行数据通信。主控单元可选用通用主控计算机。
结合图2详细介绍四足机器人足端触地检测方法。
一、运动学模型：通过惯性传感器（IMU）进行姿态解算得到的质心位置和身体姿态，结合电机编码器反馈的电机位置，计算得到每条腿的足端离地高度分别为：
Figure 768134DEST_PATH_IMAGE138
。
二、假设足端触地时，足端离地高度服从均值为
Figure 944907DEST_PATH_IMAGE140
、方差为
Figure 880502DEST_PATH_IMAGE142
的高斯分布，则足端高度接触概率模型为式（1）：
Figure 526247DEST_PATH_IMAGE004
（1）
式（1）中，
Figure 323432DEST_PATH_IMAGE006
表示足端离地高度，
Figure 105444DEST_PATH_IMAGE002
表示在足端高度为
Figure 726787DEST_PATH_IMAGE006
时的接触条件概率。
三、假设足端触地时，膝关节扭矩服从均值为
Figure 594249DEST_PATH_IMAGE018
、方差为
Figure 460705DEST_PATH_IMAGE020
的高斯分布，则膝关节扭矩接触概率模型为式（2）：
Figure 362801DEST_PATH_IMAGE014
（2）
式（2）中，
Figure 155046DEST_PATH_IMAGE016
表示膝关节扭矩，
Figure 244225DEST_PATH_IMAGE012
表示在膝关节扭矩为
Figure 163639DEST_PATH_IMAGE016
时的接触条件概率。
四、步态规划接触概率模型
四足机器人的步态是周期变化的并事先规划好的，在每一个步态周期中步态相位从0-1之间递增。每个步态周期按腿部运动状态可以分为接触阶段和摆动阶段。0≤步态相位<
Figure 670975DEST_PATH_IMAGE022
为接触阶段，
Figure 384853DEST_PATH_IMAGE024
步态相位
Figure 747613DEST_PATH_IMAGE026
1为摆动阶段；
Figure 205139DEST_PATH_IMAGE022
表示支撑相和摆动相转换时刻的相位。
例如，最常见的对角步态，0≤步态相位<0.5为接触阶段，0.5≤步态相位<1为摆动阶段。
步态相位越接近接触相中点其触地概率越大，反之越接近接触相中点其触地概率越小，如图3所示。
4.1 规划接触阶段，步态规划接触概率模型为式（3）：
Figure 301403DEST_PATH_IMAGE030
（3）
4.2 规划摆动阶段，步态规划接触概率模型为式（4）：
Figure 451761DEST_PATH_IMAGE032
（4）
式（3）、式（4）中，
Figure 30379DEST_PATH_IMAGE034
表示当前时刻在步态周期中的相位，
Figure 26017DEST_PATH_IMAGE022
表示支撑相和摆动相转换时刻的相位，
Figure 242366DEST_PATH_IMAGE036
表示步态相位的方差。
五、将步态规划接触概率作为***输入，将足端高度接触概率和膝关节扭矩接触概率作为***测量，利用卡尔曼滤波融合得到最终的四腿足端触地概率，具体计算方法如下：
1，卡尔曼滤波状态转移方程如式（5）所示：
Figure 298046DEST_PATH_IMAGE040
（5）
Figure 849113DEST_PATH_IMAGE050
，
Figure 632130DEST_PATH_IMAGE052
；
式（5）中，状态变量
Figure 217833DEST_PATH_IMAGE112
表示最终输出的四腿足端接触概率；状态转移矩阵
Figure 195147DEST_PATH_IMAGE062
；控制输入矩阵
Figure 233510DEST_PATH_IMAGE064
，
Figure 85797DEST_PATH_IMAGE066
表示状态转移过程噪声，可认为其服从高斯分布；
Figure 526006DEST_PATH_IMAGE118
：
Figure 657910DEST_PATH_IMAGE114
时刻滤波器的输入；
Figure 199881DEST_PATH_IMAGE132
：表示4×4的零矩阵；
Figure 75433DEST_PATH_IMAGE134
：表示4×4的单位矩阵；
Figure 902573DEST_PATH_IMAGE076
、
Figure 205378DEST_PATH_IMAGE078
、
Figure 969066DEST_PATH_IMAGE080
、
Figure 913888DEST_PATH_IMAGE082
为四条腿的步态规划接触概率；
Figure 63110DEST_PATH_IMAGE100
、
Figure 317243DEST_PATH_IMAGE102
、
Figure 551915DEST_PATH_IMAGE104
、
Figure 785581DEST_PATH_IMAGE106
为最终输出的四腿足端接触概率。
2，卡尔曼滤波观测方程如式（6）所示：
Figure 320468DEST_PATH_IMAGE042
（6）
Figure DEST_PATH_IMAGE144
，
Figure DEST_PATH_IMAGE146
，
Figure DEST_PATH_IMAGE148
；
Figure 886447DEST_PATH_IMAGE054
，
Figure 123262DEST_PATH_IMAGE056
；
式（6）中，
Figure 144308DEST_PATH_IMAGE068
为通过足端高度接触概率模型计算得到的四腿足端接触概率；
Figure 533701DEST_PATH_IMAGE070
为通过膝关节扭矩接触概率模型计算得到的四腿足端接触概率；
Figure 631101DEST_PATH_IMAGE072
为观测矩阵；
Figure 105945DEST_PATH_IMAGE074
表示观测噪声，可认为其服从高斯分布；
Figure 451388DEST_PATH_IMAGE134
：表示4×4的单位矩阵；
Figure 446020DEST_PATH_IMAGE128
：足端高度接触概率模型的观测噪声；
Figure 698009DEST_PATH_IMAGE130
：膝关节扭矩接触概率模型的观测噪声；
Figure DEST_PATH_IMAGE150
、
Figure 237313DEST_PATH_IMAGE086
、
Figure 616473DEST_PATH_IMAGE088
、
Figure 714879DEST_PATH_IMAGE090
为四条腿的足端接触概率；
Figure 137770DEST_PATH_IMAGE092
、
Figure 836473DEST_PATH_IMAGE094
、
Figure 737433DEST_PATH_IMAGE096
、
Figure 441078DEST_PATH_IMAGE098
为四条腿的膝关节扭矩接触概率。
3，根据卡尔曼滤波状态转移方程（5）和观测方程（6）可得到卡尔曼滤波的递推公式（7）：
Figure 300450DEST_PATH_IMAGE038
（7）
（7）中，
Figure DEST_PATH_IMAGE152
表示状态转移过程噪声的协方差矩阵，
Figure 752029DEST_PATH_IMAGE060
表示观测噪声的协方差矩阵；
Figure 987838DEST_PATH_IMAGE108
：
Figure 545989DEST_PATH_IMAGE110
时刻的后验状态估计值；
Figure 576262DEST_PATH_IMAGE112
：
Figure 712540DEST_PATH_IMAGE114
时刻的后验估计值，为滤波器的输出；
Figure 955302DEST_PATH_IMAGE116
：
Figure 882807DEST_PATH_IMAGE114
时刻的先验状态估计值，为滤波器的中间计算结果；
Figure 834714DEST_PATH_IMAGE120
：
Figure 480459DEST_PATH_IMAGE114
时刻的先验估计协方差矩阵；
Figure 41759DEST_PATH_IMAGE122
：
Figure 823770DEST_PATH_IMAGE110
时刻的后验估计协方差矩阵；
Figure 680999DEST_PATH_IMAGE124
：
Figure 814040DEST_PATH_IMAGE114
时刻后验估计协方差矩阵；
Figure 913452DEST_PATH_IMAGE126
：卡尔曼滤波增益；
Figure 815549DEST_PATH_IMAGE136
：表示单位矩阵。
4，采用公式（7）计算出最终的四腿足端接触概率
Figure DEST_PATH_IMAGE154
，如果
Figure 905996DEST_PATH_IMAGE154
大于阈值，则认为触地；否则认为不触地。
本发明在不部署多余的传感器在足端的前提下，依靠四足机器人本体基础传感器和融合算法精确估计足端与地面之间的接触状态。
以上所述的具体实施方式，对本发明的目的、技术方案和有益效果进行了进一步详细说明，所应理解的是，以上所述仅为本发明的具体实施方式而已，并不用于限定本发明的保护范围，凡在本发明的精神和原则之内，所做的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (7)

1.四足机器人足端触地检测方法，其特征在于：包括以下步骤：
S1，计算每条腿的足端离地高度，根据每条腿的足端离地高度计算出其足端高度接触概率；
根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
根据步态相位计算出步态规划接触概率；
S2，根据四条腿的步态规划接触概率、足端高度接触概率、膝关节扭矩接触概率，利用卡尔曼滤波融合得到最终的四腿足端接触概率；根据所述四腿足端接触概率，判断足端触地状态。
2.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：采用以下公式计算足端高度接触概率
Figure DEST_PATH_IMAGE002
：
Figure DEST_PATH_IMAGE004
式中，
Figure DEST_PATH_IMAGE006
表示足端离地高度，
Figure DEST_PATH_IMAGE008
为接触时足端离地高度均值、
Figure DEST_PATH_IMAGE010
为足端离地高度的方差。
3.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：采用以下公式计算膝关节扭矩接触概率
Figure DEST_PATH_IMAGE012
：
Figure DEST_PATH_IMAGE014
式中，
Figure DEST_PATH_IMAGE016
表示膝关节扭矩，
Figure DEST_PATH_IMAGE018
为接触时膝关节扭矩均值、
Figure DEST_PATH_IMAGE020
为膝关节扭矩的方差。
4.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：四足机器人的步态是周期变化的，在每一个步态周期中步态相位在0-1之间递增，每个步态周期按腿部运动状态分为接触阶段和摆动阶段；
0≤步态相位<
Figure DEST_PATH_IMAGE022
为接触阶段，
Figure DEST_PATH_IMAGE024
步态相位
Figure DEST_PATH_IMAGE026
1为摆动阶段；
在规划接触阶段，采用以下公式计算步态规划接触概率
Figure DEST_PATH_IMAGE028
：
Figure DEST_PATH_IMAGE030
在规划摆动阶段，采用以下公式计算步态规划接触概率
Figure 729260DEST_PATH_IMAGE028
：
Figure DEST_PATH_IMAGE032
式中，
Figure DEST_PATH_IMAGE034
表示当前时刻在步态周期中的相位，
Figure 6920DEST_PATH_IMAGE022
表示支撑相和摆动相转换时刻的相位，
Figure DEST_PATH_IMAGE036
表示步态相位的方差。
5.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：采用以下的递推公式计算最终的四腿足端接触概率：
Figure DEST_PATH_IMAGE038
其中，
Figure DEST_PATH_IMAGE040
，
Figure DEST_PATH_IMAGE042
；
Figure DEST_PATH_IMAGE044
，
Figure DEST_PATH_IMAGE046
，
Figure DEST_PATH_IMAGE048
；
Figure DEST_PATH_IMAGE050
，
Figure DEST_PATH_IMAGE052
；
Figure DEST_PATH_IMAGE054
，
Figure DEST_PATH_IMAGE056
；
式中，
Figure DEST_PATH_IMAGE058
表示状态转移过程噪声的协方差矩阵，
Figure DEST_PATH_IMAGE060
表示观测噪声的协方差矩阵；状态转移矩阵
Figure DEST_PATH_IMAGE062
，控制输入矩阵
Figure DEST_PATH_IMAGE064
，
Figure DEST_PATH_IMAGE066
表示状态转移过程噪声，
Figure DEST_PATH_IMAGE068
为通过足端高度接触概率模型计算得到的四腿足端接触概率，
Figure DEST_PATH_IMAGE070
为通过膝关节扭矩接触概率模型计算得到的四腿足端接触概率，
Figure DEST_PATH_IMAGE072
为观测矩阵，
Figure DEST_PATH_IMAGE074
表示观测噪声；
Figure DEST_PATH_IMAGE076
、
Figure DEST_PATH_IMAGE078
、
Figure DEST_PATH_IMAGE080
、
Figure DEST_PATH_IMAGE082
为四条腿的步态规划接触概率；
Figure DEST_PATH_IMAGE084
、
Figure DEST_PATH_IMAGE086
、
Figure DEST_PATH_IMAGE088
、
Figure DEST_PATH_IMAGE090
为四条腿的足端接触概率；
Figure DEST_PATH_IMAGE092
、
Figure DEST_PATH_IMAGE094
、
Figure DEST_PATH_IMAGE096
、
Figure DEST_PATH_IMAGE098
为四条腿的膝关节扭矩接触概率；
Figure DEST_PATH_IMAGE100
、
Figure DEST_PATH_IMAGE102
、
Figure DEST_PATH_IMAGE104
、
Figure DEST_PATH_IMAGE106
为最终输出的四腿足端接触概率；
Figure DEST_PATH_IMAGE108
：
Figure DEST_PATH_IMAGE110
时刻的后验状态估计值；
Figure DEST_PATH_IMAGE112
：k时刻的后验估计值，为滤波融合后的最终输出的四腿足端接触概率；
Figure DEST_PATH_IMAGE114
：
Figure DEST_PATH_IMAGE116
时刻的先验状态估计值，为滤波器的中间计算结果；
Figure DEST_PATH_IMAGE118
：
Figure 338323DEST_PATH_IMAGE116
时刻滤波器的输入；
Figure DEST_PATH_IMAGE120
：
Figure 795849DEST_PATH_IMAGE116
时刻的先验估计协方差矩阵；
Figure DEST_PATH_IMAGE122
：
Figure 672538DEST_PATH_IMAGE110
时刻的后验估计协方差矩阵；
Figure DEST_PATH_IMAGE124
：
Figure 557318DEST_PATH_IMAGE116
时刻后验估计协方差矩阵；
Figure DEST_PATH_IMAGE126
：卡尔曼滤波增益；
Figure DEST_PATH_IMAGE128
：足端高度接触概率模型的观测噪声；
Figure DEST_PATH_IMAGE130
：膝关节扭矩接触概率模型的观测噪声；
Figure DEST_PATH_IMAGE132
：表示4×4的零矩阵；
Figure DEST_PATH_IMAGE134
：表示4×4的单位矩阵；
Figure DEST_PATH_IMAGE136
：表示单位矩阵。
6.根据权利要求1所述的四足机器人足端触地检测方法，其特征在于：通过惯性传感器进行姿态解算得到机器人的质心位置和身体姿态，结合腿部电机编码器反馈的电机位置，计算得到每条腿的足端离地高度。
7.四足机器人本体足端触地检测***，其特征在于：包括测量单元、输入单元和卡尔曼滤波单元；测量单元包括足端高度测量模块、足端高度接触概率测量模块和膝关节扭矩接触概率测量模块；输入单元包括步态规划接触概率计算模块；
足端高度测量模块：负责计算每条腿的足端离地高度；
足端高度接触概率测量模块：负责根据每条腿的足端离地高度计算出其足端高度接触概率；
膝关节扭矩接触概率测量模块：负责根据每条腿的膝关节扭矩计算出其膝关节扭矩接触概率；
步态规划接触概率计算模块：负责计算每条腿的步态规划接触概率；
卡尔曼滤波单元：负责根据输入的步态规划接触概率以及测量的足端高度接触概率、膝关节扭矩接触概率，计算并输出最终的足端触地概率。
CN202110143895.4A 2021-02-03 2021-02-03 四足机器人足端触地检测方法及*** Active CN112478015B (zh)

Priority Applications (2)

Application Number Priority Date Filing Date Title
CN202110143895.4A CN112478015B (zh) 2021-02-03 2021-02-03 四足机器人足端触地检测方法及***
US17/540,168 US20220241992A1 (en) 2021-02-03 2021-12-01 Method and System for Detecting the Foot-end Touchdown of Quadruped Robot

Applications Claiming Priority (1)

Application Number Priority Date Filing Date Title
CN202110143895.4A CN112478015B (zh) 2021-02-03 2021-02-03 四足机器人足端触地检测方法及***

Publications (2)

Publication Number Publication Date
CN112478015A CN112478015A (zh) 2021-03-12
CN112478015B true CN112478015B (zh) 2021-04-16

Family

ID=74912156

Family Applications (1)

Application Number Title Priority Date Filing Date
CN202110143895.4A Active CN112478015B (zh) 2021-02-03 2021-02-03 四足机器人足端触地检测方法及***

Country Status (2)

Country Link
US (1) US20220241992A1 (zh)
CN (1) CN112478015B (zh)

Families Citing this family (2)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
CN113504778B (zh) * 2021-07-26 2023-09-19 广东工业大学 基于融合概率模型的足式机器人控制方法、***及设备
CN115503850B (zh) * 2022-11-17 2023-03-31 国网智能科技股份有限公司 足式机器人足端触地检测方法及***

Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2007105463A1 (ja) * 2006-03-01 2007-09-20 Toyota Jidosha Kabushiki Kaisha 脚式ロボット
CN103192898A (zh) * 2013-04-09 2013-07-10 北京交通大学 一种具有首尾平衡调节装置的仿生四足机器人
CN103192897A (zh) * 2013-04-24 2013-07-10 吉林大学 仿生六轮腿行走机构
CN104875813A (zh) * 2015-05-26 2015-09-02 上海大学 一种电驱动小型仿生四足机器人
CN107203490A (zh) * 2017-04-27 2017-09-26 南京邮电大学 一种类人型机器人平衡的控制方法
CN108567431A (zh) * 2018-05-14 2018-09-25 浙江大学 一种用于测量人体步态及步速的智能传感靴
JP2018167330A (ja) * 2017-03-29 2018-11-01 本田技研工業株式会社 ロボットの制御装置
US10196104B1 (en) * 2016-05-04 2019-02-05 Schaft Inc. Terrain Evaluation for robot locomotion
CN210180564U (zh) * 2019-08-28 2020-03-24 杭州宇树科技有限公司 一种新型足端力采集装置及应用其的四足机器人
CN111168672A (zh) * 2020-01-03 2020-05-19 广东博智林机器人有限公司 机器人运动规划方法、***、计算机设备和存储介质
CN111708042A (zh) * 2020-05-09 2020-09-25 汕头大学 一种行人轨迹预测和跟随的机器人方法及***
CN111730595A (zh) * 2020-06-28 2020-10-02 常州工学院 一种斜坡条件下双足机器人步态稳定控制方法

Family Cites Families (11)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
US20040073414A1 (en) * 2002-06-04 2004-04-15 Brown University Research Foundation Method and system for inferring hand motion from multi-cell recordings in the motor cortex using a kalman filter or a bayesian model
US8457830B2 (en) * 2010-03-22 2013-06-04 John R. Goulding In-line legged robot vehicle and method for operating
KR101438970B1 (ko) * 2012-12-27 2014-09-15 현대자동차주식회사 로봇의 보행제어방법
US20160379074A1 (en) * 2015-06-25 2016-12-29 Appropolis Inc. System and a method for tracking mobile objects using cameras and tag devices
CN109070331A (zh) * 2016-04-05 2018-12-21 株式会社日立制作所 仿人机器人
CN206734447U (zh) * 2017-06-01 2017-12-12 陈剑兴 一种消防侦查机器人行走机构
CN107507229A (zh) * 2017-07-13 2017-12-22 西北工业大学 一种基于均值飘移的机械臂实时跟踪方法
CN110116768B (zh) * 2019-04-30 2020-09-04 厦门大学 由前后牵引与上下滚动实现步态控制的四足机器人
CN111291319B (zh) * 2020-03-24 2023-07-07 广东海洋大学深圳研究院 一种应用于非高斯噪声环境下的移动机器人状态估计方法
CN111667073B (zh) * 2020-03-30 2021-12-28 西北工业大学 一种证据理论改进的模糊卡尔曼滤波目标跟踪方法
CN111605641A (zh) * 2020-04-30 2020-09-01 南京理工大学 一种小型六足仿生机器人

Patent Citations (12)

* Cited by examiner, † Cited by third party
Publication number Priority date Publication date Assignee Title
WO2007105463A1 (ja) * 2006-03-01 2007-09-20 Toyota Jidosha Kabushiki Kaisha 脚式ロボット
CN103192898A (zh) * 2013-04-09 2013-07-10 北京交通大学 一种具有首尾平衡调节装置的仿生四足机器人
CN103192897A (zh) * 2013-04-24 2013-07-10 吉林大学 仿生六轮腿行走机构
CN104875813A (zh) * 2015-05-26 2015-09-02 上海大学 一种电驱动小型仿生四足机器人
US10196104B1 (en) * 2016-05-04 2019-02-05 Schaft Inc. Terrain Evaluation for robot locomotion
JP2018167330A (ja) * 2017-03-29 2018-11-01 本田技研工業株式会社 ロボットの制御装置
CN107203490A (zh) * 2017-04-27 2017-09-26 南京邮电大学 一种类人型机器人平衡的控制方法
CN108567431A (zh) * 2018-05-14 2018-09-25 浙江大学 一种用于测量人体步态及步速的智能传感靴
CN210180564U (zh) * 2019-08-28 2020-03-24 杭州宇树科技有限公司 一种新型足端力采集装置及应用其的四足机器人
CN111168672A (zh) * 2020-01-03 2020-05-19 广东博智林机器人有限公司 机器人运动规划方法、***、计算机设备和存储介质
CN111708042A (zh) * 2020-05-09 2020-09-25 汕头大学 一种行人轨迹预测和跟随的机器人方法及***
CN111730595A (zh) * 2020-06-28 2020-10-02 常州工学院 一种斜坡条件下双足机器人步态稳定控制方法

Non-Patent Citations (2)

* Cited by examiner, † Cited by third party
Title
Trajectory redraw of quadruped crawling robot;Xudong Liu;《Proceedings of the 2017 international conference on advanced mechatronic systems》;20171209;第18-23页 *
基于Kalman滤波的四足机器人姿态数据处理方法;冯华山;《中国制造业信息化》;20081001;第37卷(第19期);第35-40页 *

Also Published As

Publication number Publication date
CN112478015A (zh) 2021-03-12
US20220241992A1 (en) 2022-08-04

Similar Documents

Publication Publication Date Title
CN112478015B (zh) 四足机器人足端触地检测方法及***
US11911916B2 (en) Natural pitch and roll
US11225294B1 (en) Mechanically-timed footsteps for a robotic device
US11911892B2 (en) Touch-down sensing for robotic devices
CN108955688B (zh) 双轮差速移动机器人定位方法及***
US9969087B1 (en) Leg collision avoidance in a robotic device
US7324872B2 (en) Robot apparatus
CN103722550B (zh) 外骨骼机器人的嵌入式***
CN110561424A (zh) 基于多传感器混合滤波器的在线机器人运动学校准方法
EP1770362B1 (en) Angular velocity measuring device and leg-moving robot
US11780515B2 (en) Mitigating sensor noise in legged robots
CN111168672B (zh) 机器人运动规划方法、***、计算机设备和存储介质
CN110967017B (zh) 一种用于双移动机器人刚体协作搬运的协同定位方法
CN113183140B (zh) 一种可换挡连接的仿生蛇形机器人及控制***
TWI382833B (zh) 行走輔助系統
CN101403619B (zh) 仿人机器人足部姿态实时检测***和方法
CN113183167A (zh) 一种足式机器人的运动控制***
JP6270334B2 (ja) ロボット制御装置及び方法
Bai et al. Graph-optimisation-based self-calibration method for IMU/odometer using preintegration theory
CN111738047A (zh) 自身位置推测方法
Sumanarathna et al. Simulation of mobile robot navigation with sensor fusion on an uneven path
JP3536084B2 (ja) 歩行ロボットの歩容制御方法
Lee et al. Optimal posture of mobile inverted pendulum using a single gyroscope and tilt sensor
Chen et al. Backstepping control with sum of squares design for omni-directional mobile robots
Rodríguez-Angeles et al. User wearable interface based on inertial sensors for unilateral master-slave robot teleoperation

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